Geo_CubicHermiteCurve.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
#define SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_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 "simmath/internal/Geo.h"

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

namespace SimTK {


//==============================================================================
//                         GEO CUBIC HERMITE CURVE
//==============================================================================
template <class P>
class Geo::CubicHermiteCurve_ {
typedef P               RealP;
typedef Vec<3,RealP>    Vec3P;

public:
CubicHermiteCurve_() {}
explicit CubicHermiteCurve_(const Vec<4,Vec3P>& A) : A(A) {} 
const Vec<4,Vec3P>& getAlgebraicCoefficients() const {return A;}
Vec<4,Vec3P> calcGeometricCoefficients() const {return calcHFromA(A);}
Vec3P evalP(RealP u) const {return evalPUsingA(A,u);}
Vec3P evalPu(RealP u) const {return evalPuUsingA(A,u);}
Vec3P evalPuu(RealP u) const {return evalPuuUsingA(A,u);}
Vec3P evalPuuu(RealP u) const {return evalPuuuUsingA(A,u);}

static Row<4,P> calcU(RealP u) {
    const RealP u2 = u*u;
    return Row<4,P>(u*u2, u2, u, 1);
}

static Row<4,P> calcFh(RealP u) {
    const RealP u2 = u*u, u3 = u*u2;
    const RealP t = 3*u2 - 2*u3;
    return Row<4,P>(1-t, t, u3-2*u2+u, u3-u2); 
}

static Row<4,P> calcFhu(RealP u) {
    const RealP u2 = u*u, u23=3*u2;
    const RealP dt = 6*(u-u2);
    return Row<4,P>(-dt, dt, u23-4*u+1, u23-2*u); 
}

static Row<4,P> calcFhuu(RealP u) {
    const RealP u6  = 6*u;
    const RealP ddt = 6 - 12*u;
    return Row<4,P>(-ddt, ddt, u6-4, u6-2); 
}

static Row<4,P> calcFhuuu(RealP u) {
    return Row<4,P>(12, -12, 6, 6); 
}

static Vec<4,Vec3P> calcAFromH(const Vec<4,Vec3P>& H) {
    const Vec3P& h0= H[0]; const Vec3P& h1= H[1];    // aliases for beauty
    const Vec3P& hu0=H[2]; const Vec3P& hu1=H[3];
    const Vec3P h01 = h0 - h1;
    return Vec<4,Vec3P>( 2*h01 +   hu0 + hu1,
                        -3*h01 - 2*hu0 - hu1,
                         hu0,
                         h0 );
}

static Vec<4,Vec3P> calcHFromA(const Vec<4,Vec3P>& A) {
    const Vec3P& a3=A[0]; const Vec3P& a2=A[1];    // aliases for beauty
    const Vec3P& a1=A[2]; const Vec3P& a0=A[3];
    return Vec<4,Vec3P>(a0, a3+a2+a1+a0, a1, 3*a3+2*a2+a1);
}

static Vec3P evalPUsingA(const Vec<4,Vec3P>& A, RealP u) { 
    const RealP u2 = u*u, u3 = u*u2;
    return u3*A[0] + u2*A[1] + u*A[2] + A[3]; // (vectors)
}
static Vec3P evalPuUsingA(const Vec<4,Vec3P>& A, RealP u) { 
    return (3*u*u)*A[0] + (2*u)*A[1] + A[2];
}
static Vec3P evalPuuUsingA(const Vec<4,Vec3P>& A, RealP u) { 
    return (6*u)*A[0] + 2*A[1];
}
static Vec3P evalPuuuUsingA(const Vec<4,Vec3P>& A, RealP u) { 
    return 6*A[0];
}

static Vec3P evalPUsingH(const Vec<4,Vec3P>& H, RealP u) { 
    return calcFh(u)*H; // 10 + 3*7 = 31 flops
}
static Vec3P evalPuUsingH(const Vec<4,Vec3P>& H, RealP u) { 
    return calcFhu(u)*H; // 10 + 3*7 = 31 flops
}
static Vec3P evalPuuUsingH(const Vec<4,Vec3P>& H, RealP u) { 
    return calcFhuu(u)*H; // 6 + 3*7 = 27 flops
}
static Vec3P evalPuuuUsingH(const Vec<4,Vec3P>& H, RealP u) { 
    return calcFhuuu(u)*H; // 0 + 3*7 = 21 flops
}

static Mat<4,4,P> getMh() {
    return Mat<4,4,P>( 2, -2,  1,  1,
                      -3,  3, -2, -1,
                       0,  0,  1,  0,
                       1,  0,  0,  0 );
}

static Vec<4,P> multiplyByMh(const Vec<4,P>& v) {
    const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
    const RealP v01 = v0-v1;
    return Vec<4,P>(2*v01+v2+v3, -3*v01-2*v2-v3, v2, v0);
}

static Mat<4,4,P> getMhInv() {
    return Mat<4,4,P>( 0,  0,  0,  1,
                       1,  1,  1,  1,
                       0,  0,  1,  0,
                       3,  2,  1,  0 );
}

static Vec<4,P> multiplyByMhInv(const Vec<4,P>& v) {
    const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
    return Vec<4,P>(v3, v0+v1+v2+v3, v2, 3*v0+2*v1+v2);
}
private:
Vec<4,Vec3P> A;
};



} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_