1 /* -------------------------------------------------------------------------- *
2 * OpenSim: RootSolver.cpp *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5 * See http://opensim.stanford.edu and the NOTICE file for more information. *
6 * OpenSim is developed at Stanford University and supported by the US *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8 * through the Warrior Web program. *
9 * *
10 * Copyright (c) 2005-2017 Stanford University and the Authors *
11 * Author(s): Frank C. Anderson *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 /*
25 * Author: Frank C. Anderson
26 */
27
28
29 // INCLUDES
30 #include <float.h>
31 #include <math.h>
32 #include "RootSolver.h"
33
34
35
36
37 using namespace OpenSim;
38 using namespace std;
39
40
41 //=============================================================================
42 // DESTRUCTOR AND CONSTRUCTORS
43 //=============================================================================
44 //_____________________________________________________________________________
45 /**
46 * Destructor.
47 */
~RootSolver()48 RootSolver::~RootSolver()
49 {
50 }
51
52 //_____________________________________________________________________________
53 /**
54 * Default constructor.
55 */
56 RootSolver::
RootSolver(VectorFunctionUncoupledNxN * aFunc)57 RootSolver(VectorFunctionUncoupledNxN *aFunc)
58 {
59 setNull();
60 _function = aFunc;
61 }
62
63 //-----------------------------------------------------------------------------
64 // CONSTRUCTION METHODS
65 //-----------------------------------------------------------------------------
66 //_____________________________________________________________________________
67 /**
68 * Set all member variables to NULL values.
69 */
70 void RootSolver::
setNull()71 setNull()
72 {
73 _function = NULL;
74 }
75
76
77 //=============================================================================
78 // SOLVE
79 //=============================================================================
80 //_____________________________________________________________________________
81 /**
82 * Solve for the roots.
83 *
84 *
85 */
86 Array<double> RootSolver::
solve(const SimTK::State & s,const Array<double> & ax,const Array<double> & bx,const Array<double> & tol)87 solve(const SimTK::State& s, const Array<double> &ax,const Array<double> &bx,
88 const Array<double> &tol)
89 {
90 int i;
91 int N = _function->getNX();
92
93 Array<double> a(0.0,N),b(0.0,N),c(0.0,N);
94 Array<double> fa(0.0,N),fb(0.0,N),fc(0.0,N);
95 Array<double> prev_step(0.0,N);
96 Array<double> tol_act(0.0,N);
97 Array<double> p(0.0,N);
98 Array<double> q(0.0,N);
99 Array<double> new_step(0.0,N);
100
101 bool finished = false;
102 Array<int> converged(0,N);
103
104
105 // INITIALIZATIONS
106 a = ax;
107 b = bx;
108 _function->evaluate(s,a,fa);
109 _function->evaluate(s,b,fb);
110 c = a;
111 fc = fa;
112
113
114 // ITERATION LOOP
115 int iter;
116 for(iter=0;!finished;iter++) {
117
118 // ABSCISSAE MANIPULATION LOOP
119 for(i=0;i<N;i++) {
120
121 // Continue?
122 // If a function is already converged no need to do any manipulation.
123 if(converged[i]) continue;
124
125 // Make c on opposite side of b.
126 // (was down at very bottom)
127 if( (fb[i]>0.0 && fc[i]>0.0) || (fb[i]<0.0 && fc[i]<0.0) ) {
128 c[i] = a[i];
129 fc[i] = fa[i];
130 }
131
132 // Record previous step
133 prev_step[i] = b[i] - a[i];
134
135 // Swap data for b to be the best approximation.
136 if( fabs(fc[i]) < fabs(fb[i]) ) {
137 a[i] = b[i]; b[i] = c[i]; c[i] = a[i];
138 fa[i]= fb[i]; fb[i]= fc[i]; fc[i]= fa[i];
139 }
140 tol_act[i] = 2.0*DBL_EPSILON*fabs(b[i]) + 0.5*tol[i];
141 new_step[i] = 0.5 * (c[i]-b[i]);
142
143 // Converged?
144 // Original convergence test:
145 if(fabs(new_step[i])<=tol_act[i] || fb[i]==(double)0.0 ) {
146 converged[i] = iter;
147 continue;
148 }
149
150 // Interpolate if prev_step was large enough and in true direction
151 if( fabs(prev_step[i])>=tol_act[i] && fabs(fa[i])>fabs(fb[i]) ) {
152 double t1,cb,t2;
153 cb = c[i]-b[i];
154
155 // Only two distinct roots, must use linear interpolation.
156 if(a[i]==c[i]) {
157 t1 = fb[i]/fa[i];
158 p[i] = cb*t1;
159 q[i] = 1.0 - t1;
160
161 // Quadratic interpolation
162 } else {
163 q[i] = fa[i]/fc[i]; t1 = fb[i]/fc[i]; t2 = fb[i]/fa[i];
164 p[i] = t2 * ( cb*q[i]*(q[i]-t1) - (b[i]-a[i])*(t1-1.0) );
165 q[i] = (q[i]-1.0) * (t1-1.0) * (t2-1.0);
166 }
167
168 // Change sign of q or p?
169 if( p[i]>(double)0.0 ) {
170 q[i] = -q[i];
171 } else {
172 p[i] = -p[i];
173 }
174
175 // If the interpolate is bad, use bisection.
176 if( p[i]<(0.75*cb*q[i] - 0.5*fabs(tol_act[i]*q[i])) && p[i]<fabs(0.5*prev_step[i]*q[i]) )
177 new_step[i] = p[i]/q[i];
178 }
179
180 // Adjust step to be not less than tolerance.
181 if( fabs(new_step[i]) < tol_act[i] ) {
182 if( new_step[i] > (double)0.0 ) {
183 new_step[i] = tol_act[i];
184 }
185 else {
186 new_step[i] = -tol_act[i];
187 }
188 }
189
190 // Save previous approximation.
191 a[i] = b[i]; fa[i] = fb[i];
192
193
194 b[i] += new_step[i];
195
196 } // END ABSCISSAE LOOP
197
198
199 // NEW FUNCTION EVALUATION
200 _function->evaluate(s, b,fb);
201
202
203 // FINISHED?
204 for(i=0;i<N;i++) {
205 finished = true;
206 if(!converged[i]) {
207 finished = false;
208 break;
209 }
210 }
211 }
212
213 // PRINT
214 //cout<<"\n\nRootSolver: found solution in "<<iter<<" iterations.\n";
215 //cout<<"converged array:\n";
216 //cout<<converged<<endl<<endl;
217 //cout<<"roots:\n";
218 //cout<<b<<endl<<endl;
219 //cout<<"errors:\n";
220 //cout<<fb<<endl;
221
222 return(b);
223 }
224