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
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
00021 if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
00022
00023
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
00037 InitQuery(world0, world1);
00038
00039
00040
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
00061 _Distance(tree0->GetNodes(), tree1->GetNodes(), minD, p0, p1);
00062
00063
00064 TransformPoint4x3(point0, p0, *world1);
00065 TransformPoint4x3(point1, p1, *world1);
00066
00067
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
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
00081 if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
00082
00083
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
00095 InitQuery(world0, world1);
00096
00097
00098
00099
00100 if (_Collide(tree0->GetNodes(), tree1->GetNodes(), tolerance)){
00101
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
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
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
00245 VertexPointers VP0;
00246 VertexPointers VP1;
00247 mIMesh0->GetTriangle(VP0, id0);
00248 mIMesh1->GetTriangle(VP1, id1);
00249
00250
00251
00252
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
00259 return TriTriDist(u0, u1, u2,
00260 *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2],
00261 point0, point1);
00262 }