OPC_PlanesTriOverlap.h
Go to the documentation of this file.
00001 #include "../CollisionData.h"
00002 #include "../CollisionPairInserter.h"
00004 
00010 
00011 inline_ BOOL PlanesCollider::PlanesTriOverlap(udword in_clip_mask)
00012 {
00013         // Stats
00014         mNbVolumePrimTests++;
00015 
00016         const Plane* p = mPlanes;
00017         udword Mask = 1;
00018 
00019         while(Mask<=in_clip_mask)
00020         {
00021                 if(in_clip_mask & Mask)
00022                 {
00023                         float d0 = p->Distance(*mVP.Vertex[0]);
00024                         float d1 = p->Distance(*mVP.Vertex[1]);
00025                         float d2 = p->Distance(*mVP.Vertex[2]);
00026                         if(d0>0.0f && d1>0.0f && d2>0.0f)       return FALSE;
00027 //                      if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK))  return FALSE;
00028                         if (collisionPairInserter){
00029                             hrp::CollisionPairInserterBase &c = *collisionPairInserter;
00030                             hrp::collision_data cdata;
00031                             cdata.num_of_i_points = 2;
00032                             cdata.i_point_new[0] = cdata.i_point_new[1] = 1;
00033                             cdata.i_point_new[2] = cdata.i_point_new[3] = 0;
00034                             hrp::Vector3 relN; 
00035                             hrp::getVector3(relN, p->n);
00036                             cdata.n_vector = c.CD_Rot1*relN; 
00037                             Point p1, p2;
00038                             if (d0<=0 && d1>0 && d2>0){
00039                                 cdata.depth = -d0;
00040                                 p1 = ((*mVP.Vertex[0])*d1-(*mVP.Vertex[1])*d0)/(d1-d0);
00041                                 p2 = ((*mVP.Vertex[0])*d2-(*mVP.Vertex[2])*d0)/(d2-d0);
00042                             }else if(d0>0 && d1<=0 && d2>0){
00043                                 cdata.depth = -d1;
00044                                 p1 = ((*mVP.Vertex[1])*d0-(*mVP.Vertex[0])*d1)/(d0-d1);
00045                                 p2 = ((*mVP.Vertex[1])*d2-(*mVP.Vertex[2])*d1)/(d2-d1);
00046                             }else if(d0>0 && d1>0 && d2<=0){
00047                                 cdata.depth = -d2;
00048                                 p1 = ((*mVP.Vertex[2])*d0-(*mVP.Vertex[0])*d2)/(d0-d2);
00049                                 p2 = ((*mVP.Vertex[2])*d1-(*mVP.Vertex[1])*d2)/(d1-d2);
00050                             }else if(d0<=0 && d1<=0 && d2>0){
00051                                 cdata.depth = d0 > d1 ? -d1 : -d0;
00052                                 p1 = ((*mVP.Vertex[0])*d2-(*mVP.Vertex[2])*d0)/(d2-d0);
00053                                 p2 = ((*mVP.Vertex[1])*d2-(*mVP.Vertex[2])*d1)/(d2-d1);
00054                             }else if(d0<=0 && d1>0 && d2<=0){
00055                                 cdata.depth = d0 > d2 ? -d2 : -d0;
00056                                 p1 = ((*mVP.Vertex[0])*d1-(*mVP.Vertex[1])*d0)/(d1-d0);
00057                                 p2 = ((*mVP.Vertex[2])*d1-(*mVP.Vertex[1])*d2)/(d1-d2);
00058                             }else if(d0>0 && d1<=0 && d2<=0){
00059                                 cdata.depth = d1 > d2 ? -d2 : -d1;
00060                                 p1 = ((*mVP.Vertex[1])*d0-(*mVP.Vertex[0])*d1)/(d0-d1);
00061                                 p2 = ((*mVP.Vertex[2])*d0-(*mVP.Vertex[0])*d2)/(d0-d2);
00062                             }
00063                             if (d0>0||d1>0||d2>0){
00064                                 hrp::Vector3 v1, v2;
00065                                 hrp::getVector3(v1, p1);
00066                                 hrp::getVector3(v2, p2);
00067                                 cdata.i_points[0] = c.CD_s1 * (c.CD_Rot1 * v1 + c.CD_Trans1);
00068                                 cdata.i_points[1] = c.CD_s1 * (c.CD_Rot1 * v2 + c.CD_Trans1);
00069                                 collisionPairInserter->collisions().push_back(cdata);
00070                             }
00071                         }
00072                 }
00073                 Mask+=Mask;
00074                 p++;
00075         }
00076 /*
00077         for(udword i=0;i<6;i++)
00078         {
00079                 float d0 = p[i].Distance(mLeafVerts[0]);
00080                 float d1 = p[i].Distance(mLeafVerts[1]);
00081                 float d2 = p[i].Distance(mLeafVerts[2]);
00082                 if(d0>0.0f && d1>0.0f && d2>0.0f)       return false;
00083         }
00084 */
00085         return TRUE;
00086 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55