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]);
106RealP
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;
141 dToSide = h[1]-std::abs(c[1]);
142 if (dToSide < minDist) {which=1; minDist=dToSide;}
143 dToSide = h[2]-std::abs(c[2]);
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);
440int intersectsOrientedBoxHelper(
const OrientedBox_<P>& O,
442 Vec3P& absP_BO)
const;
445unsigned 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
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
AlignedBox_()
Construct an uninitialized AlignedBox object; the dimensions and location will be garbage.
Definition Geo_Box.h:464
AlignedBox_ & setCenter(const Vec3P ¢er)
Change the center location of this box.
Definition Geo_Box.h:475
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 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
const Box_< P > & getBox() const
Definition Geo_Box.h:490
Box_< P > & updBox()
Definition Geo_Box.h:491
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
Box_ & addToHalfLengths(const Vec3P &incr)
Change the half-dimensions of this box by adding the given vector.
Definition Geo_Box.h:77
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
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
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
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_(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
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
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
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition Geo_Box.h:528
Box_< P > & updBox()
Definition Geo_Box.h:566
RotationP & updOrientation()
Definition Geo_Box.h:560
const Box_< P > & getBox() const
Definition Geo_Box.h:565
const TransformP & getTransform() const
Definition Geo_Box.h:561
const Vec3P & getCenter() const
Definition Geo_Box.h:557
const Vec3P & getHalfLengths() const
Definition Geo_Box.h:563
OrientedBox_ & setTransform(const TransformP &newX_FB)
Change the pose of this box.
Definition Geo_Box.h:550
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
OrientedBox_(const TransformP &X_FB, const Vec3P &halfLengths)
Construct an OrientedBox with the given location and half-dimensions.
Definition Geo_Box.h:545
Vec3P & updCenter()
Definition Geo_Box.h:558
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
const RotationP & getOrientation() const
Definition Geo_Box.h:559
OrientedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition Geo_Box.h:554
TransformP & updTransform()
Definition Geo_Box.h:562
OrientedBox_()
Construct an uninitialized OrientedBox object; the dimensions and pose will be garbage.
Definition Geo_Box.h:537
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
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
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
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition VectorMath.h:120
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
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 ...