Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

coldet.cpp

Go to the documentation of this file.
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)) // enlarge the queue.
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     // @@@ add depth check
00058     //Check c=checks.back();
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) // normalize ray
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

Generated at Sat Nov 18 00:15:14 2000 for coldet by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000