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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
|
// many thanks to http://codesuppository.blogspot.com/2006_06_01_archive.html
// the code written after http://www.amillionpixels.us/bestfitobb.cpp
// which is MIT-licensed
#include <lib/base/Logging.hpp>
#include <lib/base/Math.hpp>
#include <lib/high-precision/Constants.hpp>
#include <lib/pyutil/doc_opts.hpp>
CREATE_CPP_LOCAL_LOGGER("_packObb.cpp");
template <typename Scalar> Eigen::Matrix<Scalar, 3, 3> matrixFromEulerAnglesXYZ(Scalar x, Scalar y, Scalar z)
{
Eigen::Matrix<Scalar, 3, 3> m;
m = Eigen::AngleAxis<Scalar>(x, Eigen::Matrix<Scalar, 3, 1>::UnitX()) * Eigen::AngleAxis<Scalar>(y, Eigen::Matrix<Scalar, 3, 1>::UnitY())
* Eigen::AngleAxis<Scalar>(z, Eigen::Matrix<Scalar, 3, 1>::UnitZ());
return m;
}
inline yade::Matrix3r makeFromEulerAngle(::yade::Real x, ::yade::Real y, ::yade::Real z) { return matrixFromEulerAnglesXYZ<::yade::Real>(x, y, z); }
namespace yade { // Cannot have #include directive inside.
// compute minimum bounding for a cloud of points
// returns volume
Real computeOBB(const std::vector<Vector3r>& pts, const Matrix3r& rot, Vector3r& center, Vector3r& halfSize)
{
const Real inf = std::numeric_limits<Real>::infinity();
Vector3r mn(inf, inf, inf), mx(-inf, -inf, -inf);
FOREACH(const Vector3r& pt, pts)
{
Vector3r ptT = rot * pt;
mn = mn.cwiseMin(ptT);
mx = mx.cwiseMax(ptT);
}
halfSize = .5 * (mx - mn);
center = .5 * (mn + mx);
return 8 * halfSize[0] * halfSize[1] * halfSize[2];
}
void bestFitOBB(const std::vector<Vector3r>& pts, Vector3r& center, Vector3r& halfSize, Quaternionr& rot)
{
Vector3r angle0(Vector3r::Zero()), angle(Vector3r::Zero());
Vector3r center0;
Vector3r halfSize0;
Real bestVolume = std::numeric_limits<Real>::infinity();
Real sweep = Mathr::PI / 4;
Real steps = 7.;
while (sweep >= Mathr::PI / 180.) {
bool found = false;
Real stepSize = sweep / steps;
for (Real x = angle0[0] - sweep; x <= angle0[0] + sweep; x += stepSize) {
for (Real y = angle0[1] - sweep; y < angle0[1] + sweep; y += stepSize) {
for (Real z = angle0[2] - sweep; z < angle0[2] + sweep; z += stepSize) {
Matrix3r rot0(makeFromEulerAngle(x, y, z));
Real volume = computeOBB(pts, rot0, center0, halfSize0);
if (volume < bestVolume) {
bestVolume = volume;
angle = angle0;
// set return values, in case this will be really the best fit
rot = Quaternionr(rot0);
rot = rot.conjugate();
center = center0;
halfSize = halfSize0;
found = true;
}
}
}
}
if (found) {
angle0 = angle;
sweep *= .5;
} else
return;
}
}
boost::python::tuple bestFitOBB_py(const boost::python::tuple& _pts)
{
int l = boost::python::len(_pts);
if (l <= 1) throw std::runtime_error("Cloud must have at least 2 points.");
std::vector<Vector3r> pts;
pts.resize(l);
for (int i = 0; i < l; i++)
pts[i] = boost::python::extract<Vector3r>(_pts[i]);
Quaternionr rot;
Vector3r halfSize, center;
bestFitOBB(pts, center, halfSize, rot);
return boost::python::make_tuple(center, halfSize, rot);
}
} // namespace yade
// BOOST_PYTHON_MODULE cannot be inside yade namespace, it has 'extern "C"' keyword, which strips it out of any namespaces.
BOOST_PYTHON_MODULE(_packObb)
try {
YADE_SET_DOCSTRING_OPTS;
boost::python::scope().attr("__doc__") = "Computation of oriented bounding box for cloud of points.";
boost::python::def(
"cloudBestFitOBB",
yade::bestFitOBB_py,
"Return (Vector3 center, Vector3 halfSize, Quaternion orientation) of\nbest-fit oriented bounding-box for given tuple of points\n(uses "
"brute-force velome minimization, do not use for very large clouds).");
} catch (...) {
LOG_FATAL("Importing this module caused an exception and this module is in an inconsistent state now.");
PyErr_Print();
PyErr_SetString(PyExc_SystemError, __FILE__);
boost::python::handle_exception();
throw;
}
|
