三维模型STL碰撞检测算法实现

#pragma once
#ifndef _COLLISION_H_
#define _COLLISION_H_
#include "stdafx.h"
#include "Object.h"
string checkCollision(const Object& obj1, const Object &obj2)
{
string state = "UNKNOWN";
if (obj1.radius + obj2.radius < distance(obj1.MovingCenter, obj2.MovingCenter))
{
return "NO_COLLISION";
}
for (int i = 0; i < obj1.stl.num; i++)
{
for (int j = 0; j < obj2.stl.num; j++)
{
vector3f vi1 = obj1.stl.triangle[i].point[0] - obj2.stl.triangle[j].point[0];
vector3f vi2 = obj1.stl.triangle[i].point[1] - obj2.stl.triangle[j].point[0];
vector3f vi3 = obj1.stl.triangle[i].point[2] - obj2.stl.triangle[j].point[0];
GLfloat di1 = dotProduct(vi1, obj2.stl.triangle[j].normal);
GLfloat di2 = dotProduct(vi2, obj2.stl.triangle[j].normal);
GLfloat di3 = dotProduct(vi3, obj2.stl.triangle[j].normal);

vector3f vj1 = obj2.stl.triangle[j].point[0] - obj1.stl.triangle[i].point[0];
vector3f vj2 = obj2.stl.triangle[j].point[1] - obj1.stl.triangle[i].point[0];
vector3f vj3 = obj2.stl.triangle[j].point[2] - obj1.stl.triangle[i].point[0];
GLfloat dj1 = dotProduct(vj1, obj1.stl.triangle[i].normal);
GLfloat dj2 = dotProduct(vj2, obj1.stl.triangle[i].normal);
GLfloat dj3 = dotProduct(vj3, obj1.stl.triangle[i].normal);

//case 1
if ((di1 > 0 && di2 > 0 && di3 > 0) || (di1 < 0 && di2 < 0 && di3 < 0) || (dj1 > 0 && dj2 > 0 && dj3 > 0) || (dj1 < 0 && dj2 < 0 && dj3 < 0))
{
state = "NO_COLLISION";
}
//case 2
else if ((di1 == 0 && di2 == 0 && di3 == 0) || (dj1 == 0 && dj2 == 0 && dj3 == 0))
{
state = "NO_COLLISION";
}
//case 3
else if ((di1 == 0 && di2 == 0 && di3 != 0) ||
(di1 == 0 && di2 != 0 && di3 == 0) ||
(di1 != 0 && di2 == 0 && di3 == 0) ||

(dj1 == 0 && dj2 == 0 && dj3 != 0) ||
(dj1 == 0 && dj2 != 0 && dj3 == 0) ||
(dj1 != 0 && dj2 == 0 && dj3 == 0))
{
state = "NO_COLLISION";
}
//case 4
else //i dont sure should i write condition for this
{
state = "MAYBE";
}
///////////////////////////////////////////////////////
int step = 0, r1 = 0, r2 = 0, u = 0, v = 0;
if (state == "MAYBE")
{