diff options
Diffstat (limited to 'extern/solid/src/convex/DT_PenDepth.cpp')
-rw-r--r-- | extern/solid/src/convex/DT_PenDepth.cpp | 376 |
1 files changed, 0 insertions, 376 deletions
diff --git a/extern/solid/src/convex/DT_PenDepth.cpp b/extern/solid/src/convex/DT_PenDepth.cpp deleted file mode 100644 index e1c5c9a3949..00000000000 --- a/extern/solid/src/convex/DT_PenDepth.cpp +++ /dev/null @@ -1,376 +0,0 @@ -/* - * SOLID - Software Library for Interference Detection - * - * Copyright (C) 2001-2003 Dtecta. All rights reserved. - * - * This library may be distributed under the terms of the Q Public License - * (QPL) as defined by Trolltech AS of Norway and appearing in the file - * LICENSE.QPL included in the packaging of this file. - * - * This library may be distributed and/or modified under the terms of the - * GNU General Public License (GPL) version 2 as published by the Free Software - * Foundation and appearing in the file LICENSE.GPL included in the - * packaging of this file. - * - * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Commercial use or any other use of this library not covered by either - * the QPL or the GPL requires an additional license from Dtecta. - * Please contact info@dtecta.com for enquiries about the terms of commercial - * use of this library. - */ - -#include "DT_PenDepth.h" - -#include "MT_Vector3.h" -#include "MT_Point3.h" -#include "MT_Quaternion.h" -#include "DT_Convex.h" -#include "DT_GJK.h" -#include "DT_Facet.h" - -//#define DEBUG - -const int MaxSupportPoints = 1000; -const int MaxFacets = 2000; - -static MT_Point3 pBuf[MaxSupportPoints]; -static MT_Point3 qBuf[MaxSupportPoints]; -static MT_Vector3 yBuf[MaxSupportPoints]; - -static DT_Facet facetBuf[MaxFacets]; -static int freeFacet = 0; -static DT_Facet *facetHeap[MaxFacets]; -static int num_facets; - -class DT_FacetComp { -public: - - bool operator()(const DT_Facet *face1, const DT_Facet *face2) - { - return face1->getDist2() > face2->getDist2(); - } - -} facetComp; - -inline DT_Facet *addFacet(int i0, int i1, int i2, - MT_Scalar lower2, MT_Scalar upper2) -{ - assert(i0 != i1 && i0 != i2 && i1 != i2); - if (freeFacet < MaxFacets) - { - DT_Facet *facet = new(&facetBuf[freeFacet++]) DT_Facet(i0, i1, i2); -#ifdef DEBUG - std::cout << "Facet " << i0 << ' ' << i1 << ' ' << i2; -#endif - if (facet->computeClosest(yBuf)) - { - if (facet->isClosestInternal() && - lower2 <= facet->getDist2() && facet->getDist2() <= upper2) - { - facetHeap[num_facets++] = facet; - std::push_heap(&facetHeap[0], &facetHeap[num_facets], facetComp); -#ifdef DEBUG - std::cout << " accepted" << std::endl; -#endif - } - else - { -#ifdef DEBUG - std::cout << " rejected, "; - if (!facet->isClosestInternal()) - { - std::cout << "closest point not internal"; - } - else if (lower2 > facet->getDist2()) - { - std::cout << "facet is closer than orignal facet"; - } - else - { - std::cout << "facet is further than upper bound"; - } - std::cout << std::endl; -#endif - } - - return facet; - } - } - - return 0; -} - -inline bool originInTetrahedron(const MT_Vector3& p1, const MT_Vector3& p2, - const MT_Vector3& p3, const MT_Vector3& p4) -{ - MT_Vector3 normal1 = (p2 - p1).cross(p3 - p1); - MT_Vector3 normal2 = (p3 - p2).cross(p4 - p2); - MT_Vector3 normal3 = (p4 - p3).cross(p1 - p3); - MT_Vector3 normal4 = (p1 - p4).cross(p2 - p4); - - return - normal1.dot(p1) > MT_Scalar(0.0) != normal1.dot(p4) > MT_Scalar(0.0) && - normal2.dot(p2) > MT_Scalar(0.0) != normal2.dot(p1) > MT_Scalar(0.0) && - normal3.dot(p3) > MT_Scalar(0.0) != normal3.dot(p2) > MT_Scalar(0.0) && - normal4.dot(p4) > MT_Scalar(0.0) != normal4.dot(p3) > MT_Scalar(0.0); -} - - -bool penDepth(const DT_GJK& gjk, const DT_Convex& a, const DT_Convex& b, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - - int num_verts = gjk.getSimplex(pBuf, qBuf, yBuf); - - switch (num_verts) - { - case 1: - // Touching contact. Yes, we have a collision, - // but no penetration. - return false; - case 2: - { - // We have a line segment inside the Minkowski sum containing the - // origin. Blow it up by adding three additional support points. - - MT_Vector3 dir = (yBuf[1] - yBuf[0]).normalized(); - int axis = dir.furthestAxis(); - - static MT_Scalar sin_60 = MT_sqrt(MT_Scalar(3.0)) * MT_Scalar(0.5); - - MT_Quaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, MT_Scalar(0.5)); - MT_Matrix3x3 rot_mat(rot); - - MT_Vector3 aux1 = dir.cross(MT_Vector3(axis == 0, axis == 1, axis == 2)); - MT_Vector3 aux2 = rot_mat * aux1; - MT_Vector3 aux3 = rot_mat * aux2; - - pBuf[2] = a.support(aux1); - qBuf[2] = b.support(-aux1); - yBuf[2] = pBuf[2] - qBuf[2]; - - pBuf[3] = a.support(aux2); - qBuf[3] = b.support(-aux2); - yBuf[3] = pBuf[3] - qBuf[3]; - - pBuf[4] = a.support(aux3); - qBuf[4] = b.support(-aux3); - yBuf[4] = pBuf[4] - qBuf[4]; - - if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) - { - pBuf[1] = pBuf[4]; - qBuf[1] = qBuf[4]; - yBuf[1] = yBuf[4]; - } - else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) - { - pBuf[0] = pBuf[4]; - qBuf[0] = qBuf[4]; - yBuf[0] = yBuf[4]; - } - else - { - // Origin not in initial polytope - return false; - } - - num_verts = 4; - - break; - } - case 3: - { - // We have a triangle inside the Minkowski sum containing - // the origin. First blow it up. - - MT_Vector3 v1 = yBuf[1] - yBuf[0]; - MT_Vector3 v2 = yBuf[2] - yBuf[0]; - MT_Vector3 vv = v1.cross(v2); - - pBuf[3] = a.support(vv); - qBuf[3] = b.support(-vv); - yBuf[3] = pBuf[3] - qBuf[3]; - pBuf[4] = a.support(-vv); - qBuf[4] = b.support(vv); - yBuf[4] = pBuf[4] - qBuf[4]; - - - if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) - { - pBuf[3] = pBuf[4]; - qBuf[3] = qBuf[4]; - yBuf[3] = yBuf[4]; - } - else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) - { - // Origin not in initial polytope - return false; - } - - num_verts = 4; - - break; - } - } - - // We have a tetrahedron inside the Minkowski sum containing - // the origin (if GJK did it's job right ;-) - - - if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) - { - // assert(false); - return false; - } - - num_facets = 0; - freeFacet = 0; - - DT_Facet *f0 = addFacet(0, 1, 2, MT_Scalar(0.0), MT_INFINITY); - DT_Facet *f1 = addFacet(0, 3, 1, MT_Scalar(0.0), MT_INFINITY); - DT_Facet *f2 = addFacet(0, 2, 3, MT_Scalar(0.0), MT_INFINITY); - DT_Facet *f3 = addFacet(1, 3, 2, MT_Scalar(0.0), MT_INFINITY); - - if (!f0 || f0->getDist2() == MT_Scalar(0.0) || - !f1 || f1->getDist2() == MT_Scalar(0.0) || - !f2 || f2->getDist2() == MT_Scalar(0.0) || - !f3 || f3->getDist2() == MT_Scalar(0.0)) - { - return false; - } - - f0->link(0, f1, 2); - f0->link(1, f3, 2); - f0->link(2, f2, 0); - f1->link(0, f2, 2); - f1->link(1, f3, 0); - f2->link(1, f3, 1); - - if (num_facets == 0) - { - return false; - } - - // at least one facet on the heap. - - DT_EdgeBuffer edgeBuffer(20); - - DT_Facet *facet = 0; - - MT_Scalar upper_bound2 = MT_INFINITY; - - do { - facet = facetHeap[0]; - std::pop_heap(&facetHeap[0], &facetHeap[num_facets], facetComp); - --num_facets; - - if (!facet->isObsolete()) - { - assert(facet->getDist2() > MT_Scalar(0.0)); - - if (num_verts == MaxSupportPoints) - { -#ifdef DEBUG - std::cout << "Ouch, no convergence!!!" << std::endl; -#endif - assert(false); - break; - } - - pBuf[num_verts] = a.support(facet->getClosest()); - qBuf[num_verts] = b.support(-facet->getClosest()); - yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts]; - - int index = num_verts++; - MT_Scalar far_dist2 = yBuf[index].dot(facet->getClosest()); - - // Make sure the support mapping is OK. - assert(far_dist2 > MT_Scalar(0.0)); - - GEN_set_min(upper_bound2, far_dist2 * far_dist2 / facet->getDist2()); - - if (upper_bound2 <= DT_Accuracy::depth_tolerance * facet->getDist2() -#define CHECK_NEW_SUPPORT -#ifdef CHECK_NEW_SUPPORT - || yBuf[index] == yBuf[(*facet)[0]] - || yBuf[index] == yBuf[(*facet)[1]] - || yBuf[index] == yBuf[(*facet)[2]] -#endif - ) - { - break; - } - - // Compute the silhouette cast by the new vertex - // Note that the new vertex is on the positive side - // of the current facet, so the current facet is will - // not be in the convex hull. Start local search - // from this facet. - - facet->silhouette(yBuf[index], edgeBuffer); - - if (edgeBuffer.empty()) - { - return false; - } - - DT_EdgeBuffer::const_iterator it = edgeBuffer.begin(); - DT_Facet *firstFacet = - addFacet((*it).getTarget(), (*it).getSource(), - index, facet->getDist2(), upper_bound2); - - if (!firstFacet) - { - break; - } - - firstFacet->link(0, (*it).getFacet(), (*it).getIndex()); - DT_Facet *lastFacet = firstFacet; - - ++it; - for (; it != edgeBuffer.end(); ++it) - { - DT_Facet *newFacet = - addFacet((*it).getTarget(), (*it).getSource(), - index, facet->getDist2(), upper_bound2); - - if (!newFacet) - { - break; - } - - if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) - { - break; - } - - if (!newFacet->link(2, lastFacet, 1)) - { - break; - } - - lastFacet = newFacet; - } - if (it != edgeBuffer.end()) - { - break; - } - - firstFacet->link(2, lastFacet, 1); - } - } - while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2); - -#ifdef DEBUG - std::cout << "#facets left = " << num_facets << std::endl; -#endif - - v = facet->getClosest(); - pa = facet->getClosestPoint(pBuf); - pb = facet->getClosestPoint(qBuf); - return true; -} - |