collisiondetectionmath.cpp.svn-base

自己做的小游戏

//关于我函数之间的调用关系和层次结构，参见我论文的30，31，32页的3.5节和4.1节。
//关于数据类型和数据结构的相关信息，参见我论文的33，34页的4.2.1节和4.2.2节。

#include "CollisionDetectionMath.h"
//#include <stdio.h>

CollisionDetectionMath::CollisionDetectionMath(){}
CollisionDetectionMath::~CollisionDetectionMath(){}

/*bool CollisionDetectionMath::IsLineSegmentCutByStardQuad(Vertex v1 ,Vertex v2,Vertex va,Vertex vb,Vertex vc,Vertex vd)
{
	PLANE plane;
	plane=GetPlaneEquation(va,vb,vc);
	bool sign1,sign2;
	///////////如果两点在同一侧
	if((ClassifyPointPlane(v1,plane)*ClassifyPointPlane(v2,plane))>0)
		return false;
	//////////////获得线段与平面的交点
	Vertex intersection;
	intersection=GetIntersection(v1,v2,plane);
	///////////包围盒判断，仅对aabb包围盒有效
	float maxx,minx,  maxy,miny,  maxz,minz;
	maxx=minx=va.x;
	maxy=miny=va.y;
	maxz=minz=va.z;

		if(vb.x>maxx)maxx=vb.x;
		if(vb.x<minx)minx=vb.x;
		if(vb.y>maxy)maxy=vb.y;
		if(vb.y<miny)miny=vb.y;
		if(vb.z>maxz)maxz=vb.z;
		if(vb.z<minz)minz=vb.z;

		if(vc.x>maxx)maxx=vc.x;
		if(vc.x<minx)minx=vc.x;
		if(vc.y>maxy)maxy=vc.y;
		if(vc.y<miny)miny=vc.y;
		if(vc.z>maxz)maxz=vc.z;
		if(vc.z<minz)minz=vc.z;

		if(vd.x>maxx)maxx=vd.x;
		if(vd.x<minx)minx=vd.x;
		if(vd.y>maxy)maxy=vd.y;
		if(vd.y<miny)miny=vd.y;
		if(vd.z>maxz)maxz=vd.z;
		if(vd.z<minz)minz=vd.z;

	if(intersection.x>maxx || intersection.x<minx)  
		return false;
	if(intersection.y>maxy || intersection.y<miny)
		return false;
	if(intersection.z>maxz || intersection.z<minz)
		return false;

    return true;
}*/
bool CollisionDetectionMath::IsLineSegmentCutBydQuad(Vertex v1 ,Vertex* v2,Vertex va,Vertex vb,Vertex vc,Vertex vd)
{
	bool s1,s2,s3,s4;

	s1 = linesSegmentintersect(v1,v2,va,vb);
	s2 = linesSegmentintersect(v1,v2,vb,vc);
	s3 = linesSegmentintersect(v1,v2,vc,vd);
	s4 = linesSegmentintersect(v1,v2,vd,va);

	s1 = s1 || s2 || s3 || s4;
	
	return s1;
}

bool CollisionDetectionMath::IsLineSegmentCutBydQuadExtendTo3D(Vertex v1 ,Vertex* v2,Vertex va,Vertex vb,Vertex vc,Vertex vd,float height)
{
	bool s1,s2,s3,s4;

	s1 = linesSegmentintersectExtendTo3DAdvanced(v1,v2,va,vb,height,vc);
	s2 = linesSegmentintersectExtendTo3DAdvanced(v1,v2,vb,vc,height,vd);
	s3 = linesSegmentintersectExtendTo3DAdvanced(v1,v2,vc,vd,height,va);
	s4 = linesSegmentintersectExtendTo3DAdvanced(v1,v2,vd,va,height,vb);

	s1 = s1 || s2 || s3 || s4;
	
	return s1;
}

//bool CollisionDetectionMath::IsLineSegmentCutBydQuad(Vertex v1 ,Vertex* v2,Vertex *pv)
//{
//	return IsLineSegmentCutBydQuad(v1,v2,pv[0],pv[1],pv[2],pv[3]);
//}
//
//PLANE CollisionDetectionMath::GetPlaneEquation(Vertex v1,Vertex v2,Vertex v3)
//{
//	PLANE plane;
//   	NORMAL N12,N13,N;
//   	////////////calculate n12 n13
//   	N12.nx=v2.x-v1.x;
//   	N12.ny=v2.y-v1.y;
//   	N12.nz=v2.z-v1.z;
//
//   	N13.nx=v3.x-v1.x;
//   	N13.ny=v3.y-v1.y;
//   	N13.nz=v3.z-v1.z;
//
//    N=GetTwoNormalProduct(N12,N13);
//
//    plane.A=N.nx;
//    plane.B=N.ny;
//    plane.C=N.nz;
//	plane.D= -(plane.A*v1.x+plane.B*v1.y+plane.C*v1.z);
//
//	return plane;
//}

//int CollisionDetectionMath::ClassifyPointPlane(Vertex point,PLANE plane)
//{
//	float distance=plane.A*point.x+plane.B*point.y+plane.C*point.z+plane.D;
//    if(distance==0)return COINCIDENT;   //相交
//	if(distance>0)return  IN_FRONT_OF;  //前面
//	return  IN_BACK_OF;					//后面
//}
//
//Vertex CollisionDetectionMath::GetIntersection(Vertex v1,Vertex v2,PLANE plane)
//{
//	Vertex point;	
//	double temp1=(plane.A*(v2.x-v1.x)+plane.B*(v2.y-v1.y)+plane.C*(v2.z-v1.z)); //线段v1v2在平面法向量上的投影长度
//	double temp2=-plane.D-plane.A*v1.x-plane.B*v1.y-plane.C*v1.z;   //点v1到平面的距离
//    ///// Line param is temp2/temp1
//	point.x=v1.x+float((double(v2.x-v1.x)*temp2)/temp1);
//	point.y=v1.y+float((double(v2.y-v1.y)*temp2)/temp1);
//	point.z=v1.z+float((double(v2.z-v1.z)*temp2)/temp1);
// 
//	return point;
//}

//NORMAL CollisionDetectionMath::GetTwoNormalProduct(NORMAL n1,NORMAL n2)
//{
//	double n1x,n1y,n1z,n2x,n2y,n2z;
//	n1x=n1.nx;  n1y=n1.ny;  n1z=n1.nz;
//	n2x=n2.nx;  n2y=n2.ny;  n2z=n2.nz;
//
//    double nx,ny,nz;
//
//    nx=  n1y*n2z-n2y*n1z;
//    ny=-(n1x*n2z-n2x*n1z);
//    nz=  n1x*n2y-n2x*n1y;
////	return NORMAL(float(nx),float(ny),float(nz));
//	return Normalization(nx,ny,nz);
//}
//
//NORMAL CollisionDetectionMath::Normalization(NORMAL n)
//{
//	NORMAL normal;
//	double x,y,z;
//	x=n.nx; y=n.ny; z=n.nz;
//
//	double max=sqrt(x*x+y*y+z*z);
//    if(max<0.0001)return NORMAL(0,1,0);
//
//	x=x/max;
//	y=y/max;
//	z=z/max;
//
//	normal.nx=float(x);
//	normal.ny=float(y);
//	normal.nz=float(z);
//
//	return normal;
//
//}
//
//NORMAL CollisionDetectionMath::Normalization(double x,double y,double z)
//{
//	NORMAL normal=NORMAL(float(x),float(y),float(z));
//    return Normalization(normal);
//	/*
//	double absX,absY,absZ;
//	if(x<0)absX=-x;
//	else absX=x;
//	if(y<0)absY=-y;
//	else absY=y;
//	if(z<0)absZ=-z;
//	else absZ=z;
//
//	double max=absX>absY?absX:absY;
//	max=max>absZ?max:absZ ;
//  
//	x=x/max;
//	y=y/max;
//	z=z/max;
//
//	normal.nx=float(x);
//	normal.ny=float(y);
//	normal.nz=float(z);
//
//	return normal;*/
//}

bool  CollisionDetectionMath::IsLineSegmentCutByCircle(Vertex v1,Vertex *v2,Vertex v3,float radius) //八边形
{
	bool s1,s2,s3,s4,s5,s6,s7,s8;
	Vertex va(v3.x+radius,0,v3.z);
	Vertex vb(v3.x+radius*Sin45,0,v3.z+radius*Sin45);
	Vertex vc(v3.x,0,v3.z+radius);
	Vertex vd(v3.x-radius*Sin45,0,v3.z+radius*Sin45);
	Vertex ve(v3.x-radius,0,v3.z);
	Vertex vf(v3.x-radius*Sin45,0,v3.z-radius*Sin45);
	Vertex vg(v3.x,0,v3.z-radius);
	Vertex vh(v3.x+radius*Sin45,0,v3.z-radius*Sin45);


	s1 = linesSegmentintersect(v1,v2,va,vb);
	s2 = linesSegmentintersect(v1,v2,vb,vc);
	s3 = linesSegmentintersect(v1,v2,vc,vd);
	s4 = linesSegmentintersect(v1,v2,vd,ve);
	s5 = linesSegmentintersect(v1,v2,ve,vf);
	s6 = linesSegmentintersect(v1,v2,vf,vg);
	s7 = linesSegmentintersect(v1,v2,vg,vh);
	s8 = linesSegmentintersect(v1,v2,vh,va);

	s1 = s1 || s2 || s3 || s4 || s5 || s6 || s7 || s8;
	
	return s1;
}

bool  CollisionDetectionMath::IsLineSegmentCutByCircleExtendTo3D(Vertex v1,Vertex *v2,Vertex v3,float radius,float height) //八边形
{
	bool s1,s2,s3,s4,s5,s6,s7,s8;
	Vertex va(v3.x+radius,0,v3.z);
	Vertex vb(v3.x+radius*Sin45,0,v3.z+radius*Sin45);
	Vertex vc(v3.x,0,v3.z+radius);
	Vertex vd(v3.x-radius*Sin45,0,v3.z+radius*Sin45);
	Vertex ve(v3.x-radius,0,v3.z);
	Vertex vf(v3.x-radius*Sin45,0,v3.z-radius*Sin45);
	Vertex vg(v3.x,0,v3.z-radius);
	Vertex vh(v3.x+radius*Sin45,0,v3.z-radius*Sin45);


	s1 = linesSegmentintersectExtendTo3D(v1,v2,va,vb,height);
	s2 = linesSegmentintersectExtendTo3D(v1,v2,vb,vc,height);
	s3 = linesSegmentintersectExtendTo3D(v1,v2,vc,vd,height);
	s4 = linesSegmentintersectExtendTo3D(v1,v2,vd,ve,height);
	s5 = linesSegmentintersectExtendTo3D(v1,v2,ve,vf,height);
	s6 = linesSegmentintersectExtendTo3D(v1,v2,vf,vg,height);
	s7 = linesSegmentintersectExtendTo3D(v1,v2,vg,vh,height);
	s8 = linesSegmentintersectExtendTo3D(v1,v2,vh,va,height);

	s1 = s1 || s2 || s3 || s4 || s5 || s6 || s7 || s8;
	
	return s1;
}

//关于这个函数的实现及原理参见我论文35，36页的4.3.1节和4.3.2节。
bool  CollisionDetectionMath::linesSegmentintersect(Vertex v1,Vertex *v2,Vertex v3,Vertex v4) 
{
    //printf("do\n");
	float Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset,s;
	float x1lo,x1hi,y1lo,y1hi;

	float x1=v1.x;
	float x2=v2->x;
	float x3=v3.x;
	float x4=v4.x;

	float y1=v1.z;
	float y2=v2->z;
	float y3=v3.z;
	float y4=v4.z;

	Ax = x2-x1;
	Bx = x3-x4;

	//将两个向量投影分别投影到坐标轴上做排除测试，原理与分离轴定理相同，但是并没有用分离轴定理所使用的轴。
	if(Ax<0)				//投影到x轴上做排除测试
	{						
		x1lo=x2; x1hi=x1;
	} 
	else 
	{
		x1hi=x2; x1lo=x1;
	}
	if(Bx>0) 
	{
		if(x1hi < x4 || x3 < x1lo) return false; //不相交
	} 
	else 
	{
		if(x1hi < x3 || x4 < x1lo) return false;//不相交
	}

	Ay = y2-y1;
	By = y3-y4;

	if(Ay<0)		      //投影到y轴上做排除测试
	{						
		y1lo=y2; y1hi=y1;
	}
	else 
	{
		y1hi=y2; y1lo=y1;
	}
	if(By>0) 
	{
		if(y1hi < y4 || y3 < y1lo) return false;//不相交
	} 
	else 
	{
		if(y1hi < y3 || y4 < y1lo) return false;//不相交
	}


	Cx = x1-x3;
	Cy = y1-y3;
	d = By*Cx - Bx*Cy;					
	f = Ay*Bx - Ax*By;					
	if(f>0) 
	{						
		if(d<0 || d>f) return false;//不相交
	} 
	else 
	{
		if(d>0 || d<f) return false;//不相交
	}

    e = Ax*Cy - Ay*Cx;					
    if(f>0) 
	{					
		if(e<0 || e>f) return false;//不相交
	} 
	else 
	{
		if(e>0 || e<f) return false; //不相交
	}

	if(f==0) return false;  //线段有部分重合