Geo_CubicBezierCurve.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
#define SimTK_SIMMATH_GEO_CUBIC_BEZIER_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 "simmath/internal/Geo_Point.h"
#include "simmath/internal/Geo_Sphere.h"
#include "simmath/internal/Geo_Box.h"

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

namespace SimTK {


//==============================================================================
//                          GEO CUBIC BEZIER CURVE
//==============================================================================
template <class P>
class Geo::CubicBezierCurve_ {
typedef P               RealP;
typedef Vec<3,RealP>    Vec3P;
typedef UnitVec<P,1>    UnitVec3P;
typedef Rotation_<P>    RotationP;
typedef Transform_<P>   TransformP;

public:
CubicBezierCurve_() {}

template <int S>
explicit CubicBezierCurve_(const Vec<4,Vec3P,S>& controlPoints) 
:   B(controlPoints) {} 

template <int S>
explicit CubicBezierCurve_(const Row<4,Vec3P,S>& controlPoints) 
:   B(controlPoints.positionalTranspose()) {} 

const Vec<4,Vec3P>& getControlPoints() const {return B;}
Vec<4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
Vec<4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);}
Vec3P evalP(RealP u) const {return evalPUsingB(B,u);}
Vec3P evalPu(RealP u) const {return evalPuUsingB(B,u);}
Vec3P evalPuu(RealP u) const {return evalPuuUsingB(B,u);}
Vec3P evalPuuu(RealP u) const {return evalPuuuUsingB(B,u);}

RealP calcDsdu(RealP u) const {return evalPu(u).norm();}

UnitVec3P calcUnitTangent(RealP u) const {
    const Vec3P Pu=evalPu(u);                               //  15 flops
    return Geo::calcUnitTangent(Pu);                        // ~40 flops
}

Vec3P calcCurvatureVector(RealP u) const {
    const Vec3P Pu=evalPu(u), Puu=evalPuu(u);               //  25 flops
    return Geo::calcCurvatureVector(Pu,Puu);                // ~30 flops
}

RealP calcCurvatureSqr(RealP u) {
    const Vec3P Pu=evalPu(u), Puu=evalPuu(u);               //  25 flops
    return Geo::calcCurvatureSqr(Pu,Puu);                   // ~30 flops
}

RealP calcTorsion(RealP u) {
    const Vec3P Pu=evalPu(u), Puu=evalPuu(u), Puuu=evalPuuu(u); //  28 flops
    return Geo::calcTorsion(Pu,Puu,Puuu);
}


UnitVec3P calcUnitNormal(RealP u) const {
    const Vec3P Pu=evalPu(u), Puu=evalPuu(u);               //  25 flops
    return Geo::calcUnitNormal(Pu,Puu);                     // ~80 flops
}

RealP calcCurveFrame(RealP u, TransformP& X_FP) const {
    const Vec3P Pval=evalP(u), Pu=evalPu(u), Puu=evalPuu(u); //  45 flops
    return Geo::calcCurveFrame(Pval,Pu,Puu,X_FP);
}

void split(RealP u, CubicBezierCurve_<P>& left, 
                    CubicBezierCurve_<P>& right) const {
    const RealP tol = getDefaultTol<RealP>();
    SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::CubicBezierCurve::split()",
        "Can't split curve at parameter %g; it is either out of range or"
        " too close to an end point.", (double)u);

    const RealP u1 = 1-u;
    const Vec3P p01 = u1*B[0] + u*B[1];                     // 3x9 flops
    const Vec3P p12 = u1*B[1] + u*B[2];
    const Vec3P p23 = u1*B[2] + u*B[3];
    left.B[0] = B[0];
    left.B[1] = p01;
    left.B[2] = u1*p01 + u*p12;                             // 3x3 flops

    right.B[3] = B[3];
    right.B[2] = p23;
    right.B[1] = u1*p12 + u*p23;
    left.B[3] = right.B[0] = u1*left.B[2] + u*right.B[1];   // 3x3 flops
}

void bisect(CubicBezierCurve_<P>& left, 
            CubicBezierCurve_<P>& right) const {
    const Vec3P p01 = (B[0] + B[1])/2;                     // 3x6 flops
    const Vec3P p12 = (B[1] + B[2])/2;
    const Vec3P p23 = (B[2] + B[3])/2;
    left.B[0] = B[0];
    left.B[1] = p01;
    left.B[2] = (p01 + p12)/2;                             // 3x2 flops

    right.B[3] = B[3];
    right.B[2] = p23;
    right.B[1] = (p12 + p23)/2;
    left.B[3] = right.B[0] = (left.B[2] + right.B[1])/2;   // 3x2 flops
}


Geo::Sphere_<P> calcBoundingSphere() const 
{   return Geo::Point_<P>::calcBoundingSphere(B[0],B[1],B[2],B[3]); }

Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const 
{   const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
    return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }

Geo::OrientedBox_<P> calcOrientedBoundingBox() const 
{   const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
    return Geo::Point_<P>::calcOrientedBoundingBox(points); }

static Row<4,P> calcFb(RealP u) {
    const RealP u2 = u*u, u3 = u*u2;                // powers of u
    const RealP u1 = 1-u, u12=u1*u1, u13=u1*u12;    // powers of 1-u
    return Row<4,P>(u13, 3*u*u12, 3*u2*u1, u3); 
}

static Row<4,P> calcDFb(RealP u) {
    const RealP u6=6*u, u2 = u*u, u23 = 3*u2, u29 = 9*u2;
    return Row<4,P>(u6-u23-3, u29-12*u+3, u6-u29, u23); 
}

static Row<4,P> calcD2Fb(RealP u) {
    const RealP u6  = 6*u, u18 = 18*u;
    return Row<4,P>(6-u6, u18-12, 6-u18, u6); 
}

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

template <int S>
static Vec<4,Vec3P> calcAFromB(const Vec<4,Vec3P,S>& B) {
    const Vec3P& b0=B[0]; const Vec3P& b1=B[1];    // aliases for beauty
    const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
    return Vec<4,Vec3P>(b3-b0+3*(b1-b2), 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
}

template <int S>
static Vec<4,Vec3P> calcBFromA(const Vec<4,Vec3P,S>& 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, a1/3 + a0, (a2+2*a1)/3 + a0, a3+a2+a1+a0);
}

template <int S>
static Vec<4,Vec3P> calcHFromB(const Vec<4,Vec3P,S>& B) {
    const Vec3P& b0=B[0]; const Vec3P& b1=B[1];    // aliases for beauty
    const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
    return Vec<4,Vec3P>(b0, b3, 3*(b1-b0), 3*(b3-b2));
}

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

template <int S>
static Vec3P evalPUsingB(const Vec<4,Vec3P,S>& B, RealP u) { 
    return calcFb(u)*B; // 9 + 3*7 = 30 flops
}
template <int S>
static Vec3P evalPuUsingB(const Vec<4,Vec3P,S>& B, RealP u) { 
    return calcDFb(u)*B; // 10 + 3*7 = 31 flops
}
template <int S>
static Vec3P evalPuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) { 
    return calcD2Fb(u)*B; // 5 + 3*7 = 26 flops
}
template <int S>
static Vec3P evalPuuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) { 
    return calcD3Fb(u)*B; // 0 + 3*7 = 21 flops
}
static Mat<4,4,P> getMb() {
    return Mat<4,4,P>( -1,  3, -3,  1,
                        3, -6,  3,  0,
                       -3,  3,  0,  0,
                        1,  0,  0,  0);
}

template <int S>
static Vec<4,P> multiplyByMb(const Vec<4,P,S>& b) {
    const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
    return Vec<4,P>(3*(b1-b2)+b3-b0, 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
}

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

template <int S>
static Vec<4,P> multiplyByMbInv(const Vec<4,P,S>& b) {
    const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
    return Vec<4,P>(b3, b2/3+b3, (b1+2*b2)/3+b3, b0+b1+b2+b3);
}

static Mat<4,4,P> getMhInvMb() {
    return Mat<4,4,P>(  1,  0,  0,  0,
                        0,  0,  0,  1,
                       -3,  3,  0,  0,
                        0,  0, -3,  3 );
}
template <int S>
static Vec<4,P> multiplyByMhInvMb(const Vec<4,P,S>& v) {
    const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
    return Vec<4,P>(v0, v3, 3*(v1-v0), 3*(v3-v2));
}

static Mat<4,4,P> getMbInvMh() {
    return Mat<4,4,P>(  1,  0,    0,      0,
                        1,  0, P(1)/3,    0,
                        0,  1,    0,   P(-1)/3,
                        0,  1,    0,      0 );
}
template <int S>
static Vec<4,P> multiplyByMbInvMh(const Vec<4,P,S>& v) {
    const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
    return Vec<4,P>(v0, v0+v2/3, v1-v3/3, v1);
}
private:
Vec<4,Vec3P> B;
};



} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_