Geo_BicubicBezierPatch.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
#define SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_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_CubicBezierCurve.h"

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

namespace SimTK {


//==============================================================================
//                         GEO BICUBIC BEZIER PATCH
//==============================================================================
template <class P>
class Geo::BicubicBezierPatch_ {
typedef P               RealP;
typedef Vec<3,RealP>    Vec3P;

public:
BicubicBezierPatch_() {}

explicit BicubicBezierPatch_(const Mat<4,4,Vec3P>& controlPoints) 
:   B(controlPoints) {} 


Vec3P evalP(RealP u, RealP w) const {return evalPUsingB(B,u,w);}

void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const 
{   return evalP1UsingB(B,u,w,Pu,Pw); }

void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const 
{   evalP2UsingB(B,u,w,Puu,Puw,Pww); }

void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw, 
                              Vec3P& Puww, Vec3P& Pwww) const 
{   evalP3UsingB(B,u,w,Puuu,Puuw,Puww,Pwww); }

const Mat<4,4,Vec3P>& getControlPoints() const {return B;}
Mat<4,4,Vec3P>& updControlPoints() {return B;}
Mat<4,4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);}

CubicBezierCurve_<P> getBoundaryCurveU0() const 
{   return CubicBezierCurve_<P>(B[0]); } // b11 b12 b13 b14
CubicBezierCurve_<P> getBoundaryCurveU1() const 
{   return CubicBezierCurve_<P>(B[3]); } // b41 b42 b43 b44
CubicBezierCurve_<P> getBoundaryCurveW0() const 
{   return CubicBezierCurve_<P>(B(0)); } // b11 b21 b31 b41
CubicBezierCurve_<P> getBoundaryCurveW1() const 
{   return CubicBezierCurve_<P>(B(3)); } // b14 b24 b34 b44

CubicBezierCurve_<P> calcIsoCurveU(RealP u0) const 
{   return calcIsoCurveU(B, u0); }
CubicBezierCurve_<P> calcIsoCurveW(RealP w0) const 
{   return calcIsoCurveW(B, w0); }

void splitU(RealP u, BicubicBezierPatch_<P>& patch0, 
                     BicubicBezierPatch_<P>& patch1) const {
    const RealP tol = getDefaultTol<RealP>();
    SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::BicubicBezierPatch::splitU()",
        "Can't split patch at parameter u=%g; it is either out of range or"
        " too close to an edge.", (double)u);
    CubicBezierCurve_<P> l,h;
    if (u==RealP(0.5)) // bisecting
        for (int i=0; i<4; ++i) {
            CubicBezierCurve_<P>(B(i)).bisect(l,h);
            patch0.B(i) = l.getControlPoints(); 
            patch1.B(i) = h.getControlPoints();
        }
    else 
        for (int i=0; i<4; ++i) {
            CubicBezierCurve_<P>(B(i)).split(u,l,h);
            patch0.B(i) = l.getControlPoints(); 
            patch1.B(i) = h.getControlPoints();
        }
}

void splitW(RealP w, BicubicBezierPatch_<P>& patch0, 
                     BicubicBezierPatch_<P>& patch1) const {
    const RealP tol = getDefaultTol<RealP>();
    SimTK_ERRCHK1(tol <= w && w <= 1-tol, "Geo::BicubicBezierPatch::splitW()",
        "Can't split patch at parameter w=%g; it is either out of range or"
        " too close to an edge.", (double)w);
    CubicBezierCurve_<P> l,h;
    if (w==RealP(0.5)) // bisecting
        for (int i=0; i<4; ++i) {
            CubicBezierCurve_<P>(B[i]).bisect(l,h);
            patch0.B[i] = l.getControlPoints().positionalTranspose(); 
            patch1.B[i] = h.getControlPoints().positionalTranspose();
        }
    else 
        for (int i=0; i<4; ++i) {
            CubicBezierCurve_<P>(B[i]).split(w,l,h);
            patch0.B[i] = l.getControlPoints().positionalTranspose(); 
            patch1.B[i] = h.getControlPoints().positionalTranspose();
        }
}


void split(RealP u, RealP w, BicubicBezierPatch_<P>& patch00, 
                             BicubicBezierPatch_<P>& patch01, 
                             BicubicBezierPatch_<P>& patch10, 
                             BicubicBezierPatch_<P>& patch11) const {
    const RealP tol = getDefaultTol<RealP>();
    SimTK_ERRCHK2((tol <= u && u <= 1-tol) && (tol <= w && w <= 1-tol), 
        "Geo::BicubicBezierPatch::split()",
        "Can't split patch at parametric point u,w=%g,%g; it is either out of"
        " range or too close to an edge.", (double)u, (double)w);

    BicubicBezierPatch_<P> patch0, patch1; // results of the first split

    // We split once along one direction, and then twice along the other, so
    // make sure the second direction is the cheap bisecting one.
    if (u == Real(0.5)) {
        splitW(w,patch0,patch1);            // 120 or 180 flops
        patch0.splitU(u, patch00, patch10); // 120 flops
        patch1.splitU(u, patch01, patch11); // 120 flops
    } else {
        splitU(u,patch0,patch1);            // 180 flops
        patch0.splitW(w, patch00, patch01); // 120 or 180 flops
        patch1.splitW(w, patch10, patch11); // 120 or 180 flops
    }
}

Geo::Sphere_<P> calcBoundingSphere() const 
{   const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 
    return Geo::Point_<P>::calcBoundingSphere(points); }

Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const 
{   const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 
    return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }

Geo::OrientedBox_<P> calcOrientedBoundingBox() const 
{   const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 
    return Geo::Point_<P>::calcOrientedBoundingBox(points); }

static Vec3P evalPUsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w) { 
    Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u);     // 9 flops
    Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w);     // 9 flops
    return Fbu * B * ~Fbw;                              // 3x35 flops
}

static void evalP1UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
                         Vec3P& Pu, Vec3P& Pw) {
    Row<4,P> Fbu  = CubicBezierCurve_<P>::calcFb(u);     //  9 flops
    Row<4,P> Fbw  = CubicBezierCurve_<P>::calcFb(w);     //  9 flops
    Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u);    // 10 flops
    Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w);    // 10 flops
    Pu = dFbu * B * ~Fbw;                                // 3x35
    Pw = Fbu  * B * ~dFbw;                               // 3x35
}

static void evalP2UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
                         Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
    Row<4,P> Fbu   = CubicBezierCurve_<P>::calcFb(u);     //  9 flops
    Row<4,P> Fbw   = CubicBezierCurve_<P>::calcFb(w);     //  9 flops
    Row<4,P> dFbu  = CubicBezierCurve_<P>::calcDFb(u);    // 10 flops
    Row<4,P> dFbw  = CubicBezierCurve_<P>::calcDFb(w);    // 10 flops
    Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u);   //  5 flops
    Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w);   //  5 flops
    Puu = d2Fbu * B * ~Fbw;                               // 3x35
    Puw = dFbu  * B * ~dFbw;                              // 3x35
    Pww = Fbu   * B * ~d2Fbw;                             // 3x35
}

static void evalP3UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
                         Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
    Row<4,P> Fbu   = CubicBezierCurve_<P>::calcFb(u);     //  9 flops
    Row<4,P> Fbw   = CubicBezierCurve_<P>::calcFb(w);     //  9 flops
    Row<4,P> dFbu  = CubicBezierCurve_<P>::calcDFb(u);    // 10 flops
    Row<4,P> dFbw  = CubicBezierCurve_<P>::calcDFb(w);    // 10 flops
    Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u);   //  5 flops
    Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w);   //  5 flops
    Row<4,P> d3Fbu = CubicBezierCurve_<P>::calcD3Fb(u);   //  0 
    Row<4,P> d3Fbw = CubicBezierCurve_<P>::calcD3Fb(w);   //  0
    Puuu = d3Fbu * B * ~Fbw;                              // 3x35
    Puuw = d2Fbu * B * ~dFbw;                             // 3x35
    Puww = dFbu  * B * ~d2Fbw;                            // 3x35
    Pwww = Fbu   * B * ~d3Fbw;                            // 3x35
}

static CubicBezierCurve_<P> 
calcIsoCurveU(const Mat<4,4,Vec3P>& B, RealP u0) 
{   const Row<4,Vec3P> Bu0 = CubicBezierCurve_<P>::calcFb(u0) * B;
    return CubicBezierCurve_<P>(Bu0); }

static CubicBezierCurve_<P> 
calcIsoCurveW(const Mat<4,4,Vec3P>& B, RealP w0)
{    const Vec<4,Vec3P> Bw0 = B * ~CubicBezierCurve_<P>::calcFb(w0);
     return CubicBezierCurve_<P>(Bw0); }

static Mat<4,4,Vec3P> calcAFromB(const Mat<4,4,Vec3P>& B) {
    typedef const Vec3P& Coef;
    Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3), 
         b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3), 
         b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3), 
         b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3); 
    // First calculate Mb*B:
    //       a   b   c   d
    //       e   f   g   h
    //       p   q   r   s
    //      b11 b12 b13 b14
    Vec3P a= b41-b11+3*(b21-b31), b= b42-b12+3*(b22-b32),   // 3x16 flops
          c= b43-b13+3*(b23-b33), d= b44-b14+3*(b24-b34);
    Vec3P e= 3*(b11+b31)-6*b21,   f= 3*(b12+b32)-6*b22,     // 3x16 flops
          g= 3*(b13+b33)-6*b23,   h= 3*(b14+b34)-6*b24;
    Vec3P p= 3*(b21-b11),         q= 3*(b22-b12),           // 3x8 flops
          r= 3*(b23-b13),         s= 3*(b24-b14);

    // Then calculate (Mb*B)*~Mb. (3x40 more flops)
    return Mat<4,4,Vec3P>
       ( d-a+3*(b-c),          3*(a+c)-6*b,       3*(b-a),        a,
         h-e+3*(f-g),          3*(e+g)-6*f,       3*(f-e),        e,
         s-p+3*(q-r),          3*(p+r)-6*q,       3*(q-p),        p,
         b14-b11+3*(b12-b13),  3*(b11+b13)-6*b12, 3*(b12-b11),   b11 );
}

static Mat<4,4,Vec3P> calcBFromA(const Mat<4,4,Vec3P>& A) {
    typedef const Vec3P& Coef;
    Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3), 
         a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3), 
         a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3), 
         a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3); 
    // First calculate Mb^-1*A:
    //      a03 a02 a01 a00
    //       a   b   c   d
    //       e   f   g   h
    //       p   q   r   s
    Vec3P a=a13/3+a03, b=a12/3+a02, c=a11/3+a01, d=a10/3+a00;   // 3x8 flops
    Vec3P e=(a23+2*a13)/3+a03, f=(a22+2*a12)/3+a02,             // 3x16 flops
          g=(a21+2*a11)/3+a01, h=(a20+2*a10)/3+a00;
    Vec3P p=a33+a23+a13+a03, q=a32+a22+a12+a02,                 // 3x12 flops
          r=a31+a21+a11+a01, s=a30+a20+a10+a00;


    // Then calculate (Mb^-1*A)*Mb^-T (3x36 more flops)
    return Mat<4,4,Vec3P>
       ( a00,   a01/3+a00,  (a02+2*a01)/3+a00,     a03+a02+a01+a00,
          d,      c/3+d,       (b+2*c)/3+d,             a+b+c+d,
          h,      g/3+h,       (f+2*g)/3+h,             e+f+g+h,
          s,      r/3+s,       (q+2*r) /3+s,            p+q+r+s     );
}

static Mat<4,4,Vec3P> calcHFromB(const Mat<4,4,Vec3P>& B) {
    typedef const Vec3P& Coef;
    Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3), 
         b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3), 
         b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3), 
         b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3); 

    // First calculate a few temps -- see class comments. (3x4 flops)
    Vec3P b12mb11=b12-b11, b24mb14=b24-b14, b42mb41=b42-b41, b44mb34=b44-b34;

    // Then calculate (Mh^-1 Mb) B ~(Mh^-1 Mb). (3x24 more flops)
    return Mat<4,4,Vec3P>
       (     b11,         b14,          3*b12mb11,           3*(b14-b13),
             b41,         b44,          3*b42mb41,           3*(b44-b43),
         3*(b21-b11),  3*b24mb14,  9*(b22-b21-b12mb11),  9*(b24mb14+b13-b23),
         3*(b41-b31),  3*b44mb34,  9*(b42mb41+b31-b32),  9*(b44mb34+b33-b43) );
}

static Mat<4,4,Vec3P> calcBFromH(const Mat<4,4,Vec3P>& H) {
    typedef const Vec3P& Coef;
    Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3), 
         h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3), 
         u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3), 
         u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3); 

    // First calculate a few temps -- see class comments. (3x8 flops)
    Vec3P tmp00=h00+u00/3, tmp01=h01+u01/3, 
          tmp10=h10-u10/3, tmp11=h11-u11/3;

    // Then calculate (Mb^-1 Mh) H ~(Mb^-1 Mh). (3x24 more flops)
    return Mat<4,4,Vec3P>
       (  h00,       h00+w00/3,          h01-w01/3,       h01,
         tmp00,  tmp00+w00/3+t00/9,  tmp01-w01/3-t01/9,  tmp01,
         tmp10,  tmp10+w10/3-t10/9,  tmp11-w11/3+t11/9,  tmp11,
          h10,       h10+w10/3,          h11-w11/3,       h11  );
}
private:
Mat<4,4,Vec3P> B;   // 16 Bezier control points; see above for definition
};



} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_