bool CollisionModel3DImpl::sphereCollision(float origin[3], float radius)
{
m_NumTriangles = 0;
bool collided = false;
m_ColType=Sphere;
Vector3D O;
if (m_Static)
O=Transform(*(Vector3D*)origin,m_InvTransform);
else
{
Matrix3D inv=m_Transform.Inverse();
O=Transform(*(Vector3D*)origin,inv);
}
std::vector<BoxTreeNode*> checks;
checks.push_back(&m_Root);
while (!checks.empty())
{
BoxTreeNode* b=checks.back();
checks.pop_back();
if (b->intersect(O,radius))
{
int sons=b->getSonsNumber();
if (sons)
while (sons--) checks.push_back(b->getSon(sons));
else
{
int tri=b->getTrianglesNumber();
while (tri--)
{
BoxedTriangle* bt=b->getTriangle(tri);
Triangle* t=static_cast<Triangle*>(bt);
if (t->intersect(O,radius,m_ColPoint))
{
m_ColTri1[m_NumTriangles++]=*bt;
m_iColTri1=getTriangleIndex(bt);
collided = true;
if(m_NumTriangles > 63)
return true;
//return true;
}
}
}
}
}
return collided;
}
void TetrahedronSetTopologyContainer::createTrianglesInTetrahedronArray()
{
if(!hasTriangles())
createTriangleSetArray();
if(hasTrianglesInTetrahedron())
clearTrianglesInTetrahedron();
m_trianglesInTetrahedron.resize( getNumberOfTetrahedra());
helper::ReadAccessor< Data< sofa::helper::vector<Tetrahedron> > > m_tetrahedron = d_tetrahedron;
for(unsigned int i = 0; i < m_tetrahedron.size(); ++i)
{
const Tetrahedron &t=m_tetrahedron[i];
// adding triangles in the triangle list of the ith tetrahedron i
for (unsigned int j=0; j<4; ++j)
{
const int triangleIndex = getTriangleIndex(t[(j+1)%4], t[(j+2)%4], t[(j+3)%4]);
m_trianglesInTetrahedron[i][j] = (unsigned int) triangleIndex;
}
}
}
bool CollisionModel3DImpl::collision(CollisionModel3D* other,
int AccuracyDepth,
int MaxProcessingTime,
float* other_transform)
{
m_ColType=Models;
CollisionModel3DImpl* o=static_cast<CollisionModel3DImpl*>(other);
if (!m_Final) throw Inconsistency();
if (!o->m_Final) throw Inconsistency();
Matrix3D t=( other_transform==NULL ? o->m_Transform : *((Matrix3D*)other_transform) );
if (m_Static) t *= m_InvTransform;
else t *= m_Transform.Inverse();
RotationState rs(t);
if (AccuracyDepth<0) AccuracyDepth=0xFFFFFF;
if (MaxProcessingTime==0) MaxProcessingTime=0xFFFFFF;
DWORD EndTime,BeginTime = GetTickCount();
int num=Max(m_Triangles.size(),o->m_Triangles.size());
int Allocated=Max(64,(num>>4));
std::vector<Check> checks(Allocated);
int queue_idx=1;
Check& c=checks[0];
c.m_first=&m_Root;
c.depth=0;
c.m_second=&o->m_Root;
while (queue_idx>0)
{
if (queue_idx>(Allocated/2)) // enlarge the queue.
{
Check c;
checks.insert(checks.end(),Allocated,c);
Allocated*=2;
}
EndTime=GetTickCount();
if (EndTime >= (BeginTime+MaxProcessingTime)) throw TimeoutExpired();
// @@@ add depth check
//Check c=checks.back();
Check& c=checks[--queue_idx];
BoxTreeNode* first=c.m_first;
BoxTreeNode* second=c.m_second;
assert(first!=NULL);
assert(second!=NULL);
if (first->intersect(*second,rs))
{
int tnum1=first->getTrianglesNumber();
int tnum2=second->getTrianglesNumber();
if (tnum1>0 && tnum2>0)
{
{
for(int i=0;i<tnum2;i++)
{
BoxedTriangle* bt2=second->getTriangle(i);
Triangle tt(Transform(bt2->v1,rs.t),Transform(bt2->v2,rs.t),Transform(bt2->v3,rs.t));
for(int j=0;j<tnum1;j++)
{
BoxedTriangle* bt1=first->getTriangle(j);
if (tt.intersect(*bt1))
{
m_ColTri1=*bt1;
m_iColTri1=getTriangleIndex(bt1);
m_ColTri2=tt;
m_iColTri2=o->getTriangleIndex(bt2);
return true;
}
}
}
}
}
else
if (first->getSonsNumber()==0)
{
BoxTreeNode* s1=second->getSon(0);
BoxTreeNode* s2=second->getSon(1);
assert(s1!=NULL);
assert(s2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=first;
c1.m_second=s1;
Check& c2=checks[queue_idx++];
c2.m_first=first;
c2.m_second=s2;
}
else
if (second->getSonsNumber()==0)
{
BoxTreeNode* f1=first->getSon(0);
BoxTreeNode* f2=first->getSon(1);
assert(f1!=NULL);
assert(f2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=f1;
c1.m_second=second;
Check& c2=checks[queue_idx++];
c2.m_first=f2;
c2.m_second=second;
}
else
{
float v1=first->getVolume();
float v2=second->getVolume();
if (v1>v2)
{
BoxTreeNode* f1=first->getSon(0);
BoxTreeNode* f2=first->getSon(1);
assert(f1!=NULL);
assert(f2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=f1;
c1.m_second=second;
Check& c2=checks[queue_idx++];
c2.m_first=f2;
c2.m_second=second;
}
else
{
BoxTreeNode* s1=second->getSon(0);
BoxTreeNode* s2=second->getSon(1);
assert(s1!=NULL);
assert(s2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=first;
c1.m_second=s1;
Check& c2=checks[queue_idx++];
c2.m_first=first;
c2.m_second=s2;
}
}
}
}
return false;
}
bool CollisionModel3DImpl::rayCollision(float origin[3],
float direction[3],
bool closest,
float segmin,
float segmax)
{
float mintparm=9e9f,tparm;
Vector3D col_point;
m_ColType=Ray;
Vector3D O;
Vector3D D;
if (m_Static)
{
O=Transform(*(Vector3D*)origin,m_InvTransform);
D=rotateVector(*(Vector3D*)direction,m_InvTransform);
}
else
{
Matrix3D inv=m_Transform.Inverse();
O=Transform(*(Vector3D*)origin,inv);
D=rotateVector(*(Vector3D*)direction,inv);
}
if (segmin!=0.0f) // normalize ray
{
O+=segmin*D;
segmax-=segmin;
segmin=0.0f;
}
if (segmax<segmin)
{
D=-D;
segmax=-segmax;
}
std::vector<BoxTreeNode*> checks;
checks.push_back(&m_Root);
while (!checks.empty())
{
BoxTreeNode* b=checks.back();
checks.pop_back();
if (b->intersect(O,D,segmax))
{
int sons=b->getSonsNumber();
if (sons)
while (sons--) checks.push_back(b->getSon(sons));
else
{
int tri=b->getTrianglesNumber();
while (tri--)
{
BoxedTriangle* bt=b->getTriangle(tri);
Triangle* t=static_cast<Triangle*>(bt);
if (t->intersect(O,D,col_point,tparm,segmax))
{
if (closest)
{
if (tparm<mintparm)
{
mintparm=tparm;
m_ColTri1=*bt;
m_iColTri1=getTriangleIndex(bt);
m_ColPoint=col_point;
}
}
else
{
m_ColTri1=*bt;
m_iColTri1=getTriangleIndex(bt);
m_ColPoint=col_point;
return true;
}
}
}
}
}
}
if (closest && mintparm<9e9f) return true;
return false;
}