//-------------------------------------------------------------------------------
// @ ::IsPointInTriangle()
//-------------------------------------------------------------------------------
// Returns true if point on triangle plane lies inside triangle (3D version)
// Assumes triangle is not degenerate
//-------------------------------------------------------------------------------
bool IsPointInTriangle( const IvVector3& point, const IvVector3& P0,
const IvVector3& P1, const IvVector3& P2 )
{
IvVector3 v0 = P1 - P0;
IvVector3 v1 = P2 - P1;
IvVector3 n = v0.Cross(v1);
IvVector3 wTest = v0.Cross(point - P0);
if ( wTest.Dot(n) < 0.0f )
{
return false;
}
wTest = v1.Cross(point - P1);
if ( wTest.Dot(n) < 0.0f )
{
return false;
}
IvVector3 v2 = P0 - P2;
wTest = v2.Cross(point - P2);
if ( wTest.Dot(n) < 0.0f )
{
return false;
}
return true;
}
//----------------------------------------------------------------------------
// @ ::DistanceSquared()
// ---------------------------------------------------------------------------
// Returns the distance squared between lines.
//-----------------------------------------------------------------------------
float DistanceSquared( const IvLine3& line0, const IvLine3& line1,
float& s_c, float& t_c )
{
IvVector3 w0 = line0.mOrigin - line1.mOrigin;
float a = line0.mDirection.Dot( line0.mDirection );
float b = line0.mDirection.Dot( line1.mDirection );
float c = line1.mDirection.Dot( line1.mDirection );
float d = line0.mDirection.Dot( w0 );
float e = line1.mDirection.Dot( w0 );
float denom = a*c - b*b;
if ( IvIsZero(denom) )
{
s_c = 0.0f;
t_c = e/c;
IvVector3 wc = w0 - t_c*line1.mDirection;
return wc.Dot(wc);
}
else
{
s_c = ((b*e - c*d)/denom);
t_c = ((a*e - b*d)/denom);
IvVector3 wc = w0 + s_c*line0.mDirection
- t_c*line1.mDirection;
return wc.Dot(wc);
}
} // End of ::DistanceSquared()
//----------------------------------------------------------------------------
// @ ::DistanceSquared()
// ---------------------------------------------------------------------------
// Returns the distance squared between line and point.
//-----------------------------------------------------------------------------
float DistanceSquared( const IvLine3& line, const IvVector3& point, float& t_c )
{
IvVector3 w = point - line.mOrigin;
float vsq = line.mDirection.Dot(line.mDirection);
float proj = w.Dot(line.mDirection);
t_c = proj/vsq;
return w.Dot(w) - t_c*proj;
} // End of ::DistanceSquared()
//-------------------------------------------------------------------------------
// @ SimObject::HandleCollision()
//-------------------------------------------------------------------------------
// Handle collisions between this object and another one
//-------------------------------------------------------------------------------
void
SimObject::HandleCollision( SimObject* other )
{
IvVector3 collisionNormal, collisionPoint;
float penetration;
// if the two objects are colliding
if (Colliding(other, collisionNormal, collisionPoint, penetration))
{
// push out by penetration, scaled by relative masses
float relativeMass = mMass/(mMass + other->mMass);
mTranslate -= (1.0f-relativeMass)*penetration*collisionNormal;
other->mTranslate += relativeMass*penetration*collisionNormal;
// compute relative velocity
IvVector3 r1 = collisionPoint - mTranslate;
IvVector3 r2 = collisionPoint - other->mTranslate;
IvVector3 vel1 = mVelocity + Cross( mAngularVelocity, r1 );
IvVector3 vel2 = other->mVelocity + Cross( other->mAngularVelocity, r2 );
IvVector3 relativeVelocity = vel1 - vel2;
// determine if we're heading away from each other
float vDotN = relativeVelocity.Dot( collisionNormal );
if (vDotN < 0)
return;
// compute impulse factor
float denominator = (1.0f/mMass + 1.0f/other->mMass)*(collisionNormal.Dot(collisionNormal));
// compute angular factors
IvVector3 cross1 = Cross(r1, collisionNormal);
IvVector3 cross2 = Cross(r2, collisionNormal);
cross1 = mWorldMomentsInverse*cross1;
cross2 = other->mWorldMomentsInverse*cross2;
IvVector3 sum = Cross(cross1, r1) + Cross(cross2, r2);
denominator += (sum.Dot(collisionNormal));
// compute j
float modifiedVel = vDotN/denominator;
float j1 = -(1.0f+mElasticity)*modifiedVel;
float j2 = (1.0f+mElasticity)*modifiedVel;
// update velocities
mVelocity += j1/mMass*collisionNormal;
other->mVelocity += j2/other->mMass*collisionNormal;
// update angular velocities
mAngularMomentum += Cross(r1, j1*collisionNormal);
mAngularVelocity = mWorldMomentsInverse*mAngularMomentum;
other->mAngularMomentum += Cross(r2, j2*collisionNormal);
other->mAngularVelocity = other->mWorldMomentsInverse*other->mAngularMomentum;
}
} // End of SimObject::HandleCollision()
//----------------------------------------------------------------------------
// @ IvLine3::ClosestPoint()
// ---------------------------------------------------------------------------
// Returns the closest point on line to point.
//-----------------------------------------------------------------------------
IvVector3 IvLine3::ClosestPoint(const IvVector3& point) const
{
IvVector3 w = point - mOrigin;
float vsq = mDirection.Dot(mDirection);
float proj = w.Dot(mDirection);
return mOrigin + (proj/vsq)*mDirection;
} // End of IvLine3::ClosestPoint()
//-------------------------------------------------------------------------------
// @ ::TestLineOverlap()
//-------------------------------------------------------------------------------
// Helper for TriangleIntersect()
//
// This tests whether the rearranged triangles overlap, by checking the intervals
// where their edges cross the common line between the two planes. If the
// interval for P is [i,j] and Q is [k,l], then there is intersection if the
// intervals overlap. Previous algorithms computed these intervals directly,
// this tests implictly by using two "plane tests."
//-------------------------------------------------------------------------------
inline bool
TestLineOverlap( const IvVector3& P0, const IvVector3& P1,
const IvVector3& P2, const IvVector3& Q0,
const IvVector3& Q1, const IvVector3& Q2 )
{
// get "plane normal"
IvVector3 normal = (P1-P0).Cross(Q0-P0);
// fails test, no intersection
if ( normal.Dot( Q1 - P0 ) > kEpsilon )
return false;
// get "plane normal"
normal = (P2-P0).Cross(Q2-P0);
// fails test, no intersection
if ( normal.Dot( Q0 - P0 ) > kEpsilon )
return false;
// intersection!
return true;
}
//-------------------------------------------------------------------------------
// @ ::TriangleIntersect()
//-------------------------------------------------------------------------------
// Returns true if ray intersects triangle
//-------------------------------------------------------------------------------
bool
TriangleIntersect( float& t, const IvVector3& P0, const IvVector3& P1,
const IvVector3& P2, const IvRay3& ray )
{
// test ray direction against triangle
IvVector3 e1 = P1 - P0;
IvVector3 e2 = P2 - P0;
IvVector3 p = ray.GetDirection().Cross(e2);
float a = e1.Dot(p);
// if result zero, no intersection or infinite intersections
// (ray parallel to triangle plane)
if ( IvIsZero(a) )
return false;
// compute denominator
float f = 1.0f/a;
// compute barycentric coordinates
IvVector3 s = ray.GetOrigin() - P0;
float u = f*s.Dot(p);
// ray falls outside triangle
if (u < 0.0f || u > 1.0f)
return false;
IvVector3 q = s.Cross(e1);
float v = f*ray.GetDirection().Dot(q);
// ray falls outside triangle
if (v < 0.0f || u+v > 1.0f)
return false;
// compute line parameter
t = f*e2.Dot(q);
return (t >= 0.0f);
}
//-------------------------------------------------------------------------------
// @ IvQuat::Set()
//-------------------------------------------------------------------------------
// Set quaternion based on start and end vectors
//-------------------------------------------------------------------------------
void
IvQuat::Set( const IvVector3& from, const IvVector3& to )
{
// Ensure that our vectors are unit
ASSERT( from.IsUnit() && to.IsUnit() );
// get axis of rotation
IvVector3 axis = from.Cross( to );
// get cos of angle between vectors
float costheta = from.Dot( to );
// if vectors are 180 degrees apart
if ( costheta <= -1.0f )
{
// find orthogonal vector
IvVector3 orthoVector;
orthoVector.Normalize();
w = 0.0f;
x = orthoVector.x;
y = orthoVector.y;
z = orthoVector.z;
return;
}
// use trig identities to get the values we want
float factor = IvSqrt( 2.0f*(1.0f + costheta) );
float scaleFactor = 1.0f/factor;
// set values
w = 0.5f*factor;
x = scaleFactor*axis.x;
y = scaleFactor*axis.y;
z = scaleFactor*axis.z;
} // End of IvQuat::Set()
//-------------------------------------------------------------------------------
// @ ::TriangleIntersect()
//-------------------------------------------------------------------------------
// Returns true if triangles P0P1P2 and Q0Q1Q2 intersect
// Assumes triangle is not degenerate
//
// This is not the algorithm presented in the text. Instead, it is based on a
// recent article by Guigue and Devillers in the July 2003 issue Journal of
// Graphics Tools. As it is faster than the ERIT algorithm, under ordinary
// circumstances it would have been discussed in the text, but it arrived too late.
//
// More information and the original source code can be found at
// http://www.acm.org/jgt/papers/GuigueDevillers03/
//
// A nearly identical algorithm was in the same issue of JGT, by Shen Heng and
// Tang. See http://www.acm.org/jgt/papers/ShenHengTang03/ for source code.
//
// Yes, this is complicated. Did you think testing triangles would be easy?
//-------------------------------------------------------------------------------
bool TriangleIntersect( const IvVector3& P0, const IvVector3& P1,
const IvVector3& P2, const IvVector3& Q0,
const IvVector3& Q1, const IvVector3& Q2 )
{
// test P against Q's plane
IvVector3 normalQ = (Q1-Q0).Cross(Q2-Q0);
float testP0 = normalQ.Dot( P0 - Q0 );
float testP1 = normalQ.Dot( P1 - Q0 );
float testP2 = normalQ.Dot( P2 - Q0 );
// P doesn't intersect Q's plane
if (testP0*testP1 > kEpsilon && testP0*testP2 > kEpsilon )
return false;
// test Q against P's plane
IvVector3 normalP = (P1-P0).Cross(P2-P0);
float testQ0 = normalP.Dot( Q0 - P0 );
float testQ1 = normalP.Dot( Q1 - P0 );
float testQ2 = normalP.Dot( Q2 - P0 );
// Q doesn't intersect P's plane
if (testQ0*testQ1 > kEpsilon && testQ0*testQ2 > kEpsilon )
return false;
// now we rearrange P's vertices such that the lone vertex (the one that lies
// in its own half-space of Q) is first. We also permute the other
// triangle's vertices so that P0 will "see" them in counterclockwise order
// Once reordered, we pass the vertices down to a helper function which will
// reorder Q's vertices, and then test
// P0 in Q's positive half-space
if (testP0 > kEpsilon)
{
// P1 in Q's positive half-space (so P2 is lone vertex)
if (testP1 > kEpsilon)
return AdjustQ(P2, P0, P1, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
// P2 in Q's positive half-space (so P1 is lone vertex)
else if (testP2 > kEpsilon)
return AdjustQ(P1, P2, P0, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
// P0 is lone vertex
else
return AdjustQ(P0, P1, P2, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
}
// P0 in Q's negative half-space
else if (testP0 < -kEpsilon)
{
// P1 in Q's negative half-space (so P2 is lone vertex)
if (testP1 < -kEpsilon)
return AdjustQ(P2, P0, P1, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
// P2 in Q's negative half-space (so P1 is lone vertex)
else if (testP2 < -kEpsilon)
return AdjustQ(P1, P2, P0, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
// P0 is lone vertex
else
return AdjustQ(P0, P1, P2, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
}
// P0 on Q's plane
else
{
// P1 in Q's negative half-space
if (testP1 < -kEpsilon)
{
// P2 in Q's negative half-space (P0 is lone vertex)
if (testP2 < -kEpsilon)
return AdjustQ(P0, P1, P2, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
// P2 in positive half-space or on plane (P1 is lone vertex)
else
return AdjustQ(P1, P2, P0, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
}
// P1 in Q's positive half-space
else if (testP1 > kEpsilon)
{
// P2 in Q's positive half-space (P0 is lone vertex)
if (testP2 > kEpsilon)
return AdjustQ(P0, P1, P2, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
// P2 in negative half-space or on plane (P1 is lone vertex)
else
return AdjustQ(P1, P2, P0, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
}
// P1 lies on Q's plane too
else
{
// P2 in Q's positive half-space (P2 is lone vertex)
if (testP2 > kEpsilon)
return AdjustQ(P2, P0, P1, Q0, Q1, Q2, testQ0, testQ1, testQ2, normalP);
// P2 in Q's negative half-space (P2 is lone vertex)
// note different ordering for Q vertices
else if (testP2 < -kEpsilon)
return AdjustQ(P2, P0, P1, Q0, Q2, Q1, testQ0, testQ2, testQ1, normalP);
// all three points lie on Q's plane, default to 2D test
else
return CoplanarTriangleIntersect(P0, P1, P2, Q0, Q1, Q2, normalP);
}
}
}
