00001 #include "sysdep.h"
00002 #include "box.h"
00003 #include "mytritri.h"
00004
00005 __CD__BEGIN
00007
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);
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