DTboolean PrimitiveCollisions::ray_intersect_plane ( const Vector3 &from, const Vector3 &direction,
const Plane &p, DTfloat &t)
{
DTfloat dir_dot_n = Vector3::dot(p.normal(), direction);
if ( dir_dot_n < EPSILON && dir_dot_n > -EPSILON)
return false;
t = (p.D() - Vector3::dot(from, p.normal())) / dir_dot_n;
return true;
}
Frustum::Frustum(const Matrix4& m)
{
if (m == Matrix4::Zero)
{
return;
}
Plane p;
// Extract the LEFT plane
p.A(m.x.w + m.x.x);
p.B(m.y.w + m.y.x);
p.C(m.z.w + m.z.x);
p.D(m.t.w + m.t.x);
p.Normalize();
left = p;
// Extract the RIGHT plane
p.A(m.x.w - m.x.x);
p.B(m.y.w - m.y.x);
p.C(m.z.w - m.z.x);
p.D(m.t.w - m.t.x);
p.Normalize();
right = p;
// Extract the BOTTOM plane
p.A(m.x.w + m.x.y);
p.B(m.y.w + m.y.y);
p.C(m.z.w + m.z.y);
p.D(m.t.w + m.t.y);
p.Normalize();
bottom = p;
// Extract the TOP plane
p.A(m.x.w - m.x.y);
p.B(m.y.w - m.y.y);
p.C(m.z.w - m.z.y);
p.D(m.t.w - m.t.y);
p.Normalize();
top = p;
// Extract the NEAR plane
p.A(m.x.w + m.x.z);
p.B(m.y.w + m.y.z);
p.C(m.z.w + m.z.z);
p.D(m.t.w + m.t.z);
p.Normalize();
front = p;
// Extract the FAR plane
p.A(m.x.w - m.x.z);
p.B(m.y.w - m.y.z);
p.C(m.z.w - m.z.z);
p.D(m.t.w - m.t.z);
p.Normalize();
back = p;
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
Vec3d StructGridCutPlane::planeLineIntersection(const Plane& plane, const Vec3d& p1, const Vec3d& p2, const double s1, const double s2, double* s)
{
// From http://local.wasp.uwa.edu.au/~pbourke/geometry/planeline/
//
// P1 (x1,y1,z1) and P2 (x2,y2,z2)
//
// P = P1 + u (P2 - P1)
//
// A*x1 + B*y1 + C*z1 + D
// u = ---------------------------------
// A*(x1-x2) + B*(y1-y2) + C*(z1-z2)
CVF_ASSERT(s);
const Vec3d v = p2 - p1;
double denominator = -(plane.A()*v.x() + plane.B()*v.y() + plane.C()*v.z());
if (denominator != 0)
{
double u = (plane.A()*p1.x() + plane.B()*p1.y() + plane.C()*p1.z() + plane.D())/denominator;
if (u > 0.0 && u < 1.0)
{
*s = s1 + u*(s2 - s1);
return (p1 + u*v);
}
else
{
if (u >= 1.0)
{
*s = s2;
return p2;
}
else
{
*s = s1;
return p1;
}
}
}
else
{
*s = s1;
return p1;
}
}
// http://astronomy.swin.edu.au/~pbourke/geometry/planeline/
bool Line::IntersectsPlane(const Plane& plane, Vector3* intersection) const
{
HELIUM_MATH_FUNCTION_TIMER();
float32_t den = (plane.A() * (m_Origin.x - m_Point.x)) + (plane.B() * (m_Origin.y - m_Point.y)) + (plane.C() * (m_Origin.z - m_Point.z));
if (fabs(den) < HELIUM_VALUE_NEAR_ZERO)
{
return false;
}
else
{
if (intersection)
{
float32_t mu = ( (plane.A() * m_Origin.x) + (plane.B() * m_Origin.y) + (plane.C() * m_Origin.z) + plane.D() ) / den;
(*intersection) = m_Origin + (m_Point - m_Origin) * mu;
}
return true;
}
}