collisiondetectionmath.cpp.svn-base

自己做的小游戏

	s = d / f ;
	//printf("%f\n",s);
	v2->x = x1 + s * Ax ;
	v2->z = y1 + s * Ay ;
	return true; //相交
}

bool  CollisionDetectionMath::linesSegmentintersectExtendTo3D(Vertex v1,Vertex *v2,Vertex v3,Vertex v4,float height) 
{
    //printf("do\n");
	float Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset,s;
	float x1lo,x1hi,y1lo,y1hi,temHeight;

	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 bound box test*/
		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 bound box test*/
		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;					/* alpha numerator*/
	f = Ay*Bx - Ax*By;					/* both denominator*/
	if(f>0) 
	{						/* alpha tests*/
		if(d<0 || d>f) return false;//不相交
	} 
	else 
	{
		if(d>0 || d<f) return false;//不相交
	}

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

	/*compute intersection coordinates*/

	if(f==0) return false;  //线段有部分重合
	/*num = d*Ax;						// numerator 
	offset = SAME_SIGNS(num,f) ? f/2 : -f/2;		//round direction
	v2->x = x1 + (num+offset) / f;				//intersection x 

	num = d*Ay;
	offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
	v2->z = y1 + (num+offset) / f;				// intersection y 
	*/

	s = d / f ;
	//printf("%f\n",s);
	temHeight = v1.y + s * v2->y ;
	if (temHeight > height)
		return false;
	v2->x = x1 + s * Ax ;
	v2->y = temHeight;
	v2->z = y1 + s * Ay ;
	return true; //相交
}

bool  CollisionDetectionMath::linesSegmentintersectExtendTo3DAdvanced(Vertex v1,Vertex *v2,Vertex v3,Vertex v4,float height,Vertex advancedTest) 
{
    //printf("do\n");
	float Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset,s;
	float x1lo,x1hi,y1lo,y1hi,temHeight;

	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 bound box test*/
		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 bound box test*/
		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;					/* alpha numerator*/
	f = Ay*Bx - Ax*By;					/* both denominator*/
	if(f>0) 
	{						/* alpha tests*/
		if(d<0 || d>f) return false;//不相交
	} 
	else 
	{
		if(d>0 || d<f) return false;//不相交
	}

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

	/*compute intersection coordinates*/

	if(f==0) return false;  //线段有部分重合
	/*num = d*Ax;						// numerator 
	offset = SAME_SIGNS(num,f) ? f/2 : -f/2;		//round direction
	v2->x = x1 + (num+offset) / f;				//intersection x 

	num = d*Ay;
	offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
	v2->z = y1 + (num+offset) / f;				// intersection y 
	*/

	s = d / f ;
	//printf("%f\n",s);
	temHeight = v1.y + s * v2->y ;
	if (temHeight > height)
	{
		if(v2->y == height)
		{
			return true;
		}
		else if(v2->y < height)
		{
			v3.y = height;
			v4.y = height;
			advancedTest.y = height;
			GetIntersection(v1,v2,v3,v4,advancedTest);
			return true;
		}
		return false;
	}
	v2->x = x1 + s * Ax ;
	v2->y = temHeight;
	v2->z = y1 + s * Ay ;
	return true; //相交
}

//关于这个函数的实现及原理参见我论文38，39，40页的4.3.5节。具体每个变量的意义对应论文40页的伪代码，注意
//v1对应伪代码中的o，v3对应伪代码中的d，va，vb，vc分别对应伪代码中的v0,v1,v2，其余变量意义相同。
//sub是两个向量相减的内联函数，dot是两个向量点乘（内积）的内联函数，cross是两个向量叉乘（外积）的内联函数。
//有兴趣可以将它们改成操作符重载函数。
bool CollisionDetectionMath::IsLineSegmentCutByTriangle(Vertex v1,Vertex *v2,Vertex va,Vertex vb,Vertex vc)
{
	Vertex v3,e1,e2,p,s,q;
	float a,f,u,v,t;
	
	v3 = sub(*v2,v1);
	e1 = sub(vb,va);	
	e2 = sub(vc,va);
	p = cross (v3,e2);
	
	a = dot(e1,p);
	//printf("a = %f\n",a);
	if (a > -EPSILON && a < EPSILON)
		return false;
	f = 1/a;
	
	s = sub(v1,va);

	u = dot(s,p) * f;
	//printf("u = %f\n",u);
	if(u < 0 || u > 1)
		return false;

	q = cross(s,e1);
	v = dot(v3,q) * f;
	//printf("v = %f\n",v);
	if(v < 0 || u + v > 1)
		return false;
	
	t = dot(e2,q) * f;
	//printf("t = %f\n",t);
	if(t < 0 || t > 1)
		return false;
	v2->x = v1.x + t * v3.x;
	v2->y = v1.y + t * v3.y;
	v2->z = v1.z + t * v3.z;

	return true;
}


void CollisionDetectionMath::GetIntersection(Vertex v1,Vertex* v2,Vertex va,Vertex vb,Vertex vc)
{	
	//PLANE plane;
	Vertex N12,N13,N;  //这三个是向量
   	////////////calculate n12 n13
	N12 = sub( vb , va );
	N13 = sub( vc , va );

    N = cross( N12 , N13);

    //plane.A = N.x;
    //plane.B = N.y;
    //plane.C = N.z;
	float planeD = -(N.x*va.x+N.y*va.y+N.z*va.z);

	double temp1=(N.x*(v2->x-v1.x)+N.y*(v2->y-v1.y)+N.z*(v2->z-v1.z)); //线段v1v2在平面法向量上的投影长度
	double temp2=-planeD-N.x*v1.x-N.y*v1.y-N.z*v1.z;   //点v1到平面的距离
    ///// Line param is temp2/temp1
	v2->x=v1.x+float((double(v2->x-v1.x)*temp2)/temp1);
	v2->y=v1.y+float((double(v2->y-v1.y)*temp2)/temp1);
	v2->z=v1.z+float((double(v2->z-v1.z)*temp2)/temp1);
}

//void main()
//{
//   Vertex v1(0,0,0);
//   Vertex v2(10,5,10);
//   Vertex va(1,0,1);
//   Vertex vb(5,0,1);
//   Vertex vc(5,0,5);
//   Vertex vd(1,0,5);
//   Vertex v3(3,0,3);
//   float radius = 3;
//
//   CollisionDetectionMath e;
//   //bool res = e.IsLineSegmentCutBydQuadExtendTo3D(v1 , &v2, va,vb, vc, vd);
//   //printf("%d\n",res);
//   //printf("%f  %f   %f\n",v2.x,v2.y,v2.z);
//
//   bool res1 = e.IsLineSegmentCutByCircleExtendTo3D(v1,&v2,v3,radius);
//	printf("%f  %f   %f\n",v2.x,v2.y,v2.z);
//}

//void main()
//{
//   Vertex v1(5,5,-5);
//   Vertex v2(5,5,10);
//   Vertex va(1,1,0);
//   Vertex vb(5,1,0);
//   Vertex vc(1,5,0);
//   Vertex vd(1,0,5);
//   Vertex v3(3,0,3);
//   float radius = 3;
//
//   CollisionDetectionMath e;
//   bool res = e.IsLineSegmentCutByTriangle(v1 , &v2, va,vb, vc);
//   printf("%d\n",res);
//   printf("%f  %f   %f\n",v2.x,v2.y,v2.z);
//}