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; }