dali-physics/third-party/bullet3/src/LinearMath/btConvexHull.cpp

   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 class int3
  26 {
  27 public:
  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
  42 inline btPlane PlaneFlip(const btPlane &plane) { return btPlane(-plane.normal, -plane.dist); }
  43 inline int operator==(const btPlane &a, const btPlane &b) { return (a.normal == b.normal && a.dist == b.dist); }
  44 inline int coplanar(const btPlane &a, const btPlane &b) { return (a == b || a == PlaneFlip(b)); }
  45
  46 //--------- Utility Functions ------
  47
  48 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
  49 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
  50
  51 btVector3 ThreePlaneIntersection(const btPlane &p0, const btPlane &p1, const btPlane &p2);
  52 btVector3 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
  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 btVector3 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
  96 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
  97 {
  98         return point - plane.normal * (btDot(point, plane.normal) + plane.dist);
  99 }
 100
 101 btVector3 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
 111 btScalar 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
 142 btScalar planetestepsilon = PAPERWIDTH;
 143
 144 typedef ConvexH::HalfEdge HalfEdge;
 145
 146 ConvexH::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
 153 int PlaneTest(const btPlane &p, const btVector3 &v);
 154 int 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
 161 int SplitTest(ConvexH &convex, const btPlane &plane);
 162 int 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
 172 class VertFlag
 173 {
 174 public:
 175         unsigned char planetest;
 176         unsigned char junk;
 177         unsigned char undermap;
 178         unsigned char overmap;
 179 };
 180 class EdgeFlag
 181 {
 182 public:
 183         unsigned char planetest;
 184         unsigned char fixes;
 185         short undermap;
 186         short overmap;
 187 };
 188 class PlaneFlag
 189 {
 190 public:
 191         unsigned char undermap;
 192         unsigned char overmap;
 193 };
 194 class Coplanar
 195 {
 196 public:
 197         unsigned short ea;
 198         unsigned char v0;
 199         unsigned char v1;
 200 };
 201
 202 template <class T>
 203 int 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
 217 btVector3 orth(const btVector3 &v);
 218 btVector3 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
 232 template <class T>
 233 int 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                 {
 245                         btScalar s = btSin(SIMD_RADS_PER_DEG * (x));
 246                         btScalar c = btCos(SIMD_RADS_PER_DEG * (x));
 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
 278 int operator==(const int3 &a, const int3 &b);
 279 int 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
 288 int above(btVector3 *vertices, const int3 &t, const btVector3 &p, btScalar epsilon);
 289 int 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 }
 294 int hasedge(const int3 &t, int a, int b);
 295 int 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 }
 304 int hasvert(const int3 &t, int v);
 305 int hasvert(const int3 &t, int v)
 306 {
 307         return (t[0] == v || t[1] == v || t[2] == v);
 308 }
 309 int shareedge(const int3 &a, const int3 &b);
 310 int 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
 321 class btHullTriangle;
 322
 323 class btHullTriangle : public int3
 324 {
 325 public:
 326         int3 n;
 327         int id;
 328         int vmax;
 329         btScalar rise;
 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         }
 335         ~btHullTriangle()
 336         {
 337         }
 338         int &neib(int a, int b);
 339 };
 340
 341 int &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 }
 355 void HullLibrary::b2bfix(btHullTriangle *s, btHullTriangle *t)
 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
 371 void HullLibrary::removeb2b(btHullTriangle *s, btHullTriangle *t)
 372 {
 373         b2bfix(s, t);
 374         deAllocateTriangle(s);
 375
 376         deAllocateTriangle(t);
 377 }
 378
 379 void HullLibrary::checkit(btHullTriangle *t)
 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
 403 btHullTriangle *HullLibrary::allocateTriangle(int a, int b, int c)
 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
 413 void HullLibrary::deAllocateTriangle(btHullTriangle *tri)
 414 {
 415         btAssert(m_tris[tri->id] == tri);
 416         m_tris[tri->id] = NULL;
 417         tri->~btHullTriangle();
 418         btAlignedFree(tri);
 419 }
 420
 421 void HullLibrary::extrude(btHullTriangle *t0, int v)
 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]]);
 440         deAllocateTriangle(t0);
 441 }
 442
 443 btHullTriangle *HullLibrary::extrudable(btScalar epsilon)
 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
 457 int4 HullLibrary::FindSimplex(btVector3 *verts, int verts_count, btAlignedObjectArray<int> &allow)
 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
 498 int 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);
 504         btAlignedObjectArray<int> isextreme;
 505         isextreme.reserve(verts_count);
 506         btAlignedObjectArray<int> allow;
 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
 605 int 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;
 609         btAlignedObjectArray<int> ts;
 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]);
 618                         deAllocateTriangle(m_tris[i]);
 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
 633 bool 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
 645 void ReleaseHull(PHullResult &result);
 646 void 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
 670 HullError 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
 680         btAlignedObjectArray<btVector3> vertexSource;
 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.
 709                         btAlignedObjectArray<btVector3> vertexScratch;
 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
 728                                 if (desc.HasHullFlag(QF_REVERSE_ORDER))
 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;
 764                                                 if (desc.HasHullFlag(QF_REVERSE_ORDER))
 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
 789 HullError 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
 804 static 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
 814 btScalar GetDist(btScalar px, btScalar py, btScalar pz, const btScalar *p2);
 815 btScalar 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
 824 bool 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
 834         m_vertexIndexMapping.resize(0);
 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                         {
 961                                 /// XXX might be broken
 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                         }
1000                         m_vertexIndexMapping.push_back(j);
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
1074 void 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 }