1 /*
2 Stan Melax Convex Hull Computation
3 Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include <string.h>
17
18 #include "btConvexHull.h"
19 #include "btAlignedObjectArray.h"
20 #include "btMinMax.h"
21 #include "btVector3.h"
22
23
24
25
26
27 //----------------------------------
28
29 class int3
30 {
31 public:
32 int x,y,z;
int3()33 int3(){};
int3(int _x,int _y,int _z)34 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
operator [](int i) const35 const int& operator[](int i) const {return (&x)[i];}
operator [](int i)36 int& operator[](int i) {return (&x)[i];}
37 };
38
39
40 //------- btPlane ----------
41
42
PlaneFlip(const btPlane & plane)43 inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
operator ==(const btPlane & a,const btPlane & b)44 inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
coplanar(const btPlane & a,const btPlane & b)45 inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
46
47
48 //--------- Utility Functions ------
49
50 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
51 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
52
53 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
ThreePlaneIntersection(const btPlane & p0,const btPlane & p1,const btPlane & p2)54 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
55 {
56 btVector3 N1 = p0.normal;
57 btVector3 N2 = p1.normal;
58 btVector3 N3 = p2.normal;
59
60 btVector3 n2n3; n2n3 = N2.cross(N3);
61 btVector3 n3n1; n3n1 = N3.cross(N1);
62 btVector3 n1n2; n1n2 = N1.cross(N2);
63
64 btScalar quotient = (N1.dot(n2n3));
65
66 btAssert(btFabs(quotient) > btScalar(0.000001));
67
68 quotient = btScalar(-1.) / quotient;
69 n2n3 *= p0.dist;
70 n3n1 *= p1.dist;
71 n1n2 *= p2.dist;
72 btVector3 potentialVertex = n2n3;
73 potentialVertex += n3n1;
74 potentialVertex += n1n2;
75 potentialVertex *= quotient;
76
77 btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
78 return result;
79
80 }
81
82 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
83 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
84 btVector3 NormalOf(const btVector3 *vert, const int n);
85
86
PlaneLineIntersection(const btPlane & plane,const btVector3 & p0,const btVector3 & p1)87 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
88 {
89 // returns the point where the line p0-p1 intersects the plane n&d
90 btVector3 dif;
91 dif = p1-p0;
92 btScalar dn= btDot(plane.normal,dif);
93 btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
94 return p0 + (dif*t);
95 }
96
PlaneProject(const btPlane & plane,const btVector3 & point)97 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
98 {
99 return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
100 }
101
TriNormal(const btVector3 & v0,const btVector3 & v1,const btVector3 & v2)102 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
103 {
104 // return the normal of the triangle
105 // inscribed by v0, v1, and v2
106 btVector3 cp=btCross(v1-v0,v2-v1);
107 btScalar m=cp.length();
108 if(m==0) return btVector3(1,0,0);
109 return cp*(btScalar(1.0)/m);
110 }
111
112
DistanceBetweenLines(const btVector3 & ustart,const btVector3 & udir,const btVector3 & vstart,const btVector3 & vdir,btVector3 * upoint,btVector3 * vpoint)113 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
114 {
115 btVector3 cp;
116 cp = btCross(udir,vdir).normalized();
117
118 btScalar distu = -btDot(cp,ustart);
119 btScalar distv = -btDot(cp,vstart);
120 btScalar dist = (btScalar)fabs(distu-distv);
121 if(upoint)
122 {
123 btPlane plane;
124 plane.normal = btCross(vdir,cp).normalized();
125 plane.dist = -btDot(plane.normal,vstart);
126 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
127 }
128 if(vpoint)
129 {
130 btPlane plane;
131 plane.normal = btCross(udir,cp).normalized();
132 plane.dist = -btDot(plane.normal,ustart);
133 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
134 }
135 return dist;
136 }
137
138
139
140
141
142
143
144 #define COPLANAR (0)
145 #define UNDER (1)
146 #define OVER (2)
147 #define SPLIT (OVER|UNDER)
148 #define PAPERWIDTH (btScalar(0.001))
149
150 btScalar planetestepsilon = PAPERWIDTH;
151
152
153
154 typedef ConvexH::HalfEdge HalfEdge;
155
ConvexH(int vertices_size,int edges_size,int facets_size)156 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
157 {
158 vertices.resize(vertices_size);
159 edges.resize(edges_size);
160 facets.resize(facets_size);
161 }
162
163
164 int PlaneTest(const btPlane &p, const btVector3 &v);
PlaneTest(const btPlane & p,const btVector3 & v)165 int PlaneTest(const btPlane &p, const btVector3 &v) {
166 btScalar a = btDot(v,p.normal)+p.dist;
167 int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
168 return flag;
169 }
170
171 int SplitTest(ConvexH &convex,const btPlane &plane);
SplitTest(ConvexH & convex,const btPlane & plane)172 int SplitTest(ConvexH &convex,const btPlane &plane) {
173 int flag=0;
174 for(int i=0;i<convex.vertices.size();i++) {
175 flag |= PlaneTest(plane,convex.vertices[i]);
176 }
177 return flag;
178 }
179
180 class VertFlag
181 {
182 public:
183 unsigned char planetest;
184 unsigned char junk;
185 unsigned char undermap;
186 unsigned char overmap;
187 };
188 class EdgeFlag
189 {
190 public:
191 unsigned char planetest;
192 unsigned char fixes;
193 short undermap;
194 short overmap;
195 };
196 class PlaneFlag
197 {
198 public:
199 unsigned char undermap;
200 unsigned char overmap;
201 };
202 class Coplanar{
203 public:
204 unsigned short ea;
205 unsigned char v0;
206 unsigned char v1;
207 };
208
209
210
211
212
213
214
215
216 template<class T>
maxdirfiltered(const T * p,int count,const T & dir,btAlignedObjectArray<int> & allow)217 int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
218 {
219 btAssert(count);
220 int m=-1;
221 for(int i=0;i<count;i++)
222 if(allow[i])
223 {
224 if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
225 m=i;
226 }
227 btAssert(m!=-1);
228 return m;
229 }
230
231 btVector3 orth(const btVector3 &v);
orth(const btVector3 & v)232 btVector3 orth(const btVector3 &v)
233 {
234 btVector3 a=btCross(v,btVector3(0,0,1));
235 btVector3 b=btCross(v,btVector3(0,1,0));
236 if (a.length() > b.length())
237 {
238 return a.normalized();
239 } else {
240 return b.normalized();
241 }
242 }
243
244
245 template<class T>
maxdirsterid(const T * p,int count,const T & dir,btAlignedObjectArray<int> & allow)246 int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
247 {
248 int m=-1;
249 while(m==-1)
250 {
251 m = maxdirfiltered(p,count,dir,allow);
252 if(allow[m]==3) return m;
253 T u = orth(dir);
254 T v = btCross(u,dir);
255 int ma=-1;
256 for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
257 {
258 btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
259 btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
260 int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
261 if(ma==m && mb==m)
262 {
263 allow[m]=3;
264 return m;
265 }
266 if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
267 {
268 int mc = ma;
269 for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
270 {
271 btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
272 btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
273 int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
274 if(mc==m && md==m)
275 {
276 allow[m]=3;
277 return m;
278 }
279 mc=md;
280 }
281 }
282 ma=mb;
283 }
284 allow[m]=0;
285 m=-1;
286 }
287 btAssert(0);
288 return m;
289 }
290
291
292
293
294 int operator ==(const int3 &a,const int3 &b);
operator ==(const int3 & a,const int3 & b)295 int operator ==(const int3 &a,const int3 &b)
296 {
297 for(int i=0;i<3;i++)
298 {
299 if(a[i]!=b[i]) return 0;
300 }
301 return 1;
302 }
303
304
305 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
above(btVector3 * vertices,const int3 & t,const btVector3 & p,btScalar epsilon)306 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
307 {
308 btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
309 return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
310 }
311 int hasedge(const int3 &t, int a,int b);
hasedge(const int3 & t,int a,int b)312 int hasedge(const int3 &t, int a,int b)
313 {
314 for(int i=0;i<3;i++)
315 {
316 int i1= (i+1)%3;
317 if(t[i]==a && t[i1]==b) return 1;
318 }
319 return 0;
320 }
321 int hasvert(const int3 &t, int v);
hasvert(const int3 & t,int v)322 int hasvert(const int3 &t, int v)
323 {
324 return (t[0]==v || t[1]==v || t[2]==v) ;
325 }
326 int shareedge(const int3 &a,const int3 &b);
shareedge(const int3 & a,const int3 & b)327 int shareedge(const int3 &a,const int3 &b)
328 {
329 int i;
330 for(i=0;i<3;i++)
331 {
332 int i1= (i+1)%3;
333 if(hasedge(a,b[i1],b[i])) return 1;
334 }
335 return 0;
336 }
337
338 class btHullTriangle;
339
340
341
342 class btHullTriangle : public int3
343 {
344 public:
345 int3 n;
346 int id;
347 int vmax;
348 btScalar rise;
btHullTriangle(int a,int b,int c)349 btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
350 {
351 vmax=-1;
352 rise = btScalar(0.0);
353 }
~btHullTriangle()354 ~btHullTriangle()
355 {
356 }
357 int &neib(int a,int b);
358 };
359
360
neib(int a,int b)361 int &btHullTriangle::neib(int a,int b)
362 {
363 static int er=-1;
364 int i;
365 for(i=0;i<3;i++)
366 {
367 int i1=(i+1)%3;
368 int i2=(i+2)%3;
369 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
370 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
371 }
372 btAssert(0);
373 return er;
374 }
b2bfix(btHullTriangle * s,btHullTriangle * t)375 void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
376 {
377 int i;
378 for(i=0;i<3;i++)
379 {
380 int i1=(i+1)%3;
381 int i2=(i+2)%3;
382 int a = (*s)[i1];
383 int b = (*s)[i2];
384 btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
385 btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
386 m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
387 m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
388 }
389 }
390
removeb2b(btHullTriangle * s,btHullTriangle * t)391 void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
392 {
393 b2bfix(s,t);
394 deAllocateTriangle(s);
395
396 deAllocateTriangle(t);
397 }
398
checkit(btHullTriangle * t)399 void HullLibrary::checkit(btHullTriangle *t)
400 {
401 (void)t;
402
403 int i;
404 btAssert(m_tris[t->id]==t);
405 for(i=0;i<3;i++)
406 {
407 int i1=(i+1)%3;
408 int i2=(i+2)%3;
409 int a = (*t)[i1];
410 int b = (*t)[i2];
411
412 // release compile fix
413 (void)i1;
414 (void)i2;
415 (void)a;
416 (void)b;
417
418 btAssert(a!=b);
419 btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
420 }
421 }
422
allocateTriangle(int a,int b,int c)423 btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
424 {
425 void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
426 btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
427 tr->id = m_tris.size();
428 m_tris.push_back(tr);
429
430 return tr;
431 }
432
deAllocateTriangle(btHullTriangle * tri)433 void HullLibrary::deAllocateTriangle(btHullTriangle* tri)
434 {
435 btAssert(m_tris[tri->id]==tri);
436 m_tris[tri->id]=NULL;
437 tri->~btHullTriangle();
438 btAlignedFree(tri);
439 }
440
441
extrude(btHullTriangle * t0,int v)442 void HullLibrary::extrude(btHullTriangle *t0,int v)
443 {
444 int3 t= *t0;
445 int n = m_tris.size();
446 btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
447 ta->n = int3(t0->n[0],n+1,n+2);
448 m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
449 btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
450 tb->n = int3(t0->n[1],n+2,n+0);
451 m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
452 btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
453 tc->n = int3(t0->n[2],n+0,n+1);
454 m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
455 checkit(ta);
456 checkit(tb);
457 checkit(tc);
458 if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
459 if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
460 if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
461 deAllocateTriangle(t0);
462
463 }
464
extrudable(btScalar epsilon)465 btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
466 {
467 int i;
468 btHullTriangle *t=NULL;
469 for(i=0;i<m_tris.size();i++)
470 {
471 if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
472 {
473 t = m_tris[i];
474 }
475 }
476 return (t->rise >epsilon)?t:NULL ;
477 }
478
479
480
481
FindSimplex(btVector3 * verts,int verts_count,btAlignedObjectArray<int> & allow)482 int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
483 {
484 btVector3 basis[3];
485 basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
486 int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
487 int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
488 basis[0] = verts[p0]-verts[p1];
489 if(p0==p1 || basis[0]==btVector3(0,0,0))
490 return int4(-1,-1,-1,-1);
491 basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
492 basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
493 if (basis[1].length() > basis[2].length())
494 {
495 basis[1].normalize();
496 } else {
497 basis[1] = basis[2];
498 basis[1].normalize ();
499 }
500 int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
501 if(p2 == p0 || p2 == p1)
502 {
503 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
504 }
505 if(p2 == p0 || p2 == p1)
506 return int4(-1,-1,-1,-1);
507 basis[1] = verts[p2] - verts[p0];
508 basis[2] = btCross(basis[1],basis[0]).normalized();
509 int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
510 if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
511 if(p3==p0||p3==p1||p3==p2)
512 return int4(-1,-1,-1,-1);
513 btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
514 if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);}
515 return int4(p0,p1,p2,p3);
516 }
517
calchullgen(btVector3 * verts,int verts_count,int vlimit)518 int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
519 {
520 if(verts_count <4) return 0;
521 if(vlimit==0) vlimit=1000000000;
522 int j;
523 btVector3 bmin(*verts),bmax(*verts);
524 btAlignedObjectArray<int> isextreme;
525 isextreme.reserve(verts_count);
526 btAlignedObjectArray<int> allow;
527 allow.reserve(verts_count);
528
529 for(j=0;j<verts_count;j++)
530 {
531 allow.push_back(1);
532 isextreme.push_back(0);
533 bmin.setMin (verts[j]);
534 bmax.setMax (verts[j]);
535 }
536 btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
537 btAssert (epsilon != 0.0);
538
539
540 int4 p = FindSimplex(verts,verts_count,allow);
541 if(p.x==-1) return 0; // simplex failed
542
543
544
545 btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point
546 btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
547 btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
548 btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
549 btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
550 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
551 checkit(t0);checkit(t1);checkit(t2);checkit(t3);
552
553 for(j=0;j<m_tris.size();j++)
554 {
555 btHullTriangle *t=m_tris[j];
556 btAssert(t);
557 btAssert(t->vmax<0);
558 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
559 t->vmax = maxdirsterid(verts,verts_count,n,allow);
560 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
561 }
562 btHullTriangle *te;
563 vlimit-=4;
564 while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
565 {
566 //int3 ti=*te;
567 int v=te->vmax;
568 btAssert(v != -1);
569 btAssert(!isextreme[v]); // wtf we've already done this vertex
570 isextreme[v]=1;
571 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
572 j=m_tris.size();
573 while(j--) {
574 if(!m_tris[j]) continue;
575 int3 t=*m_tris[j];
576 if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
577 {
578 extrude(m_tris[j],v);
579 }
580 }
581 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
582 j=m_tris.size();
583 while(j--)
584 {
585 if(!m_tris[j]) continue;
586 if(!hasvert(*m_tris[j],v)) break;
587 int3 nt=*m_tris[j];
588 if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
589 {
590 btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
591 btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
592 extrude(nb,v);
593 j=m_tris.size();
594 }
595 }
596 j=m_tris.size();
597 while(j--)
598 {
599 btHullTriangle *t=m_tris[j];
600 if(!t) continue;
601 if(t->vmax>=0) break;
602 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
603 t->vmax = maxdirsterid(verts,verts_count,n,allow);
604 if(isextreme[t->vmax])
605 {
606 t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
607 }
608 else
609 {
610 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
611 }
612 }
613 vlimit --;
614 }
615 return 1;
616 }
617
calchull(btVector3 * verts,int verts_count,TUIntArray & tris_out,int & tris_count,int vlimit)618 int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
619 {
620 int rc=calchullgen(verts,verts_count, vlimit) ;
621 if(!rc) return 0;
622 btAlignedObjectArray<int> ts;
623 int i;
624
625 for(i=0;i<m_tris.size();i++)
626 {
627 if(m_tris[i])
628 {
629 for(int j=0;j<3;j++)
630 ts.push_back((*m_tris[i])[j]);
631 deAllocateTriangle(m_tris[i]);
632 }
633 }
634 tris_count = ts.size()/3;
635 tris_out.resize(ts.size());
636
637 for (i=0;i<ts.size();i++)
638 {
639 tris_out[i] = static_cast<unsigned int>(ts[i]);
640 }
641 m_tris.resize(0);
642
643 return 1;
644 }
645
646
647
648
649
ComputeHull(unsigned int vcount,const btVector3 * vertices,PHullResult & result,unsigned int vlimit)650 bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
651 {
652
653 int tris_count;
654 int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
655 if(!ret) return false;
656 result.mIndexCount = (unsigned int) (tris_count*3);
657 result.mFaceCount = (unsigned int) tris_count;
658 result.mVertices = (btVector3*) vertices;
659 result.mVcount = (unsigned int) vcount;
660 return true;
661
662 }
663
664
665 void ReleaseHull(PHullResult &result);
ReleaseHull(PHullResult & result)666 void ReleaseHull(PHullResult &result)
667 {
668 if ( result.m_Indices.size() )
669 {
670 result.m_Indices.clear();
671 }
672
673 result.mVcount = 0;
674 result.mIndexCount = 0;
675 result.mVertices = 0;
676 }
677
678
679 //*********************************************************************
680 //*********************************************************************
681 //******** HullLib header
682 //*********************************************************************
683 //*********************************************************************
684
685 //*********************************************************************
686 //*********************************************************************
687 //******** HullLib implementation
688 //*********************************************************************
689 //*********************************************************************
690
CreateConvexHull(const HullDesc & desc,HullResult & result)691 HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
692 HullResult &result) // contains the resulst
693 {
694 HullError ret = QE_FAIL;
695
696
697 PHullResult hr;
698
699 unsigned int vcount = desc.mVcount;
700 if ( vcount < 8 ) vcount = 8;
701
702 btAlignedObjectArray<btVector3> vertexSource;
703 vertexSource.resize(static_cast<int>(vcount));
704
705 btVector3 scale;
706
707 unsigned int ovcount;
708
709 bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
710
711 if ( ok )
712 {
713
714
715 // if ( 1 ) // scale vertices back to their original size.
716 {
717 for (unsigned int i=0; i<ovcount; i++)
718 {
719 btVector3& v = vertexSource[static_cast<int>(i)];
720 v[0]*=scale[0];
721 v[1]*=scale[1];
722 v[2]*=scale[2];
723 }
724 }
725
726 ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
727
728 if ( ok )
729 {
730
731 // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
732 btAlignedObjectArray<btVector3> vertexScratch;
733 vertexScratch.resize(static_cast<int>(hr.mVcount));
734
735 BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
736
737 ret = QE_OK;
738
739 if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
740 {
741 result.mPolygons = false;
742 result.mNumOutputVertices = ovcount;
743 result.m_OutputVertices.resize(static_cast<int>(ovcount));
744 result.mNumFaces = hr.mFaceCount;
745 result.mNumIndices = hr.mIndexCount;
746
747 result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
748
749 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
750
751 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
752 {
753
754 const unsigned int *source = &hr.m_Indices[0];
755 unsigned int *dest = &result.m_Indices[0];
756
757 for (unsigned int i=0; i<hr.mFaceCount; i++)
758 {
759 dest[0] = source[2];
760 dest[1] = source[1];
761 dest[2] = source[0];
762 dest+=3;
763 source+=3;
764 }
765
766 }
767 else
768 {
769 memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
770 }
771 }
772 else
773 {
774 result.mPolygons = true;
775 result.mNumOutputVertices = ovcount;
776 result.m_OutputVertices.resize(static_cast<int>(ovcount));
777 result.mNumFaces = hr.mFaceCount;
778 result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
779 result.m_Indices.resize(static_cast<int>(result.mNumIndices));
780 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
781
782 // if ( 1 )
783 {
784 const unsigned int *source = &hr.m_Indices[0];
785 unsigned int *dest = &result.m_Indices[0];
786 for (unsigned int i=0; i<hr.mFaceCount; i++)
787 {
788 dest[0] = 3;
789 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
790 {
791 dest[1] = source[2];
792 dest[2] = source[1];
793 dest[3] = source[0];
794 }
795 else
796 {
797 dest[1] = source[0];
798 dest[2] = source[1];
799 dest[3] = source[2];
800 }
801
802 dest+=4;
803 source+=3;
804 }
805 }
806 }
807 ReleaseHull(hr);
808 }
809 }
810
811 return ret;
812 }
813
814
815
ReleaseResult(HullResult & result)816 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
817 {
818 if ( result.m_OutputVertices.size())
819 {
820 result.mNumOutputVertices=0;
821 result.m_OutputVertices.clear();
822 }
823 if ( result.m_Indices.size() )
824 {
825 result.mNumIndices=0;
826 result.m_Indices.clear();
827 }
828 return QE_OK;
829 }
830
831
addPoint(unsigned int & vcount,btVector3 * p,btScalar x,btScalar y,btScalar z)832 static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
833 {
834 // XXX, might be broken
835 btVector3& dest = p[vcount];
836 dest[0] = x;
837 dest[1] = y;
838 dest[2] = z;
839 vcount++;
840 }
841
842 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
GetDist(btScalar px,btScalar py,btScalar pz,const btScalar * p2)843 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
844 {
845
846 btScalar dx = px - p2[0];
847 btScalar dy = py - p2[1];
848 btScalar dz = pz - p2[2];
849
850 return dx*dx+dy*dy+dz*dz;
851 }
852
853
854
CleanupVertices(unsigned int svcount,const btVector3 * svertices,unsigned int stride,unsigned int & vcount,btVector3 * vertices,btScalar normalepsilon,btVector3 & scale)855 bool HullLibrary::CleanupVertices(unsigned int svcount,
856 const btVector3 *svertices,
857 unsigned int stride,
858 unsigned int &vcount, // output number of vertices
859 btVector3 *vertices, // location to store the results.
860 btScalar normalepsilon,
861 btVector3& scale)
862 {
863 if ( svcount == 0 ) return false;
864
865 m_vertexIndexMapping.resize(0);
866
867
868 #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
869
870 vcount = 0;
871
872 btScalar recip[3]={0.f,0.f,0.f};
873
874 if ( scale )
875 {
876 scale[0] = 1;
877 scale[1] = 1;
878 scale[2] = 1;
879 }
880
881 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
882 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
883
884 const char *vtx = (const char *) svertices;
885
886 // if ( 1 )
887 {
888 for (unsigned int i=0; i<svcount; i++)
889 {
890 const btScalar *p = (const btScalar *) vtx;
891
892 vtx+=stride;
893
894 for (int j=0; j<3; j++)
895 {
896 if ( p[j] < bmin[j] ) bmin[j] = p[j];
897 if ( p[j] > bmax[j] ) bmax[j] = p[j];
898 }
899 }
900 }
901
902 btScalar dx = bmax[0] - bmin[0];
903 btScalar dy = bmax[1] - bmin[1];
904 btScalar dz = bmax[2] - bmin[2];
905
906 btVector3 center;
907
908 center[0] = dx*btScalar(0.5) + bmin[0];
909 center[1] = dy*btScalar(0.5) + bmin[1];
910 center[2] = dz*btScalar(0.5) + bmin[2];
911
912 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
913 {
914
915 btScalar len = FLT_MAX;
916
917 if ( dx > EPSILON && dx < len ) len = dx;
918 if ( dy > EPSILON && dy < len ) len = dy;
919 if ( dz > EPSILON && dz < len ) len = dz;
920
921 if ( len == FLT_MAX )
922 {
923 dx = dy = dz = btScalar(0.01); // one centimeter
924 }
925 else
926 {
927 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
928 if ( dy < EPSILON ) dy = len * btScalar(0.05);
929 if ( dz < EPSILON ) dz = len * btScalar(0.05);
930 }
931
932 btScalar x1 = center[0] - dx;
933 btScalar x2 = center[0] + dx;
934
935 btScalar y1 = center[1] - dy;
936 btScalar y2 = center[1] + dy;
937
938 btScalar z1 = center[2] - dz;
939 btScalar z2 = center[2] + dz;
940
941 addPoint(vcount,vertices,x1,y1,z1);
942 addPoint(vcount,vertices,x2,y1,z1);
943 addPoint(vcount,vertices,x2,y2,z1);
944 addPoint(vcount,vertices,x1,y2,z1);
945 addPoint(vcount,vertices,x1,y1,z2);
946 addPoint(vcount,vertices,x2,y1,z2);
947 addPoint(vcount,vertices,x2,y2,z2);
948 addPoint(vcount,vertices,x1,y2,z2);
949
950 return true; // return cube
951
952
953 }
954 else
955 {
956 if ( scale )
957 {
958 scale[0] = dx;
959 scale[1] = dy;
960 scale[2] = dz;
961
962 recip[0] = 1 / dx;
963 recip[1] = 1 / dy;
964 recip[2] = 1 / dz;
965
966 center[0]*=recip[0];
967 center[1]*=recip[1];
968 center[2]*=recip[2];
969
970 }
971
972 }
973
974
975
976 vtx = (const char *) svertices;
977
978 for (unsigned int i=0; i<svcount; i++)
979 {
980 const btVector3 *p = (const btVector3 *)vtx;
981 vtx+=stride;
982
983 btScalar px = p->getX();
984 btScalar py = p->getY();
985 btScalar pz = p->getZ();
986
987 if ( scale )
988 {
989 px = px*recip[0]; // normalize
990 py = py*recip[1]; // normalize
991 pz = pz*recip[2]; // normalize
992 }
993
994 // if ( 1 )
995 {
996 unsigned int j;
997
998 for (j=0; j<vcount; j++)
999 {
1000 /// XXX might be broken
1001 btVector3& v = vertices[j];
1002
1003 btScalar x = v[0];
1004 btScalar y = v[1];
1005 btScalar z = v[2];
1006
1007 btScalar dx = btFabs(x - px );
1008 btScalar dy = btFabs(y - py );
1009 btScalar dz = btFabs(z - pz );
1010
1011 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
1012 {
1013 // ok, it is close enough to the old one
1014 // now let us see if it is further from the center of the point cloud than the one we already recorded.
1015 // in which case we keep this one instead.
1016
1017 btScalar dist1 = GetDist(px,py,pz,center);
1018 btScalar dist2 = GetDist(v[0],v[1],v[2],center);
1019
1020 if ( dist1 > dist2 )
1021 {
1022 v[0] = px;
1023 v[1] = py;
1024 v[2] = pz;
1025
1026 }
1027
1028 break;
1029 }
1030 }
1031
1032 if ( j == vcount )
1033 {
1034 btVector3& dest = vertices[vcount];
1035 dest[0] = px;
1036 dest[1] = py;
1037 dest[2] = pz;
1038 vcount++;
1039 }
1040 m_vertexIndexMapping.push_back(j);
1041 }
1042 }
1043
1044 // ok..now make sure we didn't prune so many vertices it is now invalid.
1045 // if ( 1 )
1046 {
1047 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
1048 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
1049
1050 for (unsigned int i=0; i<vcount; i++)
1051 {
1052 const btVector3& p = vertices[i];
1053 for (int j=0; j<3; j++)
1054 {
1055 if ( p[j] < bmin[j] ) bmin[j] = p[j];
1056 if ( p[j] > bmax[j] ) bmax[j] = p[j];
1057 }
1058 }
1059
1060 btScalar dx = bmax[0] - bmin[0];
1061 btScalar dy = bmax[1] - bmin[1];
1062 btScalar dz = bmax[2] - bmin[2];
1063
1064 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
1065 {
1066 btScalar cx = dx*btScalar(0.5) + bmin[0];
1067 btScalar cy = dy*btScalar(0.5) + bmin[1];
1068 btScalar cz = dz*btScalar(0.5) + bmin[2];
1069
1070 btScalar len = FLT_MAX;
1071
1072 if ( dx >= EPSILON && dx < len ) len = dx;
1073 if ( dy >= EPSILON && dy < len ) len = dy;
1074 if ( dz >= EPSILON && dz < len ) len = dz;
1075
1076 if ( len == FLT_MAX )
1077 {
1078 dx = dy = dz = btScalar(0.01); // one centimeter
1079 }
1080 else
1081 {
1082 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
1083 if ( dy < EPSILON ) dy = len * btScalar(0.05);
1084 if ( dz < EPSILON ) dz = len * btScalar(0.05);
1085 }
1086
1087 btScalar x1 = cx - dx;
1088 btScalar x2 = cx + dx;
1089
1090 btScalar y1 = cy - dy;
1091 btScalar y2 = cy + dy;
1092
1093 btScalar z1 = cz - dz;
1094 btScalar z2 = cz + dz;
1095
1096 vcount = 0; // add box
1097
1098 addPoint(vcount,vertices,x1,y1,z1);
1099 addPoint(vcount,vertices,x2,y1,z1);
1100 addPoint(vcount,vertices,x2,y2,z1);
1101 addPoint(vcount,vertices,x1,y2,z1);
1102 addPoint(vcount,vertices,x1,y1,z2);
1103 addPoint(vcount,vertices,x2,y1,z2);
1104 addPoint(vcount,vertices,x2,y2,z2);
1105 addPoint(vcount,vertices,x1,y2,z2);
1106
1107 return true;
1108 }
1109 }
1110
1111 return true;
1112 }
1113
BringOutYourDead(const btVector3 * verts,unsigned int vcount,btVector3 * overts,unsigned int & ocount,unsigned int * indices,unsigned indexcount)1114 void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
1115 {
1116 btAlignedObjectArray<int>tmpIndices;
1117 tmpIndices.resize(m_vertexIndexMapping.size());
1118 int i;
1119
1120 for (i=0;i<m_vertexIndexMapping.size();i++)
1121 {
1122 tmpIndices[i] = m_vertexIndexMapping[i];
1123 }
1124
1125 TUIntArray usedIndices;
1126 usedIndices.resize(static_cast<int>(vcount));
1127 memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
1128
1129 ocount = 0;
1130
1131 for (i=0; i<int (indexcount); i++)
1132 {
1133 unsigned int v = indices[i]; // original array index
1134
1135 btAssert( v >= 0 && v < vcount );
1136
1137 if ( usedIndices[static_cast<int>(v)] ) // if already remapped
1138 {
1139 indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
1140 }
1141 else
1142 {
1143
1144 indices[i] = ocount; // new index mapping
1145
1146 overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
1147 overts[ocount][1] = verts[v][1];
1148 overts[ocount][2] = verts[v][2];
1149
1150 for (int k=0;k<m_vertexIndexMapping.size();k++)
1151 {
1152 if (tmpIndices[k]==int(v))
1153 m_vertexIndexMapping[k]=ocount;
1154 }
1155
1156 ocount++; // increment output vert count
1157
1158 btAssert( ocount >=0 && ocount <= vcount );
1159
1160 usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
1161
1162
1163 }
1164 }
1165
1166
1167 }
1168