Geo_BicubicHermitePatch.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
#define SimTK_SIMMATH_GEO_BICUBIC_HERMITE_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 <cassert>
#include <cmath>
#include <algorithm>

namespace SimTK {


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

public:
BicubicHermitePatch_() {}
explicit BicubicHermitePatch_(const Mat<4,4,Vec3P>& A) : A(A) {} 
const Mat<4,4,Vec3P>& getAlgebraicCoefficients() const {return A;}
Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromA(A);}

Vec3P evalP(RealP u, RealP w) const {return evalPUsingA(A,u,w);}

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

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

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

static Mat<4,4,Vec3P> calcAFromH(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 Mh*H:
    //       a   b   c   d
    //       p   q   r   s
    //      u00 u01 t00 t01
    //      h00 h01 w00 w01
    Vec3P a=2*(h00-h10)+u00+u10,   b=2*(h01-h11)+u01+u11,
          c=2*(w00-w10)+t00+t10,   d=2*(w01-w11)+t01+t11;
    Vec3P p=3*(h10-h00)-2*u00-u10, q=3*(h11-h01)-2*u01-u11,
          r=3*(w10-w00)-2*t00-t10, s=3*(w11-w01)-2*t01-t11;

    // Then calculate (Mh*H)*~Mh.
    Vec3P bma=b-a, qmp=q-p;
    return Mat<4,4,Vec3P>
       (     c+d-2*bma,            3*bma-2*c-d,        c,   a,
             r+s-2*qmp,            3*qmp-2*r-s,        r,   p,
        2*(u00-u01)+t00+t01,  3*(u01-u00)-2*t00-t01,  t00, u00,
        2*(h00-h01)+w00+w01,  3*(h01-h00)-2*w00-w01,  w00, h00  );
}

static Mat<4,4,Vec3P> calcHFromA(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 Mh^-1*A:
    //      a03 a02 a01 a00
    //       a   b   c   d
    //      a13 a12 a11 a10
    //       p   q   r   s
    Vec3P a=a33+a23+a13+a03, b=a32+a22+a12+a02, // 3x12 flops
          c=a31+a21+a11+a01, d=a30+a20+a10+a00;
    Vec3P p=3*a33+2*a23+a13, q=3*a32+2*a22+a12, // 3x16 flops
          r=3*a31+2*a21+a11, s=3*a30+2*a20+a10;

    // Then calculate (Mh^-1*A)*Mh^-T (3x28 flops)
    return Mat<4,4,Vec3P>
       ( a00,  a03+a02+a01+a00,  a01,  3*a03+2*a02+a01,
          d,       a+b+c+d,       c,      3*a+2*b+c,
         a10,  a13+a12+a11+a10,  a11,  3*a13+2*a12+a11,
          s,       p+q+r+s,       r,      3*p+2*q+r     );
}

static Vec3P evalPUsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w) { 
    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); 

    const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
    Vec3P p =   u3*(a33*w3 + a32*w2 + a31*w + a30)
              + u2*(a23*w3 + a22*w2 + a21*w + a20)
              + u *(a13*w3 + a12*w2 + a11*w + a10)
              +    (a03*w3 + a02*w2 + a01*w + a00);
    return p;
}

static void evalP1UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
                         Vec3P& Pu, Vec3P& Pw) {
    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); 

    const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
    Pu =   3*u2*(a33*w3 + a32*w2 + a31*w + a30)
         + 2* u*(a23*w3 + a22*w2 + a21*w + a20)
         +      (a13*w3 + a12*w2 + a11*w + a10);
    Pw =   3*w2*(u3*a33 + u2*a23 + u*a13 + a03)
         + 2* w*(u3*a32 + u2*a22 + u*a12 + a02)
         +      (u3*a31 + u2*a21 + u*a11 + a01);
}

static void evalP2UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
                         Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
    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),
         a03=A(3,0), a02=A(3,1); 

    const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
    Puu =   6*u*(a33*w3 + a32*w2 + a31*w + a30)
          + 2  *(a23*w3 + a22*w2 + a21*w + a20);
    Pww =   6*w*(u3*a33 + u2*a23 + u*a13 + a03)
          + 2  *(u3*a32 + u2*a22 + u*a12 + a02);
    Puw =   3*u2*(3*a33*w2 + 2*a32*w + a31)
          + 2*u *(3*a23*w2 + 2*a22*w + a21)
          +      (3*a13*w2 + 2*a12*w + a11);
}

static void evalP3UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
                         Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
    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), 
         a03=A(3,0), a02=A(3,1); 

    const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
    Puuu = 6*(a33*w3 + a32*w2 + a31*w + a30);
    Pwww = 6*(u3*a33 + u2*a23 + u*a13 + a03);
    Puuw = 6*u*(3*a33*w2 + 2*a32*w + a31) 
           +    6*a23*w2 + 4*a22*w + 2*a21;
    Puww = 6*w*(3*u2*a33 + 2*u*a23 + a13) 
           +    6*u2*a32 + 4*u*a22 + 2*a12;
}
private:
Mat<4,4,Vec3P> A;   // algebraic coefficients
};



} // namespace SimTK

#endif // SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_