1 #ifndef SimTK_SIMMATH_GEO_BOX_H_ 2 #define SimTK_SIMMATH_GEO_BOX_H_ 50 typedef Vec<2,P> Vec2P;
51 typedef Vec<3,P> Vec3P;
52 typedef UnitVec<P,1> UnitVec3P;
53 typedef Mat<3,3,P> Mat33P;
54 typedef Rotation_<P> RotationP;
55 typedef Transform_<P> TransformP;
67 SimTK_ERRCHK3(halfLengths >= 0,
"Geo::Box_::setHalfLengths()",
68 "Half lengths must be nonnegative; got %g,%g,%g.",
69 (
double)halfLengths[0],(
double)halfLengths[1],(
double)halfLengths[2]);
80 "Half lengths must be nonnegative but were %g,%g,%g after change.",
81 (
double)h[0],(
double)h[1],(
double)h[2]);
106 RealP
findArea()
const {
return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);}
112 const Vec3P absPt = pt.
abs();
124 for (
int i=0; i<3; ++i) {
125 if (c[i] < -h[i]) {c[i]=-h[i]; ptWasInside=
false;}
126 else if (c[i] > h[i]) {c[i]= h[i]; ptWasInside=
false;}
139 RealP dToSide = h[0]-
std::abs(c[0]);
140 int which=0; RealP minDist=dToSide;
142 if (dToSide < minDist) {which=1; minDist=dToSide;}
144 if (dToSide < minDist) {which=2; minDist=dToSide;}
146 c[which] = c[which] < 0 ? -h[which] : h[which];
156 const Vec3P absPt = pt.
abs();
158 if (absPt[0] > h[0]) d2 +=
square(absPt[0]-h[0]);
159 if (absPt[1] > h[1]) d2 +=
square(absPt[1]-h[1]);
160 if (absPt[2] > h[2]) d2 +=
square(absPt[2]-h[2]);
173 return Vec3P(d[0]<0 ? -h[0]:h[0], d[1]<0 ? -h[1]:h[1], d[2]<0 ? -h[2]:h[2]);
182 const Vec3P grow = h + sphere.
getRadius();
184 if (absCtr[0] > grow[0]) d2 +=
square(absCtr[0]-grow[0]);
185 if (absCtr[1] > grow[1]) d2 +=
square(absCtr[1]-grow[1]);
186 if (absCtr[2] > grow[2]) d2 +=
square(absCtr[2]-grow[2]);
198 if (absCtr[0] > grow[0]) d2 +=
square(absCtr[0]-grow[0]);
199 if (absCtr[1] > grow[1]) d2 +=
square(absCtr[1]-grow[1]);
200 if (absCtr[2] > grow[2]) d2 +=
square(absCtr[2]-grow[2]);
211 if (absCtr[0] > h[0]+r)
return false;
212 if (absCtr[1] > h[1]+r)
return false;
213 if (absCtr[2] > h[2]+r)
return false;
224 if (absCtr[0] > h[0]+aabh[0])
return false;
225 if (absCtr[1] > h[1]+aabh[1])
return false;
226 if (absCtr[2] > h[2]+aabh[2])
return false;
270 return Vec3P(vx&0x4 ? h[0]:-h[0], vx&0x2 ? h[1]:-h[1], vx&0x1 ? h[2]:-h[2]);
277 if (d[0] >= 0) vx += 0x4;
278 if (d[1] >= 0) vx += 0x2;
279 if (d[2] >= 0) vx += 0x1;
288 return UnitVec3P(Vec3P(vx&0x4 ? c:nc, vx&0x2 ? c:nc, vx&0x1 ? c:nc),
true);
294 int faces[2], which[2];
297 for (
int i=0; i<2; ++i) {
309 int faces[2], which[2];
312 for (
int i=0; i<2; ++i) {
316 return UnitVec3P(n,
true);
325 static const int axis[12] = {2,1,2,1,0,2,0,0,1,1,2,0};
343 static const int axis[6] = {0, 1, 2, 0, 1, 2};
344 static const int dir[6] = {-1,-1,-1, 1, 1, 1};
369 static const int verts[6][4] = {{0,1,3,2},{0,4,5,1},{0,2,6,4},
370 {7,5,4,6},{7,6,2,3},{7,3,1,5}};
371 for (
int i=0; i<4; ++i) v[i] = verts[fx][i];
378 static const int faces[8][3] = {{0,1,2},{0,1,5},{0,2,4},{0,4,5},
379 {1,2,3},{1,3,5},{2,3,4},{3,4,5}};
380 static const int which[8][3] = {{0,0,0},{1,3,2},{3,1,2},{2,3,1},
381 {1,3,2},{2,1,3},{2,3,1},{0,0,0}};
382 for (
int i=0; i<3; ++i) {f[i]=faces[vx][i]; w[i]=which[vx][i];}
388 static const int verts[12][2] = {{0,1},{1,3},{2,3},{0,2},
389 {0,4},{4,5},{1,5},{2,6},
390 {4,6},{5,7},{6,7},{3,7}};
391 for (
int i=0; i<2; ++i) v[i] = verts[ex][i];
397 static const int edges[8][3] = {{0,3,4},{0,1,6},{2,3,7},{1,2,11},
398 {4,5,8},{5,6,9},{7,8,10},{9,10,11}};
399 static const int which[8][3] = {{0,0,0},{1,0,0},{0,1,0},{1,1,0},
400 {1,0,0},{1,1,0},{1,1,0},{1,1,1}};
401 for (
int i=0; i<3; ++i) {e[i]=edges[vx][i]; w[i]=which[vx][i];}
409 static const int edges[6][4] = {{0,1,2, 3},{ 4,5,6, 0},{ 3,7,8,4},
410 {9,5,8,10},{10,7,2,11},{11,1,6,9}};
411 for (
int i=0; i<4; ++i) e[i] = edges[fx][i];
417 static const int faces[12][2] = {{0,1},{0,5},{0,4},{0,2},
418 {1,2},{1,3},{1,5},{2,4},
419 {2,3},{3,5},{3,4},{4,5}};
420 static const int which[12][2] = {{0,3},{1,1},{2,2},{3,0},
421 {0,3},{1,1},{2,2},{1,1},
422 {2,2},{0,3},{3,0},{3,0}};
423 for (
int i=0; i<2; ++i) {f[i] = faces[ex][i]; w[i] = which[ex][i];}
436 order[0] = shortest; order[2] = longest;
437 order[1] = shortest.getThirdAxis(longest);
442 Vec3P& absP_BO)
const;
445 unsigned char order[3];
468 : center(center), box(box) {}
472 : center(center), box(halfLengths) {}
476 { this->center=center;
return *
this; }
497 {
return box.containsPoint(pt_F - center); }
507 const RealP tol = Geo::getDefaultTol<P>();
508 const RealP maxdim =
max(getCenter().
abs());
510 const RealP scale =
std::max(maxdim, maxrad);
511 const RealP incr =
std::max(scale*Geo::getEps<P>(), tol);
512 box.addToHalfLengths(Vec3P(incr));
542 : X_FB(X_FB), box(box) {}
546 : X_FB(X_FB), box(halfLengths) {}
551 { X_FB=newX_FB;
return *
this; }
573 {
return box.containsPoint(~X_FB*pt_F); }
583 const RealP tol = Geo::getDefaultTol<P>();
584 const RealP maxdim =
max(getCenter().
abs());
586 const RealP scale =
std::max(maxdim, maxrad);
587 const RealP incr =
std::max(scale*Geo::getEps<P>(), tol);
588 box.addToHalfLengths(Vec3P(incr));
600 #endif // SimTK_SIMMATH_GEO_BOX_H_ AlignedBox_(const Vec3P ¢er, const Geo::Box_< P > &box)
Construct an AlignedBox with the given box shape with the center located as given.
Definition: Geo_Box.h:467
Box_< P > & updBox()
Definition: Geo_Box.h:566
const RotationP & getOrientation() const
Definition: Geo_Box.h:559
AlignedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:479
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:345
bool mayIntersectOrientedBox(const Geo::OrientedBox_< P > &ob) const
Given an oriented box whose pose is measured and expressed in the frame of this box, return true if the two boxes may be intersecting.
bool intersectsSphere(const Geo::Sphere_< P > &sphere) const
Given a sphere with center measured and expressed in the box frame, return true if the box and sphere...
Definition: Geo_Box.h:208
RealP findDistanceSqrToSphere(const Geo::Sphere_< P > &sphere) const
Return the square of the distance from this box to a given sphere whose center location is measured f...
Definition: Geo_Box.h:180
const Box_< P > & getBox() const
Definition: Geo_Box.h:565
void getVertexFaces(int vx, int f[3], int w[3]) const
Each vertex has three incident faces.
Definition: Geo_Box.h:376
TransformP & updTransform()
Definition: Geo_Box.h:562
Box_ & setHalfLengths(const Vec3P &halfLengths)
Change the half-dimensions of this box.
Definition: Geo_Box.h:66
int findSupportVertex(const Vec3P &d) const
Find the vertex (0-7) that is furthest in the direction d, which is given in the box frame...
Definition: Geo_Box.h:275
const Box_< P > & getBox() const
Definition: Geo_Box.h:490
UnitVec3P getFaceNormal(int fx) const
Return the outward normal for the given face as a unit vector in the box frame.
Definition: Geo_Box.h:361
static int getNumEdges()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:256
RealP findArea() const
Calculate the surface area of this box, ignoring degeneracy (meaning that all pairs of sides are coun...
Definition: Geo_Box.h:106
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
RealP findVolume() const
Calculate the volume of this box.
Definition: Geo_Box.h:103
A CoordinateDirection is a CoordinateAxis plus a direction indicating the positive or negative direct...
Definition: CoordinateAxis.h:244
CoordinateDirection getFaceCoordinateDirection(int fx) const
Return the outward normal for the given face as a CoordinateDirection.
Definition: Geo_Box.h:341
bool containsPoint(const Vec3P &pt) const
Given a point measured and expressed in the box frame, determine whether it is inside the box (we cou...
Definition: Geo_Box.h:111
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
RealP getOrderedHalfLength(int i) const
Get lengths in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:91
RealP findDistanceSqrToAlignedBox(const Geo::AlignedBox_< P > &aab) const
Return the square of the distance from this box to an axis-aligned box whose center location is measu...
Definition: Geo_Box.h:194
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin...
Definition: Geo.h:61
AlignedBox_()
Construct an uninitialized AlignedBox object; the dimensions and location will be garbage...
Definition: Geo_Box.h:464
bool containsPoint(const Vec3P &pt_F) const
Given a point measured and expressed in the base frame F, determine whether it is strictly contained ...
Definition: Geo_Box.h:496
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
AlignedBox_ & stretchBoundary()
Stretch this box in place by a small amount to ensure that there will be no roundoff problems if this...
Definition: Geo_Box.h:506
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
OrientedBox_ & setTransform(const TransformP &newX_FB)
Change the pose of this box.
Definition: Geo_Box.h:550
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo.h:63
A 3d box aligned with an unspecified frame F and centered at a given point measured from that frame's...
Definition: Geo.h:62
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
void getVertexEdges(int vx, int e[3], int w[3]) const
Each vertex has three incident edges.
Definition: Geo_Box.h:395
unsigned char square(unsigned char u)
Definition: Scalar.h:351
Vec3P & updCenter()
Definition: Geo_Box.h:558
Vec3P findSupportPoint(const Vec3 &d) const
Find a supporting point on the surface of the box in the given direction, which must be expressed in ...
Definition: Geo_Box.h:171
CoordinateAxis getOrderedAxis(int i) const
Get axes in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:97
static int getNumFaces()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:257
Vec3P findClosestPointOfSolidBox(const Vec3P &pt, bool &ptWasInside) const
Given a point location in the box frame, return the closest point of the solid box, and a flag saying whether the given point was inside the box, using the same definition of "inside" as the containsPoint() method.
Definition: Geo_Box.h:121
bool intersectsOrientedBox(const Geo::OrientedBox_< P > &ob) const
Given an oriented box whose pose is measured and expressed in the frame of this box, return true if the two boxes intersect.
RealP getRadius() const
Get the sphere's radius.
Definition: Geo_Sphere.h:111
CoordinateAxis getAxis() const
This is the coordinate axis XAxis, YAxis, or ZAxis contained in this CoordinateDirection. Use getDirection() to determine whether this is the positive or negative direction.
Definition: CoordinateAxis.h:274
CoordinateDirection getEdgeCoordinateDirection(int ex) const
Return the direction of an edge, going from its first vertex to its second vertex, as a CoordinateDirection.
Definition: Geo_Box.h:323
static int getNumVertices()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:255
const Vec3P & getCenter() const
Definition: Geo_Box.h:557
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
AlignedBox_ & setCenter(const Vec3P ¢er)
Change the center location of this box.
Definition: Geo_Box.h:475
Box_(const Vec3P &halfLengths)
Construct a Box with the given nonnegative half-dimensions.
Definition: Geo_Box.h:62
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
Defines geometric primitive shapes and algorthms.
const Vec3P & getCenter() const
Get the location of the sphere's center point.
Definition: Geo_Sphere.h:107
#define SimTK_OOSQRT2
One over the square root of 2; also half the square root of 2 since 1/sqrt(2) == 2^(-1/2) == sqrt(2)/...
Definition: Constants.h:184
bool intersectsAlignedBox(const Geo::AlignedBox_< P > &aab) const
Given an aligned box with center measured and expressed in the from of this box, return true if the t...
Definition: Geo_Box.h:221
void getEdgeVertices(int ex, int v[2]) const
An edge connects two vertices.
Definition: Geo_Box.h:386
Vec3P & updCenter()
Return a writable reference to the center location of this box.
Definition: Geo_Box.h:485
Box_ & addToHalfLengths(const Vec3P &incr)
Change the half-dimensions of this box by adding the given vector.
Definition: Geo_Box.h:77
OrientedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:554
Vec3P findClosestPointOnSurface(const Vec3P &pt, bool &ptWasInside) const
Given a point location in the box frame, return the closest point on the box surface, and a flag saying whether the given point was inside the box, using the same definition of "inside" as the containsPoint() method.
Definition: Geo_Box.h:135
const TransformP & getTransform() const
Definition: Geo_Box.h:561
Box_< P > & updBox()
Definition: Geo_Box.h:491
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
void getFaceVertices(int fx, int v[4]) const
A face has four vertices ordered counterclockwise about the face normal.
Definition: Geo_Box.h:367
UnitVec3P getEdgeNormal(int ex) const
Edge normals point diagonally outwards from the edge.
Definition: Geo_Box.h:306
This is the header file that every Simmath compilation unit should include first. ...
const Vec3P & getHalfLengths() const
Return the half-lengths of this box as a Vec3 from the center to the first quadrant vertex...
Definition: Geo_Box.h:88
OrientedBox_ & stretchBoundary()
Stretch this box in place by a small amount to ensure that there will be no roundoff problems if this...
Definition: Geo_Box.h:582
const Vec3P & getHalfLengths() const
Return the half-lengths of this box as a Vec3 from the center to the first quadrant vertex...
Definition: Geo_Box.h:488
RotationP & updOrientation()
Definition: Geo_Box.h:560
int getDirection() const
Returns 1 or -1 to indicate the direction along the coordinate axis returned by getAxis().
Definition: CoordinateAxis.h:277
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
Vec3P getVertexPos(int vx) const
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:268
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo.h:56
AlignedBox_(const Vec3P ¢er, const Vec3P &halfLengths)
Construct an AlignedBox with the given center location and half-dimensions.
Definition: Geo_Box.h:471
OrientedBox_()
Construct an uninitialized OrientedBox object; the dimensions and pose will be garbage.
Definition: Geo_Box.h:537
OrientedBox_(const TransformP &X_FB, const Geo::Box_< P > &box)
Construct an OrientedBox with the given box shape with positioned and oriented according to the given...
Definition: Geo_Box.h:541
UnitVec3P getVertexNormal(int vx) const
Vertex normals point diagonally outwards from the box corners.
Definition: Geo_Box.h:285
void getFaceEdges(int fx, int e[4]) const
A face has four edges, ordered by the vertex ordering: v0-v1, v1-v2, v2-v3, v3-v1.
Definition: Geo_Box.h:407
UnitVec3P getEdgeDirection(int ex) const
Return a unit vector aligned with the selected edge, pointing in the direction from the first vertex ...
Definition: Geo_Box.h:335
const Vec3P & getCenter() const
Return the location of the center of this box (box frame origin).
Definition: Geo_Box.h:483
Vec3P getFaceCenter(int fx) const
Return the center point position for the given face.
Definition: Geo_Box.h:351
OrientedBox_(const TransformP &X_FB, const Vec3P &halfLengths)
Construct an OrientedBox with the given location and half-dimensions.
Definition: Geo_Box.h:545
Box_()
Construct an uninitialized Box object; the dimensions will be garbage.
Definition: Geo_Box.h:59
Vec3P getEdgeCenter(int ex) const
Return the center point of the specified edge, in the box frame.
Definition: Geo_Box.h:292
RealP findDistanceSqrToPoint(const Vec3P &pt) const
Return the square of the distance from this box to a given point whose location is measured from and ...
Definition: Geo_Box.h:155
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
#define SimTK_SQRT3
The square root of 3.
Definition: Constants.h:214
const Vec3P & getHalfLengths() const
Definition: Geo_Box.h:563
bool containsPoint(const Vec3P &pt_F) const
Given a point measured and expressed in the base frame F, determine whether it is strictly contained ...
Definition: Geo_Box.h:572
void getEdgeFaces(int ex, int f[2], int w[2]) const
An edge is between two faces.
Definition: Geo_Box.h:415
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...