Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Scalar.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATRIX_SCALAR_H_
2 #define SimTK_SIMMATRIX_SCALAR_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
33 #include "SimTKcommon/internal/common.h"
34 #include "SimTKcommon/Constants.h"
35 #include "SimTKcommon/internal/Exception.h"
36 #include "SimTKcommon/internal/ExceptionMacros.h"
37 #include "SimTKcommon/internal/String.h"
38 #include "SimTKcommon/internal/Array.h"
39 
40 #include "SimTKcommon/internal/conjugate.h"
41 #include "SimTKcommon/internal/CompositeNumericalTypes.h"
42 #include "SimTKcommon/internal/NTraits.h"
43 #include "SimTKcommon/internal/negator.h"
44 
45 #include <complex>
46 #include <cmath>
47 #include <climits>
48 #include <cassert>
49 #include <utility>
50 
51 namespace SimTK {
52 
54  // Handy default-precision definitions //
56 
57 typedef conjugate<Real> Conjugate; // like Complex
58 
91 extern SimTK_SimTKCOMMON_EXPORT const Real NaN;
95 extern SimTK_SimTKCOMMON_EXPORT const Real Infinity;
96 
100 extern SimTK_SimTKCOMMON_EXPORT const Real Eps;
104 extern SimTK_SimTKCOMMON_EXPORT const Real SqrtEps;
110 extern SimTK_SimTKCOMMON_EXPORT const Real TinyReal;
115 extern SimTK_SimTKCOMMON_EXPORT const Real SignificantReal;
116 
119 extern SimTK_SimTKCOMMON_EXPORT const Real LeastPositiveReal;
123 extern SimTK_SimTKCOMMON_EXPORT const Real MostPositiveReal;
126 extern SimTK_SimTKCOMMON_EXPORT const Real LeastNegativeReal;
130 extern SimTK_SimTKCOMMON_EXPORT const Real MostNegativeReal;
131 
135 extern SimTK_SimTKCOMMON_EXPORT const int NumDigitsReal;
136 
142 extern SimTK_SimTKCOMMON_EXPORT const int LosslessNumDigitsReal; // double ~20,
143  // float ~9
144 
145  // Carefully calculated constants, with convenient memory addresses.
146 
147 extern SimTK_SimTKCOMMON_EXPORT const Real Zero;
148 extern SimTK_SimTKCOMMON_EXPORT const Real One;
149 extern SimTK_SimTKCOMMON_EXPORT const Real MinusOne;
150 extern SimTK_SimTKCOMMON_EXPORT const Real Two;
151 extern SimTK_SimTKCOMMON_EXPORT const Real Three;
152 
153 extern SimTK_SimTKCOMMON_EXPORT const Real OneHalf;
154 extern SimTK_SimTKCOMMON_EXPORT const Real OneThird;
155 extern SimTK_SimTKCOMMON_EXPORT const Real OneFourth;
156 extern SimTK_SimTKCOMMON_EXPORT const Real OneFifth;
157 extern SimTK_SimTKCOMMON_EXPORT const Real OneSixth;
158 extern SimTK_SimTKCOMMON_EXPORT const Real OneSeventh;
159 extern SimTK_SimTKCOMMON_EXPORT const Real OneEighth;
160 extern SimTK_SimTKCOMMON_EXPORT const Real OneNinth;
161 extern SimTK_SimTKCOMMON_EXPORT const Real Pi;
162 extern SimTK_SimTKCOMMON_EXPORT const Real OneOverPi;
163 extern SimTK_SimTKCOMMON_EXPORT const Real E;
164 extern SimTK_SimTKCOMMON_EXPORT const Real Log2E;
165 extern SimTK_SimTKCOMMON_EXPORT const Real Log10E;
166 extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt2;
167 extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt2;
168 extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt3;
169 extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt3;
170 extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot2;
171 extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot3;
172 extern SimTK_SimTKCOMMON_EXPORT const Real Ln2;
173 extern SimTK_SimTKCOMMON_EXPORT const Real Ln10;
174 
180 extern SimTK_SimTKCOMMON_EXPORT const Complex I;
183 
184  // SOME SCALAR UTILITIES //
186 
187 
201 // We use the trick that v & v-1 returns the value that is v with its
202 // rightmost bit cleared (if it has a rightmost bit set).
203 inline bool atMostOneBitIsSet(unsigned char v) {return (v&(v-1))==0;}
204 inline bool atMostOneBitIsSet(unsigned short v) {return (v&(v-1))==0;}
205 inline bool atMostOneBitIsSet(unsigned int v) {return (v&(v-1))==0;}
206 inline bool atMostOneBitIsSet(unsigned long v) {return (v&(v-1))==0;}
207 inline bool atMostOneBitIsSet(unsigned long long v) {return (v&(v-1))==0;}
208 inline bool atMostOneBitIsSet(signed char v) {return (v&(v-1))==0;}
209 inline bool atMostOneBitIsSet(char v) {return (v&(v-1))==0;}
210 inline bool atMostOneBitIsSet(short v) {return (v&(v-1))==0;}
211 inline bool atMostOneBitIsSet(int v) {return (v&(v-1))==0;}
212 inline bool atMostOneBitIsSet(long v) {return (v&(v-1))==0;}
213 inline bool atMostOneBitIsSet(long long v) {return (v&(v-1))==0;}
215 
230 inline bool exactlyOneBitIsSet(unsigned char v) {return v && atMostOneBitIsSet(v);}
231 inline bool exactlyOneBitIsSet(unsigned short v) {return v && atMostOneBitIsSet(v);}
232 inline bool exactlyOneBitIsSet(unsigned int v) {return v && atMostOneBitIsSet(v);}
233 inline bool exactlyOneBitIsSet(unsigned long v) {return v && atMostOneBitIsSet(v);}
234 inline bool exactlyOneBitIsSet(unsigned long long v) {return v && atMostOneBitIsSet(v);}
235 inline bool exactlyOneBitIsSet(signed char v) {return v && atMostOneBitIsSet(v);}
236 inline bool exactlyOneBitIsSet(char v) {return v && atMostOneBitIsSet(v);}
237 inline bool exactlyOneBitIsSet(short v) {return v && atMostOneBitIsSet(v);}
238 inline bool exactlyOneBitIsSet(int v) {return v && atMostOneBitIsSet(v);}
239 inline bool exactlyOneBitIsSet(long v) {return v && atMostOneBitIsSet(v);}
240 inline bool exactlyOneBitIsSet(long long v) {return v && atMostOneBitIsSet(v);}
242 
269 
270 inline bool signBit(unsigned char u) {return false;}
271 inline bool signBit(unsigned short u) {return false;}
272 inline bool signBit(unsigned int u) {return false;}
273 inline bool signBit(unsigned long u) {return false;}
274 inline bool signBit(unsigned long long u) {return false;}
275 
276 // Note that plain 'char' type is not overloaded -- see above.
277 
278 // We're assuming sizeof(char)==1, short==2, int==4, long long==8 which is safe
279 // for all our anticipated platforms. But some 64 bit implementations have
280 // sizeof(long)==4 while others have sizeof(long)==8 so we'll use the ANSI
281 // standard value LONG_MIN which is a long value with just the high bit set.
282 // (We're assuming two's complement integers here; I haven't seen anything
283 // else in decades.)
284 inline bool signBit(signed char i) {return ((unsigned char)i & 0x80U) != 0;}
285 inline bool signBit(short i) {return ((unsigned short)i & 0x8000U) != 0;}
286 inline bool signBit(int i) {return ((unsigned int)i & 0x80000000U) != 0;}
287 inline bool signBit(long long i) {return ((unsigned long long)i & 0x8000000000000000ULL) != 0;}
288 
289 inline bool signBit(long i) {return ((unsigned long)i
290  & (unsigned long)LONG_MIN) != 0;}
291 
292 inline bool signBit(const float& f) {return std::signbit(f);}
293 inline bool signBit(const double& d) {return std::signbit(d);}
294 inline bool signBit(const negator<float>& nf) {return std::signbit(-nf);} // !!
295 inline bool signBit(const negator<double>& nd) {return std::signbit(-nd);} // !!
296 
311 inline unsigned int sign(unsigned char u) {return u==0 ? 0 : 1;}
312 inline unsigned int sign(unsigned short u) {return u==0 ? 0 : 1;}
313 inline unsigned int sign(unsigned int u) {return u==0 ? 0 : 1;}
314 inline unsigned int sign(unsigned long u) {return u==0 ? 0 : 1;}
315 inline unsigned int sign(unsigned long long u) {return u==0 ? 0 : 1;}
316 
317 // Don't overload for plain "char" because it may be signed or unsigned
318 // depending on the compiler.
319 
320 inline int sign(signed char i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
321 inline int sign(short i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
322 inline int sign(int i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
323 inline int sign(long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
324 inline int sign(long long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
325 
326 inline int sign(const float& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
327 inline int sign(const double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
328 inline int sign(const long double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
329 
330 inline int sign(const negator<float>& x) {return -sign(-x);} // -x is free
331 inline int sign(const negator<double>& x) {return -sign(-x);}
332 inline int sign(const negator<long double>& x) {return -sign(-x);}
334 
351 inline unsigned char square(unsigned char u) {return u*u;}
352 inline unsigned short square(unsigned short u) {return u*u;}
353 inline unsigned int square(unsigned int u) {return u*u;}
354 inline unsigned long square(unsigned long u) {return u*u;}
355 inline unsigned long long square(unsigned long long u) {return u*u;}
356 
357 inline char square(char c) {return c*c;}
358 
359 inline signed char square(signed char i) {return i*i;}
360 inline short square(short i) {return i*i;}
361 inline int square(int i) {return i*i;}
362 inline long square(long i) {return i*i;}
363 inline long long square(long long i) {return i*i;}
364 
365 inline float square(const float& x) {return x*x;}
366 inline double square(const double& x) {return x*x;}
367 inline long double square(const long double& x) {return x*x;}
368 
369 // Negation is free for negators, so we can square them and clean
370 // them up at the same time at no extra cost.
371 inline float square(const negator<float>& x) {return square(-x);}
372 inline double square(const negator<double>& x) {return square(-x);}
373 inline long double square(const negator<long double>& x) {return square(-x);}
374 
375 // It is safer to templatize using complex classes, and doesn't make
376 // debugging any worse since complex is already templatized.
377 // 5 flops vs. 6 for general complex multiply.
378 template <class P> inline
379 std::complex<P> square(const std::complex<P>& x) {
380  const P re=x.real(), im=x.imag();
381  return std::complex<P>(re*re-im*im, 2*re*im);
382 }
383 
384 // We can square a conjugate and clean it up back to complex at
385 // the same time at zero cost (or maybe 1 negation depending
386 // on how the compiler handles the "-2" below).
387 template <class P> inline
388 std::complex<P> square(const conjugate<P>& x) {
389  const P re=x.real(), nim=x.negImag();
390  return std::complex<P>(re*re-nim*nim, -2*re*nim);
391 }
392 
393 template <class P> inline
394 std::complex<P> square(const negator< std::complex<P> >& x) {
395  return square(-x); // negation is free for negators
396 }
397 
398 // Note return type here after squaring negator<conjugate>
399 // is complex, not conjugate.
400 template <class P> inline
401 std::complex<P> square(const negator< conjugate<P> >& x) {
402  return square(-x); // negation is free for negators
403 }
405 
424 inline unsigned char cube(unsigned char u) {return u*u*u;}
425 inline unsigned short cube(unsigned short u) {return u*u*u;}
426 inline unsigned int cube(unsigned int u) {return u*u*u;}
427 inline unsigned long cube(unsigned long u) {return u*u*u;}
428 inline unsigned long long cube(unsigned long long u) {return u*u*u;}
429 
430 inline char cube(char c) {return c*c*c;}
431 
432 inline signed char cube(signed char i) {return i*i*i;}
433 inline short cube(short i) {return i*i*i;}
434 inline int cube(int i) {return i*i*i;}
435 inline long cube(long i) {return i*i*i;}
436 inline long long cube(long long i) {return i*i*i;}
437 
438 inline float cube(const float& x) {return x*x*x;}
439 inline double cube(const double& x) {return x*x*x;}
440 inline long double cube(const long double& x) {return x*x*x;}
441 
442 // To keep this cheap we'll defer getting rid of the negator<> until
443 // some other operation. We cube -x and then recast that to negator<>
444 // on return, for a total cost of 2 flops.
445 inline negator<float> cube(const negator<float>& x) {
446  return negator<float>::recast(cube(-x));
447 }
448 inline negator<double> cube(const negator<double>& x) {
449  return negator<double>::recast(cube(-x));
450 }
451 inline negator<long double> cube(const negator<long double>& x) {
452  return negator<long double>::recast(cube(-x));
453 }
454 
455 // Cubing a complex this way is cheaper than doing it by
456 // multiplication. Cost here is 8 flops vs. 11 for a square
457 // followed by a multiply.
458 template <class P> inline
459 std::complex<P> cube(const std::complex<P>& x) {
460  const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
461  return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
462 }
463 
464 // Cubing a negated complex allows us to cube and eliminate the
465 // negator at the same time for zero extra cost. Compare the
466 // expressions here to the normal cube above to see the free
467 // sign changes in both parts. 8 flops here.
468 template <class P> inline
469 std::complex<P> cube(const negator< std::complex<P> >& x) {
470  // -x is free for a negator
471  const P nre=(-x).real(), nim=(-x).imag(), rr=nre*nre, ii=nim*nim;
472  return std::complex<P>(nre*(3*ii-rr), nim*(ii-3*rr));
473 }
474 
475 // Cubing a conjugate this way saves a lot over multiplying, and
476 // also lets us convert the result to complex for free.
477 template <class P> inline
478 std::complex<P> cube(const conjugate<P>& x) {
479  const P re=x.real(), nim=x.negImag(), rr=re*re, ii=nim*nim;
480  return std::complex<P>(re*(rr-3*ii), nim*(ii-3*rr));
481 }
482 
483 
484 // Cubing a negated conjugate this way saves a lot over multiplying, and
485 // also lets us convert the result to non-negated complex for free.
486 template <class P> inline
487 std::complex<P> cube(const negator< conjugate<P> >& x) {
488  // -x is free for a negator
489  const P nre=(-x).real(), im=(-x).negImag(), rr=nre*nre, ii=im*im;
490  return std::complex<P>(nre*(3*ii-rr), im*(3*rr-ii));
491 }
493 
525 
541 inline double& clampInPlace(double low, double& v, double high)
542 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
544 inline float& clampInPlace(float low, float& v, float high)
545 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
547 inline long double& clampInPlace(long double low, long double& v, long double high)
548 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
549 
550  // Floating point clamps with integer bounds; without these
551  // explicit casts are required.
552 
555 inline double& clampInPlace(int low, double& v, int high)
556 { return clampInPlace((double)low,v,(double)high); }
559 inline float& clampInPlace(int low, float& v, int high)
560 { return clampInPlace((float)low,v,(float)high); }
563 inline long double& clampInPlace(int low, long double& v, int high)
564 { return clampInPlace((long double)low,v,(long double)high); }
565 
568 inline double& clampInPlace(int low, double& v, double high)
569 { return clampInPlace((double)low,v,high); }
572 inline float& clampInPlace(int low, float& v, float high)
573 { return clampInPlace((float)low,v,high); }
576 inline long double& clampInPlace(int low, long double& v, long double high)
577 { return clampInPlace((long double)low,v,high); }
578 
581 inline double& clampInPlace(double low, double& v, int high)
582 { return clampInPlace(low,v,(double)high); }
585 inline float& clampInPlace(float low, float& v, int high)
586 { return clampInPlace(low,v,(float)high); }
589 inline long double& clampInPlace(long double low, long double& v, int high)
590 { return clampInPlace(low,v,(long double)high); }
591 
593 inline unsigned char& clampInPlace(unsigned char low, unsigned char& v, unsigned char high)
594 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
596 inline unsigned short& clampInPlace(unsigned short low, unsigned short& v, unsigned short high)
597 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
599 inline unsigned int& clampInPlace(unsigned int low, unsigned int& v, unsigned int high)
600 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
602 inline unsigned long& clampInPlace(unsigned long low, unsigned long& v, unsigned long high)
603 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
605 inline unsigned long long& clampInPlace(unsigned long long low, unsigned long long& v, unsigned long long high)
606 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
607 
609 inline char& clampInPlace(char low, char& v, char high)
610 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
612 inline signed char& clampInPlace(signed char low, signed char& v, signed char high)
613 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
614 
616 inline short& clampInPlace(short low, short& v, short high)
617 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
619 inline int& clampInPlace(int low, int& v, int high)
620 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
622 inline long& clampInPlace(long low, long& v, long high)
623 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
625 inline long long& clampInPlace(long long low, long long& v, long long high)
626 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
627 
629 inline negator<float>& clampInPlace(float low, negator<float>& v, float high)
630 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
632 inline negator<double>& clampInPlace(double low, negator<double>& v, double high)
633 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
635 inline negator<long double>& clampInPlace(long double low, negator<long double>& v, long double high)
636 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
637 
638 
639 
660 inline double clamp(double low, double v, double high)
661 { return clampInPlace(low,v,high); }
663 inline float clamp(float low, float v, float high)
664 { return clampInPlace(low,v,high); }
666 inline long double clamp(long double low, long double v, long double high)
667 { return clampInPlace(low,v,high); }
668 
671 inline double clamp(int low, double v, int high)
672 { return clampInPlace(low,v,high); }
675 inline float clamp(int low, float v, int high)
676 { return clampInPlace(low,v,high); }
679 inline long double clamp(int low, long double v, int high)
680 { return clampInPlace(low,v,high); }
681 
684 inline double clamp(int low, double v, double high)
685 { return clampInPlace(low,v,high); }
688 inline float clamp(int low, float v, float high)
689 { return clampInPlace(low,v,high); }
692 inline long double clamp(int low, long double v, long double high)
693 { return clampInPlace(low,v,high); }
694 
697 inline double clamp(double low, double v, int high)
698 { return clampInPlace(low,v,high); }
701 inline float clamp(float low, float v, int high)
702 { return clampInPlace(low,v,high); }
705 inline long double clamp(long double low, long double v, int high)
706 { return clampInPlace(low,v,high); }
707 
709 inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high)
710 { return clampInPlace(low,v,high); }
712 inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high)
713 { return clampInPlace(low,v,high); }
715 inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high)
716 { return clampInPlace(low,v,high); }
718 inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high)
719 { return clampInPlace(low,v,high); }
721 inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high)
722 { return clampInPlace(low,v,high); }
723 
725 inline char clamp(char low, char v, char high)
726 { return clampInPlace(low,v,high); }
728 inline signed char clamp(signed char low, signed char v, signed char high)
729 { return clampInPlace(low,v,high); }
730 
732 inline short clamp(short low, short v, short high)
733 { return clampInPlace(low,v,high); }
735 inline int clamp(int low, int v, int high)
736 { return clampInPlace(low,v,high); }
738 inline long clamp(long low, long v, long high)
739 { return clampInPlace(low,v,high); }
741 inline long long clamp(long long low, long long v, long long high)
742 { return clampInPlace(low,v,high); }
743 
744 
745 // These aren't strictly necessary but are here to help the
746 // compiler find the right overload to call. These cost an
747 // extra flop because the negator<T> has to be cast to T which
748 // requires that the pending negation be performed. Note that
749 // the result types are not negated.
750 
755 inline float clamp(float low, negator<float> v, float high)
756 { return clamp(low,(float)v,high); }
761 inline double clamp(double low, negator<double> v, double high)
762 { return clamp(low,(double)v,high); }
767 inline long double clamp(long double low, negator<long double> v, long double high)
768 { return clamp(low,(long double)v,high); }
809 
824 inline double stepUp(double x)
825 { assert(0 <= x && x <= 1);
826  return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
827 
828 
843 inline double stepDown(double x) {return 1.0 -stepUp(x);}
844 
845 
920 inline double stepAny(double y0, double yRange,
921  double x0, double oneOverXRange,
922  double x)
923 { double xadj = (x-x0)*oneOverXRange;
924  assert(-NTraits<double>::getSignificant() <= xadj
925  && xadj <= 1 + NTraits<double>::getSignificant());
926  clampInPlace(0.0,xadj,1.0);
927  return y0 + yRange*stepUp(xadj); }
928 
932 inline double dstepUp(double x) {
933  assert(0 <= x && x <= 1);
934  const double xxm1=x*(x-1);
935  return 30*xxm1*xxm1; //30x^2-60x^3+30x^4
936 }
937 
941 inline double dstepDown(double x) {return -dstepUp(x);}
942 
946 inline double dstepAny(double yRange,
947  double x0, double oneOverXRange,
948  double x)
949 { double xadj = (x-x0)*oneOverXRange;
950  assert(-NTraits<double>::getSignificant() <= xadj
951  && xadj <= 1 + NTraits<double>::getSignificant());
952  clampInPlace(0.0,xadj,1.0);
953  return yRange*oneOverXRange*dstepUp(xadj); }
954 
958 inline double d2stepUp(double x) {
959  assert(0 <= x && x <= 1);
960  return 60*x*(1+x*(2*x-3)); //60x-180x^2+120x^3
961 }
962 
966 inline double d2stepDown(double x) {return -d2stepUp(x);}
967 
971 inline double d2stepAny(double yRange,
972  double x0, double oneOverXRange,
973  double x)
974 { double xadj = (x-x0)*oneOverXRange;
975  assert(-NTraits<double>::getSignificant() <= xadj
976  && xadj <= 1 + NTraits<double>::getSignificant());
977  clampInPlace(0.0,xadj,1.0);
978  return yRange*square(oneOverXRange)*d2stepUp(xadj); }
979 
983 inline double d3stepUp(double x) {
984  assert(0 <= x && x <= 1);
985  return 60+360*x*(x-1); //60-360*x+360*x^2
986 }
987 
991 inline double d3stepDown(double x) {return -d3stepUp(x);}
992 
996 inline double d3stepAny(double yRange,
997  double x0, double oneOverXRange,
998  double x)
999 { double xadj = (x-x0)*oneOverXRange;
1000  assert(-NTraits<double>::getSignificant() <= xadj
1001  && xadj <= 1 + NTraits<double>::getSignificant());
1002  clampInPlace(0.0,xadj,1.0);
1003  return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
1004 
1005  // float
1006 
1008 inline float stepUp(float x)
1009 { assert(0 <= x && x <= 1);
1010  return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
1012 inline float stepDown(float x) {return 1.0f-stepUp(x);}
1014 inline float stepAny(float y0, float yRange,
1015  float x0, float oneOverXRange,
1016  float x)
1017 { float xadj = (x-x0)*oneOverXRange;
1018  assert(-NTraits<float>::getSignificant() <= xadj
1019  && xadj <= 1 + NTraits<float>::getSignificant());
1020  clampInPlace(0.0f,xadj,1.0f);
1021  return y0 + yRange*stepUp(xadj); }
1022 
1024 inline float dstepUp(float x)
1025 { assert(0 <= x && x <= 1);
1026  const float xxm1=x*(x-1);
1027  return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
1029 inline float dstepDown(float x) {return -dstepUp(x);}
1031 inline float dstepAny(float yRange,
1032  float x0, float oneOverXRange,
1033  float x)
1034 { float xadj = (x-x0)*oneOverXRange;
1035  assert(-NTraits<float>::getSignificant() <= xadj
1036  && xadj <= 1 + NTraits<float>::getSignificant());
1037  clampInPlace(0.0f,xadj,1.0f);
1038  return yRange*oneOverXRange*dstepUp(xadj); }
1039 
1041 inline float d2stepUp(float x)
1042 { assert(0 <= x && x <= 1);
1043  return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
1045 inline float d2stepDown(float x) {return -d2stepUp(x);}
1047 inline float d2stepAny(float yRange,
1048  float x0, float oneOverXRange,
1049  float x)
1050 { float xadj = (x-x0)*oneOverXRange;
1051  assert(-NTraits<float>::getSignificant() <= xadj
1052  && xadj <= 1 + NTraits<float>::getSignificant());
1053  clampInPlace(0.0f,xadj,1.0f);
1054  return yRange*square(oneOverXRange)*d2stepUp(xadj); }
1055 
1057 inline float d3stepUp(float x)
1058 { assert(0 <= x && x <= 1);
1059  return 60+360*x*(x-1); } //60-360*x+360*x^2
1061 inline float d3stepDown(float x) {return -d3stepUp(x);}
1063 inline float d3stepAny(float yRange,
1064  float x0, float oneOverXRange,
1065  float x)
1066 { float xadj = (x-x0)*oneOverXRange;
1067  assert(-NTraits<float>::getSignificant() <= xadj
1068  && xadj <= 1 + NTraits<float>::getSignificant());
1069  clampInPlace(0.0f,xadj,1.0f);
1070  return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
1071 
1072  // long double
1073 
1075 inline long double stepUp(long double x)
1076 { assert(0 <= x && x <= 1);
1077  return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
1079 inline long double stepDown(long double x) {return 1.0L-stepUp(x);}
1081 inline long double stepAny(long double y0, long double yRange,
1082  long double x0, long double oneOverXRange,
1083  long double x)
1084 { long double xadj = (x-x0)*oneOverXRange;
1085  assert(-NTraits<long double>::getSignificant() <= xadj
1086  && xadj <= 1 + NTraits<long double>::getSignificant());
1087  clampInPlace(0.0L,xadj,1.0L);
1088  return y0 + yRange*stepUp(xadj); }
1089 
1090 
1092 inline long double dstepUp(long double x)
1093 { assert(0 <= x && x <= 1);
1094  const long double xxm1=x*(x-1);
1095  return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
1097 inline long double dstepDown(long double x) {return -dstepUp(x);}
1099 inline long double dstepAny(long double yRange,
1100  long double x0, long double oneOverXRange,
1101  long double x)
1102 { long double xadj = (x-x0)*oneOverXRange;
1103  assert(-NTraits<long double>::getSignificant() <= xadj
1104  && xadj <= 1 + NTraits<long double>::getSignificant());
1105  clampInPlace(0.0L,xadj,1.0L);
1106  return yRange*oneOverXRange*dstepUp(xadj); }
1107 
1109 inline long double d2stepUp(long double x)
1110 { assert(0 <= x && x <= 1);
1111  return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
1113 inline long double d2stepDown(long double x) {return -d2stepUp(x);}
1115 inline long double d2stepAny(long double yRange,
1116  long double x0, long double oneOverXRange,
1117  long double x)
1118 { long double xadj = (x-x0)*oneOverXRange;
1119  assert(-NTraits<long double>::getSignificant() <= xadj
1120  && xadj <= 1 + NTraits<long double>::getSignificant());
1121  clampInPlace(0.0L,xadj,1.0L);
1122  return yRange*square(oneOverXRange)*d2stepUp(xadj); }
1123 
1124 
1126 inline long double d3stepUp(long double x)
1127 { assert(0 <= x && x <= 1);
1128  return 60+360*x*(x-1); } //60-360*x+360*x^2
1130 inline long double d3stepDown(long double x) {return -d3stepUp(x);}
1132 inline long double d3stepAny(long double yRange,
1133  long double x0, long double oneOverXRange,
1134  long double x)
1135 { long double xadj = (x-x0)*oneOverXRange;
1136  assert(-NTraits<long double>::getSignificant() <= xadj
1137  && xadj <= 1 + NTraits<long double>::getSignificant());
1138  clampInPlace(0.0L,xadj,1.0L);
1139  return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
1140 
1141  // int converts to double; only supplied for stepUp(), stepDown()
1144 inline double stepUp(int x) {return stepUp((double)x);}
1147 inline double stepDown(int x) {return stepDown((double)x);}
1148 
1149 
1208  // Doxygen should skip this helper template function
1210 template <class T> // float or double
1211 static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
1212  static const T a[] =
1213  { (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
1214  (T)0.03742563713, (T)0.01451196212 };
1215  static const T b[] =
1216  { (T)0.5, (T)0.12498593597, (T)0.06880248576,
1217  (T)0.03328355346, (T)0.00441787012 };
1218  static const T c[] =
1219  { (T)1, (T)0.44325141463, (T)0.06260601220,
1220  (T)0.04757383546, (T)0.01736506451 };
1221  static const T d[] =
1222  { (T)0, (T)0.24998368310, (T)0.09200180037,
1223  (T)0.04069697526, (T)0.00526449639 };
1224 
1225  const T SignificantReal = NTraits<T>::getSignificant();
1226  const T PiOver2 = NTraits<T>::getPi()/2;
1227  const T Infinity = NTraits<T>::getInfinity();
1228 
1229  SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1230  "approxCompleteEllipticIntegralsKE()",
1231  "Argument m (%g) is outside the legal range [0,1].", (double)m);
1232  if (m >= 1) return std::make_pair(Infinity, (T)1);
1233  if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1234 
1235  const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2; // 4 flops
1236  const T lnm2 = std::log(m1); // ~50 flops
1237 
1238  // The rest is 35 flops.
1239  const T K = (a[0] + a[1]*m1 + a[2]*m2 + a[3]*m3 + a[4]*m4)
1240  - lnm2*(b[0] + b[1]*m1 + b[2]*m2 + b[3]*m3 + b[4]*m4);
1241  const T E = (c[0] + c[1]*m1 + c[2]*m2 + c[3]*m3 + c[4]*m4)
1242  - lnm2*( d[1]*m1 + d[2]*m2 + d[3]*m3 + d[4]*m4);
1243 
1244  return std::make_pair(K,E);
1245 }
1284 inline std::pair<double,double>
1285 approxCompleteEllipticIntegralsKE(double m)
1286 { return approxCompleteEllipticIntegralsKE_T<double>(m); }
1292 inline std::pair<float,float>
1293 approxCompleteEllipticIntegralsKE(float m)
1294 { return approxCompleteEllipticIntegralsKE_T<float>(m); }
1301 inline std::pair<double,double>
1302 approxCompleteEllipticIntegralsKE(int m)
1303 { return approxCompleteEllipticIntegralsKE_T<double>((double)m); }
1304 
1305  // Doxygen should skip this template helper function
1307 template <class T> // float or double
1308 static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
1309  const T SignificantReal = NTraits<T>::getSignificant();
1310  const T TenEps = 10*NTraits<T>::getEps();
1311  const T Infinity = NTraits<T>::getInfinity();
1312  const T PiOver2 = NTraits<T>::getPi() / 2;
1313 
1314  // Allow a little slop in the argument since it may have resulted
1315  // from a numerical operation that gave 0 or 1 plus or minus
1316  // roundoff noise.
1317  SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1318  "completeEllipticIntegralsKE()",
1319  "Argument m (%g) is outside the legal range [0,1].", (double)m);
1320  if (m >= 1) return std::make_pair(Infinity, (T)1);
1321  if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1322 
1323  const T k = std::sqrt(1-m); // ~25 flops
1324  T v1=1, w1=k, c1=1, d1=k*k; // initialize iteration
1325  do { // ~50 flops per iteration
1326  T v2 = (v1+w1)/2;
1327  T w2 = std::sqrt(v1*w1);
1328  T c2 = (c1+d1)/2;
1329  T d2 = (w1*c1+v1*d1)/(v1+w1);
1330  v1=v2; w1=w2; c1=c2; d1=d2;
1331  } while(std::abs(v1-w1) >= TenEps);
1332 
1333  const T K = PiOver2/v1; // ~20 flops
1334  const T E = K*c1;
1335 
1336  return std::make_pair(K,E);
1337 }
1364 inline std::pair<double,double> completeEllipticIntegralsKE(double m)
1365 { return completeEllipticIntegralsKE_T<double>(m); }
1373 inline std::pair<float,float> completeEllipticIntegralsKE(float m)
1374 { return completeEllipticIntegralsKE_T<float>(m); }
1381 inline std::pair<double,double> completeEllipticIntegralsKE(int m)
1382 { return completeEllipticIntegralsKE_T<double>((double)m); }
1383 
1386 } // namespace SimTK
1387 
1388 #endif //SimTK_SIMMATRIX_SCALAR_H_