1#ifndef SimTK_SIMMATRIX_SCALAR_H_
2#define SimTK_SIMMATRIX_SCALAR_H_
270inline bool signBit(
unsigned char u) {
return false;}
271inline bool signBit(
unsigned short u) {
return false;}
272inline bool signBit(
unsigned int u) {
return false;}
273inline bool signBit(
unsigned long u) {
return false;}
274inline bool signBit(
unsigned long long u) {
return false;}
284inline bool signBit(
signed char i) {
return ((
unsigned char)i & 0x80U) != 0;}
285inline bool signBit(
short i) {
return ((
unsigned short)i & 0x8000U) != 0;}
286inline bool signBit(
int i) {
return ((
unsigned int)i & 0x80000000U) != 0;}
287inline bool signBit(
long long i) {
return ((
unsigned long long)i & 0x8000000000000000ULL) != 0;}
289inline bool signBit(
long i) {
return ((
unsigned long)i
290 & (
unsigned long)LONG_MIN) != 0;}
292inline bool signBit(
const float& f) {
return std::signbit(f);}
293inline bool signBit(
const double& d) {
return std::signbit(d);}
311inline unsigned int sign(
unsigned char u) {
return u==0 ? 0 : 1;}
312inline unsigned int sign(
unsigned short u) {
return u==0 ? 0 : 1;}
313inline unsigned int sign(
unsigned int u) {
return u==0 ? 0 : 1;}
314inline unsigned int sign(
unsigned long u) {
return u==0 ? 0 : 1;}
315inline unsigned int sign(
unsigned long long u) {
return u==0 ? 0 : 1;}
320inline int sign(
signed char i) {
return i>0 ? 1 : (i<0 ? -1 : 0);}
321inline int sign(
short i) {
return i>0 ? 1 : (i<0 ? -1 : 0);}
322inline int sign(
int i) {
return i>0 ? 1 : (i<0 ? -1 : 0);}
323inline int sign(
long i) {
return i>0 ? 1 : (i<0 ? -1 : 0);}
324inline int sign(
long long i) {
return i>0 ? 1 : (i<0 ? -1 : 0);}
326inline int sign(
const float& x) {
return x>0 ? 1 : (x<0 ? -1 : 0);}
327inline int sign(
const double& x) {
return x>0 ? 1 : (x<0 ? -1 : 0);}
349inline unsigned char square(
unsigned char u) {
return u*u;}
350inline unsigned short square(
unsigned short u) {
return u*u;}
351inline unsigned int square(
unsigned int u) {
return u*u;}
352inline unsigned long square(
unsigned long u) {
return u*u;}
353inline unsigned long long square(
unsigned long long u) {
return u*u;}
357inline signed char square(
signed char i) {
return i*i;}
358inline short square(
short i) {
return i*i;}
361inline long long square(
long long i) {
return i*i;}
363inline float square(
const float& x) {
return x*x;}
364inline double square(
const double& x) {
return x*x;}
374template <
class P>
inline
375std::complex<P>
square(
const std::complex<P>& x) {
376 const P re=x.real(), im=x.imag();
377 return std::complex<P>(re*re-im*im, 2*re*im);
383template <
class P>
inline
385 const P re=x.real(), nim=x.negImag();
386 return std::complex<P>(re*re-nim*nim, -2*re*nim);
389template <
class P>
inline
396template <
class P>
inline
420inline unsigned char cube(
unsigned char u) {
return u*u*u;}
421inline unsigned short cube(
unsigned short u) {
return u*u*u;}
422inline unsigned int cube(
unsigned int u) {
return u*u*u;}
423inline unsigned long cube(
unsigned long u) {
return u*u*u;}
424inline unsigned long long cube(
unsigned long long u) {
return u*u*u;}
426inline char cube(
char c) {
return c*c*c;}
428inline signed char cube(
signed char i) {
return i*i*i;}
429inline short cube(
short i) {
return i*i*i;}
430inline int cube(
int i) {
return i*i*i;}
431inline long cube(
long i) {
return i*i*i;}
432inline long long cube(
long long i) {
return i*i*i;}
434inline float cube(
const float& x) {
return x*x*x;}
435inline double cube(
const double& x) {
return x*x*x;}
450template <
class P>
inline
451std::complex<P>
cube(
const std::complex<P>& x) {
452 const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
453 return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
460template <
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));
469template <
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));
478template <
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; }
570inline unsigned char&
clampInPlace(
unsigned char low,
unsigned char& v,
unsigned char high)
571{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
573inline unsigned short&
clampInPlace(
unsigned short low,
unsigned short& v,
unsigned short high)
574{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
576inline unsigned int&
clampInPlace(
unsigned int low,
unsigned int& v,
unsigned int high)
577{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
579inline unsigned long&
clampInPlace(
unsigned long low,
unsigned long& v,
unsigned long high)
580{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
582inline unsigned long long&
clampInPlace(
unsigned long long low,
unsigned long long& v,
unsigned long long high)
583{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
587{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
589inline signed char&
clampInPlace(
signed char low,
signed char& v,
signed char high)
590{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
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; }
602inline long long&
clampInPlace(
long long low,
long long& v,
long long high)
603{ assert(low<=high);
if (v<low) v=low;
else if (v>high) v=high;
return v; }
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; }
634inline double clamp(
double low,
double v,
double high)
637inline float clamp(
float low,
float v,
float high)
642inline double clamp(
int low,
double v,
int high)
646inline float clamp(
int low,
float v,
int high)
651inline double clamp(
int low,
double v,
double high)
655inline float clamp(
int low,
float v,
float high)
660inline double clamp(
double low,
double v,
int high)
664inline float clamp(
float low,
float v,
int high)
668inline unsigned char clamp(
unsigned char low,
unsigned char v,
unsigned char high)
671inline unsigned short clamp(
unsigned short low,
unsigned short v,
unsigned short high)
674inline unsigned int clamp(
unsigned int low,
unsigned int v,
unsigned int high)
677inline unsigned long clamp(
unsigned long low,
unsigned long v,
unsigned long high)
680inline unsigned long long clamp(
unsigned long long low,
unsigned long long v,
unsigned long long high)
684inline char clamp(
char low,
char v,
char high)
687inline signed char clamp(
signed char low,
signed char v,
signed char high)
691inline short clamp(
short low,
short v,
short high)
694inline int clamp(
int low,
int v,
int high)
697inline long clamp(
long low,
long v,
long high)
700inline 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)); }
873inline 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;
1095static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
1096 static const T a[] =
1097 { (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
1098 (T)0.03742563713, (T)0.01451196212 };
1099 static const T b[] =
1100 { (T)0.5, (T)0.12498593597, (T)0.06880248576,
1101 (T)0.03328355346, (T)0.00441787012 };
1102 static const T c[] =
1103 { (T)1, (T)0.44325141463, (T)0.06260601220,
1104 (T)0.04757383546, (T)0.01736506451 };
1105 static const T d[] =
1106 { (T)0, (T)0.24998368310, (T)0.09200180037,
1107 (T)0.04069697526, (T)0.00526449639 };
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);
1168inline std::pair<double,double>
1170{
return approxCompleteEllipticIntegralsKE_T<double>(m); }
1176inline std::pair<float,float>
1178{
return approxCompleteEllipticIntegralsKE_T<float>(m); }
1185inline std::pair<double,double>
1187{
return approxCompleteEllipticIntegralsKE_T<double>((
double)m); }
1192static 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); }
This file defines the Array_<T,X> class and related support classes including base classes ArrayViewC...
The purpose of the CNT<T> class is to hide the differences between built-in numerical types and compo...
High precision mathematical and physical constants.
This file contains macros which are convenient to use for sprinkling error checking around liberally ...
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:285
This file contains classes and typedefs needed to provide uniform handling of floating point numeric ...
Mandatory first inclusion for any Simbody source or header file.
#define SimTK_SimTKCOMMON_EXPORT
Definition SimTKcommon/include/SimTKcommon/internal/common.h:224
SimTK::conjugate<R> should be instantiated only for float, double.
Definition conjugate.h:178
negator<N>, where N is a number type (real, complex, conjugate), is represented in memory identically...
Definition negator.h:75
static const negator< N > & recast(const N &val)
Definition negator.h:235
This file defines the conjugate<R> template class, where R is one of the three built-in real types.
const Real MinusOne
Real(-1)
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
const Real Sqrt3
Real(sqrt(3))
const Real OneOverPi
1/Real(pi)
const Real Log2E
Real(log2(e)) (log base 2)
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
const Real OneSixth
Real(1)/6.
const Real OneFourth
Real(1)/4.
const Real OneNinth
Real(1)/9.
const Real CubeRoot3
Real(3^(1/3)) (cube root of 3)
const Real OneThird
Real(1)/3.
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
const int LosslessNumDigitsReal
This is the smallest number of decimal digits you should store in a text file if you want to be able ...
const Real E
e = Real(exp(1))
const Real OneEighth
Real(1)/8.
const Real Sqrt2
Real(sqrt(2))
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
const Real LeastNegativeReal
This is the largest negative real number (that is, closest to zero) that can be expressed in values o...
const Real CubeRoot2
Real(2^(1/3)) (cube root of 2)
const Real OneFifth
Real(1)/5.
const Real MostPositiveReal
This is the largest finite positive real number that can be expressed in the Real type; ~1e+308 when ...
const Real OneHalf
Real(1)/2.
const Real Ln2
Real(ln(2)) (natural log of 2)
const Real OneOverSqrt3
Real(1/sqrt(3))
const Real Ln10
Real(ln(10)) (natural log of 10)
const Real OneSeventh
Real(1)/7.
const int NumDigitsReal
This is the number of decimal digits that can be reliably stored and retrieved in the default Real pr...
const Real MostNegativeReal
This is the smallest finite negative real number that can be expressed in values of type Real....
const Real OneOverSqrt2
1/sqrt(2)==sqrt(2)/2 as Real
const Real Eps
Epsilon is the size of roundoff noise; it is the smallest positive number of default-precision type R...
const Real Log10E
Real(log10(e)) (log base 10)
const Real TinyReal
TinyReal is a floating point value smaller than the floating point precision; it is defined as Eps^(5...
const Real LeastPositiveReal
This is the smallest positive real number that can be expressed in the type Real; it is ~1e-308 when ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
double stepUp(double x)
Interpolate smoothly from 0 up to 1 as the input argument goes from 0 to 1, with first and second der...
Definition Scalar.h:777
std::complex< Real > Complex
This is the default complex type for SimTK, with precision for the real and imaginary parts set to th...
Definition SimTKcommon/include/SimTKcommon/internal/common.h:609
double stepDown(double x)
Interpolate smoothly from 1 down to 0 as the input argument goes from 0 to 1, with first and second d...
Definition Scalar.h:796
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition Scalar.h:949
double d2stepDown(double x)
Second derivative of stepDown(): d^2/dx^2 stepDown(x).
Definition Scalar.h:919
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition Scalar.h:899
double d2stepUp(double x)
Second derivative of stepUp(): d^2/dx^2 stepUp(x).
Definition Scalar.h:911
double dstepDown(double x)
First derivative of stepDown(): d/dx stepDown(x).
Definition Scalar.h:894
double dstepUp(double x)
First derivative of stepUp(): d/dx stepUp(x).
Definition Scalar.h:885
double & clampInPlace(double low, double &v, double high)
Check that low <= v <= high and modify v in place if necessary to bring it into that range.
Definition Scalar.h:533
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition Scalar.h:924
unsigned int sign(unsigned char u)
Definition Scalar.h:311
double d3stepDown(double x)
Third derivative of stepDown(): d^3/dx^3 stepDown(x).
Definition Scalar.h:944
const float & real(const conjugate< float > &c)
Definition conjugate.h:482
conjugate< Real > Conjugate
Definition Scalar.h:57
bool atMostOneBitIsSet(unsigned char v)
Definition Scalar.h:203
std::pair< double, double > completeEllipticIntegralsKE(double m)
Given 0<=m<=1, return complete elliptic integrals of the first and second kinds, K(m) and E(m),...
Definition Scalar.h:1248
unsigned char square(unsigned char u)
Definition Scalar.h:349
double stepAny(double y0, double yRange, double x0, double oneOverXRange, double x)
Interpolate smoothly from y0 to y1 as the input argument goes from x0 to x1, with first and second de...
Definition Scalar.h:873
bool exactlyOneBitIsSet(unsigned char v)
Definition Scalar.h:230
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition SimTKcommon/include/SimTKcommon/internal/common.h:606
std::pair< double, double > approxCompleteEllipticIntegralsKE(double m)
Given 0<=m<=1, return complete elliptic integrals of the first and second kinds, K(m) and E(m),...
Definition Scalar.h:1169
double clamp(double low, double v, double high)
If v is in range low <= v <= high then return v, otherwise return the nearest bound; this function do...
Definition Scalar.h:634
const negator< float > & imag(const conjugate< float > &c)
Definition conjugate.h:483
double d3stepUp(double x)
Third derivative of stepUp(): d^3/dx^3 stepUp(x).
Definition Scalar.h:936
bool signBit(unsigned char u)
Definition Scalar.h:270
unsigned char cube(unsigned char u)
Definition Scalar.h:420
This file defines the negator<N> template which is an adaptor for the numeric types N (Real,...