Geo.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_H_
#define SimTK_SIMMATH_GEO_H_

/* -------------------------------------------------------------------------- *
 *                        Simbody(tm): SimTKmath                              *
 * -------------------------------------------------------------------------- *
 * 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) 2011-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.h"
#include "simmath/internal/common.h"

#include <cassert>
#include <cmath>
#include <algorithm>

namespace SimTK {

//==============================================================================
//                                    GEO
//==============================================================================
class SimTK_SIMMATH_EXPORT Geo {
public:
template <class P> class Point_;
template <class P> class Sphere_;
template <class P> class LineSeg_;
template <class P> class Line_;
template <class P> class Plane_;
template <class P> class Circle_;
template <class P> class Box_;
template <class P> class AlignedBox_;
template <class P> class OrientedBox_;
template <class P> class Triangle_;
template <class P> class CubicHermiteCurve_;
template <class P> class BicubicHermitePatch_;
template <class P> class CubicBezierCurve_;
template <class P> class BicubicBezierPatch_;

typedef Point_<Real>        Point;
typedef Sphere_<Real>       Sphere;
typedef LineSeg_<Real>      LineSeg;
typedef Line_<Real>         Line;
typedef Plane_<Real>        Plane;
typedef Circle_<Real>       Circle;
typedef Box_<Real>          Box;
typedef AlignedBox_<Real>   AlignedBox;
typedef OrientedBox_<Real>  OrientedBox;
typedef Triangle_<Real>     Triangle;
typedef CubicHermiteCurve_<Real>    CubicHermiteCurve;
typedef BicubicHermitePatch_<Real>  BicubicHermitePatch;
typedef CubicBezierCurve_<Real>     CubicBezierCurve;
typedef BicubicBezierPatch_<Real>   BicubicBezierPatch;

template <class RealP, int S> static bool
isCusp(const Vec<3,RealP,S>& Pu) 
{   return Pu.normSqr() < getDefaultTolSqr<RealP>(); }

template <class RealP, int S> static bool
isInflectionPoint(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) 
{ return (Pu % Puu).normSqr() < getDefaultTolSqr<RealP>(); }

template <class RealP, int S> static UnitVec<RealP,1> 
calcUnitTangent(const Vec<3,RealP,S>& Pu) {
    const RealP dsdu = Pu.norm();               // ~25 flops
    SimTK_ERRCHK(dsdu >= getDefaultTol<RealP>(), 
        "Geo::calcUnitTangent()", "Unit tangent undefined at a cusp.");

    return UnitVec<RealP,1>(Pu/dsdu, true);     // ~13 flops
}

template <class RealP, int S> static Vec<3,RealP> 
calcCurvatureVector(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
    const RealP Pu2 = Pu.normSqr();                 // (ds/du)^2, 5 flops
    SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(), 
        "Geo::calcCurvatureVector()", "Curvature undefined at a cusp.");
    const RealP PuPuu     = dot(Pu,Puu);
    const RealP uPrimeSqr = 1/Pu2;                          // ~10 flops
    const RealP u2Prime   = -PuPuu * square(uPrimeSqr);     //   8 flops
    return uPrimeSqr*Puu + u2Prime*Pu;                      //   9 flops
}

template <class RealP, int S> static UnitVec<RealP,1> 
calcUnitNormal(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
    typedef UnitVec<RealP,1> UnitVec3P;

    const RealP Pu2 = Pu.normSqr();                 // (ds/du)^2, 5 flops
    SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(), 
        "Geo::calcUnitNormal()", "The normal is undefined at a cusp.");

    // Now check if we're at an inflection point, meaning |Pu X Puu|==0.
    // Use this handy identity from Struik eq. 3-9 to avoid calculating
    // the cross product: (Pu X Puu)^2 = Pu^2Puu^2 - (~Pu*Puu)^2
    const RealP Puu2 = Puu.normSqr();                   // 5 flops
    const RealP PuPuu = dot(Pu,Puu);                    // 5 flops
    const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu);     // 3 flops
    if (PuXPuu2 < getDefaultTolSqr<RealP>())            // 1 flop
        return UnitVec3P(Pu).perp();
    // Calculate the curvature vector, negate, and normalize.
    const RealP uPrimeSqr = 1/Pu2;                      // ~10 flops
    const RealP u2Prime   = -PuPuu * square(uPrimeSqr); //   3 flops
    const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu;  //   9 flops
    return UnitVec3P(-c);                               // ~40 flops
}

template <class RealP, int S> static RealP 
calcCurveFrame(const Vec<3,RealP,S>&    P, 
               const Vec<3,RealP,S>&    Pu, 
               const Vec<3,RealP,S>&    Puu,
               Transform_<RealP>&       X_FP) {
    typedef UnitVec<RealP,1> UnitVec3P;

    const RealP Pu2 = Pu.normSqr();                 // (ds/du)^2, 5 flops
    SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(), 
        "Geo::calcCurveFrame()", "Curve frame is undefined at a cusp.");

    // Set the point P(u) as the frame origin.
    X_FP.updP() = P;

    // Calculate the unit tangent t, our y axis.
    const RealP uPrimeSqr = 1/Pu2;                          // ~10 flops
    const RealP uPrime    = std::sqrt(uPrimeSqr);           // ~20 flops
    const UnitVec3P t(uPrime*Pu, true);                     //   3 flops 

    // Next calculate unit normal n, our x axis. See calcUnitNormal() above
    // for theory.
    const RealP Puu2 = Puu.normSqr();                       // 5 flops
    const RealP PuPuu = dot(Pu,Puu);                        // 5 flops
    const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu);         // 3 flops
    UnitVec3P n; // unit normal
    RealP     k; // curvature magnitude
    if (PuXPuu2 < getDefaultTolSqr<RealP>()) {              // 1 flop
        k = 0;
        n = t.perp(); // arbitrary
    } else {
        // Calculate the curvature vector, negate, and normalize.
        const RealP u2Prime = -PuPuu * square(uPrimeSqr);   //   8 flops
        const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu;  //   9 flops
        k = c.norm();                       // curvature >= 0, ~25 flops
        n = UnitVec3P((-1/k)*c, true);                      // ~13 flops
    }

    // Finally calculate the binormal, our z axis. No need to normalize
    // here because n and t are perpendicular unit vectors.
    const UnitVec3P b(n % t, true);                      // 9 flops

    // Construct the coordinate frame without normalizing.
    X_FP.updR().setRotationFromUnitVecsTrustMe(n,t,b);

    return k;
}

template <class RealP, int S> static RealP
calcCurvatureSqr(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
    const RealP Pu2 = Pu.normSqr();   // (ds/du)^2, 5 flops
    SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(), 
        "Geo::calcCurvatureSqr()", "Curvature is undefined at a cusp.");
    const RealP num = cross(Pu,Puu).normSqr();      //  14 flops
    const RealP den = cube(Pu2);                    //   2 flops
    return num/den;                                 // ~10 flops
}

template <class RealP, int S> static RealP
calcTorsion(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu, 
            const Vec<3,RealP,S>& Puuu) {
    const Vec<3,RealP> PuXPuu = cross(Pu,Puu);              //   9 flops
    const RealP PuXPuu2 = PuXPuu.normSqr();                 //   5 flops  
    SimTK_ERRCHK(PuXPuu2 >= getDefaultTolSqr<RealP>(), "Geo::calcTorsion()", 
        "Torsion is undefined at a cusp or inflection point.");
    const RealP num = dot(PuXPuu, Puuu);                    //   5 flops
    return num/PuXPuu2;                                     // ~10 flops
}
template <class RealP> static void
findClosestPointsOfTwoLines
   (const Vec<3,RealP>& p0, const UnitVec<RealP,1>& d0,
    const Vec<3,RealP>& p1, const UnitVec<RealP,1>& d1,
    Vec<3,RealP>& x0, Vec<3,RealP>& x1, bool& linesAreParallel)
{
    const Vec<3,RealP> w = p1-p0; // vector from p0 to p1; 3 flops
    const RealP s2Theta = (d0 % d1).normSqr(); // sin^2(angle); 14 flops
    const RealP d =  dot(w,d0);     // 5 flops
    const RealP e = -dot(w,d1);     // 6 flops

    RealP t0, t1; // line parameters of closest points
    if (s2Theta < square(NTraits<RealP>::getSignificant())) { // 3 flops
        // Lines are parallel. Return a point on each line midway between
        // the origin points.
        linesAreParallel = true; // parallel
        t0 = d/2; t1 = e/2;      // 2 flops
    } else {
        linesAreParallel = false;
        const RealP cTheta = dot(d0,d1); // cos(angle between lines); 5 flops
        const RealP oos2Theta = 1/s2Theta; // about 10 flops
        t0 = (e*cTheta + d) * oos2Theta;   // 3 flops
        t1 = (d*cTheta + e) * oos2Theta;   // 3 flops
    }

    x0 = p0 + t0*d0;    // 6 flops
    x1 = p1 + t1*d1;    // 6 flops
}

template <class RealP> static RealP getDefaultTol() 
{   return NTraits<RealP>::getSignificant(); }
template <class RealP> static RealP getDefaultTolSqr()
{   return square(getDefaultTol<RealP>()); }

template <class RealP> static RealP getEps() 
{   return NTraits<RealP>::getEps(); }
template <class RealP> static RealP getNaN() 
{   return NTraits<RealP>::getNaN(); }
template <class RealP> static RealP getInfinity() 
{   return NTraits<RealP>::getInfinity(); }

template <class RealP> static RealP stretchBy(RealP length, RealP tol) {
    SimTK_ERRCHK2(tol >= getEps<RealP>(), "Geo::stretchBy()",
        "The supplied tolerance %g is too small; must be at least %g"
        " for significance at this precision.", 
        (double)tol, (double)getEps<RealP>());

    return length + std::max(length*tol, tol);
}

template <class RealP> static RealP stretch(RealP length)
{   return stretchBy(length, getDefaultTol<RealP>()); }

};


} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_H_