//------------------------------------------------------------------------------- // @ ::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); } } }