blob: b0ea2b4bf51a0cc94e76d1a26b4b9e407f29c2d4 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
#include "linalg.h"
#include <boost/math/special_functions/fpclassify.hpp>
// FIXME: We can achieve better pruning by either:
// o Recalculate the box based on the transformed object
// o Store boxes are oriented bounding boxes and implement oriented
// bbox intersection
// FIXME: This function can be generalized, but we don't need it atm.
/*!
Transforms the given bounding box by transforming each of its 8 vertices.
Returns a new bounding box.
*/
BoundingBox operator*(const Transform3d &m, const BoundingBox &box)
{
BoundingBox newbox;
Vector3d boxvec[2] = { box.min(), box.max() };
for (int k=0;k<2;k++) {
for (int j=0;j<2;j++) {
for (int i=0;i<2;i++) {
newbox.extend(m * Vector3d(boxvec[i][0], boxvec[j][1], boxvec[k][2]));
}
}
}
return newbox;
}
bool matrix_contains_infinity( const Eigen::Transform3d &m )
{
for (int i=0;i<m.matrix().rows();i++) {
for (int j=0;j<m.matrix().cols();j++) {
if ((boost::math::isinf)(m(i,j))) return true;
}
}
return false;
}
bool matrix_contains_nan( const Eigen::Transform3d &m )
{
for (int i=0;i<m.matrix().rows();i++) {
for (int j=0;j<m.matrix().cols();j++) {
if ((boost::math::isnan)(m(i,j))) return true;
}
}
return false;
}
|
