Home | History | Annotate | Download | only in LinearMath
      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;
     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 
     43 inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
     44 inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
     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);
     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 
     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 				static 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 
     97 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
     98 {
     99 	return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
    100 }
    101 
    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 
    113 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
    114 {
    115 	static 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 
    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);
    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);
    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>
    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);
    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>
    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);
    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);
    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);
    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);
    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);
    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;
    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 	}
    354 	~btHullTriangle()
    355 	{
    356 	}
    357 	int &neib(int a,int b);
    358 };
    359 
    360 
    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 }
    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 
    391 void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
    392 {
    393 	b2bfix(s,t);
    394 	deAllocateTriangle(s);
    395 
    396 	deAllocateTriangle(t);
    397 }
    398 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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);
    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 
    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 
    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 
    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);
    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 
    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 
   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