SSVTreeCollider.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include "SSVTreeCollider.h"
00003 #include "DistFuncs.h"
00004 
00005 static bool debug = false;
00006 
00007 SSVTreeCollider::SSVTreeCollider()
00008 {
00009 }
00010 
00011 bool SSVTreeCollider::Distance(BVTCache& cache, 
00012                                float& minD, Point &point0, Point&point1,
00013                                const Matrix4x4* world0, const Matrix4x4* world1)
00014 {
00015     // Checkings
00016     if(!cache.Model0 || !cache.Model1)                                                          return false;
00017     if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes())      return false;
00018     if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized())        return false;
00019 
00020     // Checkings
00021     if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))      return false;
00022     
00023     // Simple double-dispatch
00024     const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
00025     const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
00026     Distance(T0, T1, world0, world1, &cache, minD, point0, point1);
00027         return true;
00028 }
00029 
00030 void SSVTreeCollider::Distance(const AABBCollisionTree* tree0, 
00031                                const AABBCollisionTree* tree1, 
00032                                const Matrix4x4* world0, const Matrix4x4* world1, 
00033                                Pair* cache, float& minD, Point &point0, Point&point1)
00034 {
00035     if (debug) std::cout << "Distance()" << std::endl;
00036     // Init collision query
00037     InitQuery(world0, world1);
00038     
00039     // Compute initial value using temporal coherency
00040     // todo : cache should be used 
00041 
00042     const AABBCollisionNode *n;
00043     for (unsigned int i=0; i<tree0->GetNbNodes(); i++){
00044         n = tree0->GetNodes()+i;
00045         if (n->IsLeaf()){
00046             mId0 = n->GetPrimitive();
00047             break;
00048         }
00049     } 
00050     for (unsigned int i=0; i<tree1->GetNbNodes(); i++){
00051         n = tree1->GetNodes()+i;
00052         if (n->IsLeaf()){
00053             mId1 = n->GetPrimitive();
00054             break;
00055         }
00056     } 
00057     Point p0, p1;
00058     minD = PrimDist(mId0, mId1, p0, p1);
00059     
00060     // Perform distance computation
00061     _Distance(tree0->GetNodes(), tree1->GetNodes(), minD, p0, p1);
00062 
00063     // transform points
00064     TransformPoint4x3(point0, p0, *world1);
00065     TransformPoint4x3(point1, p1, *world1);
00066 
00067     // update cache
00068     cache->id0 = mId0;
00069     cache->id1 = mId1;
00070 }
00071 
00072 bool SSVTreeCollider::Collide(BVTCache& cache, double tolerance,
00073                               const Matrix4x4* world0, const Matrix4x4* world1)
00074 {
00075     // Checkings
00076     if(!cache.Model0 || !cache.Model1)                                                          return false;
00077     if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes())      return false;
00078     if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized())        return false;
00079 
00080     // Checkings
00081     if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))      return false;
00082     
00083     // Simple double-dispatch
00084     const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
00085     const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
00086     return Collide(T0, T1, world0, world1, &cache, tolerance);
00087 }
00088 
00089 bool SSVTreeCollider::Collide(const AABBCollisionTree* tree0, 
00090                               const AABBCollisionTree* tree1, 
00091                               const Matrix4x4* world0, const Matrix4x4* world1, 
00092                               Pair* cache, double tolerance)
00093 {
00094     // Init collision query
00095     InitQuery(world0, world1);
00096     
00097     // todo : cache should be used 
00098 
00099     // Perform collision detection
00100     if (_Collide(tree0->GetNodes(), tree1->GetNodes(), tolerance)){
00101         // update cache
00102         cache->id0 = mId0;
00103         cache->id1 = mId1;
00104         return true;
00105     }
00106 
00107     return false;
00108 }
00109 
00110 float SSVTreeCollider::SsvSsvDist(const AABBCollisionNode *b0, 
00111                                   const AABBCollisionNode *b1)
00112 {
00113     CollisionAABB::ssv_type t1, t2;
00114     t1 = b0->mAABB.mType;
00115     t2 = b1->mAABB.mType;
00116     if (t1 == CollisionAABB::SSV_PSS && t2 == CollisionAABB::SSV_PSS){
00117         return PssPssDist(b0->mAABB.mRadius, b0->mAABB.mCenter, 
00118                           b1->mAABB.mRadius, b1->mAABB.mCenter);
00119     }else if (t1 == CollisionAABB::SSV_PSS && t2 == CollisionAABB::SSV_LSS){
00120         return PssLssDist(b0->mAABB.mRadius, b0->mAABB.mCenter,
00121                           b1->mAABB.mRadius, b1->mAABB.mPoint0, b1->mAABB.mPoint1);
00122     }else if (t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_PSS){
00123         return LssPssDist(b0->mAABB.mRadius, b0->mAABB.mPoint0, b0->mAABB.mPoint1,
00124                           b1->mAABB.mRadius, b1->mAABB.mCenter);
00125     }else if (t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_LSS){
00126         return PssPssDist(sqrtf(b0->GetSize()), b0->mAABB.mCenter, 
00127                           sqrtf(b1->GetSize()), b1->mAABB.mCenter);
00128     }else{
00129         std::cerr << "this ssv combination is not supported" << std::endl;
00130     }
00131 }
00132 
00133 float SSVTreeCollider::PssPssDist(float r0, const Point& center0, float r1, const Point& center1)
00134 {
00135     Point c0;
00136     TransformPoint(c0, center0, mR0to1, mT0to1);
00137     return (center1-c0).Magnitude() - r0 - r1;
00138 }
00139 
00140 float SSVTreeCollider::PssLssDist(float r0, const Point& center0, float r1, const Point& point0, const Point& point1)
00141 {
00142     Point c0;
00143     TransformPoint(c0, center0, mR0to1, mT0to1);
00144     float d = PointSegDist(c0, point0, point1);
00145     return d - r0 - r1;
00146 }
00147 
00148 float SSVTreeCollider::LssPssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& center0)
00149 {
00150     Point p0, p1;
00151     TransformPoint(p0, point0, mR0to1, mT0to1);
00152     TransformPoint(p1, point1, mR0to1, mT0to1);
00153     float d = PointSegDist(center0, p0, p1);
00154     return d - r0 - r1;
00155 }
00156 
00157 float SSVTreeCollider::LssLssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3)
00158 {
00159     Point p0, p1;
00160     TransformPoint(p0, point0, mR0to1, mT0to1);
00161     TransformPoint(p1, point1, mR0to1, mT0to1);
00162     float d = SegSegDist(p0, p1, point2, point3);
00163     return d - r0 - r1;
00164 }
00165 
00166 void SSVTreeCollider::_Distance(const AABBCollisionNode* b0, const AABBCollisionNode* b1,
00167                                 float& minD, Point& point0, Point& point1)
00168 {
00169     if (debug) std::cout << "_Distance()" << std::endl;
00170 
00171     mNowNode0 = b0;
00172     mNowNode1 = b1;
00173     float d;
00174 
00175     // Perform BV-BV distance test
00176     d = SsvSsvDist(b0, b1);
00177 
00178     if(d > minD) return;
00179     
00180     if(b0->IsLeaf() && b1->IsLeaf()) { 
00181         Point p0, p1;
00182         d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1);
00183         if (d < minD){
00184             minD = d;
00185             point0 = p0;
00186             point1 = p1;
00187             mId0 = b0->GetPrimitive();
00188             mId1 = b1->GetPrimitive(); 
00189         }
00190         return;
00191     }
00192     
00193     if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
00194         {
00195             _Distance(b0->GetNeg(), b1, minD, point0, point1);
00196             _Distance(b0->GetPos(), b1, minD, point0, point1);
00197         }
00198     else
00199         {
00200             _Distance(b0, b1->GetNeg(), minD, point0, point1);
00201             _Distance(b0, b1->GetPos(), minD, point0, point1);
00202         }
00203 }
00204 
00205 bool SSVTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1,
00206                                double tolerance)
00207 {
00208     mNowNode0 = b0;
00209     mNowNode1 = b1;
00210     float d;
00211 
00212     // Perform BV-BV distance test
00213     d = SsvSsvDist(b0, b1);
00214 
00215     if(d > tolerance) return false;
00216     
00217     if(b0->IsLeaf() && b1->IsLeaf()) { 
00218         Point p0, p1;
00219         d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1);
00220         if (d <= tolerance){
00221             mId0 = b0->GetPrimitive();
00222             mId1 = b1->GetPrimitive(); 
00223             return true;
00224         }else{
00225             return false;
00226         }
00227     }
00228     
00229     if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
00230         {
00231             if (_Collide(b0->GetNeg(), b1, tolerance)) return true;
00232             if (_Collide(b0->GetPos(), b1, tolerance)) return true;
00233         }
00234     else
00235         {
00236             if (_Collide(b0, b1->GetNeg(), tolerance)) return true;
00237             if (_Collide(b0, b1->GetPos(), tolerance)) return true;
00238         }
00239     return false;
00240 }
00241 
00242 float SSVTreeCollider::PrimDist(udword id0, udword id1, Point& point0, Point& point1)
00243 {
00244     // Request vertices from the app
00245     VertexPointers VP0;
00246     VertexPointers VP1;
00247     mIMesh0->GetTriangle(VP0, id0);
00248     mIMesh1->GetTriangle(VP1, id1);
00249     
00250     // Modified by S-cubed, Inc.
00251     // Transform from space 0 (old : 1) to space 1 (old : 0)
00252     // CD では変換が逆なのであわせる。  
00253     Point u0,u1,u2;
00254     TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
00255     TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
00256     TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);
00257 
00258     // Perform triangle-triangle distance test
00259     return TriTriDist(u0, u1, u2,
00260                       *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2],
00261                       point0, point1);
00262 }


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:57