tgCollission.cpp
Go to the documentation of this file.
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         // p = ray.start + ray.dir * z (1)
00023         // p*n + d = 0   ; point p must be on plane (with normal vector n and distance d) (2)
00024         
00025         n = GetNormal(t1, t2, t3); // plane normal
00026         
00027         double d = - t1 * n;    // distance of plane form origin 
00028         
00029         // from (1) and (2)
00030         z = -( ray.start * n + d ) / (ray.dir * n);
00031 
00032         p = ray.start + ray.dir * (float)z;
00033         
00034         // test if point lies in triangle or not
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 //      printf("tgCollission::IntersectModels: %f %f\n", d, (m1.m_bs.radius+m2.m_bs.radius));
00101         
00102         if(d >= (m1.m_bs.radius+m2.m_bs.radius))
00103                 return false;
00104         
00105         // further tests
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 //      printf("tgCollission::IntersectModels: %f %f\n", d, (m1.m_bs.radius+m2.m_bs.radius));
00123         
00124         if(d >= (m1.m_bs.radius+m2.m_bs.radius))
00125                 return false;
00126         
00127         // further tests
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 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:26