void CollisionModel3DImpl::finalize()
{
if (m_Final) throw Inconsistency();
// Prepare initial triangle list
m_Final=true;
for(unsigned i=0;i<m_Triangles.size();i++)
{
BoxedTriangle& bt=m_Triangles[i];
m_Root.m_Boxes.push_back(&bt);
}
int logdepth=0;
for(int num=m_Triangles.size();num>0;num>>=1,logdepth++);
m_Root.m_logdepth=int(logdepth*1.5f);
m_Root.divide(0);
}
void CollisionModel3D::finalize()
{
if (d->m_isFinalized)
throw Inconsistency();
// Prepare initial triangle list
d->m_isFinalized = true;
for (unsigned i = 0; i < d->m_triangles.size(); ++i) {
BoxedTriangle& bt = d->m_triangles[i];
d->m_root.m_Boxes.push_back(&bt);
}
int logdepth = 0;
for (std::size_t num = d->m_triangles.size(); num > 0; num >>= 1)
++logdepth;
d->m_root.m_logdepth = int(logdepth*1.5f);
d->m_root.divide(0);
}
void CollisionModel3DImpl::addTriangle(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3)
{
if (m_Final) throw Inconsistency();
m_Triangles.push_back(BoxedTriangle(v1,v2,v3));
}
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 CollisionModel3D::modelCollision(ModelCollisionTest *test, int maxProcessingTime) const
{
const CollisionModel3D* other = test->otherModel();
test->m_collides = false;
test->m_iColTri = -1;
test->m_iOtherColTri = -1;
test->m_colPointIsDirty = true;
test->m_maxProcessingTimedOut = false;
if (!d->m_isFinalized)
throw Inconsistency();
if (!other->d->m_isFinalized)
throw Inconsistency();
Matrix3D t = test->otherModelTransform() == NULL ? other->d->m_transform :
Private::toMatrix3D(test->otherModelTransform());
if (d->m_isStatic)
t *= d->m_invTransform;
else
t *= d->m_transform.Inverse();
RotationState rs(t);
int accuracyDepth = test->accuracyDepth();
if (accuracyDepth < 0)
accuracyDepth = 0xFFFFFF;
if (maxProcessingTime == 0)
maxProcessingTime = 0xFFFFFF;
const DWORD beginTime = GetTickCount();
DWORD endTime = 0;
const std::size_t num = std::max(d->m_triangles.size(), other->d->m_triangles.size());
std::size_t allocated = std::max(std::size_t(64), (num>>4));
std::vector<Check> checks(allocated);
int queue_idx = 1;
{ // Initialize first Check object
checks.front().m_first = &d->m_root;
checks.front().m_depth = 0;
checks.front().m_second = &other->d->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)) {
test->m_maxProcessingTimedOut = true;
return false;
}
// @@@ add depth check
//Check c=checks.back();
Check& c = checks[--queue_idx];
const BoxTreeNode* first = c.m_first;
const BoxTreeNode* second = c.m_second;
assert(first != NULL);
assert(second != NULL);
if (first->intersect(*second, rs)) {
std::size_t tnum1 = first->getTrianglesNumber();
std::size_t tnum2 = second->getTrianglesNumber();
if (tnum1 > 0 && tnum2 > 0) {
for (int i = 0; i < tnum2; i++) {
const BoxedTriangle* bt2 = second->getTriangle(i);
const Triangle tt(Transform(bt2->v1, rs.t),
Transform(bt2->v2, rs.t),
Transform(bt2->v3, rs.t));
for (int j = 0; j < tnum1; j++) {
const BoxedTriangle* bt1 = first->getTriangle(j);
if (tt.intersect(*bt1)) {
bt1->copyCoords(test->m_colTri);
test->m_iColTri = d->getTriangleIndex(bt1);
tt.copyCoords(test->m_otherColTri);
test->m_iOtherColTri = other->d->getTriangleIndex(bt2);
test->m_collides = true;
return true;
}
}
}
}
else if (first->getSonsNumber() == 0) {
const BoxTreeNode* s1 = second->getSon(0);
const 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) {
const BoxTreeNode* f1 = first->getSon(0);
const 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) {
const BoxTreeNode* f1=first->getSon(0);
const 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 {
const BoxTreeNode* s1 = second->getSon(0);
const 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;
}
