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