diff options
Diffstat (limited to 'extern/solid/src/convex/DT_GJK.h')
-rw-r--r-- | extern/solid/src/convex/DT_GJK.h | 438 |
1 files changed, 0 insertions, 438 deletions
diff --git a/extern/solid/src/convex/DT_GJK.h b/extern/solid/src/convex/DT_GJK.h deleted file mode 100644 index d8f44acf85e..00000000000 --- a/extern/solid/src/convex/DT_GJK.h +++ /dev/null @@ -1,438 +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. - */ - -#ifndef DT_GJK_H -#define DT_GJK_H - -//#define USE_BACKUP_PROCEDURE -#define JOHNSON_ROBUST -#define FAST_CLOSEST - -#include "MT_Point3.h" -#include "MT_Vector3.h" -#include "GEN_MinMax.h" -#include "DT_Accuracy.h" - - -class DT_GJK { -private: - typedef unsigned int T_Bits; - inline static bool subseteq(T_Bits a, T_Bits b) { return (a & b) == a; } - inline static bool contains(T_Bits a, T_Bits b) { return (a & b) != 0x0; } - -public: - DT_GJK() : - m_bits(0x0), - m_all_bits(0x0) - {} - - bool emptySimplex() const { return m_bits == 0x0; } - bool fullSimplex() const { return m_bits == 0xf; } - - void reset() - { - m_bits = 0x0; - m_all_bits = 0x0; - } - - bool inSimplex(const MT_Vector3& w) - { - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(m_all_bits, bit) && w == m_y[i]) - { - return true; - } - } - return false; - } - - void addVertex(const MT_Vector3& w) - { - assert(!fullSimplex()); - m_last = 0; - m_last_bit = 0x1; - while (contains(m_bits, m_last_bit)) - { - ++m_last; - m_last_bit <<= 1; - } - m_y[m_last] = w; - m_ylen2[m_last] = w.length2(); - m_all_bits = m_bits | m_last_bit; - - update_cache(); - compute_det(); - } - - void addVertex(const MT_Vector3& w, const MT_Point3& p, const MT_Point3& q) - { - addVertex(w); - m_p[m_last] = p; - m_q[m_last] = q; - } - - int getSimplex(MT_Point3 *pBuf, MT_Point3 *qBuf, MT_Vector3 *yBuf) const - { - int num_verts = 0; - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(m_bits, bit)) - { - pBuf[num_verts] = m_p[i]; - qBuf[num_verts] = m_q[i]; - yBuf[num_verts] = m_y[i]; - -#ifdef DEBUG - std::cout << "Point " << i << " = " << m_y[i] << std::endl; -#endif - - ++num_verts; - } - } - return num_verts; - } - - void compute_points(MT_Point3& p1, MT_Point3& p2) - { - MT_Scalar sum = MT_Scalar(0.0); - p1.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - p2.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(m_bits, bit)) - { - sum += m_det[m_bits][i]; - p1 += m_p[i] * m_det[m_bits][i]; - p2 += m_q[i] * m_det[m_bits][i]; - } - } - - assert(sum > MT_Scalar(0.0)); - MT_Scalar s = MT_Scalar(1.0) / sum; - p1 *= s; - p2 *= s; - } - - bool closest(MT_Vector3& v) - { -#ifdef FAST_CLOSEST - T_Bits s; - for (s = m_bits; s != 0x0; --s) - { - if (subseteq(s, m_bits) && valid(s | m_last_bit)) - { - m_bits = s | m_last_bit; - compute_vector(m_bits, v); - return true; - } - } - if (valid(m_last_bit)) - { - m_bits = m_last_bit; - m_maxlen2 = m_ylen2[m_last]; - v = m_y[m_last]; - return true; - } -#else - T_Bits s; - for (s = m_all_bits; s != 0x0; --s) - { - if (subseteq(s, m_all_bits) && valid(s)) - { - m_bits = s; - compute_vector(m_bits, v); - return true; - } - } -#endif - - // Original GJK calls the backup procedure at this point. -#ifdef USE_BACKUP_PROCEDURE - backup_closest(MT_Vector3& v); -#endif - return false; - } - - void backup_closest(MT_Vector3& v) - { - MT_Scalar min_dist2 = MT_INFINITY; - - T_Bits s; - for (s = m_all_bits; s != 0x0; --s) - { - if (subseteq(s, m_all_bits) && proper(s)) - { - MT_Vector3 u; - compute_vector(s, u); - MT_Scalar dist2 = u.length2(); - if (dist2 < min_dist2) - { - min_dist2 = dist2; - m_bits = s; - v = u; - } - } - } - } - - MT_Scalar maxVertex() { return m_maxlen2; } - - -private: - void update_cache(); - void compute_det(); - - bool valid(T_Bits s) - { - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(m_all_bits, bit)) - { - if (contains(s, bit)) - { - if (m_det[s][i] <= MT_Scalar(0.0)) - { - return false; - } - } - else if (m_det[s | bit][i] > MT_Scalar(0.0)) - { - return false; - } - } - } - return true; - } - - bool proper(T_Bits s) - { - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(s, bit) && m_det[s][i] <= MT_Scalar(0.0)) - { - return false; - } - } - return true; - } - - void compute_vector(T_Bits s, MT_Vector3& v) - { - m_maxlen2 = MT_Scalar(0.0); - MT_Scalar sum = MT_Scalar(0.0); - v .setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); - - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(s, bit)) - { - sum += m_det[s][i]; - GEN_set_max(m_maxlen2, m_ylen2[i]); - v += m_y[i] * m_det[s][i]; - } - } - - assert(sum > MT_Scalar(0.0)); - - v /= sum; - } - -private: - MT_Scalar m_det[16][4]; // cached sub-determinants - MT_Vector3 m_edge[4][4]; - -#ifdef JOHNSON_ROBUST - MT_Scalar m_norm[4][4]; -#endif - - MT_Point3 m_p[4]; // support points of object A in local coordinates - MT_Point3 m_q[4]; // support points of object B in local coordinates - MT_Vector3 m_y[4]; // support points of A - B in world coordinates - MT_Scalar m_ylen2[4]; // Squared lengths support points y - - MT_Scalar m_maxlen2; // Maximum squared length to a vertex of the current - // simplex - T_Bits m_bits; // identifies current simplex - T_Bits m_last; // identifies last found support point - T_Bits m_last_bit; // m_last_bit == 0x1 << last - T_Bits m_all_bits; // m_all_bits == m_bits | m_last_bit -}; - - - - -inline void DT_GJK::update_cache() -{ - int i; - T_Bits bit; - for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) - { - if (contains(m_bits, bit)) - { - m_edge[i][m_last] = m_y[i] - m_y[m_last]; - m_edge[m_last][i] = -m_edge[i][m_last]; - -#ifdef JOHNSON_ROBUST - m_norm[i][m_last] = m_norm[m_last][i] = m_edge[i][m_last].length2(); -#endif - - } - } -} - -#ifdef JOHNSON_ROBUST - -inline void DT_GJK::compute_det() -{ - m_det[m_last_bit][m_last] = 1; - - int i; - T_Bits si; - for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) - { - if (contains(m_bits, si)) - { - T_Bits s2 = si | m_last_bit; - m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); - m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]); - - int j; - T_Bits sj; - for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1) - { - if (contains(m_bits, sj)) - { - int k; - T_Bits s3 = sj | s2; - - k = m_norm[i][j] < m_norm[m_last][j] ? i : m_last; - m_det[s3][j] = m_det[s2][i] * m_edge[k][j].dot(m_y[i]) + - m_det[s2][m_last] * m_edge[k][j].dot(m_y[m_last]); - k = m_norm[j][i] < m_norm[m_last][i] ? j : m_last; - m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[k][i].dot(m_y[j]) + - m_det[sj|m_last_bit][m_last] * m_edge[k][i].dot(m_y[m_last]); - k = m_norm[i][m_last] < m_norm[j][m_last] ? i : j; - m_det[s3][m_last] = m_det[sj|si][j] * m_edge[k][m_last].dot(m_y[j]) + - m_det[sj|si][i] * m_edge[k][m_last].dot(m_y[i]); - } - } - } - } - - if (m_all_bits == 0xf) - { - int k; - - k = m_norm[1][0] < m_norm[2][0] ? (m_norm[1][0] < m_norm[3][0] ? 1 : 3) : (m_norm[2][0] < m_norm[3][0] ? 2 : 3); - - m_det[0xf][0] = m_det[0xe][1] * m_edge[k][0].dot(m_y[1]) + - m_det[0xe][2] * m_edge[k][0].dot(m_y[2]) + - m_det[0xe][3] * m_edge[k][0].dot(m_y[3]); - - k = m_norm[0][1] < m_norm[2][1] ? (m_norm[0][1] < m_norm[3][1] ? 0 : 3) : (m_norm[2][1] < m_norm[3][1] ? 2 : 3); - - m_det[0xf][1] = m_det[0xd][0] * m_edge[k][1].dot(m_y[0]) + - m_det[0xd][2] * m_edge[k][1].dot(m_y[2]) + - m_det[0xd][3] * m_edge[k][1].dot(m_y[3]); - - k = m_norm[0][2] < m_norm[1][2] ? (m_norm[0][2] < m_norm[3][2] ? 0 : 3) : (m_norm[1][2] < m_norm[3][2] ? 1 : 3); - - m_det[0xf][2] = m_det[0xb][0] * m_edge[k][2].dot(m_y[0]) + - m_det[0xb][1] * m_edge[k][2].dot(m_y[1]) + - m_det[0xb][3] * m_edge[k][2].dot(m_y[3]); - - k = m_norm[0][3] < m_norm[1][3] ? (m_norm[0][3] < m_norm[2][3] ? 0 : 2) : (m_norm[1][3] < m_norm[2][3] ? 1 : 2); - - m_det[0xf][3] = m_det[0x7][0] * m_edge[k][3].dot(m_y[0]) + - m_det[0x7][1] * m_edge[k][3].dot(m_y[1]) + - m_det[0x7][2] * m_edge[k][3].dot(m_y[2]); - } -} - -#else - -inline void DT_GJK::compute_det() -{ - m_det[m_last_bit][m_last] = 1; - - int i; - T_Bits si; - for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) - { - if (contains(m_bits, si)) - { - T_Bits s2 = si | m_last_bit; - m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); - m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]); - - int j; - T_Bits sj; - for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1) - { - if (contains(m_bits, sj)) - { - T_Bits s3 = sj | s2; - m_det[s3][j] = m_det[s2][i] * m_edge[i][j].dot(m_y[i]) + - m_det[s2][m_last] * m_edge[i][j].dot(m_y[m_last]); - m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[j][i].dot(m_y[j]) + - m_det[sj|m_last_bit][m_last] * m_edge[j][i].dot(m_y[m_last]); - m_det[s3][m_last] = m_det[sj|si][j] * m_edge[j][m_last].dot(m_y[j]) + - m_det[sj|si][i] * m_edge[j][m_last].dot(m_y[i]); - } - } - } - } - - if (m_all_bits == 0xf) - { - m_det[0xf][0] = m_det[0xe][1] * m_edge[1][0].dot(m_y[1]) + - m_det[0xe][2] * m_edge[1][0].dot(m_y[2]) + - m_det[0xe][3] * m_edge[1][0].dot(m_y[3]); - m_det[0xf][1] = m_det[0xd][0] * m_edge[0][1].dot(m_y[0]) + - m_det[0xd][2] * m_edge[0][1].dot(m_y[2]) + - m_det[0xd][3] * m_edge[0][1].dot(m_y[3]); - m_det[0xf][2] = m_det[0xb][0] * m_edge[0][2].dot(m_y[0]) + - m_det[0xb][1] * m_edge[0][2].dot(m_y[1]) + - m_det[0xb][3] * m_edge[0][2].dot(m_y[3]); - m_det[0xf][3] = m_det[0x7][0] * m_edge[0][3].dot(m_y[0]) + - m_det[0x7][1] * m_edge[0][3].dot(m_y[1]) + - m_det[0x7][2] * m_edge[0][3].dot(m_y[2]); - } -} - -#endif - -#endif |