obbcollision.cpp
来自「obb碰撞检测类,可以在虚拟环境中进行碰撞检测」· C++ 代码 · 共 1,305 行 · 第 1/3 页
CPP
1,305 行
temppoint.x=hand[0][2].center_point.x;
temppoint.y=hand[0][2].center_point.y;
// temppoint.y=0;
temppoint.z=hand[0][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[0][2].center_point.y>(objects[1].center_point.y+ob_Cylinder_height/2)))
continue;
else
collision[i][2]=true;
}
}
if(ob_Sphere)
{
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[2].NUMPTS;x++)
{
temp_objectpoint[x].x=objects[2].points[x].x;
temp_objectpoint[x].y=objects[2].points[x].y;
temp_objectpoint[x].z=objects[2].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[2].max,&objects[2].min);
if((hand[i][j].max.x-objects[2].min.x)*(hand[i][j].min.x-objects[2].max.x)>0.0
||(hand[i][j].max.y-objects[2].min.y)*(hand[i][j].min.y-objects[2].max.y)>0.0
||(hand[i][j].max.z-objects[2].min.z)*(hand[i][j].min.z-objects[2].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[2].NUMPTS;x++)
{
temp_objectpoint[x].x=objects[2].points[x].x;
temp_objectpoint[x].y=objects[2].points[x].y;
temp_objectpoint[x].z=objects[2].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[2].max,&objects[2].min);
if((hand[i][j].max.x-objects[2].min.x)*(hand[i][j].min.x-objects[2].max.x)>0.0
||(hand[i][j].max.y-objects[2].min.y)*(hand[i][j].min.y-objects[2].max.y)>0.0
||(hand[i][j].max.z-objects[2].min.z)*(hand[i][j].min.z-objects[2].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[2].NUMPTS;x++)
{
temp_objectpoint[x].x=objects[2].points[x].x;
temp_objectpoint[x].y=objects[2].points[x].y;
temp_objectpoint[x].z=objects[2].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[2].max,&objects[2].min);
if((hand[i][j].max.x-objects[2].min.x)*(hand[i][j].min.x-objects[2].max.x)>0.0
||(hand[i][j].max.y-objects[2].min.y)*(hand[i][j].min.y-objects[2].max.y)>0.0
||(hand[i][j].max.z-objects[2].min.z)*(hand[i][j].min.z-objects[2].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;
d=pow((objects[2].center_point.x-hand[i][2].center_point.x),2)+
pow((objects[2].center_point.z-hand[i][2].center_point.z),2)+
pow((objects[2].center_point.y-hand[i][2].center_point.y),2);
if(d>pow((radius+ob_Sphere_radius),2))
continue;
else
collision[i][2]=true;
}
}
for(i=0;i<3;i++) //手指与桌面的碰撞
{
if(hand[i][2].center_point.y>10)
continue;
else
collision1=true;
}
for(i=0;i<3;i++)
{
point_finger[i].x=hand[i][2].center_point.x;
point_finger[i].y=hand[i][2].center_point.y;
point_finger[i].z=hand[i][2].center_point.z;
}
GraphicPoint[0]=0.6*hand[0][2].center_point.y;
}
void COBBCollision::Build_Mold()//构建几何模型
{
int i;
if(ob_Box)
{ //计算长方体目标的几何模型
objects[0].NUMPTS=8;
objects[0].center_point.x=moveX;
objects[0].center_point.y=ob_Box_height/2+moveY;
objects[0].center_point.z=moveZ;
// object_center.y=objects[0].center_point.y;
struct point temp;
temp.x=0;temp.y=0;temp.z=0;
Dingdian_Zuobiao(objects[0].points,temp,ob_Box_length,ob_Box_height
,ob_Box_width,0,0);
for(i=0;i<objects[0].NUMPTS;i++)
{
RotateZ(&objects[0].points[i], rotatedZ);
RotateY(&objects[0].points[i],-rotatedY);
RotateX(&objects[0].points[i],-rotatedX);
// RotateX(&objects[0].points[i],rotatedX);
// RotateY(&objects[0].points[i],rotatedY);
// RotateZ(&objects[0].points[i],-rotatedZ);
Translate(&objects[0].points[i],objects[0].center_point);
}
}
if(ob_Cylinder)
{ //计算圆柱体目标的几何模型
objects[1].NUMPTS=8;
objects[1].center_point.x=moveX;
objects[1].center_point.y=ob_Cylinder_height/2+moveY;
objects[1].center_point.z=moveZ;
// object_center.y=objects[1].center_point.y;
struct point temp;
temp.x=0;temp.y=0;temp.z=0;
Dingdian_Zuobiao(objects[1].points,temp,2*ob_Cylinder_radius,ob_Cylinder_height
,2*ob_Cylinder_radius,0,0);
for(i=0;i<objects[1].NUMPTS;i++)
{
RotateZ(&objects[1].points[i], rotatedZ);
RotateY(&objects[1].points[i],-rotatedY);
RotateX(&objects[1].points[i],-rotatedX);
// RotateX(&objects[0].points[i],rotatedX);
// RotateY(&objects[0].points[i],rotatedY);
// RotateZ(&objects[0].points[i],-rotatedZ);
Translate(&objects[1].points[i],objects[1].center_point);
}
RotateX(&objects[1].center_point,-hand_rx);
}
if(ob_Sphere)
{ //计算长方体目标的几何模型
objects[2].NUMPTS=8;
objects[2].center_point.x=moveX;
objects[2].center_point.y=ob_Sphere_radius+moveY;
objects[2].center_point.z=moveZ;
// object_center.y=objects[2].center_point.y;
struct point temp;
temp.x=0;temp.y=0;temp.z=0;
Dingdian_Zuobiao(objects[2].points,temp,2*ob_Sphere_radius,2*ob_Sphere_radius
,2*ob_Sphere_radius,0,0);
for(i=0;i<objects[2].NUMPTS;i++)
{
/* RotateZ(&objects[2].points[i], rotatedZ);
RotateY(&objects[2].points[i],-rotatedY);
RotateX(&objects[2].points[i],-rotatedX);*/
Translate(&objects[2].points[i],objects[2].center_point);
}
}
{ //计算手指1的第一根指节的几何模型
hand[0][0].NUMPTS=8;
hand[0][0].center_point.x=fingerInit[0].x-flength*sin(first[0]*PI/180)/2;
hand[0][0].center_point.y=fingerInit[0].y-flength*cos(first[0]*PI/180)/2;
hand[0][0].center_point.z=fingerInit[0].z;
Dingdian_Zuobiao(hand[0][0].points,hand[0][0].center_point,width,flength,width,-first[0]);
}
{ //计算手指1的第二根指节的几何模型
hand[0][1].NUMPTS=8;
hand[0][1].center_point.x=fingerInit[0].x-(flength+slength*cos(first[1]*PI/180)/2)
*sin(first[0]*PI/180);
hand[0][1].center_point.y=fingerInit[0].y-(flength+slength*cos(first[1]*PI/180)/2)
*cos(first[0]*PI/180);
hand[0][1].center_point.z=fingerInit[0].z+slength*sin(first[1]*PI/180)/2;
Dingdian_Zuobiao(hand[0][1].points,hand[0][1].center_point,width,slength,width,-first[0],first[1]);
for(i=0;i<8;i++)
{
RotateZ(&hand[0][1].points[i], hand_rz);
RotateY(&hand[0][1].points[i],-hand_ry);
RotateX(&hand[0][1].points[i],-hand_rx);
}
}
{ //计算手指1的第三根指节的几何模型
hand[0][2].NUMPTS=8;
hand[0][2].center_point.x=fingerInit[0].x-(flength+slength*cos(first[1]*PI/180)
+(tlength+length+0.5)*cos((first[1]+first[2])*PI/180))*sin(first[0]*PI/180);
hand[0][2].center_point.y=fingerInit[0].y-(flength+slength*cos(first[1]*PI/180)
+(tlength+length+0.5)*cos((first[1]+first[2])*PI/180))*cos(first[0]*PI/180);
hand[0][2].center_point.z=fingerInit[0].z+slength*sin(first[1]*PI/180)
+(tlength+length+0.5)*sin((first[1]+first[2])*PI/180);
Dingdian_Zuobiao(hand[0][2].points,hand[0][2].center_point,width,slength,width,-first[0],first[1]);
RotateZ(&hand[0][2].center_point, hand_rz);
RotateY(&hand[0][2].center_point,-hand_ry);
RotateX(&hand[0][2].center_point,-hand_rx);
}
{ //计算手指2的第一根指节的几何模型
hand[1][0].NUMPTS=8;
hand[1][0].center_point.x=fingerInit[1].x+flength*sin(second[0]*PI/180)/2;
hand[1][0].center_point.y=fingerInit[1].y-flength*cos(second[0]*PI/180)/2;
hand[1][0].center_point.z=fingerInit[1].z;
Dingdian_Zuobiao(hand[1][0].points,hand[1][0].center_point,width,flength,width,second[0]);
}
{ //计算手指2的第二根指节的几何模型
hand[1][1].NUMPTS=8;
hand[1][1].center_point.x=fingerInit[1].x+(flength+slength*cos(second[1]*PI/180)/2)
*sin(second[0]*PI/180);
hand[1][1].center_point.y=fingerInit[1].y-(flength+slength*cos(second[1]*PI/180)/2
)*cos(second[0]*PI/180);
hand[1][1].center_point.z=fingerInit[1].z-slength*sin(second[1]*PI/180)/2;
Dingdian_Zuobiao(hand[1][1].points,hand[1][1].center_point,width,slength,width,second[0],-second[1]);
for(i=0;i<8;i++)
{
RotateZ(&hand[1][1].points[i], hand_rz);
RotateY(&hand[1][1].points[i],-hand_ry);
RotateX(&hand[1][1].points[i],-hand_rx);
}
}
{ //计算手指2的第三根指节的几何模型
hand[1][2].NUMPTS=8;
hand[1][2].center_point.x=fingerInit[1].x+(flength+slength*cos(second[1]*PI/180)
+(tlength+length+0.5)*cos((second[1]+second[2])*PI/180))*sin(second[0]*PI/180);
hand[1][2].center_point.y=fingerInit[1].y-(flength+slength*cos(second[1]*PI/180)
+(tlength+length+0.5)*cos((second[1]+second[2])*PI/180))*cos(second[0]*PI/180);
hand[1][2].center_point.z=fingerInit[1].z-slength*sin(second[1]*PI/180)
-(tlength+length+0.5)*sin((second[1]+second[2])*PI/180);
Dingdian_Zuobiao(hand[1][2].points,hand[1][2].center_point,width,slength,width,second[0],-second[1]);
RotateZ(&hand[1][2].center_point, hand_rz);
RotateY(&hand[1][2].center_point,-hand_ry);
RotateX(&hand[1][2].center_point,-hand_rx);
}
{ //计算手指3的第一根指节的几何模型
hand[2][0].NUMPTS=8;
hand[2][0].center_point.x=fingerInit[2].x+flength*sin(third[0]*PI/180)/2;
hand[2][0].center_point.y=fingerInit[2].y-flength*cos(third[0]*PI/180)/2;
hand[2][0].center_point.z=fingerInit[2].z;
Dingdian_Zuobiao(hand[2][0].points,hand[2][0].center_point,width,flength,width,third[0]);
}
{ //计算手指3的第二根指节的几何模型
hand[2][1].NUMPTS=8;
hand[2][1].center_point.x=fingerInit[2].x+(flength+slength*cos(third[1]*PI/180)/2)
*sin(third[0]*PI/180);
hand[2][1].center_point.y=fingerInit[2].y-(flength+slength*cos(third[1]*PI/180)/2
)*cos(third[0]*PI/180);
hand[2][1].center_point.z=fingerInit[2].z-slength*sin(third[1]*PI/180)/2;
Dingdian_Zuobiao(hand[2][1].points,hand[2][1].center_point,width,slength,width,third[0],-third[1]);
for(i=0;i<8;i++)
{
RotateZ(&hand[2][1].points[i], hand_rz);
RotateY(&hand[2][1].points[i],-hand_ry);
RotateX(&hand[2][1].points[i],-hand_rx);
}
}
{ //计算手指3的第三根指节的几何模型
hand[2][2].NUMPTS=8;
hand[2][2].center_point.x=fingerInit[2].x+(flength+slength*cos(third[1]*PI/180)
+(tlength+length+0.5)*cos((third[1]+third[2])*PI/180))*sin(third[0]*PI/180);
hand[2][2].center_point.y=fingerInit[2].y-(flength+slength*cos(third[1]*PI/180)
+(tlength+length+0.5)*cos((third[1]+third[2])*PI/180))*cos(third[0]*PI/180);
hand[2][2].center_point.z=fingerInit[2].z-slength*sin(third[1]*PI/180)
-(tlength+length+0.5)*sin((third[1]+third[2])*PI/180);
Dingdian_Zuobiao(hand[2][2].points,hand[2][2].center_point,width,slength,width,third[0],-third[1]);
RotateZ(&hand[2][2].center_point, hand_rz);
RotateY(&hand[2][2].center_point,-hand_ry);
RotateX(&hand[2][2].center_point,-hand_rx);
}
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?