1 #ifndef SimTK_SIMMATRIX_SCALAR_H_ 2 #define SimTK_SIMMATRIX_SCALAR_H_ 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;}
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;}
289 inline bool signBit(
long i) {
return ((
unsigned long)i
290 & (
unsigned long)LONG_MIN) != 0;}
292 inline bool signBit(
const float& f) {
return std::signbit(f);}
293 inline bool signBit(
const double& d) {
return std::signbit(d);}
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;}
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);}
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);}
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;}
357 inline signed char square(
signed char i) {
return i*i;}
358 inline short square(
short i) {
return i*i;}
361 inline long long square(
long long i) {
return i*i;}
363 inline float square(
const float& x) {
return x*x;}
364 inline double square(
const double& x) {
return x*x;}
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);
383 template <
class P>
inline 385 const P re=x.real(), nim=x.negImag();
386 return std::complex<P>(re*re-nim*nim, -2*re*nim);
389 template <
class P>
inline 396 template <
class P>
inline 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;}
426 inline char cube(
char c) {
return c*c*c;}
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;}
434 inline float cube(
const float& x) {
return x*x*x;}
435 inline double cube(
const double& x) {
return x*x*x;}
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));
460 template <
class P>
inline 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));
469 template <
class P>
inline 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));
478 template <
class P>
inline 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));
534 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
537 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
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; }
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; }
594 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
597 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
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; }
607 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
610 { assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
634 inline double clamp(
double low,
double v,
double high)
637 inline float clamp(
float low,
float v,
float high)
642 inline double clamp(
int low,
double v,
int high)
646 inline float clamp(
int low,
float v,
int high)
651 inline double clamp(
int low,
double v,
double high)
655 inline float clamp(
int low,
float v,
float high)
660 inline double clamp(
double low,
double v,
int high)
664 inline float clamp(
float low,
float v,
int high)
668 inline unsigned char clamp(
unsigned char low,
unsigned char v,
unsigned char high)
671 inline unsigned short clamp(
unsigned short low,
unsigned short v,
unsigned short high)
674 inline unsigned int clamp(
unsigned int low,
unsigned int v,
unsigned int high)
677 inline unsigned long clamp(
unsigned long low,
unsigned long v,
unsigned long high)
680 inline unsigned long long clamp(
unsigned long long low,
unsigned long long v,
unsigned long long high)
684 inline char clamp(
char low,
char v,
char high)
687 inline signed char clamp(
signed char low,
signed char v,
signed char high)
691 inline short clamp(
short low,
short v,
short high)
694 inline int clamp(
int low,
int v,
int high)
697 inline long clamp(
long low,
long v,
long high)
700 inline long long clamp(
long long low,
long long v,
long long high)
715 {
return clamp(low,(
float)v,high); }
721 {
return clamp(low,(
double)v,high); }
778 { assert(0 <= x && x <= 1);
779 return x*x*x*(10+x*(6*x-15)); }
873 inline double stepAny(
double y0,
double yRange,
874 double x0,
double oneOverXRange,
876 {
double xadj = (x-x0)*oneOverXRange;
880 return y0 + yRange*
stepUp(xadj); }
886 assert(0 <= x && x <= 1);
887 const double xxm1=x*(x-1);
900 double x0,
double oneOverXRange,
902 {
double xadj = (x-x0)*oneOverXRange;
906 return yRange*oneOverXRange*
dstepUp(xadj); }
912 assert(0 <= x && x <= 1);
913 return 60*x*(1+x*(2*x-3));
925 double x0,
double oneOverXRange,
927 {
double xadj = (x-x0)*oneOverXRange;
937 assert(0 <= x && x <= 1);
938 return 60+360*x*(x-1);
950 double x0,
double oneOverXRange,
952 {
double xadj = (x-x0)*oneOverXRange;
962 { assert(0 <= x && x <= 1);
963 return x*x*x*(10+x*(6*x-15)); }
968 float x0,
float oneOverXRange,
970 {
float xadj = (x-x0)*oneOverXRange;
974 return y0 + yRange*
stepUp(xadj); }
978 { assert(0 <= x && x <= 1);
979 const float xxm1=x*(x-1);
980 return 30*xxm1*xxm1; }
985 float x0,
float oneOverXRange,
987 {
float xadj = (x-x0)*oneOverXRange;
991 return yRange*oneOverXRange*
dstepUp(xadj); }
995 { assert(0 <= x && x <= 1);
996 return 60*x*(1+x*(2*x-3)); }
1001 float x0,
float oneOverXRange,
1003 {
float xadj = (x-x0)*oneOverXRange;
1011 { assert(0 <= x && x <= 1);
1012 return 60+360*x*(x-1); }
1017 float x0,
float oneOverXRange,
1019 {
float xadj = (x-x0)*oneOverXRange;
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 };
1110 const T PiOver2 = NTraits<T>::getPi()/2;
1111 const T
Infinity = NTraits<T>::getInfinity();
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);
1119 const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2;
1120 const T lnm2 = std::log(m1);
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);
1128 return std::make_pair(K,
E);
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); }
1192 static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
1194 const T TenEps = 10*NTraits<T>::getEps();
1195 const T
Infinity = NTraits<T>::getInfinity();
1196 const T PiOver2 = NTraits<T>::getPi() / 2;
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);
1207 const T k = std::sqrt(1-m);
1208 T v1=1, w1=k, c1=1, d1=k*k;
1211 T w2 = std::sqrt(v1*w1);
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);
1217 const T K = PiOver2/v1;
1220 return std::make_pair(K,
E);
1249 {
return completeEllipticIntegralsKE_T<double>(m); }
1258 {
return completeEllipticIntegralsKE_T<float>(m); }
1266 {
return completeEllipticIntegralsKE_T<double>((double)m); }
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 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...
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
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.
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 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)
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