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