Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/solid/src/convex/DT_PenDepth.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/solid/src/convex/DT_PenDepth.cpp')
-rw-r--r--extern/solid/src/convex/DT_PenDepth.cpp376
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;
-}
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-19 09:23:24 +0300