{ This Code Was Created By Jan Koci 2001 Visit My Site At koci.opengl.cz } unit K3dMath; interface uses OpenGL, Math; type TPoint3D=record x: TGLFloat; y: TGLFloat; z: TGLFloat; end; function CalcCollVector(Vect1,Vect2,Vect3:TPoint3D): TPoint3D; function CalcCollVector2(Vect1,Vect2:TPoint3D): TPoint3D; function NormalizeVector(Vect1: TPoint3d): TPoint3d; function AngleBetweenVectors(Vector1,Vector2: TPoint3D): TGLFloat; function Magnitude(vNormal: TPoint3D): TGLFloat; function IntersectedPolygon(Poly1, Poly2, Poly3, Line1, Line2: TPoint3D{; VerticeCount: TGLInt}): boolean; implementation /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns a vector between 3 points ///// /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function CalcCollVector(Vect1,Vect2,Vect3:TPoint3D): TPoint3D; var longi,vx1,vy1,vz1,vx2,vy2,vz2:TGLFloat; VectRes:TPoint3D; begin vx1:=Vect1.x-Vect2.x;vy1:=Vect1.y-Vect2.y;vz1:=Vect1.z-Vect2.z; vx2:=Vect2.x-Vect3.x;vy2:=Vect2.y-Vect3.y;vz2:=Vect2.z-Vect3.z; with VectRes do begin x:=vy1*vz2 - vz1*vy2; y:=vz1*vx2 - vx1*vz2; z:=vx1*vy2 - vy1*vx2; //Optimalization vector longi:=sqrt(sqr (x) + sqr(y) + sqr(z)); if longi>0 then //avoid zero division error x:=x/longi;y:=y/longi;z:=z/longi; end; Result := VectRes; end; /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns a vector between 2 points ///// /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function CalcCollVector2(Vect1,Vect2:TPoint3D): TPoint3D; var longi,vx1,vy1,vz1{,vx2,vy2,vz2}:TGLFloat; VectRes:TPoint3D; begin vx1:=Vect1.x-Vect2.x; vy1:=Vect1.y-Vect2.y; vz1:=Vect1.z-Vect2.z; with VectRes do begin x:=vx1; y:=vy1; z:=vz1; //Optimalization vector longi:=sqrt(sqr (x) + sqr(y) + sqr(z)); if longi>0 then //avoid zero division error x:=x/longi;y:=y/longi;z:=z/longi; end; Result := VectRes; end; /////////////////////////////////////// NORMALIZE VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// Normalize vector ///// /////////////////////////////////////// VECTOR \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function NormalizeVector(Vect1: TPoint3d): TPoint3d; var longi: TGLFloat; begin with Vect1 do begin //Optimalization vector longi:=sqrt(sqr (x) + sqr(y) + sqr(z)); if longi>0 then //avoid zero division error x:=x/longi;y:=y/longi;z:=z/longi; // x := x/Magnitude(Vect) end; Result := Vect1; end; /////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the distance between a plane and the origin ///// /////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function PlaneDistance(Normal, Point: TPoint3D): TGLFloat; var distance: TGLFLoat; // This variable holds the distance from the plane tot he origin begin distance := - ((Normal.x * Point.x) + (Normal.y * Point.y) + (Normal.z * Point.z)); // Basically, the negated dot product of the normal of the plane and the point. (More about the dot product in another tutorial) Result := distance; end; /////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks to see if a line intersects a plane ///// /////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function IntersectedPlane(Poly1, Poly2, Poly3, Line1, Line2: TPoint3D): Boolean; var distance1, distance2: TGLFloat; // The distances from the 2 points of the line from the plane distance: TGLFLoat; vNormal: TPoint3D; OriginDistance: TGLFloat; begin // We need to get the normal of our plane to go any further vNormal := CalcCollVector(Poly1,Poly2,Poly3); ////////////////////////// // Let's find the distance our plane is from the origin. We can find this value // from the normal to the plane (polygon) and any point that lies on that plane (Any vertice) OriginDistance := PlaneDistance(vNormal, Poly1); /////////// // Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane) distance1 := ((vNormal.x * Line1.x) + // Ax + (vNormal.y * Line1.y) + // By + (vNormal.z * Line1.z)) + originDistance; // Cz + D // Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane) distance2 := ((vNormal.x * Line2.x) + // Ax + (vNormal.y * Line2.y) + // By + (vNormal.z * Line2.z)) + originDistance; // Cz + D // Now that we have 2 distances from the plane, if we times them together we either // get a positive or negative number. If it's a negative number, that means we collided! // This is because the 2 points must be on either side of the plane (IE. -1 * 1 = -1). distance := distance1 * distance2; if (distance) <= 0 then // Check to see if both point's distances are both negative or both positive Result := false // Return false if each point has the same sign. -1 and 1 would mean each point is on either side of the plane. -1 -2 or 3 4 wouldn't... else Result := true; // The line intersected the plane, Return TRUE end; /////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This computers the dot product of 2 vectors ///// /////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function Dot(vVector1, vVector2: TPoint3D): TGLFloat; begin Result := ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) ); // The dot product is this equation: V1.V2 = (V1.x * V2.x + V1.y * V2.y + V1.z * V2.z) // In math terms, it looks like this: V1.V2 = ||V1|| ||V2|| cos(theta) // The '.' means DOT. The || || is magnitude. So the magnitude of V1 times the magnitude // of V2 times the cosine of the angle. It seems confusing now, but it will become more clear. // This function is used for a ton of things, which we will cover in other tutorials. // For this tutorial, we use it to compute the angle between 2 vectors. If the vectors // are normalize, the dot product returns the cosine of the angle between the 2 vectors. // What does that mean? Well, it doesn't return the actual angle, it returns the value of: // cos(angle). Well, what if we want to get the actual angle? Then we use the arc cosine. // There is more on this in the below function AngleBetweenVectors(). Let's give some // applications of using the dot product. How would you tell if the angle between the // 2 vectors is perpendicular (90 degrees)? Well, if we normalize the vectors we can // get rid of the ||V1|| * ||V2|| in front, which just leaves us with: cos(theta). // If a vector is normalize, it's magnitude is 1, so it would be: 1 * 1 * cos(theta) , // which is pointless, so we discard that part of the equation. So, What is the cosine of 90? // If you punch it in your calculator you will find that it's 0. So that means // if the dot product of 2 angles is 0, then they are perpendicular. What we did in // our mind is take the arc cosine of 0, which is 90 (or PI/2 in radians). More on this below. end; /////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the intersection point of the line that intersects the plane ///// /////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function IntersectionPoint(vNormal, Line1, Line2: TPoint3D; distance: TGLFLoat): TPoint3D; var vPoint, vLineDir: TPoint3D; // Variables to hold the point and the line's direction Numerator, Denominator, dist: TGLFloat; begin //pocita bod, na trouhelniku, ktery protina cara // Here comes the confusing part. We need to find the 3D point that is actually // on the plane. Here are some steps to do that: // 1) First we need to get the vector of our line, Then normalize it so it's a length of 1 vLineDir := CalcCollVector2(Line2,Line1); // Get the Vector of the line // 2) Use the plane equation (distance = Ax + By + Cz + D) to find the distance from one of our points to the plane. // Here I just chose a arbitrary point as the point to find that distance. You notice we negate that // distance. We negate the distance because we want to eventually go BACKWARDS from our point to the plane. // By doing this is will basically bring us back to the plane to find our intersection point. Numerator := - (vNormal.x * Line1.x + // Use the plane equation with the normal and the line vNormal.y * Line1.y + vNormal.z * Line1.z + distance); // 3) If we take the dot product between our line vector and the normal of the polygon, // this will give us the cosine of the angle between the 2 (since they are both normalized - length 1). // We will then divide our Numerator by this value to find the offset towards the plane from our arbitrary point. Denominator := Dot(vNormal, vLineDir); // Get the dot product of the line's vector and the normal of the plane // Since we are using division, we need to make sure we don't get a divide by zero error // If we do get a 0, that means that there are INFINATE points because the the line is // on the plane (the normal is perpendicular to the line - (Normal.Vector = 0)). // In this case, we should just return any point on the line. if( Denominator = 0) then begin // Check so we don't divide by zero Result := Line1; end else begin // Return an arbitrary point on the line // We divide the (distance from the point to the plane) by (the dot product) // to get the distance (dist) that we need to move from our arbitrary point. We need // to then times this distance (dist) by our line's vector (direction). When you times // a scalar (single number) by a vector you move along that vector. That is what we are // doing. We are moving from our arbitrary point we chose from the line BACK to the plane // along the lines vector. It seems logical to just get the numerator, which is the distance // from the point to the line, and then just move back that much along the line's vector. // Well, the distance from the plane means the SHORTEST distance. What about in the case that // the line is almost parallel with the polygon, but doesn't actually intersect it until half // way down the line's length. The distance from the plane is short, but the distance from // the actual intersection point is pretty long. If we divide the distance by the dot product // of our line vector and the normal of the plane, we get the correct length. Cool huh? dist := Numerator / Denominator; // Divide to get the multiplying (percentage) factor // Now, like we said above, we times the dist by the vector, then add our arbitrary point. // This essentially moves the point along the vector to a certain distance. This now gives // us the intersection point. Yay! vPoint.x := (Line1.x + (vLineDir.x * dist)); vPoint.y := (Line1.y + (vLineDir.y * dist)); vPoint.z := (Line1.z + (vLineDir.z * dist)); Result := vPoint; // Return the intersection point end; end; /////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This returns the magnitude of a normal (or any other vector) ///// /////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function Magnitude(vNormal: TPoint3D): TGLFloat; begin // This will give us the magnitude or "Norm" as some say, of our normal. // Here is the equation: magnitude = sqrt(V.x^2 + V.y^2 + V.z^2) Where V is the vector Result := sqrt( (vNormal.x * vNormal.x) + (vNormal.y * vNormal.y) + (vNormal.z * vNormal.z) ); end; /////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* ///// ///// This checks to see if a point is inside the ranges of a polygon ///// /////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\* function AngleBetweenVectors(Vector1,Vector2: TPoint3D): TGLFloat; var dotProduct: TGLFloat; vectorsMagnitude: TGLFloat; Angle: TGLFloat; begin // Remember, above we said that the Dot Product of returns the cosine of the angle // between 2 vectors? Well, that is assuming they are unit vectors (normalize vectors). // So, if we don't have a unit vector, then instead of just saying arcCos(DotProduct(A, B)) // We need to divide the dot product by the magnitude of the 2 vectors multiplied by each other. // Here is the equation: arc cosine of (V . W / || V || * || W || ) // the || V || means the magnitude of V. This then cancels out the magnitudes dot product magnitudes. // But basically, if you have normalize vectors already, you can forget about the magnitude part. // Get the dot product of the vectors dotProduct := Dot(Vector1, Vector2); // Get the product of both of the vectors magnitudes vectorsMagnitude := Magnitude(Vector1) * Magnitude(Vector2) ; // Return the arc cosine of the (dotProduct / vectorsMagnitude) which is the angle in RADIANS. // (IE. PI/2 radians = 90 degrees PI radians = 180 degrees 2*PI radians = 360 degrees) // To convert radians to degress use this equation: radians * (PI / 180) // TO convert degrees to radians use this equation: degrees * (180 / PI) Angle := ArcCos(dotProduct / vectorsMagnitude); Result := RadToGrad(Angle); //convert radians to degrees end; function InsidePolygon(vIntersection,Poly1, Poly2, Poly3: TPoint3D): boolean; var Angle, Angle1, Angle2, Angle3: TGLFloat; // Initialize the angle vA, vB: TPoint3D; // Create temp vectors begin // Just because we intersected the plane, doesn't mean we were anywhere near the polygon. // This functions checks our intersection point to make sure it is inside of the polygon. // This is another tough function to grasp at first, but let me try and explain. // It's a brilliant method really, what it does is create triangles within the polygon // from the intersection point. It then adds up the inner angle of each of those triangles. // If the angles together add up to 360 degrees (or 2 * PI in radians) then we are inside! // If the angle is under that value, we must be outside of polygon. To further // understand why this works, take a pencil and draw a perfect triangle. Draw a dot in // the middle of the triangle. Now, from that dot, draw a line to each of the vertices. // Now, we have 3 triangles within that triangle right? Now, we know that if we add up // all of the angles in a triangle we get 360 right? Well, that is kinda what we are doing, // but the inverse of that. Say your triangle is an isosceles triangle, so add up the angles // and you will get 360 degree angles. 90 + 90 + 90 is 360. ////Angle1 vA := CalcCollVector2(Poly1,vIntersection); // Subtract the intersection point from the current vertex vB := CalcCollVector2(Poly2, vIntersection); // Subtract the point from the next vertex Angle1 := AngleBetweenVectors(vA, vB); // Find the angle between the 2 vectors and add them all up as we go along ////Angle2 vA := CalcCollVector2(Poly2,vIntersection); vB := CalcCollVector2(Poly3, vIntersection); Angle2 := AngleBetweenVectors(vA, vB); ////Angle3 vA := CalcCollVector2(Poly3,vIntersection); vB := CalcCollVector2(Poly1, vIntersection); Angle3 := AngleBetweenVectors(vA, vB); // Now that we have the total angles added up, we need to check if they add up to 360 degrees. // Since we are using the dot product, we are working in radians, so we check if the angles // equals 2*PI. We defined PI in 3DMath.h. You will notice that we use a MATCH_FACTOR // in conjunction with our desired degree. This is because of the inaccuracy when working // with floating point numbers. It usually won't always be perfectly 2 * PI, so we need // to use a little twiddling. I use .9999, but you can change this to fit your own desired accuracy. Angle := Angle1 + Angle2 + Angle3; if(Angle >= 360) then // If the angle is greater than 2 PI, (360 degrees) Result := TRUE // The point is inside of the polygon else Result := FALSE; // If you get here, it obviously wasn't inside the polygon, so Return FALSE end; function IntersectedPolygon(Poly1, Poly2, Poly3, Line1, Line2: TPoint3D): boolean; var vNormal: TPoint3D; OriginDistance: TGLFloat; vIntersection: TPoint3D; begin // First we check to see if our line intersected the plane. If this isn't true // there is no need to go on, so return false immediately. // We pass in address of vNormal and originDistance so we only calculate it once // Reference if (IntersectedPlane(Poly1, Poly2, Poly3, Line1, Line2)) then begin Result := false; end else begin // Now that we have our normal and distance passed back from IntersectedPlane(), // we can use it to calculate the intersection point. The intersection point // is the point that actually is ON the plane. It is between the line. We need // this point test next, if we are inside the polygon. To get the I-Point, we // give our function the normal of the plan, the points of the line, and the originDistance. vNormal := CalcCollVector(Poly1,Poly2,Poly3); ////////////////////////// // Let's find the distance our plane is from the origin. We can find this value // from the normal to the plane (polygon) and any point that lies on that plane (Any vertice) OriginDistance := PlaneDistance(vNormal, Poly1); vIntersection := IntersectionPoint(vNormal, Line1, Line2, originDistance); // Now that we have the intersection point, we need to test if it's inside the polygon. // To do this, we pass in : // (our intersection point, the polygon, and the number of vertices our polygon has) if (InsidePolygon(vIntersection, Poly1, Poly2, Poly3)) then begin Result := true; // We collided! Return success end else begin // If we get here, we must have NOT collided Result := false; // There was no collision, so return false end; //end if (InsidePolygon(vIntersection, Poly1, Poly2, Poly3)) end; //end if (IntersectedPlane(Poly1, Poly2, Poly3, Line1, Line2)) end; /////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// * // Now, instead of just testing against the plane, we take it a step further // and test if we actually hit the polygon. This is a more usable collision. // We give our function the polygon, the line to test with, and the number of vertices of our polygon { procedure CalcCollision(val: TGLInt); begin bCollided[val] := IntersectedPolygon(Point1[val], Point2[val], Point3[val] , Line1, Line2, 3); // Below we draw the line that the polygon will be colliding with. (RED is collision) glPushMatrix; glDisable(GL_TEXTURE_2D); if bCollided[1] then begin // If we collided, change the color of the line to illustrate this. glColor3f(1, 0, 0); // Make the line RED if we collided with the triangle's plane end else begin glColor3f(1, 1, 0); // Make the line YELLOW if we didn't collide end; glBegin(GL_LINES); // This is our BEGIN to draw Line glVertex3f(Line1.x, Line1.y, Line1.z); glVertex3f(Line2.x, Line2.y, Line2.z); glEnd; glPopMatrix; end; } end.