diff options
Diffstat (limited to 'extern/solid/src/convex/DT_Convex.cpp')
-rw-r--r-- | extern/solid/src/convex/DT_Convex.cpp | 426 |
1 files changed, 0 insertions, 426 deletions
diff --git a/extern/solid/src/convex/DT_Convex.cpp b/extern/solid/src/convex/DT_Convex.cpp deleted file mode 100644 index 3be47f6ed02..00000000000 --- a/extern/solid/src/convex/DT_Convex.cpp +++ /dev/null @@ -1,426 +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_Convex.h" -#include "GEN_MinMax.h" - -//#define DEBUG -#define SAFE_EXIT - -#include "DT_GJK.h" -#include "DT_PenDepth.h" - -#include <algorithm> -#include <new> - -#include "MT_BBox.h" -#include "DT_Sphere.h" -#include "DT_Minkowski.h" - -#include "DT_Accuracy.h" - -#ifdef STATISTICS -int num_iterations = 0; -int num_irregularities = 0; -#endif - -MT_BBox DT_Convex::bbox() const -{ - MT_Point3 min(-supportH(MT_Vector3(-1.0f, 0.0f, 0.0f)), - -supportH(MT_Vector3(0.0f, -1.0f, 0.0f)), - -supportH(MT_Vector3(0.0f, 0.0f, -1.0f))); - MT_Point3 max( supportH(MT_Vector3(1.0f, 0.0f, 0.0f)), - supportH(MT_Vector3(0.0f, 1.0f, 0.0f)), - supportH(MT_Vector3(0.0f, 0.0f, 1.0f))); - - - return MT_BBox(min, max); -} - -MT_BBox DT_Convex::bbox(const MT_Matrix3x3& basis) const -{ - MT_Point3 min(-supportH(-basis[0]), - -supportH(-basis[1]), - -supportH(-basis[2])); - MT_Point3 max( supportH( basis[0]), - supportH( basis[1]), - supportH( basis[2])); - return MT_BBox(min, max); -} - -MT_BBox DT_Convex::bbox(const MT_Transform& t, MT_Scalar margin) const -{ - MT_Point3 min(t.getOrigin()[0] - supportH(-t.getBasis()[0]) - margin, - t.getOrigin()[1] - supportH(-t.getBasis()[1]) - margin, - t.getOrigin()[2] - supportH(-t.getBasis()[2]) - margin); - MT_Point3 max(t.getOrigin()[0] + supportH( t.getBasis()[0]) + margin, - t.getOrigin()[1] + supportH( t.getBasis()[1]) + margin, - t.getOrigin()[2] + supportH( t.getBasis()[2]) + margin); - return MT_BBox(min, max); -} - -bool DT_Convex::ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const -{ - // Still working on this one... - return false; -} - -bool intersect(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v) -{ - DT_GJK gjk; - -#ifdef STATISTICS - num_iterations = 0; -#endif - MT_Scalar dist2 = MT_INFINITY; - - do - { - MT_Point3 p = a.support(-v); - MT_Point3 q = b.support(v); - MT_Vector3 w = p - q; - - if (v.dot(w) > MT_Scalar(0.0)) - { - return false; - } - - gjk.addVertex(w); - -#ifdef STATISTICS - ++num_iterations; -#endif - if (!gjk.closest(v)) - { -#ifdef STATISTICS - ++num_irregularities; -#endif - return false; - } - -#ifdef SAFE_EXIT - MT_Scalar prev_dist2 = dist2; -#endif - - dist2 = v.length2(); - -#ifdef SAFE_EXIT - if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) - { - return false; - } -#endif - } - while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); - - v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - - return true; -} - - - - -bool common_point(const DT_Convex& a, const DT_Convex& b, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - DT_GJK gjk; - -#ifdef STATISTICS - num_iterations = 0; -#endif - - MT_Scalar dist2 = MT_INFINITY; - - do - { - MT_Point3 p = a.support(-v); - MT_Point3 q = b.support(v); - MT_Vector3 w = p - q; - - if (v.dot(w) > MT_Scalar(0.0)) - { - return false; - } - - gjk.addVertex(w, p, q); - -#ifdef STATISTICS - ++num_iterations; -#endif - if (!gjk.closest(v)) - { -#ifdef STATISTICS - ++num_irregularities; -#endif - return false; - } - -#ifdef SAFE_EXIT - MT_Scalar prev_dist2 = dist2; -#endif - - dist2 = v.length2(); - -#ifdef SAFE_EXIT - if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) - { - return false; - } -#endif - } - while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); - - gjk.compute_points(pa, pb); - - v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - - return true; -} - - - - - - - -bool penetration_depth(const DT_Convex& a, const DT_Convex& b, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - DT_GJK gjk; - -#ifdef STATISTICS - num_iterations = 0; -#endif - - MT_Scalar dist2 = MT_INFINITY; - - do - { - MT_Point3 p = a.support(-v); - MT_Point3 q = b.support(v); - MT_Vector3 w = p - q; - - if (v.dot(w) > MT_Scalar(0.0)) - { - return false; - } - - gjk.addVertex(w, p, q); - -#ifdef STATISTICS - ++num_iterations; -#endif - if (!gjk.closest(v)) - { -#ifdef STATISTICS - ++num_irregularities; -#endif - return false; - } - -#ifdef SAFE_EXIT - MT_Scalar prev_dist2 = dist2; -#endif - - dist2 = v.length2(); - -#ifdef SAFE_EXIT - if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) - { - return false; - } -#endif - } - while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); - - - return penDepth(gjk, a, b, v, pa, pb); - -} - -bool hybrid_penetration_depth(const DT_Convex& a, MT_Scalar a_margin, - const DT_Convex& b, MT_Scalar b_margin, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - MT_Scalar margin = a_margin + b_margin; - if (margin > MT_Scalar(0.0)) - { - MT_Scalar margin2 = margin * margin; - - DT_GJK gjk; - -#ifdef STATISTICS - num_iterations = 0; -#endif - MT_Scalar dist2 = MT_INFINITY; - - do - { - MT_Point3 p = a.support(-v); - MT_Point3 q = b.support(v); - - MT_Vector3 w = p - q; - - MT_Scalar delta = v.dot(w); - - if (delta > MT_Scalar(0.0) && delta * delta > dist2 * margin2) - { - return false; - } - - if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) - { - gjk.compute_points(pa, pb); - MT_Scalar s = MT_sqrt(dist2); - assert(s > MT_Scalar(0.0)); - pa -= v * (a_margin / s); - pb += v * (b_margin / s); - return true; - } - - gjk.addVertex(w, p, q); - -#ifdef STATISTICS - ++num_iterations; -#endif - if (!gjk.closest(v)) - { -#ifdef STATISTICS - ++num_irregularities; -#endif - gjk.compute_points(pa, pb); - MT_Scalar s = MT_sqrt(dist2); - assert(s > MT_Scalar(0.0)); - pa -= v * (a_margin / s); - pb += v * (b_margin / s); - return true; - } - -#ifdef SAFE_EXIT - MT_Scalar prev_dist2 = dist2; -#endif - - dist2 = v.length2(); - -#ifdef SAFE_EXIT - if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) - { - gjk.backup_closest(v); - dist2 = v.length2(); - gjk.compute_points(pa, pb); - MT_Scalar s = MT_sqrt(dist2); - assert(s > MT_Scalar(0.0)); - pa -= v * (a_margin / s); - pb += v * (b_margin / s); - return true; - } -#endif - } - while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); - - } - // Second GJK phase. compute points on the boundary of the offset object - - return penetration_depth((a_margin > MT_Scalar(0.0) ? - static_cast<const DT_Convex&>(DT_Minkowski(a, DT_Sphere(a_margin))) : - static_cast<const DT_Convex&>(a)), - (b_margin > MT_Scalar(0.0) ? - static_cast<const DT_Convex&>(DT_Minkowski(b, DT_Sphere(b_margin))) : - static_cast<const DT_Convex&>(b)), v, pa, pb); -} - - -MT_Scalar closest_points(const DT_Convex& a, const DT_Convex& b, MT_Scalar max_dist2, - MT_Point3& pa, MT_Point3& pb) -{ - MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - - DT_GJK gjk; - -#ifdef STATISTICS - num_iterations = 0; -#endif - - MT_Scalar dist2 = MT_INFINITY; - - do - { - MT_Point3 p = a.support(-v); - MT_Point3 q = b.support(v); - MT_Vector3 w = p - q; - - MT_Scalar delta = v.dot(w); - if (delta > MT_Scalar(0.0) && delta * delta > dist2 * max_dist2) - { - return MT_INFINITY; - } - - if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) - { - break; - } - - gjk.addVertex(w, p, q); - -#ifdef STATISTICS - ++num_iterations; - if (num_iterations > 1000) - { - std::cout << "v: " << v << " w: " << w << std::endl; - } -#endif - if (!gjk.closest(v)) - { -#ifdef STATISTICS - ++num_irregularities; -#endif - break; - } - -#ifdef SAFE_EXIT - MT_Scalar prev_dist2 = dist2; -#endif - - dist2 = v.length2(); - -#ifdef SAFE_EXIT - if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) - { - gjk.backup_closest(v); - dist2 = v.length2(); - break; - } -#endif - } - while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); - - assert(!gjk.emptySimplex()); - - if (dist2 <= max_dist2) - { - gjk.compute_points(pa, pb); - } - - return dist2; -} |