void CollideWithWorld(Point &sourcePoint, Vector velocityVector)
{
float DistanceToTravel;
DistanceToTravel = float(sqrt((velocityVector.x * velocityVector.x) +
(velocityVector.y * velocityVector.y) +
(velocityVector.z * velocityVector.z)));
if(DistanceToTravel < EPSILON)
{
sourcePoint.x += velocityVector.x;
sourcePoint.y += velocityVector.y;
sourcePoint.z += velocityVector.z;
return;//sourcePoint;
}
Point destinationPoint;
destinationPoint.x = sourcePoint.x + velocityVector.x;
destinationPoint.y = sourcePoint.y + velocityVector.y;
destinationPoint.z = sourcePoint.z + velocityVector.z;
//PQBoundingBoxList *list; // pointer to linked list of bounding boxes
PQBoundingBox *box; // pointer to bounding box
//list = bblist;
box = GetPolyList(bblist);
if(box == NULL)
{
sourcePoint.x += velocityVector.x;
sourcePoint.y += velocityVector.y;
sourcePoint.z += velocityVector.z;
return;// sourcePoint;
}
bool firstTimeThrough = true;
float NearDistance = -1.0f;
Poly nearestCollider;
Point nearestIntersectionPoint;
Point nearestPolygonIntersectionPoint;
for(int i = 0; i < box->numtriangles; i++)
{
Point pOrigin;
Point p1,p2,p3;
Poly poly1;
p1.x = box->triangles[0].x;
p1.y = box->triangles[0].y;
p1.z = box->triangles[0].z;
pOrigin = p1;
p2.x = box->triangles[1].x;
p2.y = box->triangles[1].y;
p2.z = box->triangles[1].z;
p3.x = box->triangles[2].x;
p3.y = box->triangles[2].y;
p3.z = box->triangles[2].z;
poly1.c1 = p1;
poly1.c2 = p2;
poly1.c3 = p3;
Vector pNormal = CalculateNormal(p1, p2, p3);
Vector pTemp;
pTemp.x = -pNormal.x;
pTemp.y = -pNormal.y;
pTemp.z = -pNormal.z;
float pDist = intersect(sourcePoint, pTemp, pOrigin, pNormal);
Point planeIntersectionPoint;
Point ellipsoidIntersectionPoint;
Vector directionalRadius;
directionalRadius.x = -pNormal.x * radiusVector.x;
directionalRadius.y = -pNormal.y * radiusVector.y;
directionalRadius.z = -pNormal.z * radiusVector.z;
float radius;
radius = float(sqrt((directionalRadius.x * directionalRadius.x) +
(directionalRadius.y * directionalRadius.y) +
(directionalRadius.z * directionalRadius.z)));
if(fabs(pDist) <= radius)
{
Vector temp;
temp.x = -(pNormal.x / pNormal.x) * pDist;
temp.y = -(pNormal.y / pNormal.y) * pDist;
temp.z = -(pNormal.z / pNormal.z) * pDist;
planeIntersectionPoint.x = sourcePoint.x + temp.x;
planeIntersectionPoint.y = sourcePoint.y + temp.y;
planeIntersectionPoint.z = sourcePoint.z + temp.z;
}
else
{
Vector temp;
temp.x = -(pNormal.x / pNormal.x) * radius;
temp.y = -(pNormal.y / pNormal.y) * radius;
temp.z = -(pNormal.z / pNormal.z) * radius;
ellipsoidIntersectionPoint.x = sourcePoint.x + temp.x;
ellipsoidIntersectionPoint.y = sourcePoint.y + temp.y;
ellipsoidIntersectionPoint.z = sourcePoint.z + temp.z;
float t = intersect(ellipsoidIntersectionPoint, velocityVector, pOrigin, pNormal);
Vector V;
V.x = (velocityVector.x / velocityVector.x) * t;
V.y = (velocityVector.y / velocityVector.y) * t;
V.z = (velocityVector.z / velocityVector.z) * t;
}
Point polygonIntersectionPoint = planeIntersectionPoint;
if(CheckPointInTriangle(planeIntersectionPoint, p1, p2, p3))
{
polygonIntersectionPoint = closestPointOnTriangle(p1, p2, p3, planeIntersectionPoint);
}
Vector negativeVelocityVector;
negativeVelocityVector.x = -velocityVector.x;
negativeVelocityVector.y = -velocityVector.y;
negativeVelocityVector.z = -velocityVector.z;
float t = intersectSphere(sourcePoint, radiusVector, polygonIntersectionPoint, negativeVelocityVector);
if(t >= 0.0f && t <= DistanceToTravel)
{
Vector V;
V = NormalizeVector(negativeVelocityVector);
V.x *= t;
V.y *= t;
V.z *= t;
Vector intersectPoint;
intersectPoint.x = polygonIntersectionPoint.x + V.x;
intersectPoint.y = polygonIntersectionPoint.y + V.y;
intersectPoint.z = polygonIntersectionPoint.z + V.z;
if(firstTimeThrough || t < NearDistance)
{
NearDistance = t;
nearestCollider = poly1;
nearestIntersectionPoint = intersectPoint;
nearestPolygonIntersectionPoint = polygonIntersectionPoint;
firstTimeThrough = false;
}
}
}
if(firstTimeThrough)
{
sourcePoint.x += velocityVector.x;
sourcePoint.y += velocityVector.y;
sourcePoint.z += velocityVector.z;
return;// sourcePoint;
}
Vector V;
V = NormalizeVector(velocityVector);
V.x *= float(NearDistance - EPSILON);
V.y *= float(NearDistance - EPSILON);
V.z *= float(NearDistance - EPSILON);
sourcePoint.x += V.x;
sourcePoint.y += V.y;
sourcePoint.z += V.z;
Point slidePlaneOrigin = nearestPolygonIntersectionPoint;
Vector slidePlaneNormal = nearestPolygonIntersectionPoint;
slidePlaneNormal.x -= sourcePoint.x;
slidePlaneNormal.y -= sourcePoint.y;
slidePlaneNormal.z -= sourcePoint.z;
float time = intersect(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
slidePlaneNormal = NormalizeVector(slidePlaneNormal);
slidePlaneNormal.x *= time;
slidePlaneNormal.y *= time;
slidePlaneNormal.z *= time;
Vector destinationProjectionNormal = slidePlaneNormal;
Point newDestinationPoint = destinationPoint;
newDestinationPoint.x += destinationProjectionNormal.x;
newDestinationPoint.y += destinationProjectionNormal.y;
newDestinationPoint.z += destinationProjectionNormal.z;
Vector newVelocityVector = newDestinationPoint;
newVelocityVector.x -= nearestPolygonIntersectionPoint.x;
newVelocityVector.y -= nearestPolygonIntersectionPoint.y;
newVelocityVector.z -= nearestPolygonIntersectionPoint.z;
CollideWithWorld(sourcePoint, newVelocityVector);
//return sourcePoint;
}
</code> </i>
Collision Detection Problem
Im working on a 3d rpg engine, and were at collision detection, this is my CD code i have running, but its not properly working, perhaps its either my code or another aspect of code, could anyone look this over and let me know if it looks ok?
i cleaned it up alil using operator overloading to save some space. and fixed my normalizing of vector, but it still drops right through the floor and dont collide with anything
i thought i did indent it, anyway, heres all my collision stuff pretty much..
#define Point Vectorstruct Poly{ Point c1; Point c2; Point c3;};#define EPSILON 0.0001#define PI 3.14519Vector radiusVector(0.2f, 0.2f, 0.2f);Point Cross_Product(Point a, Point b){ Point returnValue; returnValue.x = (a.y * b.z) - (a.z * b.y); returnValue.y = (a.z * b.x) - (a.x * b.z); returnValue.z = (a.x * b.y) - (a.y * b.x); return returnValue;}float Dot_Product(Point a, Point b){ return ((a.x * b.x) + (a.y * b.y) + (a.z * b.z));}float Distance(Point a, Point b){ float x, y, z; x = a.x - b.x; y = a.y - b.y; z = a.z - b.z; return float(sqrt((x*x) + (y*y) + (z*z)));}Vector NormalizeVector(Vector v){ v.x = v.x / fabs(v.x); v.y = v.y / fabs(v.y); v.z = v.z / fabs(v.z); return v;}bool CheckPointInTriangle(Point p, Point a, Point b, Point c) { float total_angles = 0.0f; // make the 3 vectors Point v1, v2, v3; v1.x = p.x - a.x; v1.y = p.y - a.y; v1.z = p.z - a.z; v2.x = p.x - b.x; v2.y = p.y - b.y; v2.z = p.z - b.z; v3.x = p.x - c.x; v3.y = p.y - c.y; v3.z = p.z - c.z; v1 = NormalizeVector(v1); v2 = NormalizeVector(v2); v3 = NormalizeVector(v3); total_angles += float(acos(Dot_Product(v1,v2))); total_angles += float(acos(Dot_Product(v2,v3))); total_angles += float(acos(Dot_Product(v3,v1))); // allow a small margin because of the limited precision of // floating point math. if (fabs(total_angles-2*PI) <= 0.005) return (TRUE); return(FALSE);}float intersect(Point pOrigin, Vector pNormal, Point rOrigin, Vector rVector){ float d = -((pNormal.x * pOrigin.x) + (pNormal.y * pOrigin.y) + (pNormal.z * pOrigin.z)); float numer = ((pNormal.x * rOrigin.x) + (pNormal.y * rOrigin.y) + (pNormal.z * rOrigin.z) + d); float denom = ((pNormal.x * rVector.x) + (pNormal.y * rVector.y) + (pNormal.z * rVector.z)); return -(numer / denom);}float intersectSphere(Point rO, Vector rV, Point sO, float sR){ Vector Q; Q.x = sO.x - rO.x; Q.y = sO.y - rO.y; Q.z = sO.z - rO.z; float c = float(sqrt((Q.x * Q.x) + (Q.y * Q.y) + (Q.z * Q.z))); float v; v = ((Q.x * rV.x) + (Q.y * rV.y) + (Q.z * rV.z)); float d = (sR * sR) - ((c * c) + (v * v)); // If there was no intersection, return -1 if (d < 0.0) return -1.0; // Return the distance to the [first] intersecting point return v - float(sqrt(d));} Point closestPointOnLine(Point a, Point b, Point p){ // Determine t (the length of the vector from ‘a’ to ‘p’) Vector c = p - a; Vector V = b - a; float d = Distance(a, b); float t = ((V.x * c.x) + (V.y + c.y) + (V.z + c.z)); // Check to see if ‘t’ is beyond the extents of the line segment if (t < 0) return a; if (t > 0) return b; // Return the point between ‘a’ and ‘b’ V = NormalizeVector(V); V *= t; V += a; return V;}Point closestPointOnTriangle(Point a, Point b, Point c, Point p){ Point Rab = closestPointOnLine(a, b, p); Point Rbc = closestPointOnLine(b, c, p); Point Rca = closestPointOnLine(c, a, p); float dab, dbc, dca; dab = Distance(Rab, p); dbc = Distance(Rbc, p); dca = Distance(Rca, p); if(dab <= dbc && dab <= dca) return Rab; if(dbc <= dab && dbc <= dca) return Rbc; if(dca <= dab && dca <= dbc) return Rca; return Rab;}Vector CalculateNormal(Point p1, Point p2, Point p3){ float qX, qY, qZ, pX, pY, pZ; Vector normal; qX = p2.x - p1.x; qY = p2.y - p1.y; qZ = p2.z - p1.z; pX = p3.x - p1.x; pY = p3.y - p1.y; pZ = p3.z - p1.z; normal.x = (pY * qZ) - (pZ * qY); normal.y = (pZ * qX) - (pX * qZ); normal.z = (pX * qY) - (pY * qX); return normal;}PQBoundingBox *GetPolyList(PQBoundingBoxList *list){ PQBoundingBox *box; static PQBoundingBoxList *tmp=NULL; float x = camera->GetX(); float y = camera->GetY(); float z = camera->GetZ(); if (tmp == NULL) tmp = list; while(tmp != NULL) { // Get current bounding box from the list box = tmp->ptr; tmp = tmp->next; // Check to see if our location is withing the bb if ( x >= box->min.x && x <= box->max.x && y >= box->min.y && y <= box->max.y && z >= box->min.z && z <= box->max.z ) return box; } return NULL;}void CollideWithWorld(Point &sourcePoint, Vector velocityVector){ float DistanceToTravel = float(sqrt((velocityVector.x * velocityVector.x) + (velocityVector.y * velocityVector.y) + (velocityVector.z * velocityVector.z))); if(DistanceToTravel < EPSILON) { sourcePoint += velocityVector; return;//sourcePoint; } Point destinationPoint = sourcePoint + velocityVector; //PQBoundingBoxList *list; // pointer to linked list of bounding boxes PQBoundingBox *box; // pointer to bounding box //list = bblist; box = GetPolyList(bblist); if(box == NULL) { sourcePoint += velocityVector; return;// sourcePoint; } bool firstTimeThrough = true; float NearDistance = -1.0f; Poly nearestCollider; Point nearestIntersectionPoint; Point nearestPolygonIntersectionPoint; while (box != NULL) { for(int i = 0; i < box->numtriangles; i++) { Point pOrigin; Point p1,p2,p3; Poly poly1; p1.x = box->triangles[0].x; p1.y = box->triangles[0].y;<br> p1.z = box->triangles[0].z;<br> pOrigin = p1;<br> p2.x = box->triangles[1].x;<br> p2.y = box->triangles[1].y;<br> p2.z = box->triangles[1].z;<br> p3.x = box->triangles[2].x;<br> p3.y = box->triangles[2].y;<br> p3.z = box->triangles[2].z;<br> poly1.c1 = p1;<br> poly1.c2 = p2;<br> poly1.c3 = p3;<br> Vector pNormal = CalculateNormal(p1, p2, p3);<br> float pDist = intersect(sourcePoint, -pNormal, pOrigin, pNormal);<br><br> Point planeIntersectionPoint;<br> Point ellipsoidIntersectionPoint;<br><br> Vector directionalRadius = -pNormal * radiusVector;<br> float radius = float(sqrt((directionalRadius.x * directionalRadius.x) +<br> (directionalRadius.y * directionalRadius.y) +<br> (directionalRadius.z * directionalRadius.z)));<br> if(fabs(pDist) <= radius)<br> {<br> Vector temp = -pNormal * pDist;<br> planeIntersectionPoint = sourcePoint + temp;<br> }<br> else<br> {<br> Vector temp = -pNormal * radius;<br> ellipsoidIntersectionPoint = sourcePoint + temp;<br> float t = intersect(ellipsoidIntersectionPoint, velocityVector, pOrigin, pNormal);<br> Vector V = velocityVector * t;<br><br> // Added: planeIntersectionPoint = ellipsoidIntersectionPoint + V;<br> planeIntersectionPoint = ellipsoidIntersectionPoint + V;<br><br> }<br> Point polygonIntersectionPoint = planeIntersectionPoint;<br><br> if(CheckPointInTriangle(planeIntersectionPoint, p1, p2, p3))<br> {<br> polygonIntersectionPoint = closestPointOnTriangle(p1, p2, p3, planeIntersectionPoint);<br> }<br><br> float t = intersectSphere(sourcePoint, radiusVector, polygonIntersectionPoint, radius);<br><br> if(t >= 0.0f && t <= DistanceToTravel)<br> {<br> Vector V;<br> V = NormalizeVector(-velocityVector) * t;<br> Vector intersectPoint = polygonIntersectionPoint + V;<br><br> if(firstTimeThrough || t < NearDistance)<br> {<br> NearDistance = t;<br> nearestCollider = poly1;<br> nearestIntersectionPoint = intersectPoint;<br> nearestPolygonIntersectionPoint = polygonIntersectionPoint;<br> firstTimeThrough = false;<br> }<br> }<br> }<br> box = GetPolyList(bblist);<br> }<br> if(firstTimeThrough)<br> {<br> sourcePoint += velocityVector;<br> return;// sourcePoint;<br> }<br> Vector V = NormalizeVector(velocityVector);<br> V *= float(NearDistance - EPSILON);<br> sourcePoint += V;<br><br> Point slidePlaneOrigin = nearestPolygonIntersectionPoint;<br> Vector slidePlaneNormal = nearestPolygonIntersectionPoint;<br> slidePlaneNormal -= sourcePoint;<br><br> float time = intersect(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);<br><br> slidePlaneNormal = NormalizeVector(slidePlaneNormal) * time;<br><br> Vector destinationProjectionNormal = slidePlaneNormal;<br><br> Point newDestinationPoint = destinationPoint + destinationProjectionNormal;<br><br> Vector newVelocityVector = newDestinationPoint - nearestPolygonIntersectionPoint;<br><br> CollideWithWorld(sourcePoint, newVelocityVector);<br> //return sourcePoint;<br>}<br><br>Point Collision_Detection(Point &sourcePoint, Vector velocityVector, Vector gravityVector)<br>{<br> //Point destinationPoint;<br> //eventually<br> velocityVector += gravityVector;<br> CollideWithWorld(sourcePoint, velocityVector);<br> return sourcePoint;<br>}<br></pre><br><br>Edited by - rgirard413 on November 6, 2001 2:17:04 PM </i> <br><br>Edited by - rgirard413 on November 6, 2001 2:17:27 PM
This topic is closed to new replies.
Advertisement
Popular Topics
Advertisement