1 #ifndef SimTK_SIMMATH_GEO_BOX_H_
2 #define SimTK_SIMMATH_GEO_BOX_H_
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]);}
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];
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]);
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) {
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);
440 int intersectsOrientedBoxHelper(
const OrientedBox_<P>& O,
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());
509 const RealP maxrad =
max(getHalfLengths());
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());
585 const RealP maxrad =
max(getHalfLengths());
586 const RealP scale =
std::max(maxdim, maxrad);
587 const RealP incr =
std::max(scale*Geo::getEps<P>(), tol);
588 box.addToHalfLengths(
Vec3P(incr));
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
Defines geometric primitive shapes and algorthms.
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
A CoordinateDirection is a CoordinateAxis plus a direction indicating the positive or negative direct...
Definition: CoordinateAxis.h:244
int getDirection() const
Returns 1 or -1 to indicate the direction along the coordinate axis returned by getAxis().
Definition: CoordinateAxis.h:277
CoordinateAxis getAxis() const
This is the coordinate axis XAxis, YAxis, or ZAxis contained in this CoordinateDirection....
Definition: CoordinateAxis.h:274
A 3d box aligned with an unspecified frame F and centered at a given point measured from that frame's...
Definition: Geo_Box.h:457
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
AlignedBox_(const Vec3P ¢er, const Vec3P &halfLengths)
Construct an AlignedBox with the given center location and half-dimensions.
Definition: Geo_Box.h:471
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 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
Vec3P & updCenter()
Return a writable reference to the center location of this box.
Definition: Geo_Box.h:485
AlignedBox_()
Construct an uninitialized AlignedBox object; the dimensions and location will be garbage.
Definition: Geo_Box.h:464
const Vec3P & getCenter() const
Return the location of the center of this box (box frame origin).
Definition: Geo_Box.h:483
AlignedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:479
const Box_< P > & getBox() const
Definition: Geo_Box.h:490
AlignedBox_ & setCenter(const Vec3P ¢er)
Change the center location of this box.
Definition: Geo_Box.h:475
Box_< P > & updBox()
Definition: Geo_Box.h:491
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
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin.
Definition: Geo_Box.h:48
void getEdgeVertices(int ex, int v[2]) const
An edge connects two vertices.
Definition: Geo_Box.h:386
RealP findVolume() const
Calculate the volume of this box.
Definition: Geo_Box.h:103
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
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
void getVertexFaces(int vx, int f[3], int w[3]) const
Each vertex has three incident faces.
Definition: Geo_Box.h:376
UnitVec3P getEdgeNormal(int ex) const
Edge normals point diagonally outwards from the edge.
Definition: Geo_Box.h:306
static int getNumEdges()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:256
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
Box_()
Construct an uninitialized Box object; the dimensions will be garbage.
Definition: Geo_Box.h:59
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
static int getNumVertices()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:255
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
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
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
CoordinateDirection getFaceCoordinateDirection(int fx) const
Return the outward normal for the given face as a CoordinateDirection.
Definition: Geo_Box.h:341
RealP getOrderedHalfLength(int i) const
Get lengths in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:91
bool intersectsOrientedBox(const Geo::OrientedBox_< P > &ob) const
Given an oriented box whose pose is measured and expressed in the frame of this box,...
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
void getFaceVertices(int fx, int v[4]) const
A face has four vertices ordered counterclockwise about the face normal.
Definition: Geo_Box.h:367
Box_ & setHalfLengths(const Vec3P &halfLengths)
Change the half-dimensions of this box.
Definition: Geo_Box.h:66
Box_(const Vec3P &halfLengths)
Construct a Box with the given nonnegative half-dimensions.
Definition: Geo_Box.h:62
Vec3P getFaceCenter(int fx) const
Return the center point position for the given face.
Definition: Geo_Box.h:351
Vec3P findClosestPointOnSurface(const Vec3P &pt, bool &ptWasInside) const
Given a point location in the box frame, return the closest point on the box surface,...
Definition: Geo_Box.h:135
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
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
CoordinateDirection getEdgeCoordinateDirection(int ex) const
Return the direction of an edge, going from its first vertex to its second vertex,...
Definition: Geo_Box.h:323
Vec3P getEdgeCenter(int ex) const
Return the center point of the specified edge, in the box frame.
Definition: Geo_Box.h:292
static int getNumFaces()
Use bits in the vertex number to pick the signs, with 0=negative, 1=positive:
Definition: Geo_Box.h:257
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
Vec3P findClosestPointOfSolidBox(const Vec3P &pt, bool &ptWasInside) const
Given a point location in the box frame, return the closest point of the solid box,...
Definition: Geo_Box.h:121
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 getEdgeFaces(int ex, int f[2], int w[2]) const
An edge is between two faces.
Definition: Geo_Box.h:415
UnitVec3P getVertexNormal(int vx) const
Vertex normals point diagonally outwards from the box corners.
Definition: Geo_Box.h:285
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
bool mayIntersectOrientedBox(const Geo::OrientedBox_< P > &ob) const
Given an oriented box whose pose is measured and expressed in the frame of this box,...
CoordinateAxis getOrderedAxis(int i) const
Get axes in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:97
void getVertexEdges(int vx, int e[3], int w[3]) const
Each vertex has three incident edges.
Definition: Geo_Box.h:395
Box_ & addToHalfLengths(const Vec3P &incr)
Change the half-dimensions of this box by adding the given vector.
Definition: Geo_Box.h:77
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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo_Box.h:528
RotationP & updOrientation()
Definition: Geo_Box.h:560
Vec3P & updCenter()
Definition: Geo_Box.h:558
const Box_< P > & getBox() const
Definition: Geo_Box.h:565
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
Box_< P > & updBox()
Definition: Geo_Box.h:566
OrientedBox_ & setTransform(const TransformP &newX_FB)
Change the pose of this box.
Definition: Geo_Box.h:550
const Vec3P & getHalfLengths() const
Definition: Geo_Box.h:563
const TransformP & getTransform() const
Definition: Geo_Box.h:561
OrientedBox_(const TransformP &X_FB, const Vec3P &halfLengths)
Construct an OrientedBox with the given location and half-dimensions.
Definition: Geo_Box.h:545
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
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
TransformP & updTransform()
Definition: Geo_Box.h:562
const RotationP & getOrientation() const
Definition: Geo_Box.h:559
const Vec3P & getCenter() const
Definition: Geo_Box.h:557
OrientedBox_()
Construct an uninitialized OrientedBox object; the dimensions and pose will be garbage.
Definition: Geo_Box.h:537
OrientedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:554
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo_Sphere.h:47
RealP getRadius() const
Get the sphere's radius.
Definition: Geo_Sphere.h:111
const Vec3P & getCenter() const
Get the location of the sphere's center point.
Definition: Geo_Sphere.h:107
The Geo class collects geometric primitives intended to deal with raw, fixed-size geometric shapes oc...
Definition: Geo.h:53
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Rotation.h:111
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:56
This is a fixed-length column vector designed for no-overhead inline computation.
Definition: Vec.h:184
TAbs abs() const
Elementwise absolute value; that is, the return value has the same dimension as this Vec but with eac...
Definition: Vec.h:347
#define SimTK_SQRT3
The square root of 3.
Definition: Constants.h:214
#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
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
unsigned char square(unsigned char u)
Definition: Scalar.h:349
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...