Bullet Collision Detection & Physics Library
btConvexHull.cpp
Go to the documentation of this file.
1/*
2Stan Melax Convex Hull Computation
3Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include <string.h>
17
18#include "btConvexHull.h"
20#include "btMinMax.h"
21#include "btVector3.h"
22
23
24
25
26
27//----------------------------------
28
29class int3
30{
31public:
32 int x,y,z;
33 int3(){};
34 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
35 const int& operator[](int i) const {return (&x)[i];}
36 int& operator[](int i) {return (&x)[i];}
37};
38
39
40//------- btPlane ----------
41
42
43inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
44inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
45inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
46
47
48//--------- Utility Functions ------
49
50btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
51btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
52
53btVector3 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
82btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
83btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
84btVector3 NormalOf(const btVector3 *vert, const int n);
85
86
87btVector3 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
97btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
98{
99 return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
100}
101
102btVector3 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
113btScalar 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
151
152
153
155
156ConvexH::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
164int PlaneTest(const btPlane &p, const btVector3 &v);
165int PlaneTest(const btPlane &p, const btVector3 &v) {
166 btScalar a = btDot(v,p.normal)+p.dist;
168 return flag;
169}
170
171int SplitTest(ConvexH &convex,const btPlane &plane);
172int 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
181{
182public:
183 unsigned char planetest;
184 unsigned char junk;
185 unsigned char undermap;
186 unsigned char overmap;
187};
189{
190public:
191 unsigned char planetest;
192 unsigned char fixes;
193 short undermap;
194 short overmap;
195};
197{
198public:
199 unsigned char undermap;
200 unsigned char overmap;
201};
203public:
204 unsigned short ea;
205 unsigned char v0;
206 unsigned char v1;
207};
208
209
210
211
212
213
214
215
216template<class T>
217int 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
231btVector3 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
245template<class T>
246int 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 {
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 {
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
294int operator ==(const int3 &a,const int3 &b);
295int 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
305int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
306int 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}
311int hasedge(const int3 &t, int a,int b);
312int 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}
321int hasvert(const int3 &t, int v);
322int hasvert(const int3 &t, int v)
323{
324 return (t[0]==v || t[1]==v || t[2]==v) ;
325}
326int shareedge(const int3 &a,const int3 &b);
327int 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
338class btHullTriangle;
339
340
341
342class btHullTriangle : public int3
343{
344public:
346 int id;
347 int vmax;
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 }
355 {
356 }
357 int &neib(int a,int b);
358};
359
360
361int &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}
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
392{
393 b2bfix(s,t);
395
397}
398
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
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
434{
435 btAssert(m_tris[tri->id]==tri);
436 m_tris[tri->id]=NULL;
437 tri->~btHullTriangle();
438 btAlignedFree(tri);
439}
440
441
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]]);
462
463}
464
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
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
518int 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);
525 isextreme.reserve(verts_count);
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 {
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 {
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
618int 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;
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]);
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
650bool 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
665void 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
691HullError 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
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.
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
816HullError 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
832static 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
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
855bool 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 {
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
1114void 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}
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define EPSILON
ConvexH::HalfEdge HalfEdge
btVector3 ThreePlaneIntersection(const btPlane &p0, const btPlane &p1, const btPlane &p2)
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL)
int maxdirsterid(const T *p, int count, const T &dir, btAlignedObjectArray< int > &allow)
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
#define OVER
static void addPoint(unsigned int &vcount, btVector3 *p, btScalar x, btScalar y, btScalar z)
int operator==(const btPlane &a, const btPlane &b)
void ReleaseHull(PHullResult &result)
#define COPLANAR
int hasvert(const int3 &t, int v)
btPlane PlaneFlip(const btPlane &plane)
btVector3 NormalOf(const btVector3 *vert, const int n)
int coplanar(const btPlane &a, const btPlane &b)
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
int maxdirfiltered(const T *p, int count, const T &dir, btAlignedObjectArray< int > &allow)
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
#define UNDER
int SplitTest(ConvexH &convex, const btPlane &plane)
int hasedge(const int3 &t, int a, int b)
int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon)
btScalar planetestepsilon
int PlaneTest(const btPlane &p, const btVector3 &v)
#define PAPERWIDTH
btScalar GetDist(btScalar px, btScalar py, btScalar pz, const btScalar *p2)
int shareedge(const int3 &a, const int3 &b)
btVector3 orth(const btVector3 &v)
HullError
Definition: btConvexHull.h:109
@ QE_OK
Definition: btConvexHull.h:110
@ QE_FAIL
Definition: btConvexHull.h:111
@ QF_TRIANGLES
Definition: btConvexHull.h:50
@ QF_REVERSE_ORDER
Definition: btConvexHull.h:51
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
#define SIMD_RADS_PER_DEG
Definition: btScalar.h:507
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btSin(btScalar x)
Definition: btScalar.h:477
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
btScalar btCos(btScalar x)
Definition: btScalar.h:476
void btSwap(T &a, T &b)
Definition: btScalar.h:621
#define btAssert(x)
Definition: btScalar.h:131
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:901
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:931
btAlignedObjectArray< HalfEdge > edges
Definition: btConvexHull.h:145
btAlignedObjectArray< btPlane > facets
Definition: btConvexHull.h:146
btAlignedObjectArray< btVector3 > vertices
Definition: btConvexHull.h:144
unsigned short ea
unsigned char v0
unsigned char v1
short overmap
short undermap
unsigned char planetest
unsigned char fixes
bool HasHullFlag(HullFlag flag) const
Definition: btConvexHull.h:83
unsigned int mVcount
Definition: btConvexHull.h:100
btScalar mNormalEpsilon
Definition: btConvexHull.h:103
unsigned int mMaxVertices
Definition: btConvexHull.h:104
const btVector3 * mVertices
Definition: btConvexHull.h:101
unsigned int mVertexStride
Definition: btConvexHull.h:102
btHullTriangle * extrudable(btScalar epsilon)
class btHullTriangle * allocateTriangle(int a, int b, int c)
bool ComputeHull(unsigned int vcount, const btVector3 *vertices, PHullResult &result, unsigned int vlimit)
HullError ReleaseResult(HullResult &result)
void BringOutYourDead(const btVector3 *verts, unsigned int vcount, btVector3 *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount)
HullError CreateConvexHull(const HullDesc &desc, HullResult &result)
void deAllocateTriangle(btHullTriangle *)
int calchull(btVector3 *verts, int verts_count, TUIntArray &tris_out, int &tris_count, int vlimit)
btAlignedObjectArray< int > m_vertexIndexMapping
Definition: btConvexHull.h:191
void b2bfix(btHullTriangle *s, btHullTriangle *t)
void removeb2b(btHullTriangle *s, btHullTriangle *t)
btAlignedObjectArray< class btHullTriangle * > m_tris
Definition: btConvexHull.h:187
void checkit(btHullTriangle *t)
void extrude(class btHullTriangle *t0, int v)
int calchullgen(btVector3 *verts, int verts_count, int vlimit)
int4 FindSimplex(btVector3 *verts, int verts_count, btAlignedObjectArray< int > &allow)
bool CleanupVertices(unsigned int svcount, const btVector3 *svertices, unsigned int stride, unsigned int &vcount, btVector3 *vertices, btScalar normalepsilon, btVector3 &scale)
btAlignedObjectArray< btVector3 > m_OutputVertices
Definition: btConvexHull.h:39
btAlignedObjectArray< unsigned int > m_Indices
Definition: btConvexHull.h:42
unsigned int mNumOutputVertices
Definition: btConvexHull.h:38
unsigned int mNumFaces
Definition: btConvexHull.h:40
bool mPolygons
Definition: btConvexHull.h:37
unsigned int mNumIndices
Definition: btConvexHull.h:41
unsigned int mFaceCount
Definition: btConvexHull.h:175
unsigned int mVcount
Definition: btConvexHull.h:173
unsigned int mIndexCount
Definition: btConvexHull.h:174
btVector3 * mVertices
Definition: btConvexHull.h:176
TUIntArray m_Indices
Definition: btConvexHull.h:177
unsigned char overmap
unsigned char undermap
unsigned char junk
unsigned char undermap
unsigned char overmap
unsigned char planetest
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void push_back(const T &_Val)
int & neib(int a, int b)
btHullTriangle(int a, int b, int c)
btVector3 normal
Definition: btConvexHull.h:117
btScalar dist
Definition: btConvexHull.h:118
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
int & operator[](int i)
int3(int _x, int _y, int _z)
const int & operator[](int i) const
int x
Definition: btConvexHull.h:154