Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Geo_BicubicBezierPatch.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
2 #define SimTK_SIMMATH_GEO_BICUBIC_BEZIER_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"
32 #include "simmath/internal/common.h"
33 #include "simmath/internal/Geo.h"
34 #include "simmath/internal/Geo_CubicBezierCurve.h"
35 
36 #include <cassert>
37 #include <cmath>
38 #include <algorithm>
39 
40 namespace SimTK {
41 
42 
43 //==============================================================================
44 // GEO BICUBIC BEZIER PATCH
45 //==============================================================================
100 template <class P>
101 class Geo::BicubicBezierPatch_ {
102 typedef P RealP;
103 typedef Vec<3,RealP> Vec3P;
104 
105 public:
107 BicubicBezierPatch_() {}
108 
113 explicit BicubicBezierPatch_(const Mat<4,4,Vec3P>& controlPoints)
114 : B(controlPoints) {}
115 
116 
120 Vec3P evalP(RealP u, RealP w) const {return evalPUsingB(B,u,w);}
121 
125 void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const
126 { return evalP1UsingB(B,u,w,Pu,Pw); }
127 
132 void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const
133 { evalP2UsingB(B,u,w,Puu,Puw,Pww); }
134 
139 void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw,
140  Vec3P& Puww, Vec3P& Pwww) const
141 { evalP3UsingB(B,u,w,Puuu,Puuw,Puww,Pwww); }
142 
146 const Mat<4,4,Vec3P>& getControlPoints() const {return B;}
150 Mat<4,4,Vec3P>& updControlPoints() {return B;}
154 Mat<4,4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
158 Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);}
159 
163 CubicBezierCurve_<P> getBoundaryCurveU0() const
164 { return CubicBezierCurve_<P>(B[0]); } // b11 b12 b13 b14
168 CubicBezierCurve_<P> getBoundaryCurveU1() const
169 { return CubicBezierCurve_<P>(B[3]); } // b41 b42 b43 b44
173 CubicBezierCurve_<P> getBoundaryCurveW0() const
174 { return CubicBezierCurve_<P>(B(0)); } // b11 b21 b31 b41
178 CubicBezierCurve_<P> getBoundaryCurveW1() const
179 { return CubicBezierCurve_<P>(B(3)); } // b14 b24 b34 b44
180 
184 CubicBezierCurve_<P> calcIsoCurveU(RealP u0) const
185 { return calcIsoCurveU(B, u0); }
189 CubicBezierCurve_<P> calcIsoCurveW(RealP w0) const
190 { return calcIsoCurveW(B, w0); }
191 
206 void splitU(RealP u, BicubicBezierPatch_<P>& patch0,
207  BicubicBezierPatch_<P>& patch1) const {
208  const RealP tol = getDefaultTol<RealP>();
209  SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::BicubicBezierPatch::splitU()",
210  "Can't split patch at parameter u=%g; it is either out of range or"
211  " too close to an edge.", (double)u);
212  CubicBezierCurve_<P> l,h;
213  if (u==RealP(0.5)) // bisecting
214  for (int i=0; i<4; ++i) {
215  CubicBezierCurve_<P>(B(i)).bisect(l,h);
216  patch0.B(i) = l.getControlPoints();
217  patch1.B(i) = h.getControlPoints();
218  }
219  else
220  for (int i=0; i<4; ++i) {
221  CubicBezierCurve_<P>(B(i)).split(u,l,h);
222  patch0.B(i) = l.getControlPoints();
223  patch1.B(i) = h.getControlPoints();
224  }
225 }
226 
241 void splitW(RealP w, BicubicBezierPatch_<P>& patch0,
242  BicubicBezierPatch_<P>& patch1) const {
243  const RealP tol = getDefaultTol<RealP>();
244  SimTK_ERRCHK1(tol <= w && w <= 1-tol, "Geo::BicubicBezierPatch::splitW()",
245  "Can't split patch at parameter w=%g; it is either out of range or"
246  " too close to an edge.", (double)w);
247  CubicBezierCurve_<P> l,h;
248  if (w==RealP(0.5)) // bisecting
249  for (int i=0; i<4; ++i) {
250  CubicBezierCurve_<P>(B[i]).bisect(l,h);
251  patch0.B[i] = l.getControlPoints().positionalTranspose();
252  patch1.B[i] = h.getControlPoints().positionalTranspose();
253  }
254  else
255  for (int i=0; i<4; ++i) {
256  CubicBezierCurve_<P>(B[i]).split(w,l,h);
257  patch0.B[i] = l.getControlPoints().positionalTranspose();
258  patch1.B[i] = h.getControlPoints().positionalTranspose();
259  }
260 }
261 
262 
278 void split(RealP u, RealP w, BicubicBezierPatch_<P>& patch00,
279  BicubicBezierPatch_<P>& patch01,
280  BicubicBezierPatch_<P>& patch10,
281  BicubicBezierPatch_<P>& patch11) const {
282  const RealP tol = getDefaultTol<RealP>();
283  SimTK_ERRCHK2((tol <= u && u <= 1-tol) && (tol <= w && w <= 1-tol),
284  "Geo::BicubicBezierPatch::split()",
285  "Can't split patch at parametric point u,w=%g,%g; it is either out of"
286  " range or too close to an edge.", (double)u, (double)w);
287 
288  BicubicBezierPatch_<P> patch0, patch1; // results of the first split
289 
290  // We split once along one direction, and then twice along the other, so
291  // make sure the second direction is the cheap bisecting one.
292  if (u == Real(0.5)) {
293  splitW(w,patch0,patch1); // 120 or 180 flops
294  patch0.splitU(u, patch00, patch10); // 120 flops
295  patch1.splitU(u, patch01, patch11); // 120 flops
296  } else {
297  splitU(u,patch0,patch1); // 180 flops
298  patch0.splitW(w, patch00, patch01); // 120 or 180 flops
299  patch1.splitW(w, patch10, patch11); // 120 or 180 flops
300  }
301 }
302 
310 Geo::Sphere_<P> calcBoundingSphere() const
311 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
312  return Geo::Point_<P>::calcBoundingSphere(points); }
313 
321 Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const
322 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
323  return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }
324 
332 Geo::OrientedBox_<P> calcOrientedBoundingBox() const
333 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
334  return Geo::Point_<P>::calcOrientedBoundingBox(points); }
335 
344 static Vec3P evalPUsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w) {
345  Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
346  Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
347  return Fbu * B * ~Fbw; // 3x35 flops
348 }
349 
354 static void evalP1UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
355  Vec3P& Pu, Vec3P& Pw) {
356  Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
357  Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
358  Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
359  Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
360  Pu = dFbu * B * ~Fbw; // 3x35
361  Pw = Fbu * B * ~dFbw; // 3x35
362 }
363 
368 static void evalP2UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
369  Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
370  Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
371  Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
372  Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
373  Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
374  Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops
375  Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops
376  Puu = d2Fbu * B * ~Fbw; // 3x35
377  Puw = dFbu * B * ~dFbw; // 3x35
378  Pww = Fbu * B * ~d2Fbw; // 3x35
379 }
380 
385 static void evalP3UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
386  Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
387  Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
388  Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
389  Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
390  Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
391  Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops
392  Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops
393  Row<4,P> d3Fbu = CubicBezierCurve_<P>::calcD3Fb(u); // 0
394  Row<4,P> d3Fbw = CubicBezierCurve_<P>::calcD3Fb(w); // 0
395  Puuu = d3Fbu * B * ~Fbw; // 3x35
396  Puuw = d2Fbu * B * ~dFbw; // 3x35
397  Puww = dFbu * B * ~d2Fbw; // 3x35
398  Pwww = Fbu * B * ~d3Fbw; // 3x35
399 }
400 
404 static CubicBezierCurve_<P>
405 calcIsoCurveU(const Mat<4,4,Vec3P>& B, RealP u0)
406 { const Row<4,Vec3P> Bu0 = CubicBezierCurve_<P>::calcFb(u0) * B;
407  return CubicBezierCurve_<P>(Bu0); }
408 
412 static CubicBezierCurve_<P>
413 calcIsoCurveW(const Mat<4,4,Vec3P>& B, RealP w0)
414 { const Vec<4,Vec3P> Bw0 = B * ~CubicBezierCurve_<P>::calcFb(w0);
415  return CubicBezierCurve_<P>(Bw0); }
416 
419 static Mat<4,4,Vec3P> calcAFromB(const Mat<4,4,Vec3P>& B) {
420  typedef const Vec3P& Coef;
421  Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3),
422  b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3),
423  b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3),
424  b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3);
425  // First calculate Mb*B:
426  // a b c d
427  // e f g h
428  // p q r s
429  // b11 b12 b13 b14
430  Vec3P a= b41-b11+3*(b21-b31), b= b42-b12+3*(b22-b32), // 3x16 flops
431  c= b43-b13+3*(b23-b33), d= b44-b14+3*(b24-b34);
432  Vec3P e= 3*(b11+b31)-6*b21, f= 3*(b12+b32)-6*b22, // 3x16 flops
433  g= 3*(b13+b33)-6*b23, h= 3*(b14+b34)-6*b24;
434  Vec3P p= 3*(b21-b11), q= 3*(b22-b12), // 3x8 flops
435  r= 3*(b23-b13), s= 3*(b24-b14);
436 
437  // Then calculate (Mb*B)*~Mb. (3x40 more flops)
438  return Mat<4,4,Vec3P>
439  ( d-a+3*(b-c), 3*(a+c)-6*b, 3*(b-a), a,
440  h-e+3*(f-g), 3*(e+g)-6*f, 3*(f-e), e,
441  s-p+3*(q-r), 3*(p+r)-6*q, 3*(q-p), p,
442  b14-b11+3*(b12-b13), 3*(b11+b13)-6*b12, 3*(b12-b11), b11 );
443 }
444 
447 static Mat<4,4,Vec3P> calcBFromA(const Mat<4,4,Vec3P>& A) {
448  typedef const Vec3P& Coef;
449  Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
450  a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
451  a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
452  a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
453  // First calculate Mb^-1*A:
454  // a03 a02 a01 a00
455  // a b c d
456  // e f g h
457  // p q r s
458  Vec3P a=a13/3+a03, b=a12/3+a02, c=a11/3+a01, d=a10/3+a00; // 3x8 flops
459  Vec3P e=(a23+2*a13)/3+a03, f=(a22+2*a12)/3+a02, // 3x16 flops
460  g=(a21+2*a11)/3+a01, h=(a20+2*a10)/3+a00;
461  Vec3P p=a33+a23+a13+a03, q=a32+a22+a12+a02, // 3x12 flops
462  r=a31+a21+a11+a01, s=a30+a20+a10+a00;
463 
464 
465  // Then calculate (Mb^-1*A)*Mb^-T (3x36 more flops)
466  return Mat<4,4,Vec3P>
467  ( a00, a01/3+a00, (a02+2*a01)/3+a00, a03+a02+a01+a00,
468  d, c/3+d, (b+2*c)/3+d, a+b+c+d,
469  h, g/3+h, (f+2*g)/3+h, e+f+g+h,
470  s, r/3+s, (q+2*r) /3+s, p+q+r+s );
471 }
472 
475 static Mat<4,4,Vec3P> calcHFromB(const Mat<4,4,Vec3P>& B) {
476  typedef const Vec3P& Coef;
477  Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3),
478  b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3),
479  b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3),
480  b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3);
481 
482  // First calculate a few temps -- see class comments. (3x4 flops)
483  Vec3P b12mb11=b12-b11, b24mb14=b24-b14, b42mb41=b42-b41, b44mb34=b44-b34;
484 
485  // Then calculate (Mh^-1 Mb) B ~(Mh^-1 Mb). (3x24 more flops)
486  return Mat<4,4,Vec3P>
487  ( b11, b14, 3*b12mb11, 3*(b14-b13),
488  b41, b44, 3*b42mb41, 3*(b44-b43),
489  3*(b21-b11), 3*b24mb14, 9*(b22-b21-b12mb11), 9*(b24mb14+b13-b23),
490  3*(b41-b31), 3*b44mb34, 9*(b42mb41+b31-b32), 9*(b44mb34+b33-b43) );
491 }
492 
495 static Mat<4,4,Vec3P> calcBFromH(const Mat<4,4,Vec3P>& H) {
496  typedef const Vec3P& Coef;
497  Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3),
498  h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3),
499  u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3),
500  u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3);
501 
502  // First calculate a few temps -- see class comments. (3x8 flops)
503  Vec3P tmp00=h00+u00/3, tmp01=h01+u01/3,
504  tmp10=h10-u10/3, tmp11=h11-u11/3;
505 
506  // Then calculate (Mb^-1 Mh) H ~(Mb^-1 Mh). (3x24 more flops)
507  return Mat<4,4,Vec3P>
508  ( h00, h00+w00/3, h01-w01/3, h01,
509  tmp00, tmp00+w00/3+t00/9, tmp01-w01/3-t01/9, tmp01,
510  tmp10, tmp10+w10/3-t10/9, tmp11-w11/3+t11/9, tmp11,
511  h10, h10+w10/3, h11-w11/3, h11 );
512 }
515 private:
516 Mat<4,4,Vec3P> B; // 16 Bezier control points; see above for definition
517 };
518 
519 
520 
521 } // namespace SimTK
522 
523 #endif // SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_