2
Stan Melax Convex Hull Computation
3
Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
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:
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.
18
#include "btConvexHull.h"
19
#include "btAlignedObjectArray.h"
21
#include "btVector3.h"
34
//----------------------------------
41
int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
42
const int& operator[](int i) const {return (&x)[i];}
43
int& operator[](int i) {return (&x)[i];}
47
//------- btPlane ----------
50
inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
51
inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
52
inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
55
//--------- Utility Functions ------
57
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
58
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
60
btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
61
btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
63
btVector3 N1 = p0.normal;
64
btVector3 N2 = p1.normal;
65
btVector3 N3 = p2.normal;
67
btVector3 n2n3; n2n3 = N2.cross(N3);
68
btVector3 n3n1; n3n1 = N3.cross(N1);
69
btVector3 n1n2; n1n2 = N1.cross(N2);
71
btScalar quotient = (N1.dot(n2n3));
73
btAssert(btFabs(quotient) > btScalar(0.000001));
75
quotient = btScalar(-1.) / quotient;
79
btVector3 potentialVertex = n2n3;
80
potentialVertex += n3n1;
81
potentialVertex += n1n2;
82
potentialVertex *= quotient;
84
btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
89
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
90
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
91
btVector3 NormalOf(const btVector3 *vert, const int n);
94
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
96
// returns the point where the line p0-p1 intersects the plane n&d
99
btScalar dn= btDot(plane.normal,dif);
100
btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
104
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
106
return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
109
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
111
// return the normal of the triangle
112
// inscribed by v0, v1, and v2
113
btVector3 cp=btCross(v1-v0,v2-v1);
114
btScalar m=cp.length();
115
if(m==0) return btVector3(1,0,0);
116
return cp*(btScalar(1.0)/m);
120
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
123
cp = btCross(udir,vdir).normalized();
125
btScalar distu = -btDot(cp,ustart);
126
btScalar distv = -btDot(cp,vstart);
127
btScalar dist = (btScalar)fabs(distu-distv);
131
plane.normal = btCross(vdir,cp).normalized();
132
plane.dist = -btDot(plane.normal,vstart);
133
*upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
138
plane.normal = btCross(udir,cp).normalized();
139
plane.dist = -btDot(plane.normal,ustart);
140
*vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
154
#define SPLIT (OVER|UNDER)
155
#define PAPERWIDTH (btScalar(0.001))
157
btScalar planetestepsilon = PAPERWIDTH;
161
typedef ConvexH::HalfEdge HalfEdge;
163
ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
165
vertices.resize(vertices_size);
166
edges.resize(edges_size);
167
facets.resize(facets_size);
171
int PlaneTest(const btPlane &p, const btVector3 &v);
172
int PlaneTest(const btPlane &p, const btVector3 &v) {
173
btScalar a = btDot(v,p.normal)+p.dist;
174
int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
178
int SplitTest(ConvexH &convex,const btPlane &plane);
179
int SplitTest(ConvexH &convex,const btPlane &plane) {
181
for(int i=0;i<convex.vertices.size();i++) {
182
flag |= PlaneTest(plane,convex.vertices[i]);
190
unsigned char planetest;
192
unsigned char undermap;
193
unsigned char overmap;
198
unsigned char planetest;
206
unsigned char undermap;
207
unsigned char overmap;
224
int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
228
for(int i=0;i<count;i++)
231
if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
238
btVector3 orth(const btVector3 &v);
239
btVector3 orth(const btVector3 &v)
241
btVector3 a=btCross(v,btVector3(0,0,1));
242
btVector3 b=btCross(v,btVector3(0,1,0));
243
if (a.length() > b.length())
245
return a.normalized();
247
return b.normalized();
253
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
258
m = maxdirfiltered(p,count,dir,allow);
259
if(allow[m]==3) return m;
261
T v = btCross(u,dir);
263
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
265
btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
266
btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
267
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
273
if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
276
for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
278
btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
279
btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
280
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
301
int operator ==(const int3 &a,const int3 &b);
302
int operator ==(const int3 &a,const int3 &b)
306
if(a[i]!=b[i]) return 0;
312
int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
313
int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
315
btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
316
return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
318
int hasedge(const int3 &t, int a,int b);
319
int hasedge(const int3 &t, int a,int b)
324
if(t[i]==a && t[i1]==b) return 1;
328
int hasvert(const int3 &t, int v);
329
int hasvert(const int3 &t, int v)
331
return (t[0]==v || t[1]==v || t[2]==v) ;
333
int shareedge(const int3 &a,const int3 &b);
334
int shareedge(const int3 &a,const int3 &b)
340
if(hasedge(a,b[i1],b[i])) return 1;
345
class btHullTriangle;
349
class btHullTriangle : public int3
356
btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
359
rise = btScalar(0.0);
364
int &neib(int a,int b);
368
int &btHullTriangle::neib(int a,int b)
376
if((*this)[i]==a && (*this)[i1]==b) return n[i2];
377
if((*this)[i]==b && (*this)[i1]==a) return n[i2];
382
void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
391
btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
392
btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
393
m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
394
m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
398
void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
401
deAllocateTriangle(s);
403
deAllocateTriangle(t);
406
void HullLibrary::checkit(btHullTriangle *t)
411
btAssert(m_tris[t->id]==t);
419
// release compile fix
426
btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
430
btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
432
void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
433
btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
434
tr->id = m_tris.size();
435
m_tris.push_back(tr);
440
void HullLibrary::deAllocateTriangle(btHullTriangle* tri)
442
btAssert(m_tris[tri->id]==tri);
443
m_tris[tri->id]=NULL;
444
tri->~btHullTriangle();
449
void HullLibrary::extrude(btHullTriangle *t0,int v)
452
int n = m_tris.size();
453
btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
454
ta->n = int3(t0->n[0],n+1,n+2);
455
m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
456
btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
457
tb->n = int3(t0->n[1],n+2,n+0);
458
m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
459
btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
460
tc->n = int3(t0->n[2],n+0,n+1);
461
m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
465
if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
466
if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
467
if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
468
deAllocateTriangle(t0);
472
btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
475
btHullTriangle *t=NULL;
476
for(i=0;i<m_tris.size();i++)
478
if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
483
return (t->rise >epsilon)?t:NULL ;
489
int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
492
basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
493
int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
494
int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
495
basis[0] = verts[p0]-verts[p1];
496
if(p0==p1 || basis[0]==btVector3(0,0,0))
497
return int4(-1,-1,-1,-1);
498
basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
499
basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
500
if (basis[1].length() > basis[2].length())
502
basis[1].normalize();
505
basis[1].normalize ();
507
int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
508
if(p2 == p0 || p2 == p1)
510
p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
512
if(p2 == p0 || p2 == p1)
513
return int4(-1,-1,-1,-1);
514
basis[1] = verts[p2] - verts[p0];
515
basis[2] = btCross(basis[1],basis[0]).normalized();
516
int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
517
if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
518
if(p3==p0||p3==p1||p3==p2)
519
return int4(-1,-1,-1,-1);
520
btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
521
if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
522
return int4(p0,p1,p2,p3);
525
int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
527
if(verts_count <4) return 0;
528
if(vlimit==0) vlimit=1000000000;
530
btVector3 bmin(*verts),bmax(*verts);
531
btAlignedObjectArray<int> isextreme;
532
isextreme.reserve(verts_count);
533
btAlignedObjectArray<int> allow;
534
allow.reserve(verts_count);
536
for(j=0;j<verts_count;j++)
539
isextreme.push_back(0);
540
bmin.setMin (verts[j]);
541
bmax.setMax (verts[j]);
543
btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
544
btAssert (epsilon != 0.0);
547
int4 p = FindSimplex(verts,verts_count,allow);
548
if(p.x==-1) return 0; // simplex failed
552
btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point
553
btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
554
btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
555
btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
556
btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
557
isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
558
checkit(t0);checkit(t1);checkit(t2);checkit(t3);
560
for(j=0;j<m_tris.size();j++)
562
btHullTriangle *t=m_tris[j];
565
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
566
t->vmax = maxdirsterid(verts,verts_count,n,allow);
567
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
571
while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
576
btAssert(!isextreme[v]); // wtf we've already done this vertex
578
//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
581
if(!m_tris[j]) continue;
583
if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
585
extrude(m_tris[j],v);
588
// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
592
if(!m_tris[j]) continue;
593
if(!hasvert(*m_tris[j],v)) break;
595
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) )
597
btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
598
btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
606
btHullTriangle *t=m_tris[j];
608
if(t->vmax>=0) break;
609
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
610
t->vmax = maxdirsterid(verts,verts_count,n,allow);
611
if(isextreme[t->vmax])
613
t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
617
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
625
int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
627
int rc=calchullgen(verts,verts_count, vlimit) ;
629
btAlignedObjectArray<int> ts;
632
for(i=0;i<m_tris.size();i++)
637
ts.push_back((*m_tris[i])[j]);
638
deAllocateTriangle(m_tris[i]);
641
tris_count = ts.size()/3;
642
tris_out.resize(ts.size());
644
for (i=0;i<ts.size();i++)
646
tris_out[i] = static_cast<unsigned int>(ts[i]);
657
bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
661
int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
662
if(!ret) return false;
663
result.mIndexCount = (unsigned int) (tris_count*3);
664
result.mFaceCount = (unsigned int) tris_count;
665
result.mVertices = (btVector3*) vertices;
666
result.mVcount = (unsigned int) vcount;
672
void ReleaseHull(PHullResult &result);
673
void ReleaseHull(PHullResult &result)
675
if ( result.m_Indices.size() )
677
result.m_Indices.clear();
681
result.mIndexCount = 0;
682
result.mVertices = 0;
686
//*********************************************************************
687
//*********************************************************************
688
//******** HullLib header
689
//*********************************************************************
690
//*********************************************************************
692
//*********************************************************************
693
//*********************************************************************
694
//******** HullLib implementation
695
//*********************************************************************
696
//*********************************************************************
698
HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
699
HullResult &result) // contains the resulst
701
HullError ret = QE_FAIL;
706
unsigned int vcount = desc.mVcount;
707
if ( vcount < 8 ) vcount = 8;
709
btAlignedObjectArray<btVector3> vertexSource;
710
vertexSource.resize(static_cast<int>(vcount));
714
unsigned int ovcount;
716
bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
722
// if ( 1 ) // scale vertices back to their original size.
724
for (unsigned int i=0; i<ovcount; i++)
726
btVector3& v = vertexSource[static_cast<int>(i)];
733
ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
738
// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
739
btAlignedObjectArray<btVector3> vertexScratch;
740
vertexScratch.resize(static_cast<int>(hr.mVcount));
742
BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
746
if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
748
result.mPolygons = false;
749
result.mNumOutputVertices = ovcount;
750
result.m_OutputVertices.resize(static_cast<int>(ovcount));
751
result.mNumFaces = hr.mFaceCount;
752
result.mNumIndices = hr.mIndexCount;
754
result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
756
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
758
if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
761
const unsigned int *source = &hr.m_Indices[0];
762
unsigned int *dest = &result.m_Indices[0];
764
for (unsigned int i=0; i<hr.mFaceCount; i++)
776
memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
781
result.mPolygons = true;
782
result.mNumOutputVertices = ovcount;
783
result.m_OutputVertices.resize(static_cast<int>(ovcount));
784
result.mNumFaces = hr.mFaceCount;
785
result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
786
result.m_Indices.resize(static_cast<int>(result.mNumIndices));
787
memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
791
const unsigned int *source = &hr.m_Indices[0];
792
unsigned int *dest = &result.m_Indices[0];
793
for (unsigned int i=0; i<hr.mFaceCount; i++)
796
if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
823
HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
825
if ( result.m_OutputVertices.size())
827
result.mNumOutputVertices=0;
828
result.m_OutputVertices.clear();
830
if ( result.m_Indices.size() )
832
result.mNumIndices=0;
833
result.m_Indices.clear();
839
static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
841
// XXX, might be broken
842
btVector3& dest = p[vcount];
849
btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
850
btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
853
btScalar dx = px - p2[0];
854
btScalar dy = py - p2[1];
855
btScalar dz = pz - p2[2];
857
return dx*dx+dy*dy+dz*dz;
862
bool HullLibrary::CleanupVertices(unsigned int svcount,
863
const btVector3 *svertices,
865
unsigned int &vcount, // output number of vertices
866
btVector3 *vertices, // location to store the results.
867
btScalar normalepsilon,
870
if ( svcount == 0 ) return false;
872
m_vertexIndexMapping.resize(0);
875
#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
879
btScalar recip[3]={0.f,0.f,0.f};
888
btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
889
btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
891
const char *vtx = (const char *) svertices;
895
for (unsigned int i=0; i<svcount; i++)
897
const btScalar *p = (const btScalar *) vtx;
901
for (int j=0; j<3; j++)
903
if ( p[j] < bmin[j] ) bmin[j] = p[j];
904
if ( p[j] > bmax[j] ) bmax[j] = p[j];
909
btScalar dx = bmax[0] - bmin[0];
910
btScalar dy = bmax[1] - bmin[1];
911
btScalar dz = bmax[2] - bmin[2];
915
center[0] = dx*btScalar(0.5) + bmin[0];
916
center[1] = dy*btScalar(0.5) + bmin[1];
917
center[2] = dz*btScalar(0.5) + bmin[2];
919
if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
922
btScalar len = FLT_MAX;
924
if ( dx > EPSILON && dx < len ) len = dx;
925
if ( dy > EPSILON && dy < len ) len = dy;
926
if ( dz > EPSILON && dz < len ) len = dz;
928
if ( len == FLT_MAX )
930
dx = dy = dz = btScalar(0.01); // one centimeter
934
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
935
if ( dy < EPSILON ) dy = len * btScalar(0.05);
936
if ( dz < EPSILON ) dz = len * btScalar(0.05);
939
btScalar x1 = center[0] - dx;
940
btScalar x2 = center[0] + dx;
942
btScalar y1 = center[1] - dy;
943
btScalar y2 = center[1] + dy;
945
btScalar z1 = center[2] - dz;
946
btScalar z2 = center[2] + dz;
948
addPoint(vcount,vertices,x1,y1,z1);
949
addPoint(vcount,vertices,x2,y1,z1);
950
addPoint(vcount,vertices,x2,y2,z1);
951
addPoint(vcount,vertices,x1,y2,z1);
952
addPoint(vcount,vertices,x1,y1,z2);
953
addPoint(vcount,vertices,x2,y1,z2);
954
addPoint(vcount,vertices,x2,y2,z2);
955
addPoint(vcount,vertices,x1,y2,z2);
957
return true; // return cube
983
vtx = (const char *) svertices;
985
for (unsigned int i=0; i<svcount; i++)
987
const btVector3 *p = (const btVector3 *)vtx;
990
btScalar px = p->getX();
991
btScalar py = p->getY();
992
btScalar pz = p->getZ();
996
px = px*recip[0]; // normalize
997
py = py*recip[1]; // normalize
998
pz = pz*recip[2]; // normalize
1005
for (j=0; j<vcount; j++)
1007
/// XXX might be broken
1008
btVector3& v = vertices[j];
1014
btScalar dx = btFabs(x - px );
1015
btScalar dy = btFabs(y - py );
1016
btScalar dz = btFabs(z - pz );
1018
if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
1020
// ok, it is close enough to the old one
1021
// now let us see if it is further from the center of the point cloud than the one we already recorded.
1022
// in which case we keep this one instead.
1024
btScalar dist1 = GetDist(px,py,pz,center);
1025
btScalar dist2 = GetDist(v[0],v[1],v[2],center);
1027
if ( dist1 > dist2 )
1041
btVector3& dest = vertices[vcount];
1047
m_vertexIndexMapping.push_back(j);
1051
// ok..now make sure we didn't prune so many vertices it is now invalid.
1054
btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
1055
btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
1057
for (unsigned int i=0; i<vcount; i++)
1059
const btVector3& p = vertices[i];
1060
for (int j=0; j<3; j++)
1062
if ( p[j] < bmin[j] ) bmin[j] = p[j];
1063
if ( p[j] > bmax[j] ) bmax[j] = p[j];
1067
btScalar dx = bmax[0] - bmin[0];
1068
btScalar dy = bmax[1] - bmin[1];
1069
btScalar dz = bmax[2] - bmin[2];
1071
if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
1073
btScalar cx = dx*btScalar(0.5) + bmin[0];
1074
btScalar cy = dy*btScalar(0.5) + bmin[1];
1075
btScalar cz = dz*btScalar(0.5) + bmin[2];
1077
btScalar len = FLT_MAX;
1079
if ( dx >= EPSILON && dx < len ) len = dx;
1080
if ( dy >= EPSILON && dy < len ) len = dy;
1081
if ( dz >= EPSILON && dz < len ) len = dz;
1083
if ( len == FLT_MAX )
1085
dx = dy = dz = btScalar(0.01); // one centimeter
1089
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
1090
if ( dy < EPSILON ) dy = len * btScalar(0.05);
1091
if ( dz < EPSILON ) dz = len * btScalar(0.05);
1094
btScalar x1 = cx - dx;
1095
btScalar x2 = cx + dx;
1097
btScalar y1 = cy - dy;
1098
btScalar y2 = cy + dy;
1100
btScalar z1 = cz - dz;
1101
btScalar z2 = cz + dz;
1103
vcount = 0; // add box
1105
addPoint(vcount,vertices,x1,y1,z1);
1106
addPoint(vcount,vertices,x2,y1,z1);
1107
addPoint(vcount,vertices,x2,y2,z1);
1108
addPoint(vcount,vertices,x1,y2,z1);
1109
addPoint(vcount,vertices,x1,y1,z2);
1110
addPoint(vcount,vertices,x2,y1,z2);
1111
addPoint(vcount,vertices,x2,y2,z2);
1112
addPoint(vcount,vertices,x1,y2,z2);
1121
void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
1123
btAlignedObjectArray<int>tmpIndices;
1124
tmpIndices.resize(m_vertexIndexMapping.size());
1127
for (i=0;i<m_vertexIndexMapping.size();i++)
1129
tmpIndices[i] = m_vertexIndexMapping[i];
1132
TUIntArray usedIndices;
1133
usedIndices.resize(static_cast<int>(vcount));
1134
memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
1138
for (i=0; i<int (indexcount); i++)
1140
unsigned int v = indices[i]; // original array index
1142
btAssert( v >= 0 && v < vcount );
1144
if ( usedIndices[static_cast<int>(v)] ) // if already remapped
1146
indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
1151
indices[i] = ocount; // new index mapping
1153
overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
1154
overts[ocount][1] = verts[v][1];
1155
overts[ocount][2] = verts[v][2];
1157
for (int k=0;k<m_vertexIndexMapping.size();k++)
1159
if (tmpIndices[k]==int(v))
1160
m_vertexIndexMapping[k]=ocount;
1163
ocount++; // increment output vert count
1165
btAssert( ocount >=0 && ocount <= vcount );
1167
usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping