Simbody  3.5
Geo_Box.h
Go to the documentation of this file.
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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines