Added the Solid 3.5 sources to the blender source tree.
[blender.git] / extern / solid / src / convex / DT_Convex.cpp
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 #include "DT_Convex.h"
25 #include "GEN_MinMax.h"
26
27 //#define DEBUG
28 #define SAFE_EXIT
29
30 #include "DT_GJK.h"
31 #include "DT_PenDepth.h"
32
33 #include <algorithm>
34 #include <new>
35
36 #include "MT_BBox.h"
37 #include "DT_Sphere.h"
38 #include "DT_Minkowski.h"
39
40 #include "DT_Accuracy.h"
41
42 #ifdef STATISTICS
43 int num_iterations = 0;
44 int num_irregularities = 0;
45 #endif
46
47 MT_BBox DT_Convex::bbox() const 
48 {
49         MT_Point3 min(-supportH(MT_Vector3(-1.0f, 0.0f, 0.0f)),
50                   -supportH(MT_Vector3(0.0f, -1.0f, 0.0f)),
51                                   -supportH(MT_Vector3(0.0f, 0.0f, -1.0f)));
52         MT_Point3 max( supportH(MT_Vector3(1.0f, 0.0f, 0.0f)),
53                    supportH(MT_Vector3(0.0f, 1.0f, 0.0f)),
54                                    supportH(MT_Vector3(0.0f, 0.0f, 1.0f)));
55
56         
57     return MT_BBox(min, max);
58 }
59
60 MT_BBox DT_Convex::bbox(const MT_Matrix3x3& basis) const 
61 {
62     MT_Point3 min(-supportH(-basis[0]),
63                   -supportH(-basis[1]),
64                           -supportH(-basis[2])); 
65     MT_Point3 max( supportH( basis[0]),
66                    supportH( basis[1]),
67                    supportH( basis[2])); 
68     return MT_BBox(min, max);
69 }
70
71 MT_BBox DT_Convex::bbox(const MT_Transform& t, MT_Scalar margin) const 
72 {
73     MT_Point3 min(t.getOrigin()[0] - supportH(-t.getBasis()[0]) - margin,
74                   t.getOrigin()[1] - supportH(-t.getBasis()[1]) - margin,
75                           t.getOrigin()[2] - supportH(-t.getBasis()[2]) - margin); 
76     MT_Point3 max(t.getOrigin()[0] + supportH( t.getBasis()[0]) + margin,
77                   t.getOrigin()[1] + supportH( t.getBasis()[1]) + margin,
78                   t.getOrigin()[2] + supportH( t.getBasis()[2]) + margin); 
79     return MT_BBox(min, max);
80 }
81
82 bool DT_Convex::ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const
83 {
84         // Still working on this one...
85     return false;
86 }
87
88 bool intersect(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v)
89 {
90         DT_GJK gjk;
91
92 #ifdef STATISTICS
93     num_iterations = 0;
94 #endif
95         MT_Scalar dist2 = MT_INFINITY;
96
97         do
98         {
99                 MT_Point3  p = a.support(-v);   
100                 MT_Point3  q = b.support(v);
101                 MT_Vector3 w = p - q; 
102                 
103         if (v.dot(w) > MT_Scalar(0.0)) 
104                 {
105                         return false;
106                 }
107  
108                 gjk.addVertex(w);
109
110 #ifdef STATISTICS
111         ++num_iterations;
112 #endif
113         if (!gjk.closest(v)) 
114                 {
115 #ifdef STATISTICS
116             ++num_irregularities;
117 #endif
118             return false;
119         }
120
121 #ifdef SAFE_EXIT
122                 MT_Scalar prev_dist2 = dist2;
123 #endif
124
125                 dist2 = v.length2();
126
127 #ifdef SAFE_EXIT
128                 if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
129                 {
130                         return false;
131                 }
132 #endif
133     } 
134     while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
135
136     v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
137
138     return true;
139 }
140
141
142
143
144 bool common_point(const DT_Convex& a, const DT_Convex& b,
145                   MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
146 {
147         DT_GJK gjk;
148
149 #ifdef STATISTICS
150     num_iterations = 0;
151 #endif
152
153         MT_Scalar dist2 = MT_INFINITY;
154
155     do
156         {
157                 MT_Point3  p = a.support(-v);   
158                 MT_Point3  q = b.support(v);
159                 MT_Vector3 w = p - q; 
160                 
161         if (v.dot(w) > MT_Scalar(0.0)) 
162                 {
163                         return false;
164                 }
165  
166                 gjk.addVertex(w, p, q);
167
168 #ifdef STATISTICS
169         ++num_iterations;
170 #endif
171         if (!gjk.closest(v)) 
172                 {
173 #ifdef STATISTICS
174             ++num_irregularities;
175 #endif
176             return false;
177         }               
178                 
179 #ifdef SAFE_EXIT
180                 MT_Scalar prev_dist2 = dist2;
181 #endif
182
183                 dist2 = v.length2();
184
185 #ifdef SAFE_EXIT
186                 if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
187                 {
188                         return false;
189                 }
190 #endif
191     }
192     while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
193     
194         gjk.compute_points(pa, pb);
195
196     v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
197
198     return true;
199 }
200
201
202
203
204
205
206         
207 bool penetration_depth(const DT_Convex& a, const DT_Convex& b,
208                        MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
209 {
210         DT_GJK gjk;
211
212 #ifdef STATISTICS
213     num_iterations = 0;
214 #endif
215
216         MT_Scalar dist2 = MT_INFINITY;
217
218     do
219         {
220                 MT_Point3  p = a.support(-v);   
221                 MT_Point3  q = b.support(v);
222                 MT_Vector3 w = p - q; 
223                 
224         if (v.dot(w) > MT_Scalar(0.0)) 
225                 {
226                         return false;
227                 }
228  
229                 gjk.addVertex(w, p, q);
230
231 #ifdef STATISTICS
232         ++num_iterations;
233 #endif
234         if (!gjk.closest(v)) 
235                 {
236 #ifdef STATISTICS
237             ++num_irregularities;
238 #endif
239             return false;
240         }               
241                 
242 #ifdef SAFE_EXIT
243                 MT_Scalar prev_dist2 = dist2;
244 #endif
245
246                 dist2 = v.length2();
247
248 #ifdef SAFE_EXIT
249                 if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
250                 {
251                         return false;
252                 }
253 #endif
254     }
255     while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
256     
257
258         return penDepth(gjk, a, b, v, pa, pb);
259
260 }
261
262 bool hybrid_penetration_depth(const DT_Convex& a, MT_Scalar a_margin, 
263                                                           const DT_Convex& b, MT_Scalar b_margin,
264                               MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
265 {
266         MT_Scalar margin = a_margin + b_margin;
267         if (margin > MT_Scalar(0.0))
268         {
269                 MT_Scalar margin2 = margin * margin;
270
271                 DT_GJK gjk;
272
273 #ifdef STATISTICS
274                 num_iterations = 0;
275 #endif
276                 MT_Scalar dist2 = MT_INFINITY;
277
278                 do
279                 {
280                         MT_Point3  p = a.support(-v);   
281                         MT_Point3  q = b.support(v);
282                         
283                         MT_Vector3 w = p - q; 
284                         
285                         MT_Scalar delta = v.dot(w);
286                         
287                         if (delta > MT_Scalar(0.0) && delta * delta > dist2 * margin2) 
288                         {
289                                 return false;
290                         }
291                         
292                         if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2)
293                         {
294                                 gjk.compute_points(pa, pb);
295                                 MT_Scalar s = MT_sqrt(dist2);
296                                 assert(s > MT_Scalar(0.0));
297                                 pa -= v * (a_margin / s);
298                                 pb += v * (b_margin / s);
299                                 return true;
300                         }
301                         
302                         gjk.addVertex(w, p, q);
303                         
304 #ifdef STATISTICS
305                         ++num_iterations;
306 #endif
307                         if (!gjk.closest(v)) 
308                         {
309 #ifdef STATISTICS
310                                 ++num_irregularities;
311 #endif
312                                 gjk.compute_points(pa, pb);
313                                 MT_Scalar s = MT_sqrt(dist2);
314                                 assert(s > MT_Scalar(0.0));
315                                 pa -= v * (a_margin / s);
316                                 pb += v * (b_margin / s);
317                                 return true;
318                         }
319                         
320 #ifdef SAFE_EXIT
321                         MT_Scalar prev_dist2 = dist2;
322 #endif
323                         
324                         dist2 = v.length2();
325                         
326 #ifdef SAFE_EXIT
327                         if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
328                         { 
329                                 gjk.backup_closest(v);
330                                 dist2 = v.length2();
331                                 gjk.compute_points(pa, pb);
332                                 MT_Scalar s = MT_sqrt(dist2);
333                                 assert(s > MT_Scalar(0.0));
334                                 pa -= v * (a_margin / s);
335                                 pb += v * (b_margin / s);
336                                 return true;
337                         }
338 #endif
339                 }
340                 while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
341                 
342         }
343         // Second GJK phase. compute points on the boundary of the offset object
344         
345         return penetration_depth((a_margin > MT_Scalar(0.0) ? 
346                                                           static_cast<const DT_Convex&>(DT_Minkowski(a, DT_Sphere(a_margin))) : 
347                                                           static_cast<const DT_Convex&>(a)), 
348                                                          (b_margin > MT_Scalar(0.0) ? 
349                                                           static_cast<const DT_Convex&>(DT_Minkowski(b, DT_Sphere(b_margin))) : 
350                                                           static_cast<const DT_Convex&>(b)), v, pa, pb);
351 }
352
353
354 MT_Scalar closest_points(const DT_Convex& a, const DT_Convex& b, MT_Scalar max_dist2,
355                          MT_Point3& pa, MT_Point3& pb) 
356 {
357         MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
358         
359     DT_GJK gjk;
360
361 #ifdef STATISTICS
362     num_iterations = 0;
363 #endif
364
365         MT_Scalar dist2 = MT_INFINITY;
366
367     do
368         {
369                 MT_Point3  p = a.support(-v);   
370                 MT_Point3  q = b.support(v);
371                 MT_Vector3 w = p - q; 
372
373                 MT_Scalar delta = v.dot(w);
374                 if (delta > MT_Scalar(0.0) && delta * delta > dist2 * max_dist2) 
375                 {
376                         return MT_INFINITY;
377                 }
378
379                 if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) 
380                 {
381             break;
382                 }
383
384                 gjk.addVertex(w, p, q);
385
386 #ifdef STATISTICS
387         ++num_iterations;
388         if (num_iterations > 1000) 
389                 {
390                         std::cout << "v: " << v << " w: " << w << std::endl;
391                 }
392 #endif
393         if (!gjk.closest(v)) 
394                 {
395 #ifdef STATISTICS
396             ++num_irregularities;
397 #endif
398             break;
399         }
400
401 #ifdef SAFE_EXIT
402                 MT_Scalar prev_dist2 = dist2;
403 #endif
404
405                 dist2 = v.length2();
406
407 #ifdef SAFE_EXIT
408                 if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
409                 {
410          gjk.backup_closest(v);
411          dist2 = v.length2();
412                         break;
413                 }
414 #endif
415     }
416     while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
417
418         assert(!gjk.emptySimplex());
419         
420         if (dist2 <= max_dist2)
421         {
422                 gjk.compute_points(pa, pb);
423         }
424         
425         return dist2;
426 }