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

天才小熊猫oo

已于 2022-12-29 16:25:05 修改

阅读量831

点赞数 2

分类专栏：从0开发游戏引擎文章标签：游戏引擎算法线性代数矩阵

于 2022-09-18 01:22:46 首次发布

本文链接：https://blog.csdn.net/qq_33531923/article/details/126913216

版权

从0开发游戏引擎专栏收录该内容

14 篇文章 1 订阅

订阅专栏

该模块主要是实现了各种形状之间是否发生了碰撞检测计算。

Collider.h

#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(); 
};

Collider.cpp

#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()
{
}