空间算法总结

////////////////////////////////////////////////////////////////////////////////////

// ***.h

#define PI 3.1415926535897932     // This is our famous PI
#define TWOPI 6.28318530717958
#define PIDIV2 1.57079632679489

#define DTOR (PI/180.0f)
#define RTOD (180.0f/PI)

#define SQR(x) (x*x)

#define BEHIND  0
#define INTERSECTS 1
#define FRONT  2

 

// This is our basic 3D point/vector class
struct CVector3
{
public:
 
 // A default constructor
 CVector3() {}

 // This is our constructor that allows us to initialize our data upon creating an instance
 CVector3(float X, float Y, float Z)
 {
  x = X; y = Y; z = Z;
 }

 // Here we overload the + operator so we can add vectors together
 CVector3 operator+(CVector3 vVector)
 {
  // Return the added vectors result.
  return CVector3(vVector.x + x, vVector.y + y, vVector.z + z);
 }

 // Here we overload the - operator so we can subtract vectors
 CVector3 operator-(CVector3 vVector)
 {
  // Return the subtracted vectors result
  return CVector3(x - vVector.x, y - vVector.y, z - vVector.z);
 }
 
 // Here we overload the * operator so we can multiply by scalars
 CVector3 operator*(float num)
 {
  // Return the scaled vector
  return CVector3(x * num, y * num, z * num);
 }

 // Here we overload the / operator so we can divide by a scalar
 CVector3 operator/(float num)
 {
  // Return the scale vector
  return CVector3(x / num, y / num, z / num);
 }

 // Dot 点积 2007.3.26 ml
//  float dot(CVector3 vVector)
//  {
//   return (x*vVector.x + y*vVector.y + z*vVector.z);
//  }

 float x, y, z;      
};
class lp3DPointArray : public CTypedPtrArray<CPtrArray, CVector3*> {};

#define VERTEX CVector3

//由3DS.cpp文件中提取出来的几个函数
CVector3 Vector(CVector3 vPoint1, CVector3 vPoint2);

CVector3 AddVector(CVector3 vVector1, CVector3 vVector2);

CVector3 DivideVectorByScaler(CVector3 vVector1, float Scaler);

// This returns the absolute value of "num"
float Absolute(float num);

// This returns a perpendicular vector from 2 given vectors by taking the cross product.
// 下面的函数返回两个矢量的叉积
CVector3 Cross(CVector3 vVector1, CVector3 vVector2);

// This returns the dot product between 2 vectors
// 下面的函数返回两个矢量的点积
float Dot(CVector3 vVector1, CVector3 vVector2);

// This returns the magnitude of a normal (or any other vector)
// 计算矢量的模
float Magnitude(CVector3 vNormal);

// This returns a normalize vector (A vector exactly of length 1)
// 标准化一个矢量[0-1]
CVector3 Normalize(CVector3 vNormal);

// This returns the normal of a polygon (The direction the polygon is facing)
// 多边形面的法线
CVector3 Normal(CVector3 vPolygon[]);

// This returns the distance between 2 3D points
// 两点间的距离
float Distance(CVector3 vPoint1, CVector3 vPoint2);

// This returns the point on the line segment vA_vB that is closest to point vPoint
// 计算线段vA_vB上距离点vPoint最近的点
CVector3 ClosestPointOnLine(CVector3 vA, CVector3 vB, CVector3 vPoint);

// This returns the distance the plane is from the origin (0, 0, 0)
// It takes the normal to the plane, along with ANY point that lies on the plane (any corner)
// 点到平面的距离
float PlaneDistance(CVector3 Normal, CVector3 Point);

// This takes a triangle (plane) and line and returns true if they intersected
// 检测光线与三角形(平面)是否相交
bool IntersectedPlane(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance);

// This returns the angle between 2 vectors
// 两个矢量间的夹角
double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2);

// This returns an intersection point of a polygon and a line (assuming intersects the plane)
// 类似检测光线与三角形(平面)是否相交
CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance);

// This returns true if the intersection point is inside of the polygon
// 判断一个点是否在一个多边形面内 <如果是凹多边形面，夹角求和＝＝360度计算将存在误差，不提倡使用此函数>
bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount);

// Use this function to test collision between a line and polygon
// 检测一条线和平面是否相交
bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount);

// This function classifies a sphere according to a plane. (BEHIND, in FRONT, or INTERSECTS)
// 判断一个球相对一个平面的位置关系 ： 前面、后面或相交
int ClassifySphere(CVector3 &vCenter,
       CVector3 &vNormal, CVector3 &vPoint, float radius, float &distance);

// This takes in the sphere center, radius, polygon vertices and vertex count.
// This function is only called if the intersection point failed.  The sphere
// could still possibly be intersecting the polygon, but on it's edges.
// 球面与平面相交（平面切球面的情况）
bool EdgeSphereCollision(CVector3 &vCenter,
       CVector3 vPolygon[], int vertexCount, float radius);

// This returns true if the sphere is intersecting with the polygon.
// 检测球体与平面的碰撞
bool SpherePolygonCollision(CVector3 vPolygon[],
       CVector3 &vCenter, int vertexCount, float radius);

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *

// This returns the offset the sphere needs to move in order to not intersect the plane
// 返回一个球为了不与平面碰撞需要平移的数值
CVector3 GetCollisionOffset(CVector3 &vNormal, float radius, float distance);

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *

// 点到直线的距离 [毕达哥拉斯定理]
float PointToLineDistance(VERTEX vtQ/*点*/,VERTEX VtP1/*直线的第一个点*/,VERTEX vtP2/*直线的第二个点*/);

// 计算向量A在向量B上的投影
CVector3 PointAProjPointB(CVector3 A, CVector3 B);

// 计算向量A在向量B上的投影的模
float MagnitudeOfPointAProjPointB(CVector3 A, CVector3 B);

 

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

***.cpp

 

//***********************************************************************//
//                   //
//  - "Talk to me like I'm a 3 year old!" Programming Lessons -   //
//                                                                       //
//  $Author:  DigiBen  digiben@gametutorials.com    //
//                   //
//  $Program:  CameraWorldCollision        //
//                   //
//  $Description: Shows how to check if camera and world collide  //
//                   //
//  $Date:   1/23/02            //
//                   //
//***********************************************************************//
#include <float.h> // This is so we can use _isnan() for acos()
#include "Mymath.h"

/////////////////////////////////////// ABSOLUTE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the absolute value of the number passed in
/////
/////////////////////////////////////// ABSOLUTE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

float Absolute(float num)
{
 // If num is less than zero, we want to return the absolute value of num.
 // This is simple, either we times num by -1 or subtract it from 0.
 if(num < 0)
  return (0 - num);

 // Return the original number because it was already positive
 return num;
}

/////////////////////////////////////// CROSS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns a perpendicular vector from 2 given vectors by taking the cross product.
/////
/////////////////////////////////////// CROSS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
            
CVector3 Cross(CVector3 vVector1, CVector3 vVector2)
{
 CVector3 vNormal;         // The vector to hold the cross product

 // The X value for the vector is:  (V1.y * V2.z) - (V1.z * V2.y)             // Get the X value
 vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
              
 // The Y value for the vector is:  (V1.z * V2.x) - (V1.x * V2.z)
 vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
              
 // The Z value for the vector is:  (V1.x * V2.y) - (V1.y * V2.x)
 vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));

 return vNormal;          // Return the cross product (Direction the polygon is facing - Normal)
}

/////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the magnitude of a normal (or any other vector)
/////
/////////////////////////////////////// MAGNITUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

float Magnitude(CVector3 vNormal)
{
 // 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

 return (float)sqrt( (vNormal.x * vNormal.x) + (vNormal.y * vNormal.y) + (vNormal.z * vNormal.z) );
}

/////////////////////////////////////// NORMALIZE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns a normalize vector (A vector exactly of length 1)
/////
/////////////////////////////////////// NORMALIZE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

CVector3 Normalize(CVector3 vNormal)
{
 float magnitude = Magnitude(vNormal);    // Get the magnitude of our normal

 // Now that we have the magnitude, we can divide our normal by that magnitude.
 // That will make our normal a total length of 1.  This makes it easier to work with too.

 vNormal.x /= magnitude;        // Divide the X value of our normal by it's magnitude
 vNormal.y /= magnitude;        // Divide the Y value of our normal by it's magnitude
 vNormal.z /= magnitude;        // Divide the Z value of our normal by it's magnitude

 // Finally, return our normalized normal.

 return vNormal;          // Return the new normal of length 1.
}

/////////////////////////////////////// NORMAL \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the normal of a polygon (The direction the polygon is facing)
/////
/////////////////////////////////////// NORMAL \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

CVector3 Normal(CVector3 vPolygon[])     
{              // Get 2 vectors from the polygon (2 sides), Remember the order!
 CVector3 vVector1 = vPolygon[2] - vPolygon[0];
 CVector3 vVector2 = vPolygon[1] - vPolygon[0];

 CVector3 vNormal = Cross(vVector1, vVector2);  // Take the cross product of our 2 vectors to get a perpendicular vector

 // Now we have a normal, but it's at a strange length, so let's make it length 1.

 vNormal = Normalize(vNormal);      // Use our function we created to normalize the normal (Makes it a length of one)

 return vNormal;          // Return our normal at our desired length
}

/////////////////////////////////// DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the distance between 2 3D points
/////
/////////////////////////////////// DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

float Distance(CVector3 vPoint1, CVector3 vPoint2)
{
 // This is the classic formula used in beginning algebra to return the
 // distance between 2 points.  Since it's 3D, we just add the z dimension:
 //
 // Distance = sqrt(  (P2.x - P1.x)^2 + (P2.y - P1.y)^2 + (P2.z - P1.z)^2 )
 //
 double distance = sqrt( (vPoint2.x - vPoint1.x) * (vPoint2.x - vPoint1.x) +
          (vPoint2.y - vPoint1.y) * (vPoint2.y - vPoint1.y) +
          (vPoint2.z - vPoint1.z) * (vPoint2.z - vPoint1.z) );

 // Return the distance between the 2 points
 return (float)distance;
}

////////////////////////////// CLOSEST POINT ON LINE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the point on the line vA_vB that is closest to the point vPoint
/////
////////////////////////////// CLOSEST POINT ON LINE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

CVector3 ClosestPointOnLine(CVector3 vA, CVector3 vB, CVector3 vPoint)
{
 // Create the vector from end point vA to our point vPoint.
 CVector3 vVector1 = vPoint - vA;

 // Create a normalized direction vector from end point vA to end point vB
    CVector3 vVector2 = Normalize(vB - vA);

 // Use the distance formula to find the distance of the line segment (or magnitude)
    float d = Distance(vA, vB);

 // Using the dot product, we project the vVector1 onto the vector vVector2.
 // This essentially gives us the distance from our projected vector from vA.
    float t = Dot(vVector2, vVector1);

 // If our projected distance from vA, "t", is less than or equal to 0, it must
 // be closest to the end point vA.  We want to return this end point.
    if (t <= 0)
  return vA;

 // If our projected distance from vA, "t", is greater than or equal to the magnitude
 // or distance of the line segment, it must be closest to the end point vB.  So, return vB.
    if (t >= d)
  return vB;
 
 // Here we create a vector that is of length t and in the direction of vVector2
    CVector3 vVector3 = vVector2 * t;

 // To find the closest point on the line segment, we just add vVector3 to the original
 // end point vA. 
    CVector3 vClosestPoint = vA + vVector3;

 // Return the closest point on the line segment
 return vClosestPoint;
}

/////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the distance between a plane and the origin
/////
/////////////////////////////////// PLANE DISTANCE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
         
float PlaneDistance(CVector3 Normal, CVector3 Point)
{ 
 float distance = 0;         // This variable holds the distance from the plane tot he origin

 // Use the plane equation to find the distance (Ax + By + Cz + D = 0)  We want to find D.
 // So, we come up with D = -(Ax + By + Cz)
              // Basically, the negated dot product of the normal of the plane and the point. (More about the dot product in another tutorial)
 distance = - ((Normal.x * Point.x) + (Normal.y * Point.y) + (Normal.z * Point.z));

 return distance;         // Return the distance
}

/////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This checks to see if a line intersects a plane 检测光线与平面是否相交
/////
/////////////////////////////////// INTERSECTED PLANE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
           
bool IntersectedPlane(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance)
{
 float distance1=0, distance2=0;      // The distances from the 2 points of the line from the plane
   
 vNormal = Normal(vPoly);       // We need to get the normal of our plane to go any further

 // 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 vertex)
 originDistance = PlaneDistance(vNormal, vPoly[0]);

 // Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane)

 distance1 = ((vNormal.x * vLine[0].x)  +     // Ax +
           (vNormal.y * vLine[0].y)  +     // Bx +
     (vNormal.z * vLine[0].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 * vLine[1].x)  +     // Ax +
           (vNormal.y * vLine[1].y)  +     // Bx +
     (vNormal.z * vLine[1].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).

 if(distance1 * distance2 >= 0)   // Check to see if both point's distances are both negative or both positive
    return 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...
     
 return true;       // The line intersected the plane, Return TRUE
}

/////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This computers the dot product of 2 vectors
/////
/////////////////////////////////// DOT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

float Dot(CVector3 vVector1, CVector3 vVector2)
{
 // 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)
 
    //    (V1.x * V2.x        +        V1.y * V2.y        +        V1.z * V2.z)
 return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}

/////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This checks to see if a point is inside the ranges of a polygon
/////
/////////////////////////////////// ANGLE BETWEEN VECTORS \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2)
{       
 // Get the dot product of the vectors
 float dotProduct = Dot(Vector1, Vector2);    

 // Get the product of both of the vectors magnitudes
 float vectorsMagnitude = Magnitude(Vector1) * Magnitude(Vector2) ;

 // Get the angle in radians between the 2 vectors
 double angle = acos( dotProduct / vectorsMagnitude );

 // Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinate
 if(_isnan(angle))
  return 0;
 
 // Return the angle in radians
 return( angle );
}

/////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the intersection point of the line that intersects the plane
/////
/////////////////////////////////// INTERSECTION POINT \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
           
CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance)
{
 CVector3 vPoint, vLineDir;     // Variables to hold the point and the line's direction
 double Numerator = 0.0, Denominator = 0.0, dist = 0.0;

 // 1)  First we need to get the vector of our line, Then normalize it so it's a length of 1
 vLineDir = vLine[1] - vLine[0];  // Get the Vector of the line
 vLineDir = Normalize(vLineDir);    // Normalize the lines vector

 // 2) Use the plane equation (distance = Ax + By + Cz + D) to find the
 // distance from one of our points to the plane.
 Numerator = - (vNormal.x * vLine[0].x +  // Use the plane equation with the normal and the line
       vNormal.y * vLine[0].y +
       vNormal.z * vLine[0].z + distance);

 // 3) If we take the dot product between our line vector and the normal of the polygon,
 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.0)      // Check so we don't divide by zero
  return vLine[0];      // Return an arbitrary point on the line

 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.
 vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
 vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
 vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));

 return vPoint;        // Return the intersection point
}

/////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This checks to see if a point is inside the ranges of a polygon
/////
/////////////////////////////////// INSIDE POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount)
{
 const double MATCH_FACTOR = 0.99;  // Used to cover up the error in floating point
 double Angle = 0.0;      // Initialize the angle
 CVector3 vA, vB;      // Create temp vectors
 
 for (int i = 0; i < verticeCount; i++)  // Go in a circle to each vertex and get the angle between
 { 
  vA = Poly[i] - vIntersection;   // Subtract the intersection point from the current vertex
            // Subtract the point from the next vertex
  vB = Poly[(i + 1) % verticeCount] - vIntersection;
            
  Angle += AngleBetweenVectors(vA, vB); // Find the angle between the 2 vectors and add them all up as we go along
 }
           
 if(Angle >= (MATCH_FACTOR * (2.0 * PI)) ) // If the angle is greater than 2 PI, (360 degrees)
  return true;       // The point is inside of the polygon
  
 return false;        // If you get here, it obviously wasn't inside the polygon, so Return FALSE
}

/////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This checks if a line is intersecting a polygon
/////
/////////////////////////////////// INTERSECTED POLYGON \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

bool IntersectedPolygon(CVector3 vPoly[], CVector3 vLine[], int verticeCount)
{
 CVector3 vNormal;
 float originDistance = 0;

 // First, make sure our line intersects the plane
          // Reference   // Reference
 if(!IntersectedPlane(vPoly, vLine,   vNormal,   originDistance))
  return false;

 // Now that we have our normal and distance passed back from IntersectedPlane(),
 // we can use it to calculate the intersection point. 
 CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);

 // Now that we have the intersection point, we need to test if it's inside the polygon.
 if(InsidePolygon(vIntersection, vPoly, verticeCount))
  return true;       // We collided!   Return success

 return false;        // There was no collision, so return false
}

///////////////////////////////// CLASSIFY SPHERE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This tells if a sphere is BEHIND, in FRONT, or INTERSECTS a plane, also it's distance
/////
///////////////////////////////// CLASSIFY SPHERE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

int ClassifySphere(CVector3 &vPos,
       CVector3 &vNormal, CVector3 &vPoint, float radius, float &distance)
{
 // First we need to find the distance our polygon plane is from the origin.
 float d = (float)PlaneDistance(vNormal, vPoint);

 // Here we use the famous distance formula to find the distance the center point
 // of the sphere is from the polygon's plane. 
 distance = (vNormal.x * vPos.x + vNormal.y * vPos.y + vNormal.z * vPos.z + d);

 // If the absolute value of the distance we just found is less than the radius,
 // the sphere intersected the plane.
 if(Absolute(distance) < radius)
  return INTERSECTS;
 // Else, if the distance is greater than or equal to the radius, the sphere is
 // completely in FRONT of the plane.
 else if(distance >= radius)
  return FRONT;
 
 // If the sphere isn't intersecting or in FRONT of the plane, it must be BEHIND
 return BEHIND;
}

///////////////////////////////// EDGE SPHERE COLLSIION \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns true if the sphere is intersecting any of the edges of the polygon
/////
///////////////////////////////// EDGE SPHERE COLLSIION \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
bool EdgeSphereCollision(CVector3 &vCenter,
       CVector3 vPolygon[], int vertexCount, float radius)
{
 CVector3 vPoint;

 // This function takes in the sphere's center, the polygon's vertices, the vertex count
 // and the radius of the sphere.  We will return true from this function if the sphere
 // is intersecting any of the edges of the polygon. 

 // Go through all of the vertices in the polygon
 for(int i = 0; i < vertexCount; i++)
 {
  // This returns the closest point on the current edge to the center of the sphere.
  vPoint = ClosestPointOnLine(vPolygon[i], vPolygon[(i + 1) % vertexCount], vCenter);
  
  // Now, we want to calculate the distance between the closest point and the center
  float distance = Distance(vPoint, vCenter);

  // If the distance is less than the radius, there must be a collision so return true
  if(distance < radius)
   return true;
 }

 // The was no intersection of the sphere and the edges of the polygon
 return false;
}

////////////////////////////// SPHERE POLYGON COLLISION \\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns true if our sphere collides with the polygon passed in
/////
////////////////////////////// SPHERE POLYGON COLLISION \\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
bool SpherePolygonCollision(CVector3 vPolygon[],
       CVector3 &vCenter, int vertexCount, float radius)
{
 // 1) STEP ONE - Finding the sphere's classification
 
 // Let's use our Normal() function to return us the normal to this polygon
 CVector3 vNormal = Normal(vPolygon);

 // This will store the distance our sphere is from the plane
 float distance = 0.0f;

 // This is where we determine if the sphere is in FRONT, BEHIND, or INTERSECTS the plane
 int classification = ClassifySphere(vCenter, vNormal, vPolygon[0], radius, distance);

 // If the sphere intersects the polygon's plane, then we need to check further
 if(classification == INTERSECTS)
 {
  // 2) STEP TWO - Finding the psuedo intersection point on the plane

  // Now we want to project the sphere's center onto the polygon's plane
  CVector3 vOffset = vNormal * distance;

  // Once we have the offset to the plane, we just subtract it from the center
  // of the sphere.  "vPosition" now a point that lies on the plane of the polygon.
  CVector3 vPosition = vCenter - vOffset;

  // 3) STEP THREE - Check if the intersection point is inside the polygons perimeter

  // If the intersection point is inside the perimeter of the polygon, it returns true.
  // We pass in the intersection point, the list of vertices and vertex count of the poly.
  if(InsidePolygon(vPosition, vPolygon, 3))
   return true; // We collided!
  else
  {
   // 4) STEP FOUR - Check the sphere intersects any of the polygon's edges

   // If we get here, we didn't find an intersection point in the perimeter.
   // We now need to check collision against the edges of the polygon.
   if(EdgeSphereCollision(vCenter, vPolygon, vertexCount, radius))
   {
    return true; // We collided!
   }
  }
 }

 // If we get here, there is obviously no collision
 return false;
}

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *

///////////////////////////////// GET COLLISION OFFSET \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*
/////
///// This returns the offset to move the center of the sphere off the collided polygon
/////
///////////////////////////////// GET COLLISION OFFSET \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*

CVector3 GetCollisionOffset(CVector3 &vNormal, float radius, float distance)
{
 CVector3 vOffset = CVector3(0, 0, 0);

 // Once we find if a collision has taken place, we need make sure the sphere
 // doesn't move into the wall.  In our app, the position will actually move into
 // the wall, but we check our collision detection before we render the scene, which
 // eliminates the bounce back effect it would cause.  The question is, how do we
 // know which direction to move the sphere back?  In our collision detection, we
 // account for collisions on both sides of the polygon.  Usually, you just need
 // to worry about the side with the normal vector and positive distance.  If
 // you don't want to back face cull and have 2 sided planes, I check for both sides.
 //
 // Let me explain the math that is going on here.  First, we have the normal to
 // the plane, the radius of the sphere, as well as the distance the center of the
 // sphere is from the plane.  In the case of the sphere colliding in the front of
 // the polygon, we can just subtract the distance from the radius, then multiply
 // that new distance by the normal of the plane.  This projects that leftover
 // distance along the normal vector.  For instance, say we have these values:
 //
 // vNormal = (1, 0, 0)  radius = 5  distance = 3
 //
 // If we subtract the distance from the radius we get: (5 - 3 = 2)
 // The number 2 tells us that our sphere is over the plane by a distance of 2.
 // So basically, we need to move the sphere back 2 units.  How do we know which
 // direction though?  This part is easy, we have a normal vector that tells us the
 // direction of the plane. 
 // If we multiply the normal by the left over distance we get:  (2, 0, 0)
 // This new offset vectors tells us which direction and how much to move back.
 // We then subtract this offset from the sphere's position, giving is the new
 // position that is lying right on top of the plane.  Ba da bing!
 // If we are colliding from behind the polygon (not usual), we do the opposite
 // signs as seen below:
 
 // If our distance is greater than zero, we are in front of the polygon
 if(distance > 0)
 {
  // Find the distance that our sphere is overlapping the plane, then
  // find the direction vector to move our sphere.
  float distanceOver = radius - distance;
  vOffset = vNormal * distanceOver;
 }
 else // Else colliding from behind the polygon
 {
  // Find the distance that our sphere is overlapping the plane, then
  // find the direction vector to move our sphere.
  float distanceOver = radius + distance;
  vOffset = vNormal * -distanceOver;
 }

 // There is one problem with check for collisions behind the polygon, and that
 // is if you are moving really fast and your center goes past the front of the
 // polygon, it will then assume you were colliding from behind and not let
 // you back in.  Most likely you will take out the if / else check, but I
 // figured I would show both ways in case someone didn't want to back face cull.

 // Return the offset we need to move back to not be intersecting the polygon.
 return vOffset;
}

/////// * /////////// * /////////// * NEW * /////// * /////////// * /////////// *
float PointToLineDistance(VERTEX vtQ/*点*/,VERTEX VtP1/*直线的第一个点*/,VERTEX vtP2/*直线的第二个点*/)
{
 float dis;
 VERTEX vttemp = vtQ - VtP1;

 VERTEX vtLine = vtP2 - VtP1;

 dis = Magnitude(vttemp)*Magnitude(vttemp) - Dot(vttemp,vtLine)/(Magnitude(vtLine)*Magnitude(vtLine));

 dis = sqrt(dis);
 return dis;
}

CVector3 PointAProjPointB(CVector3 A, CVector3 B)
{
 float dot = Dot(A,B);
 float res = (Magnitude(B)*Magnitude(B));
 float x = dot * B.x / res;
 float y = dot * B.y / res;
 float z = dot * B.z / res;

 return CVector3(x,y,z);
}

float MagnitudeOfPointAProjPointB(CVector3 A, CVector3 B)
{
 return Dot(A,B)/Magnitude(B);
}
/////////////////////////////////////////////////////////////////////////////////
//
// * QUICK NOTES *
//
// Nothing really new added to this file since the last collision tutorial.  We did
// however tweak the EdgePlaneCollision() function to handle the camera collision
// better around edges.
//
//
// Ben Humphrey (DigiBen)
// Game Programmer
// DigiBen@GameTutorials.com
// Co-Web Host of http://www.gametutorials.com/
//
//

//  下面的这些函数主要用来计算顶点的法向量，顶点的法向量主要用来计算光照
// 下面的宏定义计算一个矢量的长度
#define Mag(Normal) (sqrt(Normal.x*Normal.x + Normal.y*Normal.y + Normal.z*Normal.z))
// 下面的函数求两点决定的矢量
CVector3 Vector(CVector3 vPoint1, CVector3 vPoint2)
{ CVector3 vVector;       
 vVector.x = vPoint1.x - vPoint2.x;   
 vVector.y = vPoint1.y - vPoint2.y;   
 vVector.z = vPoint1.z - vPoint2.z;   
 return vVector;        
}
// 下面的函数两个矢量相加
CVector3 AddVector(CVector3 vVector1, CVector3 vVector2)
{ CVector3 vResult;       
 vResult.x = vVector2.x + vVector1.x;  
 vResult.y = vVector2.y + vVector1.y;  
 vResult.z = vVector2.z + vVector1.z;  
 return vResult;        
}
// 下面的函数处理矢量的缩放
CVector3 DivideVectorByScaler(CVector3 vVector1, float Scaler)
{ CVector3 vResult;       
 vResult.x = vVector1.x / Scaler;   
 vResult.y = vVector1.y / Scaler;   
 vResult.z = vVector1.z / Scaler;   
 return vResult;        
}