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_GJK.h
diff options
context:
space:
mode:
Diffstat (limited to 'extern/solid/src/convex/DT_GJK.h')
-rw-r--r--extern/solid/src/convex/DT_GJK.h438
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
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-19 10:02:13 +0300