1 /* -------------------------------------------------------------------------- *
2 * OpenSim: SmoothSegmentedFunctionFactory.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): Matthew Millard *
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 // INCLUDES
25 //=============================================================================
26
27 #include "SmoothSegmentedFunctionFactory.h"
28 //=============================================================================
29 // STATICS
30 //=============================================================================
31 using namespace SimTK;
32 using namespace OpenSim;
33 using namespace std;
34
35
36 //static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample
37 //each Bezier corner section
38
39 //static double SMOOTHING = 0; //The amount of smoothing to use when fitting
40 //3rd order splines to the quintic Bezier
41 //functions
42 //static bool DEBUG = true; //When this is set to true, each function's debug
43 //routine will be called, which usually results
44 //in a text file of its output being produced
45
46 //static double UTOL = (double)SimTK::Eps*1e2;
47
48 //static double INTTOL = (double)SimTK::Eps*1e4;
49
50 //static int MAXITER = 20;
51 //=============================================================================
52 // UTILITY FUNCTIONS
53 //=============================================================================
scaleCurviness(double curviness)54 double SmoothSegmentedFunctionFactory::scaleCurviness(double curviness)
55 {
56 double c = 0.1 + 0.8*curviness;
57 return c;
58 }
59 //=============================================================================
60 // MUSCLE CURVE FITTING FUNCTIONS
61 //=============================================================================
62 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberActiveForceLengthCurve(double x0,double x1,double x2,double x3,double ylow,double dydx,double curviness,bool computeIntegral,const std::string & curveName)63 createFiberActiveForceLengthCurve(double x0, double x1, double x2,
64 double x3, double ylow, double dydx, double curviness,
65 bool computeIntegral, const std::string& curveName)
66 {
67 //Ensure that the inputs are within a valid range
68 double rootEPS = sqrt(SimTK::Eps);
69 SimTK_ERRCHK1_ALWAYS( (x0>=0 && x1>x0+rootEPS
70 && x2>x1+rootEPS && x3>x2+rootEPS),
71 "SmoothSegmentedFunctionFactory::createFiberActiveForceLengthCurve",
72 "%s: This must be true: 0 < lce0 < lce1 < lce2 < lce3",
73 curveName.c_str());
74 SimTK_ERRCHK1_ALWAYS( ylow >= 0,
75 "SmoothSegmentedFunctionFactory::createFiberActiveForceLengthCurve",
76 "%s: shoulderVal must be greater than, or equal to 0",
77 curveName.c_str());
78 double dydxUpperBound = (1-ylow)/(x2-x1);
79 SimTK_ERRCHK2_ALWAYS(dydx >= 0 && dydx < dydxUpperBound,
80 "SmoothSegmentedFunctionFactory::createFiberActiveForceLengthCurve",
81 "%s: plateauSlope must be greater than 0 and less than %f",
82 curveName.c_str(),dydxUpperBound);
83 SimTK_ERRCHK1_ALWAYS( (curviness >= 0 && curviness <= 1),
84 "SmoothSegmentedFunctionFactory::createFiberActiveForceLengthCurve",
85 "%s: curviness must be between 0 and 1",
86 curveName.c_str());
87
88 std::string name = curveName;
89 name.append(".createFiberActiveForceLengthCurve");
90
91
92
93 //Translate the users parameters into Bezier curves
94 double c = scaleCurviness(curviness);
95
96 //The active force length curve is made up of 5 elbow shaped sections.
97 //Compute the locations of the joining point of each elbow section.
98
99 //Calculate the location of the shoulder
100 double xDelta = 0.05*x2; //half the width of the sarcomere 0.0259,
101 //but TM.Winter's data has a wider shoulder than
102 //this
103
104 double xs = (x2-xDelta);//x1 + 0.75*(x2-x1);
105
106 //Calculate the intermediate points located on the ascending limb
107 double y0 = 0;
108 double dydx0 = 0;
109
110 double y1 = 1 - dydx*(xs-x1);
111 double dydx01= 1.25*(y1-y0)/(x1-x0);//(y1-y0)/(x1-(x0+xDelta));
112
113 double x01 = x0 + 0.5*(x1-x0); //x0 + xDelta + 0.5*(x1-(x0+xDelta));
114 double y01 = y0 + 0.5*(y1-y0);
115
116 //Calculate the intermediate points of the shallow ascending plateau
117 double x1s = x1 + 0.5*(xs-x1);
118 double y1s = y1 + 0.5*(1-y1);
119 double dydx1s= dydx;
120
121 //double dydx01c0 = 0.5*(y1s-y01)/(x1s-x01) + 0.5*(y01-y0)/(x01-x0);
122 //double dydx01c1 = 2*( (y1-y0)/(x1-x0));
123 //double dydx01(1-c)*dydx01c0 + c*dydx01c1;
124
125 //x2 entered
126 double y2 = 1;
127 double dydx2 = 0;
128
129 //Descending limb
130 //x3 entered
131 double y3 = 0;
132 double dydx3 = 0;
133
134 double x23 = (x2+xDelta) + 0.5*(x3-(x2+xDelta)); //x2 + 0.5*(x3-x2);
135 double y23 = y2 + 0.5*(y3-y2);
136
137 //double dydx23c0 = 0.5*((y23-y2)/(x23-x2)) + 0.5*((y3-y23)/(x3-x23));
138 //double dydx23c1 = 2*(y3-y2)/(x3-x2);
139 double dydx23 = (y3-y2)/((x3-xDelta)-(x2+xDelta));
140 //(1-c)*dydx23c0 + c*dydx23c1;
141
142 //Compute the locations of the control points
143 SimTK::Matrix p0 = SegmentedQuinticBezierToolkit::
144 calcQuinticBezierCornerControlPoints(x0,ylow,dydx0,x01,y01,dydx01,c);
145 SimTK::Matrix p1 = SegmentedQuinticBezierToolkit::
146 calcQuinticBezierCornerControlPoints(x01,y01,dydx01,x1s,y1s,dydx1s,c);
147 SimTK::Matrix p2 = SegmentedQuinticBezierToolkit::
148 calcQuinticBezierCornerControlPoints(x1s,y1s,dydx1s,x2, y2, dydx2,c);
149 SimTK::Matrix p3 = SegmentedQuinticBezierToolkit::
150 calcQuinticBezierCornerControlPoints(x2, y2, dydx2,x23,y23,dydx23,c);
151 SimTK::Matrix p4 = SegmentedQuinticBezierToolkit::
152 calcQuinticBezierCornerControlPoints(x23,y23,dydx23,x3,ylow,dydx3,c);
153
154 SimTK::Matrix mX(6,5), mY(6,5);
155 mX(0) = p0(0);
156 mX(1) = p1(0);
157 mX(2) = p2(0);
158 mX(3) = p3(0);
159 mX(4) = p4(0);
160
161 mY(0) = p0(1);
162 mY(1) = p1(1);
163 mY(2) = p2(1);
164 mY(3) = p3(1);
165 mY(4) = p4(1);
166
167 //std::string curveName = muscleName;
168 //curveName.append("_fiberActiveForceLengthCurve");
169 SmoothSegmentedFunction* mclCrvFcn =
170 new SmoothSegmentedFunction(
171 mX,mY,x0,x3,ylow,ylow,0,0,computeIntegral,
172 true, curveName);
173 return mclCrvFcn;
174 }
175
176 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberForceVelocityCurve(double fmaxE,double dydxC,double dydxNearC,double dydxIso,double dydxE,double dydxNearE,double concCurviness,double eccCurviness,bool computeIntegral,const std::string & curveName)177 createFiberForceVelocityCurve(double fmaxE,
178 double dydxC, double dydxNearC,
179 double dydxIso,
180 double dydxE, double dydxNearE,
181 double concCurviness,double eccCurviness,
182 bool computeIntegral, const std::string& curveName)
183 {
184 //Ensure that the inputs are within a valid range
185 SimTK_ERRCHK1_ALWAYS( fmaxE > 1.0,
186 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
187 "%s: fmaxE must be greater than 1",curveName.c_str());
188 SimTK_ERRCHK1_ALWAYS( (dydxC >= 0.0 && dydxC < 1),
189 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
190 "%s: dydxC must be greater than or equal to 0"
191 "and less than 1",curveName.c_str());
192 SimTK_ERRCHK1_ALWAYS( (dydxNearC > dydxC && dydxNearC <= 1),
193 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
194 "%s: dydxNearC must be greater than or equal to 0"
195 "and less than 1",curveName.c_str());
196 SimTK_ERRCHK2_ALWAYS( dydxIso > 1,
197 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
198 "%s: dydxIso must be greater than (fmaxE-1)/1 (%f)",curveName.c_str(),
199 ((fmaxE-1.0)/1.0));
200 SimTK_ERRCHK2_ALWAYS( (dydxE >= 0.0 && dydxE < (fmaxE-1)),
201 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
202 "%s: dydxE must be greater than or equal to 0"
203 "and less than fmaxE-1 (%f)",curveName.c_str(),(fmaxE-1));
204 SimTK_ERRCHK2_ALWAYS( (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)),
205 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
206 "%s: dydxNearE must be greater than or equal to dydxE"
207 "and less than fmaxE-1 (%f)",curveName.c_str(),(fmaxE-1));
208 SimTK_ERRCHK1_ALWAYS( (concCurviness <= 1.0 && concCurviness >= 0),
209 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
210 "%s: concCurviness must be between 0 and 1",curveName.c_str());
211 SimTK_ERRCHK1_ALWAYS( (eccCurviness <= 1.0 && eccCurviness >= 0),
212 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
213 "%s: eccCurviness must be between 0 and 1",curveName.c_str());
214
215 std::string name = curveName;
216 name.append(".createFiberForceVelocityCurve");
217
218 //Translate the users parameters into Bezier point locations
219 double cC = scaleCurviness(concCurviness);
220 double cE = scaleCurviness(eccCurviness);
221
222 //Compute the concentric control point locations
223 double xC = -1;
224 double yC = 0;
225
226 double xNearC = -0.9;
227 double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC);
228
229 double xIso = 0;
230 double yIso = 1;
231
232 double xE = 1;
233 double yE = fmaxE;
234
235 double xNearE = 0.9;
236 double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE);
237
238
239 SimTK::Matrix concPts1 = SegmentedQuinticBezierToolkit::
240 calcQuinticBezierCornerControlPoints(xC,yC,dydxC,
241 xNearC, yNearC,dydxNearC,cC);
242 SimTK::Matrix concPts2 = SegmentedQuinticBezierToolkit::
243 calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC,
244 xIso, yIso, dydxIso, cC);
245 SimTK::Matrix eccPts1 = SegmentedQuinticBezierToolkit::
246 calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso,
247 xNearE, yNearE, dydxNearE, cE);
248
249 SimTK::Matrix eccPts2 = SegmentedQuinticBezierToolkit::
250 calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE,
251 xE, yE, dydxE, cE);
252
253 SimTK::Matrix mX(6,4), mY(6,4);
254 mX(0) = concPts1(0);
255 mX(1) = concPts2(0);
256 mX(2) = eccPts1(0);
257 mX(3) = eccPts2(0);
258
259 mY(0) = concPts1(1);
260 mY(1) = concPts2(1);
261 mY(2) = eccPts1(1);
262 mY(3) = eccPts2(1);
263
264 //std::string curveName = muscleName;
265 //curveName.append("_fiberForceVelocityCurve");
266 SmoothSegmentedFunction* mclCrvFcn =
267 new SmoothSegmentedFunction(mX,mY,xC,xE,yC,yE,dydxC,dydxE,
268 computeIntegral, true, curveName);
269 return mclCrvFcn;
270 }
271
272
273 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberForceVelocityInverseCurve(double fmaxE,double dydxC,double dydxNearC,double dydxIso,double dydxE,double dydxNearE,double concCurviness,double eccCurviness,bool computeIntegral,const std::string & curveName)274 createFiberForceVelocityInverseCurve(double fmaxE,
275 double dydxC, double dydxNearC,
276 double dydxIso,
277 double dydxE, double dydxNearE,
278 double concCurviness, double eccCurviness,
279 bool computeIntegral, const std::string& curveName)
280 {
281 //Ensure that the inputs are within a valid range
282 SimTK_ERRCHK1_ALWAYS( fmaxE > 1.0,
283 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
284 "%s: fmaxE must be greater than 1",curveName.c_str());
285 SimTK_ERRCHK1_ALWAYS( (dydxC > SimTK::SignificantReal && dydxC < 1),
286 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
287 "%s: dydxC must be greater than 0"
288 "and less than 1",curveName.c_str());
289 SimTK_ERRCHK1_ALWAYS( (dydxNearC > dydxC && dydxNearC < 1),
290 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
291 "%s: dydxNearC must be greater than 0"
292 "and less than 1",curveName.c_str());
293 SimTK_ERRCHK1_ALWAYS( dydxIso > 1,
294 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
295 "%s: dydxIso must be greater than or equal to 1",curveName.c_str());
296 SimTK_ERRCHK2_ALWAYS( (dydxE > SimTK::SignificantReal && dydxE < (fmaxE-1)),
297 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
298 "%s: dydxE must be greater than or equal to 0"
299 "and less than fmaxE-1 (%f)",curveName.c_str(),(fmaxE-1));
300 SimTK_ERRCHK2_ALWAYS( (dydxNearE >= dydxE && dydxNearE < (fmaxE-1)),
301 "SmoothSegmentedFunctionFactory::createFiberForceVelocityCurve",
302 "%s: dydxNearE must be greater than or equal to dydxE"
303 "and less than fmaxE-1 (%f)",curveName.c_str(),(fmaxE-1));
304 SimTK_ERRCHK1_ALWAYS( (concCurviness <= 1.0 && concCurviness >= 0),
305 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
306 "%s: concCurviness must be between 0 and 1",curveName.c_str());
307 SimTK_ERRCHK1_ALWAYS( (eccCurviness <= 1.0 && eccCurviness >= 0),
308 "SmoothSegmentedFunctionFactory::createFiberForceVelocityInverseCurve",
309 "%s: eccCurviness must be between 0 and 1",curveName.c_str());
310
311 std::string name = curveName;
312 name.append(".createFiberForceVelocityInverseCurve");
313
314 //Translate the users parameters into Bezier point locations
315 double cC = scaleCurviness(concCurviness);
316 double cE = scaleCurviness(eccCurviness);
317
318 //Compute the concentric control point locations
319 double xC = -1;
320 double yC = 0;
321
322 double xNearC = -0.9;
323 double yNearC = yC + 0.5*dydxNearC*(xNearC-xC) + 0.5*dydxC*(xNearC-xC);
324
325 double xIso = 0;
326 double yIso = 1;
327
328 double xE = 1;
329 double yE = fmaxE;
330
331 double xNearE = 0.9;
332 double yNearE = yE + 0.5*dydxNearE*(xNearE-xE) + 0.5*dydxE*(xNearE-xE);
333
334
335 SimTK::Matrix concPts1 = SegmentedQuinticBezierToolkit::
336 calcQuinticBezierCornerControlPoints(xC,yC,dydxC,
337 xNearC, yNearC,dydxNearC,cC);
338 SimTK::Matrix concPts2 = SegmentedQuinticBezierToolkit::
339 calcQuinticBezierCornerControlPoints(xNearC,yNearC,dydxNearC,
340 xIso, yIso, dydxIso, cC);
341 SimTK::Matrix eccPts1 = SegmentedQuinticBezierToolkit::
342 calcQuinticBezierCornerControlPoints(xIso, yIso, dydxIso,
343 xNearE, yNearE, dydxNearE, cE);
344
345 SimTK::Matrix eccPts2 = SegmentedQuinticBezierToolkit::
346 calcQuinticBezierCornerControlPoints(xNearE, yNearE, dydxNearE,
347 xE, yE, dydxE, cE);
348
349 SimTK::Matrix mX(6,4), mY(6,4);
350 mX(0) = concPts1(0);
351 mX(1) = concPts2(0);
352 mX(2) = eccPts1(0);
353 mX(3) = eccPts2(0);
354
355 mY(0) = concPts1(1);
356 mY(1) = concPts2(1);
357 mY(2) = eccPts1(1);
358 mY(3) = eccPts2(1);
359
360 SmoothSegmentedFunction* mclCrvFcn = new
361 SmoothSegmentedFunction(mY,mX,yC,yE,xC,xE,1/dydxC,1/dydxE,
362 computeIntegral,true, curveName);
363 return mclCrvFcn;
364
365 }
366
367 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberCompressiveForcePennationCurve(double phi0,double k,double curviness,bool computeIntegral,const std::string & curveName)368 createFiberCompressiveForcePennationCurve(double phi0, double k,
369 double curviness, bool computeIntegral, const std::string& curveName)
370 {
371 //Check the input arguments
372 SimTK_ERRCHK1_ALWAYS( (phi0>0 && phi0<(SimTK::Pi/2.0)) ,
373 "SmoothSegmentedFunctionFactory::createFiberCompressiveForcePennationCurve",
374 "%s: phi0 must be greater than 0, and less than Pi/2",curveName.c_str());
375
376 SimTK_ERRCHK2_ALWAYS( k > (1.0/(SimTK::Pi/2.0-phi0)) ,
377 "SmoothSegmentedFunctionFactory::createFiberCompressiveForcePennationCurve",
378 "%s: k must be greater than %f",curveName.c_str(),
379 (1.0/(SimTK::Pi/2.0-phi0)));
380
381 SimTK_ERRCHK1_ALWAYS( (curviness>=0 && curviness <= 1) ,
382 "SmoothSegmentedFunctionFactory::createFiberCompressiveForcePennationCurve",
383 "%s: curviness must be between 0.0 and 1.0",curveName.c_str());
384
385 std::string name=curveName;
386 name.append(".createFiberCompressiveForcePennationCurve");
387
388 //Translate the user parameters to quintic Bezier points
389 double c = scaleCurviness(curviness);
390 double x0 = phi0;
391 double y0 = 0;
392 double dydx0 = 0;
393 double x1 = SimTK::Pi/2.0;
394 double y1 = 1;
395 double dydx1 = k;
396
397 SimTK::Matrix ctrlPts = SegmentedQuinticBezierToolkit::
398 calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c);
399
400 SimTK::Matrix mX(6,1), mY(6,1);
401 mX = ctrlPts(0);
402 mY = ctrlPts(1);
403
404 //std::string curveName = muscleName;
405 //curveName.append("_fiberCompressiveForcePennationCurve");
406 SmoothSegmentedFunction* mclCrvFcn =
407 new SmoothSegmentedFunction(mX,mY,x0,x1,y0,y1,dydx0,dydx1,computeIntegral,
408 true,curveName);
409
410 //If in debug, print the function
411 return mclCrvFcn;
412
413 }
414
415 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberCompressiveForceCosPennationCurve(double cosPhi0,double k,double curviness,bool computeIntegral,const std::string & curveName)416 createFiberCompressiveForceCosPennationCurve(double cosPhi0, double k,
417 double curviness, bool computeIntegral, const std::string& curveName)
418 {
419 //Check the input arguments
420 SimTK_ERRCHK1_ALWAYS( (cosPhi0>0 && cosPhi0 < 1) ,
421 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceCosPennationCurve",
422 "%s: cosPhi0 must be greater than 0, and less than 1",curveName.c_str());
423
424 SimTK_ERRCHK1_ALWAYS( k < 1/cosPhi0 ,
425 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceCosPennationCurve",
426 "%s: k must be less than 0",curveName.c_str());
427
428 SimTK_ERRCHK1_ALWAYS( (curviness>=0 && curviness <= 1) ,
429 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceCosPennationCurve",
430 "%s: curviness must be between 0.0 and 1.0",curveName.c_str());
431
432 std::string name=curveName;
433 name.append(".createFiberCompressiveForceCosPennationCurve");
434
435 //Translate the user parameters to quintic Bezier points
436 double c = scaleCurviness(curviness);
437 double x0 = 0;
438 double y0 = 1;
439 double dydx0 = k;
440 double x1 = cosPhi0;
441 double y1 = 0;
442 double dydx1 = 0;
443
444 SimTK::Matrix ctrlPts = SegmentedQuinticBezierToolkit::
445 calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c);
446
447 SimTK::Matrix mX(6,1), mY(6,1);
448 mX = ctrlPts(0);
449 mY = ctrlPts(1);
450
451 //std::string curveName = muscleName;
452 //curveName.append("_fiberCompressiveForceCosPennationCurve");
453 SmoothSegmentedFunction* mclCrvFcn =
454 new SmoothSegmentedFunction(mX,mY,x0,x1,y0,y1,dydx0,dydx1,computeIntegral,
455 false,curveName);
456
457 //If in debug, print the function
458 return mclCrvFcn;
459
460 }
461
462 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberCompressiveForceLengthCurve(double lmax,double k,double curviness,bool computeIntegral,const std::string & curveName)463 createFiberCompressiveForceLengthCurve(double lmax, double k,
464 double curviness, bool computeIntegral,
465 const std::string& curveName)
466 {
467 //Check the input arguments
468 SimTK_ERRCHK1_ALWAYS( lmax>0 ,
469 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceLength",
470 "%s: l0 must be greater than 0",curveName.c_str());
471
472 SimTK_ERRCHK2_ALWAYS( k < -(1.0/lmax) ,
473 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceLength",
474 "%s: k must be less than %f",curveName.c_str(),-(1.0/lmax));
475
476 SimTK_ERRCHK1_ALWAYS( (curviness>=0 && curviness <= 1) ,
477 "SmoothSegmentedFunctionFactory::createFiberCompressiveForceLength",
478 "%s: curviness must be between 0.0 and 1.0",curveName.c_str());
479
480 std::string caller = curveName;
481 caller.append(".createFiberCompressiveForceLength");
482
483 //Translate the user parameters to quintic Bezier points
484 double c = scaleCurviness(curviness);
485 double x0 = 0.0;
486 double y0 = 1;
487 double dydx0 = k;
488 double x1 = lmax;
489 double y1 = 0;
490 double dydx1 = 0;
491
492 SimTK::Matrix ctrlPts = SegmentedQuinticBezierToolkit::
493 calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c);
494
495 SimTK::Matrix mX(6,1), mY(6,1);
496 mX(0) = ctrlPts(0);
497 mY(0) = ctrlPts(1);
498
499 // curveName = muscleName;
500 //curveName.append("_fiberCompressiveForceLengthCurve");
501 SmoothSegmentedFunction* mclCrvFcn =
502 new SmoothSegmentedFunction(mX,mY,x0,x1,y0,y1,dydx0,dydx1,computeIntegral,
503 false,curveName);
504
505 return mclCrvFcn;
506
507 }
508
509
510 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createFiberForceLengthCurve(double eZero,double eIso,double kLow,double kIso,double curviness,bool computeIntegral,const std::string & curveName)511 createFiberForceLengthCurve(double eZero, double eIso,
512 double kLow, double kIso, double curviness,
513 bool computeIntegral, const std::string& curveName)
514 {
515
516 //Check the input arguments
517 SimTK_ERRCHK1_ALWAYS( eIso > eZero ,
518 "SmoothSegmentedFunctionFactory::createFiberForceLength",
519 "%s: The following must hold: eIso > eZero",curveName.c_str());
520
521 SimTK_ERRCHK2_ALWAYS( kIso > (1.0/(eIso-eZero)) ,
522 "SmoothSegmentedFunctionFactory::createFiberForceLength",
523 "%s: kiso must be greater than 1/(eIso-eZero) (%f)",
524 curveName.c_str(),(1.0/(eIso-eZero)));
525
526 SimTK_ERRCHK1_ALWAYS(kLow > 0.0 && kLow < 1/(eIso-eZero),
527 "SmoothSegmentedFunctionFactory::createFiberForceLength",
528 "%s: kLow must be greater than 0 and less than or equal to 1",
529 curveName.c_str());
530
531 SimTK_ERRCHK1_ALWAYS( (curviness>=0 && curviness <= 1) ,
532 "SmoothSegmentedFunctionFactory::createFiberForceLength",
533 "%s: curviness must be between 0.0 and 1.0",curveName.c_str());
534
535 std::string callerName = curveName;
536 callerName.append(".createFiberForceLength");
537
538 //Translate the user parameters to quintic Bezier points
539 /*
540 double c = scaleCurviness(curviness);
541 double x0 = 1.0 + e0;
542 double y0 = 0;
543 double dydx0 = 0;
544 double x1 = 1.0 + e1;
545 double y1 = 1;
546 double dydx1 = kiso;
547
548 SimTK::Matrix ctrlPts = SegmentedQuinticBezierToolkit::
549 calcQuinticBezierCornerControlPoints(x0,y0,dydx0,x1,y1,dydx1,c,callerName);
550
551 SimTK::Matrix mX(6,1), mY(6,1);
552 mX(0) = ctrlPts(0);
553 mY(0) = ctrlPts(1);
554 */
555
556 //Translate the user parameters to quintic Bezier points
557 double c = scaleCurviness(curviness);
558 double xZero = 1+eZero;
559 double yZero = 0;
560
561 double xIso = 1 + eIso;
562 double yIso = 1;
563
564 double deltaX = min(0.1*(1.0/kIso), 0.1*(xIso-xZero));
565
566 double xLow = xZero + deltaX;
567 double xfoot = xZero + 0.5*(xLow-xZero);
568 double yfoot = 0;
569 double yLow = yfoot + kLow*(xLow-xfoot);
570
571 //Compute the Quintic Bezier control points
572 SimTK::Matrix p0 = SegmentedQuinticBezierToolkit::
573 calcQuinticBezierCornerControlPoints(xZero, yZero, 0,
574 xLow, yLow, kLow,c);
575
576 SimTK::Matrix p1 = SegmentedQuinticBezierToolkit::
577 calcQuinticBezierCornerControlPoints(xLow, yLow, kLow,
578 xIso, yIso, kIso, c);
579 SimTK::Matrix mX(6,2);
580 SimTK::Matrix mY(6,2);
581
582 mX(0) = p0(0);
583 mY(0) = p0(1);
584
585 mX(1) = p1(0);
586 mY(1) = p1(1);
587
588
589 //std::string curveName = muscleName;
590 //curveName.append("_tendonForceLengthCurve");
591 //Instantiate a muscle curve object
592 SmoothSegmentedFunction* mclCrvFcn =
593 new SmoothSegmentedFunction( mX, mY,
594 xZero, xIso,
595 yZero, yIso,
596 0.0, kIso,
597 computeIntegral,
598 true,curveName);
599
600
601 return mclCrvFcn;
602 }
603
604
605
606
607 SmoothSegmentedFunction* SmoothSegmentedFunctionFactory::
createTendonForceLengthCurve(double eIso,double kIso,double fToe,double curviness,bool computeIntegral,const std::string & curveName)608 createTendonForceLengthCurve( double eIso, double kIso,
609 double fToe, double curviness,
610 bool computeIntegral,
611 const std::string& curveName)
612 {
613 //Check the input arguments
614 //eIso>0
615 SimTK_ERRCHK2_ALWAYS( eIso>0 ,
616 "SmoothSegmentedFunctionFactory::createTendonForceLengthCurve",
617 "%s: eIso must be greater than 0, but %f was entered",
618 curveName.c_str(),eIso);
619
620 SimTK_ERRCHK2_ALWAYS( (fToe>0 && fToe < 1) ,
621 "SmoothSegmentedFunctionFactory::createTendonForceLengthCurve",
622 "%s: fToe must be greater than 0 and less than 1, but %f was entered",
623 curveName.c_str(),fToe);
624
625 SimTK_ERRCHK3_ALWAYS( kIso > (1/eIso) ,
626 "SmoothSegmentedFunctionFactory::createTendonForceLengthCurve",
627 "%s : kIso must be greater than 1/eIso, (%f), but kIso (%f) was entered",
628 curveName.c_str(), (1/eIso),kIso);
629
630 SimTK_ERRCHK2_ALWAYS( (curviness>=0 && curviness <= 1) ,
631 "SmoothSegmentedFunctionFactory::createTendonForceLengthCurve",
632 "%s : curviness must be between 0.0 and 1.0, but %f was entered"
633 , curveName.c_str(),curviness);
634
635 std::string callerName = curveName;
636 callerName.append(".createTendonForceLengthCurve");
637
638 //Translate the user parameters to quintic Bezier points
639 double c = scaleCurviness(curviness);
640 double x0 = 1.0;
641 double y0 = 0;
642 double dydx0 = 0;
643
644 double xIso = 1.0 + eIso;
645 double yIso = 1;
646 double dydxIso = kIso;
647
648 //Location where the curved section becomes linear
649 double yToe = fToe;
650 double xToe = (yToe-1)/kIso + xIso;
651
652
653 //To limit the 2nd derivative of the toe region the line it tends to
654 //has to intersect the x axis to the right of the origin
655 double xFoot = 1.0+(xToe-1.0)/10.0;
656 double yFoot = 0;
657 //double dydxToe = (yToe-yFoot)/(xToe-xFoot);
658
659 //Compute the location of the corner formed by the average slope of the
660 //toe and the slope of the linear section
661 double yToeMid = yToe*0.5;
662 double xToeMid = (yToeMid-yIso)/kIso + xIso;
663 double dydxToeMid = (yToeMid-yFoot)/(xToeMid-xFoot);
664
665 //Compute the location of the control point to the left of the corner
666 double xToeCtrl = xFoot + 0.5*(xToeMid-xFoot);
667 double yToeCtrl = yFoot + dydxToeMid*(xToeCtrl-xFoot);
668
669
670
671 //Compute the Quintic Bezier control points
672 SimTK::Matrix p0 = SegmentedQuinticBezierToolkit::
673 calcQuinticBezierCornerControlPoints(x0,y0,dydx0,
674 xToeCtrl,yToeCtrl,dydxToeMid,c);
675 SimTK::Matrix p1 = SegmentedQuinticBezierToolkit::
676 calcQuinticBezierCornerControlPoints(xToeCtrl, yToeCtrl, dydxToeMid,
677 xToe, yToe, dydxIso, c);
678 SimTK::Matrix mX(6,2);
679 SimTK::Matrix mY(6,2);
680
681 mX(0) = p0(0);
682 mY(0) = p0(1);
683
684 mX(1) = p1(0);
685 mY(1) = p1(1);
686
687 //std::string curveName = muscleName;
688 //curveName.append("_tendonForceLengthCurve");
689 //Instantiate a muscle curve object
690 SmoothSegmentedFunction* mclCrvFcn =
691 new SmoothSegmentedFunction( mX, mY,
692 x0, xToe,
693 y0, yToe,
694 dydx0, dydxIso,
695 computeIntegral,
696 true,curveName);
697
698 return mclCrvFcn;
699 }
700
701
702