Simbody 3.9
Loading...
Searching...
No Matches
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
38#include "SimTKcommon/internal/conjugate.h"
39#include "SimTKcommon/internal/CompositeNumericalTypes.h"
40#include "SimTKcommon/internal/NTraits.h"
41#include "SimTKcommon/internal/negator.h"
42
43#include <complex>
44#include <cmath>
45#include <climits>
46#include <cassert>
47#include <utility>
48
49namespace SimTK {
50
52 // Handy default-precision definitions //
54
55typedef conjugate<Real> Conjugate; // like Complex
56
85inline constexpr Real NaN = NTraits<Real>::getNaN();
89inline constexpr Real Infinity = NTraits<Real>::getInfinity();
90
94inline constexpr Real Eps = NTraits<Real>::getEps();
98inline constexpr Real SqrtEps = NTraits<Real>::getSqrtEps();
104inline constexpr Real TinyReal = NTraits<Real>::getTiny();
109inline constexpr Real SignificantReal = NTraits<Real>::getSignificant();
110
113inline constexpr Real LeastPositiveReal = NTraits<Real>::getLeastPositive();
117inline constexpr Real MostPositiveReal = NTraits<Real>::getMostPositive();
120inline constexpr Real LeastNegativeReal = NTraits<Real>::getLeastNegative();
124inline constexpr Real MostNegativeReal = NTraits<Real>::getMostNegative();
125
129inline constexpr int NumDigitsReal = NTraits<Real>::getNumDigits();
130
136inline constexpr int LosslessNumDigitsReal = NTraits<Real>::getLosslessNumDigits(); // double ~20, float ~9
137
138inline constexpr Real Zero = NTraits<Real>::getZero();
139inline constexpr Real One = NTraits<Real>::getOne();
140inline constexpr Real MinusOne = NTraits<Real>::getMinusOne();
141inline constexpr Real Two = NTraits<Real>::getTwo();
142inline constexpr Real Three = NTraits<Real>::getThree();
143
144inline constexpr Real OneHalf = NTraits<Real>::getOneHalf();
145inline constexpr Real OneThird = NTraits<Real>::getOneThird();
146inline constexpr Real OneFourth = NTraits<Real>::getOneFourth();
147inline constexpr Real OneFifth = NTraits<Real>::getOneFifth();
148inline constexpr Real OneSixth = NTraits<Real>::getOneSixth();
149inline constexpr Real OneSeventh = NTraits<Real>::getOneSeventh();
150inline constexpr Real OneEighth = NTraits<Real>::getOneEighth();
151inline constexpr Real OneNinth = NTraits<Real>::getOneNinth();
152inline constexpr Real Pi = NTraits<Real>::getPi();
153inline constexpr Real OneOverPi = NTraits<Real>::getOneOverPi();
154inline constexpr Real E = NTraits<Real>::getE();
155inline constexpr Real Log2E = NTraits<Real>::getLog2E();
156inline constexpr Real Log10E = NTraits<Real>::getLog10E();
157inline constexpr Real Sqrt2 = NTraits<Real>::getSqrt2();
158inline constexpr Real OneOverSqrt2 = NTraits<Real>::getOneOverSqrt2();
159inline constexpr Real Sqrt3 = NTraits<Real>::getSqrt3();
160inline constexpr Real OneOverSqrt3 = NTraits<Real>::getOneOverSqrt3();
161inline constexpr Real CubeRoot2 = NTraits<Real>::getCubeRoot2();
162inline constexpr Real CubeRoot3 = NTraits<Real>::getCubeRoot3();
163inline constexpr Real Ln2 = NTraits<Real>::getLn2();
164inline constexpr Real Ln10 = NTraits<Real>::getLn10();
165
170inline constexpr Complex I = NTraits<Complex>::getI();
174 // SOME SCALAR UTILITIES //
176
177
191// We use the trick that v & v-1 returns the value that is v with its
192// rightmost bit cleared (if it has a rightmost bit set).
193inline bool atMostOneBitIsSet(unsigned char v) {return (v&(v-1))==0;}
194inline bool atMostOneBitIsSet(unsigned short v) {return (v&(v-1))==0;}
195inline bool atMostOneBitIsSet(unsigned int v) {return (v&(v-1))==0;}
196inline bool atMostOneBitIsSet(unsigned long v) {return (v&(v-1))==0;}
197inline bool atMostOneBitIsSet(unsigned long long v) {return (v&(v-1))==0;}
198inline bool atMostOneBitIsSet(signed char v) {return (v&(v-1))==0;}
199inline bool atMostOneBitIsSet(char v) {return (v&(v-1))==0;}
200inline bool atMostOneBitIsSet(short v) {return (v&(v-1))==0;}
201inline bool atMostOneBitIsSet(int v) {return (v&(v-1))==0;}
202inline bool atMostOneBitIsSet(long v) {return (v&(v-1))==0;}
203inline bool atMostOneBitIsSet(long long v) {return (v&(v-1))==0;}
205
220inline bool exactlyOneBitIsSet(unsigned char v) {return v && atMostOneBitIsSet(v);}
221inline bool exactlyOneBitIsSet(unsigned short v) {return v && atMostOneBitIsSet(v);}
222inline bool exactlyOneBitIsSet(unsigned int v) {return v && atMostOneBitIsSet(v);}
223inline bool exactlyOneBitIsSet(unsigned long v) {return v && atMostOneBitIsSet(v);}
224inline bool exactlyOneBitIsSet(unsigned long long v) {return v && atMostOneBitIsSet(v);}
225inline bool exactlyOneBitIsSet(signed char v) {return v && atMostOneBitIsSet(v);}
226inline bool exactlyOneBitIsSet(char v) {return v && atMostOneBitIsSet(v);}
227inline bool exactlyOneBitIsSet(short v) {return v && atMostOneBitIsSet(v);}
228inline bool exactlyOneBitIsSet(int v) {return v && atMostOneBitIsSet(v);}
229inline bool exactlyOneBitIsSet(long v) {return v && atMostOneBitIsSet(v);}
230inline bool exactlyOneBitIsSet(long long v) {return v && atMostOneBitIsSet(v);}
232
259// Don't remove these unused formal parameter names 'u'; doxygen barfs.
260inline bool signBit(unsigned char u) {return false;}
261inline bool signBit(unsigned short u) {return false;}
262inline bool signBit(unsigned int u) {return false;}
263inline bool signBit(unsigned long u) {return false;}
264inline bool signBit(unsigned long long u) {return false;}
265
266// Note that plain 'char' type is not overloaded -- see above.
267
268// We're assuming sizeof(char)==1, short==2, int==4, long long==8 which is safe
269// for all our anticipated platforms. But some 64 bit implementations have
270// sizeof(long)==4 while others have sizeof(long)==8 so we'll use the ANSI
271// standard value LONG_MIN which is a long value with just the high bit set.
272// (We're assuming two's complement integers here; I haven't seen anything
273// else in decades.)
274inline bool signBit(signed char i) {return ((unsigned char)i & 0x80U) != 0;}
275inline bool signBit(short i) {return ((unsigned short)i & 0x8000U) != 0;}
276inline bool signBit(int i) {return ((unsigned int)i & 0x80000000U) != 0;}
277inline bool signBit(long long i) {return ((unsigned long long)i & 0x8000000000000000ULL) != 0;}
278
279inline bool signBit(long i) {return ((unsigned long)i
280 & (unsigned long)LONG_MIN) != 0;}
281
282inline bool signBit(const float& f) {return std::signbit(f);}
283inline bool signBit(const double& d) {return std::signbit(d);}
284inline bool signBit(const negator<float>& nf) {return std::signbit(-nf);} // !!
285inline bool signBit(const negator<double>& nd) {return std::signbit(-nd);} // !!
286
301inline unsigned int sign(unsigned char u) {return u==0 ? 0 : 1;}
302inline unsigned int sign(unsigned short u) {return u==0 ? 0 : 1;}
303inline unsigned int sign(unsigned int u) {return u==0 ? 0 : 1;}
304inline unsigned int sign(unsigned long u) {return u==0 ? 0 : 1;}
305inline unsigned int sign(unsigned long long u) {return u==0 ? 0 : 1;}
306
307// Don't overload for plain "char" because it may be signed or unsigned
308// depending on the compiler.
309
310inline int sign(signed char i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
311inline int sign(short i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
312inline int sign(int i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
313inline int sign(long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
314inline int sign(long long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
315
316inline int sign(const float& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
317inline int sign(const double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
318
319inline int sign(const negator<float>& x) {return -sign(-x);} // -x is free
320inline int sign(const negator<double>& x) {return -sign(-x);}
322
339inline unsigned char square(unsigned char u) {return u*u;}
340inline unsigned short square(unsigned short u) {return u*u;}
341inline unsigned int square(unsigned int u) {return u*u;}
342inline unsigned long square(unsigned long u) {return u*u;}
343inline unsigned long long square(unsigned long long u) {return u*u;}
344
345inline char square(char c) {return c*c;}
346
347inline signed char square(signed char i) {return i*i;}
348inline short square(short i) {return i*i;}
349inline int square(int i) {return i*i;}
350inline long square(long i) {return i*i;}
351inline long long square(long long i) {return i*i;}
352
353inline float square(const float& x) {return x*x;}
354inline double square(const double& x) {return x*x;}
355
356// Negation is free for negators, so we can square them and clean
357// them up at the same time at no extra cost.
358inline float square(const negator<float>& x) {return square(-x);}
359inline double square(const negator<double>& x) {return square(-x);}
360
361// It is safer to templatize using complex classes, and doesn't make
362// debugging any worse since complex is already templatized.
363// 5 flops vs. 6 for general complex multiply.
364template <class P> inline
365std::complex<P> square(const std::complex<P>& x) {
366 const P re=x.real(), im=x.imag();
367 return std::complex<P>(re*re-im*im, 2*re*im);
368}
369
370// We can square a conjugate and clean it up back to complex at
371// the same time at zero cost (or maybe 1 negation depending
372// on how the compiler handles the "-2" below).
373template <class P> inline
374std::complex<P> square(const conjugate<P>& x) {
375 const P re=x.real(), nim=x.negImag();
376 return std::complex<P>(re*re-nim*nim, -2*re*nim);
377}
378
379template <class P> inline
380std::complex<P> square(const negator< std::complex<P> >& x) {
381 return square(-x); // negation is free for negators
382}
383
384// Note return type here after squaring negator<conjugate>
385// is complex, not conjugate.
386template <class P> inline
387std::complex<P> square(const negator< conjugate<P> >& x) {
388 return square(-x); // negation is free for negators
389}
391
410inline unsigned char cube(unsigned char u) {return u*u*u;}
411inline unsigned short cube(unsigned short u) {return u*u*u;}
412inline unsigned int cube(unsigned int u) {return u*u*u;}
413inline unsigned long cube(unsigned long u) {return u*u*u;}
414inline unsigned long long cube(unsigned long long u) {return u*u*u;}
415
416inline char cube(char c) {return c*c*c;}
417
418inline signed char cube(signed char i) {return i*i*i;}
419inline short cube(short i) {return i*i*i;}
420inline int cube(int i) {return i*i*i;}
421inline long cube(long i) {return i*i*i;}
422inline long long cube(long long i) {return i*i*i;}
423
424inline float cube(const float& x) {return x*x*x;}
425inline double cube(const double& x) {return x*x*x;}
426
427// To keep this cheap we'll defer getting rid of the negator<> until
428// some other operation. We cube -x and then recast that to negator<>
429// on return, for a total cost of 2 flops.
430inline negator<float> cube(const negator<float>& x) {
431 return negator<float>::recast(cube(-x));
432}
433inline negator<double> cube(const negator<double>& x) {
434 return negator<double>::recast(cube(-x));
435}
436
437// Cubing a complex this way is cheaper than doing it by
438// multiplication. Cost here is 8 flops vs. 11 for a square
439// followed by a multiply.
440template <class P> inline
441std::complex<P> cube(const std::complex<P>& x) {
442 const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
443 return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
444}
445
446// Cubing a negated complex allows us to cube and eliminate the
447// negator at the same time for zero extra cost. Compare the
448// expressions here to the normal cube above to see the free
449// sign changes in both parts. 8 flops here.
450template <class P> inline
451std::complex<P> cube(const negator< std::complex<P> >& x) {
452 // -x is free for a negator
453 const P nre=(-x).real(), nim=(-x).imag(), rr=nre*nre, ii=nim*nim;
454 return std::complex<P>(nre*(3*ii-rr), nim*(ii-3*rr));
455}
456
457// Cubing a conjugate this way saves a lot over multiplying, and
458// also lets us convert the result to complex for free.
459template <class P> inline
460std::complex<P> cube(const conjugate<P>& x) {
461 const P re=x.real(), nim=x.negImag(), rr=re*re, ii=nim*nim;
462 return std::complex<P>(re*(rr-3*ii), nim*(ii-3*rr));
463}
464
465
466// Cubing a negated conjugate this way saves a lot over multiplying, and
467// also lets us convert the result to non-negated complex for free.
468template <class P> inline
469std::complex<P> cube(const negator< conjugate<P> >& x) {
470 // -x is free for a negator
471 const P nre=(-x).real(), im=(-x).negImag(), rr=nre*nre, ii=im*im;
472 return std::complex<P>(nre*(3*ii-rr), im*(3*rr-ii));
473}
475
507
523inline double& clampInPlace(double low, double& v, double high)
524{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
526inline float& clampInPlace(float low, float& v, float high)
527{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
528
529 // Floating point clamps with integer bounds; without these
530 // explicit casts are required.
531
534inline double& clampInPlace(int low, double& v, int high)
535{ return clampInPlace((double)low,v,(double)high); }
538inline float& clampInPlace(int low, float& v, int high)
539{ return clampInPlace((float)low,v,(float)high); }
540
543inline double& clampInPlace(int low, double& v, double high)
544{ return clampInPlace((double)low,v,high); }
547inline float& clampInPlace(int low, float& v, float high)
548{ return clampInPlace((float)low,v,high); }
549
552inline double& clampInPlace(double low, double& v, int high)
553{ return clampInPlace(low,v,(double)high); }
556inline float& clampInPlace(float low, float& v, int high)
557{ return clampInPlace(low,v,(float)high); }
558
560inline unsigned char& clampInPlace(unsigned char low, unsigned char& v, unsigned char high)
561{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
563inline unsigned short& clampInPlace(unsigned short low, unsigned short& v, unsigned short high)
564{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
566inline unsigned int& clampInPlace(unsigned int low, unsigned int& v, unsigned int high)
567{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
569inline unsigned long& clampInPlace(unsigned long low, unsigned long& v, unsigned long high)
570{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
572inline unsigned long long& clampInPlace(unsigned long long low, unsigned long long& v, unsigned long long high)
573{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
574
576inline char& clampInPlace(char low, char& v, char high)
577{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
579inline signed char& clampInPlace(signed char low, signed char& v, signed char high)
580{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
581
583inline short& clampInPlace(short low, short& v, short high)
584{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
586inline int& clampInPlace(int low, int& v, int high)
587{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
589inline long& clampInPlace(long low, long& v, long high)
590{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
592inline long long& clampInPlace(long long low, long long& v, long long high)
593{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
594
596inline negator<float>& clampInPlace(float low, negator<float>& v, float high)
597{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
599inline negator<double>& clampInPlace(double low, negator<double>& v, double high)
600{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
601
602
603
624inline double clamp(double low, double v, double high)
625{ return clampInPlace(low,v,high); }
627inline float clamp(float low, float v, float high)
628{ return clampInPlace(low,v,high); }
629
632inline double clamp(int low, double v, int high)
633{ return clampInPlace(low,v,high); }
636inline float clamp(int low, float v, int high)
637{ return clampInPlace(low,v,high); }
638
641inline double clamp(int low, double v, double high)
642{ return clampInPlace(low,v,high); }
645inline float clamp(int low, float v, float high)
646{ return clampInPlace(low,v,high); }
647
650inline double clamp(double low, double v, int high)
651{ return clampInPlace(low,v,high); }
654inline float clamp(float low, float v, int high)
655{ return clampInPlace(low,v,high); }
656
658inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high)
659{ return clampInPlace(low,v,high); }
661inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high)
662{ return clampInPlace(low,v,high); }
664inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high)
665{ return clampInPlace(low,v,high); }
667inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high)
668{ return clampInPlace(low,v,high); }
670inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high)
671{ return clampInPlace(low,v,high); }
672
674inline char clamp(char low, char v, char high)
675{ return clampInPlace(low,v,high); }
677inline signed char clamp(signed char low, signed char v, signed char high)
678{ return clampInPlace(low,v,high); }
679
681inline short clamp(short low, short v, short high)
682{ return clampInPlace(low,v,high); }
684inline int clamp(int low, int v, int high)
685{ return clampInPlace(low,v,high); }
687inline long clamp(long low, long v, long high)
688{ return clampInPlace(low,v,high); }
690inline long long clamp(long long low, long long v, long long high)
691{ return clampInPlace(low,v,high); }
692
693
694// These aren't strictly necessary but are here to help the
695// compiler find the right overload to call. These cost an
696// extra flop because the negator<T> has to be cast to T which
697// requires that the pending negation be performed. Note that
698// the result types are not negated.
699
704inline float clamp(float low, negator<float> v, float high)
705{ return clamp(low,(float)v,high); }
710inline double clamp(double low, negator<double> v, double high)
711{ return clamp(low,(double)v,high); }
752
767inline double stepUp(double x)
768{ assert(0 <= x && x <= 1);
769 return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
770
771
786inline double stepDown(double x) {return 1.0 -stepUp(x);}
787
788
863inline double stepAny(double y0, double yRange,
864 double x0, double oneOverXRange,
865 double x)
866{ double xadj = (x-x0)*oneOverXRange;
867 assert(-NTraits<double>::getSignificant() <= xadj
868 && xadj <= 1 + NTraits<double>::getSignificant());
869 clampInPlace(0.0,xadj,1.0);
870 return y0 + yRange*stepUp(xadj); }
871
875inline double dstepUp(double x) {
876 assert(0 <= x && x <= 1);
877 const double xxm1=x*(x-1);
878 return 30*xxm1*xxm1; //30x^2-60x^3+30x^4
879}
880
884inline double dstepDown(double x) {return -dstepUp(x);}
885
889inline double dstepAny(double yRange,
890 double x0, double oneOverXRange,
891 double x)
892{ double xadj = (x-x0)*oneOverXRange;
893 assert(-NTraits<double>::getSignificant() <= xadj
894 && xadj <= 1 + NTraits<double>::getSignificant());
895 clampInPlace(0.0,xadj,1.0);
896 return yRange*oneOverXRange*dstepUp(xadj); }
897
901inline double d2stepUp(double x) {
902 assert(0 <= x && x <= 1);
903 return 60*x*(1+x*(2*x-3)); //60x-180x^2+120x^3
904}
905
909inline double d2stepDown(double x) {return -d2stepUp(x);}
910
914inline double d2stepAny(double yRange,
915 double x0, double oneOverXRange,
916 double x)
917{ double xadj = (x-x0)*oneOverXRange;
918 assert(-NTraits<double>::getSignificant() <= xadj
919 && xadj <= 1 + NTraits<double>::getSignificant());
920 clampInPlace(0.0,xadj,1.0);
921 return yRange*square(oneOverXRange)*d2stepUp(xadj); }
922
926inline double d3stepUp(double x) {
927 assert(0 <= x && x <= 1);
928 return 60+360*x*(x-1); //60-360*x+360*x^2
929}
930
934inline double d3stepDown(double x) {return -d3stepUp(x);}
935
939inline double d3stepAny(double yRange,
940 double x0, double oneOverXRange,
941 double x)
942{ double xadj = (x-x0)*oneOverXRange;
943 assert(-NTraits<double>::getSignificant() <= xadj
944 && xadj <= 1 + NTraits<double>::getSignificant());
945 clampInPlace(0.0,xadj,1.0);
946 return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
947
948 // float
949
951inline float stepUp(float x)
952{ assert(0 <= x && x <= 1);
953 return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
955inline float stepDown(float x) {return 1.0f-stepUp(x);}
957inline float stepAny(float y0, float yRange,
958 float x0, float oneOverXRange,
959 float x)
960{ float xadj = (x-x0)*oneOverXRange;
961 assert(-NTraits<float>::getSignificant() <= xadj
962 && xadj <= 1 + NTraits<float>::getSignificant());
963 clampInPlace(0.0f,xadj,1.0f);
964 return y0 + yRange*stepUp(xadj); }
965
967inline float dstepUp(float x)
968{ assert(0 <= x && x <= 1);
969 const float xxm1=x*(x-1);
970 return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
972inline float dstepDown(float x) {return -dstepUp(x);}
974inline float dstepAny(float yRange,
975 float x0, float oneOverXRange,
976 float x)
977{ float xadj = (x-x0)*oneOverXRange;
978 assert(-NTraits<float>::getSignificant() <= xadj
979 && xadj <= 1 + NTraits<float>::getSignificant());
980 clampInPlace(0.0f,xadj,1.0f);
981 return yRange*oneOverXRange*dstepUp(xadj); }
982
984inline float d2stepUp(float x)
985{ assert(0 <= x && x <= 1);
986 return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
988inline float d2stepDown(float x) {return -d2stepUp(x);}
990inline float d2stepAny(float yRange,
991 float x0, float oneOverXRange,
992 float x)
993{ float xadj = (x-x0)*oneOverXRange;
994 assert(-NTraits<float>::getSignificant() <= xadj
995 && xadj <= 1 + NTraits<float>::getSignificant());
996 clampInPlace(0.0f,xadj,1.0f);
997 return yRange*square(oneOverXRange)*d2stepUp(xadj); }
998
1000inline float d3stepUp(float x)
1001{ assert(0 <= x && x <= 1);
1002 return 60+360*x*(x-1); } //60-360*x+360*x^2
1004inline float d3stepDown(float x) {return -d3stepUp(x);}
1006inline float d3stepAny(float yRange,
1007 float x0, float oneOverXRange,
1008 float x)
1009{ float xadj = (x-x0)*oneOverXRange;
1010 assert(-NTraits<float>::getSignificant() <= xadj
1011 && xadj <= 1 + NTraits<float>::getSignificant());
1012 clampInPlace(0.0f,xadj,1.0f);
1013 return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
1014
1015 // int converts to double; only supplied for stepUp(), stepDown()
1018inline double stepUp(int x) {return stepUp((double)x);}
1021inline double stepDown(int x) {return stepDown((double)x);}
1022
1023
1082
// Doxygen should skip this helper template function
1084template <class T> // float or double
1085static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
1086 static const T a[] =
1087 { (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
1088 (T)0.03742563713, (T)0.01451196212 };
1089 static const T b[] =
1090 { (T)0.5, (T)0.12498593597, (T)0.06880248576,
1091 (T)0.03328355346, (T)0.00441787012 };
1092 static const T c[] =
1093 { (T)1, (T)0.44325141463, (T)0.06260601220,
1094 (T)0.04757383546, (T)0.01736506451 };
1095 static const T d[] =
1096 { (T)0, (T)0.24998368310, (T)0.09200180037,
1097 (T)0.04069697526, (T)0.00526449639 };
1098
1099 const T SignificantReal = NTraits<T>::getSignificant();
1100 const T PiOver2 = NTraits<T>::getPi()/2;
1101 const T Infinity = NTraits<T>::getInfinity();
1102
1103 SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1104 "approxCompleteEllipticIntegralsKE()",
1105 "Argument m (%g) is outside the legal range [0,1].", (double)m);
1106 if (m >= 1) return std::make_pair(Infinity, (T)1);
1107 if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1108
1109 const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2; // 4 flops
1110 const T lnm2 = std::log(m1); // ~50 flops
1111
1112 // The rest is 35 flops.
1113 const T K = (a[0] + a[1]*m1 + a[2]*m2 + a[3]*m3 + a[4]*m4)
1114 - lnm2*(b[0] + b[1]*m1 + b[2]*m2 + b[3]*m3 + b[4]*m4);
1115 const T E = (c[0] + c[1]*m1 + c[2]*m2 + c[3]*m3 + c[4]*m4)
1116 - lnm2*( d[1]*m1 + d[2]*m2 + d[3]*m3 + d[4]*m4);
1117
1118 return std::make_pair(K,E);
1119}
1158inline std::pair<double,double>
1159approxCompleteEllipticIntegralsKE(double m)
1160{ return approxCompleteEllipticIntegralsKE_T<double>(m); }
1166inline std::pair<float,float>
1167approxCompleteEllipticIntegralsKE(float m)
1168{ return approxCompleteEllipticIntegralsKE_T<float>(m); }
1175inline std::pair<double,double>
1176approxCompleteEllipticIntegralsKE(int m)
1177{ return approxCompleteEllipticIntegralsKE_T<double>((double)m); }
1178
1179
// Doxygen should skip this template helper function
1181template <class T> // float or double
1182static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
1183 const T SignificantReal = NTraits<T>::getSignificant();
1184 const T TenEps = 10*NTraits<T>::getEps();
1185 const T Infinity = NTraits<T>::getInfinity();
1186 const T PiOver2 = NTraits<T>::getPi() / 2;
1187
1188 // Allow a little slop in the argument since it may have resulted
1189 // from a numerical operation that gave 0 or 1 plus or minus
1190 // roundoff noise.
1191 SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
1192 "completeEllipticIntegralsKE()",
1193 "Argument m (%g) is outside the legal range [0,1].", (double)m);
1194 if (m >= 1) return std::make_pair(Infinity, (T)1);
1195 if (m <= 0) return std::make_pair(PiOver2, PiOver2);
1196
1197 const T k = std::sqrt(1-m); // ~25 flops
1198 T v1=1, w1=k, c1=1, d1=k*k; // initialize iteration
1199 do { // ~50 flops per iteration
1200 T v2 = (v1+w1)/2;
1201 T w2 = std::sqrt(v1*w1);
1202 T c2 = (c1+d1)/2;
1203 T d2 = (w1*c1+v1*d1)/(v1+w1);
1204 v1=v2; w1=w2; c1=c2; d1=d2;
1205 } while(std::abs(v1-w1) >= TenEps);
1206
1207 const T K = PiOver2/v1; // ~20 flops
1208 const T E = K*c1;
1209
1210 return std::make_pair(K,E);
1211}
1238inline std::pair<double,double> completeEllipticIntegralsKE(double m)
1239{ return completeEllipticIntegralsKE_T<double>(m); }
1247inline std::pair<float,float> completeEllipticIntegralsKE(float m)
1248{ return completeEllipticIntegralsKE_T<float>(m); }
1255inline std::pair<double,double> completeEllipticIntegralsKE(int m)
1256{ return completeEllipticIntegralsKE_T<double>((double)m); }
1257
1260} // namespace SimTK
1261
1262#endif //SimTK_SIMMATRIX_SCALAR_H_
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::NTraits
Definition NTraits.h:486
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::Sqrt2
constexpr Real Sqrt2
Real(sqrt(2))
Definition Scalar.h:157
SimTK::TinyReal
constexpr Real TinyReal
TinyReal is a floating point value smaller than the floating point precision; it is defined as Eps^(5...
Definition Scalar.h:104
SimTK::LeastPositiveReal
constexpr Real LeastPositiveReal
This is the smallest positive real number that can be expressed in the type Real; it is ~1e-308 when ...
Definition Scalar.h:113
SimTK::Eps
constexpr Real Eps
Epsilon is the size of roundoff noise; it is the smallest positive number of default-precision type R...
Definition Scalar.h:94
SimTK::Log2E
constexpr Real Log2E
Real(log2(e)) (log base 2)
Definition Scalar.h:155
SimTK::NaN
constexpr Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Definition Scalar.h:85
SimTK::LeastNegativeReal
constexpr Real LeastNegativeReal
This is the largest negative real number (that is, closest to zero) that can be expressed in values o...
Definition Scalar.h:120
SimTK::I
constexpr Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
Definition Scalar.h:170
SimTK::MinusOne
constexpr Real MinusOne
Real(-1)
Definition Scalar.h:140
SimTK::MostPositiveReal
constexpr Real MostPositiveReal
This is the largest finite positive real number that can be expressed in the Real type; ~1e+308 when ...
Definition Scalar.h:117
SimTK::OneOverPi
constexpr Real OneOverPi
1/Real(pi)
Definition Scalar.h:153
SimTK::OneNinth
constexpr Real OneNinth
Real(1)/9.
Definition Scalar.h:151
SimTK::Zero
constexpr Real Zero
Real(0)
Definition Scalar.h:138
SimTK::Log10E
constexpr Real Log10E
Real(log10(e)) (log base 10)
Definition Scalar.h:156
SimTK::LosslessNumDigitsReal
constexpr 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:136
SimTK::OneOverSqrt3
constexpr Real OneOverSqrt3
Real(1/sqrt(3))
Definition Scalar.h:160
SimTK::OneThird
constexpr Real OneThird
Real(1)/3.
Definition Scalar.h:145
SimTK::Pi
constexpr Real Pi
Real(pi)
Definition Scalar.h:152
SimTK::Ln2
constexpr Real Ln2
Real(ln(2)) (natural log of 2)
Definition Scalar.h:163
SimTK::One
constexpr Real One
Real(1)
Definition Scalar.h:139
SimTK::SignificantReal
constexpr Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
Definition Scalar.h:109
SimTK::MostNegativeReal
constexpr Real MostNegativeReal
This is the smallest finite negative real number that can be expressed in values of type Real....
Definition Scalar.h:124
SimTK::SqrtEps
constexpr Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
Definition Scalar.h:98
SimTK::Three
constexpr Real Three
Real(3)
Definition Scalar.h:142
SimTK::Ln10
constexpr Real Ln10
Real(ln(10)) (natural log of 10)
Definition Scalar.h:164
SimTK::CubeRoot3
constexpr Real CubeRoot3
Real(3^(1/3)) (cube root of 3)
Definition Scalar.h:162
SimTK::NumDigitsReal
constexpr int NumDigitsReal
This is the number of decimal digits that can be reliably stored and retrieved in the default Real pr...
Definition Scalar.h:129
SimTK::CubeRoot2
constexpr Real CubeRoot2
Real(2^(1/3)) (cube root of 2)
Definition Scalar.h:161
SimTK::E
constexpr Real E
e = Real(exp(1))
Definition Scalar.h:154
SimTK::OneHalf
constexpr Real OneHalf
Real(1)/2.
Definition Scalar.h:144
SimTK::OneSixth
constexpr Real OneSixth
Real(1)/6.
Definition Scalar.h:148
SimTK::OneOverSqrt2
constexpr Real OneOverSqrt2
1/sqrt(2)==sqrt(2)/2 as Real
Definition Scalar.h:158
SimTK::OneFourth
constexpr Real OneFourth
Real(1)/4.
Definition Scalar.h:146
SimTK::OneFifth
constexpr Real OneFifth
Real(1)/5.
Definition Scalar.h:147
SimTK::Sqrt3
constexpr Real Sqrt3
Real(sqrt(3))
Definition Scalar.h:159
SimTK::Infinity
constexpr Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
Definition Scalar.h:89
SimTK::OneSeventh
constexpr Real OneSeventh
Real(1)/7.
Definition Scalar.h:149
SimTK::OneEighth
constexpr Real OneEighth
Real(1)/8.
Definition Scalar.h:150
SimTK::Two
constexpr Real Two
Real(2)
Definition Scalar.h:141
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:767
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:629
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:786
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:939
SimTK::d2stepDown
double d2stepDown(double x)
Second derivative of stepDown(): d^2/dx^2 stepDown(x).
Definition Scalar.h:909
SimTK::dstepAny
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition Scalar.h:889
SimTK::d2stepUp
double d2stepUp(double x)
Second derivative of stepUp(): d^2/dx^2 stepUp(x).
Definition Scalar.h:901
SimTK::dstepDown
double dstepDown(double x)
First derivative of stepDown(): d/dx stepDown(x).
Definition Scalar.h:884
SimTK::dstepUp
double dstepUp(double x)
First derivative of stepUp(): d/dx stepUp(x).
Definition Scalar.h:875
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:523
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:914
SimTK::sign
unsigned int sign(unsigned char u)
Definition Scalar.h:301
SimTK::d3stepDown
double d3stepDown(double x)
Third derivative of stepDown(): d^3/dx^3 stepDown(x).
Definition Scalar.h:934
SimTK::real
const float & real(const conjugate< float > &c)
Definition conjugate.h:482
SimTK::Conjugate
conjugate< Real > Conjugate
Definition Scalar.h:55
SimTK::atMostOneBitIsSet
bool atMostOneBitIsSet(unsigned char v)
Definition Scalar.h:193
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:1238
SimTK::square
unsigned char square(unsigned char u)
Definition Scalar.h:339
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:863
SimTK::exactlyOneBitIsSet
bool exactlyOneBitIsSet(unsigned char v)
Definition Scalar.h:220
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:626
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:1159
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:624
SimTK::imag
const negator< float > & imag(const conjugate< float > &c)
Definition conjugate.h:483
SimTK::d3stepUp
double d3stepUp(double x)
Third derivative of stepUp(): d^3/dx^3 stepUp(x).
Definition Scalar.h:926
SimTK::signBit
bool signBit(unsigned char u)
Definition Scalar.h:260
SimTK::cube
unsigned char cube(unsigned char u)
Definition Scalar.h:410
negator.h
This file defines the negator<N> template which is an adaptor for the numeric types N (Real,...