Main Page   Namespace List   Class Hierarchy   Compound List   File List   Compound Members   File Members  

collision.cpp

Go to the documentation of this file.
00001 // Collision detection routines and helper functions 
00002 // (by Alan Baylis 2001, Adapted from the work of Kasper Fauerby - aka Telemachos)
00003 
00004 #include <math.h>
00005 #include "locmath.h"
00006 #include "collision.h"
00007 #include "vector.h"
00008 #include "mmgr.h"
00009 
00010 extern int numPolygons;
00011 
00012 void SetLength(VECTOR& v, float l)
00013 {
00014  float len = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);    
00015  v.x *= l/len;
00016  v.y *= l/len;
00017  v.z *= l/len;
00018 } 
00019 
00020 bool IsZeroVector(VECTOR& v)
00021 {
00022   if ((v.x == 0.0f) && (v.y == 0.0f) && (v.z == 0.0f))
00023     return TRUE;
00024   return FALSE;    
00025 }
00026 
00027 VECTOR Wedge(VECTOR v1, VECTOR v2)
00028 {
00029  VECTOR result;
00030  
00031  result.x = (v1.y * v2.z) - (v2.y * v1.z);
00032  result.y = (v1.z * v2.x) - (v2.z * v1.x);
00033  result.z = (v1.x * v2.y) - (v2.x * v1.y);     
00034  
00035  return (result);    
00036 }
00037 
00038 float IntersectRayPlane(VECTOR rOrigin, VECTOR rVector, VECTOR pOrigin, VECTOR pNormal) 
00039 {
00040   
00041   float d = - (DotProduct(pNormal,pOrigin));
00042  
00043   float numer = DotProduct(pNormal,rOrigin) + d;
00044   float denom = DotProduct(pNormal,rVector);
00045   
00046   if (denom == 0)  // normal is orthogonal to vector, cant intersect
00047    return (-1.0f);
00048    
00049   return -(numer / denom);    
00050 }
00051 
00052 float IntersectRaySphere(VECTOR rO, VECTOR rV, VECTOR sO, float sR) 
00053 {
00054    VECTOR TempVect;
00055    TempVect.x = sO.x - rO.x;
00056    TempVect.y = sO.y - rO.y;
00057    TempVect.z = sO.z - rO.z;
00058    VECTOR Q = TempVect;
00059    
00060    float c = MagnitudeVector(Q);
00061    float v = DotProduct(Q,rV);
00062    float d = sR*sR - (c*c - v*v);
00063 
00064    // If there was no intersection, return -1
00065    if (d < 0.0) return (-1.0f);
00066 
00067    // Return the distance to the [first] intersecting point
00068    return (v - sqrt(d));
00069 }
00070 
00071 bool CheckPointInTriangle(VECTOR point, VECTOR a, VECTOR b, VECTOR c) 
00072 {
00073   
00074   float total_angles = 0.0f;
00075        
00076   // make the 3 vectors
00077   VECTOR TempVect;
00078   TempVect.x = point.x - a.x;
00079   TempVect.y = point.y - a.y;
00080   TempVect.z = point.z - a.z;
00081   VECTOR v1 = TempVect;
00082   TempVect.x = point.x - b.x;
00083   TempVect.y = point.y - b.y;
00084   TempVect.z = point.z - b.z;
00085   VECTOR v2 = TempVect;
00086   TempVect.x = point.x - c.x;
00087   TempVect.y = point.y - c.y;
00088   TempVect.z = point.z - c.z;
00089   VECTOR v3 = TempVect;
00090   
00091   v1 = GetUnitVector(v1);
00092   v2 = GetUnitVector(v2);
00093   v3 = GetUnitVector(v3);
00094   float Dot1 = DotProduct(v1,v2);
00095   if (Dot1 < -1)
00096     Dot1 = -1;
00097   if (Dot1 > 1)
00098     Dot1 = 1;
00099   total_angles += acos(Dot1);   
00100   float Dot2 = DotProduct(v2,v3);
00101   if (Dot2 < -1)
00102     Dot2 = -1;
00103   if (Dot2 > 1)
00104     Dot2 = 1;
00105   total_angles += acos(Dot2);
00106   float Dot3 = DotProduct(v3,v1);
00107   if (Dot3 < -1)
00108     Dot3 = -1;
00109   if (Dot3 > 1)
00110     Dot3 = 1;
00111   total_angles += acos(Dot3); 
00112      
00113   if (fabs(total_angles-2*pi) <= 0.005)
00114    return (TRUE);
00115      
00116   return(FALSE);
00117 }
00118 
00119 VECTOR ClosestPointOnLine(VECTOR& a, VECTOR& b, VECTOR& p) 
00120 {
00121    // Determine t (the length of the vector from ‘a’ to ‘p’)
00122    VECTOR TempVect;
00123    TempVect.x = p.x - a.x;
00124    TempVect.y = p.y - a.y;
00125    TempVect.z = p.z - a.z;
00126    VECTOR c = TempVect;
00127    TempVect.x = b.x - a.x;
00128    TempVect.y = b.y - a.y;
00129    TempVect.z = b.z - a.z;
00130    VECTOR V = TempVect; 
00131       
00132    float d = MagnitudeVector(V);
00133       
00134    V = GetUnitVector(V);  
00135    double t = DotProduct(V,c);
00136 
00137    // Check to see if ‘t’ is beyond the extents of the line segment
00138    if (t < 0.0f) return (a);
00139    if (t > d) return (b);
00140  
00141    // Return the point between ‘a’ and ‘b’
00142    //set length of V to t. V is normalized so this is easy
00143    V.x = V.x * t;
00144    V.y = V.y * t;
00145    V.z = V.z * t;
00146 
00147    TempVect.x = a.x + V.x;           
00148    TempVect.y = a.y + V.y;           
00149    TempVect.z = a.z + V.z;           
00150    return (TempVect);    
00151 }
00152 
00153 VECTOR ClosestPointOnPolygon(VECTOR a, VECTOR c, VECTOR b, VECTOR p) 
00154 {
00155     
00156   VECTOR Rab = ClosestPointOnLine(a, b, p);
00157   VECTOR Rbc = ClosestPointOnLine(b, c, p);
00158   VECTOR Rca = ClosestPointOnLine(c, a, p);
00159   
00160   VECTOR TempVect;
00161   TempVect.x = p.x - Rab.x;  
00162   TempVect.y = p.y - Rab.y;  
00163   TempVect.z = p.z - Rab.z;  
00164   float dAB = MagnitudeVector(TempVect);
00165   TempVect.x = p.x - Rbc.x;  
00166   TempVect.y = p.y - Rbc.y;  
00167   TempVect.z = p.z - Rbc.z;  
00168   float dBC = MagnitudeVector(TempVect);
00169   TempVect.x = p.x - Rca.x;  
00170   TempVect.y = p.y - Rca.y;  
00171   TempVect.z = p.z - Rca.z;  
00172   float dCA = MagnitudeVector(TempVect);
00173   
00174   float min = dAB;
00175   VECTOR result = Rab;
00176   
00177   if (dBC < min) 
00178   {
00179     min = dBC;
00180     result = Rbc;
00181   }
00182  
00183   if (dCA < min)
00184     result = Rca;
00185   
00186     return (result);    
00187 }
00188 
00189 bool CheckPointInSphere(VECTOR point, VECTOR sO, float sR) 
00190 {
00191   VECTOR TempVect;
00192   TempVect.x = point.x - sO.x;
00193   TempVect.y = point.y - sO.y;
00194   TempVect.z = point.z - sO.z;
00195  
00196   float d = MagnitudeVector(TempVect);
00197  
00198   if(d <= sR) 
00199     return TRUE;
00200   return FALSE;    
00201 }
00202 
00203 VECTOR TangentPlaneNormalOfEllipsoid(VECTOR point, VECTOR eO, VECTOR eR)
00204 {
00205  VECTOR TempVect;
00206  TempVect.x = point.x - eO.x;
00207  TempVect.y = point.y - eO.y;
00208  TempVect.z = point.z - eO.z;
00209  VECTOR p = TempVect;
00210 
00211  float a2 = eR.x * eR.x;
00212  float b2 = eR.y * eR.y;
00213  float c2 = eR.z * eR.z;
00214 
00215  
00216  VECTOR res;
00217  res.x = p.x / a2;
00218  res.y = p.y / b2;
00219  res.z = p.z / c2;
00220 
00221  res = GetUnitVector(res);    
00222  return (res);    
00223 }
00224 
00225 int ClassifyPoint(VECTOR point, VECTOR pO, VECTOR pN)
00226 {
00227  VECTOR TempVect;
00228  TempVect.x = pO.x - point.x;
00229  TempVect.y = pO.y - point.y;
00230  TempVect.z = pO.z - point.z;
00231  VECTOR dir = TempVect;
00232  float d = DotProduct(dir, pN);
00233 
00234  if (d < -0.01f)
00235    return 1;
00236  else
00237    if (d > 0.01f)
00238      return -1;
00239    return 0;
00240 }
00241 
00242 void CheckForCollision(POLYGON* polygon, VECTOR* destination, CollisionPacket* colPackage)
00243 {
00244     colPackage->foundCollision = FALSE;
00245 
00246       // from package
00247       VECTOR eRadius = colPackage->eRadius;
00248       VECTOR source = colPackage->sourcePoint;
00249       VECTOR velocity = colPackage->velocity;
00250 
00251       // keep a copy of this as it's needed a few times
00252       VECTOR normalizedVelocity = GetUnitVector(velocity);
00253 
00254       // intersection data
00255       VECTOR sIPoint;    // sphere intersection point
00256       VECTOR pIPoint;    // plane intersection point
00257       VECTOR polyIPoint; // polygon intersection point
00258 
00259       float distToPlaneIntersection;
00260       float distToSphereIntersection;
00261 
00262      // loop through all the polygons of the world
00263      for(int i = 0; i < numPolygons; i++)
00264       {
00265 
00266           // Get plane normal
00267           VECTOR pOrigin;
00268           pOrigin.x = polygon[i].Vertex[0].x;        //Set plane origin
00269           pOrigin.y = polygon[i].Vertex[0].y;
00270           pOrigin.z = polygon[i].Vertex[0].z;
00271           VECTOR pNormal = polygon[i].GetNormal();
00272 
00273           // Normalize plane normal
00274           pNormal = GetUnitVector(pNormal);
00275 
00276           if (DotProduct(pNormal, normalizedVelocity) <= 1.0f)
00277           {
00278               // Calculate sphere intersection point
00279               VECTOR eIPoint;
00280               eIPoint.x = source.x - pNormal.x;  //Source point + inverted plane normal
00281               eIPoint.y = source.y - pNormal.y;
00282               eIPoint.z = source.z - pNormal.z;
00283 
00284               // shoot ray along the velocity vector
00285               distToPlaneIntersection = IntersectRayPlane(eIPoint, normalizedVelocity, pOrigin, pNormal);
00286 
00287               // calculate plane intersection point
00288               pIPoint.x = eIPoint.x + distToPlaneIntersection * normalizedVelocity.x;
00289               pIPoint.y = eIPoint.y + distToPlaneIntersection * normalizedVelocity.y;
00290               pIPoint.z = eIPoint.z + distToPlaneIntersection * normalizedVelocity.z;
00291 
00292               int pClass = ClassifyPoint(eIPoint, pOrigin, pNormal);
00293 
00294               // find the plane intersection point
00295               if (pClass == -1)
00296               {               // plane spans sphere
00297 
00298                 // find plane intersection point by shooting a ray from the
00299                 // sphere intersection point along the planes normal.
00300                 distToPlaneIntersection = IntersectRayPlane(eIPoint, pNormal, pOrigin, pNormal);
00301 
00302                 // calculate plane intersection point
00303                 pIPoint.x = eIPoint.x + distToPlaneIntersection * pNormal.x;
00304                 pIPoint.y = eIPoint.y + distToPlaneIntersection * pNormal.y;
00305                 pIPoint.z = eIPoint.z + distToPlaneIntersection * pNormal.z;
00306               }
00307 
00308               // find polygon intersection point. By default we assume its equal to the
00309               // plane intersection point. In that case we already know the distance to it.
00310               polyIPoint = pIPoint;
00311               distToSphereIntersection = distToPlaneIntersection;
00312 
00313               VECTOR a, b, c;
00314               a.x = polygon[i].Vertex[0].x;
00315               a.y = polygon[i].Vertex[0].y;
00316               a.z = polygon[i].Vertex[0].z;
00317               b.x = polygon[i].Vertex[1].x;
00318               b.y = polygon[i].Vertex[1].y;
00319               b.z = polygon[i].Vertex[1].z;
00320               c.x = polygon[i].Vertex[2].x;
00321               c.y = polygon[i].Vertex[2].y;
00322               c.z = polygon[i].Vertex[2].z;
00323 
00324               if(!CheckPointInTriangle(pIPoint, a, b, c))
00325               {
00326                 // if not in triangle
00327                     polyIPoint = ClosestPointOnPolygon(a, b, c, pIPoint);
00328 
00329                     VECTOR invertednormalizedVelocity;
00330                     invertednormalizedVelocity.x = -normalizedVelocity.x;
00331                     invertednormalizedVelocity.y = -normalizedVelocity.y;
00332                     invertednormalizedVelocity.z = -normalizedVelocity.z;
00333                     distToSphereIntersection = IntersectRaySphere(polyIPoint, invertednormalizedVelocity, source, 1.0f);
00334 
00335                     // we cannot know if the ray will actually hit the sphere so we need this check
00336                     if (distToSphereIntersection > 0)
00337                     {
00338                           // calculate true ellipsoid intersection point
00339                           eIPoint.x = polyIPoint.x + distToSphereIntersection * invertednormalizedVelocity.x;
00340                           eIPoint.y = polyIPoint.y + distToSphereIntersection * invertednormalizedVelocity.y;
00341                           eIPoint.z = polyIPoint.z + distToSphereIntersection * invertednormalizedVelocity.z;
00342                     }
00343               }
00344 
00345               // any chance of hit ?
00346               if ((distToSphereIntersection > 0) && (distToSphereIntersection <= MagnitudeVector(velocity)))
00347               {
00348                       // if first hit, or closest hit so far
00349                      if ((colPackage->foundCollision == FALSE) || (distToSphereIntersection < colPackage->nearestDistance))
00350                      {
00351                            colPackage->nearestDistance = distToSphereIntersection;
00352                            colPackage->nearestSphereIntersectionPoint = eIPoint;
00353                            colPackage->nearestPolygonIntersectionPoint = polyIPoint;
00354                            colPackage->foundCollision = TRUE;
00355                      }
00356               }
00357         }
00358     }
00359 
00360     if (!colPackage->foundCollision)
00361         return;
00362 
00363     //Collision response
00364 
00365     // OK, first task is to move close to where we hit something :
00366       VECTOR newSourcePoint;
00367 
00368       // only update if we are not already very close
00369       if (colPackage->nearestDistance >= epsilon)
00370       {
00371 
00372             VECTOR V = velocity;
00373               SetLength(V, colPackage->nearestDistance-epsilon);
00374             newSourcePoint.x = source.x + V.x;
00375             newSourcePoint.y = source.y + V.y;
00376             newSourcePoint.z = source.z + V.z;
00377       }
00378     else
00379             newSourcePoint = source;
00380 
00381     // Determine the sliding plane (we do this now, because we're about to
00382       // change sourcePoint)
00383       VECTOR slidePlaneOrigin = colPackage->nearestPolygonIntersectionPoint;
00384       VECTOR slidePlaneNormal;
00385       slidePlaneNormal.x = newSourcePoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00386       slidePlaneNormal.y = newSourcePoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00387       slidePlaneNormal.z = newSourcePoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00388         // We now project the destination point onto the sliding plane
00389       float l = IntersectRayPlane(*destination, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
00390      VECTOR newDestinationPoint;
00391       newDestinationPoint.x = destination->x + l * slidePlaneNormal.x;
00392       newDestinationPoint.y = destination->y + l * slidePlaneNormal.y;
00393       newDestinationPoint.z = destination->z + l * slidePlaneNormal.z;
00394 
00395     VECTOR newVelocityVector;
00396       newVelocityVector.x = newDestinationPoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00397       newVelocityVector.y = newDestinationPoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00398      newVelocityVector.z = newDestinationPoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00399 
00400     destination->x = newSourcePoint.x + newVelocityVector.x;
00401       destination->y = newSourcePoint.y + newVelocityVector.y;
00402       destination->z = newSourcePoint.z + newVelocityVector.z;
00403     colPackage->sourcePoint = newSourcePoint;
00404     colPackage->velocity = newVelocityVector;
00405     colPackage->foundCollision = 0;
00406         CheckForCollision(polygon, destination, colPackage);
00407 }

Generated on Fri Dec 23 05:19:54 2005 for Particles by doxygen1.2.15