282 lines
7.3 KiB
C++
282 lines
7.3 KiB
C++
/* ColDet - C++ 3D Collision Detection Library
|
|
* Copyright (C) 2000 Amir Geva
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Library General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Library General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Library General Public
|
|
* License along with this library; if not, write to the
|
|
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
|
|
* Boston, MA 02111-1307, USA.
|
|
*
|
|
* Any comments, questions and bug reports send to:
|
|
* photon@photoneffect.com
|
|
*
|
|
* Or visit the home page: http://photoneffect.com/coldet/
|
|
*/
|
|
#include "sysdep.h"
|
|
#include "box.h"
|
|
#include "mytritri.h"
|
|
|
|
__CD__BEGIN
|
|
////////////////////////////////////////////////////
|
|
// code from here is used in detection process
|
|
|
|
int BoxTreeInnerNode::getTrianglesNumber()
|
|
{
|
|
return m_Boxes.size();
|
|
}
|
|
|
|
BoxedTriangle* BoxTreeInnerNode::getTriangle(int which)
|
|
{
|
|
if (which<0 || which>=getTrianglesNumber()) return NULL;
|
|
return m_Boxes[which];
|
|
}
|
|
|
|
RotationState::RotationState(const Matrix3D& transform)
|
|
: t(transform)
|
|
{
|
|
N[0]=Vector3D(t._11,t._12,t._13);
|
|
N[1]=Vector3D(t._21,t._22,t._23);
|
|
N[2]=Vector3D(t._31,t._32,t._33);
|
|
}
|
|
|
|
inline float DotWithCol(const Vector3D& v, const Matrix3& m, int col)
|
|
{
|
|
return v.x*m(0,col) + v.y*m(1,col) + v.z*m(2,col);
|
|
}
|
|
|
|
bool Box::intersect(const Vector3D& O, float radius)
|
|
{
|
|
Vector3D mx=m_Pos+m_Size;
|
|
float dist=0.0f;
|
|
for(int i=0;i<3;i++)
|
|
{
|
|
if (O[i] < m_Pos[i])
|
|
{
|
|
float d=O[i]-m_Pos[i];
|
|
dist+=d*d;
|
|
}
|
|
else
|
|
if (O[i] > mx[i])
|
|
{
|
|
float d=O[i]-mx[i];
|
|
dist+=d*d;
|
|
}
|
|
}
|
|
return (dist <= (radius*radius));
|
|
}
|
|
|
|
bool Box::intersect(const Vector3D& O, const Vector3D& D,
|
|
float segmax)
|
|
{
|
|
if (segmax>3e30f) return intersect(O,D); // infinite ray
|
|
Vector3D abs_segdir, abs_diff, abs_cross;
|
|
|
|
Vector3D segdir=0.5f*segmax*D;
|
|
Vector3D seg_center=O+segdir;
|
|
Vector3D diff=seg_center - getCenter();
|
|
int i;
|
|
for(i=0;i<3;i++)
|
|
{
|
|
abs_segdir[i]=flabs(segdir[i]);
|
|
abs_diff[i]=flabs(diff[i]);
|
|
float f=getSize()[i] + abs_segdir[i];
|
|
if (abs_diff[i] > f) return false;
|
|
}
|
|
Vector3D cross=CrossProduct(segdir,diff);
|
|
int idx[] = {0,1,2,0,1};
|
|
for(i=0;i<3;i++)
|
|
{
|
|
int i1=idx[i+1];
|
|
int i2=idx[i+2];
|
|
abs_cross[i] = flabs(cross[i]);
|
|
float f = getSize()[i1]*abs_segdir[i2] + getSize()[i2]*abs_segdir[i1];
|
|
if ( abs_cross[i] > f ) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool Box::intersect(const Vector3D& O, const Vector3D& D)
|
|
{
|
|
Vector3D abs_segdir, abs_cross;
|
|
float f;
|
|
Vector3D diff = O - getCenter();
|
|
|
|
for(int i=0;i<3;i++)
|
|
{
|
|
abs_segdir[i] = flabs(D[i]);
|
|
if ( flabs(diff[i])>m_Size[i] && diff[i]*D[i]>=0.0f )
|
|
return false;
|
|
}
|
|
|
|
Vector3D cross = CrossProduct(D,diff);
|
|
|
|
abs_cross[0] = flabs(cross[0]);
|
|
f = m_Size[1]*abs_segdir[2] + m_Size[2]*abs_segdir[1];
|
|
if ( abs_cross[0] > f )
|
|
return false;
|
|
|
|
abs_cross[1] = flabs(cross[1]);
|
|
f = m_Size[0]*abs_segdir[2] + m_Size[2]*abs_segdir[0];
|
|
if ( abs_cross[1] > f )
|
|
return false;
|
|
|
|
abs_cross[2] = flabs(cross[2]);
|
|
f = m_Size[0]*abs_segdir[1] + m_Size[1]*abs_segdir[0];
|
|
if ( abs_cross[2] > f )
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Box::intersect(const Box& b, RotationState& rs)
|
|
{
|
|
const Vector3D bCenter=Transform(b.getCenter(),rs.t);
|
|
Vector3D EA=0.5f*getSize();
|
|
Vector3D EB=0.5f*b.getSize();
|
|
Vector3D distance=bCenter-getCenter();
|
|
Matrix3 C,abs_C;
|
|
float R0,R1,R,R01;
|
|
int i;
|
|
|
|
for(i=0;i<3;i++)
|
|
{
|
|
C(i,0)=rs.N[0][i];
|
|
C(i,1)=rs.N[1][i];
|
|
C(i,2)=rs.N[2][i];
|
|
abs_C(i,0)=flabs(C(i,0));
|
|
abs_C(i,1)=flabs(C(i,1));
|
|
abs_C(i,2)=flabs(C(i,2));
|
|
R=flabs(distance[i]);
|
|
R1=EB*abs_C.baseRow(i);
|
|
R01=EA[i]+R1;
|
|
if (R>R01) return false;
|
|
}
|
|
for(i=0;i<3;i++)
|
|
{
|
|
R=flabs(rs.N[i]*distance);
|
|
R0=DotWithCol(EA,abs_C,i);
|
|
R01=R0+EB[i];
|
|
if (R>R01) return false;
|
|
}
|
|
|
|
R=flabs(distance.z*C(1,0) - distance.y*C(2,0));
|
|
R0=EA.y*abs_C(2,0) + EA.z*abs_C(1,0);
|
|
R1=EB.y*abs_C(0,2) + EB.z*abs_C(0,1);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.z*C(1,1) - distance.y*C(2,1));
|
|
R0=EA.y*abs_C(2,1) + EA.z*abs_C(1,1);
|
|
R1=EB.x*abs_C(0,2) + EB.z*abs_C(0,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.z*C(1,2) - distance.y*C(2,2));
|
|
R0=EA.y*abs_C(2,2) + EA.z*abs_C(1,2);
|
|
R1=EB.x*abs_C(0,1) + EB.y*abs_C(0,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.x*C(2,0) - distance.z*C(0,0));
|
|
R0=EA.x*abs_C(2,0) + EA.z*abs_C(0,0);
|
|
R1=EB.y*abs_C(1,2) + EB.z*abs_C(1,1);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.x*C(2,1) - distance.z*C(0,1));
|
|
R0=EA.x*abs_C(2,1) + EA.z*abs_C(0,1);
|
|
R1=EB.x*abs_C(1,2) + EB.z*abs_C(1,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.x*C(2,2) - distance.z*C(0,2));
|
|
R0=EA.x*abs_C(2,2) + EA.z*abs_C(0,2);
|
|
R1=EB.x*abs_C(1,1) + EB.y*abs_C(1,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.y*C(0,0) - distance.x*C(1,0));
|
|
R0=EA.x*abs_C(1,0) + EA.y*abs_C(0,0);
|
|
R1=EB.y*abs_C(2,2) + EB.z*abs_C(2,1);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.y*C(0,1) - distance.x*C(1,1));
|
|
R0=EA.x*abs_C(1,1) + EA.y*abs_C(0,1);
|
|
R1=EB.x*abs_C(2,2) + EB.z*abs_C(2,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
R=flabs(distance.y*C(0,2) - distance.x*C(1,2));
|
|
R0=EA.x*abs_C(1,2) + EA.y*abs_C(0,2);
|
|
R1=EB.x*abs_C(2,1) + EB.y*abs_C(2,0);
|
|
R01=R0+R1;
|
|
if (R>R01) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
extern "C" {
|
|
int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
|
|
float U0[3],float U1[3],float U2[3]);
|
|
};
|
|
|
|
Triangle::Triangle(const Vector3D& _1, const Vector3D& _2, const Vector3D& _3)
|
|
: v1(_1), v2(_2), v3(_3), center((1.0f/3.0f)*(_1+_2+_3))
|
|
{}
|
|
|
|
bool Triangle::intersect(const Vector3D& O, const Vector3D& D, Vector3D& cp,
|
|
float& tparm, float segmax)
|
|
{
|
|
Plane p(v1,v2,v3);
|
|
float denom=p.normal*D;
|
|
if (IsZero(denom)) return false;
|
|
float t=-(p.d+p.normal*O)/denom;
|
|
if (t<=0.0f) return false;
|
|
if (t>segmax) return false;
|
|
TriangleDesc td(*this,p);
|
|
cp=O+t*D;
|
|
if (td.pointInTri(cp))
|
|
{
|
|
tparm=t;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool Triangle::intersect(const Vector3D& O, float radius, Vector3D& cp)
|
|
{
|
|
Plane p(v1,v2,v3);
|
|
float dist=p.Classify(O);
|
|
if (flabs(dist) > radius) return false;
|
|
Vector3D point=O-dist*p.normal;
|
|
TriangleDesc td(*this,p);
|
|
if (td.pointInTri(point))
|
|
{
|
|
cp=point;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool Triangle::intersect(const Triangle& t) const
|
|
{
|
|
return (tri_tri_intersect((float*)&v1.x,
|
|
(float*)&v2.x,
|
|
(float*)&v3.x,
|
|
(float*)&t.v1.x,
|
|
(float*)&t.v2.x,
|
|
(float*)&t.v3.x) != 0);
|
|
}
|
|
|
|
__CD__END
|