Simbody
3.5
|
00001 #ifndef SimTK_SIMMATH_GEO_BOX_H_ 00002 #define SimTK_SIMMATH_GEO_BOX_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-14 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 00030 #include "SimTKcommon.h" 00031 #include "simmath/internal/common.h" 00032 #include "simmath/internal/Geo.h" 00033 00034 #include <cassert> 00035 #include <cmath> 00036 #include <algorithm> 00037 00038 namespace SimTK { 00039 00040 //============================================================================== 00041 // GEO BOX 00042 //============================================================================== 00047 template <class P> 00048 class Geo::Box_ { 00049 typedef P RealP; 00050 typedef Vec<2,P> Vec2P; 00051 typedef Vec<3,P> Vec3P; 00052 typedef UnitVec<P,1> UnitVec3P; 00053 typedef Mat<3,3,P> Mat33P; 00054 typedef Rotation_<P> RotationP; 00055 typedef Transform_<P> TransformP; 00056 00057 public: 00059 Box_() {} 00062 Box_(const Vec3P& halfLengths) {setHalfLengths(halfLengths);} 00063 00066 Box_& setHalfLengths(const Vec3P& halfLengths) { 00067 SimTK_ERRCHK3(halfLengths >= 0, "Geo::Box_::setHalfLengths()", 00068 "Half lengths must be nonnegative; got %g,%g,%g.", 00069 (double)halfLengths[0],(double)halfLengths[1],(double)halfLengths[2]); 00070 h = halfLengths; 00071 sortEdges(); 00072 return *this; 00073 } 00074 00077 Box_& addToHalfLengths(const Vec3P& incr) { 00078 h += incr; 00079 SimTK_ERRCHK3(h >= 0, "Geo::Box_::addToHalfLengths()", 00080 "Half lengths must be nonnegative but were %g,%g,%g after change.", 00081 (double)h[0],(double)h[1],(double)h[2]); 00082 sortEdges(); 00083 return *this; 00084 } 00085 00088 const Vec3P& getHalfLengths() const {return h;} 00089 00091 RealP getOrderedHalfLength(int i) const { 00092 SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedHalfLength()"); 00093 return h[order[i]]; 00094 } 00095 00097 CoordinateAxis getOrderedAxis(int i) const { 00098 SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedAxis()"); 00099 return CoordinateAxis(order[i]); 00100 } 00101 00103 RealP findVolume() const {return 8*h[0]*h[1]*h[2];} 00106 RealP findArea() const {return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);} 00107 00111 bool containsPoint(const Vec3P& pt) const { 00112 const Vec3P absPt = pt.abs(); // reflect to first quadrant 00113 return absPt <= h; 00114 } 00115 00121 Vec3P findClosestPointOfSolidBox(const Vec3P& pt, bool& ptWasInside) const { 00122 Vec3P c(pt); 00123 ptWasInside = true; // tentatively 00124 for (int i=0; i<3; ++i) { 00125 if (c[i] < -h[i]) {c[i]=-h[i]; ptWasInside=false;} 00126 else if (c[i] > h[i]) {c[i]= h[i]; ptWasInside=false;} 00127 } 00128 return c; 00129 } 00130 00135 Vec3P findClosestPointOnSurface(const Vec3P& pt, bool& ptWasInside) const { 00136 Vec3P c = findClosestPointOfSolidBox(pt, ptWasInside); 00137 00138 if (ptWasInside) { // every |c[i]| <= h[i] 00139 RealP dToSide = h[0]-std::abs(c[0]); // distance to closest x-face 00140 int which=0; RealP minDist=dToSide; 00141 dToSide = h[1]-std::abs(c[1]); 00142 if (dToSide < minDist) {which=1; minDist=dToSide;} 00143 dToSide = h[2]-std::abs(c[2]); 00144 if (dToSide < minDist) {which=2; minDist=dToSide;} 00145 // Now project the point to the nearest side. 00146 c[which] = c[which] < 0 ? -h[which] : h[which]; 00147 } 00148 return c; 00149 } 00150 00155 RealP findDistanceSqrToPoint(const Vec3P& pt) const { 00156 const Vec3P absPt = pt.abs(); // reflect to first quadrant 00157 RealP d2 = 0; 00158 if (absPt[0] > h[0]) d2 += square(absPt[0]-h[0]); 00159 if (absPt[1] > h[1]) d2 += square(absPt[1]-h[1]); 00160 if (absPt[2] > h[2]) d2 += square(absPt[2]-h[2]); 00161 return d2; 00162 } 00163 00164 00171 Vec3P findSupportPoint(const Vec3& d) const { 00172 // Basically transferring the sign from d to h, but with 0 treated as 1. 00173 return Vec3P(d[0]<0 ? -h[0]:h[0], d[1]<0 ? -h[1]:h[1], d[2]<0 ? -h[2]:h[2]); 00174 } 00175 00180 RealP findDistanceSqrToSphere(const Geo::Sphere_<P>& sphere) const { 00181 const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant 00182 const Vec3P grow = h + sphere.getRadius(); // 3 flops 00183 RealP d2 = 0; 00184 if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]); 00185 if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]); 00186 if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]); 00187 return d2; 00188 } 00189 00194 RealP findDistanceSqrToAlignedBox(const Geo::AlignedBox_<P>& aab) const { 00195 const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant 00196 const Vec3P grow = h + aab.getHalfLengths(); 00197 RealP d2 = 0; 00198 if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]); 00199 if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]); 00200 if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]); 00201 return d2; 00202 } 00203 00208 bool intersectsSphere(const Geo::Sphere_<P>& sphere) const { 00209 const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant 00210 const RealP r = sphere.getRadius(); 00211 if (absCtr[0] > h[0]+r) return false; 00212 if (absCtr[1] > h[1]+r) return false; 00213 if (absCtr[2] > h[2]+r) return false; 00214 return true; 00215 } 00216 00221 bool intersectsAlignedBox(const Geo::AlignedBox_<P>& aab) const { 00222 const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant 00223 const Vec3P& aabh = aab.getHalfLengths(); 00224 if (absCtr[0] > h[0]+aabh[0]) return false; 00225 if (absCtr[1] > h[1]+aabh[1]) return false; 00226 if (absCtr[2] > h[2]+aabh[2]) return false; 00227 return true; 00228 } 00229 00238 SimTK_SIMMATH_EXPORT bool 00239 intersectsOrientedBox(const Geo::OrientedBox_<P>& ob) const; 00240 00248 SimTK_SIMMATH_EXPORT bool 00249 mayIntersectOrientedBox(const Geo::OrientedBox_<P>& ob) const; 00250 00251 00255 static int getNumVertices() {return 8;} 00256 static int getNumEdges() {return 12;} 00257 static int getNumFaces() {return 6;} 00258 00268 Vec3P getVertexPos(int vx) const { 00269 SimTK_INDEXCHECK(vx,8,"Geo::Box::getVertexPos()"); 00270 return Vec3P(vx&0x4 ? h[0]:-h[0], vx&0x2 ? h[1]:-h[1], vx&0x1 ? h[2]:-h[2]); 00271 } 00272 00275 int findSupportVertex(const Vec3P& d) const { 00276 int vx = 0; 00277 if (d[0] >= 0) vx += 0x4; // see table in getVertexPos(). 00278 if (d[1] >= 0) vx += 0x2; 00279 if (d[2] >= 0) vx += 0x1; 00280 return vx; 00281 } 00282 00285 UnitVec3P getVertexNormal(int vx) const { 00286 SimTK_INDEXCHECK(vx,8,"Geo::Box::getVertexNormal()"); 00287 const RealP c = 1/(RealP)SimTK_SQRT3, nc = -c; 00288 return UnitVec3P(Vec3P(vx&0x4 ? c:nc, vx&0x2 ? c:nc, vx&0x1 ? c:nc),true); 00289 } 00290 00292 Vec3P getEdgeCenter(int ex) const { 00293 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeCenter()"); 00294 int faces[2], which[2]; 00295 getEdgeFaces(ex, faces, which); 00296 Vec3P c(0); 00297 for (int i=0; i<2; ++i) { 00298 const CoordinateDirection fd = getFaceCoordinateDirection(faces[i]); 00299 c[fd.getAxis()] = fd.getDirection() * h[fd.getAxis()]; 00300 } 00301 return c; 00302 } 00303 00306 UnitVec3P getEdgeNormal(int ex) const { 00307 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeNormal()"); 00308 const RealP oosqrt2 = (RealP)SimTK_OOSQRT2; 00309 int faces[2], which[2]; 00310 getEdgeFaces(ex, faces, which); 00311 Vec3P n(0); 00312 for (int i=0; i<2; ++i) { 00313 const CoordinateDirection fd = getFaceCoordinateDirection(faces[i]); 00314 n[fd.getAxis()] = fd.getDirection() * oosqrt2; 00315 } 00316 return UnitVec3P(n, true); 00317 } 00318 00319 00320 00323 CoordinateDirection getEdgeCoordinateDirection(int ex) const { 00324 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeDirection()"); 00325 static const int axis[12] = {2,1,2,1,0,2,0,0,1,1,2,0}; 00326 return CoordinateDirection(CoordinateAxis(axis[ex]), 1); // all in + dir 00327 } 00328 00329 00335 UnitVec3P getEdgeDirection(int ex) const { 00336 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeUnitVec()"); 00337 return UnitVec3P(getEdgeCoordinateDirection(ex)); 00338 } 00339 00341 CoordinateDirection getFaceCoordinateDirection(int fx) const { 00342 SimTK_INDEXCHECK(fx,6,"Geo::Box::getFaceDirection()"); 00343 static const int axis[6] = {0, 1, 2, 0, 1, 2}; 00344 static const int dir[6] = {-1,-1,-1, 1, 1, 1}; 00345 return CoordinateDirection(CoordinateAxis(axis[fx]), dir[fx]); 00346 } 00347 00351 Vec3P getFaceCenter(int fx) const { 00352 SimTK_INDEXCHECK(fx,6,"Geo::Box::getFaceCenter()"); 00353 const CoordinateDirection fd = getFaceCoordinateDirection(fx); 00354 Vec3P c(0); 00355 c[fd.getAxis()] = fd.getDirection() * h[fd.getAxis()]; 00356 return c; 00357 } 00358 00361 UnitVec3P getFaceNormal(int fx) const { 00362 SimTK_INDEXCHECK(fx,6,"Geo::Box::getFaceNormal()"); 00363 return UnitVec3P(getFaceCoordinateDirection(fx)); 00364 } 00365 00367 void getFaceVertices(int fx, int v[4]) const { 00368 SimTK_INDEXCHECK(fx,6,"Geo::Box::getFaceVertices()"); 00369 static const int verts[6][4] = {{0,1,3,2},{0,4,5,1},{0,2,6,4}, 00370 {7,5,4,6},{7,6,2,3},{7,3,1,5}}; 00371 for (int i=0; i<4; ++i) v[i] = verts[fx][i]; 00372 } 00373 00376 void getVertexFaces(int vx, int f[3], int w[3]) const { 00377 SimTK_INDEXCHECK(vx,8,"Geo::Box::getVertexFaces()"); 00378 static const int faces[8][3] = {{0,1,2},{0,1,5},{0,2,4},{0,4,5}, 00379 {1,2,3},{1,3,5},{2,3,4},{3,4,5}}; 00380 static const int which[8][3] = {{0,0,0},{1,3,2},{3,1,2},{2,3,1}, 00381 {1,3,2},{2,1,3},{2,3,1},{0,0,0}}; 00382 for (int i=0; i<3; ++i) {f[i]=faces[vx][i]; w[i]=which[vx][i];} 00383 } 00384 00386 void getEdgeVertices(int ex, int v[2]) const { 00387 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeVertices()"); 00388 static const int verts[12][2] = {{0,1},{1,3},{2,3},{0,2}, // 0-3 00389 {0,4},{4,5},{1,5},{2,6}, // 4-7 00390 {4,6},{5,7},{6,7},{3,7}}; // 8-11 00391 for (int i=0; i<2; ++i) v[i] = verts[ex][i]; 00392 } 00395 void getVertexEdges(int vx, int e[3], int w[3]) const { 00396 SimTK_INDEXCHECK(vx,8,"Geo::Box::getVertexEdges()"); 00397 static const int edges[8][3] = {{0,3,4},{0,1,6},{2,3,7},{1,2,11}, 00398 {4,5,8},{5,6,9},{7,8,10},{9,10,11}}; 00399 static const int which[8][3] = {{0,0,0},{1,0,0},{0,1,0},{1,1,0}, 00400 {1,0,0},{1,1,0},{1,1,0},{1,1,1}}; 00401 for (int i=0; i<3; ++i) {e[i]=edges[vx][i]; w[i]=which[vx][i];} 00402 } 00403 00404 00407 void getFaceEdges(int fx, int e[4]) const { 00408 SimTK_INDEXCHECK(fx,6,"Geo::Box::getFaceEdges()"); 00409 static const int edges[6][4] = {{0,1,2, 3},{ 4,5,6, 0},{ 3,7,8,4}, 00410 {9,5,8,10},{10,7,2,11},{11,1,6,9}}; 00411 for (int i=0; i<4; ++i) e[i] = edges[fx][i]; 00412 } 00415 void getEdgeFaces(int ex, int f[2], int w[2]) const { 00416 SimTK_INDEXCHECK(ex,12,"Geo::Box::getEdgeFaces()"); 00417 static const int faces[12][2] = {{0,1},{0,5},{0,4},{0,2}, // 0-3 00418 {1,2},{1,3},{1,5},{2,4}, // 4-7 00419 {2,3},{3,5},{3,4},{4,5}}; // 8-11 00420 static const int which[12][2] = {{0,3},{1,1},{2,2},{3,0}, // 0-3 00421 {0,3},{1,1},{2,2},{1,1}, // 4-7 00422 {2,2},{0,3},{3,0},{3,0}}; // 8-11 00423 for (int i=0; i<2; ++i) {f[i] = faces[ex][i]; w[i] = which[ex][i];} 00424 } 00425 00426 00427 00428 private: 00429 // Call this whenever an edge length changes. Each axis will appear once. 00430 void sortEdges() { 00431 CoordinateAxis shortest = XAxis, longest = ZAxis; 00432 if (h[YAxis] < h[shortest]) shortest=YAxis; 00433 if (h[ZAxis] < h[shortest]) shortest=ZAxis; 00434 if (h[XAxis] > h[longest]) longest=XAxis; 00435 if (h[YAxis] > h[longest]) longest=YAxis; 00436 order[0] = shortest; order[2] = longest; 00437 order[1] = shortest.getThirdAxis(longest); // not shortest or longest 00438 } 00439 00440 int intersectsOrientedBoxHelper(const OrientedBox_<P>& O, 00441 Mat33P& absR_BO, 00442 Vec3P& absP_BO) const; 00443 00444 Vec3P h; // half-dimensions of the box 00445 unsigned char order[3]; // 0,1,2 reordered short to long 00446 }; 00447 00448 00449 00450 //============================================================================== 00451 // GEO ALIGNED BOX 00452 //============================================================================== 00456 template <class P> 00457 class SimTK_SIMMATH_EXPORT Geo::AlignedBox_ { 00458 typedef P RealP; 00459 typedef Vec<3,RealP> Vec3P; 00460 00461 public: 00464 AlignedBox_() {} 00467 AlignedBox_(const Vec3P& center, const Geo::Box_<P>& box) 00468 : center(center), box(box) {} 00471 AlignedBox_(const Vec3P& center, const Vec3P& halfLengths) 00472 : center(center), box(halfLengths) {} 00473 00475 AlignedBox_& setCenter(const Vec3P& center) 00476 { this->center=center; return *this; } 00477 00479 AlignedBox_& setHalfLengths(const Vec3P& halfLengths) 00480 { box.setHalfLengths(halfLengths); return *this; } 00481 00483 const Vec3P& getCenter() const {return center;} 00485 Vec3P& updCenter() {return center;} 00488 const Vec3P& getHalfLengths() const {return box.getHalfLengths();} 00489 // no updHalfLengths() 00490 const Box_<P>& getBox() const {return box;} 00491 Box_<P>& updBox() {return box;} 00492 00496 bool containsPoint(const Vec3P& pt_F) const 00497 { return box.containsPoint(pt_F - center); } // shift to box frame B 00498 00506 AlignedBox_& stretchBoundary() { 00507 const RealP tol = Geo::getDefaultTol<P>(); 00508 const RealP maxdim = max(getCenter().abs()); 00509 const RealP maxrad = max(getHalfLengths()); 00510 const RealP scale = std::max(maxdim, maxrad); 00511 const RealP incr = std::max(scale*Geo::getEps<P>(), tol); 00512 box.addToHalfLengths(Vec3P(incr)); 00513 return *this; 00514 } 00515 00516 private: 00517 Vec3P center; 00518 Geo::Box_<P> box; 00519 }; 00520 00521 00522 //============================================================================== 00523 // GEO ORIENTED BOX 00524 //============================================================================== 00527 template <class P> 00528 class SimTK_SIMMATH_EXPORT Geo::OrientedBox_ { 00529 typedef P RealP; 00530 typedef Vec<3,P> Vec3P; 00531 typedef Rotation_<P> RotationP; 00532 typedef Transform_<P> TransformP; 00533 00534 public: 00537 OrientedBox_() {} 00541 OrientedBox_(const TransformP& X_FB, const Geo::Box_<P>& box) 00542 : X_FB(X_FB), box(box) {} 00545 OrientedBox_(const TransformP& X_FB, const Vec3P& halfLengths) 00546 : X_FB(X_FB), box(halfLengths) {} 00547 00548 00550 OrientedBox_& setTransform(const TransformP& newX_FB) 00551 { X_FB=newX_FB; return *this; } 00552 00554 OrientedBox_& setHalfLengths(const Vec3P& halfLengths) 00555 { box.setHalfLengths(halfLengths); return *this; } 00556 00557 const Vec3P& getCenter() const {return X_FB.p();} 00558 Vec3P& updCenter() {return X_FB.updP();} 00559 const RotationP& getOrientation() const {return X_FB.R();} 00560 RotationP& updOrientation() {return X_FB.updR();} 00561 const TransformP& getTransform() const {return X_FB;} 00562 TransformP& updTransform() {return X_FB;} 00563 const Vec3P& getHalfLengths() const {return box.getHalfLengths();} 00564 // no updHalfLengths() 00565 const Box_<P>& getBox() const {return box;} 00566 Box_<P>& updBox() {return box;} 00567 00568 00572 bool containsPoint(const Vec3P& pt_F) const 00573 { return box.containsPoint(~X_FB*pt_F); } // shift to box frame B 00574 00582 OrientedBox_& stretchBoundary() { 00583 const RealP tol = Geo::getDefaultTol<P>(); 00584 const RealP maxdim = max(getCenter().abs()); 00585 const RealP maxrad = max(getHalfLengths()); 00586 const RealP scale = std::max(maxdim, maxrad); 00587 const RealP incr = std::max(scale*Geo::getEps<P>(), tol); 00588 box.addToHalfLengths(Vec3P(incr)); 00589 return *this; 00590 } 00591 00592 private: 00593 TransformP X_FB; 00594 Geo::Box_<P> box; 00595 }; 00596 00597 00598 } // namespace SimTK 00599 00600 #endif // SimTK_SIMMATH_GEO_BOX_H_