obbcollision.cpp

obb碰撞检测类,可以在虚拟环境中进行碰撞检测

// OBBCollision.cpp: implementation of the COBBCollision class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "VOF.h"
#include "OBBCollision.h"
#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
#define cross(D,A,B) D.x=A.y*B.z-A.z*B.y; D.y=A.z*B.x-A.x*B.z; D.z=A.x*B.y-A.y*B.x
//#define PI  3.14159265

bool    collision1;
bool  collision[3][3];
extern  point  point_finger[3];//手指顶点坐标
extern  point  object_center;
extern  int  GraphicPoint[200];
extern  bool SecondGrasp;

COBBCollision::COBBCollision()
{
	width=20;
	radius=width/2;
	flength=26.5;
	slength=50.0;
	tlength=width/2;
	length=float(58.0-tlength-radius);

	first[0]=0;first[1]=0;first[2]=0;
	second[0]=0;second[1]=0;second[2]=0;
	third[0]=0;third[1]=0;third[2]=0;

	collision1=false;
	for(int i=0;i<3;i++)
		for(int j=0;j<3;j++)
			collision[i][j]=false;

	moveX=0;moveY=0;moveZ=0;
    rotatedX=0;rotatedY=0;rotatedZ=0;
	hand_rx=0;hand_ry=0;hand_rz=0;


}

COBBCollision::~COBBCollision()
{

}

void COBBCollision::Collision_Detection()//碰撞检测
{

	Build_Mold();
Build_MoldGst();

	int  x,y,i,j;
	
    bool  disjoint;
	float  angle[3][3];
	for(i=0;i<3;i++)
	{
		angle[0][i]=first[i];
		angle[1][i]=-second[i];
		angle[2][i]=-third[i];
	}
	struct point temp_handpoint[8];
	struct point temp_objectpoint[8];

    if(ob_Box)
	{
	for(i=0;i<3;i++)           //对虚拟手和目标物进行碰撞检测
			for(j=1;j<2;j++)
				{
	     			disjoint=false;
					for(x=0;x<hand[i][j].NUMPTS;x++)
					{
						temp_handpoint[x].x=hand[i][j].points[x].x;
						temp_handpoint[x].y=hand[i][j].points[x].y;
						temp_handpoint[x].z=hand[i][j].points[x].z;

						RotateX(&temp_handpoint[x],rotatedX);
	            		RotateY(&temp_handpoint[x],rotatedY);
            			RotateZ(&temp_handpoint[x],-rotatedZ);	            		
					}

					for(x=0;x<objects[0].NUMPTS;x++)
					{
						temp_objectpoint[x].x=objects[0].points[x].x;
						temp_objectpoint[x].y=objects[0].points[x].y;
						temp_objectpoint[x].z=objects[0].points[x].z;

						RotateX(&temp_objectpoint[x],rotatedX);
	            		RotateY(&temp_objectpoint[x],rotatedY);
            			RotateZ(&temp_objectpoint[x],-rotatedZ);
					}
					
					BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                    BoundingBox(temp_objectpoint,&objects[0].max,&objects[0].min);

					if((hand[i][j].max.x-objects[0].min.x)*(hand[i][j].min.x-objects[0].max.x)>0.0
					 ||(hand[i][j].max.y-objects[0].min.y)*(hand[i][j].min.y-objects[0].max.y)>0.0
					 ||(hand[i][j].max.z-objects[0].min.z)*(hand[i][j].min.z-objects[0].max.z)>0.0)

					continue;

					for(x=0;x<hand[i][j].NUMPTS;x++)
					{
						temp_handpoint[x].x=hand[i][j].points[x].x;
						temp_handpoint[x].y=hand[i][j].points[x].y;
						temp_handpoint[x].z=hand[i][j].points[x].z;

						RotateZ(&temp_handpoint[x],angle[i][0]);
						RotateX(&temp_handpoint[x],-angle[i][1]);
				}

					for(x=0;x<objects[0].NUMPTS;x++)
					{
						temp_objectpoint[x].x=objects[0].points[x].x;
						temp_objectpoint[x].y=objects[0].points[x].y;
						temp_objectpoint[x].z=objects[0].points[x].z;

						RotateZ(&temp_objectpoint[x],angle[i][0]);
						RotateX(&temp_objectpoint[x],-angle[i][1]);
					}                 

	        		BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                    BoundingBox(temp_objectpoint,&objects[0].max,&objects[0].min);
        			if((hand[i][j].max.x-objects[0].min.x)*(hand[i][j].min.x-objects[0].max.x)>0.0
	        		 ||(hand[i][j].max.y-objects[0].min.y)*(hand[i][j].min.y-objects[0].max.y)>0.0
	        		 ||(hand[i][j].max.z-objects[0].min.z)*(hand[i][j].min.z-objects[0].max.z)>0.0)

			    		continue;
					struct point AA[3];
					struct point BB[3];
					struct point CC;
					AA[0].x=1;AA[0].y=0;AA[0].z=0;
				    AA[1].x=0;AA[1].y=1;AA[1].z=0;
					AA[2].x=0;AA[2].y=0;AA[2].z=1;

					BB[0].x=hand[i][j].points[0].x-hand[i][j].points[1].x;
					BB[0].y=hand[i][j].points[0].y-hand[i][j].points[1].y;
					BB[0].z=hand[i][j].points[0].x-hand[i][j].points[1].z;

					BB[1].x=hand[i][j].points[0].x-hand[i][j].points[2].x;
					BB[1].y=hand[i][j].points[0].y-hand[i][j].points[2].y;
					BB[1].z=hand[i][j].points[0].x-hand[i][j].points[2].z;

					BB[2].x=hand[i][j].points[0].x-hand[i][j].points[4].x;
					BB[2].y=hand[i][j].points[0].y-hand[i][j].points[4].y;
					BB[2].z=hand[i][j].points[0].x-hand[i][j].points[4].z;

					int m,n;
					float  alph,thta;
					for(m=0;m<3;m++)
						for(n=0;n<3;n++)
						{
							cross(CC,AA[m],BB[n]);
							thta=float(atan2(CC.y,CC.z));
							float temp;
							temp=float(CC.x/sqrt(CC.x*CC.x+CC.y*CC.y+CC.z*CC.z));
							alph=float(acos(temp));

							for(x=0;x<hand[i][j].NUMPTS;x++)
							{
			        			temp_handpoint[x].x=hand[i][j].points[x].x;
			        			temp_handpoint[x].y=hand[i][j].points[x].y;
					        	temp_handpoint[x].z=hand[i][j].points[x].z;

								RotateY(&temp_handpoint[x],-alph*180.0/PI);

					        	RotateX(&temp_handpoint[x],-thta*180.0/PI);
					        	
							}

			        		for(x=0;x<objects[0].NUMPTS;x++)
							{
					        	temp_objectpoint[x].x=objects[0].points[x].x;
					        	temp_objectpoint[x].y=objects[0].points[x].y;
			        			temp_objectpoint[x].z=objects[0].points[x].z;

								RotateY(&temp_objectpoint[x],-alph*180.0/PI);
				        		RotateX(&temp_objectpoint[x],-thta*180.0/PI);
					        	
							} 
							BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                            BoundingBox(temp_objectpoint,&objects[0].max,&objects[0].min);

					if((hand[i][j].max.x-objects[0].min.x)*(hand[i][j].min.x-objects[0].max.x)>0.0
	        		 ||(hand[i][j].max.y-objects[0].min.y)*(hand[i][j].min.y-objects[0].max.y)>0.0
	        		 ||(hand[i][j].max.z-objects[0].min.z)*(hand[i][j].min.z-objects[0].max.z)>0.0)

							{
								disjoint=true;
			    	        	break;
							}

						}

						if(disjoint)				
							continue;

						if(i==0)
     						collision[0][1]=true;
						if(i==1)
							collision[1][1]=true;
						if(i==2)
							collision[2][1]=true;
                 }

        float d;
		for(i=0;i<3;i++)
			{
        		d=0;
			
						temp_handpoint[0].x=hand[i][2].center_point.x;
						temp_handpoint[0].y=hand[i][2].center_point.y;
						temp_handpoint[0].z=hand[i][2].center_point.z;

						RotateX(&temp_handpoint[0],rotatedX);
	            		RotateY(&temp_handpoint[0],rotatedY);
            			RotateZ(&temp_handpoint[0],-rotatedZ);
	            		

					for(x=0;x<objects[0].NUMPTS;x++)
					{
						temp_objectpoint[x].x=objects[0].points[x].x;
						temp_objectpoint[x].y=objects[0].points[x].y;
						temp_objectpoint[x].z=objects[0].points[x].z;

						RotateX(&temp_objectpoint[x],rotatedX);
	            		RotateY(&temp_objectpoint[x],rotatedY);
            			RotateZ(&temp_objectpoint[x],-rotatedZ);
					}
						BoundingBox(temp_objectpoint,&objects[0].max,&objects[0].min);
	        	if(temp_handpoint[0].x<objects[0].min.x)
        			d=d+pow(temp_handpoint[0].x-objects[0].min.x,2);
        		if(temp_handpoint[0].y<objects[0].min.y)
        			d=d+pow(temp_handpoint[0].y-objects[0].min.y,2);
        		if(temp_handpoint[0].z<objects[0].min.z)
	        		d=d+pow(temp_handpoint[0].z-objects[0].min.z,2);

        		if(temp_handpoint[0].x>objects[0].max.x)
        			d=d+pow(temp_handpoint[0].x-objects[0].max.x,2);
        		if(temp_handpoint[0].y>objects[0].max.y)
	        		d=d+pow(temp_handpoint[0].y-objects[0].max.y,2);
	        	if(temp_handpoint[0].z>objects[0].max.z)
	        		d=d+pow(temp_handpoint[0].z-objects[0].max.z,2);

        		if(d>pow(radius,2))
	        		continue;
        		else
				{
	   	     		collision[i][2]=true;
				}   
			}
		}

  if(ob_Cylinder)
	{
	for(i=0;i<3;i++)           //对虚拟手和目标物进行碰撞检测
			for(j=1;j<2;j++)
				{
	     			disjoint=false;
					for(x=0;x<hand[i][j].NUMPTS;x++)
					{
						temp_handpoint[x].x=hand[i][j].points[x].x;
						temp_handpoint[x].y=hand[i][j].points[x].y;
						temp_handpoint[x].z=hand[i][j].points[x].z;

						RotateX(&temp_handpoint[x],rotatedX);
	            		RotateY(&temp_handpoint[x],rotatedY);
            			RotateZ(&temp_handpoint[x],-rotatedZ);
            		
					}

					for(x=0;x<objects[1].NUMPTS;x++)
					{
						temp_objectpoint[x].x=objects[1].points[x].x;
						temp_objectpoint[x].y=objects[1].points[x].y;
						temp_objectpoint[x].z=objects[1].points[x].z;

						RotateX(&temp_objectpoint[x],rotatedX);
	            		RotateY(&temp_objectpoint[x],rotatedY);
            			RotateZ(&temp_objectpoint[x],-rotatedZ);
					}
					
					BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                    BoundingBox(temp_objectpoint,&objects[1].max,&objects[1].min);
    				if((hand[i][j].max.x-objects[1].min.x)*(hand[i][j].min.x-objects[1].max.x)>0.0
					 ||(hand[i][j].max.y-objects[1].min.y)*(hand[i][j].min.y-objects[1].max.y)>0.0
					 ||(hand[i][j].max.z-objects[1].min.z)*(hand[i][j].min.z-objects[1].max.z)>0.0)
					continue;

					for(x=0;x<hand[i][j].NUMPTS;x++)
					{
						temp_handpoint[x].x=hand[i][j].points[x].x;
						temp_handpoint[x].y=hand[i][j].points[x].y;
						temp_handpoint[x].z=hand[i][j].points[x].z;

						RotateZ(&temp_handpoint[x],angle[i][0]);
						RotateX(&temp_handpoint[x],-angle[i][1]);
					}

					for(x=0;x<objects[1].NUMPTS;x++)
					{
						temp_objectpoint[x].x=objects[1].points[x].x;
						temp_objectpoint[x].y=objects[1].points[x].y;
						temp_objectpoint[x].z=objects[1].points[x].z;

						RotateZ(&temp_objectpoint[x],angle[i][0]);
						RotateX(&temp_objectpoint[x],-angle[i][1]);
					}                 

	        		BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                    BoundingBox(temp_objectpoint,&objects[1].max,&objects[1].min);
        			if((hand[i][j].max.x-objects[1].min.x)*(hand[i][j].min.x-objects[1].max.x)>0.0
	        		 ||(hand[i][j].max.y-objects[1].min.y)*(hand[i][j].min.y-objects[1].max.y)>0.0
	        		 ||(hand[i][j].max.z-objects[1].min.z)*(hand[i][j].min.z-objects[1].max.z)>0.0)

			    		continue;
					struct point AA[3];
					struct point BB[3];
					struct point CC;
					AA[0].x=1;AA[0].y=0;AA[0].z=0;
				    AA[1].x=0;AA[1].y=1;AA[1].z=0;
					AA[2].x=0;AA[2].y=0;AA[2].z=1;

					BB[0].x=hand[i][j].points[0].x-hand[i][j].points[1].x;
					BB[0].y=hand[i][j].points[0].y-hand[i][j].points[1].y;
					BB[0].z=hand[i][j].points[0].x-hand[i][j].points[1].z;

					BB[1].x=hand[i][j].points[0].x-hand[i][j].points[2].x;
					BB[1].y=hand[i][j].points[0].y-hand[i][j].points[2].y;
					BB[1].z=hand[i][j].points[0].x-hand[i][j].points[2].z;

					BB[2].x=hand[i][j].points[0].x-hand[i][j].points[4].x;
					BB[2].y=hand[i][j].points[0].y-hand[i][j].points[4].y;
					BB[2].z=hand[i][j].points[0].x-hand[i][j].points[4].z;
		

					int m,n;
					float  alph,thta;
					for(m=0;m<3;m++)
						for(n=0;n<3;n++)
						{
							cross(CC,AA[m],BB[n]);
							thta=atan2(CC.y,CC.z);
							float temp;
							temp=CC.x/sqrt(CC.x*CC.x+CC.y*CC.y+CC.z*CC.z);
							alph=acos(temp);

							for(x=0;x<hand[i][j].NUMPTS;x++)
							{
			        			temp_handpoint[x].x=hand[i][j].points[x].x;
			        			temp_handpoint[x].y=hand[i][j].points[x].y;
					        	temp_handpoint[x].z=hand[i][j].points[x].z;

					        	RotateX(&temp_handpoint[x],-thta*180.0/PI);
					        	RotateY(&temp_handpoint[x],-alph*180.0/PI);
							}

			        		for(x=0;x<objects[1].NUMPTS;x++)
							{
					        	temp_objectpoint[x].x=objects[1].points[x].x;
					        	temp_objectpoint[x].y=objects[1].points[x].y;
			        			temp_objectpoint[x].z=objects[1].points[x].z;

				        		RotateX(&temp_objectpoint[x],-thta*180.0/PI);
					        	RotateY(&temp_objectpoint[x],-alph*180.0/PI);
							} 
							BoundingBox(temp_handpoint,&hand[i][j].max,&hand[i][j].min);
                            BoundingBox(temp_objectpoint,&objects[1].max,&objects[1].min);

					if((hand[i][j].max.x-objects[1].min.x)*(hand[i][j].min.x-objects[1].max.x)>0.0
	        		 ||(hand[i][j].max.y-objects[1].min.y)*(hand[i][j].min.y-objects[1].max.y)>0.0
	        		 ||(hand[i][j].max.z-objects[1].min.z)*(hand[i][j].min.z-objects[1].max.z)>0.0)

							{
								disjoint=true;
			    	        	break;
							}

						}

						if(disjoint)				
							continue;

						if(i==0)
     						collision[0][1]=true;
						if(i==1)
							collision[1][1]=true;
						if(i==2)
							collision[2][1]=true;
                 }

        float d;


	if(!SecondGrasp)
		for(i=0;i<3;i++)
		{
			d=0;
			struct point temppoint;
			temppoint.x=hand[i][2].center_point.x;
			temppoint.y=hand[i][2].center_point.y;
//			temppoint.y=0;
			temppoint.z=hand[i][2].center_point.z;
			RotateX(&temppoint,-rotatedX);
//			RotateX(&temp_objectpoint[x],rotatedX);
//     		RotateY(&temp_objectpoint[x],rotatedY);
//     		RotateZ(&temp_objectpoint[x],-rotatedZ);
			
			d=pow((objects[1].center_point.x-temppoint.x),2)+
			  pow((objects[1].center_point.z-temppoint.z),2);

			if(d>pow((radius+ob_Cylinder_radius),2)
			||(hand[i][2].center_point.y>(objects[1].center_point.y+ob_Cylinder_height/2)))
				continue;
			else
				collision[i][2]=true;
		}
		else
			for(i=0;i<3;i++)
			{
			d=0;
			struct point temppoint;