Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
Geo_Box.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_GEO_BOX_H_
2 #define SimTK_SIMMATH_GEO_BOX_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKmath *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2011-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
30 #include "SimTKcommon.h"
32 #include "simmath/internal/Geo.h"
33 
34 #include <cassert>
35 #include <cmath>
36 #include <algorithm>
37 
38 namespace SimTK {
39 
40 //==============================================================================
41 // GEO BOX
42 //==============================================================================
47 template <class P>
48 class Geo::Box_ {
49 typedef P RealP;
50 typedef Vec<3,P> Vec3P;
51 typedef Mat<3,3,P> Mat33P;
52 typedef Rotation_<P> RotationP;
53 typedef Transform_<P> TransformP;
54 
55 public:
57 Box_() {}
60 Box_(const Vec3P& halfLengths) {setHalfLengths(halfLengths);}
61 
64 Box_& setHalfLengths(const Vec3P& halfLengths) {
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]);
68  h = halfLengths;
69  sortEdges();
70  return *this;
71 }
72 
75 Box_& addToHalfLengths(const Vec3P& incr) {
76  h += incr;
77  SimTK_ERRCHK3(h >= 0, "Geo::Box_::addToHalfLengths()",
78  "Half lengths must be nonnegative but were %g,%g,%g after change.",
79  (double)h[0],(double)h[1],(double)h[2]);
80  sortEdges();
81  return *this;
82 }
83 
86 const Vec3P& getHalfLengths() const {return h;}
87 
89 RealP getOrderedHalfLength(int i) const {
90  SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedHalfLength()");
91  return h[order[i]];
92 }
93 
96  SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedAxis()");
97  return CoordinateAxis(order[i]);
98 }
99 
101 RealP findVolume() const {return 8*h[0]*h[1]*h[2];}
104 RealP findArea() const {return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);}
105 
109 bool containsPoint(const Vec3P& pt) const {
110  const Vec3P absPt = pt.abs(); // reflect to first quadrant
111  return absPt <= h;
112 }
113 
118 RealP findDistanceSqrToPoint(const Vec3P& pt) const {
119  const Vec3P absPt = pt.abs(); // reflect to first quadrant
120  RealP d2 = 0;
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]);
124  return d2;
125 }
126 
131 RealP findDistanceSqrToSphere(const Geo::Sphere_<P>& sphere) const {
132  const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
133  const Vec3P grow = h + sphere.getRadius(); // 3 flops
134  RealP d2 = 0;
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]);
138  return d2;
139 }
140 
146  const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
147  const Vec3P grow = h + aab.getHalfLengths();
148  RealP d2 = 0;
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]);
152  return d2;
153 }
154 
159 bool intersectsSphere(const Geo::Sphere_<P>& sphere) const {
160  const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
161  const RealP r = sphere.getRadius();
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;
165  return true;
166 }
167 
173  const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
174  const Vec3P& aabh = aab.getHalfLengths();
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;
178  return true;
179 }
180 
191 
201 
202 
203 private:
204 // Call this whenever an edge length changes. Each axis will appear once.
205 void sortEdges() {
206  CoordinateAxis shortest = XAxis, longest = ZAxis;
207  if (h[YAxis] < h[shortest]) shortest=YAxis;
208  if (h[ZAxis] < h[shortest]) shortest=ZAxis;
209  if (h[XAxis] > h[longest]) longest=XAxis;
210  if (h[YAxis] > h[longest]) longest=YAxis;
211  order[0] = shortest; order[2] = longest;
212  order[1] = shortest.getThirdAxis(longest); // not shortest or longest
213 }
214 
215 int intersectsOrientedBoxHelper(const OrientedBox_<P>& O,
216  Mat33P& absR_BO,
217  Vec3P& absP_BO) const;
218 
219 Vec3P h; // half-dimensions of the box
220 unsigned char order[3]; // 0,1,2 reordered short to long
221 };
222 
223 
224 
225 //==============================================================================
226 // GEO ALIGNED BOX
227 //==============================================================================
231 template <class P>
232 class SimTK_SIMMATH_EXPORT Geo::AlignedBox_ {
233 typedef P RealP;
234 typedef Vec<3,RealP> Vec3P;
235 
236 public:
242 AlignedBox_(const Vec3P& center, const Geo::Box_<P>& box)
243 : center(center), box(box) {}
246 AlignedBox_(const Vec3P& center, const Vec3P& halfLengths)
247 : center(center), box(halfLengths) {}
248 
250 AlignedBox_& setCenter(const Vec3P& center)
251 { this->center=center; return *this; }
252 
254 AlignedBox_& setHalfLengths(const Vec3P& halfLengths)
255 { box.setHalfLengths(halfLengths); return *this; }
256 
258 const Vec3P& getCenter() const {return center;}
260 Vec3P& updCenter() {return center;}
263 const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
264 // no updHalfLengths()
265 const Box_<P>& getBox() const {return box;}
266 Box_<P>& updBox() {return box;}
267 
271 bool containsPoint(const Vec3P& pt_F) const
272 { return box.containsPoint(pt_F - center); } // shift to box frame B
273 
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));
288  return *this;
289 }
290 
291 private:
292 Vec3P center;
293 Geo::Box_<P> box;
294 };
295 
296 
297 //==============================================================================
298 // GEO ORIENTED BOX
299 //==============================================================================
302 template <class P>
303 class SimTK_SIMMATH_EXPORT Geo::OrientedBox_ {
304 typedef P RealP;
305 typedef Vec<3,P> Vec3P;
306 typedef Rotation_<P> RotationP;
307 typedef Transform_<P> TransformP;
308 
309 public:
316 OrientedBox_(const TransformP& X_FB, const Geo::Box_<P>& box)
317 : X_FB(X_FB), box(box) {}
320 OrientedBox_(const TransformP& X_FB, const Vec3P& halfLengths)
321 : X_FB(X_FB), box(halfLengths) {}
322 
323 
325 OrientedBox_& setTransform(const TransformP& newX_FB)
326 { X_FB=newX_FB; return *this; }
327 
329 OrientedBox_& setHalfLengths(const Vec3P& halfLengths)
330 { box.setHalfLengths(halfLengths); return *this; }
331 
332 const Vec3P& getCenter() const {return X_FB.p();}
333 Vec3P& updCenter() {return X_FB.updP();}
334 const RotationP& getOrientation() const {return X_FB.R();}
335 RotationP& updOrientation() {return X_FB.updR();}
336 const TransformP& getTransform() const {return X_FB;}
337 TransformP& updTransform() {return X_FB;}
338 const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
339 // no updHalfLengths()
340 const Box_<P>& getBox() const {return box;}
341 Box_<P>& updBox() {return box;}
342 
343 
347 bool containsPoint(const Vec3P& pt_F) const
348 { return box.containsPoint(~X_FB*pt_F); } // shift to box frame B
349 
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));
364  return *this;
365 }
366 
367 private:
368 TransformP X_FB;
369 Geo::Box_<P> box;
370 };
371 
372 
373 } // namespace SimTK
374 
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 &center, 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
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
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 &center)
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 &center, 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 ...