Simbody  3.5
Geo_BicubicHermitePatch.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
2 #define SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKmath *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2011-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
31 #include "SimTKcommon.h"
33 #include "simmath/internal/Geo.h"
34 
35 #include <cassert>
36 #include <cmath>
37 #include <algorithm>
38 
39 namespace SimTK {
40 
41 
42 //==============================================================================
43 // GEO BICUBIC HERMITE PATCH
44 //==============================================================================
96 template <class P>
97 class Geo::BicubicHermitePatch_ {
98 typedef P RealP;
99 typedef Vec<3,RealP> Vec3P;
100 
101 public:
105 explicit BicubicHermitePatch_(const Mat<4,4,Vec3P>& A) : A(A) {}
109 const Mat<4,4,Vec3P>& getAlgebraicCoefficients() const {return A;}
114 
118 Vec3P evalP(RealP u, RealP w) const {return evalPUsingA(A,u,w);}
119 
123 void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const
124 { return evalP1UsingA(A,u,w,Pu,Pw); }
125 
130 void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const
131 { evalP2UsingA(A,u,w,Puu,Puw,Pww); }
132 
137 void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw,
138  Vec3P& Puww, Vec3P& Pwww) const
139 { evalP3UsingA(A,u,w,Puuu,Puuw,Puww,Pwww); }
140 
148  typedef const Vec3P& Coef;
149  Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3),
150  h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3),
151  u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3),
152  u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3);
153  // First calculate Mh*H:
154  // a b c d
155  // p q r s
156  // u00 u01 t00 t01
157  // h00 h01 w00 w01
158  Vec3P a=2*(h00-h10)+u00+u10, b=2*(h01-h11)+u01+u11,
159  c=2*(w00-w10)+t00+t10, d=2*(w01-w11)+t01+t11;
160  Vec3P p=3*(h10-h00)-2*u00-u10, q=3*(h11-h01)-2*u01-u11,
161  r=3*(w10-w00)-2*t00-t10, s=3*(w11-w01)-2*t01-t11;
162 
163  // Then calculate (Mh*H)*~Mh.
164  Vec3P bma=b-a, qmp=q-p;
165  return Mat<4,4,Vec3P>
166  ( c+d-2*bma, 3*bma-2*c-d, c, a,
167  r+s-2*qmp, 3*qmp-2*r-s, r, p,
168  2*(u00-u01)+t00+t01, 3*(u01-u00)-2*t00-t01, t00, u00,
169  2*(h00-h01)+w00+w01, 3*(h01-h00)-2*w00-w01, w00, h00 );
170 }
171 
175  typedef const Vec3P& Coef;
176  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
177  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
178  a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
179  a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
180  // First calculate Mh^-1*A:
181  // a03 a02 a01 a00
182  // a b c d
183  // a13 a12 a11 a10
184  // p q r s
185  Vec3P a=a33+a23+a13+a03, b=a32+a22+a12+a02, // 3x12 flops
186  c=a31+a21+a11+a01, d=a30+a20+a10+a00;
187  Vec3P p=3*a33+2*a23+a13, q=3*a32+2*a22+a12, // 3x16 flops
188  r=3*a31+2*a21+a11, s=3*a30+2*a20+a10;
189 
190  // Then calculate (Mh^-1*A)*Mh^-T (3x28 flops)
191  return Mat<4,4,Vec3P>
192  ( a00, a03+a02+a01+a00, a01, 3*a03+2*a02+a01,
193  d, a+b+c+d, c, 3*a+2*b+c,
194  a10, a13+a12+a11+a10, a11, 3*a13+2*a12+a11,
195  s, p+q+r+s, r, 3*p+2*q+r );
196 }
197 
201 static Vec3P evalPUsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w) {
202  typedef const Vec3P& Coef;
203  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
204  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
205  a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
206  a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
207 
208  const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
209  Vec3P p = u3*(a33*w3 + a32*w2 + a31*w + a30)
210  + u2*(a23*w3 + a22*w2 + a21*w + a20)
211  + u *(a13*w3 + a12*w2 + a11*w + a10)
212  + (a03*w3 + a02*w2 + a01*w + a00);
213  return p;
214 }
215 
219 static void evalP1UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
220  Vec3P& Pu, Vec3P& Pw) {
221  typedef const Vec3P& Coef;
222  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
223  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
224  a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
225  a03=A(3,0), a02=A(3,1), a01=A(3,2);
226 
227  const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
228  Pu = 3*u2*(a33*w3 + a32*w2 + a31*w + a30)
229  + 2* u*(a23*w3 + a22*w2 + a21*w + a20)
230  + (a13*w3 + a12*w2 + a11*w + a10);
231  Pw = 3*w2*(u3*a33 + u2*a23 + u*a13 + a03)
232  + 2* w*(u3*a32 + u2*a22 + u*a12 + a02)
233  + (u3*a31 + u2*a21 + u*a11 + a01);
234 }
235 
240 static void evalP2UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
241  Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
242  typedef const Vec3P& Coef;
243  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
244  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
245  a13=A(2,0), a12=A(2,1), a11=A(2,2),
246  a03=A(3,0), a02=A(3,1);
247 
248  const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
249  Puu = 6*u*(a33*w3 + a32*w2 + a31*w + a30)
250  + 2 *(a23*w3 + a22*w2 + a21*w + a20);
251  Pww = 6*w*(u3*a33 + u2*a23 + u*a13 + a03)
252  + 2 *(u3*a32 + u2*a22 + u*a12 + a02);
253  Puw = 3*u2*(3*a33*w2 + 2*a32*w + a31)
254  + 2*u *(3*a23*w2 + 2*a22*w + a21)
255  + (3*a13*w2 + 2*a12*w + a11);
256 }
257 
262 static void evalP3UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
263  Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
264  typedef const Vec3P& Coef;
265  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
266  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
267  a13=A(2,0), a12=A(2,1),
268  a03=A(3,0), a02=A(3,1);
269 
270  const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
271  Puuu = 6*(a33*w3 + a32*w2 + a31*w + a30);
272  Pwww = 6*(u3*a33 + u2*a23 + u*a13 + a03);
273  Puuw = 6*u*(3*a33*w2 + 2*a32*w + a31)
274  + 6*a23*w2 + 4*a22*w + 2*a21;
275  Puww = 6*w*(3*u2*a33 + 2*u*a23 + a13)
276  + 6*u2*a32 + 4*u*a22 + 2*a12;
277 }
280 private:
281 Mat<4,4,Vec3P> A; // algebraic coefficients
282 };
283 
284 
285 
286 } // namespace SimTK
287 
288 #endif // SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
const Mat< 4, 4, Vec3P > & getAlgebraicCoefficients() const
Return a reference to the algebraic coefficients A that are stored in this object.
Definition: Geo_BicubicHermitePatch.h:109
BicubicHermitePatch_(const Mat< 4, 4, Vec3P > &A)
Construct a bicubic Hermite patch using the given geometry matrix B.
Definition: Geo_BicubicHermitePatch.h:105
BicubicHermitePatch_()
Construct an uninitialized patch; control points will be garbage.
Definition: Geo_BicubicHermitePatch.h:103
Vec3P evalP(RealP u, RealP w) const
Evaluate a point P(u,w) on this patch given values for the parameters u and w in [0,1].
Definition: Geo_BicubicHermitePatch.h:118
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static void evalP3UsingA(const Mat< 4, 4, Vec3P > &A, RealP u, RealP w, Vec3P &Puuu, Vec3P &Puuw, Vec3P &Puww, Vec3P &Pwww)
Given vector algebraic coefficients A and values for the curve parameters u and w in [0...
Definition: Geo_BicubicHermitePatch.h:262
void evalP2(RealP u, RealP w, Vec3P &Puu, Vec3P &Puw, Vec3P &Pww) const
Evaluate the second derivatives Puu=d2P/du2, Pww=d2P/dw2, and cross derivative Puw=Pwu=d2P/dudw on th...
Definition: Geo_BicubicHermitePatch.h:130
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
Defines geometric primitive shapes and algorthms.
static Mat< 4, 4, Vec3P > calcHFromA(const Mat< 4, 4, Vec3P > &A)
Given the vector algebraic coefficients A, return the Hermite coefficients H.
Definition: Geo_BicubicHermitePatch.h:174
void evalP1(RealP u, RealP w, Vec3P &Pu, Vec3P &Pw) const
Evaluate the tangents Pu=dP/du, Pw=dP/dw on this patch given values for the parameters u and w in [0...
Definition: Geo_BicubicHermitePatch.h:123
static void evalP2UsingA(const Mat< 4, 4, Vec3P > &A, RealP u, RealP w, Vec3P &Puu, Vec3P &Puw, Vec3P &Pww)
Given vector algebraic coefficients A and values for the curve parameters u and w in [0...
Definition: Geo_BicubicHermitePatch.h:240
This is the header file that every Simmath compilation unit should include first. ...
Mat< 4, 4, Vec3P > calcHermiteCoefficients() const
Calculate the Hermite coefficients H from the stored algebraic coefficients.
Definition: Geo_BicubicHermitePatch.h:113
static void evalP1UsingA(const Mat< 4, 4, Vec3P > &A, RealP u, RealP w, Vec3P &Pu, Vec3P &Pw)
Given vector algebraic coefficients A and values for the curve parameters u and w in [0...
Definition: Geo_BicubicHermitePatch.h:219
static Mat< 4, 4, Vec3P > calcAFromH(const Mat< 4, 4, Vec3P > &H)
Given the vector Hermite coefficients H, return the algebraic coefficients A.
Definition: Geo_BicubicHermitePatch.h:147
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
void evalP3(RealP u, RealP w, Vec3P &Puuu, Vec3P &Puuw, Vec3P &Puww, Vec3P &Pwww) const
Evaluate the third derivatives Puuu=d3P/du3, Pwww=d3P/dw3, and cross derivatives Puuw=Pwuu=Puwu=d3P/d...
Definition: Geo_BicubicHermitePatch.h:137
static Vec3P evalPUsingA(const Mat< 4, 4, Vec3P > &A, RealP u, RealP w)
Given vector algebraic coefficients A and values for the curve parameters u and w in [0...
Definition: Geo_BicubicHermitePatch.h:201