402 lines
12 KiB
C++
402 lines
12 KiB
C++
////////////////////////////////////////////////////////////////////
|
|
// OBB.inl
|
|
//
|
|
// Copyright 2007 cDc@seacave
|
|
// Distributed under the Boost Software License, Version 1.0
|
|
// (See http://www.boost.org/LICENSE_1_0.txt)
|
|
|
|
|
|
// D E F I N E S ///////////////////////////////////////////////////
|
|
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>::TOBB(bool)
|
|
:
|
|
m_rot(MATRIX::Identity()),
|
|
m_pos(POINT::Zero()),
|
|
m_ext(POINT::Zero())
|
|
{
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>::TOBB(const AABB& aabb)
|
|
{
|
|
Set(aabb);
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>::TOBB(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax)
|
|
{
|
|
Set(rot, ptMin, ptMax);
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>::TOBB(const POINT* pts, size_t n)
|
|
{
|
|
Set(pts, n);
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>::TOBB(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s)
|
|
{
|
|
Set(pts, n, tris, s);
|
|
} // constructor
|
|
template <typename TYPE, int DIMS>
|
|
template <typename CTYPE>
|
|
inline TOBB<TYPE,DIMS>::TOBB(const TOBB<CTYPE,DIMS>& rhs)
|
|
:
|
|
m_rot(rhs.m_rot.template cast<TYPE>()),
|
|
m_pos(rhs.m_pos.template cast<TYPE>()),
|
|
m_ext(rhs.m_ext.template cast<TYPE>())
|
|
{
|
|
} // copy constructor
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Set(const AABB& aabb)
|
|
{
|
|
m_rot.setIdentity();
|
|
m_pos = aabb.GetCenter();
|
|
m_ext = aabb.GetSize()/TYPE(2);
|
|
}
|
|
|
|
// build from rotation matrix from world to local, and local min/max corners
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Set(const MATRIX& rot, const POINT& ptMin, const POINT& ptMax)
|
|
{
|
|
m_rot = rot;
|
|
m_pos = (ptMax + ptMin) * TYPE(0.5);
|
|
m_ext = (ptMax - ptMin) * TYPE(0.5);
|
|
}
|
|
|
|
// Inspired from "Fitting Oriented Bounding Boxes" by James Gregson
|
|
// http://jamesgregson.blogspot.ro/2011/03/latex-test.html
|
|
|
|
// build an OBB from a vector of input points. This
|
|
// method just forms the covariance matrix and hands
|
|
// it to the build_from_covariance_matrix method
|
|
// which handles fitting the box to the points
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Set(const POINT* pts, size_t n)
|
|
{
|
|
ASSERT(n >= DIMS);
|
|
|
|
// loop over the points to find the mean point
|
|
// location and to build the covariance matrix;
|
|
// note that we only have
|
|
// to build terms for the upper triangular
|
|
// portion since the matrix is symmetric
|
|
POINT mu(POINT::Zero());
|
|
TYPE cxx=0, cxy=0, cxz=0, cyy=0, cyz=0, czz=0;
|
|
for (size_t i=0; i<n; ++i) {
|
|
const POINT& p = pts[i];
|
|
mu += p;
|
|
cxx += p(0)*p(0);
|
|
cxy += p(0)*p(1);
|
|
cxz += p(0)*p(2);
|
|
cyy += p(1)*p(1);
|
|
cyz += p(1)*p(2);
|
|
czz += p(2)*p(2);
|
|
}
|
|
const TYPE invN(TYPE(1)/TYPE(n));
|
|
cxx = (cxx - mu(0)*mu(0)*invN)*invN;
|
|
cxy = (cxy - mu(0)*mu(1)*invN)*invN;
|
|
cxz = (cxz - mu(0)*mu(2)*invN)*invN;
|
|
cyy = (cyy - mu(1)*mu(1)*invN)*invN;
|
|
cyz = (cyz - mu(1)*mu(2)*invN)*invN;
|
|
czz = (czz - mu(2)*mu(2)*invN)*invN;
|
|
|
|
// now build the covariance matrix
|
|
MATRIX C;
|
|
C(0,0) = cxx; C(0,1) = cxy; C(0,2) = cxz;
|
|
C(1,0) = cxy; C(1,1) = cyy; C(1,2) = cyz;
|
|
C(2,0) = cxz; C(2,1) = cyz; C(2,2) = czz;
|
|
|
|
// set the OBB parameters from the covariance matrix
|
|
Set(C, pts, n);
|
|
}
|
|
// builds an OBB from triangles specified as an array of
|
|
// points with integer indices into the point array. Forms
|
|
// the covariance matrix for the triangles, then uses the
|
|
// method build_from_covariance_matrix method to fit
|
|
// the box. ALL points will be fit in the box, regardless
|
|
// of whether they are indexed by a triangle or not.
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Set(const POINT* pts, size_t n, const TRIANGLE* tris, size_t s)
|
|
{
|
|
ASSERT(n >= DIMS);
|
|
|
|
// loop over the triangles this time to find the
|
|
// mean location
|
|
POINT mu(POINT::Zero());
|
|
TYPE Am=0;
|
|
TYPE cxx=0, cxy=0, cxz=0, cyy=0, cyz=0, czz=0;
|
|
for (size_t i=0; i<s; ++i) {
|
|
ASSERT(tris[i](0)<n && tris[i](1)<n && tris[i](2)<n);
|
|
const POINT& p = pts[tris[i](0)];
|
|
const POINT& q = pts[tris[i](1)];
|
|
const POINT& r = pts[tris[i](2)];
|
|
const POINT mui = (p+q+r)/TYPE(3);
|
|
const TYPE Ai = (q-p).cross(r-p).normalize()/TYPE(2);
|
|
mu += mui*Ai;
|
|
Am += Ai;
|
|
|
|
// these bits set the c terms to Am*E[xx], Am*E[xy], Am*E[xz]....
|
|
const TYPE Ai12 = Ai/TYPE(12);
|
|
cxx += (TYPE(9)*mui(0)*mui(0) + p(0)*p(0) + q(0)*q(0) + r(0)*r(0))*Ai12;
|
|
cxy += (TYPE(9)*mui(0)*mui(1) + p(0)*p(1) + q(0)*q(1) + r(0)*r(1))*Ai12;
|
|
cxz += (TYPE(9)*mui(0)*mui(2) + p(0)*p(2) + q(0)*q(2) + r(0)*r(2))*Ai12;
|
|
cyy += (TYPE(9)*mui(1)*mui(1) + p(1)*p(1) + q(1)*q(1) + r(1)*r(1))*Ai12;
|
|
cyz += (TYPE(9)*mui(1)*mui(2) + p(1)*p(2) + q(1)*q(2) + r(1)*r(2))*Ai12;
|
|
czz += (TYPE(9)*mui(2)*mui(2) + p(2)*p(2) + q(2)*q(2) + r(2)*r(2))*Ai12;
|
|
}
|
|
|
|
// divide out the Am fraction from the average position and
|
|
// covariance terms
|
|
mu /= Am;
|
|
cxx /= Am; cxy /= Am; cxz /= Am; cyy /= Am; cyz /= Am; czz /= Am;
|
|
|
|
// now subtract off the E[x]*E[x], E[x]*E[y], ... terms
|
|
cxx -= mu(0)*mu(0); cxy -= mu(0)*mu(1); cxz -= mu(0)*mu(2);
|
|
cyy -= mu(1)*mu(1); cyz -= mu(1)*mu(2); czz -= mu(2)*mu(2);
|
|
|
|
// now build the covariance matrix
|
|
MATRIX C;
|
|
C(0,0)=cxx; C(0,1)=cxy; C(0,2)=cxz;
|
|
C(1,0)=cxy; C(1,1)=cyy; C(1,2)=cyz;
|
|
C(2,0)=cxz; C(1,2)=cyz; C(2,2)=czz;
|
|
|
|
// set the obb parameters from the covariance matrix
|
|
Set(C, pts, n);
|
|
}
|
|
// method to set the OBB parameters which produce a box oriented according to
|
|
// the covariance matrix C, and that contains the given points
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Set(const MATRIX& C, const POINT* pts, size_t n)
|
|
{
|
|
// extract rotation from the covariance matrix
|
|
SetRotation(C);
|
|
// extract size and center from the given points
|
|
SetBounds(pts, n);
|
|
}
|
|
// method to set the OBB rotation which produce a box oriented according to
|
|
// the covariance matrix C (only the rotations is set)
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::SetRotation(const MATRIX& C)
|
|
{
|
|
// extract the eigenvalues and eigenvectors from C
|
|
const Eigen::SelfAdjointEigenSolver<MATRIX> es(C);
|
|
ASSERT(es.info() == Eigen::Success);
|
|
// find the right, up and forward vectors from the eigenvectors
|
|
// and set the rotation matrix using the eigenvectors
|
|
ASSERT(es.eigenvalues()(0) < es.eigenvalues()(1) && es.eigenvalues()(1) < es.eigenvalues()(2));
|
|
m_rot = es.eigenvectors().transpose();
|
|
if (m_rot.determinant() < 0)
|
|
m_rot = -m_rot;
|
|
}
|
|
// method to set the OBB center and size that contains the given points
|
|
// the rotations should be already set
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::SetBounds(const POINT* pts, size_t n)
|
|
{
|
|
ASSERT(n >= DIMS);
|
|
ASSERT(ISEQUAL((m_rot*m_rot.transpose()).trace(), TYPE(3)) && ISEQUAL(m_rot.determinant(), TYPE(1)));
|
|
|
|
// build the bounding box extents in the rotated frame
|
|
const TYPE tmax = std::numeric_limits<TYPE>::max();
|
|
POINT minim(tmax, tmax, tmax), maxim(-tmax, -tmax, -tmax);
|
|
for (size_t i=0; i<n; ++i) {
|
|
const POINT p_prime(m_rot * pts[i]);
|
|
if (minim(0) > p_prime(0)) minim(0) = p_prime(0);
|
|
if (minim(1) > p_prime(1)) minim(1) = p_prime(1);
|
|
if (minim(2) > p_prime(2)) minim(2) = p_prime(2);
|
|
if (maxim(0) < p_prime(0)) maxim(0) = p_prime(0);
|
|
if (maxim(1) < p_prime(1)) maxim(1) = p_prime(1);
|
|
if (maxim(2) < p_prime(2)) maxim(2) = p_prime(2);
|
|
}
|
|
|
|
// set the center of the OBB to be the average of the
|
|
// minimum and maximum, and the extents be half of the
|
|
// difference between the minimum and maximum
|
|
const POINT center((maxim+minim)*TYPE(0.5));
|
|
m_pos = m_rot.transpose() * center;
|
|
m_ext = (maxim-minim)*TYPE(0.5);
|
|
} // Set
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::BuildBegin()
|
|
{
|
|
m_rot = MATRIX::Zero();
|
|
m_pos = POINT::Zero();
|
|
m_ext = POINT::Zero();
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::BuildAdd(const POINT& p)
|
|
{
|
|
// store mean in m_pos
|
|
m_pos += p;
|
|
// store covariance params in m_rot
|
|
m_rot(0,0) += p(0)*p(0);
|
|
m_rot(0,1) += p(0)*p(1);
|
|
m_rot(0,2) += p(0)*p(2);
|
|
m_rot(1,0) += p(1)*p(1);
|
|
m_rot(1,1) += p(1)*p(2);
|
|
m_rot(1,2) += p(2)*p(2);
|
|
// store count in m_ext
|
|
++(*((size_t*)m_ext.data()));
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::BuildEnd()
|
|
{
|
|
const TYPE invN(TYPE(1)/TYPE(*((size_t*)m_ext.data())));
|
|
const TYPE cxx = (m_rot(0,0) - m_pos(0)*m_pos(0)*invN)*invN;
|
|
const TYPE cxy = (m_rot(0,1) - m_pos(0)*m_pos(1)*invN)*invN;
|
|
const TYPE cxz = (m_rot(0,2) - m_pos(0)*m_pos(2)*invN)*invN;
|
|
const TYPE cyy = (m_rot(1,0) - m_pos(1)*m_pos(1)*invN)*invN;
|
|
const TYPE cyz = (m_rot(1,1) - m_pos(1)*m_pos(2)*invN)*invN;
|
|
const TYPE czz = (m_rot(1,2) - m_pos(2)*m_pos(2)*invN)*invN;
|
|
|
|
// now build the covariance matrix
|
|
MATRIX C;
|
|
C(0,0) = cxx; C(0,1) = cxy; C(0,2) = cxz;
|
|
C(1,0) = cxy; C(1,1) = cyy; C(1,2) = cyz;
|
|
C(2,0) = cxz; C(2,1) = cyz; C(2,2) = czz;
|
|
SetRotation(C);
|
|
} // Build
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// check if the oriented bounding box has positive size
|
|
template <typename TYPE, int DIMS>
|
|
inline bool TOBB<TYPE,DIMS>::IsValid() const
|
|
{
|
|
return m_ext.minCoeff() > TYPE(0);
|
|
} // IsValid
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>& TOBB<TYPE,DIMS>::Enlarge(TYPE x)
|
|
{
|
|
m_ext.array() += x;
|
|
return *this;
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TOBB<TYPE,DIMS>& TOBB<TYPE,DIMS>::EnlargePercent(TYPE x)
|
|
{
|
|
m_ext *= x;
|
|
return *this;
|
|
} // Enlarge
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Update the box by the given pos delta.
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Translate(const POINT& d)
|
|
{
|
|
m_pos += d;
|
|
}
|
|
// Update the box by the given transform.
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::Transform(const MATRIX& m)
|
|
{
|
|
m_rot = m * m_rot;
|
|
m_pos = m * m_pos;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TOBB<TYPE,DIMS>::POINT TOBB<TYPE,DIMS>::GetCenter() const
|
|
{
|
|
return m_pos;
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::GetCenter(POINT& ptCenter) const
|
|
{
|
|
ptCenter = m_pos;
|
|
} // GetCenter
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TOBB<TYPE,DIMS>::POINT TOBB<TYPE,DIMS>::GetSize() const
|
|
{
|
|
return m_ext*2;
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::GetSize(POINT& ptSize) const
|
|
{
|
|
ptSize = m_ext*2;
|
|
} // GetSize
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline void TOBB<TYPE,DIMS>::GetCorners(POINT pts[numCorners]) const
|
|
{
|
|
if (DIMS == 2) {
|
|
const POINT pEAxis[2] = {
|
|
m_rot.row(0)*m_ext[0],
|
|
m_rot.row(1)*m_ext[1]
|
|
};
|
|
const POINT pos(m_rot.transpose()*m_pos);
|
|
pts[0] = pos - pEAxis[0] - pEAxis[1];
|
|
pts[1] = pos + pEAxis[0] - pEAxis[1];
|
|
pts[2] = pos + pEAxis[0] + pEAxis[1];
|
|
pts[3] = pos - pEAxis[0] + pEAxis[1];
|
|
}
|
|
if (DIMS == 3) {
|
|
const POINT pEAxis[3] = {
|
|
m_rot.row(0)*m_ext[0],
|
|
m_rot.row(1)*m_ext[1],
|
|
m_rot.row(2)*m_ext[2]
|
|
};
|
|
const POINT pos(m_rot.transpose()*m_pos);
|
|
pts[0] = pos - pEAxis[0] - pEAxis[1] - pEAxis[2];
|
|
pts[1] = pos - pEAxis[0] - pEAxis[1] + pEAxis[2];
|
|
pts[2] = pos + pEAxis[0] - pEAxis[1] - pEAxis[2];
|
|
pts[3] = pos + pEAxis[0] - pEAxis[1] + pEAxis[2];
|
|
pts[4] = pos + pEAxis[0] + pEAxis[1] - pEAxis[2];
|
|
pts[5] = pos + pEAxis[0] + pEAxis[1] + pEAxis[2];
|
|
pts[6] = pos - pEAxis[0] + pEAxis[1] - pEAxis[2];
|
|
pts[7] = pos - pEAxis[0] + pEAxis[1] + pEAxis[2];
|
|
}
|
|
} // GetCorners
|
|
// constructs the corner of the aligned bounding box in world space
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TOBB<TYPE,DIMS>::AABB TOBB<TYPE,DIMS>::GetAABB() const
|
|
{
|
|
POINT pts[numCorners];
|
|
GetCorners(pts);
|
|
return AABB(pts, numCorners);
|
|
} // GetAABB
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// computes the volume of the OBB, which is a measure of
|
|
// how tight the fit is (better OBBs will have smaller volumes)
|
|
template <typename TYPE, int DIMS>
|
|
inline TYPE TOBB<TYPE,DIMS>::GetVolume() const
|
|
{
|
|
return m_ext.prod()*numCorners;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
bool TOBB<TYPE,DIMS>::Intersects(const POINT& pt) const
|
|
{
|
|
const POINT dist(m_rot * (pt - m_pos));
|
|
if (DIMS == 2) {
|
|
return ABS(dist[0]) <= m_ext[0]
|
|
&& ABS(dist[1]) <= m_ext[1];
|
|
}
|
|
if (DIMS == 3) {
|
|
return ABS(dist[0]) <= m_ext[0]
|
|
&& ABS(dist[1]) <= m_ext[1]
|
|
&& ABS(dist[2]) <= m_ext[2];
|
|
}
|
|
} // Intersects(POINT)
|
|
/*----------------------------------------------------------------*/
|