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