From 9452f928d0b586fa27be0940f2e28931057b010d Mon Sep 17 00:00:00 2001
From: "Penn, John M 047828115" <john.m.penn@nasa.gov>
Date: Mon, 4 May 2020 16:10:35 -0500
Subject: [PATCH] Breakout Regula Falsi Bounds setter functions. Ref #995

---
 include/trick/regula_falsi.h                  |  2 ++
 .../models/contact/src/Contact.cpp            |  7 ++----
 .../Integrator/src/regula_falsi.c             | 24 ++++++++++++-------
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/include/trick/regula_falsi.h b/include/trick/regula_falsi.h
index 54f14cea..ecfca559 100644
--- a/include/trick/regula_falsi.h
+++ b/include/trick/regula_falsi.h
@@ -54,6 +54,8 @@ typedef struct {
 
 double regula_falsi(double my_time, REGULA_FALSI * R);
 void reset_regula_falsi(double my_time, REGULA_FALSI * R);
+void regula_falsi_set_upper (double my_time, double error, REGULA_FALSI * R);
+void regula_falsi_set_lower (double my_time, double error, REGULA_FALSI * R);
 
 #ifdef __cplusplus
 }
diff --git a/trick_sims/SIM_contact/models/contact/src/Contact.cpp b/trick_sims/SIM_contact/models/contact/src/Contact.cpp
index c8c25dcb..02a9d862 100755
--- a/trick_sims/SIM_contact/models/contact/src/Contact.cpp
+++ b/trick_sims/SIM_contact/models/contact/src/Contact.cpp
@@ -127,14 +127,11 @@ double Contact::collision() {
                 if ((ii==first) && (jj==second)) {
                     reset_regula_falsi( now, &ballAssociations[association_index] );
                 } else {
-                    //Set X_upper and t_upper to event values.
+                    // Update the remaining REGULA_FALSI objects to the current event state.
                     double xdiff = balls[ii]->pos[0] - balls[jj]->pos[0];
                     double ydiff = balls[ii]->pos[1] - balls[jj]->pos[1];
                     double distance = sqrt( xdiff * xdiff + ydiff * ydiff) - (balls[ii]->radius + balls[jj]->radius);
-                    ballAssociations[association_index].error = distance;
-                    ballAssociations[association_index].x_upper = distance;
-                    ballAssociations[association_index].t_upper = now;
-                    ballAssociations[association_index].upper_set = 1;
+                    regula_falsi_set_upper (now, distance, &ballAssociations[association_index]);
                 }
             }
         }
diff --git a/trick_source/sim_services/Integrator/src/regula_falsi.c b/trick_source/sim_services/Integrator/src/regula_falsi.c
index fa489d76..104409f7 100644
--- a/trick_source/sim_services/Integrator/src/regula_falsi.c
+++ b/trick_source/sim_services/Integrator/src/regula_falsi.c
@@ -4,6 +4,20 @@
 #define M_ABS(x) ((x) < 0 ? -(x) : (x))
 #include "trick/regula_falsi.h"
 
+void regula_falsi_set_upper (double time, double error, REGULA_FALSI *R) {
+    R->error = error;
+    R->x_upper = error;
+    R->t_upper = time;
+    R->upper_set = 1;
+}
+
+void regula_falsi_set_lower (double time, double error, REGULA_FALSI *R) {
+    R->error = error;
+    R->x_lower = error;
+    R->t_lower = time;
+    R->lower_set = 1;
+}
+
 double regula_falsi(
                            /* Return: Est. time to go to err
                                       function zero point */
@@ -26,15 +40,9 @@ double regula_falsi(
         }
     }
     if (R->error < 0.0) {
-        /* Set lower bounds */
-        R->x_lower = R->error;
-        R->t_lower = time;
-        R->lower_set = 1;
+        regula_falsi_set_lower(time, R->error, R);
     } else if (R->error > 0.0) {
-        /* Set upper bounds */
-        R->x_upper = R->error;
-        R->t_upper = time;
-        R->upper_set = 1;
+        regula_falsi_set_upper(time, R->error, R);
     }
     R->iterations++;