Simbody  3.8
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>
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_
Defines geometric primitive shapes and algorthms.
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
A primitive useful for computations involving a single bicubic Hermite patch.
Definition: Geo_BicubicHermitePatch.h:97
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
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
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
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
BicubicHermitePatch_()
Construct an uninitialized patch; control points will be garbage.
Definition: Geo_BicubicHermitePatch.h:103
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
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 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
Mat< 4, 4, Vec3P > calcHermiteCoefficients() const
Calculate the Hermite coefficients H from the stored algebraic coefficients.
Definition: Geo_BicubicHermitePatch.h:113
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
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,...
Definition: Geo_BicubicHermitePatch.h:118
BicubicHermitePatch_(const Mat< 4, 4, Vec3P > &A)
Construct a bicubic Hermite patch using the given geometry matrix B.
Definition: Geo_BicubicHermitePatch.h:105
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
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
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37