1 #ifndef SimTK_SIMMATH_GEO_BOX_H_
2 #define SimTK_SIMMATH_GEO_BOX_H_
50 typedef Vec<3,P> Vec3P;
51 typedef Mat<3,3,P> Mat33P;
52 typedef Rotation_<P> RotationP;
53 typedef Transform_<P> TransformP;
65 SimTK_ERRCHK3(halfLengths >= 0,
"Geo::Box_::setHalfLengths()",
66 "Half lengths must be nonnegative; got %g,%g,%g.",
67 (
double)halfLengths[0],(
double)halfLengths[1],(
double)halfLengths[2]);
78 "Half lengths must be nonnegative but were %g,%g,%g after change.",
79 (
double)h[0],(
double)h[1],(
double)h[2]);
104 RealP
findArea()
const {
return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);}
110 const Vec3P absPt = pt.
abs();
119 const Vec3P absPt = pt.
abs();
121 if (absPt[0] > h[0]) d2 +=
square(absPt[0]-h[0]);
122 if (absPt[1] > h[1]) d2 +=
square(absPt[1]-h[1]);
123 if (absPt[2] > h[2]) d2 +=
square(absPt[2]-h[2]);
133 const Vec3P grow = h + sphere.
getRadius();
135 if (absCtr[0] > grow[0]) d2 +=
square(absCtr[0]-grow[0]);
136 if (absCtr[1] > grow[1]) d2 +=
square(absCtr[1]-grow[1]);
137 if (absCtr[2] > grow[2]) d2 +=
square(absCtr[2]-grow[2]);
149 if (absCtr[0] > grow[0]) d2 +=
square(absCtr[0]-grow[0]);
150 if (absCtr[1] > grow[1]) d2 +=
square(absCtr[1]-grow[1]);
151 if (absCtr[2] > grow[2]) d2 +=
square(absCtr[2]-grow[2]);
162 if (absCtr[0] > h[0]+r)
return false;
163 if (absCtr[1] > h[1]+r)
return false;
164 if (absCtr[2] > h[2]+r)
return false;
175 if (absCtr[0] > h[0]+aabh[0])
return false;
176 if (absCtr[1] > h[1]+aabh[1])
return false;
177 if (absCtr[2] > h[2]+aabh[2])
return false;
211 order[0] = shortest; order[2] = longest;
212 order[1] = shortest.getThirdAxis(longest);
215 int intersectsOrientedBoxHelper(
const OrientedBox_<P>& O,
217 Vec3P& absP_BO)
const;
220 unsigned char order[3];
234 typedef Vec<3,RealP> Vec3P;
243 : center(center), box(box) {}
247 : center(center), box(halfLengths) {}
251 { this->center=center;
return *
this; }
272 {
return box.containsPoint(pt_F - center); }
282 const RealP tol = Geo::getDefaultTol<P>();
283 const RealP maxdim =
max(getCenter().
abs());
284 const RealP maxrad =
max(getHalfLengths());
285 const RealP scale =
std::max(maxdim, maxrad);
286 const RealP incr =
std::max(scale*Geo::getEps<P>(), tol);
287 box.addToHalfLengths(Vec3P(incr));
317 : X_FB(X_FB), box(box) {}
321 : X_FB(X_FB), box(halfLengths) {}
326 { X_FB=newX_FB;
return *
this; }
348 {
return box.containsPoint(~X_FB*pt_F); }
358 const RealP tol = Geo::getDefaultTol<P>();
359 const RealP maxdim =
max(getCenter().
abs());
360 const RealP maxrad =
max(getHalfLengths());
361 const RealP scale =
std::max(maxdim, maxrad);
362 const RealP incr =
std::max(scale*Geo::getEps<P>(), tol);
363 box.addToHalfLengths(Vec3P(incr));
375 #endif // SimTK_SIMMATH_GEO_BOX_H_
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:118
Box_ & addToHalfLengths(const Vec3P &incr)
Change the half-dimensions of this box by adding the given vector.
Definition: Geo_Box.h:75
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:242
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:109
A 3d box aligned with an unspecified frame F and centered at a given point measured from that frame's...
Definition: Geo.h:62
CoordinateAxis getOrderedAxis(int i) const
Get axes in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:95
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
RealP findArea() const
Calculate the surface area of this box, ignoring degeneracy (meaning that all pairs of sides are coun...
Definition: Geo_Box.h:104
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:145
const Box_< P > & getBox() const
Definition: Geo_Box.h:340
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
const Vec3P & getCenter() const
Return the location of the center of this box (box frame origin).
Definition: Geo_Box.h:258
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.
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:281
RotationP & updOrientation()
Definition: Geo_Box.h:335
const Vec3P & getHalfLengths() const
Definition: Geo_Box.h:338
const Vec3P & getCenter() const
Get the location of the sphere's center point.
Definition: Geo_Sphere.h:107
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
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:357
Vec3P & updCenter()
Definition: Geo_Box.h:333
Box_ & setHalfLengths(const Vec3P &halfLengths)
Change the half-dimensions of this box.
Definition: Geo_Box.h:64
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:347
RealP getOrderedHalfLength(int i) const
Get lengths in order shortest to longest; 0 is shortest, 2 is longest.
Definition: Geo_Box.h:89
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
unsigned char square(unsigned char u)
Definition: Scalar.h:351
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:131
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:316
OrientedBox_(const TransformP &X_FB, const Vec3P &halfLengths)
Construct an OrientedBox with the given location and half-dimensions.
Definition: Geo_Box.h:320
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
OrientedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:329
The Geo class collects geometric primitives intended to deal with raw, fixed-size geometric shapes oc...
Definition: Geo.h:53
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:271
const RotationP & getOrientation() const
Definition: Geo_Box.h:334
AlignedBox_ & setHalfLengths(const Vec3P &halfLengths)
Change the dimensions of this box.
Definition: Geo_Box.h:254
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
AlignedBox_()
Construct an uninitialized AlignedBox object; the dimensions and location will be garbage...
Definition: Geo_Box.h:239
Defines geometric primitive shapes and algorthms.
Box_()
Construct an uninitialized Box object; the dimensions will be garbage.
Definition: Geo_Box.h:57
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:263
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.
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin...
Definition: Geo.h:61
RealP getRadius() const
Get the sphere's radius.
Definition: Geo_Sphere.h:111
const Box_< P > & getBox() const
Definition: Geo_Box.h:265
RealP findVolume() const
Calculate the volume of this box.
Definition: Geo_Box.h:101
Vec3P & updCenter()
Return a writable reference to the center location of this box.
Definition: Geo_Box.h:260
A geometric primitive representing a sphere by its radius and center point, and a collection of spher...
Definition: Geo.h:56
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
Box_< P > & updBox()
Definition: Geo_Box.h:266
TODO: A 3d box oriented and positioned with respect to an unspecified frame F.
Definition: Geo.h:63
This is the header file that every Simmath compilation unit should include first. ...
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 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:159
const Vec3P & getCenter() const
Definition: Geo_Box.h:332
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
AlignedBox_ & setCenter(const Vec3P ¢er)
Change the center location of this box.
Definition: Geo_Box.h:250
const TransformP & getTransform() const
Definition: Geo_Box.h:336
TransformP & updTransform()
Definition: Geo_Box.h:337
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:86
AlignedBox_(const Vec3P ¢er, const Vec3P &halfLengths)
Construct an AlignedBox with the given center location and half-dimensions.
Definition: Geo_Box.h:246
OrientedBox_()
Construct an uninitialized OrientedBox object; the dimensions and pose will be garbage.
Definition: Geo_Box.h:312
Box_< P > & updBox()
Definition: Geo_Box.h:341
OrientedBox_ & setTransform(const TransformP &newX_FB)
Change the pose of this box.
Definition: Geo_Box.h:325
Box_(const Vec3P &halfLengths)
Construct a Box with the given nonnegative half-dimensions.
Definition: Geo_Box.h:60
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
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:172
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...