Simbody  3.7
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 
34 #include "SimTKcommon/Constants.h"
39 
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;
96 
100 extern SimTK_SimTKCOMMON_EXPORT const Real Eps;
116 
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;
150 extern SimTK_SimTKCOMMON_EXPORT const Real Two;
151 extern SimTK_SimTKCOMMON_EXPORT const Real Three;
152 
161 extern SimTK_SimTKCOMMON_EXPORT const Real Pi;
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;
168 extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt3;
172 extern SimTK_SimTKCOMMON_EXPORT const Real Ln2;
173 extern SimTK_SimTKCOMMON_EXPORT const Real Ln10;
174 
180 extern SimTK_SimTKCOMMON_EXPORT const Complex I;
183  // 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.
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.
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 
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>
1170 { return approxCompleteEllipticIntegralsKE_T<double>(m); }
1176 inline std::pair<float,float>
1178 { return approxCompleteEllipticIntegralsKE_T<float>(m); }
1185 inline std::pair<double,double>
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.
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_
const Real MostNegativeReal
This is the smallest finite negative real number that can be expressed in values of type Real...
const Real Ln2
Real(ln(2)) (natural log of 2)
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
const Real Three
Real(3)
const Real Sqrt3
Real(sqrt(3))
double d2stepDown(double x)
Second derivative of stepDown(): d^2/dx^2 stepDown(x).
Definition: Scalar.h:919
const Real LeastNegativeReal
This is the largest negative real number (that is, closest to zero) that can be expressed in values o...
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition: Scalar.h:924
bool signBit(unsigned char u)
Definition: Scalar.h:270
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
SimTK::conjugate<R> should be instantiated only for float, double.
Definition: String.h:45
This file defines the negator<N> template which is an adaptor for the numeric types N (Real...
const Real Two
Real(2)
This file defines the conjugate<R> template class, where R is one of the three built-in real types...
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
const Real Zero
Real(0)
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
conjugate< Real > Conjugate
Definition: Scalar.h:57
const Real OneFifth
Real(1)/5.
const Real Sqrt2
Real(sqrt(2))
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition: String.h:44
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
bool exactlyOneBitIsSet(unsigned char v)
Definition: Scalar.h:230
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
This file contains classes and typedefs needed to provide uniform handling of floating point numeric ...
const Real LeastPositiveReal
This is the smallest positive real number that can be expressed in the type Real; it is ~1e-308 when ...
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
unsigned char square(unsigned char u)
Definition: Scalar.h:349
This file defines the Array_<T,X> class and related support classes including base classes ArrayViewC...
This file contains macros which are convenient to use for sprinkling error checking around liberally ...
const Real CubeRoot2
Real(2^(1/3)) (cube root of 2)
bool atMostOneBitIsSet(unsigned char v)
Definition: Scalar.h:203
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i...
const Real OneSixth
Real(1)/6.
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition: Scalar.h:899
High precision mathematical and physical constants.
The purpose of the CNT<T> class is to hide the differences between built-in numerical types and compo...
const Real OneEighth
Real(1)/8.
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
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
const Real Log10E
Real(log10(e)) (log base 10)
const int NumDigitsReal
This is the number of decimal digits that can be reliably stored and retrieved in the default Real pr...
const Real TinyReal
TinyReal is a floating point value smaller than the floating point precision; it is defined as Eps^(5...
const Real OneFourth
Real(1)/4.
const Real Pi
Real(pi)
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
const float & real(const conjugate< float > &c)
Definition: conjugate.h:482
const Real MinusOne
Real(-1)
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition: Scalar.h:949
const Real Eps
Epsilon is the size of roundoff noise; it is the smallest positive number of default-precision type R...
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 Real One
Real(1)
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
const Real E
e = Real(exp(1))
Mandatory first inclusion for any Simbody source or header file.
static const negator< N > & recast(const N &val)
Definition: negator.h:235
const Real Ln10
Real(ln(10)) (natural log of 10)
const Real Log2E
Real(log2(e)) (log base 2)
const Real OneHalf
Real(1)/2.
const Real OneSeventh
Real(1)/7.
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
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
const Real OneOverPi
1/Real(pi)
const Real OneOverSqrt2
1/sqrt(2)==sqrt(2)/2 as Real
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
const Real MostPositiveReal
This is the largest finite positive real number that can be expressed in the Real type; ~1e+308 when ...
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
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 OneOverSqrt3
Real(1/sqrt(3))
const Real OneNinth
Real(1)/9.
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
const Real OneThird
Real(1)/3.
double dstepUp(double x)
First derivative of stepUp(): d/dx stepUp(x).
Definition: Scalar.h:885
const Real CubeRoot3
Real(3^(1/3)) (cube root of 3)
Definition: negator.h:64
double d3stepDown(double x)
Third derivative of stepDown(): d^3/dx^3 stepDown(x).
Definition: Scalar.h:944
unsigned char cube(unsigned char u)
Definition: Scalar.h:420