00001 #include "sysdep.h"
00002 #include "coldetimpl.h"
00003 #include "mytritri.h"
00004 #include <assert.h>
00005
00006 __CD__BEGIN
00007
00008 class Check
00009 {
00010 public:
00011 Check() {}
00012 Check(BoxTreeNode* f, BoxTreeNode* s, int d)
00013 : m_first(f), m_second(s), depth(d) {}
00014 BoxTreeNode* m_first;
00015 BoxTreeNode* m_second;
00016 int depth;
00017 };
00018
00019 bool CollisionModel3DImpl::collision(CollisionModel3D* other,
00020 int AccuracyDepth,
00021 int MaxProcessingTime,
00022 float* other_transform)
00023 {
00024 m_ColType=Models;
00025 CollisionModel3DImpl* o=static_cast<CollisionModel3DImpl*>(other);
00026 if (!m_Final) throw Inconsistency();
00027 if (!o->m_Final) throw Inconsistency();
00028 Matrix3D t=( other_transform==NULL ? o->m_Transform : *((Matrix3D*)other_transform) );
00029 if (m_Static) t *= m_InvTransform;
00030 else t *= m_Transform.Inverse();
00031 RotationState rs(t);
00032
00033 if (AccuracyDepth<0) AccuracyDepth=0xFFFFFF;
00034 if (MaxProcessingTime==0) MaxProcessingTime=0xFFFFFF;
00035
00036 DWORD EndTime,BeginTime = GetTickCount();
00037 int num=Max(m_Triangles.size(),o->m_Triangles.size());
00038 int Allocated=Max(64,(num>>4));
00039 std::vector<Check> checks(Allocated);
00040
00041 int queue_idx=1;
00042 Check& c=checks[0];
00043 c.m_first=&m_Root;
00044 c.depth=0;
00045 c.m_second=&o->m_Root;
00046 while (queue_idx>0)
00047 {
00048 if (queue_idx>(Allocated/2))
00049 {
00050 Check c;
00051 checks.insert(checks.end(),Allocated,c);
00052 Allocated*=2;
00053 }
00054 EndTime=GetTickCount();
00055 if (EndTime >= (BeginTime+MaxProcessingTime)) throw TimeoutExpired();
00056
00057
00058
00059 Check& c=checks[--queue_idx];
00060 BoxTreeNode* first=c.m_first;
00061 BoxTreeNode* second=c.m_second;
00062 assert(first!=NULL);
00063 assert(second!=NULL);
00064 if (first->intersect(*second,rs))
00065 {
00066 int tnum1=first->getTrianglesNumber();
00067 int tnum2=second->getTrianglesNumber();
00068 if (tnum1>0 && tnum2>0)
00069 {
00070 {
00071 for(int i=0;i<tnum2;i++)
00072 {
00073 BoxedTriangle* bt2=second->getTriangle(i);
00074 Triangle tt(Transform(bt2->v1,rs.t),Transform(bt2->v2,rs.t),Transform(bt2->v3,rs.t));
00075 for(int j=0;j<tnum1;j++)
00076 {
00077 BoxedTriangle* bt1=first->getTriangle(j);
00078 if (tt.intersect(*bt1))
00079 {
00080 m_ColTri1=*bt1;
00081 m_iColTri1=getTriangleIndex(bt1);
00082 m_ColTri2=tt;
00083 m_iColTri2=o->getTriangleIndex(bt2);
00084 return true;
00085 }
00086 }
00087 }
00088 }
00089 }
00090 else
00091 if (first->getSonsNumber()==0)
00092 {
00093 BoxTreeNode* s1=second->getSon(0);
00094 BoxTreeNode* s2=second->getSon(1);
00095 assert(s1!=NULL);
00096 assert(s2!=NULL);
00097
00098 Check& c1=checks[queue_idx++];
00099 c1.m_first=first;
00100 c1.m_second=s1;
00101
00102 Check& c2=checks[queue_idx++];
00103 c2.m_first=first;
00104 c2.m_second=s2;
00105 }
00106 else
00107 if (second->getSonsNumber()==0)
00108 {
00109 BoxTreeNode* f1=first->getSon(0);
00110 BoxTreeNode* f2=first->getSon(1);
00111 assert(f1!=NULL);
00112 assert(f2!=NULL);
00113
00114 Check& c1=checks[queue_idx++];
00115 c1.m_first=f1;
00116 c1.m_second=second;
00117
00118 Check& c2=checks[queue_idx++];
00119 c2.m_first=f2;
00120 c2.m_second=second;
00121 }
00122 else
00123 {
00124 float v1=first->getVolume();
00125 float v2=second->getVolume();
00126 if (v1>v2)
00127 {
00128 BoxTreeNode* f1=first->getSon(0);
00129 BoxTreeNode* f2=first->getSon(1);
00130 assert(f1!=NULL);
00131 assert(f2!=NULL);
00132
00133 Check& c1=checks[queue_idx++];
00134 c1.m_first=f1;
00135 c1.m_second=second;
00136
00137 Check& c2=checks[queue_idx++];
00138 c2.m_first=f2;
00139 c2.m_second=second;
00140 }
00141 else
00142 {
00143 BoxTreeNode* s1=second->getSon(0);
00144 BoxTreeNode* s2=second->getSon(1);
00145 assert(s1!=NULL);
00146 assert(s2!=NULL);
00147
00148 Check& c1=checks[queue_idx++];
00149 c1.m_first=first;
00150 c1.m_second=s1;
00151
00152 Check& c2=checks[queue_idx++];
00153 c2.m_first=first;
00154 c2.m_second=s2;
00155 }
00156 }
00157 }
00158 }
00159 return false;
00160 }
00161
00162 bool CollisionModel3DImpl::rayCollision(float origin[3],
00163 float direction[3],
00164 bool closest,
00165 float segmin,
00166 float segmax)
00167 {
00168 float mintparm=9e9f,tparm;
00169 Vector3D col_point;
00170 m_ColType=Ray;
00171 Vector3D O;
00172 Vector3D D;
00173 if (m_Static)
00174 {
00175 O=Transform(*(Vector3D*)origin,m_InvTransform);
00176 D=rotateVector(*(Vector3D*)direction,m_InvTransform);
00177 }
00178 else
00179 {
00180 Matrix3D inv=m_Transform.Inverse();
00181 O=Transform(*(Vector3D*)origin,inv);
00182 D=rotateVector(*(Vector3D*)direction,inv);
00183 }
00184 if (segmin!=0.0f)
00185 {
00186 O+=segmin*D;
00187 segmax-=segmin;
00188 segmin=0.0f;
00189 }
00190 if (segmax<segmin)
00191 {
00192 D=-D;
00193 segmax=-segmax;
00194 }
00195 std::vector<BoxTreeNode*> checks;
00196 checks.push_back(&m_Root);
00197 while (!checks.empty())
00198 {
00199 BoxTreeNode* b=checks.back();
00200 checks.pop_back();
00201 if (b->intersect(O,D,segmax))
00202 {
00203 int sons=b->getSonsNumber();
00204 if (sons)
00205 while (sons--) checks.push_back(b->getSon(sons));
00206 else
00207 {
00208 int tri=b->getTrianglesNumber();
00209 while (tri--)
00210 {
00211 BoxedTriangle* bt=b->getTriangle(tri);
00212 Triangle* t=static_cast<Triangle*>(bt);
00213 if (t->intersect(O,D,col_point,tparm,segmax))
00214 {
00215 if (closest)
00216 {
00217 if (tparm<mintparm)
00218 {
00219 mintparm=tparm;
00220 m_ColTri1=*bt;
00221 m_iColTri1=getTriangleIndex(bt);
00222 m_ColPoint=col_point;
00223 }
00224 }
00225 else
00226 {
00227 m_ColTri1=*bt;
00228 m_iColTri1=getTriangleIndex(bt);
00229 m_ColPoint=col_point;
00230 return true;
00231 }
00232 }
00233 }
00234 }
00235 }
00236 }
00237 if (closest && mintparm<9e9f) return true;
00238 return false;
00239 }
00240
00241 bool CollisionModel3DImpl::sphereCollision(float origin[3], float radius)
00242 {
00243 m_ColType=Sphere;
00244 Vector3D O;
00245 if (m_Static)
00246 O=Transform(*(Vector3D*)origin,m_InvTransform);
00247 else
00248 {
00249 Matrix3D inv=m_Transform.Inverse();
00250 O=Transform(*(Vector3D*)origin,inv);
00251 }
00252 std::vector<BoxTreeNode*> checks;
00253 checks.push_back(&m_Root);
00254 while (!checks.empty())
00255 {
00256 BoxTreeNode* b=checks.back();
00257 checks.pop_back();
00258 if (b->intersect(O,radius))
00259 {
00260 int sons=b->getSonsNumber();
00261 if (sons)
00262 while (sons--) checks.push_back(b->getSon(sons));
00263 else
00264 {
00265 int tri=b->getTrianglesNumber();
00266 while (tri--)
00267 {
00268 BoxedTriangle* bt=b->getTriangle(tri);
00269 Triangle* t=static_cast<Triangle*>(bt);
00270 if (t->intersect(O,radius,m_ColPoint))
00271 {
00272 m_ColTri1=*bt;
00273 m_iColTri1=getTriangleIndex(bt);
00274 return true;
00275 }
00276 }
00277 }
00278 }
00279 }
00280 return false;
00281 }
00282
00283 bool CollisionModel3DImpl::getCollidingTriangles(float t1[9], float t2[9], bool ModelSpace)
00284 {
00285 if (ModelSpace)
00286 {
00287 if (t1!=NULL)
00288 {
00289 *((Vector3D*)&t1[0]) = m_ColTri1.v1;
00290 *((Vector3D*)&t1[3]) = m_ColTri1.v2;
00291 *((Vector3D*)&t1[6]) = m_ColTri1.v3;
00292 }
00293 if (t2!=NULL)
00294 {
00295 *((Vector3D*)&t2[0]) = m_ColTri2.v1;
00296 *((Vector3D*)&t2[3]) = m_ColTri2.v2;
00297 *((Vector3D*)&t2[6]) = m_ColTri2.v3;
00298 }
00299 }
00300 else
00301 {
00302 if (t1!=NULL)
00303 {
00304 *((Vector3D*)&t1[0]) = Transform(m_ColTri1.v1,m_Transform);
00305 *((Vector3D*)&t1[3]) = Transform(m_ColTri1.v2,m_Transform);
00306 *((Vector3D*)&t1[6]) = Transform(m_ColTri1.v3,m_Transform);
00307 }
00308 if (t2!=NULL)
00309 {
00310 *((Vector3D*)&t2[0]) = Transform(m_ColTri2.v1,m_Transform);
00311 *((Vector3D*)&t2[3]) = Transform(m_ColTri2.v2,m_Transform);
00312 *((Vector3D*)&t2[6]) = Transform(m_ColTri2.v3,m_Transform);
00313 }
00314 }
00315 return true;
00316 }
00317
00318 bool CollisionModel3DImpl::getCollidingTriangles(int& t1, int& t2)
00319 {
00320 t1=m_iColTri1;
00321 t2=m_iColTri2;
00322 return true;
00323 }
00324
00325 bool CollisionModel3DImpl::getCollisionPoint(float p[3], bool ModelSpace)
00326 {
00327 Vector3D& v=*((Vector3D*)p);
00328 switch (m_ColType)
00329 {
00330 case Models: v=my_tri_tri_intersect(m_ColTri1,m_ColTri2); break;
00331 case Sphere:
00332 case Ray: v=m_ColPoint; break;
00333 default: v=Vector3D::Zero;
00334 }
00335 if (!ModelSpace) v=Transform(v,m_Transform);
00336 return true;
00337 }
00338
00339 bool SphereRayCollision(float center[3], float radius,
00340 float origin[3], float direction[3],
00341 float point[3])
00342 {
00343 Vector3D& C=*((Vector3D*)center);
00344 Vector3D& O=*((Vector3D*)origin);
00345 Vector3D D=((Vector3D*)direction)->Normalized();
00346 Vector3D& P=*((Vector3D*)point);
00347 Vector3D EO=C-O;
00348 float v=EO*D;
00349 float disc=radius*radius - (EO*EO - v*v);
00350 if (disc<0.0f) return false;
00351 float d=sqrt(disc);
00352 P=O+(v-d)*D;
00353 return true;
00354 }
00355
00356 bool SphereSphereCollision(float c1[3], float r1,
00357 float c2[3], float r2)
00358 {
00359 Vector3D& C1=*((Vector3D*)c1);
00360 Vector3D& C2=*((Vector3D*)c2);
00361 float dist=(C2-C1).SquareMagnitude();
00362 float sum=r1+r2;
00363 return (dist < sum*sum);
00364 }
00365
00366 __CD__END