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

box.cpp

Go to the documentation of this file.
00001 #include "sysdep.h"
00002 #include "box.h"
00003 #include "mytritri.h"
00004 
00005 __CD__BEGIN
00007 // code from here is used in detection process
00008 
00009 int BoxTreeInnerNode::getTrianglesNumber()
00010 {
00011   return m_Boxes.size();
00012 }
00013 
00014 BoxedTriangle* BoxTreeInnerNode::getTriangle(int which)
00015 {
00016   if (which<0 || which>=getTrianglesNumber()) return NULL;
00017   return m_Boxes[which];
00018 }
00019 
00020 RotationState::RotationState(const Matrix3D& transform)
00021 : t(transform)
00022 {
00023   N[0]=Vector3D(t._11,t._12,t._13);
00024   N[1]=Vector3D(t._21,t._22,t._23);
00025   N[2]=Vector3D(t._31,t._32,t._33);
00026 }
00027 
00028 inline float DotWithCol(const Vector3D& v, const Matrix3& m, int col)
00029 {
00030   return v.x*m(0,col) + v.y*m(1,col) + v.z*m(2,col);
00031 }
00032 
00033 bool Box::intersect(const Vector3D& O, float radius)
00034 {
00035   Vector3D mx=m_Pos+m_Size;
00036   float dist=0.0f;
00037   for(int i=0;i<3;i++)
00038   {
00039     if (O[i] < m_Pos[i])
00040     {
00041       float d=O[i]-m_Pos[i];
00042       dist+=d*d;
00043     }
00044     else
00045     if (O[i] > mx[i])
00046     {
00047       float d=O[i]-mx[i];
00048       dist+=d*d;
00049     }
00050   }
00051   return (dist <= (radius*radius));
00052 }
00053 
00054 bool Box::intersect(const Vector3D& O, const Vector3D& D,
00055                     float segmax)
00056 {
00057   if (segmax>3e30f) return intersect(O,D); // infinite ray
00058   Vector3D abs_segdir, abs_diff, abs_cross; 
00059 
00060   Vector3D segdir=0.5f*segmax*D;
00061   Vector3D seg_center=O+segdir;
00062   Vector3D diff=seg_center - getCenter();
00063   int i;
00064   for(i=0;i<3;i++)
00065   {
00066     abs_segdir[i]=flabs(segdir[i]);
00067     abs_diff[i]=flabs(diff[i]);
00068     float f=getSize()[i] + abs_segdir[i];
00069     if (abs_diff[i] > f) return false;
00070   }
00071   Vector3D cross=CrossProduct(segdir,diff);
00072   int idx[] = {0,1,2,0,1};
00073   for(i=0;i<3;i++)
00074   {
00075     int i1=idx[i+1];
00076     int i2=idx[i+2];
00077     abs_cross[i] = flabs(cross[i]);
00078     float f = getSize()[i1]*abs_segdir[i2] + getSize()[i2]*abs_segdir[i1];
00079     if ( abs_cross[i] > f ) return false;
00080   }
00081   return true;
00082 }
00083 
00084 bool Box::intersect(const Vector3D& O, const Vector3D& D)
00085 {
00086     Vector3D abs_segdir, abs_cross;
00087     float f;
00088     Vector3D diff = O - getCenter();
00089 
00090     for(int i=0;i<3;i++)
00091     {
00092       abs_segdir[i] = flabs(D[i]);
00093       if ( flabs(diff[i])>m_Size[i] && diff[i]*D[i]>=0.0f )
00094         return false;
00095     }
00096 
00097     Vector3D cross = CrossProduct(D,diff);
00098 
00099     abs_cross[0] = flabs(cross[0]);
00100     f = m_Size[1]*abs_segdir[2] + m_Size[2]*abs_segdir[1];
00101     if ( abs_cross[0] > f )
00102         return false;
00103 
00104     abs_cross[1] = flabs(cross[1]);
00105     f = m_Size[0]*abs_segdir[2] + m_Size[2]*abs_segdir[0];
00106     if ( abs_cross[1] > f )
00107         return false;
00108 
00109     abs_cross[2] = flabs(cross[2]);
00110     f = m_Size[0]*abs_segdir[1] + m_Size[1]*abs_segdir[0];
00111     if ( abs_cross[2] > f )
00112         return false;
00113 
00114     return true;
00115 }
00116 
00117 bool Box::intersect(const Box& b, RotationState& rs)
00118 {
00119   const Vector3D bCenter=Transform(b.getCenter(),rs.t);
00120   Vector3D EA=0.5f*getSize();
00121   Vector3D EB=0.5f*b.getSize();
00122   Vector3D distance=bCenter-getCenter();
00123   Matrix3 C,abs_C;
00124   float   R0,R1,R,R01;
00125   int i;
00126 
00127   for(i=0;i<3;i++)
00128   {
00129     C(i,0)=rs.N[0][i];
00130     C(i,1)=rs.N[1][i];
00131     C(i,2)=rs.N[2][i];
00132     abs_C(i,0)=flabs(C(i,0));
00133     abs_C(i,1)=flabs(C(i,1));
00134     abs_C(i,2)=flabs(C(i,2));
00135     R=flabs(distance[i]);
00136     R1=EB*abs_C.baseRow(i);
00137     R01=EA[i]+R1;
00138     if (R>R01) return false;
00139   }
00140   for(i=0;i<3;i++)
00141   {
00142     R=flabs(rs.N[i]*distance);
00143     R0=DotWithCol(EA,abs_C,i);
00144     R01=R0+EB[i];
00145     if (R>R01) return false;
00146   }
00147 
00148   R=flabs(distance.z*C(1,0) - distance.y*C(2,0));
00149   R0=EA.y*abs_C(2,0) + EA.z*abs_C(1,0);
00150   R1=EB.y*abs_C(0,2) + EB.z*abs_C(0,1);
00151   R01=R0+R1;
00152   if (R>R01) return false;
00153 
00154   R=flabs(distance.z*C(1,1) - distance.y*C(2,1));
00155   R0=EA.y*abs_C(2,1) + EA.z*abs_C(1,1);
00156   R1=EB.x*abs_C(0,2) + EB.z*abs_C(0,0);
00157   R01=R0+R1;
00158   if (R>R01) return false;
00159 
00160   R=flabs(distance.z*C(1,2) - distance.y*C(2,2));
00161   R0=EA.y*abs_C(2,2) + EA.z*abs_C(1,2);
00162   R1=EB.x*abs_C(0,1) + EB.y*abs_C(0,0);
00163   R01=R0+R1;
00164   if (R>R01) return false;
00165 
00166   R=flabs(distance.x*C(2,0) - distance.z*C(0,0));
00167   R0=EA.x*abs_C(2,0) + EA.z*abs_C(0,0);
00168   R1=EB.y*abs_C(1,2) + EB.z*abs_C(1,1);
00169   R01=R0+R1;
00170   if (R>R01) return false;
00171 
00172   R=flabs(distance.x*C(2,1) - distance.z*C(0,1));
00173   R0=EA.x*abs_C(2,1) + EA.z*abs_C(0,1);
00174   R1=EB.x*abs_C(1,2) + EB.z*abs_C(1,0);
00175   R01=R0+R1;
00176   if (R>R01) return false;
00177 
00178   R=flabs(distance.x*C(2,2) - distance.z*C(0,2));
00179   R0=EA.x*abs_C(2,2) + EA.z*abs_C(0,2);
00180   R1=EB.x*abs_C(1,1) + EB.y*abs_C(1,0);
00181   R01=R0+R1;
00182   if (R>R01) return false;
00183 
00184   R=flabs(distance.y*C(0,0) - distance.x*C(1,0));
00185   R0=EA.x*abs_C(1,0) + EA.y*abs_C(0,0);
00186   R1=EB.y*abs_C(2,2) + EB.z*abs_C(2,1);
00187   R01=R0+R1;
00188   if (R>R01) return false;
00189 
00190   R=flabs(distance.y*C(0,1) - distance.x*C(1,1));
00191   R0=EA.x*abs_C(1,1) + EA.y*abs_C(0,1);
00192   R1=EB.x*abs_C(2,2) + EB.z*abs_C(2,0);
00193   R01=R0+R1;
00194   if (R>R01) return false;
00195 
00196   R=flabs(distance.y*C(0,2) - distance.x*C(1,2));
00197   R0=EA.x*abs_C(1,2) + EA.y*abs_C(0,2);
00198   R1=EB.x*abs_C(2,1) + EB.y*abs_C(2,0);
00199   R01=R0+R1;
00200   if (R>R01) return false;
00201 
00202   return true;
00203 }
00204 
00205 extern "C" { 
00206 int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
00207                       float U0[3],float U1[3],float U2[3]);
00208 };
00209 
00210 Triangle::Triangle(const Vector3D& _1, const Vector3D& _2, const Vector3D& _3)
00211 : v1(_1), v2(_2), v3(_3), center((1.0f/3.0f)*(_1+_2+_3)) 
00212 {}
00213 
00214 bool Triangle::intersect(const Vector3D& O, const Vector3D& D, Vector3D& cp, 
00215                          float& tparm, float segmax)
00216 {
00217   Plane p(v1,v2,v3);
00218   float denom=p.normal*D;
00219   if (IsZero(denom)) return false;
00220   float t=-(p.d+p.normal*O)/denom;
00221   if (t<=0.0f) return false;
00222   if (t>segmax) return false;
00223   TriangleDesc td(*this,p);
00224   cp=O+t*D;
00225   if (td.pointInTri(cp))
00226   {
00227     tparm=t;
00228     return true;
00229   }
00230   return false;
00231 }
00232 
00233 bool Triangle::intersect(const Vector3D& O, float radius, Vector3D& cp)
00234 {
00235   Plane p(v1,v2,v3);
00236   float dist=p.Classify(O);
00237   if (flabs(dist) > radius) return false;
00238   Vector3D point=O-dist*p.normal;
00239   TriangleDesc td(*this,p);
00240   if (td.pointInTri(point))
00241   {
00242     cp=point;
00243     return true;
00244   }
00245   return false;
00246 }
00247 
00248 bool Triangle::intersect(const Triangle& t) const
00249 {
00250   return (tri_tri_intersect((float*)&v1.x,
00251                             (float*)&v2.x,
00252                             (float*)&v3.x,
00253                             (float*)&t.v1.x,
00254                             (float*)&t.v2.x,
00255                             (float*)&t.v3.x) != 0);
00256 }
00257 
00258 __CD__END

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