Reverted incorrect merge (missing files)
[blender.git] / extern / solid / src / convex / DT_GJK.h
1 /*
2  * SOLID - Software Library for Interference Detection
3  * 
4  * Copyright (C) 2001-2003  Dtecta.  All rights reserved.
5  *
6  * This library may be distributed under the terms of the Q Public License
7  * (QPL) as defined by Trolltech AS of Norway and appearing in the file
8  * LICENSE.QPL included in the packaging of this file.
9  *
10  * This library may be distributed and/or modified under the terms of the
11  * GNU General Public License (GPL) version 2 as published by the Free Software
12  * Foundation and appearing in the file LICENSE.GPL included in the
13  * packaging of this file.
14  *
15  * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
16  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
17  *
18  * Commercial use or any other use of this library not covered by either 
19  * the QPL or the GPL requires an additional license from Dtecta. 
20  * Please contact info@dtecta.com for enquiries about the terms of commercial
21  * use of this library.
22  */
23
24 #ifndef DT_GJK_H
25 #define DT_GJK_H
26
27 //#define USE_BACKUP_PROCEDURE
28 #define JOHNSON_ROBUST
29 #define FAST_CLOSEST
30
31 #include "MT_Point3.h"
32 #include "MT_Vector3.h"
33 #include "GEN_MinMax.h"
34 #include "DT_Accuracy.h"
35
36
37 class DT_GJK {
38 private:
39         typedef unsigned int T_Bits;
40         inline static bool subseteq(T_Bits a, T_Bits b) { return (a & b) == a; }
41         inline static bool contains(T_Bits a, T_Bits b) { return (a & b) != 0x0; }
42
43 public:
44         DT_GJK() :
45                 m_bits(0x0),
46                 m_all_bits(0x0)
47         {}
48
49         bool emptySimplex() const { return m_bits == 0x0; }
50         bool fullSimplex() const { return m_bits == 0xf; }
51
52         void reset() 
53         {
54                 m_bits = 0x0;
55                 m_all_bits = 0x0;       
56         }
57
58         bool inSimplex(const MT_Vector3& w) 
59         {
60                 int i;
61                 T_Bits bit;
62                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
63                 {
64                         if (contains(m_all_bits, bit) && w == m_y[i])
65                         {
66                                 return true;
67                         }
68                 }
69                 return false;
70         }
71
72         void addVertex(const MT_Vector3& w) 
73         {
74                 assert(!fullSimplex());
75                 m_last = 0;
76         m_last_bit = 0x1;
77         while (contains(m_bits, m_last_bit)) 
78                 { 
79                         ++m_last; 
80                         m_last_bit <<= 1; 
81                 }
82                 m_y[m_last] = w;
83                 m_ylen2[m_last] = w.length2();
84         m_all_bits = m_bits | m_last_bit;
85
86                 update_cache();
87                 compute_det();
88         }
89
90         void addVertex(const MT_Vector3& w, const MT_Point3& p, const MT_Point3& q)
91         {
92                 addVertex(w);
93                 m_p[m_last] = p;
94                 m_q[m_last] = q;
95         }
96
97         int getSimplex(MT_Point3 *pBuf, MT_Point3 *qBuf, MT_Vector3 *yBuf) const 
98         {
99                 int num_verts = 0;
100                 int i;
101                 T_Bits bit;
102                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) 
103                 {
104                         if (contains(m_bits, bit)) 
105                         {
106                                 pBuf[num_verts] = m_p[i];
107                                 qBuf[num_verts] = m_q[i];
108                                 yBuf[num_verts] = m_y[i];
109                                 
110 #ifdef DEBUG
111                                 std::cout << "Point " << i << " = " << m_y[i] << std::endl;
112 #endif
113                                 
114                                 ++num_verts;
115                         }
116                 }
117                 return num_verts;
118     }
119
120         void compute_points(MT_Point3& p1, MT_Point3& p2) 
121         {
122                 MT_Scalar sum = MT_Scalar(0.0);
123                 p1.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
124                 p2.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
125                 int i;
126                 T_Bits bit;
127                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) 
128                 {
129                         if (contains(m_bits, bit))
130                         {
131                                 sum += m_det[m_bits][i];
132                                 p1 += m_p[i] * m_det[m_bits][i];
133                                 p2 += m_q[i] * m_det[m_bits][i];
134                         }
135                 }
136
137                 assert(sum > MT_Scalar(0.0));
138                 MT_Scalar s = MT_Scalar(1.0) / sum;
139                 p1 *= s;
140                 p2 *= s;
141         }
142
143         bool closest(MT_Vector3& v) 
144         {
145 #ifdef FAST_CLOSEST
146                 T_Bits s;
147                 for (s = m_bits; s != 0x0; --s)
148                 {
149                         if (subseteq(s, m_bits) && valid(s | m_last_bit)) 
150                         {
151                                 m_bits = s | m_last_bit;
152                                 compute_vector(m_bits, v);
153                                 return true;
154                         }
155                 }
156                 if (valid(m_last_bit)) 
157                 {
158                         m_bits = m_last_bit;
159                         m_maxlen2 = m_ylen2[m_last];
160                         v = m_y[m_last];
161                         return true;
162                 }
163 #else
164                 T_Bits s;
165                 for (s = m_all_bits; s != 0x0; --s)
166                 {
167                         if (subseteq(s, m_all_bits) && valid(s)) 
168                         {
169                                 m_bits = s;
170                                 compute_vector(m_bits, v);
171                                 return true;
172                         }
173                 }
174 #endif
175                 
176                 // Original GJK calls the backup procedure at this point.
177 #ifdef USE_BACKUP_PROCEDURE
178                 backup_closest(MT_Vector3& v); 
179 #endif
180                 return false;  
181         }
182
183         void backup_closest(MT_Vector3& v)
184         {               
185                 MT_Scalar min_dist2 = MT_INFINITY;
186                 
187       T_Bits s;
188                 for (s = m_all_bits; s != 0x0; --s) 
189                 {
190                         if (subseteq(s, m_all_bits) && proper(s))
191                         {       
192                                 MT_Vector3 u;
193                                 compute_vector(s, u);
194                                 MT_Scalar dist2 = u.length2();
195                                 if (dist2 < min_dist2)
196                                 {
197                                         min_dist2 = dist2;
198                                         m_bits = s;
199                                         v = u;
200                                 }
201                         }
202                 }
203         }
204         
205         MT_Scalar maxVertex() { return m_maxlen2; }
206
207
208 private:
209         void update_cache();
210         void compute_det();
211
212         bool valid(T_Bits s) 
213         {
214                 int i;
215                 T_Bits bit;
216                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
217                 {
218                         if (contains(m_all_bits, bit)) 
219                         {
220                                 if (contains(s, bit)) 
221                                 {
222                                         if (m_det[s][i] <= MT_Scalar(0.0)) 
223                                         {
224                                                 return false; 
225                                         }
226                                 }
227                                 else if (m_det[s | bit][i] > MT_Scalar(0.0))
228                                 { 
229                                         return false;
230                                 }
231                         }
232                 }
233                 return true;
234         }
235
236         bool proper(T_Bits s)
237         { 
238                 int i;
239                 T_Bits bit;
240                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
241                 {
242                         if (contains(s, bit) && m_det[s][i] <= MT_Scalar(0.0))
243                         {
244                                 return false; 
245                         }
246                 }
247                 return true;
248         }
249
250         void compute_vector(T_Bits s, MT_Vector3& v) 
251         {
252                 m_maxlen2 = MT_Scalar(0.0);
253                 MT_Scalar sum = MT_Scalar(0.0);
254                 v .setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
255
256                 int i;
257                 T_Bits bit;
258                 for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) 
259                 {
260                         if (contains(s, bit))
261                         {
262                                 sum += m_det[s][i];
263                                 GEN_set_max(m_maxlen2, m_ylen2[i]);
264                                 v += m_y[i] * m_det[s][i];
265                         }
266                 }
267                 
268                 assert(sum > MT_Scalar(0.0));
269
270                 v /= sum;
271         }
272  
273 private:
274         MT_Scalar       m_det[16][4]; // cached sub-determinants
275     MT_Vector3  m_edge[4][4];
276
277 #ifdef JOHNSON_ROBUST
278     MT_Scalar   m_norm[4][4];
279 #endif
280
281         MT_Point3       m_p[4];    // support points of object A in local coordinates 
282         MT_Point3       m_q[4];    // support points of object B in local coordinates 
283         MT_Vector3      m_y[4];   // support points of A - B in world coordinates
284         MT_Scalar       m_ylen2[4];   // Squared lengths support points y
285
286         MT_Scalar       m_maxlen2; // Maximum squared length to a vertex of the current 
287                               // simplex
288         T_Bits          m_bits;      // identifies current simplex
289         T_Bits          m_last;      // identifies last found support point
290         T_Bits          m_last_bit;  // m_last_bit == 0x1 << last
291         T_Bits          m_all_bits;  // m_all_bits == m_bits  | m_last_bit 
292 };
293
294
295
296
297 inline void DT_GJK::update_cache() 
298 {
299         int i;
300         T_Bits bit;
301     for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
302         {
303         if (contains(m_bits, bit)) 
304                 {
305                         m_edge[i][m_last] = m_y[i] - m_y[m_last];
306                         m_edge[m_last][i] = -m_edge[i][m_last];
307
308 #ifdef JOHNSON_ROBUST
309                         m_norm[i][m_last] = m_norm[m_last][i] = m_edge[i][m_last].length2();
310 #endif
311
312                 }
313         }
314 }
315
316 #ifdef JOHNSON_ROBUST
317
318 inline void DT_GJK::compute_det() 
319 {
320     m_det[m_last_bit][m_last] = 1;
321
322         int i;
323         T_Bits si;
324     for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) 
325         {
326         if (contains(m_bits, si)) 
327                 {
328             T_Bits s2 = si | m_last_bit;
329             m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); 
330             m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]);
331
332                         int j;
333                         T_Bits sj;
334             for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1) 
335                         {
336                 if (contains(m_bits, sj)) 
337                                 {
338                                         int k;
339                     T_Bits s3 = sj | s2;                        
340                                         
341                                         k = m_norm[i][j] < m_norm[m_last][j] ? i : m_last;
342                     m_det[s3][j] = m_det[s2][i] * m_edge[k][j].dot(m_y[i]) + 
343                                    m_det[s2][m_last] * m_edge[k][j].dot(m_y[m_last]);
344                                         k = m_norm[j][i] < m_norm[m_last][i] ? j : m_last;
345                     m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[k][i].dot(m_y[j]) +  
346                                    m_det[sj|m_last_bit][m_last] * m_edge[k][i].dot(m_y[m_last]);
347                                         k = m_norm[i][m_last] < m_norm[j][m_last] ? i : j;
348                     m_det[s3][m_last] = m_det[sj|si][j] * m_edge[k][m_last].dot(m_y[j]) + 
349                                         m_det[sj|si][i] * m_edge[k][m_last].dot(m_y[i]);
350                 }
351             }
352         }
353     }
354
355     if (m_all_bits == 0xf) 
356         {
357                 int k;
358
359                 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);
360                 
361         m_det[0xf][0] = m_det[0xe][1] * m_edge[k][0].dot(m_y[1]) + 
362                         m_det[0xe][2] * m_edge[k][0].dot(m_y[2]) + 
363                         m_det[0xe][3] * m_edge[k][0].dot(m_y[3]);
364
365                 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);
366                 
367         m_det[0xf][1] = m_det[0xd][0] * m_edge[k][1].dot(m_y[0]) + 
368                         m_det[0xd][2] * m_edge[k][1].dot(m_y[2]) +
369                         m_det[0xd][3] * m_edge[k][1].dot(m_y[3]);
370
371                 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);
372                 
373         m_det[0xf][2] = m_det[0xb][0] * m_edge[k][2].dot(m_y[0]) + 
374                         m_det[0xb][1] * m_edge[k][2].dot(m_y[1]) +  
375                         m_det[0xb][3] * m_edge[k][2].dot(m_y[3]);
376
377                 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);
378                 
379         m_det[0xf][3] = m_det[0x7][0] * m_edge[k][3].dot(m_y[0]) + 
380                         m_det[0x7][1] * m_edge[k][3].dot(m_y[1]) + 
381                         m_det[0x7][2] * m_edge[k][3].dot(m_y[2]);
382     }
383 }
384
385 #else
386
387 inline void DT_GJK::compute_det() 
388 {
389     m_det[m_last_bit][m_last] = 1;
390
391         int i;
392         T_Bits si;
393     for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) 
394         {
395         if (contains(m_bits, si)) 
396                 {
397             T_Bits s2 = si | m_last_bit;
398             m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); 
399             m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]);
400
401                         int j;
402                         T_Bits sj;
403             for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1)
404                         {
405                 if (contains(m_bits, sj)) 
406                                 {
407                     T_Bits s3 = sj | s2;
408                     m_det[s3][j] = m_det[s2][i] * m_edge[i][j].dot(m_y[i]) + 
409                                    m_det[s2][m_last] * m_edge[i][j].dot(m_y[m_last]);
410                     m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[j][i].dot(m_y[j]) +  
411                                    m_det[sj|m_last_bit][m_last] * m_edge[j][i].dot(m_y[m_last]);
412                     m_det[s3][m_last] = m_det[sj|si][j] * m_edge[j][m_last].dot(m_y[j]) + 
413                                         m_det[sj|si][i] * m_edge[j][m_last].dot(m_y[i]);
414                 }
415             }
416         }
417     }
418
419     if (m_all_bits == 0xf) 
420         {
421         m_det[0xf][0] = m_det[0xe][1] * m_edge[1][0].dot(m_y[1]) + 
422                         m_det[0xe][2] * m_edge[1][0].dot(m_y[2]) + 
423                         m_det[0xe][3] * m_edge[1][0].dot(m_y[3]);
424         m_det[0xf][1] = m_det[0xd][0] * m_edge[0][1].dot(m_y[0]) + 
425                         m_det[0xd][2] * m_edge[0][1].dot(m_y[2]) +
426                         m_det[0xd][3] * m_edge[0][1].dot(m_y[3]);
427         m_det[0xf][2] = m_det[0xb][0] * m_edge[0][2].dot(m_y[0]) + 
428                         m_det[0xb][1] * m_edge[0][2].dot(m_y[1]) +  
429                         m_det[0xb][3] * m_edge[0][2].dot(m_y[3]);
430         m_det[0xf][3] = m_det[0x7][0] * m_edge[0][3].dot(m_y[0]) + 
431                         m_det[0x7][1] * m_edge[0][3].dot(m_y[1]) + 
432                         m_det[0x7][2] * m_edge[0][3].dot(m_y[2]);
433     }
434 }
435
436 #endif
437
438 #endif