00001
00002
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)
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
00065 if (d < 0.0) return (-1.0f);
00066
00067
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
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
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
00138 if (t < 0.0f) return (a);
00139 if (t > d) return (b);
00140
00141
00142
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
00243 void CheckForCollision(VECTOR* destination, int num_polygons, POLYGON* polys, CollisionPacket* colPackage)
00244 {
00245 colPackage->foundCollision = FALSE;
00246
00247
00248 VECTOR eRadius = colPackage->eRadius;
00249 VECTOR source = colPackage->sourcePoint;
00250 VECTOR velocity = colPackage->velocity;
00251
00252
00253 VECTOR normalizedVelocity = GetUnitVector(velocity);
00254
00255
00256 VECTOR sIPoint;
00257 VECTOR pIPoint;
00258 VECTOR triIPoint;
00259
00260 float distToPlaneIntersection;
00261 float distToSphereIntersection;
00262
00263
00264 for(int i = 0; i < num_polygons; i++)
00265 {
00266
00267
00268 VECTOR pOrigin;
00269 pOrigin = polys[i].Vertex[0].coords;
00270 VECTOR pNormal = polys[i].GetNormal();
00271
00272
00273 pNormal = GetUnitVector(pNormal);
00274
00275 if (DotProduct(pNormal, normalizedVelocity) <= 1.0f)
00276 {
00277
00278 VECTOR eIPoint;
00279 eIPoint.x = source.x - pNormal.x;
00280 eIPoint.y = source.y - pNormal.y;
00281 eIPoint.z = source.z - pNormal.z;
00282
00283
00284 distToPlaneIntersection = IntersectRayPlane(eIPoint, normalizedVelocity, pOrigin, pNormal);
00285
00286
00287 pIPoint.x = eIPoint.x + distToPlaneIntersection * normalizedVelocity.x;
00288 pIPoint.y = eIPoint.y + distToPlaneIntersection * normalizedVelocity.y;
00289 pIPoint.z = eIPoint.z + distToPlaneIntersection * normalizedVelocity.z;
00290
00291 int pClass = ClassifyPoint(eIPoint, pOrigin, pNormal);
00292
00293
00294 if (pClass == -1)
00295 {
00296
00297
00298
00299 distToPlaneIntersection = IntersectRayPlane(eIPoint, pNormal, pOrigin, pNormal);
00300
00301
00302 pIPoint.x = eIPoint.x + distToPlaneIntersection * pNormal.x;
00303 pIPoint.y = eIPoint.y + distToPlaneIntersection * pNormal.y;
00304 pIPoint.z = eIPoint.z + distToPlaneIntersection * pNormal.z;
00305 }
00306
00307
00308
00309 triIPoint = pIPoint;
00310 distToSphereIntersection = distToPlaneIntersection;
00311
00312 VECTOR a, b, c;
00313 a = polys[i].Vertex[0].coords;
00314 b = polys[i].Vertex[1].coords;
00315 c = polys[i].Vertex[2].coords;
00316
00317 if(!CheckPointInTriangle(pIPoint, a, b, c))
00318 {
00319
00320 triIPoint = ClosestPointOnPolygon(a, b, c, pIPoint);
00321
00322 VECTOR invertednormalizedVelocity;
00323 invertednormalizedVelocity.x = -normalizedVelocity.x;
00324 invertednormalizedVelocity.y = -normalizedVelocity.y;
00325 invertednormalizedVelocity.z = -normalizedVelocity.z;
00326 distToSphereIntersection = IntersectRaySphere(triIPoint, invertednormalizedVelocity, source, 1.0f);
00327
00328
00329 if (distToSphereIntersection > 0)
00330 {
00331
00332 eIPoint.x = triIPoint.x + distToSphereIntersection * invertednormalizedVelocity.x;
00333 eIPoint.y = triIPoint.y + distToSphereIntersection * invertednormalizedVelocity.y;
00334 eIPoint.z = triIPoint.z + distToSphereIntersection * invertednormalizedVelocity.z;
00335 }
00336 }
00337
00338
00339 if ((distToSphereIntersection > 0) && (distToSphereIntersection <= MagnitudeVector(velocity)))
00340 {
00341
00342 if ((colPackage->foundCollision == FALSE) || (distToSphereIntersection < colPackage->nearestDistance))
00343 {
00344 colPackage->nearestDistance = distToSphereIntersection;
00345 colPackage->nearestSphereIntersectionPoint = eIPoint;
00346 colPackage->nearestTriangleIntersectionPoint = triIPoint;
00347 colPackage->foundCollision = TRUE;
00348 }
00349 }
00350 }
00351 }
00352
00353 if (!colPackage->foundCollision)
00354 return;
00355
00356
00357
00358
00359 VECTOR newSourcePoint;
00360
00361
00362 if (colPackage->nearestDistance >= epsilon)
00363 {
00364
00365 VECTOR V = velocity;
00366 SetLength(V, colPackage->nearestDistance-epsilon);
00367 newSourcePoint.x = source.x + V.x;
00368 newSourcePoint.y = source.y + V.y;
00369 newSourcePoint.z = source.z + V.z;
00370 }
00371 else
00372 newSourcePoint = source;
00373
00374
00375
00376 VECTOR slidePlaneOrigin = colPackage->nearestTriangleIntersectionPoint;
00377 VECTOR slidePlaneNormal;
00378 slidePlaneNormal.x = newSourcePoint.x - colPackage->nearestTriangleIntersectionPoint.x;
00379 slidePlaneNormal.y = newSourcePoint.y - colPackage->nearestTriangleIntersectionPoint.y;
00380 slidePlaneNormal.z = newSourcePoint.z - colPackage->nearestTriangleIntersectionPoint.z;
00381
00382 float l = IntersectRayPlane(*destination, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
00383 VECTOR newDestinationPoint;
00384 newDestinationPoint.x = destination->x + l * slidePlaneNormal.x;
00385 newDestinationPoint.y = destination->y + l * slidePlaneNormal.y;
00386 newDestinationPoint.z = destination->z + l * slidePlaneNormal.z;
00387
00388 VECTOR newVelocityVector;
00389 newVelocityVector.x = newDestinationPoint.x - colPackage->nearestTriangleIntersectionPoint.x;
00390 newVelocityVector.y = newDestinationPoint.y - colPackage->nearestTriangleIntersectionPoint.y;
00391 newVelocityVector.z = newDestinationPoint.z - colPackage->nearestTriangleIntersectionPoint.z;
00392
00393 destination->x = newSourcePoint.x + newVelocityVector.x;
00394 destination->y = newSourcePoint.y + newVelocityVector.y;
00395 destination->z = newSourcePoint.z + newVelocityVector.z;
00396 colPackage->sourcePoint = newSourcePoint;
00397 colPackage->velocity = newVelocityVector;
00398 colPackage->foundCollision = 0;
00399 CheckForCollision(destination, num_polygons, polys, colPackage);
00400 }