Added the Solid 3.5 sources to the blender source tree.
[blender.git] / extern / solid / src / convex / DT_Polyhedron.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_Polyhedron.h"
25
26 #ifdef QHULL
27
28 extern "C" {
29 #include <qhull/qhull_a.h>
30 }
31
32 #include <vector>
33 #include <new>  
34
35 typedef std::vector<MT_Point3> T_VertexBuf;
36 typedef std::vector<DT_Index> T_IndexBuf;
37 typedef std::vector<T_IndexBuf> T_MultiIndexBuf;
38
39 static char options[] = "qhull Qts i Tv";
40
41 #define DK_HIERARCHY
42
43 T_IndexBuf *adjacency_graph(DT_Count count, const MT_Point3 *verts, const char *flags)
44 {
45         int curlong, totlong, exitcode;
46         
47     facetT *facet;
48     vertexT *vertex;
49     vertexT **vertexp;
50     
51     std::vector<MT::Tuple3<coordT> > array;
52         T_IndexBuf index;
53     DT_Index i;
54     for (i = 0; i != count; ++i) 
55         {
56                 if (flags == 0 || flags[i])
57                 {
58             array.push_back(MT::Tuple3<coordT>(verts[i]));
59                         index.push_back(i);
60                 }
61     }
62
63     qh_init_A(stdin, stdout, stderr, 0, NULL);
64     if ((exitcode = setjmp(qh errexit))) 
65         {
66                 exit(exitcode);
67         }
68     qh_initflags(options);
69     qh_init_B(array[0], array.size(), 3, False);
70     qh_qhull();
71     qh_check_output();
72     
73     T_IndexBuf *indexBuf = new T_IndexBuf[count];
74     FORALLfacets 
75         {
76                 setT *vertices = qh_facet3vertex(facet);
77                 
78                 T_IndexBuf  facetIndices;
79
80                 FOREACHvertex_(vertices) 
81                 {
82                         facetIndices.push_back(index[qh_pointid(vertex->point)]);
83                 }
84                 int i, j;
85                 for (i = 0, j = facetIndices.size()-1; i < (int)facetIndices.size(); j = i++)
86                 {
87                         indexBuf[facetIndices[j]].push_back(facetIndices[i]);
88                 }
89     }
90
91     
92     qh NOerrexit = True;
93     qh_freeqhull(!qh_ALL);
94     qh_memfreeshort(&curlong, &totlong);
95
96         return indexBuf;
97 }
98
99 T_IndexBuf *simplex_adjacency_graph(DT_Count count, const char *flags)
100 {
101         T_IndexBuf *indexBuf = new T_IndexBuf[count];
102
103         DT_Index index[4];
104         
105         DT_Index k = 0;
106         DT_Index i;
107         for (i = 0; i != count; ++i) 
108         {
109                 if (flags == 0 || flags[i])
110                 {
111                         index[k++] = i;
112                 }
113         }
114
115         assert(k <= 4);
116
117         for (i = 0; i != k; ++i)
118         {
119                 DT_Index j;
120                 for (j = 0; j != k; ++j)
121                 {
122                         if (i != j)
123                         {
124                                 indexBuf[index[i]].push_back(index[j]);
125                         }
126                 }
127         }
128
129         return indexBuf;
130 }
131
132 #ifdef DK_HIERARCHY
133
134 void prune(DT_Count count, T_MultiIndexBuf *cobound)
135 {
136         DT_Index i;
137         for (i = 0; i != count; ++i)
138         {
139                 assert(cobound[i].size());
140
141                 DT_Index j;
142                 for (j = 0; j != cobound[i].size() - 1; ++j)
143                 {
144                         T_IndexBuf::iterator it = cobound[i][j].begin();
145                         while (it != cobound[i][j].end())
146                         {
147                                 T_IndexBuf::iterator jt = 
148                                         std::find(cobound[i][j+1].begin(), cobound[i][j+1].end(), *it);
149
150                                 if (jt != cobound[i][j+1].end())
151                                 {
152                                         std::swap(*it, cobound[i][j].back());
153                                         cobound[i][j].pop_back();
154                                 }
155                                 else
156                                 {
157                                         ++it;
158                                 }
159                         }
160                 }
161         }       
162 }
163
164 #endif
165
166 DT_Polyhedron::DT_Polyhedron(const DT_VertexBase *base, DT_Count count, const DT_Index *indices)
167 {
168         assert(count);
169
170         std::vector<MT_Point3> vertexBuf;
171         DT_Index i;
172         for (i = 0; i != count; ++i) 
173         {
174                 vertexBuf.push_back((*base)[indices[i]]);
175         }
176
177         T_IndexBuf *indexBuf = count > 4 ? adjacency_graph(count, &vertexBuf[0], 0) : simplex_adjacency_graph(count, 0);
178         
179         std::vector<MT_Point3> pointBuf;
180         
181         for (i = 0; i != count; ++i) 
182         {
183                 if (!indexBuf[i].empty()) 
184                 {
185                         pointBuf.push_back(vertexBuf[i]);
186                 }
187         }
188                         
189         delete [] indexBuf;
190
191         m_count = pointBuf.size();
192         m_verts = new MT_Point3[m_count];       
193         std::copy(pointBuf.begin(), pointBuf.end(), &m_verts[0]);
194
195         T_MultiIndexBuf *cobound = new T_MultiIndexBuf[m_count];
196     char *flags = new char[m_count];
197         std::fill(&flags[0], &flags[m_count], 1);
198
199         DT_Count num_layers = 0;
200         DT_Count layer_count = m_count;
201         while (layer_count > 4)
202         {
203                 T_IndexBuf *indexBuf = adjacency_graph(m_count, m_verts, flags);
204                 
205                 DT_Index i;
206                 for (i = 0; i != m_count; ++i) 
207                 {
208                         if (flags[i])
209                         {
210                                 assert(!indexBuf[i].empty());
211                                 cobound[i].push_back(indexBuf[i]);
212                         }
213                 }
214                         
215                 ++num_layers;
216
217                 delete [] indexBuf;
218
219                 std::fill(&flags[0], &flags[m_count], 0);
220
221                 for (i = 0; i != m_count; ++i)
222                 {
223                         if (cobound[i].size() == num_layers) 
224                         {
225                                 T_IndexBuf& curr_cobound = cobound[i].back();   
226                                 if (!flags[i] && curr_cobound.size() <= 8)
227                                 {       
228                                         DT_Index j;
229                                         for (j  = 0; j != curr_cobound.size(); ++j)
230                                         {
231                                                 flags[curr_cobound[j]] = 1;
232                                         }
233                                 }
234                         }
235                 }
236                 
237                 layer_count = 0;
238                 
239                 for (i = 0; i != m_count; ++i)
240                 {
241                         if (flags[i])
242                         {
243                                 ++layer_count;
244                         }
245                 }       
246         }
247         
248         indexBuf = simplex_adjacency_graph(m_count, flags);
249                 
250         for (i = 0; i != m_count; ++i) 
251         {
252                 if (flags[i])
253                 {
254                         assert(!indexBuf[i].empty());
255                         cobound[i].push_back(indexBuf[i]);
256                 }
257         }
258         
259         ++num_layers;
260
261         delete [] indexBuf;
262         delete [] flags;
263                 
264
265
266 #ifdef DK_HIERARCHY
267         prune(m_count, cobound);
268 #endif
269
270         m_cobound = new T_MultiIndexArray[m_count];
271
272         for (i = 0; i != m_count; ++i)
273         {
274                 new (&m_cobound[i]) T_MultiIndexArray(cobound[i].size());
275                 
276                 DT_Index j;
277                 for (j = 0; j != cobound[i].size(); ++j)
278                 {
279                         new (&m_cobound[i][j]) DT_IndexArray(cobound[i][j].size(), &cobound[i][j][0]);
280                 }
281         }
282                 
283         delete [] cobound;
284
285         m_start_vertex = 0;
286         while (m_cobound[m_start_vertex].size() != num_layers) 
287         {
288                 ++m_start_vertex;
289                 assert(m_start_vertex < m_count);
290         }
291
292         m_curr_vertex = m_start_vertex;
293
294
295
296 DT_Polyhedron::~DT_Polyhedron() 
297 {
298         delete [] m_verts;
299     delete [] m_cobound;
300 }
301
302 #ifdef DK_HIERARCHY
303
304 MT_Scalar DT_Polyhedron::supportH(const MT_Vector3& v) const 
305 {
306     m_curr_vertex = m_start_vertex;
307     MT_Scalar d = (*this)[m_curr_vertex].dot(v);
308     MT_Scalar h = d;
309         int curr_layer;
310         for (curr_layer = m_cobound[m_start_vertex].size(); curr_layer != 0; --curr_layer)
311         {
312                 const DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][curr_layer-1];
313         DT_Index i;
314                 for (i = 0; i != curr_cobound.size(); ++i) 
315                 {
316                         d = (*this)[curr_cobound[i]].dot(v);
317                         if (d > h)
318                         {
319                                 m_curr_vertex = curr_cobound[i];
320                                 h = d;
321                         }
322                 }
323         }
324         
325     return h;
326 }
327
328 MT_Point3 DT_Polyhedron::support(const MT_Vector3& v) const 
329 {
330         m_curr_vertex = m_start_vertex;
331     MT_Scalar d = (*this)[m_curr_vertex].dot(v);
332     MT_Scalar h = d;
333         int curr_layer;
334         for (curr_layer = m_cobound[m_start_vertex].size(); curr_layer != 0; --curr_layer)
335         {
336                 const DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][curr_layer-1];
337         DT_Index i;
338                 for (i = 0; i != curr_cobound.size(); ++i) 
339                 {
340                         d = (*this)[curr_cobound[i]].dot(v);
341                         if (d > h)
342                         {
343                                 m_curr_vertex = curr_cobound[i];
344                                 h = d;
345                         }
346                 }
347         }
348         
349     return (*this)[m_curr_vertex];
350 }
351
352 #else
353
354 MT_Scalar DT_Polyhedron::supportH(const MT_Vector3& v) const 
355 {
356     int last_vertex = -1;
357     MT_Scalar d = (*this)[m_curr_vertex].dot(v);
358     MT_Scalar h = d;
359         
360         for (;;) 
361         {
362         DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][0];
363         int i = 0, n = curr_cobound.size(); 
364         while (i != n && 
365                (curr_cobound[i] == last_vertex || 
366                                 (d = (*this)[curr_cobound[i]].dot(v)) - h <= MT_abs(h) * MT_EPSILON)) 
367                 {
368             ++i;
369                 }
370                 
371         if (i == n) 
372                 {
373                         break;
374                 }
375                 
376         last_vertex = m_curr_vertex;
377         m_curr_vertex = curr_cobound[i];
378         h = d;
379     }
380     return h;
381 }
382
383 MT_Point3 DT_Polyhedron::support(const MT_Vector3& v) const 
384 {
385         int last_vertex = -1;
386     MT_Scalar d = (*this)[m_curr_vertex].dot(v);
387     MT_Scalar h = d;
388         
389     for (;;)
390         {
391         DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][0];
392         int i = 0, n = curr_cobound.size();
393         while (i != n && 
394                (curr_cobound[i] == last_vertex || 
395                                 (d = (*this)[curr_cobound[i]].dot(v)) - h <= MT_abs(h) * MT_EPSILON)) 
396                 {
397             ++i;
398                 }
399                 
400         if (i == n)
401                 {
402                         break;
403                 }
404                 
405                 last_vertex = m_curr_vertex;
406         m_curr_vertex = curr_cobound[i];
407         h = d;
408     }
409     return (*this)[m_curr_vertex];
410 }
411
412 #endif
413
414 #endif
415