/cygdrive/d/src/svn/vrut/trunk/modules/collision_detection/collision_detectors.cpp

Go to the documentation of this file.

#include "collision_detectors.h"

#include <vector>
//#include <math.h>



using namespace CollisionDetectionNamespace;


//                                                                                                                                                                                                         //
//                                                                    CollisionDetector                                                                                                      //
//                                                                                                                                                                                                         //

CollisionReport CollisionDetector::DetectCollision(void *pNode1, void *pNode2, DetectorType type)
{
       CollisionReport collisionReport;

       Collison_Detector_IFace* pDetector = GetColisionDetector(type);

       if (pDetector)
       {
              collisionReport = pDetector->FindCollision(pNode1, pNode2);
              delete pDetector;
       }

       return collisionReport;
}



Collison_Detector_IFace* CollisionDetector::GetColisionDetector(DetectorType type)
{
       Collison_Detector_IFace* pDetector = 0;

       switch(type)
       {
       case DT_AABB:
              pDetector = new Collision_detector_AABB();
              break;
       case DT_Triangle:
              pDetector = new Collision_detector_Triangles();
              break;
       case DT_Triangle_Distance:
              pDetector = new Collision_detector_Triangles_Distance();
              break;
       default:
              ;//LOG(wxT("No collision detected for this type"));
       }

       return pDetector;
}

//                                                                                                                                                                                                         //
//                                                                    Collision_detector_AABB                                                                                                //
//                                                                                                                                                                                                         //

CollisionReport Collision_detector_AABB::FindCollision(void* pNode1, void* pNode2)
{
       CollisionReport collisionReport;
       collisionReport.bCollide = true;

       AABB* a = (AABB*) pNode1;
       AABB* b = (AABB*) pNode2;


       if ( (a->MaxBound.x < b->MinBound.x || a->MinBound.x > b->MaxBound.x)
              ||     (a->MaxBound.y < b->MinBound.y || a->MinBound.y > b->MaxBound.y)
              ||     (a->MaxBound.z < b->MinBound.z || a->MinBound.z > b->MaxBound.z))
       {
              collisionReport.bCollide = false;
       }

       return collisionReport;
}


//                                                                                                                                                                                                         //
//                                                                    Collision_detector_Triangles                                                                                    //
//                                                                                                                                                                                                         //

CollisionReport Collision_detector_Triangles::FindCollision(void* pNode1, void* pNode2)
{
       CollisionReport collisionReport;

       Triangle* a = (Triangle*) pNode1;
       Triangle* b = (Triangle*) pNode2;
       
       VECTOR3 intersectionPoint1, intersectionPoint2, intersectionPoint3,intersectionPoint4, intersectionPoint5, intersectionPoint6;
       VECTOR3 finalIntersectionPoint;

       int bV1 = (int)intersectSegmentTriangle(a->v1, a->v2, b,intersectionPoint1);
       int bV2 = (int)intersectSegmentTriangle(a->v1, a->v3, b,intersectionPoint2);
       int bV3 = (int)intersectSegmentTriangle(a->v2, a->v3, b,intersectionPoint3);

       int bV4, bV5, bV6;

       int bV = bV1 + bV2 + bV3;

       switch (bV)
       {
       case 0:
              bV4 = (int)intersectSegmentTriangle(b->v1, b->v2, a,intersectionPoint4);
              bV5 = (int)intersectSegmentTriangle(b->v1, b->v3, a,intersectionPoint5);
              bV6 = (int)intersectSegmentTriangle(b->v2, b->v3, a,intersectionPoint6);
              bV = bV4 + bV5 + bV6;
              if (bV == 2)
              {
                     collisionReport.bCollide = true;

                     if (bV4 == 1 && bV5 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint4 + (intersectionPoint5 - intersectionPoint4);
                     }
                     else if (bV4 == 1 && bV6 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint4 + (intersectionPoint6 - intersectionPoint4);
                     }
                     else
                     {
                            finalIntersectionPoint = intersectionPoint6 + (intersectionPoint5 - intersectionPoint6);
                     }

              }
              break;
       case 1:
              bV4 = (int)intersectSegmentTriangle(b->v1, b->v2, a,intersectionPoint4);
              if (bV4 == 1)
              {
                     collisionReport.bCollide = true;
                     
                     if (bV1 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint1 + (intersectionPoint4 - intersectionPoint1);
                     }
                     else if (bV2 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint2 + (intersectionPoint4 - intersectionPoint2);
                     }
                     else
                     {
                            finalIntersectionPoint = intersectionPoint3 + (intersectionPoint4 - intersectionPoint3);
                     }

                     break;
              }

              bV5 = (int)intersectSegmentTriangle(b->v1, b->v3, a,intersectionPoint5);
              if (bV5 == 1)
              {
                     collisionReport.bCollide = true;
                     if (bV1 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint1 + (intersectionPoint5 - intersectionPoint1);
                     }
                     else if (bV2 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint2 + (intersectionPoint5 - intersectionPoint2);
                     }
                     else
                     {
                            finalIntersectionPoint = intersectionPoint3 + (intersectionPoint5 - intersectionPoint3);
                     }
                     break;
              }

              bV6 = (int)intersectSegmentTriangle(b->v2, b->v3, a,intersectionPoint6);
              if (bV6 == 1)
              {
                     collisionReport.bCollide = true;
                     if (bV1 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint1 + (intersectionPoint6 - intersectionPoint1);
                     }
                     else if (bV2 == 1)
                     {
                            finalIntersectionPoint = intersectionPoint2 + (intersectionPoint6 - intersectionPoint2);
                     }
                     else
                     {
                            finalIntersectionPoint = intersectionPoint3 + (intersectionPoint6 - intersectionPoint3);
                     }
              }
              break;
       case 2:
              collisionReport.bCollide = true;
              if (bV1 == 1 && bV2 == 1)
              {
                     finalIntersectionPoint = intersectionPoint1 + (intersectionPoint2 - intersectionPoint1);
              }
              else if (bV1 == 1 && bV3 == 1)
              {
                     finalIntersectionPoint = intersectionPoint1 + (intersectionPoint3 - intersectionPoint1);
              }
              else
              {
                     finalIntersectionPoint = intersectionPoint2 + (intersectionPoint3 - intersectionPoint2);
              }


              break;
       default:
              break;
       }

       if (collisionReport.bCollide)
              collisionReport.collisionPoints.push_back(finalIntersectionPoint);

       return collisionReport;
}

bool Collision_detector_Triangles::intersectSegmentTriangle(VECTOR3 p, VECTOR3 q, Triangle* triangle, VECTOR3& C)
{
       VECTOR3 ab = triangle->v2 - triangle->v1;
       VECTOR3 ac = triangle->v3 - triangle->v1;
       VECTOR3 qp = p - q;


       VECTOR3 n = ab.Cross(ac);

       float d =  qp.Dot(n);

       if (d <= 0.0f)
              return false;

       VECTOR3 ap = p - triangle->v1;

       float t = ap.Dot(n);
       if (t < 0.0f || t > d)
              return false;

       VECTOR3 e = qp.Cross(ap);
       float v = ac.Dot(e);
       if (v < 0.0f || v > d)
              return false;

       float w = -ab.Dot(e);
       if (w < 0.0f || v + w > d)
              return false;


       // Segment intersect triangle, compute intersetionPoint

       float ood = 1.0f / d;
       t *= ood;
       v *= ood;
       w *= ood;
       float u = 1.0f - v -w;

       C = p + t*(q-p);

       

       return true;
}


//                                                                                                                                                                                                         //
//                                                                    Collision_detector_Triangles_Distance                                                                    //
//                                                                                                                                                                                                         //


CollisionReport Collision_detector_Triangles_Distance::FindCollision(void* pNode1, void* pNode2)
{
       CollisionReport collisionReport;

       Triangle* a = (Triangle*) pNode1;
       Triangle* b = (Triangle*) pNode2;

       //float fDistance = 0;
       VECTOR3 C1, C2;

       VECTOR3 closestPt;

       // test all segments against both triangles
       
       collisionReport.fSqueredSeparationDistance = closestPointsSegmentSegment(a->v1,a->v2,b->v1,b->v2,C1,C2);
       closestPt = (C1 + (C2 - C1)/2);
       //fDistance = distanceSegmentSegment(a->v1,a->v2,b->v1,b->v2);

       float d = closestPointsSegmentSegment(a->v1,a->v2,b->v1,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }
       //float d = distanceSegmentSegment(a->v1,a->v2,b->v1,b->v3);
       //if (d < fDistance)
       //     fDistance = d;

       d = closestPointsSegmentSegment(a->v1,a->v2,b->v2,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v1,a->v2,b->v2,b->v3);
       //if (d < fDistance)
       //     fDistance = d;
       

       d = closestPointsSegmentSegment(a->v1,a->v3,b->v1,b->v2,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v1,a->v3,b->v1,b->v2);
       //if (d < fDistance)
       //     fDistance = d;


       d = closestPointsSegmentSegment(a->v1,a->v3,b->v1,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v1,a->v3,b->v1,b->v3);
       //if (d < fDistance)
       //     fDistance = d;

       d = closestPointsSegmentSegment(a->v1,a->v3,b->v2,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v1,a->v3,b->v2,b->v3);
       //if (d < fDistance)
       //     fDistance = d;


       d = closestPointsSegmentSegment(a->v2,a->v3,b->v1,b->v2,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v2,a->v3,b->v1,b->v2);
       //if (d < fDistance)
       //     fDistance = d;


       d = closestPointsSegmentSegment(a->v2,a->v3,b->v1,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v2,a->v3,b->v1,b->v3);
       //if (d < fDistance)
       //     fDistance = d;


       d = closestPointsSegmentSegment(a->v2,a->v3,b->v2,b->v3,C1,C2);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {      
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (C1 + (C2 - C1)/2);
       }

       //d = distanceSegmentSegment(a->v2,a->v3,b->v2,b->v3);
       //if (d < fDistance)
       //     fDistance = d;


       // Test all points against both triangles


       VECTOR3 pt = closestPointVertexTriangle(a->v1,*b);
       VECTOR3 v = (a->v1 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }


       //d = distanceVertexTriangle(a->v1,*b);
       //if (d < fDistance)
       //     fDistance = d;


       pt = closestPointVertexTriangle(a->v2,*b);
       v = (a->v2 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }


       //d = distanceVertexTriangle(a->v2,*b);
       //if (d < fDistance)
       //     fDistance = d;

       pt = closestPointVertexTriangle(a->v3,*b);
       v = (a->v3 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }

       //d = distanceVertexTriangle(a->v3,*b);
       //if (d < fDistance)
       //     fDistance = d;


       pt = closestPointVertexTriangle(b->v1,*a);
       v = (b->v1 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }


       //d = distanceVertexTriangle(b->v1,*a);
       //if (d < fDistance)
       //     fDistance = d;

       pt = closestPointVertexTriangle(b->v2,*a);
       v = (b->v2 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }

       //d = distanceVertexTriangle(b->v2,*a);
       //if (d < fDistance)
       //     fDistance = d;

       pt = closestPointVertexTriangle(b->v3,*a);
       v = (b->v3 - pt);
       d =  v.Dot(v);
       if ( d < collisionReport.fSqueredSeparationDistance)
       {
              collisionReport.fSqueredSeparationDistance = d;
              closestPt = (pt + (v / 2));
       }

       collisionReport.collisionPoints.push_back(closestPt);
       //d = distanceVertexTriangle(b->v3,*a);
       //if (d < fDistance)
       //     fDistance = d;


       return collisionReport;
}

//Computes closest points C1 and C2 between line segements S1(p1,q1) and S2(p2,q2) and returns minimal squered distance
float Collision_detector_Triangles_Distance::closestPointsSegmentSegment(VECTOR3 p1,VECTOR3 q1,VECTOR3 p2,VECTOR3 q2, VECTOR3& C1, VECTOR3& C2)
{
       float fDistance = 0;
       float s,t;    // parameters of closest points

       VECTOR3 d1 = q1 - p1; // Direction vector of S1
       VECTOR3 d2 = q2 - p2; // Direction vector of S2

       VECTOR3 r = p1 - p2;

       float a = d1.Dot(d1); // Squered lenght of S1
       float e = d2.Dot(d2); // Squered lenght of S2

       float f = d2.Dot(r);
       
       if (a <= EPSILON && e <= EPSILON)
       {
              // Both segments degenerate into points
              s = t = 0.0f;
       }
       else if (a <= EPSILON)
       {
              // first segment degenerates into point
              s = 0.0f;
              t = clamp(f/e,0.0f,1.0f);
       }
       else
       {
              float c = d1.Dot(r);
              if ( e <= EPSILON)
              {
                     // second segment degenerates into point
                     t = 0.0f;
                     s = clamp(-c/a,0.0f,1.0f);
              }
              else
              {
                     float b = d1.Dot(d2);
                     float denom = a*e - b*b;

                     // if segments not parallel compute closest point on L1 to L2
                     //     and clamp it to segment S1, else pick s
                     if (denom != 0.0f)
                            s = clamp((b*f-c*e)/denom,0.0f,1.0f);
                     else
                            s = 0.0f;

                     // Compute closest point on L2 closest to S1(s)
                     t = (b*s + f) / e;

                     // if t in [0,1] done. Else clamp t, recompute s for the new value t
                     if ( t < 0.0f)
                     {
                            t = 0.0f;
                            s = clamp(-c/a,0.0f, 1.0f);
                     }
                     else if (t > 1.0f)
                     {
                            t = 1.0f;
                            s = clamp((b-c)/a,0.0f, 1.0f);
                     }
              }
       }

       C1 = p1 + d1*s;
       C2 = p2 + d2*t;

       VECTOR3 x = C2-C1;

       fDistance = x.Dot(x);

       return fDistance;
}

//Computes minimal distance between line segements S1(p1,q1) and S2(p2,q2) 
float Collision_detector_Triangles_Distance::distanceSegmentSegment(VECTOR3 p1,VECTOR3 q1,VECTOR3 p2,VECTOR3 q2)
{
       float fDistance = 0;
       float s,t;    // parameters of closest points

       VECTOR3 d1 = q1 - p1; // Direction vector of S1
       VECTOR3 d2 = q2 - p2; // Direction vector of S2

       VECTOR3 r = p1 - p2;

       float a = d1.Dot(d1); // Squered lenght of S1
       float e = d2.Dot(d2); // Squered lenght of S2

       float f = d2.Dot(r);
       
       if (a <= EPSILON && e <= EPSILON)
       {
              // Both segments degenerate into points
              s = t = 0.0f;
       }
       else if (a <= EPSILON)
       {
              // first segment degenerates into point
              s = 0.0f;
              t = clamp(f/e,0.0f,1.0f);
       }
       else
       {
              float c = d1.Dot(r);
              if ( e <= EPSILON)
              {
                     // second segment degenerates into point
                     t = 0.0f;
                     s = clamp(-c/a,0.0f,1.0f);
              }
              else
              {
                     float b = d1.Dot(d2);
                     float denom = a*e - b*b;

                     // if segments not parallel compute closest point on L1 to L2
                     //     and clamp it to segment S1, else pick s
                     if (denom != 0.0f)
                            s = clamp((b*f-c*e)/denom,0.0f,1.0f);
                     else
                            s = 0.0f;

                     // Compute closest point on L2 closest to S1(s)
                     t = (b*s + f) / e;

                     // if t in [0,1] done. Else clamp t, recompute s for the new value t
                     if ( t < 0.0f)
                     {
                            t = 0.0f;
                            s = clamp(-c/a,0.0f, 1.0f);
                     }
                     else if (t > 1.0f)
                     {
                            t = 1.0f;
                            s = clamp((b-c)/a,0.0f, 1.0f);
                     }
              }
       }

       VECTOR3 c1 = p1 + d1*s;
       VECTOR3 c2 = p2 + d2*t;

       VECTOR3 x = c2-c1;

       fDistance = x.Dot(x);

       return fDistance;
}

float Collision_detector_Triangles_Distance::clamp(float n, float min, float max)
{
       if (n < min) return min;
       if (n > max) return max;
       return n;
}

float Collision_detector_Triangles_Distance::distanceVertexTriangle(VECTOR3 p, Triangle triangle)
{
       float fDistance = 0;
       VECTOR3 closestPoint = closestPointVertexTriangle(p,triangle);

       
       VECTOR3 d = p - closestPoint;


       fDistance = d.Dot(d);

       return fDistance;
}

VECTOR3 Collision_detector_Triangles_Distance::closestPointVertexTriangle(VECTOR3 p, Triangle triangle)
{
       VECTOR3 ab = triangle.v2 - triangle.v1;
       VECTOR3 ac = triangle.v3 - triangle.v1;
       VECTOR3 ap = p - triangle.v1;

       float d1 = ab.Dot(ap);
       float d2 = ac.Dot(ap);

       // check if p in vertex region outside A
       if ( d1 <= 0.0f && d2 <= 0.0f) // Barycentric coordinates (1,0,0)
       {
              return triangle.v1;
       }
       
       // check if p in vertex region outside B
       VECTOR3 bp = p - triangle.v2;
       float d3 = ab.Dot(bp);
       float d4 = ac.Dot(bp);

       if (d3 >= 0.0f && d4 <= d3) // Barycentric coordinates (0,1,0)
              return triangle.v2;
              
       // check if p in edge projection of AB
       float vc = d1*d4 - d3*d2;
       if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
       {
              float v = d1 / (d1-d3);
              return triangle.v1+v*ab;
       }

       // check if p in vertex region outside A
       VECTOR3 cp = p - triangle.v3;
       float d5 = ab.Dot(cp);
       float d6 = ac.Dot(cp);
       if (d6 >= 0.0f && d5 <= d6) // Barycentric coordinates (0,0,1)
              return triangle.v3;

       // check if p in edge projection of AC
       float vb = d5*d2 - d1*d6;
       if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
       {
              float w = d2 / (d2-d6);
              return triangle.v1+w*ac;
       }

       // check if p in edge projection of BC
       float va = d3*d6 - d5*d4;
       if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
       {
              float w = (d4 - d3) / ((d4-d3)+(d5-d6));
              return triangle.v2 + w * (triangle.v3-triangle.v2);
       }

       // p is inside face region
       float denom = 1.0f / (va +vb +vc);
       float v = vb * denom;
       float w = vc * denom;

       return triangle.v1 + ab*v + ac*w;
}

---