Simbody 3.7
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
39
44
45#include <complex>
46#include <cmath>
47#include <climits>
48#include <cassert>
49#include <utility>
50
51namespace SimTK {
52
54 // Handy default-precision definitions //
56
57typedef conjugate<Real> Conjugate; // like Complex
58
96
116
131
136
142extern SimTK_SimTKCOMMON_EXPORT const int LosslessNumDigitsReal; // double ~20,
143 // float ~9
144
145 // Carefully calculated constants, with convenient memory addresses.
146
148extern SimTK_SimTKCOMMON_EXPORT const Real One;
150extern SimTK_SimTKCOMMON_EXPORT const Real Two;
152
161extern SimTK_SimTKCOMMON_EXPORT const Real Pi;
163extern SimTK_SimTKCOMMON_EXPORT const Real E;
172extern SimTK_SimTKCOMMON_EXPORT const Real Ln2;
174
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).
203inline bool atMostOneBitIsSet(unsigned char v) {return (v&(v-1))==0;}
204inline bool atMostOneBitIsSet(unsigned short v) {return (v&(v-1))==0;}
205inline bool atMostOneBitIsSet(unsigned int v) {return (v&(v-1))==0;}
206inline bool atMostOneBitIsSet(unsigned long v) {return (v&(v-1))==0;}
207inline bool atMostOneBitIsSet(unsigned long long v) {return (v&(v-1))==0;}
208inline bool atMostOneBitIsSet(signed char v) {return (v&(v-1))==0;}
209inline bool atMostOneBitIsSet(char v) {return (v&(v-1))==0;}
210inline bool atMostOneBitIsSet(short v) {return (v&(v-1))==0;}
211inline bool atMostOneBitIsSet(int v) {return (v&(v-1))==0;}
212inline bool atMostOneBitIsSet(long v) {return (v&(v-1))==0;}
213inline bool atMostOneBitIsSet(long long v) {return (v&(v-1))==0;}
215
230inline bool exactlyOneBitIsSet(unsigned char v) {return v && atMostOneBitIsSet(v);}
231inline bool exactlyOneBitIsSet(unsigned short v) {return v && atMostOneBitIsSet(v);}
232inline bool exactlyOneBitIsSet(unsigned int v) {return v && atMostOneBitIsSet(v);}
233inline bool exactlyOneBitIsSet(unsigned long v) {return v && atMostOneBitIsSet(v);}
234inline bool exactlyOneBitIsSet(unsigned long long v) {return v && atMostOneBitIsSet(v);}
235inline bool exactlyOneBitIsSet(signed char v) {return v && atMostOneBitIsSet(v);}
236inline bool exactlyOneBitIsSet(char v) {return v && atMostOneBitIsSet(v);}
237inline bool exactlyOneBitIsSet(short v) {return v && atMostOneBitIsSet(v);}
238inline bool exactlyOneBitIsSet(int v) {return v && atMostOneBitIsSet(v);}
239inline bool exactlyOneBitIsSet(long v) {return v && atMostOneBitIsSet(v);}
240inline bool exactlyOneBitIsSet(long long v) {return v && atMostOneBitIsSet(v);}
242
269// Don't remove these unused formal parameter names 'u'; doxygen barfs.
270inline bool signBit(unsigned char u) {return false;}
271inline bool signBit(unsigned short u) {return false;}
272inline bool signBit(unsigned int u) {return false;}
273inline bool signBit(unsigned long u) {return false;}
274inline 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.)
284inline bool signBit(signed char i) {return ((unsigned char)i & 0x80U) != 0;}
285inline bool signBit(short i) {return ((unsigned short)i & 0x8000U) != 0;}
286inline bool signBit(int i) {return ((unsigned int)i & 0x80000000U) != 0;}
287inline bool signBit(long long i) {return ((unsigned long long)i & 0x8000000000000000ULL) != 0;}
288
289inline bool signBit(long i) {return ((unsigned long)i
290 & (unsigned long)LONG_MIN) != 0;}
291
292inline bool signBit(const float& f) {return std::signbit(f);}
293inline bool signBit(const double& d) {return std::signbit(d);}
294inline bool signBit(const negator<float>& nf) {return std::signbit(-nf);} // !!
295inline bool signBit(const negator<double>& nd) {return std::signbit(-nd);} // !!
296
311inline unsigned int sign(unsigned char u) {return u==0 ? 0 : 1;}
312inline unsigned int sign(unsigned short u) {return u==0 ? 0 : 1;}
313inline unsigned int sign(unsigned int u) {return u==0 ? 0 : 1;}
314inline unsigned int sign(unsigned long u) {return u==0 ? 0 : 1;}
315inline 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
320inline int sign(signed char i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
321inline int sign(short i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
322inline int sign(int i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
323inline int sign(long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
324inline int sign(long long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
325
326inline int sign(const float& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
327inline int sign(const double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
328
329inline int sign(const negator<float>& x) {return -sign(-x);} // -x is free
330inline int sign(const negator<double>& x) {return -sign(-x);}
332
349inline unsigned char square(unsigned char u) {return u*u;}
350inline unsigned short square(unsigned short u) {return u*u;}
351inline unsigned int square(unsigned int u) {return u*u;}
352inline unsigned long square(unsigned long u) {return u*u;}
353inline unsigned long long square(unsigned long long u) {return u*u;}
354
355inline char square(char c) {return c*c;}
356
357inline signed char square(signed char i) {return i*i;}
358inline short square(short i) {return i*i;}
359inline int square(int i) {return i*i;}
360inline long square(long i) {return i*i;}
361inline long long square(long long i) {return i*i;}
362
363inline float square(const float& x) {return x*x;}
364inline 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.
368inline float square(const negator<float>& x) {return square(-x);}
369inline 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.
374template <class P> inline
375std::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).
383template <class P> inline
384std::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
389template <class P> inline
390std::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.
396template <class P> inline
397std::complex<P> square(const negator< conjugate<P> >& x) {
398 return square(-x); // negation is free for negators
399}
401
420inline unsigned char cube(unsigned char u) {return u*u*u;}
421inline unsigned short cube(unsigned short u) {return u*u*u;}
422inline unsigned int cube(unsigned int u) {return u*u*u;}
423inline unsigned long cube(unsigned long u) {return u*u*u;}
424inline unsigned long long cube(unsigned long long u) {return u*u*u;}
425
426inline char cube(char c) {return c*c*c;}
427
428inline signed char cube(signed char i) {return i*i*i;}
429inline short cube(short i) {return i*i*i;}
430inline int cube(int i) {return i*i*i;}
431inline long cube(long i) {return i*i*i;}
432inline long long cube(long long i) {return i*i*i;}
433
434inline float cube(const float& x) {return x*x*x;}
435inline 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.
441 return negator<float>::recast(cube(-x));
442}
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.
450template <class P> inline
451std::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.
460template <class P> inline
461std::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.
469template <class P> inline
470std::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.
478template <class P> inline
479std::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
533inline 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; }
536inline 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
544inline double& clampInPlace(int low, double& v, int high)
545{ return clampInPlace((double)low,v,(double)high); }
548inline float& clampInPlace(int low, float& v, int high)
549{ return clampInPlace((float)low,v,(float)high); }
550
553inline double& clampInPlace(int low, double& v, double high)
554{ return clampInPlace((double)low,v,high); }
557inline float& clampInPlace(int low, float& v, float high)
558{ return clampInPlace((float)low,v,high); }
559
562inline double& clampInPlace(double low, double& v, int high)
563{ return clampInPlace(low,v,(double)high); }
566inline float& clampInPlace(float low, float& v, int high)
567{ return clampInPlace(low,v,(float)high); }
568
570inline 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; }
573inline 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; }
576inline 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; }
579inline 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; }
582inline 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
586inline 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; }
589inline 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
593inline 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; }
596inline 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; }
599inline 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; }
602inline 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
606inline 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; }
609inline 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
634inline double clamp(double low, double v, double high)
635{ return clampInPlace(low,v,high); }
637inline float clamp(float low, float v, float high)
638{ return clampInPlace(low,v,high); }
639
642inline double clamp(int low, double v, int high)
643{ return clampInPlace(low,v,high); }
646inline float clamp(int low, float v, int high)
647{ return clampInPlace(low,v,high); }
648
651inline double clamp(int low, double v, double high)
652{ return clampInPlace(low,v,high); }
655inline float clamp(int low, float v, float high)
656{ return clampInPlace(low,v,high); }
657
660inline double clamp(double low, double v, int high)
661{ return clampInPlace(low,v,high); }
664inline float clamp(float low, float v, int high)
665{ return clampInPlace(low,v,high); }
666
668inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high)
669{ return clampInPlace(low,v,high); }
671inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high)
672{ return clampInPlace(low,v,high); }
674inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high)
675{ return clampInPlace(low,v,high); }
677inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high)
678{ return clampInPlace(low,v,high); }
680inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high)
681{ return clampInPlace(low,v,high); }
682
684inline char clamp(char low, char v, char high)
685{ return clampInPlace(low,v,high); }
687inline signed char clamp(signed char low, signed char v, signed char high)
688{ return clampInPlace(low,v,high); }
689
691inline short clamp(short low, short v, short high)
692{ return clampInPlace(low,v,high); }
694inline int clamp(int low, int v, int high)
695{ return clampInPlace(low,v,high); }
697inline long clamp(long low, long v, long high)
698{ return clampInPlace(low,v,high); }
700inline 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
714inline float clamp(float low, negator<float> v, float high)
715{ return clamp(low,(float)v,high); }
720inline double clamp(double low, negator<double> v, double high)
721{ return clamp(low,(double)v,high); }
762
777inline 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
796inline double stepDown(double x) {return 1.0 -stepUp(x);}
797
798
873inline 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
885inline 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
894inline double dstepDown(double x) {return -dstepUp(x);}
895
899inline 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
911inline 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
919inline double d2stepDown(double x) {return -d2stepUp(x);}
920
924inline 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
936inline 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
944inline double d3stepDown(double x) {return -d3stepUp(x);}
945
949inline 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
961inline 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
965inline float stepDown(float x) {return 1.0f-stepUp(x);}
967inline 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
977inline 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
982inline float dstepDown(float x) {return -dstepUp(x);}
984inline 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
994inline float d2stepUp(float x)
995{ assert(0 <= x && x <= 1);
996 return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
998inline float d2stepDown(float x) {return -d2stepUp(x);}
1000inline 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
1010inline float d3stepUp(float x)
1011{ assert(0 <= x && x <= 1);
1012 return 60+360*x*(x-1); } //60-360*x+360*x^2
1014inline float d3stepDown(float x) {return -d3stepUp(x);}
1016inline 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()
1028inline double stepUp(int x) {return stepUp((double)x);}
1031inline double stepDown(int x) {return stepDown((double)x);}
1032
1033
1092
// Doxygen should skip this helper template function
1094template <class T> // float or double
1095static 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
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}
1168inline std::pair<double,double>
1170{ return approxCompleteEllipticIntegralsKE_T<double>(m); }
1176inline std::pair<float,float>
1178{ return approxCompleteEllipticIntegralsKE_T<float>(m); }
1185inline std::pair<double,double>
1187{ return approxCompleteEllipticIntegralsKE_T<double>((double)m); }
1188
1189
// Doxygen should skip this template helper function
1191template <class T> // float or double
1192static 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.
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}
1248inline std::pair<double,double> completeEllipticIntegralsKE(double m)
1249{ return completeEllipticIntegralsKE_T<double>(m); }
1257inline std::pair<float,float> completeEllipticIntegralsKE(float m)
1258{ return completeEllipticIntegralsKE_T<float>(m); }
1265inline 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_
This file defines the Array_<T,X> class and related support classes including base classes ArrayViewC...
The purpose of the CNT<T> class is to hide the differences between built-in numerical types and compo...
High precision mathematical and physical constants.
This file contains macros which are convenient to use for sprinkling error checking around liberally ...
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:285
This file contains classes and typedefs needed to provide uniform handling of floating point numeric ...
Mandatory first inclusion for any Simbody source or header file.
#define SimTK_SimTKCOMMON_EXPORT
Definition SimTKcommon/include/SimTKcommon/internal/common.h:224
Definition NTraits.h:436
SimTK::conjugate<R> should be instantiated only for float, double.
Definition conjugate.h:178
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition negator.h:75
static const negator< N > & recast(const N &val)
Definition negator.h:235
This file defines the conjugate<R> template class, where R is one of the three built-in real types.
const Real MinusOne
Real(-1)
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
const Real Sqrt3
Real(sqrt(3))
const Real OneOverPi
1/Real(pi)
const Real Log2E
Real(log2(e)) (log base 2)
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
const Real OneSixth
Real(1)/6.
const Real Zero
Real(0)
const Real OneFourth
Real(1)/4.
const Real OneNinth
Real(1)/9.
const Real CubeRoot3
Real(3^(1/3)) (cube root of 3)
const Real Two
Real(2)
const Real OneThird
Real(1)/3.
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
const int LosslessNumDigitsReal
This is the smallest number of decimal digits you should store in a text file if you want to be able ...
const Real E
e = Real(exp(1))
const Real One
Real(1)
const Real OneEighth
Real(1)/8.
const Real Sqrt2
Real(sqrt(2))
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
const Real LeastNegativeReal
This is the largest negative real number (that is, closest to zero) that can be expressed in values o...
const Real CubeRoot2
Real(2^(1/3)) (cube root of 2)
const Real OneFifth
Real(1)/5.
const Real MostPositiveReal
This is the largest finite positive real number that can be expressed in the Real type; ~1e+308 when ...
const Real OneHalf
Real(1)/2.
const Real Ln2
Real(ln(2)) (natural log of 2)
const Real OneOverSqrt3
Real(1/sqrt(3))
const Real Three
Real(3)
const Real Ln10
Real(ln(10)) (natural log of 10)
const Real OneSeventh
Real(1)/7.
const int NumDigitsReal
This is the number of decimal digits that can be reliably stored and retrieved in the default Real pr...
const Real MostNegativeReal
This is the smallest finite negative real number that can be expressed in values of type Real....
const Real Pi
Real(pi)
const Real OneOverSqrt2
1/sqrt(2)==sqrt(2)/2 as Real
const Real Eps
Epsilon is the size of roundoff noise; it is the smallest positive number of default-precision type R...
const Real Log10E
Real(log10(e)) (log base 10)
const Real TinyReal
TinyReal is a floating point value smaller than the floating point precision; it is defined as Eps^(5...
const Real LeastPositiveReal
This is the smallest positive real number that can be expressed in the type Real; it is ~1e-308 when ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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
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:609
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
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition Scalar.h:949
double d2stepDown(double x)
Second derivative of stepDown(): d^2/dx^2 stepDown(x).
Definition Scalar.h:919
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition Scalar.h:899
double d2stepUp(double x)
Second derivative of stepUp(): d^2/dx^2 stepUp(x).
Definition Scalar.h:911
double dstepDown(double x)
First derivative of stepDown(): d/dx stepDown(x).
Definition Scalar.h:894
double dstepUp(double x)
First derivative of stepUp(): d/dx stepUp(x).
Definition Scalar.h:885
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
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition Scalar.h:924
unsigned int sign(unsigned char u)
Definition Scalar.h:311
double d3stepDown(double x)
Third derivative of stepDown(): d^3/dx^3 stepDown(x).
Definition Scalar.h:944
const float & real(const conjugate< float > &c)
Definition conjugate.h:482
conjugate< Real > Conjugate
Definition Scalar.h:57
bool atMostOneBitIsSet(unsigned char v)
Definition Scalar.h:203
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
unsigned char square(unsigned char u)
Definition Scalar.h:349
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
bool exactlyOneBitIsSet(unsigned char v)
Definition Scalar.h:230
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:606
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
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
const negator< float > & imag(const conjugate< float > &c)
Definition conjugate.h:483
double d3stepUp(double x)
Third derivative of stepUp(): d^3/dx^3 stepUp(x).
Definition Scalar.h:936
bool signBit(unsigned char u)
Definition Scalar.h:270
unsigned char cube(unsigned char u)
Definition Scalar.h:420
This file defines the negator<N> template which is an adaptor for the numeric types N (Real,...