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