Simbody  3.8
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;
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 // Don't remove these unused formal parameter names 'u'; doxygen barfs.
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 
329 inline int sign(const negator<float>& x) {return -sign(-x);} // -x is free
330 inline int sign(const negator<double>& x) {return -sign(-x);}
332 
349 inline unsigned char square(unsigned char u) {return u*u;}
350 inline unsigned short square(unsigned short u) {return u*u;}
351 inline unsigned int square(unsigned int u) {return u*u;}
352 inline unsigned long square(unsigned long u) {return u*u;}
353 inline unsigned long long square(unsigned long long u) {return u*u;}
354 
355 inline char square(char c) {return c*c;}
356 
357 inline signed char square(signed char i) {return i*i;}
358 inline short square(short i) {return i*i;}
359 inline int square(int i) {return i*i;}
360 inline long square(long i) {return i*i;}
361 inline long long square(long long i) {return i*i;}
362 
363 inline float square(const float& x) {return x*x;}
364 inline double square(const double& x) {return x*x;}
365 
366 // Negation is free for negators, so we can square them and clean
367 // them up at the same time at no extra cost.
368 inline float square(const negator<float>& x) {return square(-x);}
369 inline double square(const negator<double>& x) {return square(-x);}
370 
371 // It is safer to templatize using complex classes, and doesn't make
372 // debugging any worse since complex is already templatized.
373 // 5 flops vs. 6 for general complex multiply.
374 template <class P> inline
375 std::complex<P> square(const std::complex<P>& x) {
376  const P re=x.real(), im=x.imag();
377  return std::complex<P>(re*re-im*im, 2*re*im);
378 }
379 
380 // We can square a conjugate and clean it up back to complex at
381 // the same time at zero cost (or maybe 1 negation depending
382 // on how the compiler handles the "-2" below).
383 template <class P> inline
384 std::complex<P> square(const conjugate<P>& x) {
385  const P re=x.real(), nim=x.negImag();
386  return std::complex<P>(re*re-nim*nim, -2*re*nim);
387 }
388 
389 template <class P> inline
390 std::complex<P> square(const negator< std::complex<P> >& x) {
391  return square(-x); // negation is free for negators
392 }
393 
394 // Note return type here after squaring negator<conjugate>
395 // is complex, not conjugate.
396 template <class P> inline
397 std::complex<P> square(const negator< conjugate<P> >& x) {
398  return square(-x); // negation is free for negators
399 }
401 
420 inline unsigned char cube(unsigned char u) {return u*u*u;}
421 inline unsigned short cube(unsigned short u) {return u*u*u;}
422 inline unsigned int cube(unsigned int u) {return u*u*u;}
423 inline unsigned long cube(unsigned long u) {return u*u*u;}
424 inline unsigned long long cube(unsigned long long u) {return u*u*u;}
425 
426 inline char cube(char c) {return c*c*c;}
427 
428 inline signed char cube(signed char i) {return i*i*i;}
429 inline short cube(short i) {return i*i*i;}
430 inline int cube(int i) {return i*i*i;}
431 inline long cube(long i) {return i*i*i;}
432 inline long long cube(long long i) {return i*i*i;}
433 
434 inline float cube(const float& x) {return x*x*x;}
435 inline double cube(const double& x) {return x*x*x;}
436 
437 // To keep this cheap we'll defer getting rid of the negator<> until
438 // some other operation. We cube -x and then recast that to negator<>
439 // on return, for a total cost of 2 flops.
440 inline negator<float> cube(const negator<float>& x) {
441  return negator<float>::recast(cube(-x));
442 }
443 inline negator<double> cube(const negator<double>& x) {
444  return negator<double>::recast(cube(-x));
445 }
446 
447 // Cubing a complex this way is cheaper than doing it by
448 // multiplication. Cost here is 8 flops vs. 11 for a square
449 // followed by a multiply.
450 template <class P> inline
451 std::complex<P> cube(const std::complex<P>& x) {
452  const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
453  return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
454 }
455 
456 // Cubing a negated complex allows us to cube and eliminate the
457 // negator at the same time for zero extra cost. Compare the
458 // expressions here to the normal cube above to see the free
459 // sign changes in both parts. 8 flops here.
460 template <class P> inline
461 std::complex<P> cube(const negator< std::complex<P> >& x) {
462  // -x is free for a negator
463  const P nre=(-x).real(), nim=(-x).imag(), rr=nre*nre, ii=nim*nim;
464  return std::complex<P>(nre*(3*ii-rr), nim*(ii-3*rr));
465 }
466 
467 // Cubing a conjugate this way saves a lot over multiplying, and
468 // also lets us convert the result to complex for free.
469 template <class P> inline
470 std::complex<P> cube(const conjugate<P>& x) {
471  const P re=x.real(), nim=x.negImag(), rr=re*re, ii=nim*nim;
472  return std::complex<P>(re*(rr-3*ii), nim*(ii-3*rr));
473 }
474 
475 
476 // Cubing a negated conjugate this way saves a lot over multiplying, and
477 // also lets us convert the result to non-negated complex for free.
478 template <class P> inline
479 std::complex<P> cube(const negator< conjugate<P> >& x) {
480  // -x is free for a negator
481  const P nre=(-x).real(), im=(-x).negImag(), rr=nre*nre, ii=im*im;
482  return std::complex<P>(nre*(3*ii-rr), im*(3*rr-ii));
483 }
485 
517 
533 inline double& clampInPlace(double low, double& v, double high)
534 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
536 inline float& clampInPlace(float low, float& v, float high)
537 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
538 
539  // Floating point clamps with integer bounds; without these
540  // explicit casts are required.
541 
544 inline double& clampInPlace(int low, double& v, int high)
545 { return clampInPlace((double)low,v,(double)high); }
548 inline float& clampInPlace(int low, float& v, int high)
549 { return clampInPlace((float)low,v,(float)high); }
550 
553 inline double& clampInPlace(int low, double& v, double high)
554 { return clampInPlace((double)low,v,high); }
557 inline float& clampInPlace(int low, float& v, float high)
558 { return clampInPlace((float)low,v,high); }
559 
562 inline double& clampInPlace(double low, double& v, int high)
563 { return clampInPlace(low,v,(double)high); }
566 inline float& clampInPlace(float low, float& v, int high)
567 { return clampInPlace(low,v,(float)high); }
568 
570 inline unsigned char& clampInPlace(unsigned char low, unsigned char& v, unsigned char high)
571 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
573 inline unsigned short& clampInPlace(unsigned short low, unsigned short& v, unsigned short high)
574 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
576 inline unsigned int& clampInPlace(unsigned int low, unsigned int& v, unsigned int high)
577 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
579 inline unsigned long& clampInPlace(unsigned long low, unsigned long& v, unsigned long high)
580 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
582 inline unsigned long long& clampInPlace(unsigned long long low, unsigned long long& v, unsigned long long high)
583 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
584 
586 inline char& clampInPlace(char low, char& v, char high)
587 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
589 inline signed char& clampInPlace(signed char low, signed char& v, signed char high)
590 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
591 
593 inline short& clampInPlace(short low, short& v, short high)
594 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
596 inline int& clampInPlace(int low, int& v, int high)
597 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
599 inline long& clampInPlace(long low, long& v, long high)
600 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
602 inline long long& clampInPlace(long long low, long long& v, long long high)
603 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
604 
606 inline negator<float>& clampInPlace(float low, negator<float>& v, float high)
607 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
609 inline negator<double>& clampInPlace(double low, negator<double>& v, double high)
610 { assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
611 
612 
613 
634 inline double clamp(double low, double v, double high)
635 { return clampInPlace(low,v,high); }
637 inline float clamp(float low, float v, float high)
638 { return clampInPlace(low,v,high); }
639 
642 inline double clamp(int low, double v, int high)
643 { return clampInPlace(low,v,high); }
646 inline float clamp(int low, float v, int high)
647 { return clampInPlace(low,v,high); }
648 
651 inline double clamp(int low, double v, double high)
652 { return clampInPlace(low,v,high); }
655 inline float clamp(int low, float v, float high)
656 { return clampInPlace(low,v,high); }
657 
660 inline double clamp(double low, double v, int high)
661 { return clampInPlace(low,v,high); }
664 inline float clamp(float low, float v, int high)
665 { return clampInPlace(low,v,high); }
666 
668 inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high)
669 { return clampInPlace(low,v,high); }
671 inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high)
672 { return clampInPlace(low,v,high); }
674 inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high)
675 { return clampInPlace(low,v,high); }
677 inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high)
678 { return clampInPlace(low,v,high); }
680 inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high)
681 { return clampInPlace(low,v,high); }
682 
684 inline char clamp(char low, char v, char high)
685 { return clampInPlace(low,v,high); }
687 inline signed char clamp(signed char low, signed char v, signed char high)
688 { return clampInPlace(low,v,high); }
689 
691 inline short clamp(short low, short v, short high)
692 { return clampInPlace(low,v,high); }
694 inline int clamp(int low, int v, int high)
695 { return clampInPlace(low,v,high); }
697 inline long clamp(long low, long v, long high)
698 { return clampInPlace(low,v,high); }
700 inline long long clamp(long long low, long long v, long long high)
701 { return clampInPlace(low,v,high); }
702 
703 
704 // These aren't strictly necessary but are here to help the
705 // compiler find the right overload to call. These cost an
706 // extra flop because the negator<T> has to be cast to T which
707 // requires that the pending negation be performed. Note that
708 // the result types are not negated.
709 
714 inline float clamp(float low, negator<float> v, float high)
715 { return clamp(low,(float)v,high); }
720 inline double clamp(double low, negator<double> v, double high)
721 { return clamp(low,(double)v,high); }
762 
777 inline double stepUp(double x)
778 { assert(0 <= x && x <= 1);
779  return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
780 
781 
796 inline double stepDown(double x) {return 1.0 -stepUp(x);}
797 
798 
873 inline double stepAny(double y0, double yRange,
874  double x0, double oneOverXRange,
875  double x)
876 { double xadj = (x-x0)*oneOverXRange;
877  assert(-NTraits<double>::getSignificant() <= xadj
878  && xadj <= 1 + NTraits<double>::getSignificant());
879  clampInPlace(0.0,xadj,1.0);
880  return y0 + yRange*stepUp(xadj); }
881 
885 inline double dstepUp(double x) {
886  assert(0 <= x && x <= 1);
887  const double xxm1=x*(x-1);
888  return 30*xxm1*xxm1; //30x^2-60x^3+30x^4
889 }
890 
894 inline double dstepDown(double x) {return -dstepUp(x);}
895 
899 inline double dstepAny(double yRange,
900  double x0, double oneOverXRange,
901  double x)
902 { double xadj = (x-x0)*oneOverXRange;
903  assert(-NTraits<double>::getSignificant() <= xadj
904  && xadj <= 1 + NTraits<double>::getSignificant());
905  clampInPlace(0.0,xadj,1.0);
906  return yRange*oneOverXRange*dstepUp(xadj); }
907 
911 inline double d2stepUp(double x) {
912  assert(0 <= x && x <= 1);
913  return 60*x*(1+x*(2*x-3)); //60x-180x^2+120x^3
914 }
915 
919 inline double d2stepDown(double x) {return -d2stepUp(x);}
920 
924 inline double d2stepAny(double yRange,
925  double x0, double oneOverXRange,
926  double x)
927 { double xadj = (x-x0)*oneOverXRange;
928  assert(-NTraits<double>::getSignificant() <= xadj
929  && xadj <= 1 + NTraits<double>::getSignificant());
930  clampInPlace(0.0,xadj,1.0);
931  return yRange*square(oneOverXRange)*d2stepUp(xadj); }
932 
936 inline double d3stepUp(double x) {
937  assert(0 <= x && x <= 1);
938  return 60+360*x*(x-1); //60-360*x+360*x^2
939 }
940 
944 inline double d3stepDown(double x) {return -d3stepUp(x);}
945 
949 inline double d3stepAny(double yRange,
950  double x0, double oneOverXRange,
951  double x)
952 { double xadj = (x-x0)*oneOverXRange;
953  assert(-NTraits<double>::getSignificant() <= xadj
954  && xadj <= 1 + NTraits<double>::getSignificant());
955  clampInPlace(0.0,xadj,1.0);
956  return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
957 
958  // float
959 
961 inline float stepUp(float x)
962 { assert(0 <= x && x <= 1);
963  return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
965 inline float stepDown(float x) {return 1.0f-stepUp(x);}
967 inline float stepAny(float y0, float yRange,
968  float x0, float oneOverXRange,
969  float x)
970 { float xadj = (x-x0)*oneOverXRange;
971  assert(-NTraits<float>::getSignificant() <= xadj
972  && xadj <= 1 + NTraits<float>::getSignificant());
973  clampInPlace(0.0f,xadj,1.0f);
974  return y0 + yRange*stepUp(xadj); }
975 
977 inline float dstepUp(float x)
978 { assert(0 <= x && x <= 1);
979  const float xxm1=x*(x-1);
980  return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
982 inline float dstepDown(float x) {return -dstepUp(x);}
984 inline float dstepAny(float yRange,
985  float x0, float oneOverXRange,
986  float x)
987 { float xadj = (x-x0)*oneOverXRange;
988  assert(-NTraits<float>::getSignificant() <= xadj
989  && xadj <= 1 + NTraits<float>::getSignificant());
990  clampInPlace(0.0f,xadj,1.0f);
991  return yRange*oneOverXRange*dstepUp(xadj); }
992 
994 inline float d2stepUp(float x)
995 { assert(0 <= x && x <= 1);
996  return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
998 inline float d2stepDown(float x) {return -d2stepUp(x);}
1000 inline float d2stepAny(float yRange,
1001  float x0, float oneOverXRange,
1002  float x)
1003 { float xadj = (x-x0)*oneOverXRange;
1004  assert(-NTraits<float>::getSignificant() <= xadj
1005  && xadj <= 1 + NTraits<float>::getSignificant());
1006  clampInPlace(0.0f,xadj,1.0f);
1007  return yRange*square(oneOverXRange)*d2stepUp(xadj); }
1008 
1010 inline float d3stepUp(float x)
1011 { assert(0 <= x && x <= 1);
1012  return 60+360*x*(x-1); } //60-360*x+360*x^2
1014 inline float d3stepDown(float x) {return -d3stepUp(x);}
1016 inline float d3stepAny(float yRange,
1017  float x0, float oneOverXRange,
1018  float x)
1019 { float xadj = (x-x0)*oneOverXRange;
1020  assert(-NTraits<float>::getSignificant() <= xadj
1021  && xadj <= 1 + NTraits<float>::getSignificant());
1022  clampInPlace(0.0f,xadj,1.0f);
1023  return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
1024 
1025  // int converts to double; only supplied for stepUp(), stepDown()
1028 inline double stepUp(int x) {return stepUp((double)x);}
1031 inline double stepDown(int x) {return stepDown((double)x);}
1032 
1033 
1092  // Doxygen should skip this helper template function
1094 template <class T> // float or double
1095 static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
1096  static const T a[] =
1097  { (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
1098  (T)0.03742563713, (T)0.01451196212 };
1099  static const T b[] =
1100  { (T)0.5, (T)0.12498593597, (T)0.06880248576,
1101  (T)0.03328355346, (T)0.00441787012 };
1102  static const T c[] =
1103  { (T)1, (T)0.44325141463, (T)0.06260601220,
1104  (T)0.04757383546, (T)0.01736506451 };
1105  static const T d[] =
1106  { (T)0, (T)0.24998368310, (T)0.09200180037,
1107  (T)0.04069697526, (T)0.00526449639 };
1108 
1109  const T SignificantReal = NTraits<T>::getSignificant();
1110  const T PiOver2 = NTraits<T>::getPi()/2;
1111  const T Infinity = NTraits<T>::getInfinity();
1112 
1113  SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1114  "approxCompleteEllipticIntegralsKE()",
1115  "Argument m (%g) is outside the legal range [0,1].", (double)m);
1116  if (m >= 1) return std::make_pair(Infinity, (T)1);
1117  if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1118 
1119  const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2; // 4 flops
1120  const T lnm2 = std::log(m1); // ~50 flops
1121 
1122  // The rest is 35 flops.
1123  const T K = (a[0] + a[1]*m1 + a[2]*m2 + a[3]*m3 + a[4]*m4)
1124  - lnm2*(b[0] + b[1]*m1 + b[2]*m2 + b[3]*m3 + b[4]*m4);
1125  const T E = (c[0] + c[1]*m1 + c[2]*m2 + c[3]*m3 + c[4]*m4)
1126  - lnm2*( d[1]*m1 + d[2]*m2 + d[3]*m3 + d[4]*m4);
1127 
1128  return std::make_pair(K,E);
1129 }
1168 inline std::pair<double,double>
1169 approxCompleteEllipticIntegralsKE(double m)
1170 { return approxCompleteEllipticIntegralsKE_T<double>(m); }
1176 inline std::pair<float,float>
1177 approxCompleteEllipticIntegralsKE(float m)
1178 { return approxCompleteEllipticIntegralsKE_T<float>(m); }
1185 inline std::pair<double,double>
1186 approxCompleteEllipticIntegralsKE(int m)
1187 { return approxCompleteEllipticIntegralsKE_T<double>((double)m); }
1188 
1189  // Doxygen should skip this template helper function
1191 template <class T> // float or double
1192 static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
1193  const T SignificantReal = NTraits<T>::getSignificant();
1194  const T TenEps = 10*NTraits<T>::getEps();
1195  const T Infinity = NTraits<T>::getInfinity();
1196  const T PiOver2 = NTraits<T>::getPi() / 2;
1197 
1198  // Allow a little slop in the argument since it may have resulted
1199  // from a numerical operation that gave 0 or 1 plus or minus
1200  // roundoff noise.
1201  SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1202  "completeEllipticIntegralsKE()",
1203  "Argument m (%g) is outside the legal range [0,1].", (double)m);
1204  if (m >= 1) return std::make_pair(Infinity, (T)1);
1205  if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1206 
1207  const T k = std::sqrt(1-m); // ~25 flops
1208  T v1=1, w1=k, c1=1, d1=k*k; // initialize iteration
1209  do { // ~50 flops per iteration
1210  T v2 = (v1+w1)/2;
1211  T w2 = std::sqrt(v1*w1);
1212  T c2 = (c1+d1)/2;
1213  T d2 = (w1*c1+v1*d1)/(v1+w1);
1214  v1=v2; w1=w2; c1=c2; d1=d2;
1215  } while(std::abs(v1-w1) >= TenEps);
1216 
1217  const T K = PiOver2/v1; // ~20 flops
1218  const T E = K*c1;
1219 
1220  return std::make_pair(K,E);
1221 }
1248 inline std::pair<double,double> completeEllipticIntegralsKE(double m)
1249 { return completeEllipticIntegralsKE_T<double>(m); }
1257 inline std::pair<float,float> completeEllipticIntegralsKE(float m)
1258 { return completeEllipticIntegralsKE_T<float>(m); }
1265 inline std::pair<double,double> completeEllipticIntegralsKE(int m)
1266 { return completeEllipticIntegralsKE_T<double>((double)m); }
1267 
1270 } // namespace SimTK
1271 
1272 #endif //SimTK_SIMMATRIX_SCALAR_H_
Array.h
This file defines the Array_<T,X> class and related support classes including base classes ArrayViewC...
CompositeNumericalTypes.h
The purpose of the CNT<T> class is to hide the differences between built-in numerical types and compo...
Constants.h
High precision mathematical and physical constants.
ExceptionMacros.h
This file contains macros which are convenient to use for sprinkling error checking around liberally ...
SimTK_ERRCHK1_ALWAYS
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
NTraits.h
This file contains classes and typedefs needed to provide uniform handling of floating point numeric ...
common.h
Mandatory first inclusion for any Simbody source or header file.
SimTK_SimTKCOMMON_EXPORT
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
SimTK::NTraits
Definition: NTraits.h:436
SimTK::conjugate
SimTK::conjugate<R> should be instantiated only for float, double.
Definition: conjugate.h:178
SimTK::negator
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: negator.h:75
SimTK::negator::recast
static const negator< N > & recast(const N &val)
Definition: negator.h:235
conjugate.h
This file defines the conjugate<R> template class, where R is one of the three built-in real types.
SimTK::MinusOne
const Real MinusOne
Real(-1)
SimTK::SqrtEps
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
SimTK::Sqrt3
const Real Sqrt3
Real(sqrt(3))
SimTK::OneOverPi
const Real OneOverPi
1/Real(pi)
SimTK::Log2E
const Real Log2E
Real(log2(e)) (log base 2)
SimTK::Infinity
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
SimTK::SignificantReal
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
SimTK::OneSixth
const Real OneSixth
Real(1)/6.
SimTK::Zero
const Real Zero
Real(0)
SimTK::OneFourth
const Real OneFourth
Real(1)/4.
SimTK::OneNinth
const Real OneNinth
Real(1)/9.
SimTK::CubeRoot3
const Real CubeRoot3
Real(3^(1/3)) (cube root of 3)
SimTK::Two
const Real Two
Real(2)
SimTK::OneThird
const Real OneThird
Real(1)/3.
SimTK::NaN
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
SimTK::LosslessNumDigitsReal
const int LosslessNumDigitsReal
This is the smallest number of decimal digits you should store in a text file if you want to be able ...
Definition: Scalar.h:142
SimTK::E
const Real E
e = Real(exp(1))
SimTK::One
const Real One
Real(1)
SimTK::OneEighth
const Real OneEighth
Real(1)/8.
SimTK::Sqrt2
const Real Sqrt2
Real(sqrt(2))
SimTK::I
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
SimTK::LeastNegativeReal
const Real LeastNegativeReal
This is the largest negative real number (that is, closest to zero) that can be expressed in values o...
SimTK::CubeRoot2
const Real CubeRoot2
Real(2^(1/3)) (cube root of 2)
SimTK::OneFifth
const Real OneFifth
Real(1)/5.
SimTK::MostPositiveReal
const Real MostPositiveReal
This is the largest finite positive real number that can be expressed in the Real type; ~1e+308 when ...
SimTK::OneHalf
const Real OneHalf
Real(1)/2.
SimTK::Ln2
const Real Ln2
Real(ln(2)) (natural log of 2)
SimTK::OneOverSqrt3
const Real OneOverSqrt3
Real(1/sqrt(3))
SimTK::Three
const Real Three
Real(3)
SimTK::Ln10
const Real Ln10
Real(ln(10)) (natural log of 10)
SimTK::OneSeventh
const Real OneSeventh
Real(1)/7.
SimTK::NumDigitsReal
const int NumDigitsReal
This is the number of decimal digits that can be reliably stored and retrieved in the default Real pr...
SimTK::MostNegativeReal
const Real MostNegativeReal
This is the smallest finite negative real number that can be expressed in values of type Real....
SimTK::Pi
const Real Pi
Real(pi)
SimTK::OneOverSqrt2
const Real OneOverSqrt2
1/sqrt(2)==sqrt(2)/2 as Real
SimTK::Eps
const Real Eps
Epsilon is the size of roundoff noise; it is the smallest positive number of default-precision type R...
SimTK::Log10E
const Real Log10E
Real(log10(e)) (log base 10)
SimTK::TinyReal
const Real TinyReal
TinyReal is a floating point value smaller than the floating point precision; it is defined as Eps^(5...
SimTK::LeastPositiveReal
const Real LeastPositiveReal
This is the smallest positive real number that can be expressed in the type Real; it is ~1e-308 when ...
SimTK
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
SimTK::stepUp
double stepUp(double x)
Interpolate smoothly from 0 up to 1 as the input argument goes from 0 to 1, with first and second der...
Definition: Scalar.h:777
SimTK::Complex
std::complex< Real > Complex
This is the default complex type for SimTK, with precision for the real and imaginary parts set to th...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:610
SimTK::approxCompleteEllipticIntegralsKE
std::pair< double, double > approxCompleteEllipticIntegralsKE(double m)
Given 0<=m<=1, return complete elliptic integrals of the first and second kinds, K(m) and E(m),...
Definition: Scalar.h:1169
SimTK::stepDown
double stepDown(double x)
Interpolate smoothly from 1 down to 0 as the input argument goes from 0 to 1, with first and second d...
Definition: Scalar.h:796
SimTK::d3stepAny
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition: Scalar.h:949
SimTK::clampInPlace
double & clampInPlace(double low, double &v, double high)
Check that low <= v <= high and modify v in place if necessary to bring it into that range.
Definition: Scalar.h:533
SimTK::d2stepDown
double d2stepDown(double x)
Second derivative of stepDown(): d^2/dx^2 stepDown(x).
Definition: Scalar.h:919
SimTK::dstepAny
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition: Scalar.h:899
SimTK::d2stepUp
double d2stepUp(double x)
Second derivative of stepUp(): d^2/dx^2 stepUp(x).
Definition: Scalar.h:911
SimTK::dstepDown
double dstepDown(double x)
First derivative of stepDown(): d/dx stepDown(x).
Definition: Scalar.h:894
SimTK::abs
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
SimTK::dstepUp
double dstepUp(double x)
First derivative of stepUp(): d/dx stepUp(x).
Definition: Scalar.h:885
SimTK::d2stepAny
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition: Scalar.h:924
SimTK::sign
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
SimTK::completeEllipticIntegralsKE
std::pair< double, double > completeEllipticIntegralsKE(double m)
Given 0<=m<=1, return complete elliptic integrals of the first and second kinds, K(m) and E(m),...
Definition: Scalar.h:1248
SimTK::d3stepDown
double d3stepDown(double x)
Third derivative of stepDown(): d^3/dx^3 stepDown(x).
Definition: Scalar.h:944
SimTK::Conjugate
conjugate< Real > Conjugate
Definition: Scalar.h:57
SimTK::atMostOneBitIsSet
bool atMostOneBitIsSet(unsigned char v)
Definition: Scalar.h:203
SimTK::imag
const negator< float > & imag(const conjugate< float > &c)
Definition: conjugate.h:483
SimTK::square
unsigned char square(unsigned char u)
Definition: Scalar.h:349
SimTK::stepAny
double stepAny(double y0, double yRange, double x0, double oneOverXRange, double x)
Interpolate smoothly from y0 to y1 as the input argument goes from x0 to x1, with first and second de...
Definition: Scalar.h:873
SimTK::exactlyOneBitIsSet
bool exactlyOneBitIsSet(unsigned char v)
Definition: Scalar.h:230
SimTK::Real
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
SimTK::clamp
double clamp(double low, double v, double high)
If v is in range low <= v <= high then return v, otherwise return the nearest bound; this function do...
Definition: Scalar.h:634
SimTK::real
const float & real(const conjugate< float > &c)
Definition: conjugate.h:482
SimTK::d3stepUp
double d3stepUp(double x)
Third derivative of stepUp(): d^3/dx^3 stepUp(x).
Definition: Scalar.h:936
SimTK::signBit
bool signBit(unsigned char u)
Definition: Scalar.h:270
SimTK::cube
unsigned char cube(unsigned char u)
Definition: Scalar.h:420
negator.h
This file defines the negator<N> template which is an adaptor for the numeric types N (Real,...