|
Simbody
3.4 (development)
|
|
Simbody
3.4 (development)
|
00001 #ifndef SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_ 00002 #define SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm): SimTKmath * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2011-12 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00031 #include "SimTKcommon.h" 00032 #include "simmath/internal/common.h" 00033 #include "simmath/internal/Geo.h" 00034 #include "simmath/internal/Geo_CubicBezierCurve.h" 00035 00036 #include <cassert> 00037 #include <cmath> 00038 #include <algorithm> 00039 00040 namespace SimTK { 00041 00042 00043 //============================================================================== 00044 // GEO BICUBIC BEZIER PATCH 00045 //============================================================================== 00100 template <class P> 00101 class Geo::BicubicBezierPatch_ { 00102 typedef P RealP; 00103 typedef Vec<3,RealP> Vec3P; 00104 00105 public: 00107 BicubicBezierPatch_() {} 00108 00113 explicit BicubicBezierPatch_(const Mat<4,4,Vec3P>& controlPoints) 00114 : B(controlPoints) {} 00115 00116 00120 Vec3P evalP(RealP u, RealP w) const {return evalPUsingB(B,u,w);} 00121 00125 void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const 00126 { return evalP1UsingB(B,u,w,Pu,Pw); } 00127 00132 void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const 00133 { evalP2UsingB(B,u,w,Puu,Puw,Pww); } 00134 00139 void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw, 00140 Vec3P& Puww, Vec3P& Pwww) const 00141 { evalP3UsingB(B,u,w,Puuu,Puuw,Puww,Pwww); } 00142 00146 const Mat<4,4,Vec3P>& getControlPoints() const {return B;} 00150 Mat<4,4,Vec3P>& updControlPoints() {return B;} 00154 Mat<4,4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);} 00158 Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);} 00159 00163 CubicBezierCurve_<P> getBoundaryCurveU0() const 00164 { return CubicBezierCurve_<P>(B[0]); } // b11 b12 b13 b14 00168 CubicBezierCurve_<P> getBoundaryCurveU1() const 00169 { return CubicBezierCurve_<P>(B[3]); } // b41 b42 b43 b44 00173 CubicBezierCurve_<P> getBoundaryCurveW0() const 00174 { return CubicBezierCurve_<P>(B(0)); } // b11 b21 b31 b41 00178 CubicBezierCurve_<P> getBoundaryCurveW1() const 00179 { return CubicBezierCurve_<P>(B(3)); } // b14 b24 b34 b44 00180 00184 CubicBezierCurve_<P> calcIsoCurveU(RealP u0) const 00185 { return calcIsoCurveU(B, u0); } 00189 CubicBezierCurve_<P> calcIsoCurveW(RealP w0) const 00190 { return calcIsoCurveW(B, w0); } 00191 00206 void splitU(RealP u, BicubicBezierPatch_<P>& patch0, 00207 BicubicBezierPatch_<P>& patch1) const { 00208 const RealP tol = getDefaultTol<RealP>(); 00209 SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::BicubicBezierPatch::splitU()", 00210 "Can't split patch at parameter u=%g; it is either out of range or" 00211 " too close to an edge.", (double)u); 00212 CubicBezierCurve_<P> l,h; 00213 if (u==RealP(0.5)) // bisecting 00214 for (int i=0; i<4; ++i) { 00215 CubicBezierCurve_<P>(B(i)).bisect(l,h); 00216 patch0.B(i) = l.getControlPoints(); 00217 patch1.B(i) = h.getControlPoints(); 00218 } 00219 else 00220 for (int i=0; i<4; ++i) { 00221 CubicBezierCurve_<P>(B(i)).split(u,l,h); 00222 patch0.B(i) = l.getControlPoints(); 00223 patch1.B(i) = h.getControlPoints(); 00224 } 00225 } 00226 00241 void splitW(RealP w, BicubicBezierPatch_<P>& patch0, 00242 BicubicBezierPatch_<P>& patch1) const { 00243 const RealP tol = getDefaultTol<RealP>(); 00244 SimTK_ERRCHK1(tol <= w && w <= 1-tol, "Geo::BicubicBezierPatch::splitW()", 00245 "Can't split patch at parameter w=%g; it is either out of range or" 00246 " too close to an edge.", (double)w); 00247 CubicBezierCurve_<P> l,h; 00248 if (w==RealP(0.5)) // bisecting 00249 for (int i=0; i<4; ++i) { 00250 CubicBezierCurve_<P>(B[i]).bisect(l,h); 00251 patch0.B[i] = l.getControlPoints().positionalTranspose(); 00252 patch1.B[i] = h.getControlPoints().positionalTranspose(); 00253 } 00254 else 00255 for (int i=0; i<4; ++i) { 00256 CubicBezierCurve_<P>(B[i]).split(w,l,h); 00257 patch0.B[i] = l.getControlPoints().positionalTranspose(); 00258 patch1.B[i] = h.getControlPoints().positionalTranspose(); 00259 } 00260 } 00261 00262 00278 void split(RealP u, RealP w, BicubicBezierPatch_<P>& patch00, 00279 BicubicBezierPatch_<P>& patch01, 00280 BicubicBezierPatch_<P>& patch10, 00281 BicubicBezierPatch_<P>& patch11) const { 00282 const RealP tol = getDefaultTol<RealP>(); 00283 SimTK_ERRCHK2((tol <= u && u <= 1-tol) && (tol <= w && w <= 1-tol), 00284 "Geo::BicubicBezierPatch::split()", 00285 "Can't split patch at parametric point u,w=%g,%g; it is either out of" 00286 " range or too close to an edge.", (double)u, (double)w); 00287 00288 BicubicBezierPatch_<P> patch0, patch1; // results of the first split 00289 00290 // We split once along one direction, and then twice along the other, so 00291 // make sure the second direction is the cheap bisecting one. 00292 if (u == Real(0.5)) { 00293 splitW(w,patch0,patch1); // 120 or 180 flops 00294 patch0.splitU(u, patch00, patch10); // 120 flops 00295 patch1.splitU(u, patch01, patch11); // 120 flops 00296 } else { 00297 splitU(u,patch0,patch1); // 180 flops 00298 patch0.splitW(w, patch00, patch01); // 120 or 180 flops 00299 patch1.splitW(w, patch10, patch11); // 120 or 180 flops 00300 } 00301 } 00302 00310 Geo::Sphere_<P> calcBoundingSphere() const 00311 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 00312 return Geo::Point_<P>::calcBoundingSphere(points); } 00313 00321 Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const 00322 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 00323 return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); } 00324 00332 Geo::OrientedBox_<P> calcOrientedBoundingBox() const 00333 { const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy 00334 return Geo::Point_<P>::calcOrientedBoundingBox(points); } 00335 00344 static Vec3P evalPUsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w) { 00345 Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops 00346 Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops 00347 return Fbu * B * ~Fbw; // 3x35 flops 00348 } 00349 00354 static void evalP1UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w, 00355 Vec3P& Pu, Vec3P& Pw) { 00356 Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops 00357 Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops 00358 Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops 00359 Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops 00360 Pu = dFbu * B * ~Fbw; // 3x35 00361 Pw = Fbu * B * ~dFbw; // 3x35 00362 } 00363 00368 static void evalP2UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w, 00369 Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) { 00370 Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops 00371 Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops 00372 Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops 00373 Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops 00374 Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops 00375 Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops 00376 Puu = d2Fbu * B * ~Fbw; // 3x35 00377 Puw = dFbu * B * ~dFbw; // 3x35 00378 Pww = Fbu * B * ~d2Fbw; // 3x35 00379 } 00380 00385 static void evalP3UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w, 00386 Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) { 00387 Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops 00388 Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops 00389 Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops 00390 Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops 00391 Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops 00392 Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops 00393 Row<4,P> d3Fbu = CubicBezierCurve_<P>::calcD3Fb(u); // 0 00394 Row<4,P> d3Fbw = CubicBezierCurve_<P>::calcD3Fb(w); // 0 00395 Puuu = d3Fbu * B * ~Fbw; // 3x35 00396 Puuw = d2Fbu * B * ~dFbw; // 3x35 00397 Puww = dFbu * B * ~d2Fbw; // 3x35 00398 Pwww = Fbu * B * ~d3Fbw; // 3x35 00399 } 00400 00404 static CubicBezierCurve_<P> 00405 calcIsoCurveU(const Mat<4,4,Vec3P>& B, RealP u0) 00406 { const Row<4,Vec3P> Bu0 = CubicBezierCurve_<P>::calcFb(u0) * B; 00407 return CubicBezierCurve_<P>(Bu0); } 00408 00412 static CubicBezierCurve_<P> 00413 calcIsoCurveW(const Mat<4,4,Vec3P>& B, RealP w0) 00414 { const Vec<4,Vec3P> Bw0 = B * ~CubicBezierCurve_<P>::calcFb(w0); 00415 return CubicBezierCurve_<P>(Bw0); } 00416 00419 static Mat<4,4,Vec3P> calcAFromB(const Mat<4,4,Vec3P>& B) { 00420 typedef const Vec3P& Coef; 00421 Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3), 00422 b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3), 00423 b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3), 00424 b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3); 00425 // First calculate Mb*B: 00426 // a b c d 00427 // e f g h 00428 // p q r s 00429 // b11 b12 b13 b14 00430 Vec3P a= b41-b11+3*(b21-b31), b= b42-b12+3*(b22-b32), // 3x16 flops 00431 c= b43-b13+3*(b23-b33), d= b44-b14+3*(b24-b34); 00432 Vec3P e= 3*(b11+b31)-6*b21, f= 3*(b12+b32)-6*b22, // 3x16 flops 00433 g= 3*(b13+b33)-6*b23, h= 3*(b14+b34)-6*b24; 00434 Vec3P p= 3*(b21-b11), q= 3*(b22-b12), // 3x8 flops 00435 r= 3*(b23-b13), s= 3*(b24-b14); 00436 00437 // Then calculate (Mb*B)*~Mb. (3x40 more flops) 00438 return Mat<4,4,Vec3P> 00439 ( d-a+3*(b-c), 3*(a+c)-6*b, 3*(b-a), a, 00440 h-e+3*(f-g), 3*(e+g)-6*f, 3*(f-e), e, 00441 s-p+3*(q-r), 3*(p+r)-6*q, 3*(q-p), p, 00442 b14-b11+3*(b12-b13), 3*(b11+b13)-6*b12, 3*(b12-b11), b11 ); 00443 } 00444 00447 static Mat<4,4,Vec3P> calcBFromA(const Mat<4,4,Vec3P>& A) { 00448 typedef const Vec3P& Coef; 00449 Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3), 00450 a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3), 00451 a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3), 00452 a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3); 00453 // First calculate Mb^-1*A: 00454 // a03 a02 a01 a00 00455 // a b c d 00456 // e f g h 00457 // p q r s 00458 Vec3P a=a13/3+a03, b=a12/3+a02, c=a11/3+a01, d=a10/3+a00; // 3x8 flops 00459 Vec3P e=(a23+2*a13)/3+a03, f=(a22+2*a12)/3+a02, // 3x16 flops 00460 g=(a21+2*a11)/3+a01, h=(a20+2*a10)/3+a00; 00461 Vec3P p=a33+a23+a13+a03, q=a32+a22+a12+a02, // 3x12 flops 00462 r=a31+a21+a11+a01, s=a30+a20+a10+a00; 00463 00464 00465 // Then calculate (Mb^-1*A)*Mb^-T (3x36 more flops) 00466 return Mat<4,4,Vec3P> 00467 ( a00, a01/3+a00, (a02+2*a01)/3+a00, a03+a02+a01+a00, 00468 d, c/3+d, (b+2*c)/3+d, a+b+c+d, 00469 h, g/3+h, (f+2*g)/3+h, e+f+g+h, 00470 s, r/3+s, (q+2*r) /3+s, p+q+r+s ); 00471 } 00472 00475 static Mat<4,4,Vec3P> calcHFromB(const Mat<4,4,Vec3P>& B) { 00476 typedef const Vec3P& Coef; 00477 Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3), 00478 b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3), 00479 b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3), 00480 b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3); 00481 00482 // First calculate a few temps -- see class comments. (3x4 flops) 00483 Vec3P b12mb11=b12-b11, b24mb14=b24-b14, b42mb41=b42-b41, b44mb34=b44-b34; 00484 00485 // Then calculate (Mh^-1 Mb) B ~(Mh^-1 Mb). (3x24 more flops) 00486 return Mat<4,4,Vec3P> 00487 ( b11, b14, 3*b12mb11, 3*(b14-b13), 00488 b41, b44, 3*b42mb41, 3*(b44-b43), 00489 3*(b21-b11), 3*b24mb14, 9*(b22-b21-b12mb11), 9*(b24mb14+b13-b23), 00490 3*(b41-b31), 3*b44mb34, 9*(b42mb41+b31-b32), 9*(b44mb34+b33-b43) ); 00491 } 00492 00495 static Mat<4,4,Vec3P> calcBFromH(const Mat<4,4,Vec3P>& H) { 00496 typedef const Vec3P& Coef; 00497 Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3), 00498 h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3), 00499 u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3), 00500 u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3); 00501 00502 // First calculate a few temps -- see class comments. (3x8 flops) 00503 Vec3P tmp00=h00+u00/3, tmp01=h01+u01/3, 00504 tmp10=h10-u10/3, tmp11=h11-u11/3; 00505 00506 // Then calculate (Mb^-1 Mh) H ~(Mb^-1 Mh). (3x24 more flops) 00507 return Mat<4,4,Vec3P> 00508 ( h00, h00+w00/3, h01-w01/3, h01, 00509 tmp00, tmp00+w00/3+t00/9, tmp01-w01/3-t01/9, tmp01, 00510 tmp10, tmp10+w10/3-t10/9, tmp11-w11/3+t11/9, tmp11, 00511 h10, h10+w10/3, h11-w11/3, h11 ); 00512 } 00515 private: 00516 Mat<4,4,Vec3P> B; // 16 Bezier control points; see above for definition 00517 }; 00518 00519 00520 00521 } // namespace SimTK 00522 00523 #endif // SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
1.7.6.1 
