1 #include "float_math.h"
2
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <assert.h>
7 #include <math.h>
8 #include <float.h>
9
10 /*----------------------------------------------------------------------
11 Copyright (c) 2004 Open Dynamics Framework Group
12 www.physicstools.org
13 All rights reserved.
14
15 Redistribution and use in source and binary forms, with or without modification, are permitted provided
16 that the following conditions are met:
17
18 Redistributions of source code must retain the above copyright notice, this list of conditions
19 and the following disclaimer.
20
21 Redistributions in binary form must reproduce the above copyright notice,
22 this list of conditions and the following disclaimer in the documentation
23 and/or other materials provided with the distribution.
24
25 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
26 be used to endorse or promote products derived from this software without specific prior written permission.
27
28 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
29 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
30 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
31 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
33 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
34 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
35 -----------------------------------------------------------------------*/
36
37 // http://codesuppository.blogspot.com
38 //
39 // mailto: jratcliff@infiniplex.net
40 //
41 // http://www.amillionpixels.us
42 //
43
44 #include "cd_hull.h"
45
46 using namespace ConvexDecomposition;
47
48 /*----------------------------------------------------------------------
49 Copyright (c) 2004 Open Dynamics Framework Group
50 www.physicstools.org
51 All rights reserved.
52
53 Redistribution and use in source and binary forms, with or without modification, are permitted provided
54 that the following conditions are met:
55
56 Redistributions of source code must retain the above copyright notice, this list of conditions
57 and the following disclaimer.
58
59 Redistributions in binary form must reproduce the above copyright notice,
60 this list of conditions and the following disclaimer in the documentation
61 and/or other materials provided with the distribution.
62
63 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
64 be used to endorse or promote products derived from this software without specific prior written permission.
65
66 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
67 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
68 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
69 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
70 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
71 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
72 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
73 -----------------------------------------------------------------------*/
74
75 #define PI 3.14159264f
76
77 //*****************************************************
78 //*****************************************************
79 //********* Stan Melax's vector math template needed
80 //********* to use his hull building code.
81 //*****************************************************
82 //*****************************************************
83
84 #define DEG2RAD (PI / 180.0f)
85 #define RAD2DEG (180.0f / PI)
86 #define SQRT_OF_2 (1.4142135f)
87 #define OFFSET(Class, Member) (((char *)(&(((Class *)NULL)->Member))) - ((char *)NULL))
88
89 namespace ConvexDecomposition
90 {
91 int argmin(float a[], int n);
92 float sqr(float a);
93 float clampf(float a);
94 float Round(float a, float precision);
95 float Interpolate(const float &f0, const float &f1, float alpha);
96
97 template <class T>
Swap(T & a,T & b)98 void Swap(T &a, T &b)
99 {
100 T tmp = a;
101 a = b;
102 b = tmp;
103 }
104
105 template <class T>
Max(const T & a,const T & b)106 T Max(const T &a, const T &b)
107 {
108 return (a > b) ? a : b;
109 }
110
111 template <class T>
Min(const T & a,const T & b)112 T Min(const T &a, const T &b)
113 {
114 return (a < b) ? a : b;
115 }
116
117 //----------------------------------
118
119 class int3
120 {
121 public:
122 int x, y, z;
int3()123 int3(){};
int3(int _x,int _y,int _z)124 int3(int _x, int _y, int _z)
125 {
126 x = _x;
127 y = _y;
128 z = _z;
129 }
operator [](int i) const130 const int &operator[](int i) const { return (&x)[i]; }
operator [](int i)131 int &operator[](int i) { return (&x)[i]; }
132 };
133
134 //-------- 2D --------
135
136 class float2
137 {
138 public:
139 float x, y;
float2()140 float2()
141 {
142 x = 0;
143 y = 0;
144 };
float2(float _x,float _y)145 float2(float _x, float _y)
146 {
147 x = _x;
148 y = _y;
149 }
operator [](int i)150 float &operator[](int i)
151 {
152 assert(i >= 0 && i < 2);
153 return ((float *)this)[i];
154 }
operator [](int i) const155 const float &operator[](int i) const
156 {
157 assert(i >= 0 && i < 2);
158 return ((float *)this)[i];
159 }
160 };
operator -(const float2 & a,const float2 & b)161 inline float2 operator-(const float2 &a, const float2 &b) { return float2(a.x - b.x, a.y - b.y); }
operator +(const float2 & a,const float2 & b)162 inline float2 operator+(const float2 &a, const float2 &b) { return float2(a.x + b.x, a.y + b.y); }
163
164 //--------- 3D ---------
165
166 class float3 // 3D
167 {
168 public:
169 float x, y, z;
float3()170 float3()
171 {
172 x = 0;
173 y = 0;
174 z = 0;
175 };
float3(float _x,float _y,float _z)176 float3(float _x, float _y, float _z)
177 {
178 x = _x;
179 y = _y;
180 z = _z;
181 };
182 //operator float *() { return &x;};
operator [](int i)183 float &operator[](int i)
184 {
185 assert(i >= 0 && i < 3);
186 return ((float *)this)[i];
187 }
operator [](int i) const188 const float &operator[](int i) const
189 {
190 assert(i >= 0 && i < 3);
191 return ((float *)this)[i];
192 }
193 #ifdef PLUGIN_3DSMAX
float3(const Point3 & p)194 float3(const Point3 &p) : x(p.x), y(p.y), z(p.z)
195 {
196 }
operator Point3()197 operator Point3() { return *((Point3 *)this); }
198 #endif
199 };
200
201 float3 &operator+=(float3 &a, const float3 &b);
202 float3 &operator-=(float3 &a, const float3 &b);
203 float3 &operator*=(float3 &v, const float s);
204 float3 &operator/=(float3 &v, const float s);
205
206 float magnitude(const float3 &v);
207 float3 normalize(const float3 &v);
208 float3 safenormalize(const float3 &v);
209 float3 vabs(const float3 &v);
210 float3 operator+(const float3 &a, const float3 &b);
211 float3 operator-(const float3 &a, const float3 &b);
212 float3 operator-(const float3 &v);
213 float3 operator*(const float3 &v, const float s);
214 float3 operator*(const float s, const float3 &v);
215 float3 operator/(const float3 &v, const float s);
operator ==(const float3 & a,const float3 & b)216 inline int operator==(const float3 &a, const float3 &b) { return (a.x == b.x && a.y == b.y && a.z == b.z); }
operator !=(const float3 & a,const float3 & b)217 inline int operator!=(const float3 &a, const float3 &b) { return (a.x != b.x || a.y != b.y || a.z != b.z); }
218 // due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb.
219 float dot(const float3 &a, const float3 &b);
220 float3 cmul(const float3 &a, const float3 &b);
221 float3 cross(const float3 &a, const float3 &b);
222 float3 Interpolate(const float3 &v0, const float3 &v1, float alpha);
223 float3 Round(const float3 &a, float precision);
224 float3 VectorMax(const float3 &a, const float3 &b);
225 float3 VectorMin(const float3 &a, const float3 &b);
226
227 class float3x3
228 {
229 public:
230 float3 x, y, z; // the 3 rows of the Matrix
float3x3()231 float3x3() {}
float3x3(float xx,float xy,float xz,float yx,float yy,float yz,float zx,float zy,float zz)232 float3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) : x(xx, xy, xz), y(yx, yy, yz), z(zx, zy, zz) {}
float3x3(float3 _x,float3 _y,float3 _z)233 float3x3(float3 _x, float3 _y, float3 _z) : x(_x), y(_y), z(_z) {}
operator [](int i)234 float3 &operator[](int i)
235 {
236 assert(i >= 0 && i < 3);
237 return (&x)[i];
238 }
operator [](int i) const239 const float3 &operator[](int i) const
240 {
241 assert(i >= 0 && i < 3);
242 return (&x)[i];
243 }
operator ()(int r,int c)244 float &operator()(int r, int c)
245 {
246 assert(r >= 0 && r < 3 && c >= 0 && c < 3);
247 return ((&x)[r])[c];
248 }
operator ()(int r,int c) const249 const float &operator()(int r, int c) const
250 {
251 assert(r >= 0 && r < 3 && c >= 0 && c < 3);
252 return ((&x)[r])[c];
253 }
254 };
255 float3x3 Transpose(const float3x3 &m);
256 float3 operator*(const float3 &v, const float3x3 &m);
257 float3 operator*(const float3x3 &m, const float3 &v);
258 float3x3 operator*(const float3x3 &m, const float &s);
259 float3x3 operator*(const float3x3 &ma, const float3x3 &mb);
260 float3x3 operator/(const float3x3 &a, const float &s);
261 float3x3 operator+(const float3x3 &a, const float3x3 &b);
262 float3x3 operator-(const float3x3 &a, const float3x3 &b);
263 float3x3 &operator+=(float3x3 &a, const float3x3 &b);
264 float3x3 &operator-=(float3x3 &a, const float3x3 &b);
265 float3x3 &operator*=(float3x3 &a, const float &s);
266 float Determinant(const float3x3 &m);
267 float3x3 Inverse(const float3x3 &a); // its just 3x3 so we simply do that cofactor method
268
269 //-------- 4D Math --------
270
271 class float4
272 {
273 public:
274 float x, y, z, w;
float4()275 float4()
276 {
277 x = 0;
278 y = 0;
279 z = 0;
280 w = 0;
281 };
float4(float _x,float _y,float _z,float _w)282 float4(float _x, float _y, float _z, float _w)
283 {
284 x = _x;
285 y = _y;
286 z = _z;
287 w = _w;
288 }
float4(const float3 & v,float _w)289 float4(const float3 &v, float _w)
290 {
291 x = v.x;
292 y = v.y;
293 z = v.z;
294 w = _w;
295 }
296 //operator float *() { return &x;};
operator [](int i)297 float &operator[](int i)
298 {
299 assert(i >= 0 && i < 4);
300 return ((float *)this)[i];
301 }
operator [](int i) const302 const float &operator[](int i) const
303 {
304 assert(i >= 0 && i < 4);
305 return ((float *)this)[i];
306 }
xyz() const307 const float3 &xyz() const { return *((float3 *)this); }
xyz()308 float3 &xyz() { return *((float3 *)this); }
309 };
310
311 struct D3DXMATRIX;
312
313 class float4x4
314 {
315 public:
316 float4 x, y, z, w; // the 4 rows
float4x4()317 float4x4() {}
float4x4(const float4 & _x,const float4 & _y,const float4 & _z,const float4 & _w)318 float4x4(const float4 &_x, const float4 &_y, const float4 &_z, const float4 &_w) : x(_x), y(_y), z(_z), w(_w) {}
float4x4(float m00,float m01,float m02,float m03,float m10,float m11,float m12,float m13,float m20,float m21,float m22,float m23,float m30,float m31,float m32,float m33)319 float4x4(float m00, float m01, float m02, float m03,
320 float m10, float m11, float m12, float m13,
321 float m20, float m21, float m22, float m23,
322 float m30, float m31, float m32, float m33)
323 : x(m00, m01, m02, m03), y(m10, m11, m12, m13), z(m20, m21, m22, m23), w(m30, m31, m32, m33) {}
operator ()(int r,int c)324 float &operator()(int r, int c)
325 {
326 assert(r >= 0 && r < 4 && c >= 0 && c < 4);
327 return ((&x)[r])[c];
328 }
operator ()(int r,int c) const329 const float &operator()(int r, int c) const
330 {
331 assert(r >= 0 && r < 4 && c >= 0 && c < 4);
332 return ((&x)[r])[c];
333 }
operator float*()334 operator float *() { return &x.x; }
operator const float*() const335 operator const float *() const { return &x.x; }
operator struct D3DXMATRIX*()336 operator struct D3DXMATRIX *() { return (struct D3DXMATRIX *)this; }
operator const struct D3DXMATRIX*() const337 operator const struct D3DXMATRIX *() const { return (struct D3DXMATRIX *)this; }
338 };
339
340 int operator==(const float4 &a, const float4 &b);
341 float4 Homogenize(const float3 &v3, const float &w = 1.0f); // Turns a 3D float3 4D vector4 by appending w
342 float4 cmul(const float4 &a, const float4 &b);
343 float4 operator*(const float4 &v, float s);
344 float4 operator*(float s, const float4 &v);
345 float4 operator+(const float4 &a, const float4 &b);
346 float4 operator-(const float4 &a, const float4 &b);
347 float4x4 operator*(const float4x4 &a, const float4x4 &b);
348 float4 operator*(const float4 &v, const float4x4 &m);
349 float4x4 Inverse(const float4x4 &m);
350 float4x4 MatrixRigidInverse(const float4x4 &m);
351 float4x4 MatrixTranspose(const float4x4 &m);
352 float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf);
353 float4x4 MatrixTranslation(const float3 &t);
354 float4x4 MatrixRotationZ(const float angle_radians);
355 float4x4 MatrixLookAt(const float3 &eye, const float3 &at, const float3 &up);
356 int operator==(const float4x4 &a, const float4x4 &b);
357
358 //-------- Quaternion ------------
359
360 class Quaternion : public float4
361 {
362 public:
Quaternion()363 Quaternion()
364 {
365 x = y = z = 0.0f;
366 w = 1.0f;
367 }
Quaternion(float3 v,float t)368 Quaternion(float3 v, float t)
369 {
370 v = normalize(v);
371 w = cosf(t / 2.0f);
372 v = v * sinf(t / 2.0f);
373 x = v.x;
374 y = v.y;
375 z = v.z;
376 }
Quaternion(float _x,float _y,float _z,float _w)377 Quaternion(float _x, float _y, float _z, float _w)
378 {
379 x = _x;
380 y = _y;
381 z = _z;
382 w = _w;
383 }
angle() const384 float angle() const { return acosf(w) * 2.0f; }
axis() const385 float3 axis() const
386 {
387 float3 a(x, y, z);
388 if (fabsf(angle()) < 0.0000001f) return float3(1, 0, 0);
389 return a * (1 / sinf(angle() / 2.0f));
390 }
xdir() const391 float3 xdir() const { return float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)); }
ydir() const392 float3 ydir() const { return float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)); }
zdir() const393 float3 zdir() const { return float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)); }
getmatrix() const394 float3x3 getmatrix() const { return float3x3(xdir(), ydir(), zdir()); }
operator float3x3()395 operator float3x3() { return getmatrix(); }
396 void Normalize();
397 };
398
399 Quaternion &operator*=(Quaternion &a, float s);
400 Quaternion operator*(const Quaternion &a, float s);
401 Quaternion operator*(const Quaternion &a, const Quaternion &b);
402 Quaternion operator+(const Quaternion &a, const Quaternion &b);
403 Quaternion normalize(Quaternion a);
404 float dot(const Quaternion &a, const Quaternion &b);
405 float3 operator*(const Quaternion &q, const float3 &v);
406 float3 operator*(const float3 &v, const Quaternion &q);
407 Quaternion slerp(Quaternion a, const Quaternion &b, float interp);
408 Quaternion Interpolate(const Quaternion &q0, const Quaternion &q1, float alpha);
409 Quaternion RotationArc(float3 v0, float3 v1); // returns quat q where q*v0=v1
410 Quaternion Inverse(const Quaternion &q);
411 float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v);
412
413 //------ Euler Angle -----
414
415 Quaternion YawPitchRoll(float yaw, float pitch, float roll);
416 float Yaw(const Quaternion &q);
417 float Pitch(const Quaternion &q);
418 float Roll(Quaternion q);
419 float Yaw(const float3 &v);
420 float Pitch(const float3 &v);
421
422 //------- Plane ----------
423
424 class Plane
425 {
426 public:
427 float3 normal;
428 float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
Plane(const float3 & n,float d)429 Plane(const float3 &n, float d) : normal(n), dist(d) {}
Plane()430 Plane() : normal(), dist(0) {}
431 void Transform(const float3 &position, const Quaternion &orientation);
432 };
433
PlaneFlip(const Plane & plane)434 inline Plane PlaneFlip(const Plane &plane) { return Plane(-plane.normal, -plane.dist); }
operator ==(const Plane & a,const Plane & b)435 inline int operator==(const Plane &a, const Plane &b) { return (a.normal == b.normal && a.dist == b.dist); }
coplanar(const Plane & a,const Plane & b)436 inline int coplanar(const Plane &a, const Plane &b) { return (a == b || a == PlaneFlip(b)); }
437
438 //--------- Utility Functions ------
439
440 float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1);
441 float3 PlaneProject(const Plane &plane, const float3 &point);
442 float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a); // projects a onto infinite line p0p1
443 float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a);
444 float3 ThreePlaneIntersection(const Plane &p0, const Plane &p1, const Plane &p2);
445 int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact = NULL, float3 *normal = NULL);
446 int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax);
447 int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact);
448 float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint = NULL, float3 *vpoint = NULL);
449 float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2);
450 float3 NormalOf(const float3 *vert, const int n);
451 Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1);
452
sqr(float a)453 float sqr(float a) { return a * a; }
clampf(float a)454 float clampf(float a) { return Min(1.0f, Max(0.0f, a)); }
455
Round(float a,float precision)456 float Round(float a, float precision)
457 {
458 return floorf(0.5f + a / precision) * precision;
459 }
460
Interpolate(const float & f0,const float & f1,float alpha)461 float Interpolate(const float &f0, const float &f1, float alpha)
462 {
463 return f0 * (1 - alpha) + f1 * alpha;
464 }
465
argmin(float a[],int n)466 int argmin(float a[], int n)
467 {
468 int r = 0;
469 for (int i = 1; i < n; i++)
470 {
471 if (a[i] < a[r])
472 {
473 r = i;
474 }
475 }
476 return r;
477 }
478
479 //------------ float3 (3D) --------------
480
operator +(const float3 & a,const float3 & b)481 float3 operator+(const float3 &a, const float3 &b)
482 {
483 return float3(a.x + b.x, a.y + b.y, a.z + b.z);
484 }
485
operator -(const float3 & a,const float3 & b)486 float3 operator-(const float3 &a, const float3 &b)
487 {
488 return float3(a.x - b.x, a.y - b.y, a.z - b.z);
489 }
490
operator -(const float3 & v)491 float3 operator-(const float3 &v)
492 {
493 return float3(-v.x, -v.y, -v.z);
494 }
495
operator *(const float3 & v,float s)496 float3 operator*(const float3 &v, float s)
497 {
498 return float3(v.x * s, v.y * s, v.z * s);
499 }
500
operator *(float s,const float3 & v)501 float3 operator*(float s, const float3 &v)
502 {
503 return float3(v.x * s, v.y * s, v.z * s);
504 }
505
operator /(const float3 & v,float s)506 float3 operator/(const float3 &v, float s)
507 {
508 return v * (1.0f / s);
509 }
510
dot(const float3 & a,const float3 & b)511 float dot(const float3 &a, const float3 &b)
512 {
513 return a.x * b.x + a.y * b.y + a.z * b.z;
514 }
515
cmul(const float3 & v1,const float3 & v2)516 float3 cmul(const float3 &v1, const float3 &v2)
517 {
518 return float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
519 }
520
cross(const float3 & a,const float3 & b)521 float3 cross(const float3 &a, const float3 &b)
522 {
523 return float3(a.y * b.z - a.z * b.y,
524 a.z * b.x - a.x * b.z,
525 a.x * b.y - a.y * b.x);
526 }
527
operator +=(float3 & a,const float3 & b)528 float3 &operator+=(float3 &a, const float3 &b)
529 {
530 a.x += b.x;
531 a.y += b.y;
532 a.z += b.z;
533 return a;
534 }
535
operator -=(float3 & a,const float3 & b)536 float3 &operator-=(float3 &a, const float3 &b)
537 {
538 a.x -= b.x;
539 a.y -= b.y;
540 a.z -= b.z;
541 return a;
542 }
543
operator *=(float3 & v,float s)544 float3 &operator*=(float3 &v, float s)
545 {
546 v.x *= s;
547 v.y *= s;
548 v.z *= s;
549 return v;
550 }
551
operator /=(float3 & v,float s)552 float3 &operator/=(float3 &v, float s)
553 {
554 float sinv = 1.0f / s;
555 v.x *= sinv;
556 v.y *= sinv;
557 v.z *= sinv;
558 return v;
559 }
560
vabs(const float3 & v)561 float3 vabs(const float3 &v)
562 {
563 return float3(fabsf(v.x), fabsf(v.y), fabsf(v.z));
564 }
565
magnitude(const float3 & v)566 float magnitude(const float3 &v)
567 {
568 return sqrtf(sqr(v.x) + sqr(v.y) + sqr(v.z));
569 }
570
normalize(const float3 & v)571 float3 normalize(const float3 &v)
572 {
573 // this routine, normalize, is ok, provided magnitude works!!
574 float d = magnitude(v);
575 if (d == 0)
576 {
577 printf("Cant normalize ZERO vector\n");
578 assert(0); // yes this could go here
579 d = 0.1f;
580 }
581 d = 1 / d;
582 return float3(v.x * d, v.y * d, v.z * d);
583 }
584
safenormalize(const float3 & v)585 float3 safenormalize(const float3 &v)
586 {
587 if (magnitude(v) <= 0.0f)
588 {
589 return float3(1, 0, 0);
590 }
591 return normalize(v);
592 }
593
Round(const float3 & a,float precision)594 float3 Round(const float3 &a, float precision)
595 {
596 return float3(Round(a.x, precision), Round(a.y, precision), Round(a.z, precision));
597 }
598
Interpolate(const float3 & v0,const float3 & v1,float alpha)599 float3 Interpolate(const float3 &v0, const float3 &v1, float alpha)
600 {
601 return v0 * (1 - alpha) + v1 * alpha;
602 }
603
VectorMin(const float3 & a,const float3 & b)604 float3 VectorMin(const float3 &a, const float3 &b)
605 {
606 return float3(Min(a.x, b.x), Min(a.y, b.y), Min(a.z, b.z));
607 }
VectorMax(const float3 & a,const float3 & b)608 float3 VectorMax(const float3 &a, const float3 &b)
609 {
610 return float3(Max(a.x, b.x), Max(a.y, b.y), Max(a.z, b.z));
611 }
612
613 // the statement v1*v2 is ambiguous since there are 3 types
614 // of vector multiplication
615 // - componantwise (for example combining colors)
616 // - dot product
617 // - cross product
618 // Therefore we never declare/implement this function.
619 // So we will never see: float3 operator*(float3 a,float3 b)
620
621 //------------ float3x3 ---------------
Determinant(const float3x3 & m)622 float Determinant(const float3x3 &m)
623 {
624 return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z;
625 }
626
Inverse(const float3x3 & a)627 float3x3 Inverse(const float3x3 &a)
628 {
629 float3x3 b;
630 float d = Determinant(a);
631 assert(d != 0);
632 for (int i = 0; i < 3; i++)
633 {
634 for (int j = 0; j < 3; j++)
635 {
636 int i1 = (i + 1) % 3;
637 int i2 = (i + 2) % 3;
638 int j1 = (j + 1) % 3;
639 int j2 = (j + 2) % 3;
640 // reverse indexs i&j to take transpose
641 b[j][i] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d;
642 }
643 }
644 // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
645 return b;
646 }
647
Transpose(const float3x3 & m)648 float3x3 Transpose(const float3x3 &m)
649 {
650 return float3x3(float3(m.x.x, m.y.x, m.z.x),
651 float3(m.x.y, m.y.y, m.z.y),
652 float3(m.x.z, m.y.z, m.z.z));
653 }
654
operator *(const float3 & v,const float3x3 & m)655 float3 operator*(const float3 &v, const float3x3 &m)
656 {
657 return float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z),
658 (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z),
659 (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z));
660 }
operator *(const float3x3 & m,const float3 & v)661 float3 operator*(const float3x3 &m, const float3 &v)
662 {
663 return float3(dot(m.x, v), dot(m.y, v), dot(m.z, v));
664 }
665
operator *(const float3x3 & a,const float3x3 & b)666 float3x3 operator*(const float3x3 &a, const float3x3 &b)
667 {
668 return float3x3(a.x * b, a.y * b, a.z * b);
669 }
670
operator *(const float3x3 & a,const float & s)671 float3x3 operator*(const float3x3 &a, const float &s)
672 {
673 return float3x3(a.x * s, a.y * s, a.z * s);
674 }
operator /(const float3x3 & a,const float & s)675 float3x3 operator/(const float3x3 &a, const float &s)
676 {
677 float t = 1 / s;
678 return float3x3(a.x * t, a.y * t, a.z * t);
679 }
operator +(const float3x3 & a,const float3x3 & b)680 float3x3 operator+(const float3x3 &a, const float3x3 &b)
681 {
682 return float3x3(a.x + b.x, a.y + b.y, a.z + b.z);
683 }
operator -(const float3x3 & a,const float3x3 & b)684 float3x3 operator-(const float3x3 &a, const float3x3 &b)
685 {
686 return float3x3(a.x - b.x, a.y - b.y, a.z - b.z);
687 }
operator +=(float3x3 & a,const float3x3 & b)688 float3x3 &operator+=(float3x3 &a, const float3x3 &b)
689 {
690 a.x += b.x;
691 a.y += b.y;
692 a.z += b.z;
693 return a;
694 }
operator -=(float3x3 & a,const float3x3 & b)695 float3x3 &operator-=(float3x3 &a, const float3x3 &b)
696 {
697 a.x -= b.x;
698 a.y -= b.y;
699 a.z -= b.z;
700 return a;
701 }
operator *=(float3x3 & a,const float & s)702 float3x3 &operator*=(float3x3 &a, const float &s)
703 {
704 a.x *= s;
705 a.y *= s;
706 a.z *= s;
707 return a;
708 }
709
ThreePlaneIntersection(const Plane & p0,const Plane & p1,const Plane & p2)710 float3 ThreePlaneIntersection(const Plane &p0, const Plane &p1, const Plane &p2)
711 {
712 float3x3 mp = Transpose(float3x3(p0.normal, p1.normal, p2.normal));
713 float3x3 mi = Inverse(mp);
714 float3 b(p0.dist, p1.dist, p2.dist);
715 return -b * mi;
716 }
717
718 //--------------- 4D ----------------
719
operator *(const float4 & v,const float4x4 & m)720 float4 operator*(const float4 &v, const float4x4 &m)
721 {
722 return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w; // yes this actually works
723 }
724
operator ==(const float4 & a,const float4 & b)725 int operator==(const float4 &a, const float4 &b)
726 {
727 return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
728 }
729
730 // Dont implement m*v for now, since that might confuse us
731 // All our transforms are based on multiplying the "row" vector on the left
732 //float4 operator*(const float4x4& m , const float4& v )
733 //{
734 // return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
735 //}
736
cmul(const float4 & a,const float4 & b)737 float4 cmul(const float4 &a, const float4 &b)
738 {
739 return float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
740 }
741
operator *(const float4 & v,float s)742 float4 operator*(const float4 &v, float s)
743 {
744 return float4(v.x * s, v.y * s, v.z * s, v.w * s);
745 }
746
operator *(float s,const float4 & v)747 float4 operator*(float s, const float4 &v)
748 {
749 return float4(v.x * s, v.y * s, v.z * s, v.w * s);
750 }
751
operator +(const float4 & a,const float4 & b)752 float4 operator+(const float4 &a, const float4 &b)
753 {
754 return float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
755 }
756
operator -(const float4 & a,const float4 & b)757 float4 operator-(const float4 &a, const float4 &b)
758 {
759 return float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
760 }
761
Homogenize(const float3 & v3,const float & w)762 float4 Homogenize(const float3 &v3, const float &w)
763 {
764 return float4(v3.x, v3.y, v3.z, w);
765 }
766
operator *(const float4x4 & a,const float4x4 & b)767 float4x4 operator*(const float4x4 &a, const float4x4 &b)
768 {
769 return float4x4(a.x * b, a.y * b, a.z * b, a.w * b);
770 }
771
MatrixTranspose(const float4x4 & m)772 float4x4 MatrixTranspose(const float4x4 &m)
773 {
774 return float4x4(
775 m.x.x, m.y.x, m.z.x, m.w.x,
776 m.x.y, m.y.y, m.z.y, m.w.y,
777 m.x.z, m.y.z, m.z.z, m.w.z,
778 m.x.w, m.y.w, m.z.w, m.w.w);
779 }
780
MatrixRigidInverse(const float4x4 & m)781 float4x4 MatrixRigidInverse(const float4x4 &m)
782 {
783 float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
784 float4x4 rot = m;
785 rot.w = float4(0, 0, 0, 1);
786 return trans_inverse * MatrixTranspose(rot);
787 }
788
MatrixPerspectiveFov(float fovy,float aspect,float zn,float zf)789 float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf)
790 {
791 float h = 1.0f / tanf(fovy / 2.0f); // view space height
792 float w = h / aspect; // view space width
793 return float4x4(
794 w, 0, 0, 0,
795 0, h, 0, 0,
796 0, 0, zf / (zn - zf), -1,
797 0, 0, zn * zf / (zn - zf), 0);
798 }
799
MatrixLookAt(const float3 & eye,const float3 & at,const float3 & up)800 float4x4 MatrixLookAt(const float3 &eye, const float3 &at, const float3 &up)
801 {
802 float4x4 m;
803 m.w.w = 1.0f;
804 m.w.xyz() = eye;
805 m.z.xyz() = normalize(eye - at);
806 m.x.xyz() = normalize(cross(up, m.z.xyz()));
807 m.y.xyz() = cross(m.z.xyz(), m.x.xyz());
808 return MatrixRigidInverse(m);
809 }
810
MatrixTranslation(const float3 & t)811 float4x4 MatrixTranslation(const float3 &t)
812 {
813 return float4x4(
814 1, 0, 0, 0,
815 0, 1, 0, 0,
816 0, 0, 1, 0,
817 t.x, t.y, t.z, 1);
818 }
819
MatrixRotationZ(const float angle_radians)820 float4x4 MatrixRotationZ(const float angle_radians)
821 {
822 float s = sinf(angle_radians);
823 float c = cosf(angle_radians);
824 return float4x4(
825 c, s, 0, 0,
826 -s, c, 0, 0,
827 0, 0, 1, 0,
828 0, 0, 0, 1);
829 }
830
operator ==(const float4x4 & a,const float4x4 & b)831 int operator==(const float4x4 &a, const float4x4 &b)
832 {
833 return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
834 }
835
Inverse(const float4x4 & m)836 float4x4 Inverse(const float4x4 &m)
837 {
838 float4x4 d;
839 float *dst = &d.x.x;
840 float tmp[12]; /* temp array for pairs */
841 float src[16]; /* array of transpose source matrix */
842 float det; /* determinant */
843 /* transpose matrix */
844 for (int i = 0; i < 4; i++)
845 {
846 src[i] = m(i, 0);
847 src[i + 4] = m(i, 1);
848 src[i + 8] = m(i, 2);
849 src[i + 12] = m(i, 3);
850 }
851 /* calculate pairs for first 8 elements (cofactors) */
852 tmp[0] = src[10] * src[15];
853 tmp[1] = src[11] * src[14];
854 tmp[2] = src[9] * src[15];
855 tmp[3] = src[11] * src[13];
856 tmp[4] = src[9] * src[14];
857 tmp[5] = src[10] * src[13];
858 tmp[6] = src[8] * src[15];
859 tmp[7] = src[11] * src[12];
860 tmp[8] = src[8] * src[14];
861 tmp[9] = src[10] * src[12];
862 tmp[10] = src[8] * src[13];
863 tmp[11] = src[9] * src[12];
864 /* calculate first 8 elements (cofactors) */
865 dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
866 dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
867 dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
868 dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
869 dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
870 dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
871 dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
872 dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
873 dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
874 dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
875 dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
876 dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
877 dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
878 dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
879 dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
880 dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];
881 /* calculate pairs for second 8 elements (cofactors) */
882 tmp[0] = src[2] * src[7];
883 tmp[1] = src[3] * src[6];
884 tmp[2] = src[1] * src[7];
885 tmp[3] = src[3] * src[5];
886 tmp[4] = src[1] * src[6];
887 tmp[5] = src[2] * src[5];
888 tmp[6] = src[0] * src[7];
889 tmp[7] = src[3] * src[4];
890 tmp[8] = src[0] * src[6];
891 tmp[9] = src[2] * src[4];
892 tmp[10] = src[0] * src[5];
893 tmp[11] = src[1] * src[4];
894 /* calculate second 8 elements (cofactors) */
895 dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
896 dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
897 dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
898 dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
899 dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
900 dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
901 dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
902 dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
903 dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
904 dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
905 dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
906 dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
907 dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
908 dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
909 dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
910 dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];
911 /* calculate determinant */
912 det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
913 /* calculate matrix inverse */
914 det = 1 / det;
915 for (int j = 0; j < 16; j++)
916 dst[j] *= det;
917 return d;
918 }
919
920 //--------- Quaternion --------------
921
operator *(const Quaternion & a,const Quaternion & b)922 Quaternion operator*(const Quaternion &a, const Quaternion &b)
923 {
924 Quaternion c;
925 c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
926 c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
927 c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
928 c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
929 return c;
930 }
931
operator *(const Quaternion & a,float b)932 Quaternion operator*(const Quaternion &a, float b)
933 {
934 return Quaternion(a.x * b, a.y * b, a.z * b, a.w * b);
935 }
936
Inverse(const Quaternion & q)937 Quaternion Inverse(const Quaternion &q)
938 {
939 return Quaternion(-q.x, -q.y, -q.z, q.w);
940 }
941
operator *=(Quaternion & q,const float s)942 Quaternion &operator*=(Quaternion &q, const float s)
943 {
944 q.x *= s;
945 q.y *= s;
946 q.z *= s;
947 q.w *= s;
948 return q;
949 }
Normalize()950 void Quaternion::Normalize()
951 {
952 float m = sqrtf(sqr(w) + sqr(x) + sqr(y) + sqr(z));
953 if (m < 0.000000001f)
954 {
955 w = 1.0f;
956 x = y = z = 0.0f;
957 return;
958 }
959 (*this) *= (1.0f / m);
960 }
961
operator *(const Quaternion & q,const float3 & v)962 float3 operator*(const Quaternion &q, const float3 &v)
963 {
964 // The following is equivalent to:
965 //return (q.getmatrix() * v);
966 float qx2 = q.x * q.x;
967 float qy2 = q.y * q.y;
968 float qz2 = q.z * q.z;
969
970 float qxqy = q.x * q.y;
971 float qxqz = q.x * q.z;
972 float qxqw = q.x * q.w;
973 float qyqz = q.y * q.z;
974 float qyqw = q.y * q.w;
975 float qzqw = q.z * q.w;
976 return float3(
977 (1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z,
978 (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z,
979 (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z);
980 }
981
operator *(const float3 & v,const Quaternion & q)982 float3 operator*(const float3 &v, const Quaternion &q)
983 {
984 assert(0); // must multiply with the quat on the left
985 return float3(0.0f, 0.0f, 0.0f);
986 }
987
operator +(const Quaternion & a,const Quaternion & b)988 Quaternion operator+(const Quaternion &a, const Quaternion &b)
989 {
990 return Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
991 }
992
dot(const Quaternion & a,const Quaternion & b)993 float dot(const Quaternion &a, const Quaternion &b)
994 {
995 return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
996 }
997
normalize(Quaternion a)998 Quaternion normalize(Quaternion a)
999 {
1000 float m = sqrtf(sqr(a.w) + sqr(a.x) + sqr(a.y) + sqr(a.z));
1001 if (m < 0.000000001)
1002 {
1003 a.w = 1;
1004 a.x = a.y = a.z = 0;
1005 return a;
1006 }
1007 return a * (1 / m);
1008 }
1009
slerp(Quaternion a,const Quaternion & b,float interp)1010 Quaternion slerp(Quaternion a, const Quaternion &b, float interp)
1011 {
1012 if (dot(a, b) < 0.0)
1013 {
1014 a.w = -a.w;
1015 a.x = -a.x;
1016 a.y = -a.y;
1017 a.z = -a.z;
1018 }
1019 float d = dot(a, b);
1020 if (d >= 1.0)
1021 {
1022 return a;
1023 }
1024 float theta = acosf(d);
1025 if (theta == 0.0f)
1026 {
1027 return (a);
1028 }
1029 return a * (sinf(theta - interp * theta) / sinf(theta)) + b * (sinf(interp * theta) / sinf(theta));
1030 }
1031
Interpolate(const Quaternion & q0,const Quaternion & q1,float alpha)1032 Quaternion Interpolate(const Quaternion &q0, const Quaternion &q1, float alpha)
1033 {
1034 return slerp(q0, q1, alpha);
1035 }
1036
YawPitchRoll(float yaw,float pitch,float roll)1037 Quaternion YawPitchRoll(float yaw, float pitch, float roll)
1038 {
1039 roll *= DEG2RAD;
1040 yaw *= DEG2RAD;
1041 pitch *= DEG2RAD;
1042 return Quaternion(float3(0.0f, 0.0f, 1.0f), yaw) * Quaternion(float3(1.0f, 0.0f, 0.0f), pitch) * Quaternion(float3(0.0f, 1.0f, 0.0f), roll);
1043 }
1044
Yaw(const Quaternion & q)1045 float Yaw(const Quaternion &q)
1046 {
1047 float3 v;
1048 v = q.ydir();
1049 return (v.y == 0.0 && v.x == 0.0) ? 0.0f : atan2f(-v.x, v.y) * RAD2DEG;
1050 }
1051
Pitch(const Quaternion & q)1052 float Pitch(const Quaternion &q)
1053 {
1054 float3 v;
1055 v = q.ydir();
1056 return atan2f(v.z, sqrtf(sqr(v.x) + sqr(v.y))) * RAD2DEG;
1057 }
1058
Roll(Quaternion q)1059 float Roll(Quaternion q)
1060 {
1061 q = Quaternion(float3(0.0f, 0.0f, 1.0f), -Yaw(q) * DEG2RAD) * q;
1062 q = Quaternion(float3(1.0f, 0.0f, 0.0f), -Pitch(q) * DEG2RAD) * q;
1063 return atan2f(-q.xdir().z, q.xdir().x) * RAD2DEG;
1064 }
1065
Yaw(const float3 & v)1066 float Yaw(const float3 &v)
1067 {
1068 return (v.y == 0.0 && v.x == 0.0) ? 0.0f : atan2f(-v.x, v.y) * RAD2DEG;
1069 }
1070
Pitch(const float3 & v)1071 float Pitch(const float3 &v)
1072 {
1073 return atan2f(v.z, sqrtf(sqr(v.x) + sqr(v.y))) * RAD2DEG;
1074 }
1075
1076 //------------- Plane --------------
1077
Transform(const float3 & position,const Quaternion & orientation)1078 void Plane::Transform(const float3 &position, const Quaternion &orientation)
1079 {
1080 // Transforms the plane to the space defined by the
1081 // given position/orientation.
1082 float3 newnormal;
1083 float3 origin;
1084
1085 newnormal = Inverse(orientation) * normal;
1086 origin = Inverse(orientation) * (-normal * dist - position);
1087
1088 normal = newnormal;
1089 dist = -dot(newnormal, origin);
1090 }
1091
1092 //--------- utility functions -------------
1093
1094 // RotationArc()
1095 // Given two vectors v0 and v1 this function
1096 // returns quaternion q where q*v0==v1.
1097 // Routine taken from game programming gems.
RotationArc(float3 v0,float3 v1)1098 Quaternion RotationArc(float3 v0, float3 v1)
1099 {
1100 Quaternion q;
1101 v0 = normalize(v0); // Comment these two lines out if you know its not needed.
1102 v1 = normalize(v1); // If vector is already unit length then why do it again?
1103 float3 c = cross(v0, v1);
1104 float d = dot(v0, v1);
1105 if (d <= -1.0f)
1106 {
1107 return Quaternion(1, 0, 0, 0);
1108 } // 180 about x axis
1109 float s = sqrtf((1 + d) * 2);
1110 q.x = c.x / s;
1111 q.y = c.y / s;
1112 q.z = c.z / s;
1113 q.w = s / 2.0f;
1114 return q;
1115 }
1116
MatrixFromQuatVec(const Quaternion & q,const float3 & v)1117 float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
1118 {
1119 // builds a 4x4 transformation matrix based on orientation q and translation v
1120 float qx2 = q.x * q.x;
1121 float qy2 = q.y * q.y;
1122 float qz2 = q.z * q.z;
1123
1124 float qxqy = q.x * q.y;
1125 float qxqz = q.x * q.z;
1126 float qxqw = q.x * q.w;
1127 float qyqz = q.y * q.z;
1128 float qyqw = q.y * q.w;
1129 float qzqw = q.z * q.w;
1130
1131 return float4x4(
1132 1 - 2 * (qy2 + qz2),
1133 2 * (qxqy + qzqw),
1134 2 * (qxqz - qyqw),
1135 0,
1136 2 * (qxqy - qzqw),
1137 1 - 2 * (qx2 + qz2),
1138 2 * (qyqz + qxqw),
1139 0,
1140 2 * (qxqz + qyqw),
1141 2 * (qyqz - qxqw),
1142 1 - 2 * (qx2 + qy2),
1143 0,
1144 v.x,
1145 v.y,
1146 v.z,
1147 1.0f);
1148 }
1149
PlaneLineIntersection(const Plane & plane,const float3 & p0,const float3 & p1)1150 float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
1151 {
1152 // returns the point where the line p0-p1 intersects the plane n&d
1153 float3 dif;
1154 dif = p1 - p0;
1155 float dn = dot(plane.normal, dif);
1156 float t = -(plane.dist + dot(plane.normal, p0)) / dn;
1157 return p0 + (dif * t);
1158 }
1159
PlaneProject(const Plane & plane,const float3 & point)1160 float3 PlaneProject(const Plane &plane, const float3 &point)
1161 {
1162 return point - plane.normal * (dot(point, plane.normal) + plane.dist);
1163 }
1164
LineProject(const float3 & p0,const float3 & p1,const float3 & a)1165 float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a)
1166 {
1167 float3 w;
1168 w = p1 - p0;
1169 float t = dot(w, (a - p0)) / (sqr(w.x) + sqr(w.y) + sqr(w.z));
1170 return p0 + w * t;
1171 }
1172
LineProjectTime(const float3 & p0,const float3 & p1,const float3 & a)1173 float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a)
1174 {
1175 float3 w;
1176 w = p1 - p0;
1177 float t = dot(w, (a - p0)) / (sqr(w.x) + sqr(w.y) + sqr(w.z));
1178 return t;
1179 }
1180
TriNormal(const float3 & v0,const float3 & v1,const float3 & v2)1181 float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2)
1182 {
1183 // return the normal of the triangle
1184 // inscribed by v0, v1, and v2
1185 float3 cp = cross(v1 - v0, v2 - v1);
1186 float m = magnitude(cp);
1187 if (m == 0) return float3(1, 0, 0);
1188 return cp * (1.0f / m);
1189 }
1190
BoxInside(const float3 & p,const float3 & bmin,const float3 & bmax)1191 int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax)
1192 {
1193 return (p.x >= bmin.x && p.x <= bmax.x &&
1194 p.y >= bmin.y && p.y <= bmax.y &&
1195 p.z >= bmin.z && p.z <= bmax.z);
1196 }
1197
BoxIntersect(const float3 & v0,const float3 & v1,const float3 & bmin,const float3 & bmax,float3 * impact)1198 int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact)
1199 {
1200 if (BoxInside(v0, bmin, bmax))
1201 {
1202 *impact = v0;
1203 return 1;
1204 }
1205 if (v0.x <= bmin.x && v1.x >= bmin.x)
1206 {
1207 float a = (bmin.x - v0.x) / (v1.x - v0.x);
1208 //v.x = bmin.x;
1209 float vy = (1 - a) * v0.y + a * v1.y;
1210 float vz = (1 - a) * v0.z + a * v1.z;
1211 if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
1212 {
1213 impact->x = bmin.x;
1214 impact->y = vy;
1215 impact->z = vz;
1216 return 1;
1217 }
1218 }
1219 else if (v0.x >= bmax.x && v1.x <= bmax.x)
1220 {
1221 float a = (bmax.x - v0.x) / (v1.x - v0.x);
1222 //v.x = bmax.x;
1223 float vy = (1 - a) * v0.y + a * v1.y;
1224 float vz = (1 - a) * v0.z + a * v1.z;
1225 if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
1226 {
1227 impact->x = bmax.x;
1228 impact->y = vy;
1229 impact->z = vz;
1230 return 1;
1231 }
1232 }
1233 if (v0.y <= bmin.y && v1.y >= bmin.y)
1234 {
1235 float a = (bmin.y - v0.y) / (v1.y - v0.y);
1236 float vx = (1 - a) * v0.x + a * v1.x;
1237 //v.y = bmin.y;
1238 float vz = (1 - a) * v0.z + a * v1.z;
1239 if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
1240 {
1241 impact->x = vx;
1242 impact->y = bmin.y;
1243 impact->z = vz;
1244 return 1;
1245 }
1246 }
1247 else if (v0.y >= bmax.y && v1.y <= bmax.y)
1248 {
1249 float a = (bmax.y - v0.y) / (v1.y - v0.y);
1250 float vx = (1 - a) * v0.x + a * v1.x;
1251 // vy = bmax.y;
1252 float vz = (1 - a) * v0.z + a * v1.z;
1253 if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
1254 {
1255 impact->x = vx;
1256 impact->y = bmax.y;
1257 impact->z = vz;
1258 return 1;
1259 }
1260 }
1261 if (v0.z <= bmin.z && v1.z >= bmin.z)
1262 {
1263 float a = (bmin.z - v0.z) / (v1.z - v0.z);
1264 float vx = (1 - a) * v0.x + a * v1.x;
1265 float vy = (1 - a) * v0.y + a * v1.y;
1266 // v.z = bmin.z;
1267 if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
1268 {
1269 impact->x = vx;
1270 impact->y = vy;
1271 impact->z = bmin.z;
1272 return 1;
1273 }
1274 }
1275 else if (v0.z >= bmax.z && v1.z <= bmax.z)
1276 {
1277 float a = (bmax.z - v0.z) / (v1.z - v0.z);
1278 float vx = (1 - a) * v0.x + a * v1.x;
1279 float vy = (1 - a) * v0.y + a * v1.y;
1280 // v.z = bmax.z;
1281 if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
1282 {
1283 impact->x = vx;
1284 impact->y = vy;
1285 impact->z = bmax.z;
1286 return 1;
1287 }
1288 }
1289 return 0;
1290 }
1291
DistanceBetweenLines(const float3 & ustart,const float3 & udir,const float3 & vstart,const float3 & vdir,float3 * upoint,float3 * vpoint)1292 float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
1293 {
1294 float3 cp;
1295 cp = normalize(cross(udir, vdir));
1296
1297 float distu = -dot(cp, ustart);
1298 float distv = -dot(cp, vstart);
1299 float dist = (float)fabs(distu - distv);
1300 if (upoint)
1301 {
1302 Plane plane;
1303 plane.normal = normalize(cross(vdir, cp));
1304 plane.dist = -dot(plane.normal, vstart);
1305 *upoint = PlaneLineIntersection(plane, ustart, ustart + udir);
1306 }
1307 if (vpoint)
1308 {
1309 Plane plane;
1310 plane.normal = normalize(cross(udir, cp));
1311 plane.dist = -dot(plane.normal, ustart);
1312 *vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir);
1313 }
1314 return dist;
1315 }
1316
VirtualTrackBall(const float3 & cop,const float3 & cor,const float3 & dir1,const float3 & dir2)1317 Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2)
1318 {
1319 // routine taken from game programming gems.
1320 // Implement track ball functionality to spin stuf on the screen
1321 // cop center of projection
1322 // cor center of rotation
1323 // dir1 old mouse direction
1324 // dir2 new mouse direction
1325 // pretend there is a sphere around cor. Then find the points
1326 // where dir1 and dir2 intersect that sphere. Find the
1327 // rotation that takes the first point to the second.
1328 float m;
1329 // compute plane
1330 float3 nrml = cor - cop;
1331 float fudgefactor = 1.0f / (magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop
1332 nrml = normalize(nrml);
1333 float dist = -dot(nrml, cor);
1334 float3 u = PlaneLineIntersection(Plane(nrml, dist), cop, cop + dir1);
1335 u = u - cor;
1336 u = u * fudgefactor;
1337 m = magnitude(u);
1338 if (m > 1)
1339 {
1340 u /= m;
1341 }
1342 else
1343 {
1344 u = u - (nrml * sqrtf(1 - m * m));
1345 }
1346 float3 v = PlaneLineIntersection(Plane(nrml, dist), cop, cop + dir2);
1347 v = v - cor;
1348 v = v * fudgefactor;
1349 m = magnitude(v);
1350 if (m > 1)
1351 {
1352 v /= m;
1353 }
1354 else
1355 {
1356 v = v - (nrml * sqrtf(1 - m * m));
1357 }
1358 return RotationArc(u, v);
1359 }
1360
1361 int countpolyhit = 0;
PolyHit(const float3 * vert,const int n,const float3 & v0,const float3 & v1,float3 * impact,float3 * normal)1362 int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal)
1363 {
1364 countpolyhit++;
1365 int i;
1366 float3 nrml(0, 0, 0);
1367 for (i = 0; i < n; i++)
1368 {
1369 int i1 = (i + 1) % n;
1370 int i2 = (i + 2) % n;
1371 nrml = nrml + cross(vert[i1] - vert[i], vert[i2] - vert[i1]);
1372 }
1373
1374 float m = magnitude(nrml);
1375 if (m == 0.0)
1376 {
1377 return 0;
1378 }
1379 nrml = nrml * (1.0f / m);
1380 float dist = -dot(nrml, vert[0]);
1381 float d0, d1;
1382 if ((d0 = dot(v0, nrml) + dist) < 0 || (d1 = dot(v1, nrml) + dist) > 0)
1383 {
1384 return 0;
1385 }
1386
1387 float3 the_point;
1388 // By using the cached plane distances d0 and d1
1389 // we can optimize the following:
1390 // the_point = planelineintersection(nrml,dist,v0,v1);
1391 float a = d0 / (d0 - d1);
1392 the_point = v0 * (1 - a) + v1 * a;
1393
1394 int inside = 1;
1395 for (int j = 0; inside && j < n; j++)
1396 {
1397 // let inside = 0 if outside
1398 float3 pp1, pp2, side;
1399 pp1 = vert[j];
1400 pp2 = vert[(j + 1) % n];
1401 side = cross((pp2 - pp1), (the_point - pp1));
1402 inside = (dot(nrml, side) >= 0.0);
1403 }
1404 if (inside)
1405 {
1406 if (normal)
1407 {
1408 *normal = nrml;
1409 }
1410 if (impact)
1411 {
1412 *impact = the_point;
1413 }
1414 }
1415 return inside;
1416 }
1417
1418 //**************************************************************************
1419 //**************************************************************************
1420 //*** Stan Melax's array template, needed to compile his hull generation code
1421 //**************************************************************************
1422 //**************************************************************************
1423
1424 template <class Type>
1425 class ArrayRet;
1426 template <class Type>
1427 class Array
1428 {
1429 public:
1430 Array(int s = 0);
1431 Array(Array<Type> &array);
1432 Array(ArrayRet<Type> &array);
1433 ~Array();
1434 void allocate(int s);
1435 void SetSize(int s);
1436 void Pack();
1437 Type &Add(Type);
1438 void AddUnique(Type);
1439 int Contains(Type);
1440 void Insert(Type, int);
1441 int IndexOf(Type);
1442 void Remove(Type);
1443 void DelIndex(int i);
1444 Type *element;
1445 int count;
1446 int array_size;
operator [](int i) const1447 const Type &operator[](int i) const
1448 {
1449 assert(i >= 0 && i < count);
1450 return element[i];
1451 }
operator [](int i)1452 Type &operator[](int i)
1453 {
1454 assert(i >= 0 && i < count);
1455 return element[i];
1456 }
Pop()1457 Type &Pop()
1458 {
1459 assert(count);
1460 count--;
1461 return element[count];
1462 }
1463 Array<Type> &operator=(Array<Type> &array);
1464 Array<Type> &operator=(ArrayRet<Type> &array);
1465 // operator ArrayRet<Type> &() { return *(ArrayRet<Type> *)this;} // this worked but i suspect could be dangerous
1466 };
1467
1468 template <class Type>
1469 class ArrayRet : public Array<Type>
1470 {
1471 };
1472
1473 template <class Type>
Array(int s)1474 Array<Type>::Array(int s)
1475 {
1476 count = 0;
1477 array_size = 0;
1478 element = NULL;
1479 if (s)
1480 {
1481 allocate(s);
1482 }
1483 }
1484
1485 template <class Type>
Array(Array<Type> & array)1486 Array<Type>::Array(Array<Type> &array)
1487 {
1488 count = 0;
1489 array_size = 0;
1490 element = NULL;
1491 for (int i = 0; i < array.count; i++)
1492 {
1493 Add(array[i]);
1494 }
1495 }
1496
1497 template <class Type>
Array(ArrayRet<Type> & array)1498 Array<Type>::Array(ArrayRet<Type> &array)
1499 {
1500 *this = array;
1501 }
1502 template <class Type>
operator =(ArrayRet<Type> & array)1503 Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array)
1504 {
1505 count = array.count;
1506 array_size = array.array_size;
1507 element = array.element;
1508 array.element = NULL;
1509 array.count = 0;
1510 array.array_size = 0;
1511 return *this;
1512 }
1513
1514 template <class Type>
operator =(Array<Type> & array)1515 Array<Type> &Array<Type>::operator=(Array<Type> &array)
1516 {
1517 count = 0;
1518 for (int i = 0; i < array.count; i++)
1519 {
1520 Add(array[i]);
1521 }
1522 return *this;
1523 }
1524
1525 template <class Type>
~Array()1526 Array<Type>::~Array()
1527 {
1528 if (element != NULL)
1529 {
1530 free(element);
1531 }
1532 count = 0;
1533 array_size = 0;
1534 element = NULL;
1535 }
1536
1537 template <class Type>
allocate(int s)1538 void Array<Type>::allocate(int s)
1539 {
1540 assert(s > 0);
1541 assert(s >= count);
1542 Type *old = element;
1543 array_size = s;
1544 element = (Type *)malloc(sizeof(Type) * array_size);
1545 assert(element);
1546 for (int i = 0; i < count; i++)
1547 {
1548 element[i] = old[i];
1549 }
1550 if (old)
1551 {
1552 free(old);
1553 }
1554 }
1555
1556 template <class Type>
SetSize(int s)1557 void Array<Type>::SetSize(int s)
1558 {
1559 if (s == 0)
1560 {
1561 if (element)
1562 {
1563 free(element);
1564 element = NULL;
1565 }
1566 array_size = s;
1567 }
1568 else
1569 {
1570 allocate(s);
1571 }
1572 count = s;
1573 }
1574
1575 template <class Type>
Pack()1576 void Array<Type>::Pack()
1577 {
1578 allocate(count);
1579 }
1580
1581 template <class Type>
Add(Type t)1582 Type &Array<Type>::Add(Type t)
1583 {
1584 assert(count <= array_size);
1585 if (count == array_size)
1586 {
1587 allocate((array_size) ? array_size * 2 : 16);
1588 }
1589 element[count++] = t;
1590 return element[count - 1];
1591 }
1592
1593 template <class Type>
Contains(Type t)1594 int Array<Type>::Contains(Type t)
1595 {
1596 int i;
1597 int found = 0;
1598 for (i = 0; i < count; i++)
1599 {
1600 if (element[i] == t) found++;
1601 }
1602 return found;
1603 }
1604
1605 template <class Type>
AddUnique(Type t)1606 void Array<Type>::AddUnique(Type t)
1607 {
1608 if (!Contains(t)) Add(t);
1609 }
1610
1611 template <class Type>
DelIndex(int i)1612 void Array<Type>::DelIndex(int i)
1613 {
1614 assert(i < count);
1615 count--;
1616 while (i < count)
1617 {
1618 element[i] = element[i + 1];
1619 i++;
1620 }
1621 }
1622
1623 template <class Type>
Remove(Type t)1624 void Array<Type>::Remove(Type t)
1625 {
1626 int i;
1627 for (i = 0; i < count; i++)
1628 {
1629 if (element[i] == t)
1630 {
1631 break;
1632 }
1633 }
1634 assert(i < count); // assert object t is in the array.
1635 DelIndex(i);
1636 for (i = 0; i < count; i++)
1637 {
1638 assert(element[i] != t);
1639 }
1640 }
1641
1642 template <class Type>
Insert(Type t,int k)1643 void Array<Type>::Insert(Type t, int k)
1644 {
1645 int i = count;
1646 Add(t); // to allocate space
1647 while (i > k)
1648 {
1649 element[i] = element[i - 1];
1650 i--;
1651 }
1652 assert(i == k);
1653 element[k] = t;
1654 }
1655
1656 template <class Type>
IndexOf(Type t)1657 int Array<Type>::IndexOf(Type t)
1658 {
1659 int i;
1660 for (i = 0; i < count; i++)
1661 {
1662 if (element[i] == t)
1663 {
1664 return i;
1665 }
1666 }
1667 assert(0);
1668 return -1;
1669 }
1670
1671 //*********************************************************************
1672 //*********************************************************************
1673 //******** Hull header
1674 //*********************************************************************
1675 //*********************************************************************
1676
1677 class PHullResult
1678 {
1679 public:
PHullResult(void)1680 PHullResult(void)
1681 {
1682 mVcount = 0;
1683 mIndexCount = 0;
1684 mFaceCount = 0;
1685 mVertices = 0;
1686 mIndices = 0;
1687 }
1688
1689 unsigned int mVcount;
1690 unsigned int mIndexCount;
1691 unsigned int mFaceCount;
1692 float *mVertices;
1693 unsigned int *mIndices;
1694 };
1695
1696 #define REAL3 float3
1697 #define REAL float
1698
1699 #define COPLANAR (0)
1700 #define UNDER (1)
1701 #define OVER (2)
1702 #define SPLIT (OVER | UNDER)
1703 #define PAPERWIDTH (0.001f)
1704
1705 float planetestepsilon = PAPERWIDTH;
1706
1707 class ConvexH
1708 {
1709 public:
1710 class HalfEdge
1711 {
1712 public:
1713 short ea; // the other half of the edge (index into edges list)
1714 unsigned char v; // the vertex at the start of this edge (index into vertices list)
1715 unsigned char p; // the facet on which this edge lies (index into facets list)
HalfEdge()1716 HalfEdge() {}
HalfEdge(short _ea,unsigned char _v,unsigned char _p)1717 HalfEdge(short _ea, unsigned char _v, unsigned char _p) : ea(_ea), v(_v), p(_p) {}
1718 };
1719 Array<REAL3> vertices;
1720 Array<HalfEdge> edges;
1721 Array<Plane> facets;
1722 ConvexH(int vertices_size, int edges_size, int facets_size);
1723 };
1724
1725 typedef ConvexH::HalfEdge HalfEdge;
1726
ConvexH(int vertices_size,int edges_size,int facets_size)1727 ConvexH::ConvexH(int vertices_size, int edges_size, int facets_size)
1728 : vertices(vertices_size), edges(edges_size), facets(facets_size)
1729 {
1730 vertices.count = vertices_size;
1731 edges.count = edges_size;
1732 facets.count = facets_size;
1733 }
1734
ConvexHDup(ConvexH * src)1735 ConvexH *ConvexHDup(ConvexH *src)
1736 {
1737 ConvexH *dst = new ConvexH(src->vertices.count, src->edges.count, src->facets.count);
1738 memcpy(dst->vertices.element, src->vertices.element, sizeof(float3) * src->vertices.count);
1739 memcpy(dst->edges.element, src->edges.element, sizeof(HalfEdge) * src->edges.count);
1740 memcpy(dst->facets.element, src->facets.element, sizeof(Plane) * src->facets.count);
1741 return dst;
1742 }
1743
PlaneTest(const Plane & p,const REAL3 & v)1744 int PlaneTest(const Plane &p, const REAL3 &v)
1745 {
1746 REAL a = dot(v, p.normal) + p.dist;
1747 int flag = (a > planetestepsilon) ? OVER : ((a < -planetestepsilon) ? UNDER : COPLANAR);
1748 return flag;
1749 }
1750
SplitTest(ConvexH & convex,const Plane & plane)1751 int SplitTest(ConvexH &convex, const Plane &plane)
1752 {
1753 int flag = 0;
1754 for (int i = 0; i < convex.vertices.count; i++)
1755 {
1756 flag |= PlaneTest(plane, convex.vertices[i]);
1757 }
1758 return flag;
1759 }
1760
1761 class VertFlag
1762 {
1763 public:
1764 unsigned char planetest;
1765 unsigned char junk;
1766 unsigned char undermap;
1767 unsigned char overmap;
1768 };
1769 class EdgeFlag
1770 {
1771 public:
1772 unsigned char planetest;
1773 unsigned char fixes;
1774 short undermap;
1775 short overmap;
1776 };
1777 class PlaneFlag
1778 {
1779 public:
1780 unsigned char undermap;
1781 unsigned char overmap;
1782 };
1783 class Coplanar
1784 {
1785 public:
1786 unsigned short ea;
1787 unsigned char v0;
1788 unsigned char v1;
1789 };
1790
AssertIntact(ConvexH & convex)1791 int AssertIntact(ConvexH &convex)
1792 {
1793 int i;
1794 int estart = 0;
1795 for (i = 0; i < convex.edges.count; i++)
1796 {
1797 if (convex.edges[estart].p != convex.edges[i].p)
1798 {
1799 estart = i;
1800 }
1801 int inext = i + 1;
1802 if (inext >= convex.edges.count || convex.edges[inext].p != convex.edges[i].p)
1803 {
1804 inext = estart;
1805 }
1806 assert(convex.edges[inext].p == convex.edges[i].p);
1807 int nb = convex.edges[i].ea;
1808 assert(nb != 255);
1809 if (nb == 255 || nb == -1) return 0;
1810 assert(nb != -1);
1811 assert(i == convex.edges[nb].ea);
1812 }
1813 for (i = 0; i < convex.edges.count; i++)
1814 {
1815 assert(COPLANAR == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v]));
1816 if (COPLANAR != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v])) return 0;
1817 if (convex.edges[estart].p != convex.edges[i].p)
1818 {
1819 estart = i;
1820 }
1821 int i1 = i + 1;
1822 if (i1 >= convex.edges.count || convex.edges[i1].p != convex.edges[i].p)
1823 {
1824 i1 = estart;
1825 }
1826 int i2 = i1 + 1;
1827 if (i2 >= convex.edges.count || convex.edges[i2].p != convex.edges[i].p)
1828 {
1829 i2 = estart;
1830 }
1831 if (i == i2) continue; // i sliced tangent to an edge and created 2 meaningless edges
1832 REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i].v],
1833 convex.vertices[convex.edges[i1].v],
1834 convex.vertices[convex.edges[i2].v]);
1835 assert(dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0);
1836 if (dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0) return 0;
1837 }
1838 return 1;
1839 }
1840
1841 // back to back quads
test_btbq()1842 ConvexH *test_btbq()
1843 {
1844 ConvexH *convex = new ConvexH(4, 8, 2);
1845 convex->vertices[0] = REAL3(0, 0, 0);
1846 convex->vertices[1] = REAL3(1, 0, 0);
1847 convex->vertices[2] = REAL3(1, 1, 0);
1848 convex->vertices[3] = REAL3(0, 1, 0);
1849 convex->facets[0] = Plane(REAL3(0, 0, 1), 0);
1850 convex->facets[1] = Plane(REAL3(0, 0, -1), 0);
1851 convex->edges[0] = HalfEdge(7, 0, 0);
1852 convex->edges[1] = HalfEdge(6, 1, 0);
1853 convex->edges[2] = HalfEdge(5, 2, 0);
1854 convex->edges[3] = HalfEdge(4, 3, 0);
1855
1856 convex->edges[4] = HalfEdge(3, 0, 1);
1857 convex->edges[5] = HalfEdge(2, 3, 1);
1858 convex->edges[6] = HalfEdge(1, 2, 1);
1859 convex->edges[7] = HalfEdge(0, 1, 1);
1860 AssertIntact(*convex);
1861 return convex;
1862 }
test_cube()1863 ConvexH *test_cube()
1864 {
1865 ConvexH *convex = new ConvexH(8, 24, 6);
1866 convex->vertices[0] = REAL3(0, 0, 0);
1867 convex->vertices[1] = REAL3(0, 0, 1);
1868 convex->vertices[2] = REAL3(0, 1, 0);
1869 convex->vertices[3] = REAL3(0, 1, 1);
1870 convex->vertices[4] = REAL3(1, 0, 0);
1871 convex->vertices[5] = REAL3(1, 0, 1);
1872 convex->vertices[6] = REAL3(1, 1, 0);
1873 convex->vertices[7] = REAL3(1, 1, 1);
1874
1875 convex->facets[0] = Plane(REAL3(-1, 0, 0), 0);
1876 convex->facets[1] = Plane(REAL3(1, 0, 0), -1);
1877 convex->facets[2] = Plane(REAL3(0, -1, 0), 0);
1878 convex->facets[3] = Plane(REAL3(0, 1, 0), -1);
1879 convex->facets[4] = Plane(REAL3(0, 0, -1), 0);
1880 convex->facets[5] = Plane(REAL3(0, 0, 1), -1);
1881
1882 convex->edges[0] = HalfEdge(11, 0, 0);
1883 convex->edges[1] = HalfEdge(23, 1, 0);
1884 convex->edges[2] = HalfEdge(15, 3, 0);
1885 convex->edges[3] = HalfEdge(16, 2, 0);
1886
1887 convex->edges[4] = HalfEdge(13, 6, 1);
1888 convex->edges[5] = HalfEdge(21, 7, 1);
1889 convex->edges[6] = HalfEdge(9, 5, 1);
1890 convex->edges[7] = HalfEdge(18, 4, 1);
1891
1892 convex->edges[8] = HalfEdge(19, 0, 2);
1893 convex->edges[9] = HalfEdge(6, 4, 2);
1894 convex->edges[10] = HalfEdge(20, 5, 2);
1895 convex->edges[11] = HalfEdge(0, 1, 2);
1896
1897 convex->edges[12] = HalfEdge(22, 3, 3);
1898 convex->edges[13] = HalfEdge(4, 7, 3);
1899 convex->edges[14] = HalfEdge(17, 6, 3);
1900 convex->edges[15] = HalfEdge(2, 2, 3);
1901
1902 convex->edges[16] = HalfEdge(3, 0, 4);
1903 convex->edges[17] = HalfEdge(14, 2, 4);
1904 convex->edges[18] = HalfEdge(7, 6, 4);
1905 convex->edges[19] = HalfEdge(8, 4, 4);
1906
1907 convex->edges[20] = HalfEdge(10, 1, 5);
1908 convex->edges[21] = HalfEdge(5, 5, 5);
1909 convex->edges[22] = HalfEdge(12, 7, 5);
1910 convex->edges[23] = HalfEdge(1, 3, 5);
1911
1912 return convex;
1913 }
ConvexHMakeCube(const REAL3 & bmin,const REAL3 & bmax)1914 ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax)
1915 {
1916 ConvexH *convex = test_cube();
1917 convex->vertices[0] = REAL3(bmin.x, bmin.y, bmin.z);
1918 convex->vertices[1] = REAL3(bmin.x, bmin.y, bmax.z);
1919 convex->vertices[2] = REAL3(bmin.x, bmax.y, bmin.z);
1920 convex->vertices[3] = REAL3(bmin.x, bmax.y, bmax.z);
1921 convex->vertices[4] = REAL3(bmax.x, bmin.y, bmin.z);
1922 convex->vertices[5] = REAL3(bmax.x, bmin.y, bmax.z);
1923 convex->vertices[6] = REAL3(bmax.x, bmax.y, bmin.z);
1924 convex->vertices[7] = REAL3(bmax.x, bmax.y, bmax.z);
1925
1926 convex->facets[0] = Plane(REAL3(-1, 0, 0), bmin.x);
1927 convex->facets[1] = Plane(REAL3(1, 0, 0), -bmax.x);
1928 convex->facets[2] = Plane(REAL3(0, -1, 0), bmin.y);
1929 convex->facets[3] = Plane(REAL3(0, 1, 0), -bmax.y);
1930 convex->facets[4] = Plane(REAL3(0, 0, -1), bmin.z);
1931 convex->facets[5] = Plane(REAL3(0, 0, 1), -bmax.z);
1932 return convex;
1933 }
ConvexHCrop(ConvexH & convex,const Plane & slice)1934 ConvexH *ConvexHCrop(ConvexH &convex, const Plane &slice)
1935 {
1936 int i;
1937 int vertcountunder = 0;
1938 int vertcountover = 0;
1939 Array<int> vertscoplanar; // existing vertex members of convex that are coplanar
1940 vertscoplanar.count = 0;
1941 Array<int> edgesplit; // existing edges that members of convex that cross the splitplane
1942 edgesplit.count = 0;
1943
1944 assert(convex.edges.count < 480);
1945
1946 EdgeFlag edgeflag[512];
1947 VertFlag vertflag[256];
1948 PlaneFlag planeflag[128];
1949 HalfEdge tmpunderedges[512];
1950 Plane tmpunderplanes[128];
1951 Coplanar coplanaredges[512];
1952 int coplanaredges_num = 0;
1953
1954 Array<REAL3> createdverts;
1955 // do the side-of-plane tests
1956 for (i = 0; i < convex.vertices.count; i++)
1957 {
1958 vertflag[i].planetest = PlaneTest(slice, convex.vertices[i]);
1959 if (vertflag[i].planetest == COPLANAR)
1960 {
1961 // ? vertscoplanar.Add(i);
1962 vertflag[i].undermap = vertcountunder++;
1963 vertflag[i].overmap = vertcountover++;
1964 }
1965 else if (vertflag[i].planetest == UNDER)
1966 {
1967 vertflag[i].undermap = vertcountunder++;
1968 }
1969 else
1970 {
1971 assert(vertflag[i].planetest == OVER);
1972 vertflag[i].overmap = vertcountover++;
1973 vertflag[i].undermap = 255; // for debugging purposes
1974 }
1975 }
1976 int vertcountunderold = vertcountunder; // for debugging only
1977
1978 int under_edge_count = 0;
1979 int underplanescount = 0;
1980 int e0 = 0;
1981
1982 for (int currentplane = 0; currentplane < convex.facets.count; currentplane++)
1983 {
1984 int estart = e0;
1985 int enextface = 0;
1986 int planeside = 0;
1987 int e1 = e0 + 1;
1988 int vout = -1;
1989 int vin = -1;
1990 int coplanaredge = -1;
1991 do
1992 {
1993 if (e1 >= convex.edges.count || convex.edges[e1].p != currentplane)
1994 {
1995 enextface = e1;
1996 e1 = estart;
1997 }
1998 HalfEdge &edge0 = convex.edges[e0];
1999 HalfEdge &edge1 = convex.edges[e1];
2000 HalfEdge &edgea = convex.edges[edge0.ea];
2001
2002 planeside |= vertflag[edge0.v].planetest;
2003 //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) {
2004 // assert(ecop==-1);
2005 // ecop=e;
2006 //}
2007
2008 if (vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER)
2009 {
2010 // both endpoints over plane
2011 edgeflag[e0].undermap = -1;
2012 }
2013 else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER)
2014 {
2015 // at least one endpoint under, the other coplanar or under
2016
2017 edgeflag[e0].undermap = under_edge_count;
2018 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
2019 tmpunderedges[under_edge_count].p = underplanescount;
2020 if (edge0.ea < e0)
2021 {
2022 // connect the neighbors
2023 assert(edgeflag[edge0.ea].undermap != -1);
2024 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
2025 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
2026 }
2027 under_edge_count++;
2028 }
2029 else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == COPLANAR)
2030 {
2031 // both endpoints coplanar
2032 // must check a 3rd point to see if UNDER
2033 int e2 = e1 + 1;
2034 if (e2 >= convex.edges.count || convex.edges[e2].p != currentplane)
2035 {
2036 e2 = estart;
2037 }
2038 assert(convex.edges[e2].p == currentplane);
2039 HalfEdge &edge2 = convex.edges[e2];
2040 if (vertflag[edge2.v].planetest == UNDER)
2041 {
2042 edgeflag[e0].undermap = under_edge_count;
2043 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
2044 tmpunderedges[under_edge_count].p = underplanescount;
2045 tmpunderedges[under_edge_count].ea = -1;
2046 // make sure this edge is added to the "coplanar" list
2047 coplanaredge = under_edge_count;
2048 vout = vertflag[edge0.v].undermap;
2049 vin = vertflag[edge1.v].undermap;
2050 under_edge_count++;
2051 }
2052 else
2053 {
2054 edgeflag[e0].undermap = -1;
2055 }
2056 }
2057 else if (vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER)
2058 {
2059 // first is under 2nd is over
2060
2061 edgeflag[e0].undermap = under_edge_count;
2062 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
2063 tmpunderedges[under_edge_count].p = underplanescount;
2064 if (edge0.ea < e0)
2065 {
2066 assert(edgeflag[edge0.ea].undermap != -1);
2067 // connect the neighbors
2068 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
2069 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
2070 vout = tmpunderedges[edgeflag[edge0.ea].undermap].v;
2071 }
2072 else
2073 {
2074 Plane &p0 = convex.facets[edge0.p];
2075 Plane &pa = convex.facets[edgea.p];
2076 createdverts.Add(ThreePlaneIntersection(p0, pa, slice));
2077 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
2078 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
2079 vout = vertcountunder++;
2080 }
2081 under_edge_count++;
2082 /// hmmm something to think about: i might be able to output this edge regarless of
2083 // wheter or not we know v-in yet. ok i;ll try this now:
2084 tmpunderedges[under_edge_count].v = vout;
2085 tmpunderedges[under_edge_count].p = underplanescount;
2086 tmpunderedges[under_edge_count].ea = -1;
2087 coplanaredge = under_edge_count;
2088 under_edge_count++;
2089
2090 if (vin != -1)
2091 {
2092 // we previously processed an edge where we came under
2093 // now we know about vout as well
2094
2095 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2096 }
2097 }
2098 else if (vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER)
2099 {
2100 // first is coplanar 2nd is over
2101
2102 edgeflag[e0].undermap = -1;
2103 vout = vertflag[edge0.v].undermap;
2104 // I hate this but i have to make sure part of this face is UNDER before ouputting this vert
2105 int k = estart;
2106 assert(edge0.p == currentplane);
2107 while (!(planeside & UNDER) && k < convex.edges.count && convex.edges[k].p == edge0.p)
2108 {
2109 planeside |= vertflag[convex.edges[k].v].planetest;
2110 k++;
2111 }
2112 if (planeside & UNDER)
2113 {
2114 tmpunderedges[under_edge_count].v = vout;
2115 tmpunderedges[under_edge_count].p = underplanescount;
2116 tmpunderedges[under_edge_count].ea = -1;
2117 coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on
2118 under_edge_count++;
2119 }
2120 }
2121 else if (vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER)
2122 {
2123 // first is over next is under
2124 // new vertex!!!
2125 assert(vin == -1);
2126 if (e0 < edge0.ea)
2127 {
2128 Plane &p0 = convex.facets[edge0.p];
2129 Plane &pa = convex.facets[edgea.p];
2130 createdverts.Add(ThreePlaneIntersection(p0, pa, slice));
2131 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
2132 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
2133 vin = vertcountunder++;
2134 }
2135 else
2136 {
2137 // find the new vertex that was created by edge[edge0.ea]
2138 int nea = edgeflag[edge0.ea].undermap;
2139 assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p);
2140 vin = tmpunderedges[nea + 1].v;
2141 assert(vin < vertcountunder);
2142 assert(vin >= vertcountunderold); // for debugging only
2143 }
2144 if (vout != -1)
2145 {
2146 // we previously processed an edge where we went over
2147 // now we know vin too
2148 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2149 }
2150 // output edge
2151 tmpunderedges[under_edge_count].v = vin;
2152 tmpunderedges[under_edge_count].p = underplanescount;
2153 edgeflag[e0].undermap = under_edge_count;
2154 if (e0 > edge0.ea)
2155 {
2156 assert(edgeflag[edge0.ea].undermap != -1);
2157 // connect the neighbors
2158 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
2159 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
2160 }
2161 assert(edgeflag[e0].undermap == under_edge_count);
2162 under_edge_count++;
2163 }
2164 else if (vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR)
2165 {
2166 // first is over next is coplanar
2167
2168 edgeflag[e0].undermap = -1;
2169 vin = vertflag[edge1.v].undermap;
2170 assert(vin != -1);
2171 if (vout != -1)
2172 {
2173 // we previously processed an edge where we came under
2174 // now we know both endpoints
2175 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2176 }
2177 }
2178 else
2179 {
2180 assert(0);
2181 }
2182
2183 e0 = e1;
2184 e1++; // do the modulo at the beginning of the loop
2185
2186 } while (e0 != estart);
2187 e0 = enextface;
2188 if (planeside & UNDER)
2189 {
2190 planeflag[currentplane].undermap = underplanescount;
2191 tmpunderplanes[underplanescount] = convex.facets[currentplane];
2192 underplanescount++;
2193 }
2194 else
2195 {
2196 planeflag[currentplane].undermap = 0;
2197 }
2198 if (vout >= 0 && (planeside & UNDER))
2199 {
2200 assert(vin >= 0);
2201 assert(coplanaredge >= 0);
2202 assert(coplanaredge != 511);
2203 coplanaredges[coplanaredges_num].ea = coplanaredge;
2204 coplanaredges[coplanaredges_num].v0 = vin;
2205 coplanaredges[coplanaredges_num].v1 = vout;
2206 coplanaredges_num++;
2207 }
2208 }
2209
2210 // add the new plane to the mix:
2211 if (coplanaredges_num > 0)
2212 {
2213 tmpunderplanes[underplanescount++] = slice;
2214 }
2215 for (i = 0; i < coplanaredges_num - 1; i++)
2216 {
2217 if (coplanaredges[i].v1 != coplanaredges[i + 1].v0)
2218 {
2219 int j = 0;
2220 for (j = i + 2; j < coplanaredges_num; j++)
2221 {
2222 if (coplanaredges[i].v1 == coplanaredges[j].v0)
2223 {
2224 Coplanar tmp = coplanaredges[i + 1];
2225 coplanaredges[i + 1] = coplanaredges[j];
2226 coplanaredges[j] = tmp;
2227 break;
2228 }
2229 }
2230 if (j >= coplanaredges_num)
2231 {
2232 assert(j < coplanaredges_num);
2233 return NULL;
2234 }
2235 }
2236 }
2237 ConvexH *punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount);
2238 ConvexH &under = *punder;
2239 int k = 0;
2240 for (i = 0; i < convex.vertices.count; i++)
2241 {
2242 if (vertflag[i].planetest != OVER)
2243 {
2244 under.vertices[k++] = convex.vertices[i];
2245 }
2246 }
2247 i = 0;
2248 while (k < vertcountunder)
2249 {
2250 under.vertices[k++] = createdverts[i++];
2251 }
2252 assert(i == createdverts.count);
2253
2254 for (i = 0; i < coplanaredges_num; i++)
2255 {
2256 under.edges[under_edge_count + i].p = underplanescount - 1;
2257 under.edges[under_edge_count + i].ea = coplanaredges[i].ea;
2258 tmpunderedges[coplanaredges[i].ea].ea = under_edge_count + i;
2259 under.edges[under_edge_count + i].v = coplanaredges[i].v0;
2260 }
2261
2262 memcpy(under.edges.element, tmpunderedges, sizeof(HalfEdge) * under_edge_count);
2263 memcpy(under.facets.element, tmpunderplanes, sizeof(Plane) * underplanescount);
2264 return punder;
2265 }
2266
candidateplane(Plane * planes,int planes_count,ConvexH * convex,float epsilon)2267 static int candidateplane(Plane *planes, int planes_count, ConvexH *convex, float epsilon)
2268 {
2269 int p = 0;
2270 REAL md = 0;
2271 int i;
2272 for (i = 0; i < planes_count; i++)
2273 {
2274 REAL d = 0;
2275 for (int j = 0; j < convex->vertices.count; j++)
2276 {
2277 d = Max(d, dot(convex->vertices[j], planes[i].normal) + planes[i].dist);
2278 }
2279 if (i == 0 || d > md)
2280 {
2281 p = i;
2282 md = d;
2283 }
2284 }
2285 return (md > epsilon) ? p : -1;
2286 }
2287
2288 template <class T>
maxdir(const T * p,int count,const T & dir)2289 inline int maxdir(const T *p, int count, const T &dir)
2290 {
2291 assert(count);
2292 int m = 0;
2293 float currDotm = dot(p[0], dir);
2294 for (int i = 1; i < count; i++)
2295 {
2296 const float currDoti = dot(p[i], dir);
2297 if (currDoti > currDotm)
2298 {
2299 currDotm = currDoti;
2300 m = i;
2301 }
2302 }
2303 return m;
2304 }
2305
2306 template <class T>
maxdirfiltered(const T * p,int count,const T & dir,Array<int> & allow)2307 int maxdirfiltered(const T *p, int count, const T &dir, Array<int> &allow)
2308 {
2309 assert(count);
2310 int m = -1;
2311 float currDotm = dot(p[0], dir);
2312 for (int i = 0; i < count; i++)
2313 {
2314 if (allow[i])
2315 {
2316 if (m == -1)
2317 {
2318 currDotm = dot(p[i], dir);
2319 m = i;
2320 }
2321 else
2322 {
2323 const float currDoti = dot(p[i], dir);
2324 if (currDoti > currDotm)
2325 {
2326 currDotm = currDoti;
2327 m = i;
2328 }
2329 }
2330 }
2331 }
2332 assert(m != -1);
2333 return m;
2334 }
2335
orth(const float3 & v)2336 float3 orth(const float3 &v)
2337 {
2338 float3 a = cross(v, float3(0, 0, 1));
2339 float3 b = cross(v, float3(0, 1, 0));
2340 return normalize((magnitude(a) > magnitude(b)) ? a : b);
2341 }
2342
2343 template <class T>
maxdirsterid(const T * p,int count,const T & dir,Array<int> & allow)2344 int maxdirsterid(const T *p, int count, const T &dir, Array<int> &allow)
2345 {
2346 int m = -1;
2347 while (m == -1)
2348 {
2349 m = maxdirfiltered(p, count, dir, allow);
2350 if (allow[m] == 3) return m;
2351 T u = orth(dir);
2352 T v = cross(u, dir);
2353 int ma = -1;
2354 for (float x = 0.0f; x <= 360.0f; x += 45.0f)
2355 {
2356 float s = sinf(DEG2RAD * (x));
2357 float c = cosf(DEG2RAD * (x));
2358 int mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
2359 if (ma == m && mb == m)
2360 {
2361 allow[m] = 3;
2362 return m;
2363 }
2364 if (ma != -1 && ma != mb) // Yuck - this is really ugly
2365 {
2366 int mc = ma;
2367 for (float xx = x - 40.0f; xx <= x; xx += 5.0f)
2368 {
2369 float s = sinf(DEG2RAD * (xx));
2370 float c = cosf(DEG2RAD * (xx));
2371 int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
2372 if (mc == m && md == m)
2373 {
2374 allow[m] = 3;
2375 return m;
2376 }
2377 mc = md;
2378 }
2379 }
2380 ma = mb;
2381 }
2382 allow[m] = 0;
2383 m = -1;
2384 }
2385 assert(0);
2386 return m;
2387 }
2388
operator ==(const int3 & a,const int3 & b)2389 int operator==(const int3 &a, const int3 &b)
2390 {
2391 for (int i = 0; i < 3; i++)
2392 {
2393 if (a[i] != b[i]) return 0;
2394 }
2395 return 1;
2396 }
2397
roll3(int3 a)2398 int3 roll3(int3 a)
2399 {
2400 int tmp = a[0];
2401 a[0] = a[1];
2402 a[1] = a[2];
2403 a[2] = tmp;
2404 return a;
2405 }
isa(const int3 & a,const int3 & b)2406 int isa(const int3 &a, const int3 &b)
2407 {
2408 return (a == b || roll3(a) == b || a == roll3(b));
2409 }
b2b(const int3 & a,const int3 & b)2410 int b2b(const int3 &a, const int3 &b)
2411 {
2412 return isa(a, int3(b[2], b[1], b[0]));
2413 }
above(float3 * vertices,const int3 & t,const float3 & p,float epsilon)2414 int above(float3 *vertices, const int3 &t, const float3 &p, float epsilon)
2415 {
2416 float3 n = TriNormal(vertices[t[0]], vertices[t[1]], vertices[t[2]]);
2417 return (dot(n, p - vertices[t[0]]) > epsilon); // EPSILON???
2418 }
hasedge(const int3 & t,int a,int b)2419 int hasedge(const int3 &t, int a, int b)
2420 {
2421 for (int i = 0; i < 3; i++)
2422 {
2423 int i1 = (i + 1) % 3;
2424 if (t[i] == a && t[i1] == b) return 1;
2425 }
2426 return 0;
2427 }
hasvert(const int3 & t,int v)2428 int hasvert(const int3 &t, int v)
2429 {
2430 return (t[0] == v || t[1] == v || t[2] == v);
2431 }
shareedge(const int3 & a,const int3 & b)2432 int shareedge(const int3 &a, const int3 &b)
2433 {
2434 int i;
2435 for (i = 0; i < 3; i++)
2436 {
2437 int i1 = (i + 1) % 3;
2438 if (hasedge(a, b[i1], b[i])) return 1;
2439 }
2440 return 0;
2441 }
2442
2443 class btHullTriangle;
2444
2445 //Array<btHullTriangle*> tris;
2446
2447 class btHullTriangle : public int3
2448 {
2449 public:
2450 int3 n;
2451 int id;
2452 int vmax;
2453 float rise;
2454 Array<btHullTriangle *> *tris;
btHullTriangle(int a,int b,int c,Array<btHullTriangle * > * pTris)2455 btHullTriangle(int a, int b, int c, Array<btHullTriangle *> *pTris) : int3(a, b, c), n(-1, -1, -1)
2456 {
2457 tris = pTris;
2458 id = tris->count;
2459 tris->Add(this);
2460 vmax = -1;
2461 rise = 0.0f;
2462 }
~btHullTriangle()2463 ~btHullTriangle()
2464 {
2465 assert((*tris)[id] == this);
2466 (*tris)[id] = NULL;
2467 }
2468 int &neib(int a, int b);
2469 };
2470
neib(int a,int b)2471 int &btHullTriangle::neib(int a, int b)
2472 {
2473 static int er = -1;
2474 int i;
2475 for (i = 0; i < 3; i++)
2476 {
2477 int i1 = (i + 1) % 3;
2478 int i2 = (i + 2) % 3;
2479 if ((*this)[i] == a && (*this)[i1] == b) return n[i2];
2480 if ((*this)[i] == b && (*this)[i1] == a) return n[i2];
2481 }
2482 assert(0);
2483 return er;
2484 }
b2bfix(btHullTriangle * s,btHullTriangle * t,Array<btHullTriangle * > & tris)2485 void b2bfix(btHullTriangle *s, btHullTriangle *t, Array<btHullTriangle *> &tris)
2486 {
2487 int i;
2488 for (i = 0; i < 3; i++)
2489 {
2490 int i1 = (i + 1) % 3;
2491 int i2 = (i + 2) % 3;
2492 int a = (*s)[i1];
2493 int b = (*s)[i2];
2494 assert(tris[s->neib(a, b)]->neib(b, a) == s->id);
2495 assert(tris[t->neib(a, b)]->neib(b, a) == t->id);
2496 tris[s->neib(a, b)]->neib(b, a) = t->neib(b, a);
2497 tris[t->neib(b, a)]->neib(a, b) = s->neib(a, b);
2498 }
2499 }
2500
removeb2b(btHullTriangle * s,btHullTriangle * t,Array<btHullTriangle * > & tris)2501 void removeb2b(btHullTriangle *s, btHullTriangle *t, Array<btHullTriangle *> &tris)
2502 {
2503 b2bfix(s, t, tris);
2504 delete s;
2505 delete t;
2506 }
2507
checkit(btHullTriangle * t,Array<btHullTriangle * > & tris)2508 void checkit(btHullTriangle *t, Array<btHullTriangle *> &tris)
2509 {
2510 int i;
2511 assert(tris[t->id] == t);
2512 for (i = 0; i < 3; i++)
2513 {
2514 int i1 = (i + 1) % 3;
2515 int i2 = (i + 2) % 3;
2516 int a = (*t)[i1];
2517 int b = (*t)[i2];
2518 assert(a != b);
2519 assert(tris[t->n[i]]->neib(b, a) == t->id);
2520 }
2521 }
extrude(btHullTriangle * t0,int v,Array<btHullTriangle * > & tris)2522 void extrude(btHullTriangle *t0, int v, Array<btHullTriangle *> &tris)
2523 {
2524 int3 t = *t0;
2525 int n = tris.count;
2526 btHullTriangle *ta = new btHullTriangle(v, t[1], t[2], &tris);
2527 ta->n = int3(t0->n[0], n + 1, n + 2);
2528 tris[t0->n[0]]->neib(t[1], t[2]) = n + 0;
2529 btHullTriangle *tb = new btHullTriangle(v, t[2], t[0], &tris);
2530 tb->n = int3(t0->n[1], n + 2, n + 0);
2531 tris[t0->n[1]]->neib(t[2], t[0]) = n + 1;
2532 btHullTriangle *tc = new btHullTriangle(v, t[0], t[1], &tris);
2533 tc->n = int3(t0->n[2], n + 0, n + 1);
2534 tris[t0->n[2]]->neib(t[0], t[1]) = n + 2;
2535 checkit(ta, tris);
2536 checkit(tb, tris);
2537 checkit(tc, tris);
2538 if (hasvert(*tris[ta->n[0]], v)) removeb2b(ta, tris[ta->n[0]], tris);
2539 if (hasvert(*tris[tb->n[0]], v)) removeb2b(tb, tris[tb->n[0]], tris);
2540 if (hasvert(*tris[tc->n[0]], v)) removeb2b(tc, tris[tc->n[0]], tris);
2541 delete t0;
2542 }
2543
extrudable(float epsilon,Array<btHullTriangle * > & tris)2544 btHullTriangle *extrudable(float epsilon, Array<btHullTriangle *> &tris)
2545 {
2546 int i;
2547 btHullTriangle *t = NULL;
2548 for (i = 0; i < tris.count; i++)
2549 {
2550 if (!t || (tris[i] && t->rise < tris[i]->rise))
2551 {
2552 t = tris[i];
2553 }
2554 }
2555 return (t->rise > epsilon) ? t : NULL;
2556 }
2557
2558 class int4
2559 {
2560 public:
2561 int x, y, z, w;
int4()2562 int4(){};
int4(int _x,int _y,int _z,int _w)2563 int4(int _x, int _y, int _z, int _w)
2564 {
2565 x = _x;
2566 y = _y;
2567 z = _z;
2568 w = _w;
2569 }
operator [](int i) const2570 const int &operator[](int i) const { return (&x)[i]; }
operator [](int i)2571 int &operator[](int i) { return (&x)[i]; }
2572 };
2573
FindSimplex(float3 * verts,int verts_count,Array<int> & allow)2574 int4 FindSimplex(float3 *verts, int verts_count, Array<int> &allow)
2575 {
2576 float3 basis[3];
2577 basis[0] = float3(0.01f, 0.02f, 1.0f);
2578 int p0 = maxdirsterid(verts, verts_count, basis[0], allow);
2579 int p1 = maxdirsterid(verts, verts_count, -basis[0], allow);
2580 basis[0] = verts[p0] - verts[p1];
2581 if (p0 == p1 || basis[0] == float3(0, 0, 0))
2582 return int4(-1, -1, -1, -1);
2583 basis[1] = cross(float3(1, 0.02f, 0), basis[0]);
2584 basis[2] = cross(float3(-0.02f, 1, 0), basis[0]);
2585 basis[1] = normalize((magnitude(basis[1]) > magnitude(basis[2])) ? basis[1] : basis[2]);
2586 int p2 = maxdirsterid(verts, verts_count, basis[1], allow);
2587 if (p2 == p0 || p2 == p1)
2588 {
2589 p2 = maxdirsterid(verts, verts_count, -basis[1], allow);
2590 }
2591 if (p2 == p0 || p2 == p1)
2592 return int4(-1, -1, -1, -1);
2593 basis[1] = verts[p2] - verts[p0];
2594 basis[2] = normalize(cross(basis[1], basis[0]));
2595 int p3 = maxdirsterid(verts, verts_count, basis[2], allow);
2596 if (p3 == p0 || p3 == p1 || p3 == p2) p3 = maxdirsterid(verts, verts_count, -basis[2], allow);
2597 if (p3 == p0 || p3 == p1 || p3 == p2)
2598 return int4(-1, -1, -1, -1);
2599 assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3));
2600 if (dot(verts[p3] - verts[p0], cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0)
2601 {
2602 Swap(p2, p3);
2603 }
2604 return int4(p0, p1, p2, p3);
2605 }
2606
calchullgen(float3 * verts,int verts_count,int vlimit,Array<btHullTriangle * > & tris)2607 int calchullgen(float3 *verts, int verts_count, int vlimit, Array<btHullTriangle *> &tris)
2608 {
2609 if (verts_count < 4) return 0;
2610 if (vlimit == 0) vlimit = 1000000000;
2611 int j;
2612 float3 bmin(*verts), bmax(*verts);
2613 Array<int> isextreme(verts_count);
2614 Array<int> allow(verts_count);
2615 for (j = 0; j < verts_count; j++)
2616 {
2617 allow.Add(1);
2618 isextreme.Add(0);
2619 bmin = VectorMin(bmin, verts[j]);
2620 bmax = VectorMax(bmax, verts[j]);
2621 }
2622 float epsilon = magnitude(bmax - bmin) * 0.001f;
2623
2624 int4 p = FindSimplex(verts, verts_count, allow);
2625 if (p.x == -1) return 0; // simplex failed
2626
2627 float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point
2628 btHullTriangle *t0 = new btHullTriangle(p[2], p[3], p[1], &tris);
2629 t0->n = int3(2, 3, 1);
2630 btHullTriangle *t1 = new btHullTriangle(p[3], p[2], p[0], &tris);
2631 t1->n = int3(3, 2, 0);
2632 btHullTriangle *t2 = new btHullTriangle(p[0], p[1], p[3], &tris);
2633 t2->n = int3(0, 1, 3);
2634 btHullTriangle *t3 = new btHullTriangle(p[1], p[0], p[2], &tris);
2635 t3->n = int3(1, 0, 2);
2636 isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1;
2637 checkit(t0, tris);
2638 checkit(t1, tris);
2639 checkit(t2, tris);
2640 checkit(t3, tris);
2641
2642 for (j = 0; j < tris.count; j++)
2643 {
2644 btHullTriangle *t = tris[j];
2645 assert(t);
2646 assert(t->vmax < 0);
2647 float3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
2648 t->vmax = maxdirsterid(verts, verts_count, n, allow);
2649 t->rise = dot(n, verts[t->vmax] - verts[(*t)[0]]);
2650 }
2651 btHullTriangle *te;
2652 vlimit -= 4;
2653 while (vlimit > 0 && (te = extrudable(epsilon, tris)))
2654 {
2655 // int3 ti=*te;
2656 int v = te->vmax;
2657 assert(!isextreme[v]); // wtf we've already done this vertex
2658 isextreme[v] = 1;
2659 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
2660 j = tris.count;
2661 while (j--)
2662 {
2663 if (!tris[j]) continue;
2664 int3 t = *tris[j];
2665 if (above(verts, t, verts[v], 0.01f * epsilon))
2666 {
2667 extrude(tris[j], v, tris);
2668 }
2669 }
2670 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
2671 j = tris.count;
2672 while (j--)
2673 {
2674 if (!tris[j]) continue;
2675 if (!hasvert(*tris[j], v)) break;
2676 int3 nt = *tris[j];
2677 if (above(verts, nt, center, 0.01f * epsilon) || magnitude(cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f)
2678 {
2679 btHullTriangle *nb = tris[tris[j]->n[0]];
2680 assert(nb);
2681 assert(!hasvert(*nb, v));
2682 assert(nb->id < j);
2683 extrude(nb, v, tris);
2684 j = tris.count;
2685 }
2686 }
2687 j = tris.count;
2688 while (j--)
2689 {
2690 btHullTriangle *t = tris[j];
2691 if (!t) continue;
2692 if (t->vmax >= 0) break;
2693 float3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
2694 t->vmax = maxdirsterid(verts, verts_count, n, allow);
2695 if (isextreme[t->vmax])
2696 {
2697 t->vmax = -1; // already done that vertex - algorithm needs to be able to terminate.
2698 }
2699 else
2700 {
2701 t->rise = dot(n, verts[t->vmax] - verts[(*t)[0]]);
2702 }
2703 }
2704 vlimit--;
2705 }
2706 return 1;
2707 }
2708
calchull(float3 * verts,int verts_count,int * & tris_out,int & tris_count,int vlimit,Array<btHullTriangle * > & tris)2709 int calchull(float3 *verts, int verts_count, int *&tris_out, int &tris_count, int vlimit, Array<btHullTriangle *> &tris)
2710 {
2711 int rc = calchullgen(verts, verts_count, vlimit, tris);
2712 if (!rc) return 0;
2713 Array<int> ts;
2714 for (int i = 0; i < tris.count; i++)
2715 if (tris[i])
2716 {
2717 for (int j = 0; j < 3; j++) ts.Add((*tris[i])[j]);
2718 delete tris[i];
2719 }
2720 tris_count = ts.count / 3;
2721 tris_out = ts.element;
2722 ts.element = NULL;
2723 ts.count = ts.array_size = 0;
2724 tris.count = 0;
2725 return 1;
2726 }
2727
calchullpbev(float3 * verts,int verts_count,int vlimit,Array<Plane> & planes,float bevangle,Array<btHullTriangle * > & tris)2728 int calchullpbev(float3 *verts, int verts_count, int vlimit, Array<Plane> &planes, float bevangle, Array<btHullTriangle *> &tris)
2729 {
2730 int i, j;
2731 planes.count = 0;
2732 int rc = calchullgen(verts, verts_count, vlimit, tris);
2733 if (!rc) return 0;
2734 for (i = 0; i < tris.count; i++)
2735 if (tris[i])
2736 {
2737 Plane p;
2738 btHullTriangle *t = tris[i];
2739 p.normal = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
2740 p.dist = -dot(p.normal, verts[(*t)[0]]);
2741 planes.Add(p);
2742 for (j = 0; j < 3; j++)
2743 {
2744 if (t->n[j] < t->id) continue;
2745 btHullTriangle *s = tris[t->n[j]];
2746 REAL3 snormal = TriNormal(verts[(*s)[0]], verts[(*s)[1]], verts[(*s)[2]]);
2747 if (dot(snormal, p.normal) >= cos(bevangle * DEG2RAD)) continue;
2748 REAL3 n = normalize(snormal + p.normal);
2749 planes.Add(Plane(n, -dot(n, verts[maxdir(verts, verts_count, n)])));
2750 }
2751 }
2752
2753 for (i = 0; i < tris.count; i++)
2754 if (tris[i])
2755 {
2756 delete tris[i]; // delete tris[i];
2757 }
2758 tris.count = 0;
2759 return 1;
2760 }
2761
overhull(Plane * planes,int planes_count,float3 * verts,int verts_count,int maxplanes,float3 * & verts_out,int & verts_count_out,int * & faces_out,int & faces_count_out,float inflate)2762 int overhull(Plane *planes, int planes_count, float3 *verts, int verts_count, int maxplanes,
2763 float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out, float inflate)
2764 {
2765 int i, j;
2766 if (verts_count < 4) return 0;
2767 maxplanes = Min(maxplanes, planes_count);
2768 float3 bmin(verts[0]), bmax(verts[0]);
2769 for (i = 0; i < verts_count; i++)
2770 {
2771 bmin = VectorMin(bmin, verts[i]);
2772 bmax = VectorMax(bmax, verts[i]);
2773 }
2774 // float diameter = magnitude(bmax-bmin);
2775 // inflate *=diameter; // RELATIVE INFLATION
2776 bmin -= float3(inflate, inflate, inflate);
2777 bmax += float3(inflate, inflate, inflate);
2778 for (i = 0; i < planes_count; i++)
2779 {
2780 planes[i].dist -= inflate;
2781 }
2782 float3 emin = bmin; // VectorMin(bmin,float3(0,0,0));
2783 float3 emax = bmax; // VectorMax(bmax,float3(0,0,0));
2784 float epsilon = magnitude(emax - emin) * 0.025f;
2785 planetestepsilon = magnitude(emax - emin) * PAPERWIDTH;
2786 // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think.
2787 // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter));
2788 ConvexH *c = ConvexHMakeCube(REAL3(bmin), REAL3(bmax));
2789 int k;
2790 while (maxplanes-- && (k = candidateplane(planes, planes_count, c, epsilon)) >= 0)
2791 {
2792 ConvexH *tmp = c;
2793 c = ConvexHCrop(*tmp, planes[k]);
2794 if (c == NULL)
2795 {
2796 c = tmp;
2797 break;
2798 } // might want to debug this case better!!!
2799 if (!AssertIntact(*c))
2800 {
2801 c = tmp;
2802 break;
2803 } // might want to debug this case better too!!!
2804 delete tmp;
2805 }
2806
2807 assert(AssertIntact(*c));
2808 //return c;
2809 faces_out = (int *)malloc(sizeof(int) * (1 + c->facets.count + c->edges.count)); // new int[1+c->facets.count+c->edges.count];
2810 faces_count_out = 0;
2811 i = 0;
2812 faces_out[faces_count_out++] = -1;
2813 k = 0;
2814 while (i < c->edges.count)
2815 {
2816 j = 1;
2817 while (j + i < c->edges.count && c->edges[i].p == c->edges[i + j].p)
2818 {
2819 j++;
2820 }
2821 faces_out[faces_count_out++] = j;
2822 while (j--)
2823 {
2824 faces_out[faces_count_out++] = c->edges[i].v;
2825 i++;
2826 }
2827 k++;
2828 }
2829 faces_out[0] = k; // number of faces.
2830 assert(k == c->facets.count);
2831 assert(faces_count_out == 1 + c->facets.count + c->edges.count);
2832 verts_out = c->vertices.element; // new float3[c->vertices.count];
2833 verts_count_out = c->vertices.count;
2834 for (i = 0; i < c->vertices.count; i++)
2835 {
2836 verts_out[i] = float3(c->vertices[i]);
2837 }
2838 c->vertices.count = c->vertices.array_size = 0;
2839 c->vertices.element = NULL;
2840 delete c;
2841 return 1;
2842 }
2843
overhullv(float3 * verts,int verts_count,int maxplanes,float3 * & verts_out,int & verts_count_out,int * & faces_out,int & faces_count_out,float inflate,float bevangle,int vlimit,Array<btHullTriangle * > & tris)2844 int overhullv(float3 *verts, int verts_count, int maxplanes,
2845 float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out, float inflate, float bevangle, int vlimit, Array<btHullTriangle *> &tris)
2846 {
2847 if (!verts_count) return 0;
2848 extern int calchullpbev(float3 * verts, int verts_count, int vlimit, Array<Plane> &planes, float bevangle, Array<btHullTriangle *> &tris);
2849 Array<Plane> planes;
2850 int rc = calchullpbev(verts, verts_count, vlimit, planes, bevangle, tris);
2851 if (!rc) return 0;
2852 return overhull(planes.element, planes.count, verts, verts_count, maxplanes, verts_out, verts_count_out, faces_out, faces_count_out, inflate);
2853 }
2854
ComputeHull(unsigned int vcount,const float * vertices,PHullResult & result,unsigned int vlimit,float inflate,Array<btHullTriangle * > & arrtris)2855 bool ComputeHull(unsigned int vcount, const float *vertices, PHullResult &result, unsigned int vlimit, float inflate, Array<btHullTriangle *> &arrtris)
2856 {
2857 int index_count;
2858 int *faces;
2859 float3 *verts_out;
2860 int verts_count_out;
2861
2862 if (inflate == 0.0f)
2863 {
2864 int *tris_out;
2865 int tris_count;
2866 int ret = calchull((float3 *)vertices, (int)vcount, tris_out, tris_count, vlimit, arrtris);
2867 if (!ret) return false;
2868 result.mIndexCount = (unsigned int)(tris_count * 3);
2869 result.mFaceCount = (unsigned int)tris_count;
2870 result.mVertices = (float *)vertices;
2871 result.mVcount = (unsigned int)vcount;
2872 result.mIndices = (unsigned int *)tris_out;
2873 return true;
2874 }
2875
2876 int ret = overhullv((float3 *)vertices, vcount, 35, verts_out, verts_count_out, faces, index_count, inflate, 120.0f, vlimit, arrtris);
2877 if (!ret) return false;
2878
2879 Array<int3> tris;
2880 int n = faces[0];
2881 int k = 1;
2882 for (int i = 0; i < n; i++)
2883 {
2884 int pn = faces[k++];
2885 for (int j = 2; j < pn; j++) tris.Add(int3(faces[k], faces[k + j - 1], faces[k + j]));
2886 k += pn;
2887 }
2888 assert(tris.count == index_count - 1 - (n * 3));
2889
2890 result.mIndexCount = (unsigned int)(tris.count * 3);
2891 result.mFaceCount = (unsigned int)tris.count;
2892 result.mVertices = (float *)verts_out;
2893 result.mVcount = (unsigned int)verts_count_out;
2894 result.mIndices = (unsigned int *)tris.element;
2895 tris.element = NULL;
2896 tris.count = tris.array_size = 0;
2897
2898 return true;
2899 }
2900
ReleaseHull(PHullResult & result)2901 void ReleaseHull(PHullResult &result)
2902 {
2903 if (result.mIndices)
2904 {
2905 free(result.mIndices);
2906 }
2907
2908 result.mVcount = 0;
2909 result.mIndexCount = 0;
2910 result.mIndices = 0;
2911 result.mVertices = 0;
2912 result.mIndices = 0;
2913 }
2914
2915 //*********************************************************************
2916 //*********************************************************************
2917 //******** HullLib header
2918 //*********************************************************************
2919 //*********************************************************************
2920
2921 //*********************************************************************
2922 //*********************************************************************
2923 //******** HullLib implementation
2924 //*********************************************************************
2925 //*********************************************************************
2926
CreateConvexHull(const HullDesc & desc,HullResult & result)2927 HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
2928 HullResult &result) // contains the resulst
2929 {
2930 HullError ret = QE_FAIL;
2931
2932 PHullResult hr;
2933
2934 unsigned int vcount = desc.mVcount;
2935 if (vcount < 8) vcount = 8;
2936
2937 float *vsource = (float *)malloc(sizeof(float) * vcount * 3);
2938
2939 float scale[3];
2940
2941 unsigned int ovcount;
2942
2943 bool ok = CleanupVertices(desc.mVcount, desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale); // normalize point cloud, remove duplicates!
2944
2945 if (ok)
2946 {
2947 if (1) // scale vertices back to their original size.
2948 {
2949 for (unsigned int i = 0; i < ovcount; i++)
2950 {
2951 float *v = &vsource[i * 3];
2952 v[0] *= scale[0];
2953 v[1] *= scale[1];
2954 v[2] *= scale[2];
2955 }
2956 }
2957
2958 float skinwidth = 0;
2959 if (desc.HasHullFlag(QF_SKIN_WIDTH))
2960 skinwidth = desc.mSkinWidth;
2961
2962 Array<btHullTriangle *> tris;
2963 ok = ComputeHull(ovcount, vsource, hr, desc.mMaxVertices, skinwidth, tris);
2964
2965 if (ok)
2966 {
2967 // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
2968 float *vscratch = (float *)malloc(sizeof(float) * hr.mVcount * 3);
2969 BringOutYourDead(hr.mVertices, hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount);
2970
2971 ret = QE_OK;
2972
2973 if (desc.HasHullFlag(QF_TRIANGLES)) // if he wants the results as triangle!
2974 {
2975 result.mPolygons = false;
2976 result.mNumOutputVertices = ovcount;
2977 result.mOutputVertices = (float *)malloc(sizeof(float) * ovcount * 3);
2978 result.mNumFaces = hr.mFaceCount;
2979 result.mNumIndices = hr.mIndexCount;
2980
2981 result.mIndices = (unsigned int *)malloc(sizeof(unsigned int) * hr.mIndexCount);
2982
2983 memcpy(result.mOutputVertices, vscratch, sizeof(float) * 3 * ovcount);
2984
2985 if (desc.HasHullFlag(QF_REVERSE_ORDER))
2986 {
2987 const unsigned int *source = hr.mIndices;
2988 unsigned int *dest = result.mIndices;
2989
2990 for (unsigned int i = 0; i < hr.mFaceCount; i++)
2991 {
2992 dest[0] = source[2];
2993 dest[1] = source[1];
2994 dest[2] = source[0];
2995 dest += 3;
2996 source += 3;
2997 }
2998 }
2999 else
3000 {
3001 memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int) * hr.mIndexCount);
3002 }
3003 }
3004 else
3005 {
3006 result.mPolygons = true;
3007 result.mNumOutputVertices = ovcount;
3008 result.mOutputVertices = (float *)malloc(sizeof(float) * ovcount * 3);
3009 result.mNumFaces = hr.mFaceCount;
3010 result.mNumIndices = hr.mIndexCount + hr.mFaceCount;
3011 result.mIndices = (unsigned int *)malloc(sizeof(unsigned int) * result.mNumIndices);
3012 memcpy(result.mOutputVertices, vscratch, sizeof(float) * 3 * ovcount);
3013
3014 if (1)
3015 {
3016 const unsigned int *source = hr.mIndices;
3017 unsigned int *dest = result.mIndices;
3018 for (unsigned int i = 0; i < hr.mFaceCount; i++)
3019 {
3020 dest[0] = 3;
3021 if (desc.HasHullFlag(QF_REVERSE_ORDER))
3022 {
3023 dest[1] = source[2];
3024 dest[2] = source[1];
3025 dest[3] = source[0];
3026 }
3027 else
3028 {
3029 dest[1] = source[0];
3030 dest[2] = source[1];
3031 dest[3] = source[2];
3032 }
3033
3034 dest += 4;
3035 source += 3;
3036 }
3037 }
3038 }
3039 ReleaseHull(hr);
3040 if (vscratch)
3041 {
3042 free(vscratch);
3043 }
3044 }
3045 }
3046
3047 if (vsource)
3048 {
3049 free(vsource);
3050 }
3051
3052 return ret;
3053 }
3054
ReleaseResult(HullResult & result)3055 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
3056 {
3057 if (result.mOutputVertices)
3058 {
3059 free(result.mOutputVertices);
3060 result.mOutputVertices = 0;
3061 }
3062 if (result.mIndices)
3063 {
3064 free(result.mIndices);
3065 result.mIndices = 0;
3066 }
3067 return QE_OK;
3068 }
3069
addPoint(unsigned int & vcount,float * p,float x,float y,float z)3070 static void addPoint(unsigned int &vcount, float *p, float x, float y, float z)
3071 {
3072 float *dest = &p[vcount * 3];
3073 dest[0] = x;
3074 dest[1] = y;
3075 dest[2] = z;
3076 vcount++;
3077 }
3078
GetDist(float px,float py,float pz,const float * p2)3079 float GetDist(float px, float py, float pz, const float *p2)
3080 {
3081 float dx = px - p2[0];
3082 float dy = py - p2[1];
3083 float dz = pz - p2[2];
3084
3085 return dx * dx + dy * dy + dz * dz;
3086 }
3087
CleanupVertices(unsigned int svcount,const float * svertices,unsigned int stride,unsigned int & vcount,float * vertices,float normalepsilon,float * scale)3088 bool HullLibrary::CleanupVertices(unsigned int svcount,
3089 const float *svertices,
3090 unsigned int stride,
3091 unsigned int &vcount, // output number of vertices
3092 float *vertices, // location to store the results.
3093 float normalepsilon,
3094 float *scale)
3095 {
3096 if (svcount == 0) return false;
3097
3098 #define EPSILON 0.000001f /* close enough to consider two floating point numbers to be 'the same'. */
3099
3100 vcount = 0;
3101
3102 float recip[3];
3103
3104 if (scale)
3105 {
3106 scale[0] = 1;
3107 scale[1] = 1;
3108 scale[2] = 1;
3109 }
3110
3111 float bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
3112 float bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
3113
3114 const char *vtx = (const char *)svertices;
3115
3116 if (1)
3117 {
3118 for (unsigned int i = 0; i < svcount; i++)
3119 {
3120 const float *p = (const float *)vtx;
3121
3122 vtx += stride;
3123
3124 for (int j = 0; j < 3; j++)
3125 {
3126 if (p[j] < bmin[j]) bmin[j] = p[j];
3127 if (p[j] > bmax[j]) bmax[j] = p[j];
3128 }
3129 }
3130 }
3131
3132 float dx = bmax[0] - bmin[0];
3133 float dy = bmax[1] - bmin[1];
3134 float dz = bmax[2] - bmin[2];
3135
3136 float center[3];
3137
3138 center[0] = dx * 0.5f + bmin[0];
3139 center[1] = dy * 0.5f + bmin[1];
3140 center[2] = dz * 0.5f + bmin[2];
3141
3142 if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3)
3143 {
3144 float len = FLT_MAX;
3145
3146 if (dx > EPSILON && dx < len) len = dx;
3147 if (dy > EPSILON && dy < len) len = dy;
3148 if (dz > EPSILON && dz < len) len = dz;
3149
3150 if (len == FLT_MAX)
3151 {
3152 dx = dy = dz = 0.01f; // one centimeter
3153 }
3154 else
3155 {
3156 if (dx < EPSILON) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3157 if (dy < EPSILON) dy = len * 0.05f;
3158 if (dz < EPSILON) dz = len * 0.05f;
3159 }
3160
3161 float x1 = center[0] - dx;
3162 float x2 = center[0] + dx;
3163
3164 float y1 = center[1] - dy;
3165 float y2 = center[1] + dy;
3166
3167 float z1 = center[2] - dz;
3168 float z2 = center[2] + dz;
3169
3170 addPoint(vcount, vertices, x1, y1, z1);
3171 addPoint(vcount, vertices, x2, y1, z1);
3172 addPoint(vcount, vertices, x2, y2, z1);
3173 addPoint(vcount, vertices, x1, y2, z1);
3174 addPoint(vcount, vertices, x1, y1, z2);
3175 addPoint(vcount, vertices, x2, y1, z2);
3176 addPoint(vcount, vertices, x2, y2, z2);
3177 addPoint(vcount, vertices, x1, y2, z2);
3178
3179 return true; // return cube
3180 }
3181 else
3182 {
3183 if (scale)
3184 {
3185 scale[0] = dx;
3186 scale[1] = dy;
3187 scale[2] = dz;
3188
3189 recip[0] = 1 / dx;
3190 recip[1] = 1 / dy;
3191 recip[2] = 1 / dz;
3192
3193 center[0] *= recip[0];
3194 center[1] *= recip[1];
3195 center[2] *= recip[2];
3196 }
3197 }
3198
3199 vtx = (const char *)svertices;
3200
3201 for (unsigned int i = 0; i < svcount; i++)
3202 {
3203 const float *p = (const float *)vtx;
3204 vtx += stride;
3205
3206 float px = p[0];
3207 float py = p[1];
3208 float pz = p[2];
3209
3210 if (scale)
3211 {
3212 px = px * recip[0]; // normalize
3213 py = py * recip[1]; // normalize
3214 pz = pz * recip[2]; // normalize
3215 }
3216
3217 if (1)
3218 {
3219 unsigned int j;
3220
3221 for (j = 0; j < vcount; j++)
3222 {
3223 float *v = &vertices[j * 3];
3224
3225 float x = v[0];
3226 float y = v[1];
3227 float z = v[2];
3228
3229 float dx = fabsf(x - px);
3230 float dy = fabsf(y - py);
3231 float dz = fabsf(z - pz);
3232
3233 if (dx < normalepsilon && dy < normalepsilon && dz < normalepsilon)
3234 {
3235 // ok, it is close enough to the old one
3236 // now let us see if it is further from the center of the point cloud than the one we already recorded.
3237 // in which case we keep this one instead.
3238
3239 float dist1 = GetDist(px, py, pz, center);
3240 float dist2 = GetDist(v[0], v[1], v[2], center);
3241
3242 if (dist1 > dist2)
3243 {
3244 v[0] = px;
3245 v[1] = py;
3246 v[2] = pz;
3247 }
3248
3249 break;
3250 }
3251 }
3252
3253 if (j == vcount)
3254 {
3255 float *dest = &vertices[vcount * 3];
3256 dest[0] = px;
3257 dest[1] = py;
3258 dest[2] = pz;
3259 vcount++;
3260 }
3261 }
3262 }
3263
3264 // ok..now make sure we didn't prune so many vertices it is now invalid.
3265 if (1)
3266 {
3267 float bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
3268 float bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
3269
3270 for (unsigned int i = 0; i < vcount; i++)
3271 {
3272 const float *p = &vertices[i * 3];
3273 for (int j = 0; j < 3; j++)
3274 {
3275 if (p[j] < bmin[j]) bmin[j] = p[j];
3276 if (p[j] > bmax[j]) bmax[j] = p[j];
3277 }
3278 }
3279
3280 float dx = bmax[0] - bmin[0];
3281 float dy = bmax[1] - bmin[1];
3282 float dz = bmax[2] - bmin[2];
3283
3284 if (dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
3285 {
3286 float cx = dx * 0.5f + bmin[0];
3287 float cy = dy * 0.5f + bmin[1];
3288 float cz = dz * 0.5f + bmin[2];
3289
3290 float len = FLT_MAX;
3291
3292 if (dx >= EPSILON && dx < len) len = dx;
3293 if (dy >= EPSILON && dy < len) len = dy;
3294 if (dz >= EPSILON && dz < len) len = dz;
3295
3296 if (len == FLT_MAX)
3297 {
3298 dx = dy = dz = 0.01f; // one centimeter
3299 }
3300 else
3301 {
3302 if (dx < EPSILON) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3303 if (dy < EPSILON) dy = len * 0.05f;
3304 if (dz < EPSILON) dz = len * 0.05f;
3305 }
3306
3307 float x1 = cx - dx;
3308 float x2 = cx + dx;
3309
3310 float y1 = cy - dy;
3311 float y2 = cy + dy;
3312
3313 float z1 = cz - dz;
3314 float z2 = cz + dz;
3315
3316 vcount = 0; // add box
3317
3318 addPoint(vcount, vertices, x1, y1, z1);
3319 addPoint(vcount, vertices, x2, y1, z1);
3320 addPoint(vcount, vertices, x2, y2, z1);
3321 addPoint(vcount, vertices, x1, y2, z1);
3322 addPoint(vcount, vertices, x1, y1, z2);
3323 addPoint(vcount, vertices, x2, y1, z2);
3324 addPoint(vcount, vertices, x2, y2, z2);
3325 addPoint(vcount, vertices, x1, y2, z2);
3326
3327 return true;
3328 }
3329 }
3330
3331 return true;
3332 }
3333
BringOutYourDead(const float * verts,unsigned int vcount,float * overts,unsigned int & ocount,unsigned int * indices,unsigned indexcount)3334 void HullLibrary::BringOutYourDead(const float *verts, unsigned int vcount, float *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount)
3335 {
3336 unsigned int *used = (unsigned int *)malloc(sizeof(unsigned int) * vcount);
3337 memset(used, 0, sizeof(unsigned int) * vcount);
3338
3339 ocount = 0;
3340
3341 for (unsigned int i = 0; i < indexcount; i++)
3342 {
3343 unsigned int v = indices[i]; // original array index
3344
3345 assert(v >= 0 && v < vcount);
3346
3347 if (used[v]) // if already remapped
3348 {
3349 indices[i] = used[v] - 1; // index to new array
3350 }
3351 else
3352 {
3353 indices[i] = ocount; // new index mapping
3354
3355 overts[ocount * 3 + 0] = verts[v * 3 + 0]; // copy old vert to new vert array
3356 overts[ocount * 3 + 1] = verts[v * 3 + 1];
3357 overts[ocount * 3 + 2] = verts[v * 3 + 2];
3358
3359 ocount++; // increment output vert count
3360
3361 assert(ocount >= 0 && ocount <= vcount);
3362
3363 used[v] = ocount; // assign new index remapping
3364 }
3365 }
3366
3367 free(used);
3368 }
3369
3370 } // namespace ConvexDecomposition
3371