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')
-rwxr-xr-xextern/solid/src/convex/DT_GJK.h438
1 files changed, 438 insertions, 0 deletions
diff --git a/extern/solid/src/convex/DT_GJK.h b/extern/solid/src/convex/DT_GJK.h
new file mode 100755
index 00000000000..d8f44acf85e
--- /dev/null
+++ b/extern/solid/src/convex/DT_GJK.h
@@ -0,0 +1,438 @@
+/*
+ * 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-08-29 03:06:31 +0300