952 lines
27 KiB
C++
952 lines
27 KiB
C++
////////////////////////////////////////////////////////////////////
|
|
// Ray.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 TTriangle<TYPE,DIMS>::TTriangle(const POINT& p0, const POINT& p1, const POINT& p2)
|
|
:
|
|
a(p0), b(p1), c(p2)
|
|
{
|
|
} // constructors
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// set attributes
|
|
template <typename TYPE, int DIMS>
|
|
inline void TTriangle<TYPE,DIMS>::Set(const POINT& p0, const POINT& p1, const POINT& p2)
|
|
{
|
|
a = p0;
|
|
b = p1;
|
|
c = p2;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// get center
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TTriangle<TYPE,DIMS>::POINT TTriangle<TYPE,DIMS>::GetCenter() const
|
|
{
|
|
return (a + b + c) / TYPE(3);
|
|
}
|
|
// get AABB
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TTriangle<TYPE,DIMS>::AABB TTriangle<TYPE,DIMS>::GetAABB() const
|
|
{
|
|
return AABB(& operator [] (0), 3);
|
|
}
|
|
// get plane
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TTriangle<TYPE,DIMS>::PLANE TTriangle<TYPE,DIMS>::GetPlane() const
|
|
{
|
|
return PLANE(a, b, c);
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
template <typename TYPE, int DIMS>
|
|
inline TRay<TYPE,DIMS>::TRay(const POINT& pOrig, const VECTOR& vDir)
|
|
:
|
|
m_vDir(vDir), m_pOrig(pOrig)
|
|
{
|
|
ASSERT(ISEQUAL(m_vDir.squaredNorm(), TYPE(1)));
|
|
} // constructors
|
|
template <typename TYPE, int DIMS>
|
|
inline TRay<TYPE,DIMS>::TRay(const POINT& pt0, const POINT& pt1, bool /*bPoints*/)
|
|
:
|
|
m_vDir((pt1-pt0).normalized()), m_pOrig(pt0)
|
|
{
|
|
} // constructors
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// set attributes
|
|
template <typename TYPE, int DIMS>
|
|
inline void TRay<TYPE,DIMS>::Set(const POINT& pOrig, const VECTOR& vDir)
|
|
{
|
|
m_vDir = vDir;
|
|
m_pOrig = pOrig;
|
|
ASSERT(ISEQUAL(m_vDir.squaredNorm(), TYPE(1)));
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline void TRay<TYPE,DIMS>::SetFromPoints(const POINT& pt0, const POINT& pt1)
|
|
{
|
|
m_vDir = (pt1-pt0).normalized();
|
|
m_pOrig = pt0;
|
|
}
|
|
template <typename TYPE, int DIMS>
|
|
inline TYPE TRay<TYPE,DIMS>::SetFromPointsLen(const POINT& pt0, const POINT& pt1)
|
|
{
|
|
m_vDir = pt1-pt0;
|
|
const TYPE len = m_vDir.norm();
|
|
if (len > TYPE(0))
|
|
m_vDir *= TYPE(1)/len;
|
|
m_pOrig = pt0;
|
|
return len;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// transform ray into matrix space
|
|
template <typename TYPE, int DIMS>
|
|
inline void TRay<TYPE,DIMS>::DeTransform(const MATRIX& _m)
|
|
{
|
|
STATIC_ASSERT(DIMS == 3);
|
|
MATRIX m(_m);
|
|
|
|
// invert translation
|
|
typedef Eigen::Matrix<TYPE,4,1> VECTOR4;
|
|
const VECTOR4 vcOrig(m_pOrig[0]-m(3,0), m_pOrig[1]-m(3,1), m_pOrig[2]-m(3,2));
|
|
|
|
// delete it from matrix
|
|
m(3,0) = m(3,1) = m(3,2) = TYPE(0);
|
|
|
|
// invert matrix and apply to ray
|
|
const MATRIX mInv = m.inverse();
|
|
m_pOrig = vcOrig * mInv;
|
|
m_vDir = VECTOR4(m_vDir) * mInv;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Transform the ray by a matrix.
|
|
template <typename TYPE, int DIMS>
|
|
TRay<TYPE,DIMS> TRay<TYPE,DIMS>::operator*(const MATRIX& m) const
|
|
{
|
|
TRay ray;
|
|
ray.SetFromPoints(m_pOrig*m, (m_pOrig+m_vDir)*m);
|
|
return ray;
|
|
} // operator *
|
|
template <typename TYPE, int DIMS>
|
|
inline TRay<TYPE,DIMS>& TRay<TYPE,DIMS>::operator*=(const MATRIX& m)
|
|
{
|
|
*this = operator * (m);
|
|
return *this;
|
|
} // operator *=
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// test for intersection with triangle
|
|
template <typename TYPE, int DIMS>
|
|
template <bool bCull>
|
|
bool TRay<TYPE,DIMS>::Intersects(const TRIANGLE& tri, TYPE *pt) const
|
|
{
|
|
const VECTOR edge1(tri.b - tri.a);
|
|
const VECTOR edge2(tri.c - tri.a);
|
|
const VECTOR pvec(m_vDir.cross(edge2));
|
|
const TYPE det(edge1.dot(pvec));
|
|
// check if ray and triangle plane are parallel
|
|
if ((bCull ? det : ABS(det)) < ZEROTOLERANCE<TYPE>() * TYPE(0.01))
|
|
return false;
|
|
const TYPE invDet(TYPE(1) / det);
|
|
const VECTOR tvec(m_pOrig - tri.a);
|
|
const TYPE u(tvec.dot(pvec) * invDet);
|
|
// check if intersection is outside of the line segment defined by points a and c
|
|
if (u < -ZEROTOLERANCE<TYPE>() * TYPE(10))
|
|
return false;
|
|
const VECTOR qvec(tvec.cross(edge1));
|
|
const TYPE v(m_vDir.dot(qvec) * invDet);
|
|
// check if intersection is outside of the line segment defined by points a and b
|
|
if (v < -ZEROTOLERANCE<TYPE>() * TYPE(10))
|
|
return false;
|
|
// check if intersection is outside of the line segment defined by points b and c
|
|
if (u + v > TYPE(1) + ZEROTOLERANCE<TYPE>() * TYPE(10))
|
|
return false;
|
|
const TYPE t(edge2.dot(qvec) * invDet);
|
|
// check if intersection is behind the ray origin
|
|
if (bCull && t < -ZEROTOLERANCE<TYPE>())
|
|
return false;
|
|
if (pt)
|
|
*pt = t;
|
|
return true;
|
|
} // Intersects(Tri)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// test for intersection with triangle at certain length (line segment);
|
|
// same as above, but test distance to intersection vs segment length
|
|
template <typename TYPE, int DIMS>
|
|
template <bool bCull>
|
|
bool TRay<TYPE,DIMS>::Intersects(const TRIANGLE& tri, TYPE fL, TYPE *t) const
|
|
{
|
|
TYPE _t;
|
|
TYPE* const pt(t ? t : &_t);
|
|
Intersects(tri, pt);
|
|
// collision but not on segment?
|
|
return *pt <= fL;
|
|
} // Intersects(Tri at length)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// test if the ray intersects the given sphere
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const SPHERE& sphere) const
|
|
{
|
|
TYPE dSq;
|
|
if (!DistanceSq(sphere.center, dSq))
|
|
return false;
|
|
return dSq <= SQUARE(sphere.radius);
|
|
}
|
|
// same as above, but returns also the distance on the ray corresponding to the closest point
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const SPHERE& sphere, TYPE& t) const
|
|
{
|
|
const VECTOR a(sphere.center - m_pOrig);
|
|
t = a.dot(m_vDir);
|
|
// point behind the ray origin
|
|
if (t < TYPE(0))
|
|
return false;
|
|
const TYPE dSq((a - m_vDir*t).squaredNorm());
|
|
return dSq <= SQUARE(sphere.radius);
|
|
} // Intersects(Sphere)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const AABB &aabb) const
|
|
{
|
|
bool to_infinity = true;
|
|
TYPE _min, _max;
|
|
// first on x value
|
|
if (m_vDir[0] == TYPE(0)) {
|
|
if (m_pOrig[0] < aabb.ptMin[0] || m_pOrig[0] > aabb.ptMax[0])
|
|
return false; // NO_INTERSECTION
|
|
} else {
|
|
if (m_vDir[0] > TYPE(0)) {
|
|
_min = (aabb.ptMin[0]-m_pOrig[0])/m_vDir[0];
|
|
_max = (aabb.ptMax[0]-m_pOrig[0])/m_vDir[0];
|
|
} else {
|
|
_min = (aabb.ptMax[0]-m_pOrig[0])/m_vDir[0];
|
|
_max = (aabb.ptMin[0]-m_pOrig[0])/m_vDir[0];
|
|
}
|
|
to_infinity = false;
|
|
}
|
|
// now on y value
|
|
if (m_vDir[1] == TYPE(0)) {
|
|
if (m_pOrig[1] < aabb.ptMin[1] || m_pOrig[1] > aabb.ptMax[1])
|
|
return false; // NO_INTERSECTION
|
|
} else {
|
|
TYPE newmin, newmax;
|
|
if (m_vDir[1] > TYPE(0)) {
|
|
newmin = (aabb.ptMin[1]-m_pOrig[1])/m_vDir[1];
|
|
newmax = (aabb.ptMax[1]-m_pOrig[1])/m_vDir[1];
|
|
} else {
|
|
newmin = (aabb.ptMax[1]-m_pOrig[1])/m_vDir[1];
|
|
newmax = (aabb.ptMin[1]-m_pOrig[1])/m_vDir[1];
|
|
}
|
|
#if 0
|
|
if (to_infinity) {
|
|
_min = newmin;
|
|
_max = newmax;
|
|
} else {
|
|
#else
|
|
if (to_infinity)
|
|
return true;
|
|
{
|
|
#endif
|
|
if (newmin > _min)
|
|
_min = newmin;
|
|
if (newmax < _max)
|
|
_max = newmax;
|
|
if (_max < _min)
|
|
return false; // NO_INTERSECTION
|
|
}
|
|
to_infinity = false;
|
|
}
|
|
// now on z value
|
|
if (DIMS == 3) {
|
|
if (m_vDir[2] == TYPE(0)) {
|
|
if (m_pOrig[2] < aabb.ptMin[2] || m_pOrig[2] > aabb.ptMax[2])
|
|
return false; // NO_INTERSECTION
|
|
} else {
|
|
TYPE newmin, newmax;
|
|
if (m_vDir[2] > TYPE(0)) {
|
|
newmin = (aabb.ptMin[2]-m_pOrig[2])/m_vDir[2];
|
|
newmax = (aabb.ptMax[2]-m_pOrig[2])/m_vDir[2];
|
|
} else {
|
|
newmin = (aabb.ptMax[2]-m_pOrig[2])/m_vDir[2];
|
|
newmax = (aabb.ptMin[2]-m_pOrig[2])/m_vDir[2];
|
|
}
|
|
#if 0
|
|
if (to_infinity) {
|
|
_min = newmin;
|
|
_max = newmax;
|
|
} else {
|
|
#else
|
|
if (to_infinity)
|
|
return true;
|
|
{
|
|
#endif
|
|
if (newmin > _min)
|
|
_min = newmin;
|
|
if (newmax < _max)
|
|
_max = newmax;
|
|
if (_max < _min)
|
|
return false; // NO_INTERSECTION
|
|
}
|
|
to_infinity = false;
|
|
}
|
|
}
|
|
ASSERT(!to_infinity);
|
|
#if 0
|
|
if (_max < _min)
|
|
return true; // POINT_INTERSECTION
|
|
#endif
|
|
return true; // SEGMENT_INTERSECTION
|
|
} // Intersects(AABB)
|
|
|
|
// test for intersection with aabb, original code by Andrew Woo,
|
|
// from "Geometric Tools...", Morgan Kaufmann Publ., 2002
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const AABB &aabb, TYPE& t) const
|
|
{
|
|
TYPE t0, t1, tmp;
|
|
TYPE tNear(-999999);
|
|
TYPE tFar ( 999999);
|
|
|
|
// first pair of planes
|
|
if (ISZERO(m_vDir[0])) {
|
|
if ((m_pOrig[0] < aabb.ptMin[0]) ||
|
|
(m_pOrig[0] > aabb.ptMax[0]))
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[0] - m_pOrig[0]) / m_vDir[0];
|
|
t1 = (aabb.ptMax[0] - m_pOrig[0]) / m_vDir[0];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < TYPE(0)) return false;
|
|
|
|
// second pair of planes
|
|
if (ISZERO(m_vDir[1])) {
|
|
if ((m_pOrig[1] < aabb.ptMin[1]) ||
|
|
(m_pOrig[1] > aabb.ptMax[1]) )
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[1] - m_pOrig[1]) / m_vDir[1];
|
|
t1 = (aabb.ptMax[1] - m_pOrig[1]) / m_vDir[1];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < TYPE(0)) return false;
|
|
|
|
if (DIMS == 3) {
|
|
// third pair of planes
|
|
if (ISZERO(m_vDir[2])) {
|
|
if ((m_pOrig[2] < aabb.ptMin[2]) ||
|
|
(m_pOrig[2] > aabb.ptMax[2]) )
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[2] - m_pOrig[2]) / m_vDir[2];
|
|
t1 = (aabb.ptMax[2] - m_pOrig[2]) / m_vDir[2];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < TYPE(0)) return false;
|
|
}
|
|
|
|
t = (tNear > TYPE(0) ? tNear : tFar);
|
|
return true;
|
|
} // Intersects(AABB)
|
|
|
|
// test for intersection with aabb, original code by Andrew Woo,
|
|
// from "Geometric Tools...", Morgan Kaufmann Publ., 2002
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const AABB &aabb, TYPE fL, TYPE *t) const
|
|
{
|
|
TYPE t0, t1, tmp, tFinal;
|
|
TYPE tNear(-999999);
|
|
TYPE tFar ( 999999);
|
|
|
|
// first pair of planes
|
|
if (ISZERO(m_vDir[0])) {
|
|
if ((m_pOrig[0] < aabb.ptMin[0]) ||
|
|
(m_pOrig[0] > aabb.ptMax[0]) )
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[0] - m_pOrig[0]) / m_vDir[0];
|
|
t1 = (aabb.ptMax[0] - m_pOrig[0]) / m_vDir[0];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < TYPE(0)) return false;
|
|
|
|
// second pair of planes
|
|
if (ISZERO(m_vDir[1])) {
|
|
if ((m_pOrig[1] < aabb.ptMin[1]) ||
|
|
(m_pOrig[1] > aabb.ptMax[1]) )
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[1] - m_pOrig[1]) / m_vDir[1];
|
|
t1 = (aabb.ptMax[1] - m_pOrig[1]) / m_vDir[1];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < TYPE(0)) return false;
|
|
|
|
if (DIMS == 3) {
|
|
// third pair of planes
|
|
if (ISZERO(m_vDir[2])) {
|
|
if ((m_pOrig[2] < aabb.ptMin[2]) ||
|
|
(m_pOrig[2] > aabb.ptMax[2]) )
|
|
return false;
|
|
}
|
|
t0 = (aabb.ptMin[2] - m_pOrig[2]) / m_vDir[2];
|
|
t1 = (aabb.ptMax[2] - m_pOrig[2]) / m_vDir[2];
|
|
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
|
|
if (t0 > tNear) tNear = t0;
|
|
if (t1 < tFar) tFar = t1;
|
|
if (tNear > tFar) return false;
|
|
if (tFar < 0) return false;
|
|
}
|
|
|
|
tFinal = (tNear > TYPE(0) ? tNear : tFar);
|
|
|
|
if (tFinal > fL) return false;
|
|
if (t) *t = tFinal;
|
|
return true;
|
|
} // Intersects(AABB) at length
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const OBB& obb) const
|
|
{
|
|
TYPE t;
|
|
return Intersects(obb, t);
|
|
} // Intersects(OBB)
|
|
|
|
// test for intersection with obb, slaps method
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const OBB& obb, TYPE& t) const
|
|
{
|
|
TYPE e, f, t1, t2, temp;
|
|
TYPE tmin(-999999);
|
|
TYPE tmax( 999999);
|
|
|
|
const POINT vP = obb.m_pos - m_pOrig;
|
|
|
|
// 1st slap
|
|
e = obb.m_rot.row(0) * vP;
|
|
f = obb.m_rot.row(0) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[0]) / f;
|
|
t2 = (e - obb.m_ext[0]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[0]) > 0.0f) || ((-e + obb.m_ext[0]) < 0.0f))
|
|
return false;
|
|
|
|
// 2nd slap
|
|
e = obb.m_rot.row(1) * vP;
|
|
f = obb.m_rot.row(1) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[1]) / f;
|
|
t2 = (e - obb.m_ext[1]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[1]) > 0.0f) || ((-e + obb.m_ext[1]) < 0.0f))
|
|
return false;
|
|
|
|
// 3rd slap
|
|
e = obb.m_rot.row(2) * vP;
|
|
f = obb.m_rot.row(2) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[2]) / f;
|
|
t2 = (e - obb.m_ext[2]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[2]) > 0) || ((-e + obb.m_ext[2]) < 0))
|
|
return false;
|
|
|
|
if (tmin > 0) {
|
|
if (t) *t = tmin;
|
|
return true;
|
|
}
|
|
|
|
t = tmax;
|
|
return true;
|
|
} // Intersects(AABB)
|
|
|
|
// test for intersection with obb at certain length (line segment),
|
|
// slaps method but compare result if true to length prior return.
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const OBB& obb, TYPE fL, TYPE *t) const
|
|
{
|
|
TYPE e, f, t1, t2, temp;
|
|
TYPE tmin(-999999);
|
|
TYPE tmax( 999999);
|
|
|
|
const POINT vP = obb.m_pos - m_pOrig;
|
|
|
|
// 1st slap
|
|
e = obb.m_rot.row(0) * vP;
|
|
f = obb.m_rot.row(0) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[0]) / f;
|
|
t2 = (e - obb.m_ext[0]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[0]) > 0) || ((-e + obb.m_ext[0]) < 0))
|
|
return false;
|
|
|
|
// 2nd slap
|
|
e = obb.m_rot.row(1) * vP;
|
|
f = obb.m_rot.row(1) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[1]) / f;
|
|
t2 = (e - obb.m_ext[1]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[1]) > 0) || ((-e + obb.m_ext[1]) < 0))
|
|
return false;
|
|
|
|
// 3rd slap
|
|
e = obb.m_rot.row(2) * vP;
|
|
f = obb.m_rot.row(2) * m_vDir;
|
|
if (!ISZERO(f)) {
|
|
t1 = (e + obb.m_ext[2]) / f;
|
|
t2 = (e - obb.m_ext[2]) / f;
|
|
|
|
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
|
|
if (t1 > tmin) tmin = t1;
|
|
if (t2 < tmax) tmax = t2;
|
|
if (tmin > tmax) return false;
|
|
if (tmax < 0.0f) return false;
|
|
} else
|
|
if (((-e - obb.m_ext[2]) > 0) || ((-e + obb.m_ext[2]) < 0))
|
|
return false;
|
|
|
|
if ((tmin > 0) && (tmin <= fL)) {
|
|
if (t) *t = tmin;
|
|
return true;
|
|
}
|
|
|
|
// intersection on line but not on segment
|
|
if (tmax > fL) return false;
|
|
|
|
if (t) *t = tmax;
|
|
|
|
return true;
|
|
} // Intersects(AABB) at length
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Intersection with PLANE from origin till infinity.
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const PLANE& plane, bool bCull, TYPE *t, POINT* pPtHit) const
|
|
{
|
|
const TYPE Vd(plane.m_vN.dot(m_vDir));
|
|
|
|
// ray parallel to plane
|
|
if (ISZERO(Vd))
|
|
return false;
|
|
|
|
// normal pointing away from ray dir
|
|
// => intersection backface if any
|
|
if (bCull && (Vd > TYPE(0)))
|
|
return false;
|
|
|
|
const TYPE Vo(-plane.Distance(m_pOrig));
|
|
|
|
const TYPE _t(Vo / Vd);
|
|
|
|
// intersection behind ray origin
|
|
if (_t < TYPE(0))
|
|
return false;
|
|
|
|
if (pPtHit)
|
|
(*pPtHit) = m_pOrig + (m_vDir * _t);
|
|
|
|
if (t)
|
|
(*t) = _t;
|
|
|
|
return true;
|
|
} // Intersects(PLANE)
|
|
// same as above, but no checks
|
|
template <typename TYPE, int DIMS>
|
|
inline TYPE TRay<TYPE,DIMS>::IntersectsDist(const PLANE& plane) const
|
|
{
|
|
const TYPE Vd(plane.m_vN.dot(m_vDir));
|
|
const TYPE Vo(-plane.Distance(m_pOrig));
|
|
return SAFEDIVIDE(Vo, Vd);
|
|
} // IntersectsDist(PLANE)
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TRay<TYPE,DIMS>::POINT TRay<TYPE,DIMS>::Intersects(const PLANE& plane) const
|
|
{
|
|
return m_pOrig + (m_vDir * IntersectsDist(plane));
|
|
} // Intersects(PLANE)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Intersection with PLANE at distance fL.
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const PLANE& plane, bool bCull, TYPE fL, TYPE *t, POINT* pPtHit) const
|
|
{
|
|
const TYPE Vd = plane.m_vN.dot(m_vDir);
|
|
|
|
// ray parallel to plane
|
|
if (ISZERO(Vd))
|
|
return false;
|
|
|
|
// normal pointing away from ray dir
|
|
// => intersection backface if any
|
|
if (bCull && (Vd > TYPE(0)))
|
|
return false;
|
|
|
|
const TYPE Vo(-plane.Distance(m_pOrig));
|
|
|
|
const TYPE _t(Vo / Vd);
|
|
|
|
// intersection behind ray origin or beyond valid range
|
|
if ((_t < TYPE(0)) || (_t > fL))
|
|
return false;
|
|
|
|
if (pPtHit)
|
|
(*pPtHit) = m_pOrig + (m_vDir * _t);
|
|
|
|
if (t)
|
|
(*t) = _t;
|
|
|
|
return true;
|
|
} // Intersects(PLANE)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Intersection with another ray.
|
|
// Rays assumed to be coplanar; returns only the distance from the origin of the first ray.
|
|
// P = R.origin + s * R.dir
|
|
// http://mathworld.wolfram.com/Line-LineIntersection.html
|
|
// (works also for 2D lines if z components is set to 0)
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const TRay& ray, TYPE& s) const
|
|
{
|
|
const POINT dir(ray.m_pOrig - m_pOrig);
|
|
const POINT n(m_vDir.cross(ray.m_vDir));
|
|
if (!ISZERO(dir.dot(n)))
|
|
return false; // lines are not coplanar
|
|
s = dir.cross(ray.m_vDir).dot(n) / n.squaredNorm();
|
|
return true;
|
|
} // Intersects(Ray)
|
|
// Same as above, but returns the actual intersection point:
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const TRay& ray, POINT& p) const
|
|
{
|
|
TYPE s;
|
|
if (!Intersects(ray, s))
|
|
return false; // lines are not coplanar
|
|
if (s < TYPE(0))
|
|
return false; // intersection behind ray origin
|
|
p = m_pOrig + m_vDir * s;
|
|
return true;
|
|
} // Intersects(Ray)
|
|
// No coplanarity assumption; returns the shortest line segment connecting the two rays:
|
|
// P1 = R1.origin + s1 * R1.dir
|
|
// P2 = R2.origin + s2 * R2.dir
|
|
// http://paulbourke.net/geometry/pointlineplane/
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Intersects(const TRay& ray, TYPE& s1, TYPE& s2) const
|
|
{
|
|
const TYPE d4321(ray.m_vDir.dot(m_vDir));
|
|
const TYPE d4343(ray.m_vDir.dot(ray.m_vDir));
|
|
const TYPE d2121(m_vDir.dot(m_vDir));
|
|
const TYPE denom = d2121 * d4343 - d4321 * d4321;
|
|
if (ISZERO(denom))
|
|
return false; // lines are parallel
|
|
const POINT dir(m_pOrig - ray.m_pOrig);
|
|
const TYPE d1321(dir.dot(m_vDir));
|
|
const TYPE d1343(dir.dot(ray.m_vDir));
|
|
const TYPE numer = d1343 * d4321 - d1321 * d4343;
|
|
s1 = numer / denom;
|
|
s2 = (d1343 + d4321 * s1) / d4343;
|
|
return true;
|
|
} // Intersects(Ray)
|
|
// Same as above, but returns only middle point of the shortest line segment:
|
|
// P = (P1 + P2) / 2
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::IntersectsAprox(const TRay& ray, POINT& p) const
|
|
{
|
|
TYPE s1, s2;
|
|
if (!Intersects(ray, s1, s2))
|
|
return false;
|
|
const POINT P1 = m_pOrig + m_vDir * s1;
|
|
const POINT P2 = ray.m_pOrig + ray.m_vDir * s2;
|
|
p = (P1+P2)*TYPE(0.5);
|
|
return true;
|
|
} // Intersects(Ray)
|
|
// Same as above, but returns the point on the given ray.
|
|
// Returns false if intersection not in front of main ray.
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::IntersectsAprox2(const TRay& ray, POINT& p) const
|
|
{
|
|
TYPE s1, s2;
|
|
if (!Intersects(ray, s1, s2))
|
|
return false;
|
|
if (s1 < TYPE(0))
|
|
return false; // intersection behind main ray origin
|
|
p = ray.m_pOrig + ray.m_vDir * s2;
|
|
return true;
|
|
} // Intersects(Ray)
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// computes the angle between the two lines
|
|
template <typename TYPE, int DIMS>
|
|
inline TYPE TRay<TYPE,DIMS>::CosAngle(const TRay& ray) const
|
|
{
|
|
ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1)));
|
|
ASSERT(ISEQUAL(ray.m_vDir.norm(), TYPE(1)));
|
|
return m_vDir.dot(ray.m_vDir);
|
|
}
|
|
// tests if the two lines are coplanar (intersect)
|
|
template <typename TYPE, int DIMS>
|
|
inline bool TRay<TYPE,DIMS>::Coplanar(const TRay& ray) const
|
|
{
|
|
const POINT dir(ray.m_pOrig - m_pOrig);
|
|
const POINT n(m_vDir.cross(ray.m_vDir));
|
|
return ISZERO(dir.dot(n));
|
|
}
|
|
// tests if the two lines are parallel
|
|
template <typename TYPE, int DIMS>
|
|
inline bool TRay<TYPE,DIMS>::Parallel(const TRay& ray) const
|
|
{
|
|
return ISZERO(m_vDir.cross(ray.m_vDir).norm());
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// Computes the position on the ray of the point projection.
|
|
// Returns 0 if it coincides with the ray origin, positive value if in front, and negative if behind.
|
|
template <typename TYPE, int DIMS>
|
|
inline TYPE TRay<TYPE,DIMS>::Classify(const POINT& pt) const
|
|
{
|
|
ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1)));
|
|
const VECTOR a(pt - m_pOrig);
|
|
return a.dot(m_vDir);
|
|
} // Classify(POINT)
|
|
template <typename TYPE, int DIMS>
|
|
inline typename TRay<TYPE,DIMS>::POINT TRay<TYPE,DIMS>::ProjectPoint(const POINT& pt) const
|
|
{
|
|
ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1)));
|
|
return m_pOrig + m_vDir*Classify(pt);
|
|
} // ProjectPoint
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// Computes the distance between the ray and a point.
|
|
// Returns false if the point is projecting behind the ray origin.
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::DistanceSq(const POINT& pt, TYPE& d) const
|
|
{
|
|
const VECTOR a(pt - m_pOrig);
|
|
const TYPE LenACos(a.dot(m_vDir));
|
|
// point behind the ray origin
|
|
if (LenACos < TYPE(0))
|
|
return false;
|
|
d = (a - m_vDir*LenACos).squaredNorm();
|
|
return true;
|
|
} // DistanceSq(POINT)
|
|
template <typename TYPE, int DIMS>
|
|
bool TRay<TYPE,DIMS>::Distance(const POINT& pt, TYPE& d) const
|
|
{
|
|
const VECTOR a(pt - m_pOrig);
|
|
const TYPE LenACos(a.dot(m_vDir));
|
|
// point behind the ray origin
|
|
if (LenACos < TYPE(0))
|
|
return false;
|
|
d = (a - m_vDir*LenACos).norm();
|
|
return true;
|
|
} // Distance(POINT)
|
|
// Same as above, but returns the distance even if the point projection is behind the origin.
|
|
template <typename TYPE, int DIMS>
|
|
TYPE TRay<TYPE,DIMS>::DistanceSq(const POINT& pt) const
|
|
{
|
|
ASSERT(ISEQUAL(m_vDir.norm(), TYPE(1)));
|
|
const VECTOR a(pt - m_pOrig);
|
|
const VECTOR b(a - m_vDir);
|
|
return b.cross(a).squaredNorm();
|
|
} // DistanceSq(POINT)
|
|
template <typename TYPE, int DIMS>
|
|
TYPE TRay<TYPE,DIMS>::Distance(const POINT& pt) const
|
|
{
|
|
return SQRT(DistanceSq(pt));
|
|
} // Distance(POINT)
|
|
// Get the point on the ray at distance t from origin
|
|
template <typename TYPE, int DIMS>
|
|
typename TRay<TYPE, DIMS>::POINT TRay<TYPE,DIMS>::GetPoint(TYPE t) const
|
|
{
|
|
return m_pOrig + m_vDir * t;
|
|
} // GetPoint
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
template <typename TYPE, int DIMS>
|
|
bool TCylinder<TYPE,DIMS>::Intersects(const SPHERE& sphere) const
|
|
{
|
|
struct Check {
|
|
static bool Intersection(const RAY& ray, const SPHERE& sphere, const VECTOR& CmV, TYPE t, TYPE radius, TYPE height) {
|
|
const POINT O(ray.m_pOrig + ray.m_vDir * height);
|
|
const VECTOR a(sphere.center - O);
|
|
if (a.squaredNorm() <= SQUARE(sphere.radius))
|
|
return true; // ray origin inside the sphere
|
|
const POINT D((CmV - ray.m_vDir * t).normalized());
|
|
const TYPE d = a.dot(D);
|
|
if (d < TYPE(0))
|
|
return false; // intersection behind the ray origin
|
|
const TYPE dSq((a - D*d).squaredNorm());
|
|
const TYPE srSq = SQUARE(sphere.radius);
|
|
if (dSq <= srSq) {
|
|
const TYPE r = d - SQRT(srSq-dSq);
|
|
ASSERT(r >= TYPE(0));
|
|
return r <= radius; // intersection before the ray end
|
|
}
|
|
return false;
|
|
}
|
|
};
|
|
const VECTOR CmV(sphere.center - ray.m_pOrig);
|
|
const TYPE t(ray.m_vDir.dot(CmV));
|
|
// sphere projects behind the cylinder origin
|
|
if (t < minHeight) {
|
|
if (t+sphere.radius < minHeight)
|
|
return false;
|
|
return Check::Intersection(ray, sphere, CmV, t, radius, minHeight);
|
|
}
|
|
// sphere projects after the cylinder end
|
|
if (t > maxHeight) {
|
|
if (t-sphere.radius > maxHeight)
|
|
return false;
|
|
return Check::Intersection(ray, sphere, CmV, t, radius, maxHeight);
|
|
}
|
|
const TYPE lenSq((CmV - ray.m_vDir * t).squaredNorm());
|
|
return lenSq <= SQUARE(sphere.radius + radius);
|
|
} // Intersects
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// Classify point to cylinder.
|
|
template <typename TYPE, int DIMS>
|
|
GCLASS TCylinder<TYPE,DIMS>::Classify(const POINT& p, TYPE& t) const
|
|
{
|
|
ASSERT(ISEQUAL(ray.m_vDir.norm(), TYPE(1)));
|
|
const VECTOR D(p - ray.m_pOrig);
|
|
t = ray.m_vDir.dot(D);
|
|
if (t < minHeight) return BACK;
|
|
if (t > maxHeight) return FRONT;
|
|
const TYPE rSq((D - ray.m_vDir*t).squaredNorm());
|
|
const TYPE radiusSq(SQUARE(radius));
|
|
if (rSq > radiusSq) return CULLED;
|
|
if (rSq < radiusSq) return VISIBLE;
|
|
return PLANAR;
|
|
} // Classify
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
template <typename TYPE, int DIMS>
|
|
bool TConeIntersect<TYPE,DIMS>::operator()(const SPHERE& sphere) const
|
|
{
|
|
const VECTOR CmV(sphere.center - cone.ray.m_pOrig);
|
|
const VECTOR D(CmV + cone.ray.m_vDir * (sphere.radius*invSinAngle));
|
|
TYPE e = D.dot(cone.ray.m_vDir);
|
|
if (e <= TYPE(0) || e*e < D.squaredNorm()*cosAngleSq)
|
|
return false;
|
|
e = CmV.dot(cone.ray.m_vDir);
|
|
if (e-sphere.radius > cone.maxHeight)
|
|
return false;
|
|
if (e < cone.minHeight) {
|
|
const TYPE lenSq = CmV.squaredNorm();
|
|
if (e*e >= lenSq*sinAngleSq)
|
|
return lenSq <= SQUARE(sphere.radius);
|
|
}
|
|
return true;
|
|
} // Intersect
|
|
/*----------------------------------------------------------------*/
|
|
|
|
// Classify point to cone.
|
|
template <typename TYPE, int DIMS>
|
|
GCLASS TConeIntersect<TYPE,DIMS>::Classify(const POINT& p, TYPE& t) const
|
|
{
|
|
ASSERT(ISEQUAL(cone.ray.m_vDir.norm(), TYPE(1)));
|
|
const VECTOR D(p - cone.ray.m_pOrig);
|
|
t = cone.ray.m_vDir.dot(D);
|
|
if (ISZERO(t))
|
|
return PLANAR;
|
|
if (t < cone.minHeight) return BACK;
|
|
if (t > cone.maxHeight) return FRONT;
|
|
ASSERT(!ISZERO(D.norm()));
|
|
const TYPE tSq(SQUARE(t));
|
|
const TYPE dSq(cosAngleSq*D.squaredNorm());
|
|
if (tSq < dSq) return CULLED;
|
|
if (tSq > dSq) return VISIBLE;
|
|
return PLANAR;
|
|
} // Classify
|
|
/*----------------------------------------------------------------*/
|
|
|
|
template <typename TYPE>
|
|
bool TestRayTriangleIntersection(unsigned iters) {
|
|
typedef SEACAVE::TTriangle<TYPE,3> Triangle;
|
|
typedef SEACAVE::TRay<TYPE,3> Ray;
|
|
typedef typename Ray::POINT Point;
|
|
typedef typename Point::Scalar Type;
|
|
constexpr Type zeroEps(std::is_same<TYPE,float>::value ? 0.01f : 0.00001f);
|
|
for (unsigned iter=0, lives=iters/100; iter<iters; ++iter) {
|
|
const Type scale(10);
|
|
const Triangle triangle(Point::Random()*scale, Point::Random()*scale, Point::Random()*scale);
|
|
Type t;
|
|
const Point center(triangle.GetCenter());
|
|
const Ray rayCenter(Point::Random()*scale, center, true);
|
|
if (!rayCenter.template Intersects<false>(triangle, &t) && !lives--)
|
|
return false;
|
|
const Point _center(rayCenter.GetPoint(t));
|
|
if ((_center-center).norm() > zeroEps && !lives--)
|
|
return false;
|
|
const BYTE o((BYTE)(RAND()%3));
|
|
const Point side(((triangle[o].template cast<double>()+triangle[(o+1)%3].template cast<double>()) / 2.0).template cast<TYPE>());
|
|
const Ray raySide(rayCenter.m_pOrig, side, true);
|
|
if (!raySide.template Intersects<false>(triangle, &t) && !lives--)
|
|
return false;
|
|
const Point _side(raySide.GetPoint(t));
|
|
if ((_side-side).norm() > zeroEps && !lives--)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|