从0开发游戏引擎之碰撞检测模块底层实现

#pragma once
class Collider
{
private:
  static int num;
public:
  /2D碰撞
  static bool Intersects(GLLine &line_a, GLLine &line_b, Vector2 &hitPoint/*(Out_HIT_Point)*/);//线段和线段
  static bool Intersects(Vector2 &point, GLLine lines[], int linesNums);//点在多边形中(用射线)
  static bool Intersects(Vector2 &point, GLPolygon &poly);//点在多边形中(用叉乘法)              
  static bool Intersects(GLCircle &circle_a, GLCircle &circle_b);//圆与圆
  static bool Intersects(GLLine &line_a, GLCircle &circle,Vector2 &hitPoint);//线与圆
  static bool Intersects(GLLine line_a[], GLCircle &circle, int linesNum);//线与圆                   
  static bool Intersects(GLRay &ray_a, GLCircle &circle,/*_Out_*/ Vector2 &hitPoint);//射线与圆
  static bool Intersects(GLRay &ray_a, GLAABB &aabb,/*_Out_*/ Vector2 &hitPoint);//射线与AABB
  static bool Intersects(GLAABB &aabb_a, GLAABB &aabb_b);//AABB与AABB
  static bool Intersects(Vector2 point,RECT rect);
  static bool Intersects(GLPolygon &poly_a, GLPolygon &poly_b);//多边形与多边形
  /3D碰撞
  static bool Intersects(GLSphere sphere,GLPlane *plane,int PlansNum);  //球和多面体
  static bool Intersects(GLPoint &point, GLAABB3D &Aabb);         //点在Aabb内
  static bool Intersects(GLPoint &point, GLPlane *plane, int PlansNum); //点和面
  static bool Intersects(GLRay3D &ray,GLAABB3D &aabb,Vector3D &vcHit);  //射线和AABB3D
  static bool Intersects(GLRay3D &ray, GLLine3D &line, Vector3D &HitPoint);
  static bool Intersects(GLRay3D &ray, GLTrianglePlane &trianglePlane,Vector3D &HitPoint);//射线和三角面
  static bool Intersects(Vector3D &RayOrig, Vector3D &RayDir, Vector3D &v0, Vector3D v1, Vector3D v2,Vector3D &HitPoint);
  static bool Intersects(GLAABB3D &AABB_1, GLAABB3D &AABB_2);       //两个立方体
  static bool Intersects(GLSphere &sphere_a, GLTrianglePlane &triplane_a);    //球和三角面
  static bool Intersects(GLSphere &sphere_a, Vector3D &line_a, Vector3D &line_b); //球和线段
  static bool Intersects(GLSphere &sphere_a, GLSphere & sphere_b);        //球和球
  static bool Intersects(GLSphere & sphere, GLPlane &plane);            //球和面
  static bool Intersects(GLLine3D &line_a, GLAABB3D &aabb, Vector3D &hitPoint);   线段与Aabb
  static bool Intersects(GLLine3D &line, GLTrianglePlane &triangle, Vector3D &hitPoint);//线段和三角面
  Collider();
  ~Collider(); 
};

#include "Engine.h"
int Collider::num =0;
Collider::Collider()
{
}
bool Collider::Intersects(GLLine &line_a, GLLine &line_b, Vector2 &hitPoint)
{
  float x, y; // 这是质点的位置变量
  float vx, vy; // 质点的速度向量分量
  float s, t; // 线段方程的两个参数    
  float x_v1, x_v2, y_v1, y_v2; // 各个参量
  float x_v1b, x_v2b, y_v1b, y_v2b;
  x_v1 = line_a.m_vcDir.x*line_a.m_len;       //向量的长度a
  y_v1 = line_a.m_vcDir.y*line_a.m_len;
  x_v2 = line_b.m_vcDir.x*line_b.m_len;       //向量的长度b
  y_v2 = line_b.m_vcDir.y*line_b.m_len;
  x_v1b = line_a.m_vcOrig.x;
  y_v1b = line_a.m_vcOrig.y;
  x_v2b = line_b.m_vcOrig.x;
  y_v2b = line_b.m_vcOrig.y;
  float d = (x_v1*(-y_v2)) - ((-x_v2)*y_v1);// 计算d,d1和d2
  float d1 = ((x_v2b - x_v1b)*(-y_v2)) - ((-x_v2)*(y_v2b - y_v1b));
  float d2 = (x_v1*(y_v2b - y_v1b)) - ((x_v2b - x_v1b)*y_v1);
    if (abs(d) < 0.001f)                          // 如果等于零做近似处理,abs()用于求绝对值
    {d = 0.001f;}
    s = d1 / d;                               //计算参量s,t
    t = d2 / d;
    if (0.0f <= s && 1.0f >= s && 0.0f <= t && 1.0f >= t)         //判断是否发生碰撞
    {
      hitPoint = line_a.m_vcOrig+line_a.m_vcDir*(line_a.m_len*s);
      return true;
    }
    else
    return false; 
}
bool Collider::Intersects(Vector2 &point, GLLine lines[], int linesNums)
{
  static Vector2 hit;
  GLLine line;
  static float x=point.x;
  static float y=point.y;
  x += 1000;
  Vector2 yanC(x, y);
  line.Set(point, yanC);
  //Gizmo::drawLine(line, sColor(1, 1, 1, 1));
  for (int i = 0;i < linesNums;i++)
  {
    if (Intersects(line, lines[i], hit))
    {
      ++num;
    }
  }
  if (num <= 0)
  {num = 0;return false;}
  else if (num == 1)
  {num = 0;return true;}
  else if (num >= 2)
  {num = 0;return false;}
  return false;
}
bool Collider::Intersects(Vector2 &point, GLPolygon &poly)
{
  return Intersects(point, poly.m_line, poly.m_lineNum);
}
bool Collider::Intersects(Vector2 point,RECT rect)
{
  if (point.x > rect.left&&point.y > rect.top&&
    point.x < rect.right&&point.y < rect.bottom)
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLCircle &circle_a, GLCircle &circle_b)
{
  float distance = (circle_a.center - circle_b.center).length();
  // 如果两圆的圆心距小于或等于两圆半径和则认为发生碰撞
  if (distance <= circle_a.radius+circle_b.radius)
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLLine &line_a, GLCircle &circle, Vector2 &hitPoint)
{ 
  Vector2 d = line_a.m_vcOrig - circle.center;
  if (d.length2q() <= circle.radius*circle.radius)
  {return false;}
  float b = line_a.m_vcDir.dot(d);
  float c = d.dot(d) - circle.radius*circle.radius;
  float dt = b*b - c;
  if (dt < 0)
  {return false;}
  //有一个或两个交点
  float t1 = -b + sqrt(dt);
  float t2 = -b - sqrt(dt);
  if (t1 >= 0 && t2 >= 0)
  {
    float f = fmin(t1, t2);
    if (f > 0.0f && f <= line_a.m_len)
    {
      hitPoint = line_a.m_vcOrig + (line_a.m_vcDir * f);
      return true;
    }
  }
  return false;
}
bool Collider::Intersects(GLLine line_a[], GLCircle &circle, int linesNum)
{
  for (int i = 0;i < linesNum;i++)
  {
    Vector2 d = line_a[i].m_vcOrig - circle.center;
    if (d.length2q() <= circle.radius*circle.radius)
    {
      return false;
    }
    float b = line_a[i].m_vcDir.dot(d);
    float c = d.dot(d) - circle.radius*circle.radius;
    float dt = b*b - c;
    if (dt < 0)
    {
      return false;
    }
    //有一个或两个交点
    float t1 = -b + sqrt(dt);
    float t2 = -b - sqrt(dt);
    if (t1 >= 0 && t2 >= 0)
    {
      float f = fmin(t1, t2);
      if (f > 0.0f && f <= line_a[i].m_len)
      {
        //hitPoint = line_a[i].m_vcOrig + (line_a[i].m_vcDir * f);
        return true;
      }
    }
  }
  return false;
}
bool Collider::Intersects(GLRay &ray_a, GLCircle &circle,/*_Out_*/ Vector2 &hitPoint)
{
  Vector2 d = ray_a.m_vcOrig - circle.center;
  if (d.length2q() <= circle.radius*circle.radius)
  {
    return false;
  }
  float b = ray_a.m_vcDir.dot(d);
  float c = d.dot(d) - circle.radius*circle.radius;
  float dt = b*b - c;
  if (dt < 0)
  {
    return false;
  }
  //有一个或两个交点
  //用二元一次方程的求根公式 ax^2+bx+c=0 (a不等于0）
  float t1 = -b + sqrt(dt);//x1=-b+sqrt（b^2-4ac)]/2ab
  float t2 = -b - sqrt(dt);//x2=-b-sqrt（b^2-4ac)/ 2ab
  if (t2 > 0.0f)//方程的根>0才有意义
  {
    hitPoint = ray_a.m_vcOrig + ray_a.m_vcDir*t2;
    return true;
  }
  if (t1 > 0.0f)
  {
    hitPoint = ray_a.m_vcOrig + ray_a.m_vcDir*t1;
    return true;
  }
  return false;
}
bool Collider::Intersects(GLRay &ray_a, GLAABB &aabb,/*_Out_*/ Vector2 &hitPoint)
{
  float tmin = 0.0f;
  float tmax = FLT_MAX;
  //平面垂直于X轴
  if (abs(ray_a.m_vcDir.x) < 0.000001f) //如果射线平行于平面的
  {
    //如果射线不在AABB框内,说明不相交 
    if (ray_a.m_vcOrig.x < aabb.m_vcMin.x || ray_a.m_vcOrig.x > aabb.m_vcMax.x)
    {
      return false;
    }
  }
  else
  {
    //计算射线的最近和最远距离
    float ood =1.0 / ray_a.m_vcDir.x;
    float t1 = (aabb.m_vcMin.x - ray_a.m_vcOrig.x) * ood;
    float t2 = (aabb.m_vcMax.x - ray_a.m_vcOrig.x) * ood;
    //交换最近和最远的平面，保证最近的是t1
    if (t1 > t2)
    {
      float temp = t1;
      t1 = t2;
      t2 = temp;
    }
    //计算交点的交叉的长度
    if (t1 > tmin) tmin = t1;
    if (t2 < tmax) tmax = t2;
    //没有碰撞就退出 
    if (tmin > tmax) { return false; }
  }// 平面垂直于X轴结束
   //平面垂直于Y轴 
  if (abs(ray_a.m_vcDir.y) < 0.000001f) //如果射线平行于平面的  
  {
    //如果射线不在AABB框内,说明不相交 
    if (ray_a.m_vcOrig.y <aabb.m_vcMin.y || ray_a.m_vcOrig.y> aabb.m_vcMax.y)
    {
      return false;
    }
  }
  else
  {
    //计算射线的最近和最远距离 
    float ood =1.0 / ray_a.m_vcDir.y;
    float t1 = (aabb.m_vcMin.y - ray_a.m_vcOrig.y) * ood;//AABB最小点减去向量的原点
    float t2 = (aabb.m_vcMax.y - ray_a.m_vcOrig.y) * ood;//AABB最大点减去向量的原点
    //如果算出的t1值大于t2值就交换它们，保证t1里面是最小的，t2里面是最大
    if (t1 > t2)
    {
      float temp = t1;
      t1 = t2;
      t2 = temp;
    }
    //计算交点的交叉的长度s  
    if (t1 > tmin) tmin = t1;
    if (t2 < tmax) tmax = t2;
    //没有找到交叉碰撞就退出
    if (tmin > tmax) { return false; }
  }
  hitPoint.x =ray_a.m_vcOrig.x + tmin * ray_a.m_vcDir.x;
  hitPoint.y = ray_a.m_vcOrig.y+ tmin * ray_a.m_vcDir.x;
  return true;
  伟哥算法1
  //GLLine line(ray_a.m_vcOrig, ray_a.m_vcDir, 999999);
  //GLLine ab[4] = {
  //  GLLine(Vector2(aabb.m_vcMax.x,aabb.m_vcMin.y),aabb.m_vcMin),
  //  GLLine(aabb.m_vcMax,Vector2(aabb.m_vcMax.x,aabb.m_vcMin.y)),
  //  GLLine(Vector2(aabb.m_vcMin.x,aabb.m_vcMax.y),aabb.m_vcMax),
  //  GLLine(aabb.m_vcMin,Vector2(aabb.m_vcMin.x,aabb.m_vcMax.y)) };
  //for (int i = 0; i < 4; ++i)
  //  if (Collider::Intersects(line, ab[i], hitPoint))
  //    return true;
}
bool Collider::Intersects(GLAABB &aabb_a, GLAABB &aabb_b)
{ 
  if (aabb_a.m_vcMax.x > aabb_b.m_vcMin.x&&aabb_a.m_vcMin.x<aabb_b.m_vcMax.x
    &&aabb_a.m_vcMax.y>aabb_b.m_vcMin.y&&aabb_a.m_vcMin.y < aabb_b.m_vcMax.y)
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLPolygon &poly_a, GLPolygon &poly_b)
{
  int lines_num = poly_a.m_lineNum + poly_b.m_lineNum;
  Vector2 *temp = new Vector2[lines_num];
  for (int i = 0; i < poly_a.m_lineNum; i++)//先求出A&B的所有正交向量
  {                       //然后放到数组里
    temp[i] = poly_a.m_line[i].m_vcDir.ortho();
  }
  for (int i = 0; i < poly_b.m_lineNum; i++)
  {
    temp[poly_a.m_lineNum+ i] = poly_b.m_line[i].m_vcDir.ortho();
  }
  for (int i = 0; i < poly_a.m_lineNum + poly_b.m_lineNum; i++)//AB的线段和
  {
    float min_a = 999999;
    float max_a = -999999;
    for (int j = 0; j < poly_a.m_lineNum;j++)
    {
      float vec = poly_a.m_line[j].m_vcOrig.dot(temp[i]);//正交好再求点乘。
      if (vec < min_a)
      {
        min_a = vec;
      }
      if (vec > max_a)
      {
        max_a = vec;
      }
    }
    float min_b = 999999;
    float max_b = -999999;
    for (int j = 0; j < poly_b.m_lineNum; j++)
    {
      float vec = poly_b.m_line[j].m_vcOrig.dot(temp[i]);
      if (vec < min_b)
      {
        min_b = vec;
      }
      if (vec > max_b)
      {
        max_b = vec;
      }
    }
    if (!(min_b <= max_a && max_b >= min_a))
      return false;
  }
  delete[]temp;
  return true;
}
bool Collider::Intersects(GLSphere sphere, GLPlane *plane, int PlansNum)
{
  for (int i = 0; i < PlansNum; ++i)
  {
    float f = (float)sphere.m_center.dot(plane[i].m_vcN) + plane[i].m_fD;
    //面法线朝外的算法，如果是面法线朝内就要反过来，f<=-sphere.m_radius
    if (f>=sphere.m_radius)//||如果圆心和面法线的向量积+(面到原点的距离),比圆的半径还要小就是没有发生碰撞
      return false;
  }
  return true;    
}
bool Collider::Intersects(GLPoint &point, GLAABB3D &Aabb)
{
  if (point.m_center.x >Aabb.m_vcMax.x) return false;
  if (point.m_center.y >Aabb.m_vcMax.y) return false;
  if (point.m_center.z > Aabb.m_vcMax.z) return false;
  if (point.m_center.x <Aabb.m_vcMin.x) return false;
  if (point.m_center.y <Aabb.m_vcMin.y) return false;
  if (point.m_center.z <Aabb.m_vcMin.z) return false;
  return true;
}
bool Collider::Intersects(GLPoint &point, GLPlane *plane, int PlansNum)
{
  for (int i = 0; i < PlansNum; i++)
  {
    if (plane[i].m_vcN.x * point.m_center.x + plane[i].m_vcN.y * point.m_center.y + plane[i].m_vcN.z * point.m_center.z + plane[i].m_fD >= 0)
    {
      return false;
    }
  }
  return true;
}
bool Collider::Intersects(GLRay3D &ray, GLAABB3D &aabb, Vector3D &vcHit)
{
  //射线公式：p(t) = p0 + tu
  //p0为射线起始点向量，t为标量，u为射线方向。
  float tmin = 0.0f;
  float tmax = FLT_MAX;
  X///
  if(abs(ray.m_vcDir.x)<0.000001f)//给一个很小的数字确保不等于0
  {
    if (ray.m_vcDir.x<aabb.m_vcMin.x || ray.m_vcDir.x>aabb.m_vcMax.x)
      return false;
  }
  else
  {
    //计算射线从近平面到远平面的距离
    float ood = 1.0f / ray.m_vcDir.x;
    float t1=(aabb.m_vcMin.x-ray.m_vcOrig.x)*ood;
    float t2=(aabb.m_vcMax.x - ray.m_vcOrig.x)*ood;
    //让t1变成近平面，让t2变成远平面
    if (t1 > t2)
    {
      float temp = t1;
      t1 = t2;
      t2 = temp;
    }
    if (t1 > tmin)
      tmin = t1;
    if (t2 < tmax)
      tmax = t2;
    if (tmin > tmax)
      return false;
  }
  //Y/
  if (abs(ray.m_vcDir.y)<0.000001f)//给一个很小的数字确保不等于0
  {
    if (ray.m_vcDir.y<aabb.m_vcMin.y || ray.m_vcDir.y>aabb.m_vcMax.y)
      return false;
  }
  else
  {
    //计算射线从近平面到远平面的距离
    float ood = 1.0f / ray.m_vcDir.y;
    float t1 = (aabb.m_vcMin.y - ray.m_vcOrig.y)*ood;
    float t2 = (aabb.m_vcMax.y - ray.m_vcOrig.y)*ood;
    //让t1变成近平面，让t2变成远平面
    if (t1 > t2)
    {
      float temp = t1;
      t1 = t2;
      t2 = temp;
    }
    if (t1 > tmin)
      tmin = t1;
    if (t2 < tmax)
      tmax = t2;
    if (tmin > tmax)
      return false;
  }
  //Z//
  if (abs(ray.m_vcDir.z)<0.000001f)//给一个很小的数字确保不等于0
  {
    if (ray.m_vcDir.z<aabb.m_vcMin.z || ray.m_vcDir.z>aabb.m_vcMax.z)
      return false;
  }
  else
  {
    //计算射线从近平面到远平面的距离
    float ood = 1.0f / ray.m_vcDir.z;
    float t1 = (aabb.m_vcMin.z - ray.m_vcOrig.z)*ood;
    float t2 = (aabb.m_vcMax.z - ray.m_vcOrig.z)*ood;
    //让t1变成近平面，让t2变成远平面
    if (t1 > t2)
    {
      float temp = t1;
      t1 = t2;
      t2 = temp;
    }
    if (t1 > tmin)
      tmin = t1;
    if (t2 < tmax)
      tmax = t2;
    if (tmin > tmax)
      return false;
  }
  vcHit.x = ray.m_vcOrig.x + tmin*ray.m_vcDir.x;
  vcHit.y = ray.m_vcOrig.y + tmin*ray.m_vcDir.y;
  vcHit.z= ray.m_vcOrig.z + tmin*ray.m_vcDir.z;
  return true;
}
bool Collider::Intersects(GLRay3D &ray, GLLine3D &line, Vector3D &HitPoint)
{
  //ray :1 //line :2
  if (ray.m_vcDir.cross(line.m_vcDir).length() < 0.0001)
    return false;
  float s = ((line.m_vcOrig - ray.m_vcOrig).cross(line.m_vcDir)).dot(ray.m_vcDir.cross(line.m_vcDir));
  float t = ((ray.m_vcOrig - line.m_vcOrig).cross(ray.m_vcDir)).dot(line.m_vcDir.cross(ray.m_vcDir));
  if (s < 0)
    return false;
  if (t < 0)
    return false;
  if (t > line.m_len)
    return false;
  Vector3D RayHit = Vector3D(ray.m_vcOrig.x + (ray.m_vcDir.x*s), ray.m_vcOrig.y + (ray.m_vcDir.y*s), ray.m_vcOrig.z + (ray.m_vcDir.z*s));
  Vector3D LineHit = Vector3D(line.m_vcOrig.x + (line.m_vcDir.x*t), line.m_vcOrig.y + (line.m_vcDir.y*t), line.m_vcOrig.z + (line.m_vcDir.z*t));
  if (abs(RayHit.x - LineHit.x) < 10 && abs(RayHit.y - LineHit.y) < 10 && abs(RayHit.z - LineHit.z) < 10)
  {
    HitPoint = RayHit;
    return true;
  }
  return false;
}
bool Collider::Intersects(GLRay3D &ray_a, GLTrianglePlane &triplane_a, Vector3D &hitPoint)
{
  Vector3D edge1 = triplane_a.m_vertex[1] - triplane_a.m_vertex[0];
  Vector3D edge2 = triplane_a.m_vertex[2] - triplane_a.m_vertex[0];
  Vector3D pvec = ray_a.m_vcDir.cross(edge2);
  float det = edge1.dot(pvec);
  if (det < 0.00001 && det > -0.00001)
    return false;
  float f_det = 1.0f / det;
  Vector3D tvec = ray_a.m_vcOrig - triplane_a.m_vertex[0];
  float u = tvec.dot(pvec) * f_det;
  if (u < 0.0f || u > 1)
    return false;
  Vector3D qvec = tvec.cross(edge1);
  float v = ray_a.m_vcDir.dot(qvec) * f_det;
  if (v < 0.0f || u + v > 1)
    return false;
  float f = edge2.dot(qvec) * f_det;
  if (f >=0.0f)
  {
    hitPoint = ray_a.m_vcOrig + (ray_a.m_vcDir * f);
    return true;
  }
  return false;
//    float t, u, v;
//    Vector3D E1 = triplane_a.m_v1 - triplane_a.m_v0;
//    Vector3D E2 = triplane_a.m_v2 - triplane_a.m_v0;
//    Vector3D P = ray_a.m_vcDir.cross(E2);
//    float det = E1.dot(P);
//    Vector3D T;
//  
//    if (det>0)
//    {
//      T = ray_a.m_vcOrig - triplane_a.m_v0;
//    }
//    else
//    {
//      T = triplane_a.m_v0 - ray_a.m_vcOrig;
//      det = -det;
//    }
//  
//    if (det< 0.00001f)
//    {
//      return false;
//    }
//  
//    u = T.dot(P);
//    if (u<0.0f || u>det)
//      return false;
//  
//    Vector3D Q = T.cross(E1);
//    v = ray_a.m_vcDir.dot(Q);
//    if (v<0.0f || u + v>det)
//      return false;
//  
//    t = E2.dot(Q);
//    float fInvDet = 1.0f / det;
//    (t *= fInvDet);
//    (u *= fInvDet);
//    (v *= fInvDet);
//    hitPoint = ray_a.m_vcOrig + ray_a.m_vcDir*t;
//  
//    return true;
}
bool Collider::Intersects(Vector3D &RayOrig, Vector3D &RayDir, Vector3D &v0, Vector3D v1, Vector3D v2, Vector3D &HitPoint)
{
  float u, v, t;
  Vector3D E1 =v1 - v0;
  Vector3D E2 =v2 - v0;
  Vector3D P = RayDir.cross(E2);
  float det = E1.dot(P);
  Vector3D T;
  if (det>0)
  {
    T = RayOrig - v0;
  }
  else
  {
    T = v0 - RayOrig;
    det = -det;
  }
  if (det< 0.00001f)
  {
    return false;
  }
  u = T.dot(P);
  if (u<0.0f || u>det)
    return false;
  Vector3D Q = T.cross(E1);
  v = RayDir.dot(Q);
  if (v<0.0f || u + v>det)
    return false;
  t = E2.dot(Q);
  float fInvDet = 1.0f /det;
  HitPoint.x = (t *= fInvDet);
  HitPoint.y = (u *= fInvDet);
  HitPoint.z = (v *= fInvDet);
  return true;
}
bool Collider::Intersects(GLAABB3D &aabb_a, GLAABB3D &aabb_b)
{
  if (aabb_a.m_vcMax.x > aabb_b.m_vcMin.x&&aabb_a.m_vcMin.x<aabb_b.m_vcMax.x
    &&aabb_a.m_vcMax.y>aabb_b.m_vcMin.y&&aabb_a.m_vcMin.y < aabb_b.m_vcMax.y&&
    aabb_a.m_vcMax.z>aabb_b.m_vcMin.z&&aabb_a.m_vcMin.z < aabb_b.m_vcMax.z)
  {
    return true;
  }
  return false;
//  if (aabb_a.m_vcMin.x < aabb_b.m_vcMax.x&&aabb_a.m_vcMin.y<aabb_b.m_vcMax.y&&
//    aabb_a.m_vcMax.x>aabb_b.m_vcMin.x&&aabb_a.m_vcMax.y > aabb_b.m_vcMin.y
//    && aabb_a.m_vcMin.z < aabb_b.m_vcMax.z&&aabb_a.m_vcMax.z>aabb_b.m_vcMin.z)
//  {
//    return true;
//  }
//  return false;
}
bool Collider::Intersects(GLSphere & sphere_a, GLSphere & sphere_b)
{
  if ((sphere_a.m_center - sphere_b.m_center).length() < sphere_a.m_radius + sphere_b.m_radius)
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLSphere &sphere_a, GLTrianglePlane & triplane_a)
{
  float f = sphere_a.m_center.dot(triplane_a.m_vcN) + triplane_a.m_fD;
  if (f < 0)
  {
    return false;
  }
  if (f > sphere_a.m_radius)
  {
    return false;
  }
  //测试三角形的点在球内
  for (int i = 0; i < 3; i++)
  {
    if (sphere_a.IsPointIn(triplane_a.m_vertex[i]))
    {
      return true;
    }
  }
  //测试三角形的三条边与球相交
  for (int i = 0; i < 3; i++)
  {
    if (sphere_a.Intersects(triplane_a.m_vertex[i % 3], triplane_a.m_vertex[(i + 1) % 3]))
      return true;
  }
  //测试三角形包含球
  float t = (triplane_a.m_vcN.dot(sphere_a.m_center) + triplane_a.m_fD) / (triplane_a.m_vcN.length2q());
  Vector3D p = sphere_a.m_center + triplane_a.m_vcN*t;
  if (triplane_a.IsPointIn(p))
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLSphere & sphere_a, Vector3D & line_a, Vector3D &line_b)
{
  Vector3D d1 = sphere_a.m_center - line_a;
  if (d1.length2q() <= sphere_a.m_radius*sphere_a.m_radius)
    return true;
  Vector3D b_dir = (line_b - line_a).Normalize();
  float s = d1.dot(b_dir);
  if (s < 0)
    return false;
  Vector3D d2 = sphere_a.m_center - line_b;
  if (d2.length2q() <= sphere_a.m_radius*sphere_a.m_radius)
    return true;
  float m = d1.length2q() - s*s;
  if (m <= sphere_a.m_radius*sphere_a.m_radius)
    return true;
  return false;
}
bool Collider::Intersects(GLSphere & sphere, GLPlane &plane)
{
  float len = sphere.m_center.dot(plane.m_vcN) + plane.m_fD;
  if (len >= 0 && len <= sphere.m_radius)
  {
    return true;
  }
  return false;
}
bool Collider::Intersects(GLLine3D & line_a, GLAABB3D & aabb, Vector3D & hitPoint)
{
  GLRay3D ray;
  ray.Set(line_a.m_vcOrig, line_a.m_vcDir);
  bool key = Intersects(ray, aabb, hitPoint);
  if (key == false)
  {
    return false;
  }
  else
  {
    if ((line_a.m_vcOrig - hitPoint).length2q() > line_a.m_len*line_a.m_len)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
}
bool Collider::Intersects(GLLine3D & line, GLTrianglePlane & triangle, Vector3D & hitPoint)
{
  GLRay3D ray;
  ray.Set(line.m_vcOrig, line.m_vcDir);
  bool key = Intersects(ray, triangle, hitPoint);
  if (key == false)
  {
    return false;
  }
  else
  {
    if ((line.m_vcOrig - hitPoint).length2q() <= line.m_len*line.m_len)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
}
Collider::~Collider()
{
}