00001
00002 #include <blort/TomGine/tgCollission.h>
00003 #include <stdexcept>
00004
00005 using namespace TomGine;
00006
00007 vec3 tgCollission::GetNormal(const vec3& v1, const vec3& v2, const vec3& v3)
00008 {
00009 vec3 n, e1, e2;
00010
00011 e1 = v2 - v1;
00012 e2 = v3 - v1;
00013
00014 n.cross(e1, e2);
00015 n.normalize();
00016
00017 return n;
00018 }
00019
00020 bool tgCollission::IntersectRayTriangle(vec3& p, vec3& n, double& z, const tgRay& ray, const vec3& t1, const vec3& t2, const vec3& t3)
00021 {
00022
00023
00024
00025 n = GetNormal(t1, t2, t3);
00026
00027 double d = - t1 * n;
00028
00029
00030 z = -( ray.start * n + d ) / (ray.dir * n);
00031
00032 p = ray.start + ray.dir * (float)z;
00033
00034
00035 return PointInTriangle(p, t1, t2, t3);
00036 }
00037
00038 bool tgCollission::IntersectRayModel(std::vector<vec3> &pl, std::vector<vec3> &nl, std::vector<double> &zl, const tgRay& ray, const tgModel& model)
00039 {
00040 bool hit = false;
00041 pl.clear();
00042 zl.clear();
00043
00044 for(unsigned int i=0; i<model.m_faces.size(); i++)
00045 {
00046 vec3 p;
00047 vec3 n;
00048 double z;
00049
00050 if(model.m_faces[i].v.size()<3)
00051 throw std::runtime_error("[tgCollission::IntersectRayModel] Error number of vertices to low (<3) for a face");
00052
00053 vec3 t1 = model.m_vertices[model.m_faces[i].v[0]].pos;
00054 vec3 t2 = model.m_vertices[model.m_faces[i].v[1]].pos;
00055 vec3 t3 = model.m_vertices[model.m_faces[i].v[2]].pos;
00056
00057 if(IntersectRayTriangle(p, n, z, ray, t1, t2, t3))
00058 {
00059 pl.push_back(p);
00060 nl.push_back(n);
00061 zl.push_back(z);
00062 hit = true;
00063 }
00064
00065 if(model.m_faces[i].v.size()==4)
00066 {
00067 t1 = model.m_vertices[model.m_faces[i].v[2]].pos;
00068 t2 = model.m_vertices[model.m_faces[i].v[3]].pos;
00069 t3 = model.m_vertices[model.m_faces[i].v[0]].pos;
00070
00071 if(IntersectRayTriangle(p, n, z, ray, t1, t2, t3))
00072 {
00073 pl.push_back(p);
00074 nl.push_back(n);
00075 zl.push_back(z);
00076 hit = true;
00077 }
00078 }
00079
00080 if(model.m_faces[i].v.size()>4)
00081 throw std::runtime_error("[tgCollission::IntersectRayModel] Error number of vertices to low (<3) for a face");
00082
00083 }
00084 return hit;
00085 }
00086
00087 bool tgCollission::IntersectModels(const tgModel &m1, const tgModel &m2, const tgPose &p1, const tgPose& p2)
00088 {
00089 vec3 t1, t2;
00090 mat3 R1, R2;
00091
00092 p1.GetPose(R1, t1);
00093 p2.GetPose(R2, t2);
00094
00095 vec3 center1 = t1 + R1 * m1.m_bs.center;
00096 vec3 center2 = t2 + R2 * m2.m_bs.center;
00097
00098 float d = (center1 - center2).length();
00099
00100
00101
00102 if(d >= (m1.m_bs.radius+m2.m_bs.radius))
00103 return false;
00104
00105
00106 return true;
00107 }
00108
00109 bool tgCollission::IntersectModels(const tgRenderModel &m1, const tgRenderModel &m2)
00110 {
00111 vec3 t1, t2;
00112 mat3 R1, R2;
00113
00114 m1.m_pose.GetPose(R1, t1);
00115 m2.m_pose.GetPose(R2, t2);
00116
00117 vec3 center1 = t1 + R1 * m1.m_bs.center;
00118 vec3 center2 = t2 + R2 * m2.m_bs.center;
00119
00120 float d = (center1 - center2).length();
00121
00122
00123
00124 if(d >= (m1.m_bs.radius+m2.m_bs.radius))
00125 return false;
00126
00127
00128 return true;
00129 }
00130
00131
00132 bool tgCollission::PointOnSameSide(const vec3& p1, const vec3& p2, const vec3& a, const vec3& b)
00133 {
00134 vec3 cp1, cp2;
00135 cp1.cross(b-a, p1-a);
00136 cp2.cross(b-a, p2-a);
00137
00138 return ((cp1*cp2) >= 0.0);
00139 }
00140
00141 bool tgCollission::PointInTriangle(const vec3& p, const vec3& t1, const vec3& t2, const vec3& t3)
00142 {
00143
00144 return( PointOnSameSide( p, t1, t2, t3 ) &&
00145 PointOnSameSide( p, t2, t3, t1 ) &&
00146 PointOnSameSide( p, t3, t1, t2 ));
00147 }
00148