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
Author
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?
Author
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
Author
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
