obbcollision.cpp
来自「obb碰撞检测类,可以在虚拟环境中进行碰撞检测」· C++ 代码 · 共 1,305 行 · 第 1/3 页
CPP
1,305 行
}
//计算顶点坐标
void COBBCollision::Dingdian_Zuobiao(point points[], point center_point,double x, double y, double z,double angle0,double angle1)
{
double a,b,c,x1,y1,z1,x2,y2,z2;
a=x;b=y;c=z;
points[0].x=a/2;
points[0].y=b/2;
points[0].z=c/2;
points[1].x=-a/2;
points[1].y=+b/2;
points[1].z=+c/2;
points[2].x=+a/2;
points[2].y=-b/2;
points[2].z=+c/2;
points[3].x=-a/2;
points[3].y=-b/2;
points[3].z=+c/2;
points[4].x=+a/2;
points[4].y=+b/2;
points[4].z=-c/2;
points[5].x=-a/2;
points[5].y=+b/2;
points[5].z=-c/2;
points[6].x=+a/2;
points[6].y=-b/2;
points[6].z=-c/2;
points[7].x=-a/2;
points[7].y=-b/2;
points[7].z=-c/2;
for(int i=0;i<8;i++)
{
x2=points[i].x;
y2=points[i].y;
z2=points[i].z;
x1=x2*cos(angle0*PI/180)-y2*sin(angle0*PI/180);
y1=x2*sin(angle0*PI/180)+y2*cos(angle0*PI/180);
z1=z2;
if(angle0==90||angle0==-90)
{
points[i].x= x1*cos(-angle1*PI/180)-z1*sin(-angle1*PI/180);
points[i].y= y1;
points[i].z= x1*sin(-angle1*PI/180)+z1*cos(-angle1*PI/180);
}
else
{
points[i].x=x1;
points[i].y= y1*cos(angle1*PI/180)+z1*sin(angle1*PI/180);
points[i].z=-y1*sin(angle1*PI/180)+z1*cos(angle1*PI/180); //绕x轴作旋转变换
}
}
points[0].x+=center_point.x;
points[0].y+=center_point.y;
points[0].z+=center_point.z;
points[1].x+=center_point.x;
points[1].y+=center_point.y;
points[1].z+=center_point.z;
points[2].x+=center_point.x;
points[2].y+=center_point.y;
points[2].z+=center_point.z;
points[3].x+=center_point.x;
points[3].y+=center_point.y;
points[3].z+=center_point.z;
points[4].x+=center_point.x;
points[4].y+=center_point.y;
points[4].z+=center_point.z;
points[5].x+=center_point.x;
points[5].y+=center_point.y;
points[5].z+=center_point.z;
points[6].x+=center_point.x;
points[6].y+=center_point.y;
points[6].z+=center_point.z;
points[7].x+=center_point.x;
points[7].y+=center_point.y;
points[7].z+=center_point.z;
}
void COBBCollision::RotateX(point *ptr, float angle) //绕X轴旋转
{
float x,y,z;
x=ptr->x;
y=ptr->y;
z=ptr->z;
ptr->x= x;
ptr->y= y*cos(angle*PI/180)+z*sin(angle*PI/180);
ptr->z=-y*sin(angle*PI/180)+z*cos(angle*PI/180);
}
void COBBCollision::RotateY(point *ptr, float angle) //绕Y轴旋转
{
float x,y,z;
x=ptr->x;
y=ptr->y;
z=ptr->z;
ptr->x= x*cos(angle*PI/180)-z*sin(angle*PI/180);
ptr->y= y;
ptr->z=x*sin(angle*PI/180)+z*cos(angle*PI/180);
}
void COBBCollision::RotateZ(point *ptr, float angle) //绕z轴旋转
{
float x,y,z;
x=ptr->x;
y=ptr->y;
z=ptr->z;
ptr->x=float(x*cos(angle*PI/180)-y*sin(angle*PI/180));
ptr->y=float(x*sin(angle*PI/180)+y*cos(angle*PI/180));
ptr->z=float(z);
}
void COBBCollision::Translate(point *ptr, point center_point) //平移
{
ptr->x+=center_point.x;
ptr->y+=center_point.y;
ptr->z+=center_point.z;
}
//计算中点坐标
void COBBCollision::Point_Center(point *ptr, point pt1, point pt2, int i,int n)
{
ptr->x=pt1.x+(i+1)*(pt2.x-pt1.x)/n;
ptr->y=pt1.y+(i+1)*(pt2.y-pt1.y)/n;
ptr->z=pt1.z+(i+1)*(pt2.z-pt1.z)/n;
}
//计算圆柱的顶点坐标
void COBBCollision::Vertex_Cylinder(point points[], float radius, float height)
{
for(int i=0;i<36;i++)
{
points[i].x=radius*sin(10*i*PI/180);
points[i].y=0;
points[i].z=radius*cos(10*i*PI/180);
points[i+36].x=points[i].x;
points[i+36].y=-height;
points[i+36].z=points[i].z;
}
}
void COBBCollision::BoundingBox(point points[], point *max, point *min)
{
int x;
max->x=float(-HUGE_VAL);
max->y=float(-HUGE_VAL);
max->z=float(-HUGE_VAL);
min->x=float(HUGE_VAL);
min->y=float(HUGE_VAL);
min->z=float(HUGE_VAL);
for(x=0;x<8;x++)
{
max->x=(max->x<points[x].x) ? points[x].x:max->x;
max->y=(max->y<points[x].y) ? points[x].y:max->y;
max->z=(max->z<points[x].z) ? points[x].z:max->z;
min->x=(min->x>points[x].x) ? points[x].x:min->x;
min->y=(min->y>points[x].y) ? points[x].y:min->y;
min->z=(min->z>points[x].z) ? points[x].z:min->z;
}
}
void COBBCollision::Dingdian_ZuobiaoGst(struct point points[8],CMatrix center_pointgst,double x,double y,double z)
{ double a,b,c,x1,y1,z1,x2,y2,z2;
a=x;b=y;c=z;
points[0].x=a/2;
points[0].y=b/2;
points[0].z=c/2;
points[1].x=-a/2;
points[1].y=+b/2;
points[1].z=+c/2;
points[2].x=+a/2;
points[2].y=-b/2;
points[2].z=+c/2;
points[3].x=-a/2;
points[3].y=-b/2;
points[3].z=+c/2;
points[4].x=+a/2;
points[4].y=+b/2;
points[4].z=-c/2;
points[5].x=-a/2;
points[5].y=+b/2;
points[5].z=-c/2;
points[6].x=+a/2;
points[6].y=-b/2;
points[6].z=-c/2;
points[7].x=-a/2;
points[7].y=-b/2;
points[7].z=-c/2;
CVector3 point[8];
int i;
for(i=0;i<8;i++)
{point[i].x=points[i].x;
point[i].y=points[i].y;
point[i].z=points[i].z;
}
CMatrix p[8];
for(i=0;i<8;i++)
{p[i]=Vector3ToMatrix41(&point[i],1);
p[i]=center_pointgst*p[i];
}
for(i=0;i<8;i++)
{point[i]=MatrixToVector3(0,0,p[i]);
}
for(i=0;i<8;i++)
{points[i].x=point[i].x;
points[i].y=point[i].y;
points[i].z=point[i].z;
}
}
void COBBCollision::Build_MoldGst()
{ 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);
}
}
CVector3 c_point;
{ //计算手指1的第一根指节的几何模型
hand[0][0].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[0][0].center_pointgst);
hand[0][0].center_point.x=c_point.x;
hand[0][0].center_point.y=c_point.y;
hand[0][0].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[0][0].points,hand[0][0].center_pointgst,width,flength,width);
}
{ //计算手指1的第二根指节的几何模型
hand[0][1].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[0][1].center_pointgst);
hand[0][1].center_point.x=c_point.x;
hand[0][1].center_point.y=c_point.y;
hand[0][1].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[0][1].points,hand[0][1].center_pointgst,width,slength,width);
}
{ //计算手指1的第三根指节的几何模型
hand[0][2].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[0][2].center_pointgst);
hand[0][2].center_point.x=c_point.x;
hand[0][2].center_point.y=c_point.y;
hand[0][2].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[0][2].points,hand[0][2].center_pointgst,width,slength,width);
}
{ //计算手指2的第一根指节的几何模型
hand[1][0].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[1][0].center_pointgst);
hand[1][0].center_point.x=c_point.x;
hand[1][0].center_point.y=c_point.y;
hand[1][0].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[1][0].points,hand[1][0].center_pointgst,width,flength,width);
}
{ //计算手指2的第二根指节的几何模型
hand[1][1].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[1][1].center_pointgst);
hand[1][1].center_point.x=c_point.x;
hand[1][1].center_point.y=c_point.y;
hand[1][1].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[1][1].points,hand[1][1].center_pointgst,width,slength,width);
}
{ //计算手指2的第三根指节的几何模型
hand[1][2].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[1][2].center_pointgst);
hand[1][2].center_point.x=c_point.x;
hand[1][2].center_point.y=c_point.y;
hand[1][2].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[1][2].points,hand[1][2].center_pointgst,width,slength,width);
}
{ //计算手指3的第一根指节的几何模型
hand[2][0].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[2][0].center_pointgst);
hand[2][0].center_point.x=c_point.x;
hand[2][0].center_point.y=c_point.y;
hand[2][0].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[2][0].points,hand[2][0].center_pointgst,width,flength,width);
}
{ //计算手指3的第二根指节的几何模型
hand[2][1].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[2][1].center_pointgst);
hand[2][1].center_point.x=c_point.x;
hand[2][1].center_point.y=c_point.y;
hand[2][1].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[2][1].points,hand[2][1].center_pointgst,width,slength,width);
}
{ //计算手指3的第三根指节的几何模型
hand[2][2].NUMPTS=8;
c_point=MatrixToVector3(0,3,hand[2][2].center_pointgst);
hand[2][2].center_point.x=c_point.x;
hand[2][2].center_point.y=c_point.y;
hand[2][2].center_point.z=c_point.z;
Dingdian_ZuobiaoGst(hand[2][2].points,hand[2][2].center_pointgst,width,slength,width);
}
}
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?