Scalar.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATRIX_SCALAR_H_
#define SimTK_SIMMATRIX_SCALAR_H_

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2005-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon/internal/common.h"
#include "SimTKcommon/Constants.h"
#include "SimTKcommon/internal/Exception.h"
#include "SimTKcommon/internal/ExceptionMacros.h"
#include "SimTKcommon/internal/String.h"
#include "SimTKcommon/internal/Array.h"

#include "SimTKcommon/internal/conjugate.h"
#include "SimTKcommon/internal/CompositeNumericalTypes.h"
#include "SimTKcommon/internal/NTraits.h"
#include "SimTKcommon/internal/negator.h"

#include <complex>
#include <cmath>
#include <climits>
#include <cassert>
#include <utility>

namespace SimTK {

    // Handy default-precision definitions //

typedef conjugate<Real> Conjugate;  // like Complex

extern SimTK_SimTKCOMMON_EXPORT const Real NaN; 
extern SimTK_SimTKCOMMON_EXPORT const Real Infinity;

extern SimTK_SimTKCOMMON_EXPORT const Real Eps;
extern SimTK_SimTKCOMMON_EXPORT const Real SqrtEps;
extern SimTK_SimTKCOMMON_EXPORT const Real TinyReal; 
extern SimTK_SimTKCOMMON_EXPORT const Real SignificantReal; 

extern SimTK_SimTKCOMMON_EXPORT const Real LeastPositiveReal; 
extern SimTK_SimTKCOMMON_EXPORT const Real MostPositiveReal;  
extern SimTK_SimTKCOMMON_EXPORT const Real LeastNegativeReal;
extern SimTK_SimTKCOMMON_EXPORT const Real MostNegativeReal;

extern SimTK_SimTKCOMMON_EXPORT const int NumDigitsReal; 

extern SimTK_SimTKCOMMON_EXPORT const int LosslessNumDigitsReal; // double ~20, 
                                                                 // float ~9

    // Carefully calculated constants, with convenient memory addresses.

extern SimTK_SimTKCOMMON_EXPORT const Real Zero;        
extern SimTK_SimTKCOMMON_EXPORT const Real One;         
extern SimTK_SimTKCOMMON_EXPORT const Real MinusOne;    
extern SimTK_SimTKCOMMON_EXPORT const Real Two;         
extern SimTK_SimTKCOMMON_EXPORT const Real Three;       

extern SimTK_SimTKCOMMON_EXPORT const Real OneHalf;     
extern SimTK_SimTKCOMMON_EXPORT const Real OneThird;    
extern SimTK_SimTKCOMMON_EXPORT const Real OneFourth;   
extern SimTK_SimTKCOMMON_EXPORT const Real OneFifth;    
extern SimTK_SimTKCOMMON_EXPORT const Real OneSixth;    
extern SimTK_SimTKCOMMON_EXPORT const Real OneSeventh;  
extern SimTK_SimTKCOMMON_EXPORT const Real OneEighth;   
extern SimTK_SimTKCOMMON_EXPORT const Real OneNinth;    
extern SimTK_SimTKCOMMON_EXPORT const Real Pi;          
extern SimTK_SimTKCOMMON_EXPORT const Real OneOverPi;   
extern SimTK_SimTKCOMMON_EXPORT const Real E;           
extern SimTK_SimTKCOMMON_EXPORT const Real Log2E;       
extern SimTK_SimTKCOMMON_EXPORT const Real Log10E;      
extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt2;       
extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt2;
extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt3;       
extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt3;
extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot2;   
extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot3;   
extern SimTK_SimTKCOMMON_EXPORT const Real Ln2;         
extern SimTK_SimTKCOMMON_EXPORT const Real Ln10;        
       
extern SimTK_SimTKCOMMON_EXPORT const Complex I;
    // SOME SCALAR UTILITIES //


// We use the trick that v & v-1 returns the value that is v with its
// rightmost bit cleared (if it has a rightmost bit set).
inline bool atMostOneBitIsSet(unsigned char v)      {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(unsigned short v)     {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(unsigned int v)       {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(unsigned long v)      {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(unsigned long long v) {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(signed char v)        {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(char v)               {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(short v)              {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(int v)                {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(long v)               {return (v&(v-1))==0;}
inline bool atMostOneBitIsSet(long long v)          {return (v&(v-1))==0;}

inline bool exactlyOneBitIsSet(unsigned char v)      {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(unsigned short v)     {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(unsigned int v)       {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(unsigned long v)      {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(unsigned long long v) {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(signed char v)        {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(char v)               {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(short v)              {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(int v)                {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(long v)               {return v && atMostOneBitIsSet(v);}
inline bool exactlyOneBitIsSet(long long v)          {return v && atMostOneBitIsSet(v);}

// Don't remove these unused formal parameter names 'u'; doxygen barfs.
inline bool signBit(unsigned char u)      {return false;}
inline bool signBit(unsigned short u)     {return false;}
inline bool signBit(unsigned int u)       {return false;}
inline bool signBit(unsigned long u)      {return false;}
inline bool signBit(unsigned long long u) {return false;}

// Note that plain 'char' type is not overloaded -- see above.

// We're assuming sizeof(char)==1, short==2, int==4, long long==8 which is safe
// for all our anticipated platforms. But some 64 bit implementations have
// sizeof(long)==4 while others have sizeof(long)==8 so we'll use the ANSI
// standard value LONG_MIN which is a long value with just the high bit set.
// (We're assuming two's complement integers here; I haven't seen anything
// else in decades.)
inline bool signBit(signed char i) {return ((unsigned char)i      & 0x80U) != 0;}
inline bool signBit(short       i) {return ((unsigned short)i     & 0x8000U) != 0;}
inline bool signBit(int         i) {return ((unsigned int)i       & 0x80000000U) != 0;}
inline bool signBit(long long   i) {return ((unsigned long long)i & 0x8000000000000000ULL) != 0;}

inline bool signBit(long        i) {return ((unsigned long)i
                                            & (unsigned long)LONG_MIN) != 0;}

inline bool signBit(const float&  f) {return std::signbit(f);}
inline bool signBit(const double& d) {return std::signbit(d);}
inline bool signBit(const negator<float>&  nf) {return std::signbit(-nf);} // !!
inline bool signBit(const negator<double>& nd) {return std::signbit(-nd);} // !!

inline unsigned int sign(unsigned char      u) {return u==0 ? 0 : 1;}
inline unsigned int sign(unsigned short     u) {return u==0 ? 0 : 1;}
inline unsigned int sign(unsigned int       u) {return u==0 ? 0 : 1;}
inline unsigned int sign(unsigned long      u) {return u==0 ? 0 : 1;}
inline unsigned int sign(unsigned long long u) {return u==0 ? 0 : 1;}

// Don't overload for plain "char" because it may be signed or unsigned
// depending on the compiler.

inline int sign(signed char i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
inline int sign(short       i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
inline int sign(int         i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
inline int sign(long        i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
inline int sign(long long   i) {return i>0 ? 1 : (i<0 ? -1 : 0);}

inline int sign(const float&       x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
inline int sign(const double&      x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
inline int sign(const long double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}

inline int sign(const negator<float>&       x) {return -sign(-x);} // -x is free
inline int sign(const negator<double>&      x) {return -sign(-x);}
inline int sign(const negator<long double>& x) {return -sign(-x);}

inline unsigned char  square(unsigned char  u) {return u*u;}
inline unsigned short square(unsigned short u) {return u*u;}
inline unsigned int   square(unsigned int   u) {return u*u;}
inline unsigned long  square(unsigned long  u) {return u*u;}
inline unsigned long long square(unsigned long long u) {return u*u;}

inline char        square(char c) {return c*c;}

inline signed char square(signed char i) {return i*i;}
inline short       square(short       i) {return i*i;}
inline int         square(int         i) {return i*i;}
inline long        square(long        i) {return i*i;}
inline long long   square(long long   i) {return i*i;}

inline float       square(const float&       x) {return x*x;}
inline double      square(const double&      x) {return x*x;}
inline long double square(const long double& x) {return x*x;}

// Negation is free for negators, so we can square them and clean
// them up at the same time at no extra cost.
inline float       square(const negator<float>&       x) {return square(-x);}
inline double      square(const negator<double>&      x) {return square(-x);}
inline long double square(const negator<long double>& x) {return square(-x);}

// It is safer to templatize using complex classes, and doesn't make
// debugging any worse since complex is already templatized. 
// 5 flops vs. 6 for general complex multiply.
template <class P> inline 
std::complex<P> square(const std::complex<P>& x) {
    const P re=x.real(), im=x.imag();
    return std::complex<P>(re*re-im*im, 2*re*im);
}

// We can square a conjugate and clean it up back to complex at
// the same time at zero cost (or maybe 1 negation depending
// on how the compiler handles the "-2" below).
template <class P> inline 
std::complex<P> square(const conjugate<P>& x) {
    const P re=x.real(), nim=x.negImag();
    return std::complex<P>(re*re-nim*nim, -2*re*nim);
}

template <class P> inline
std::complex<P> square(const negator< std::complex<P> >& x) {
    return square(-x); // negation is free for negators
}

// Note return type here after squaring negator<conjugate>
// is complex, not conjugate.
template <class P> inline
std::complex<P> square(const negator< conjugate<P> >& x) {
    return square(-x); // negation is free for negators
}

inline unsigned char  cube(unsigned char  u) {return u*u*u;}
inline unsigned short cube(unsigned short u) {return u*u*u;}
inline unsigned int   cube(unsigned int   u) {return u*u*u;}
inline unsigned long  cube(unsigned long  u) {return u*u*u;}
inline unsigned long long cube(unsigned long long u) {return u*u*u;}

inline char        cube(char c) {return c*c*c;}

inline signed char cube(signed char i) {return i*i*i;}
inline short       cube(short       i) {return i*i*i;}
inline int         cube(int         i) {return i*i*i;}
inline long        cube(long        i) {return i*i*i;}
inline long long   cube(long long   i) {return i*i*i;}

inline float       cube(const float&       x) {return x*x*x;}
inline double      cube(const double&      x) {return x*x*x;}
inline long double cube(const long double& x) {return x*x*x;}

// To keep this cheap we'll defer getting rid of the negator<> until
// some other operation. We cube -x and then recast that to negator<>
// on return, for a total cost of 2 flops.
inline negator<float> cube(const negator<float>& x) {
    return negator<float>::recast(cube(-x));
}
inline negator<double> cube(const negator<double>& x) {
    return negator<double>::recast(cube(-x));
}
inline negator<long double> cube(const negator<long double>& x) {
    return negator<long double>::recast(cube(-x));
}

// Cubing a complex this way is cheaper than doing it by
// multiplication. Cost here is 8 flops vs. 11 for a square
// followed by a multiply.
template <class P> inline
std::complex<P> cube(const std::complex<P>& x) {
    const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
    return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
}

// Cubing a negated complex allows us to cube and eliminate the
// negator at the same time for zero extra cost. Compare the 
// expressions here to the normal cube above to see the free
// sign changes in both parts. 8 flops here.
template <class P> inline
std::complex<P> cube(const negator< std::complex<P> >& x) {
    // -x is free for a negator
    const P nre=(-x).real(), nim=(-x).imag(), rr=nre*nre, ii=nim*nim;
    return std::complex<P>(nre*(3*ii-rr), nim*(ii-3*rr));
}

// Cubing a conjugate this way saves a lot over multiplying, and
// also lets us convert the result to complex for free.
template <class P> inline
std::complex<P> cube(const conjugate<P>& x) {
    const P re=x.real(), nim=x.negImag(), rr=re*re, ii=nim*nim;
    return std::complex<P>(re*(rr-3*ii), nim*(ii-3*rr));
}


// Cubing a negated conjugate this way saves a lot over multiplying, and
// also lets us convert the result to non-negated complex for free.
template <class P> inline
std::complex<P> cube(const negator< conjugate<P> >& x) {
    // -x is free for a negator
    const P nre=(-x).real(), im=(-x).negImag(), rr=nre*nre, ii=im*im;
    return std::complex<P>(nre*(3*ii-rr), im*(3*rr-ii));
}


inline double& clampInPlace(double low, double& v, double high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline float& clampInPlace(float low, float& v, float high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline long double& clampInPlace(long double low, long double& v, long double high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }

    // Floating point clamps with integer bounds; without these
    // explicit casts are required.

inline double& clampInPlace(int low, double& v, int high) 
{   return clampInPlace((double)low,v,(double)high); }
inline float& clampInPlace(int low, float& v, int high) 
{   return clampInPlace((float)low,v,(float)high); }
inline long double& clampInPlace(int low, long double& v, int high) 
{   return clampInPlace((long double)low,v,(long double)high); }

inline double& clampInPlace(int low, double& v, double high) 
{   return clampInPlace((double)low,v,high); }
inline float& clampInPlace(int low, float& v, float high) 
{   return clampInPlace((float)low,v,high); }
inline long double& clampInPlace(int low, long double& v, long double high) 
{   return clampInPlace((long double)low,v,high); }

inline double& clampInPlace(double low, double& v, int high) 
{   return clampInPlace(low,v,(double)high); }
inline float& clampInPlace(float low, float& v, int high) 
{   return clampInPlace(low,v,(float)high); }
inline long double& clampInPlace(long double low, long double& v, int high) 
{   return clampInPlace(low,v,(long double)high); }

inline unsigned char& clampInPlace(unsigned char low, unsigned char& v, unsigned char high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline unsigned short& clampInPlace(unsigned short low, unsigned short& v, unsigned short high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline unsigned int& clampInPlace(unsigned int low, unsigned int& v, unsigned int high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline unsigned long& clampInPlace(unsigned long low, unsigned long& v, unsigned long high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline unsigned long long& clampInPlace(unsigned long long low, unsigned long long& v, unsigned long long high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }

inline char& clampInPlace(char low, char& v, char high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline signed char& clampInPlace(signed char low, signed char& v, signed char high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }

inline short& clampInPlace(short low, short& v, short high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline int& clampInPlace(int low, int& v, int high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline long& clampInPlace(long low, long& v, long high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline long long& clampInPlace(long long low, long long& v, long long high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }

inline negator<float>& clampInPlace(float low, negator<float>& v, float high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline negator<double>& clampInPlace(double low, negator<double>& v, double high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
inline negator<long double>& clampInPlace(long double low, negator<long double>& v, long double high) 
{   assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }



inline double clamp(double low, double v, double high) 
{   return clampInPlace(low,v,high); }
inline float clamp(float low, float v, float high) 
{   return clampInPlace(low,v,high); }
inline long double clamp(long double low, long double v, long double high) 
{   return clampInPlace(low,v,high); }

inline double clamp(int low, double v, int high) 
{   return clampInPlace(low,v,high); }
inline float clamp(int low, float v, int high) 
{   return clampInPlace(low,v,high); }
inline long double clamp(int low, long double v, int high) 
{   return clampInPlace(low,v,high); }

inline double clamp(int low, double v, double high) 
{   return clampInPlace(low,v,high); }
inline float clamp(int low, float v, float high) 
{   return clampInPlace(low,v,high); }
inline long double clamp(int low, long double v, long double high) 
{   return clampInPlace(low,v,high); }

inline double clamp(double low, double v, int high) 
{   return clampInPlace(low,v,high); }
inline float clamp(float low, float v, int high) 
{   return clampInPlace(low,v,high); }
inline long double clamp(long double low, long double v, int high) 
{   return clampInPlace(low,v,high); }

inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high) 
{   return clampInPlace(low,v,high); }
inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high) 
{   return clampInPlace(low,v,high); }
inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high) 
{   return clampInPlace(low,v,high); }
inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high) 
{   return clampInPlace(low,v,high); }
inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high) 
{   return clampInPlace(low,v,high); }

inline char clamp(char low, char v, char high) 
{   return clampInPlace(low,v,high); }
inline signed char clamp(signed char low, signed char v, signed char high) 
{   return clampInPlace(low,v,high); }

inline short clamp(short low, short v, short high) 
{   return clampInPlace(low,v,high); }
inline int clamp(int low, int v, int high) 
{   return clampInPlace(low,v,high); }
inline long clamp(long low, long v, long high) 
{   return clampInPlace(low,v,high); }
inline long long clamp(long long low, long long v, long long high) 
{   return clampInPlace(low,v,high); }


// These aren't strictly necessary but are here to help the
// compiler find the right overload to call. These cost an
// extra flop because the negator<T> has to be cast to T which
// requires that the pending negation be performed. Note that
// the result types are not negated.

inline float clamp(float low, negator<float> v, float high)
{   return clamp(low,(float)v,high); }
inline double clamp(double low, negator<double> v, double high) 
{   return clamp(low,(double)v,high); }
inline long double clamp(long double low, negator<long double> v, long double high) 
{   return clamp(low,(long double)v,high); }

inline double stepUp(double x)
{   assert(0 <= x && x <= 1);
    return x*x*x*(10+x*(6*x-15)); }  //10x^3-15x^4+6x^5


inline double stepDown(double x) {return 1.0 -stepUp(x);}


inline double stepAny(double y0, double yRange,
                      double x0, double oneOverXRange,
                      double x) 
{   double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<double>::getSignificant());
    clampInPlace(0.0,xadj,1.0);
    return y0 + yRange*stepUp(xadj); }

inline double dstepUp(double x) {
    assert(0 <= x && x <= 1);
    const double xxm1=x*(x-1);
    return 30*xxm1*xxm1;        //30x^2-60x^3+30x^4
}

inline double dstepDown(double x) {return -dstepUp(x);}

inline double dstepAny(double yRange,
                       double x0, double oneOverXRange,
                       double x) 
{   double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<double>::getSignificant());
    clampInPlace(0.0,xadj,1.0);
    return yRange*oneOverXRange*dstepUp(xadj); }

inline double d2stepUp(double x) {
    assert(0 <= x && x <= 1);
    return 60*x*(1+x*(2*x-3));  //60x-180x^2+120x^3
}

inline double d2stepDown(double x) {return -d2stepUp(x);}

inline double d2stepAny(double yRange,
                        double x0, double oneOverXRange,
                        double x) 
{   double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<double>::getSignificant());
    clampInPlace(0.0,xadj,1.0);
    return yRange*square(oneOverXRange)*d2stepUp(xadj); }

inline double d3stepUp(double x) {
    assert(0 <= x && x <= 1);
    return 60+360*x*(x-1);      //60-360*x+360*x^2
}

inline double d3stepDown(double x) {return -d3stepUp(x);}

inline double d3stepAny(double yRange,
                        double x0, double oneOverXRange,
                        double x) 
{   double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<double>::getSignificant());
    clampInPlace(0.0,xadj,1.0);
    return yRange*cube(oneOverXRange)*d3stepUp(xadj); }

            // float

inline float stepUp(float x) 
{   assert(0 <= x && x <= 1);
    return x*x*x*(10+x*(6*x-15)); }  //10x^3-15x^4+6x^5
inline float stepDown(float x) {return 1.0f-stepUp(x);}
inline float stepAny(float y0, float yRange,
                     float x0, float oneOverXRange,
                     float x) 
{   float xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<float>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<float>::getSignificant());
    clampInPlace(0.0f,xadj,1.0f);
    return y0 + yRange*stepUp(xadj); }

inline float dstepUp(float x) 
{   assert(0 <= x && x <= 1);
    const float xxm1=x*(x-1);
    return 30*xxm1*xxm1; }  //30x^2-60x^3+30x^4
inline float dstepDown(float x) {return -dstepUp(x);}
inline float dstepAny(float yRange,
                      float x0, float oneOverXRange,
                      float x) 
{   float xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<float>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<float>::getSignificant());
    clampInPlace(0.0f,xadj,1.0f);
    return yRange*oneOverXRange*dstepUp(xadj); }

inline float d2stepUp(float x) 
{   assert(0 <= x && x <= 1);
    return 60*x*(1+x*(2*x-3)); }    //60x-180x^2+120x^3
inline float d2stepDown(float x) {return -d2stepUp(x);}
inline float d2stepAny(float yRange,
                       float x0, float oneOverXRange,
                       float x) 
{   float xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<float>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<float>::getSignificant());
    clampInPlace(0.0f,xadj,1.0f);
    return yRange*square(oneOverXRange)*d2stepUp(xadj); }

inline float d3stepUp(float x) 
{   assert(0 <= x && x <= 1);
    return 60+360*x*(x-1); }    //60-360*x+360*x^2
inline float d3stepDown(float x) {return -d3stepUp(x);}
inline float d3stepAny(float yRange,
                       float x0, float oneOverXRange,
                       float x) 
{   float xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<float>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<float>::getSignificant());
    clampInPlace(0.0f,xadj,1.0f);
    return yRange*cube(oneOverXRange)*d3stepUp(xadj); }

            // long double

inline long double stepUp(long double x) 
{   assert(0 <= x && x <= 1);
    return x*x*x*(10+x*(6*x-15)); }  //10x^3-15x^4+6x^5
inline long double stepDown(long double x) {return 1.0L-stepUp(x);}
inline long double stepAny(long double y0, long double yRange,
                           long double x0, long double oneOverXRange,
                           long double x) 
{   long double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<long double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<long double>::getSignificant());
    clampInPlace(0.0L,xadj,1.0L);
    return y0 + yRange*stepUp(xadj); }


inline long double dstepUp(long double x) 
{   assert(0 <= x && x <= 1);
    const long double xxm1=x*(x-1);
    return 30*xxm1*xxm1; }          //30x^2-60x^3+30x^4
inline long double dstepDown(long double x) {return -dstepUp(x);}
inline long double dstepAny(long double yRange,
                            long double x0, long double oneOverXRange,
                            long double x) 
{   long double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<long double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<long double>::getSignificant());
    clampInPlace(0.0L,xadj,1.0L);
    return yRange*oneOverXRange*dstepUp(xadj); }

inline long double d2stepUp(long double x) 
{   assert(0 <= x && x <= 1);
    return 60*x*(1+x*(2*x-3)); }    //60x-180x^2+120x^3
inline long double d2stepDown(long double x) {return -d2stepUp(x);}
inline long double d2stepAny(long double yRange,
                             long double x0, long double oneOverXRange,
                             long double x) 
{   long double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<long double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<long double>::getSignificant());
    clampInPlace(0.0L,xadj,1.0L);
    return yRange*square(oneOverXRange)*d2stepUp(xadj); }


inline long double d3stepUp(long double x) 
{   assert(0 <= x && x <= 1);
    return 60+360*x*(x-1); }        //60-360*x+360*x^2
inline long double d3stepDown(long double x) {return -d3stepUp(x);}
inline long double d3stepAny(long double yRange,
                             long double x0, long double oneOverXRange,
                             long double x) 
{   long double xadj = (x-x0)*oneOverXRange;    
    assert(-NTraits<long double>::getSignificant() <= xadj
           && xadj <= 1 + NTraits<long double>::getSignificant());
    clampInPlace(0.0L,xadj,1.0L);
    return yRange*cube(oneOverXRange)*d3stepUp(xadj); }

        // int converts to double; only supplied for stepUp(), stepDown()
inline double stepUp(int x) {return stepUp((double)x);}
inline double stepDown(int x) {return stepDown((double)x);}


 // Doxygen should skip this helper template function
template <class T> // float or double 
static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
    static const T a[] =
    {   (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
        (T)0.03742563713, (T)0.01451196212 };
    static const T b[] = 
    {   (T)0.5,           (T)0.12498593597, (T)0.06880248576,
        (T)0.03328355346, (T)0.00441787012 };
    static const T c[] =
    {   (T)1,             (T)0.44325141463, (T)0.06260601220,
        (T)0.04757383546, (T)0.01736506451 };
    static const T d[] =
    {   (T)0,             (T)0.24998368310, (T)0.09200180037,
        (T)0.04069697526, (T)0.00526449639 };

    const T SignificantReal = NTraits<T>::getSignificant();
    const T PiOver2         = NTraits<T>::getPi()/2;
    const T Infinity        = NTraits<T>::getInfinity();

    SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
        "approxCompleteEllipticIntegralsKE()", 
        "Argument m (%g) is outside the legal range [0,1].", (double)m);
    if (m >= 1) return std::make_pair(Infinity, (T)1);
    if (m <= 0) return std::make_pair(PiOver2, PiOver2);

    const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2; // 4 flops
    const T lnm2 = std::log(m1);  // ~50 flops

    // The rest is 35 flops.
    const T K = (a[0] + a[1]*m1 + a[2]*m2 + a[3]*m3 + a[4]*m4) 
         - lnm2*(b[0] + b[1]*m1 + b[2]*m2 + b[3]*m3 + b[4]*m4);
    const T E = (c[0] + c[1]*m1 + c[2]*m2 + c[3]*m3 + c[4]*m4) 
         - lnm2*(       d[1]*m1 + d[2]*m2 + d[3]*m3 + d[4]*m4);

    return std::make_pair(K,E);
}
inline std::pair<double,double> 
approxCompleteEllipticIntegralsKE(double m)
{   return approxCompleteEllipticIntegralsKE_T<double>(m); }
inline std::pair<float,float> 
approxCompleteEllipticIntegralsKE(float m)
{   return approxCompleteEllipticIntegralsKE_T<float>(m); }
inline std::pair<double,double> 
approxCompleteEllipticIntegralsKE(int m)
{   return approxCompleteEllipticIntegralsKE_T<double>((double)m); }

 // Doxygen should skip this template helper function
template <class T> // float or double
static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
    const T SignificantReal = NTraits<T>::getSignificant();
    const T TenEps          = 10*NTraits<T>::getEps();
    const T Infinity        = NTraits<T>::getInfinity();
    const T PiOver2         = NTraits<T>::getPi() / 2;

    // Allow a little slop in the argument since it may have resulted
    // from a numerical operation that gave 0 or 1 plus or minus
    // roundoff noise.
    SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
        "completeEllipticIntegralsKE()", 
        "Argument m (%g) is outside the legal range [0,1].", (double)m);
    if (m >= 1) return std::make_pair(Infinity, (T)1);
    if (m <= 0) return std::make_pair(PiOver2, PiOver2);

    const T k = std::sqrt(1-m); // ~25 flops
    T v1=1, w1=k, c1=1, d1=k*k; // initialize iteration
    do { // ~50 flops per iteration
        T v2 = (v1+w1)/2;
        T w2 = std::sqrt(v1*w1);
        T c2 = (c1+d1)/2;
        T d2 = (w1*c1+v1*d1)/(v1+w1);
        v1=v2; w1=w2; c1=c2; d1=d2;
    } while(std::abs(v1-w1) >= TenEps);

    const T K = PiOver2/v1; // ~20 flops
    const T E = K*c1;

    return std::make_pair(K,E);
}
inline std::pair<double,double> completeEllipticIntegralsKE(double m)
{   return completeEllipticIntegralsKE_T<double>(m); }
inline std::pair<float,float> completeEllipticIntegralsKE(float m)
{   return completeEllipticIntegralsKE_T<float>(m); }
inline std::pair<double,double> completeEllipticIntegralsKE(int m)
{   return completeEllipticIntegralsKE_T<double>((double)m); }

} // namespace SimTK

#endif //SimTK_SIMMATRIX_SCALAR_H_