1 #include "float_math.h"
10 /*----------------------------------------------------------------------
11 Copyright (c) 2004 Open Dynamics Framework Group
15 Redistribution and use in source and binary forms, with or without modification, are permitted provided
16 that the following conditions are met:
18 Redistributions of source code must retain the above copyright notice, this list of conditions
19 and the following disclaimer.
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.
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.
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 -----------------------------------------------------------------------*/
37 // http://codesuppository.blogspot.com
39 // mailto: jratcliff@infiniplex.net
41 // http://www.amillionpixels.us
46 using namespace ConvexDecomposition;
48 /*----------------------------------------------------------------------
49 Copyright (c) 2004 Open Dynamics Framework Group
53 Redistribution and use in source and binary forms, with or without modification, are permitted provided
54 that the following conditions are met:
56 Redistributions of source code must retain the above copyright notice, this list of conditions
57 and the following disclaimer.
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.
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.
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 -----------------------------------------------------------------------*/
75 #define PI 3.14159264f
77 //*****************************************************
78 //*****************************************************
79 //********* Stan Melax's vector math template needed
80 //********* to use his hull building code.
81 //*****************************************************
82 //*****************************************************
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))
89 namespace ConvexDecomposition
93 int argmin(float a[],int n);
95 float clampf(float a) ;
96 float Round(float a,float precision);
97 float Interpolate(const float &f0,const float &f1,float alpha) ;
110 T Max(const T &a,const T &b)
116 T Min(const T &a,const T &b)
121 //----------------------------------
128 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
129 const int& operator[](int i) const {return (&x)[i];}
130 int& operator[](int i) {return (&x)[i];}
134 //-------- 2D --------
141 float2(float _x,float _y){x=_x;y=_y;}
142 float& operator[](int i) {assert(i>=0&&i<2);return ((float*)this)[i];}
143 const float& operator[](int i) const {assert(i>=0&&i<2);return ((float*)this)[i];}
145 inline float2 operator-( const float2& a, const float2& b ){return float2(a.x-b.x,a.y-b.y);}
146 inline float2 operator+( const float2& a, const float2& b ){return float2(a.x+b.x,a.y+b.y);}
148 //--------- 3D ---------
154 float3(){x=0;y=0;z=0;};
155 float3(float _x,float _y,float _z){x=_x;y=_y;z=_z;};
156 //operator float *() { return &x;};
157 float& operator[](int i) {assert(i>=0&&i<3);return ((float*)this)[i];}
158 const float& operator[](int i) const {assert(i>=0&&i<3);return ((float*)this)[i];}
159 # ifdef PLUGIN_3DSMAX
160 float3(const Point3 &p):x(p.x),y(p.y),z(p.z){}
161 operator Point3(){return *((Point3*)this);}
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 );
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 );
181 inline int operator==( const float3 &a, const float3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); }
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);
197 float3 x,y,z; // the 3 rows of the Matrix
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){}
200 float3x3(float3 _x,float3 _y,float3 _z):x(_x),y(_y),z(_z){}
201 float3& operator[](int i) {assert(i>=0&&i<3);return (&x)[i];}
202 const float3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];}
203 float& operator()(int r, int c) {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
204 const float& operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
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
221 //-------- 4D Math --------
227 float4(){x=0;y=0;z=0;w=0;};
228 float4(float _x,float _y,float _z,float _w){x=_x;y=_y;z=_z;w=_w;}
229 float4(const float3 &v,float _w){x=v.x;y=v.y;z=v.z;w=_w;}
230 //operator float *() { return &x;};
231 float& operator[](int i) {assert(i>=0&&i<4);return ((float*)this)[i];}
232 const float& operator[](int i) const {assert(i>=0&&i<4);return ((float*)this)[i];}
233 const float3& xyz() const { return *((float3*)this);}
234 float3& xyz() { return *((float3*)this);}
243 float4 x,y,z,w; // the 4 rows
245 float4x4(const float4 &_x, const float4 &_y, const float4 &_z, const float4 &_w):x(_x),y(_y),z(_z),w(_w){}
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){}
251 float& operator()(int r, int c) {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
252 const float& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
253 operator float* () {return &x.x;}
254 operator const float* () const {return &x.x;}
255 operator struct D3DXMATRIX* () { return (struct D3DXMATRIX*) this;}
256 operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;}
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 );
279 //-------- Quaternion ------------
281 class Quaternion :public float4
284 Quaternion() { x = y = z = 0.0f; w = 1.0f; }
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; }
286 Quaternion(float _x, float _y, float _z, float _w){x=_x;y=_y;z=_z;w=_w;}
287 float angle() const { return acosf(w)*2.0f; }
288 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)); }
289 float3 xdir() const { return float3( 1-2*(y*y+z*z), 2*(x*y+w*z), 2*(x*z-w*y) ); }
290 float3 ydir() const { return float3( 2*(x*y-w*z),1-2*(x*x+z*z), 2*(y*z+w*x) ); }
291 float3 zdir() const { return float3( 2*(x*z+w*y), 2*(y*z-w*x),1-2*(x*x+y*y) ); }
292 float3x3 getmatrix() const { return float3x3( xdir(), ydir(), zdir() ); }
293 operator float3x3() { return getmatrix(); }
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);
312 //------ Euler Angle -----
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 );
322 //------- Plane ----------
328 float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
329 Plane(const float3 &n,float d):normal(n),dist(d){}
330 Plane():normal(),dist(0){}
331 void Transform(const float3 &position, const Quaternion &orientation);
334 inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);}
335 inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
336 inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); }
339 //--------- Utility Functions ------
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);
355 float sqr(float a) {return a*a;}
356 float clampf(float a) {return Min(1.0f,Max(0.0f,a));}
359 float Round(float a,float precision)
361 return floorf(0.5f+a/precision)*precision;
365 float Interpolate(const float &f0,const float &f1,float alpha)
367 return f0*(1-alpha) + f1*alpha;
371 int argmin(float a[],int n)
386 //------------ float3 (3D) --------------
390 float3 operator+( const float3& a, const float3& b )
392 return float3(a.x+b.x, a.y+b.y, a.z+b.z);
396 float3 operator-( const float3& a, const float3& b )
398 return float3( a.x-b.x, a.y-b.y, a.z-b.z );
402 float3 operator-( const float3& v )
404 return float3( -v.x, -v.y, -v.z );
408 float3 operator*( const float3& v, float s )
410 return float3( v.x*s, v.y*s, v.z*s );
414 float3 operator*( float s, const float3& v )
416 return float3( v.x*s, v.y*s, v.z*s );
420 float3 operator/( const float3& v, float s )
425 float dot( const float3& a, const float3& b )
427 return a.x*b.x + a.y*b.y + a.z*b.z;
430 float3 cmul( const float3 &v1, const float3 &v2)
432 return float3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z);
436 float3 cross( const float3& a, const float3& b )
438 return float3( a.y*b.z - a.z*b.y,
446 float3& operator+=( float3& a , const float3& b )
455 float3& operator-=( float3& a , const float3& b )
464 float3& operator*=(float3& v , float s )
473 float3& operator/=(float3& v , float s )
475 float sinv = 1.0f / s;
482 float3 vabs(const float3 &v)
484 return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z));
488 float magnitude( const float3& v )
490 return sqrtf(sqr(v.x) + sqr( v.y)+ sqr(v.z));
495 float3 normalize( const float3 &v )
497 // this routine, normalize, is ok, provided magnitude works!!
498 float d=magnitude(v);
501 printf("Cant normalize ZERO vector\n");
502 assert(0);// yes this could go here
506 return float3(v.x*d,v.y*d,v.z*d);
509 float3 safenormalize(const float3 &v)
511 if(magnitude(v)<=0.0f)
513 return float3(1,0,0);
518 float3 Round(const float3 &a,float precision)
520 return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
524 float3 Interpolate(const float3 &v0,const float3 &v1,float alpha)
526 return v0*(1-alpha) + v1*alpha;
529 float3 VectorMin(const float3 &a,const float3 &b)
531 return float3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));
533 float3 VectorMax(const float3 &a,const float3 &b)
535 return float3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));
538 // the statement v1*v2 is ambiguous since there are 3 types
539 // of vector multiplication
540 // - componantwise (for example combining colors)
543 // Therefore we never declare/implement this function.
544 // So we will never see: float3 operator*(float3 a,float3 b)
549 //------------ float3x3 ---------------
550 float Determinant(const float3x3 &m)
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 ;
556 float3x3 Inverse(const float3x3 &a)
559 float d=Determinant(a);
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;
573 // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
578 float3x3 Transpose( const float3x3& m )
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));
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));
591 float3 operator*(const float3x3 &m,const float3& v ) {
592 return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
596 float3x3 operator*( const float3x3& a, const float3x3& b )
598 return float3x3(a.x*b,a.y*b,a.z*b);
601 float3x3 operator*( const float3x3& a, const float& s )
603 return float3x3(a.x*s, a.y*s ,a.z*s);
605 float3x3 operator/( const float3x3& a, const float& s )
608 return float3x3(a.x*t, a.y*t ,a.z*t);
610 float3x3 operator+( const float3x3& a, const float3x3& b )
612 return float3x3(a.x+b.x, a.y+b.y, a.z+b.z);
614 float3x3 operator-( const float3x3& a, const float3x3& b )
616 return float3x3(a.x-b.x, a.y-b.y, a.z-b.z);
618 float3x3 &operator+=( float3x3& a, const float3x3& b )
625 float3x3 &operator-=( float3x3& a, const float3x3& b )
632 float3x3 &operator*=( float3x3& a, const float& s )
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);
650 //--------------- 4D ----------------
652 float4 operator*( const float4& v, const float4x4& m )
654 return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works
657 int operator==( const float4 &a, const float4 &b )
659 return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
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 )
667 // return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
672 float4 cmul( const float4 &a, const float4 &b)
674 return float4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w);
678 float4 operator*( const float4 &v, float s)
680 return float4(v.x*s,v.y*s,v.z*s,v.w*s);
684 float4 operator*( float s, const float4 &v)
686 return float4(v.x*s,v.y*s,v.z*s,v.w*s);
690 float4 operator+( const float4 &a, const float4 &b)
692 return float4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);
697 float4 operator-( const float4 &a, const float4 &b)
699 return float4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);
703 float4 Homogenize(const float3 &v3,const float &w)
705 return float4(v3.x,v3.y,v3.z,w);
710 float4x4 operator*( const float4x4& a, const float4x4& b )
712 return float4x4(a.x*b,a.y*b,a.z*b,a.w*b);
715 float4x4 MatrixTranspose(const float4x4 &m)
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 );
724 float4x4 MatrixRigidInverse(const float4x4 &m)
726 float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
728 rot.w = float4(0,0,0,1);
729 return trans_inverse * MatrixTranspose(rot);
733 float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf )
735 float h = 1.0f/tanf(fovy/2.0f); // view space height
736 float w = h / aspect ; // view space width
740 0, 0, zf/(zn-zf) , -1,
741 0, 0, zn*zf/(zn-zf) , 0 );
746 float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up)
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);
758 float4x4 MatrixTranslation(const float3 &t)
768 float4x4 MatrixRotationZ(const float angle_radians)
770 float s = sinf(angle_radians);
771 float c = cosf(angle_radians);
781 int operator==( const float4x4 &a, const float4x4 &b )
783 return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
787 float4x4 Inverse(const float4x4 &m)
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++) {
799 src[i + 12] = m(i,3);
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 */
865 for ( int j = 0; j < 16; j++)
871 //--------- Quaternion --------------
873 Quaternion operator*( const Quaternion& a, const Quaternion& b )
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;
884 Quaternion operator*( const Quaternion& a, float b )
886 return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b);
889 Quaternion Inverse(const Quaternion &q)
891 return Quaternion(-q.x,-q.y,-q.z,q.w);
894 Quaternion& operator*=( Quaternion& q, const float s )
902 void Quaternion::Normalize()
904 float m = sqrtf(sqr(w)+sqr(x)+sqr(y)+sqr(z));
913 float3 operator*( const Quaternion& q, const float3& v )
915 // The following is equivalent to:
916 //return (q.getmatrix() * v);
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;
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 );
933 float3 operator*( const float3& v, const Quaternion& q )
935 assert(0); // must multiply with the quat on the left
936 return float3(0.0f,0.0f,0.0f);
939 Quaternion operator+( const Quaternion& a, const Quaternion& b )
941 return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w);
944 float dot( const Quaternion &a,const Quaternion &b )
946 return (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z);
949 Quaternion normalize( Quaternion a )
951 float m = sqrtf(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z));
961 Quaternion slerp( Quaternion a, const Quaternion& b, float interp )
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));
980 Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) {
981 return slerp(q0,q1,alpha);
985 Quaternion YawPitchRoll( float yaw, float pitch, float roll )
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);
993 float Yaw( const Quaternion& q )
997 return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
1000 float Pitch( const Quaternion& q )
1004 return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
1007 float Roll( Quaternion q )
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;
1014 float Yaw( const float3& v )
1016 return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
1019 float Pitch( const float3& v )
1021 return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
1025 //------------- Plane --------------
1028 void Plane::Transform(const float3 &position, const Quaternion &orientation) {
1029 // Transforms the plane to the space defined by the
1030 // given position/orientation.
1034 newnormal = Inverse(orientation)*normal;
1035 origin = Inverse(orientation)*(-normal*dist - position);
1038 dist = -dot(newnormal, origin);
1044 //--------- utility functions -------------
1047 // Given two vectors v0 and v1 this function
1048 // returns quaternion q where q*v0==v1.
1049 // Routine taken from game programming gems.
1050 Quaternion RotationArc(float3 v0,float3 v1){
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);
1066 float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
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;
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;
1100 float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
1102 // returns the point where the line p0-p1 intersects the plane n&d
1105 float dn= dot(plane.normal,dif);
1106 float t = -(plane.dist+dot(plane.normal,p0) )/dn;
1107 return p0 + (dif*t);
1110 float3 PlaneProject(const Plane &plane, const float3 &point)
1112 return point - plane.normal * (dot(point,plane.normal)+plane.dist);
1115 float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a)
1119 float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
1124 float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a)
1128 float t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
1134 float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2)
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);
1146 int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax)
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 );
1154 int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact)
1156 if(BoxInside(v0,bmin,bmax))
1161 if(v0.x<=bmin.x && v1.x>=bmin.x)
1163 float a = (bmin.x-v0.x)/(v1.x-v0.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)
1175 else if(v0.x >= bmax.x && v1.x <= bmax.x)
1177 float a = (bmax.x-v0.x)/(v1.x-v0.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)
1189 if(v0.y<=bmin.y && v1.y>=bmin.y)
1191 float a = (bmin.y-v0.y)/(v1.y-v0.y);
1192 float vx = (1-a) *v0.x + a*v1.x;
1194 float vz = (1-a) *v0.z + a*v1.z;
1195 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
1203 else if(v0.y >= bmax.y && v1.y <= bmax.y)
1205 float a = (bmax.y-v0.y)/(v1.y-v0.y);
1206 float vx = (1-a) *v0.x + a*v1.x;
1208 float vz = (1-a) *v0.z + a*v1.z;
1209 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
1217 if(v0.z<=bmin.z && v1.z>=bmin.z)
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;
1223 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
1231 else if(v0.z >= bmax.z && v1.z <= bmax.z)
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;
1237 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
1249 float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
1252 cp = normalize(cross(udir,vdir));
1254 float distu = -dot(cp,ustart);
1255 float distv = -dot(cp,vstart);
1256 float dist = (float)fabs(distu-distv);
1260 plane.normal = normalize(cross(vdir,cp));
1261 plane.dist = -dot(plane.normal,vstart);
1262 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
1267 plane.normal = normalize(cross(udir,cp));
1268 plane.dist = -dot(plane.normal,ustart);
1269 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
1275 Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2)
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.
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);
1302 u=u - (nrml * sqrtf(1-m*m));
1304 float3 v= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir2);
1314 v=v - (nrml * sqrtf(1-m*m));
1316 return RotationArc(u,v);
1321 int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal)
1330 nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]);
1333 float m = magnitude(nrml);
1338 nrml = nrml * (1.0f/m);
1339 float dist = -dot(nrml,vert[0]);
1341 if((d0=dot(v0,nrml)+dist) <0 || (d1=dot(v1,nrml)+dist) >0)
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;
1355 for(int j=0;inside && j<n;j++)
1357 // let inside = 0 if outside
1358 float3 pp1,pp2,side;
1360 pp2 = vert[(j+1)%n];
1361 side = cross((pp2-pp1),(the_point-pp1));
1362 inside = (dot(nrml,side) >= 0.0);
1366 if(normal){*normal=nrml;}
1367 if(impact){*impact=the_point;}
1372 //**************************************************************************
1373 //**************************************************************************
1374 //*** Stan Melax's array template, needed to compile his hull generation code
1375 //**************************************************************************
1376 //**************************************************************************
1378 template <class Type> class ArrayRet;
1379 template <class Type> class Array {
1382 Array(Array<Type> &array);
1383 Array(ArrayRet<Type> &array);
1385 void allocate(int s);
1386 void SetSize(int s);
1389 void AddUnique(Type);
1391 void Insert(Type,int);
1394 void DelIndex(int i);
1398 const Type &operator[](int i) const { assert(i>=0 && i<count); return element[i]; }
1399 Type &operator[](int i) { assert(i>=0 && i<count); return element[i]; }
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
1406 template <class Type> class ArrayRet:public Array<Type>
1410 template <class Type> Array<Type>::Array(int s)
1422 template <class Type> Array<Type>::Array(Array<Type> &array)
1427 for(int i=0;i<array.count;i++)
1434 template <class Type> Array<Type>::Array(ArrayRet<Type> &array)
1438 template <class Type> Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array)
1441 array_size = array.array_size;
1442 element = array.element;
1450 template <class Type> Array<Type> &Array<Type>::operator=(Array<Type> &array)
1453 for(int i=0;i<array.count;i++)
1460 template <class Type> Array<Type>::~Array()
1462 if (element != NULL)
1466 count=0;array_size=0;element=NULL;
1469 template <class Type> void Array<Type>::allocate(int s)
1473 Type *old = element;
1475 element = (Type *) malloc( sizeof(Type)*array_size);
1477 for(int i=0;i<count;i++)
1487 template <class Type> void Array<Type>::SetSize(int s)
1505 template <class Type> void Array<Type>::Pack()
1510 template <class Type> Type& Array<Type>::Add(Type t)
1512 assert(count<=array_size);
1513 if(count==array_size)
1515 allocate((array_size)?array_size *2:16);
1517 element[count++] = t;
1518 return element[count-1];
1521 template <class Type> int Array<Type>::Contains(Type t)
1525 for(i=0;i<count;i++)
1527 if(element[i] == t) found++;
1532 template <class Type> void Array<Type>::AddUnique(Type t)
1534 if(!Contains(t)) Add(t);
1538 template <class Type> void Array<Type>::DelIndex(int i)
1544 element[i] = element[i+1];
1549 template <class Type> void Array<Type>::Remove(Type t)
1552 for(i=0;i<count;i++)
1559 assert(i<count); // assert object t is in the array.
1561 for(i=0;i<count;i++)
1563 assert(element[i] != t);
1567 template <class Type> void Array<Type>::Insert(Type t,int k)
1570 Add(t); // to allocate space
1573 element[i]=element[i-1];
1581 template <class Type> int Array<Type>::IndexOf(Type t)
1584 for(i=0;i<count;i++)
1597 //*********************************************************************
1598 //*********************************************************************
1599 //******** Hull header
1600 //*********************************************************************
1601 //*********************************************************************
1616 unsigned int mVcount;
1617 unsigned int mIndexCount;
1618 unsigned int mFaceCount;
1620 unsigned int *mIndices;
1624 #define REAL3 float3
1627 #define COPLANAR (0)
1630 #define SPLIT (OVER|UNDER)
1631 #define PAPERWIDTH (0.001f)
1633 float planetestepsilon = PAPERWIDTH;
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)
1646 HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
1648 Array<REAL3> vertices;
1649 Array<HalfEdge> edges;
1650 Array<Plane> facets;
1651 ConvexH(int vertices_size,int edges_size,int facets_size);
1654 typedef ConvexH::HalfEdge HalfEdge;
1656 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
1657 :vertices(vertices_size)
1659 ,facets(facets_size)
1661 vertices.count=vertices_size;
1662 edges.count = edges_size;
1663 facets.count = facets_size;
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);
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);
1681 int SplitTest(ConvexH &convex,const Plane &plane) {
1683 for(int i=0;i<convex.vertices.count;i++) {
1684 flag |= PlaneTest(plane,convex.vertices[i]);
1692 unsigned char planetest;
1694 unsigned char undermap;
1695 unsigned char overmap;
1700 unsigned char planetest;
1701 unsigned char fixes;
1708 unsigned char undermap;
1709 unsigned char overmap;
1718 int AssertIntact(ConvexH &convex) {
1721 for(i=0;i<convex.edges.count;i++) {
1722 if(convex.edges[estart].p!= convex.edges[i].p) {
1726 if(inext>= convex.edges.count || convex.edges[inext].p != convex.edges[i].p) {
1729 assert(convex.edges[inext].p == convex.edges[i].p);
1730 int nb = convex.edges[i].ea;
1732 if(nb==255 || nb==-1) return 0;
1734 assert(i== convex.edges[nb].ea);
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) {
1743 if(i1>= convex.edges.count || convex.edges[i1].p != convex.edges[i].p) {
1747 if(i2>= convex.edges.count || convex.edges[i2].p != convex.edges[i].p) {
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;
1760 // back to back quads
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);
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);
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);
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);
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);
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);
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);
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);
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);
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);
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);
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);
1851 ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
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
1861 assert(convex.edges.count<480);
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;
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++;
1880 else if(vertflag[i].planetest == UNDER) {
1881 vertflag[i].undermap = vertcountunder++;
1884 assert(vertflag[i].planetest == OVER);
1885 vertflag[i].overmap = vertcountover++;
1886 vertflag[i].undermap = 255; // for debugging purposes
1889 int vertcountunderold = vertcountunder; // for debugging only
1891 int under_edge_count =0;
1892 int underplanescount=0;
1895 for(int currentplane=0; currentplane<convex.facets.count; currentplane++) {
1902 int coplanaredge = -1;
1905 if(e1 >= convex.edges.count || convex.edges[e1].p!=currentplane) {
1909 HalfEdge &edge0 = convex.edges[e0];
1910 HalfEdge &edge1 = convex.edges[e1];
1911 HalfEdge &edgea = convex.edges[edge0.ea];
1914 planeside |= vertflag[edge0.v].planetest;
1915 //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) {
1916 // assert(ecop==-1);
1921 if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){
1922 // both endpoints over plane
1923 edgeflag[e0].undermap = -1;
1925 else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER) {
1926 // at least one endpoint under, the other coplanar or under
1928 edgeflag[e0].undermap = under_edge_count;
1929 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
1930 tmpunderedges[under_edge_count].p = underplanescount;
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;
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
1943 if(e2>=convex.edges.count || convex.edges[e2].p!=currentplane) {
1946 assert(convex.edges[e2].p==currentplane);
1947 HalfEdge &edge2 = convex.edges[e2];
1948 if(vertflag[edge2.v].planetest==UNDER) {
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;
1961 edgeflag[e0].undermap = -1;
1964 else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) {
1965 // first is under 2nd is over
1967 edgeflag[e0].undermap = under_edge_count;
1968 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
1969 tmpunderedges[under_edge_count].p = underplanescount;
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;
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++;
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;
1995 // we previously processed an edge where we came under
1996 // now we know about vout as well
1998 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
2002 else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) {
2003 // first is coplanar 2nd is over
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
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;
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
2023 else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) {
2024 // first is over next is under
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++;
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
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!!
2049 tmpunderedges[under_edge_count].v = vin;
2050 tmpunderedges[under_edge_count].p = underplanescount;
2051 edgeflag[e0].undermap = under_edge_count;
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;
2058 assert(edgeflag[e0].undermap == under_edge_count);
2061 else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) {
2062 // first is over next is coplanar
2064 edgeflag[e0].undermap = -1;
2065 vin = vertflag[edge1.v].undermap;
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!!
2080 e1++; // do the modulo at the beginning of the loop
2082 } while(e0!=estart) ;
2084 if(planeside&UNDER) {
2085 planeflag[currentplane].undermap = underplanescount;
2086 tmpunderplanes[underplanescount] = convex.facets[currentplane];
2090 planeflag[currentplane].undermap = 0;
2092 if(vout>=0 && (planeside&UNDER)) {
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++;
2103 // add the new plane to the mix:
2104 if(coplanaredges_num>0) {
2105 tmpunderplanes[underplanescount++]=slice;
2107 for(i=0;i<coplanaredges_num-1;i++) {
2108 if(coplanaredges[i].v1 != coplanaredges[i+1].v0) {
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;
2118 if(j>=coplanaredges_num)
2120 assert(j<coplanaredges_num);
2125 ConvexH *punder = new ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount);
2126 ConvexH &under = *punder;
2128 for(i=0;i<convex.vertices.count;i++) {
2129 if(vertflag[i].planetest != OVER){
2130 under.vertices[k++] = convex.vertices[i];
2134 while(k<vertcountunder) {
2135 under.vertices[k++] = createdverts[i++];
2137 assert(i==createdverts.count);
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;
2146 memcpy(under.edges.element,tmpunderedges,sizeof(HalfEdge)*under_edge_count);
2147 memcpy(under.facets.element,tmpunderplanes,sizeof(Plane)*underplanescount);
2153 static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,float epsilon)
2158 for(i=0;i<planes_count;i++)
2161 for(int j=0;j<convex->vertices.count;j++)
2163 d = Max(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
2171 return (md>epsilon)?p:-1;
2175 inline int maxdir(const T *p,int count,const T &dir)
2179 float currDotm = dot(p[0], dir);
2180 for(int i=1;i<count;i++)
2182 const float currDoti = dot(p[i], dir);
2183 if(currDoti > currDotm)
2185 currDotm = currDoti;
2194 int maxdirfiltered(const T *p,int count,const T &dir,Array<int> &allow)
2198 float currDotm = dot(p[0], dir);
2199 for(int i=0;i<count;i++)
2205 currDotm = dot(p[i], dir);
2210 const float currDoti = dot(p[i], dir);
2211 if (currDoti>currDotm)
2213 currDotm = currDoti;
2223 float3 orth(const float3 &v)
2225 float3 a=cross(v,float3(0,0,1));
2226 float3 b=cross(v,float3(0,1,0));
2227 return normalize((magnitude(a)>magnitude(b))?a:b);
2232 int maxdirsterid(const T *p,int count,const T &dir,Array<int> &allow)
2237 m = maxdirfiltered(p,count,dir,allow);
2238 if(allow[m]==3) return m;
2242 for(float x = 0.0f ; x<= 360.0f ; x+= 45.0f)
2244 float s = sinf(DEG2RAD*(x));
2245 float c = cosf(DEG2RAD*(x));
2246 int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
2252 if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
2255 for(float xx = x-40.0f ; xx <= x ; xx+= 5.0f)
2257 float s = sinf(DEG2RAD*(xx));
2258 float c = cosf(DEG2RAD*(xx));
2259 int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
2280 int operator ==(const int3 &a,const int3 &b)
2282 for(int i=0;i<3;i++)
2284 if(a[i]!=b[i]) return 0;
2297 int isa(const int3 &a,const int3 &b)
2299 return ( a==b || roll3(a)==b || a==roll3(b) );
2301 int b2b(const int3 &a,const int3 &b)
2303 return isa(a,int3(b[2],b[1],b[0]));
2305 int above(float3* vertices,const int3& t, const float3 &p, float epsilon)
2307 float3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
2308 return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
2310 int hasedge(const int3 &t, int a,int b)
2312 for(int i=0;i<3;i++)
2315 if(t[i]==a && t[i1]==b) return 1;
2319 int hasvert(const int3 &t, int v)
2321 return (t[0]==v || t[1]==v || t[2]==v) ;
2323 int shareedge(const int3 &a,const int3 &b)
2329 if(hasedge(a,b[i1],b[i])) return 1;
2334 class btHullTriangle;
2336 //Array<btHullTriangle*> tris;
2338 class btHullTriangle : public int3
2345 Array<btHullTriangle*>* tris;
2346 btHullTriangle(int a,int b,int c, Array<btHullTriangle*>* pTris):int3(a,b,c),n(-1,-1,-1)
2356 assert((*tris)[id]==this);
2359 int &neib(int a,int b);
2363 int &btHullTriangle::neib(int a,int b)
2371 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
2372 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
2377 void b2bfix(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
2386 assert(tris[s->neib(a,b)]->neib(b,a) == s->id);
2387 assert(tris[t->neib(a,b)]->neib(b,a) == t->id);
2388 tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
2389 tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
2393 void removeb2b(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
2400 void checkit(btHullTriangle *t, Array<btHullTriangle*>& tris)
2403 assert(tris[t->id]==t);
2411 assert( tris[t->n[i]]->neib(b,a) == t->id);
2414 void extrude(btHullTriangle *t0,int v, Array<btHullTriangle*>& tris)
2418 btHullTriangle* ta = new btHullTriangle(v,t[1],t[2], &tris);
2419 ta->n = int3(t0->n[0],n+1,n+2);
2420 tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
2421 btHullTriangle* tb = new btHullTriangle(v,t[2],t[0], &tris);
2422 tb->n = int3(t0->n[1],n+2,n+0);
2423 tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
2424 btHullTriangle* tc = new btHullTriangle(v,t[0],t[1], &tris);
2425 tc->n = int3(t0->n[2],n+0,n+1);
2426 tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
2430 if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]], tris);
2431 if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]], tris);
2432 if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]], tris);
2437 btHullTriangle *extrudable(float epsilon, Array<btHullTriangle*>& tris)
2440 btHullTriangle *t=NULL;
2441 for(i=0;i<tris.count;i++)
2443 if(!t || (tris[i] && t->rise<tris[i]->rise))
2448 return (t->rise >epsilon)?t:NULL ;
2456 int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
2457 const int& operator[](int i) const {return (&x)[i];}
2458 int& operator[](int i) {return (&x)[i];}
2463 int4 FindSimplex(float3 *verts,int verts_count,Array<int> &allow)
2466 basis[0] = float3( 0.01f, 0.02f, 1.0f );
2467 int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
2468 int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
2469 basis[0] = verts[p0]-verts[p1];
2470 if(p0==p1 || basis[0]==float3(0,0,0))
2471 return int4(-1,-1,-1,-1);
2472 basis[1] = cross(float3( 1, 0.02f, 0),basis[0]);
2473 basis[2] = cross(float3(-0.02f, 1, 0),basis[0]);
2474 basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]);
2475 int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
2476 if(p2 == p0 || p2 == p1)
2478 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
2480 if(p2 == p0 || p2 == p1)
2481 return int4(-1,-1,-1,-1);
2482 basis[1] = verts[p2] - verts[p0];
2483 basis[2] = normalize(cross(basis[1],basis[0]));
2484 int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
2485 if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
2486 if(p3==p0||p3==p1||p3==p2)
2487 return int4(-1,-1,-1,-1);
2488 assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
2489 if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
2490 return int4(p0,p1,p2,p3);
2493 int calchullgen(float3 *verts,int verts_count, int vlimit,Array<btHullTriangle*>& tris)
2495 if(verts_count <4) return 0;
2496 if(vlimit==0) vlimit=1000000000;
2498 float3 bmin(*verts),bmax(*verts);
2499 Array<int> isextreme(verts_count);
2500 Array<int> allow(verts_count);
2501 for(j=0;j<verts_count;j++)
2505 bmin = VectorMin(bmin,verts[j]);
2506 bmax = VectorMax(bmax,verts[j]);
2508 float epsilon = magnitude(bmax-bmin) * 0.001f;
2511 int4 p = FindSimplex(verts,verts_count,allow);
2512 if(p.x==-1) return 0; // simplex failed
2516 float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // a valid interior point
2517 btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1], &tris); t0->n=int3(2,3,1);
2518 btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0], &tris); t1->n=int3(3,2,0);
2519 btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3], &tris); t2->n=int3(0,1,3);
2520 btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2], &tris); t3->n=int3(1,0,2);
2521 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
2522 checkit(t0, tris);checkit(t1, tris);checkit(t2, tris);checkit(t3, tris);
2524 for(j=0;j<tris.count;j++)
2526 btHullTriangle *t=tris[j];
2529 float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2530 t->vmax = maxdirsterid(verts,verts_count,n,allow);
2531 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
2535 while(vlimit >0 && (te=extrudable(epsilon, tris)))
2539 assert(!isextreme[v]); // wtf we've already done this vertex
2541 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
2544 if(!tris[j]) continue;
2546 if(above(verts,t,verts[v],0.01f*epsilon))
2548 extrude(tris[j],v, tris);
2551 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
2555 if(!tris[j]) continue;
2556 if(!hasvert(*tris[j],v)) break;
2558 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 )
2560 btHullTriangle *nb = tris[tris[j]->n[0]];
2561 assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
2562 extrude(nb,v, tris);
2569 btHullTriangle *t=tris[j];
2571 if(t->vmax>=0) break;
2572 float3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2573 t->vmax = maxdirsterid(verts,verts_count,n,allow);
2574 if(isextreme[t->vmax])
2576 t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
2580 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
2588 int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit, Array<btHullTriangle*>& tris)
2590 int rc=calchullgen(verts,verts_count, vlimit, tris) ;
2593 for(int i=0;i<tris.count;i++)if(tris[i])
2595 for(int j=0;j<3;j++)ts.Add((*tris[i])[j]);
2598 tris_count = ts.count/3;
2599 tris_out = ts.element;
2600 ts.element=NULL; ts.count=ts.array_size=0;
2605 int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris)
2609 int rc = calchullgen(verts,verts_count,vlimit, tris);
2611 for(i=0;i<tris.count;i++)if(tris[i])
2614 btHullTriangle *t = tris[i];
2615 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
2616 p.dist = -dot(p.normal, verts[(*t)[0]]);
2620 if(t->n[j]<t->id) continue;
2621 btHullTriangle *s = tris[t->n[j]];
2622 REAL3 snormal = TriNormal(verts[(*s)[0]],verts[(*s)[1]],verts[(*s)[2]]);
2623 if(dot(snormal,p.normal)>=cos(bevangle*DEG2RAD)) continue;
2624 REAL3 n = normalize(snormal+p.normal);
2625 planes.Add(Plane(n,-dot(n,verts[maxdir(verts,verts_count,n)])));
2629 for(i=0;i<tris.count;i++)if(tris[i])
2631 delete tris[i]; // delete tris[i];
2637 int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int maxplanes,
2638 float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate)
2641 if(verts_count <4) return 0;
2642 maxplanes = Min(maxplanes,planes_count);
2643 float3 bmin(verts[0]),bmax(verts[0]);
2644 for(i=0;i<verts_count;i++)
2646 bmin = VectorMin(bmin,verts[i]);
2647 bmax = VectorMax(bmax,verts[i]);
2649 // float diameter = magnitude(bmax-bmin);
2650 // inflate *=diameter; // RELATIVE INFLATION
2651 bmin -= float3(inflate,inflate,inflate);
2652 bmax += float3(inflate,inflate,inflate);
2653 for(i=0;i<planes_count;i++)
2655 planes[i].dist -= inflate;
2657 float3 emin = bmin; // VectorMin(bmin,float3(0,0,0));
2658 float3 emax = bmax; // VectorMax(bmax,float3(0,0,0));
2659 float epsilon = magnitude(emax-emin) * 0.025f;
2660 planetestepsilon = magnitude(emax-emin) * PAPERWIDTH;
2661 // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think.
2662 // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter));
2663 ConvexH *c = ConvexHMakeCube(REAL3(bmin),REAL3(bmax));
2665 while(maxplanes-- && (k=candidateplane(planes,planes_count,c,epsilon))>=0)
2668 c = ConvexHCrop(*tmp,planes[k]);
2669 if(c==NULL) {c=tmp; break;} // might want to debug this case better!!!
2670 if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!!
2674 assert(AssertIntact(*c));
2676 faces_out = (int*)malloc(sizeof(int)*(1+c->facets.count+c->edges.count)); // new int[1+c->facets.count+c->edges.count];
2679 faces_out[faces_count_out++]=-1;
2681 while(i<c->edges.count)
2684 while(j+i<c->edges.count && c->edges[i].p==c->edges[i+j].p) { j++; }
2685 faces_out[faces_count_out++]=j;
2688 faces_out[faces_count_out++] = c->edges[i].v;
2693 faces_out[0]=k; // number of faces.
2694 assert(k==c->facets.count);
2695 assert(faces_count_out == 1+c->facets.count+c->edges.count);
2696 verts_out = c->vertices.element; // new float3[c->vertices.count];
2697 verts_count_out = c->vertices.count;
2698 for(i=0;i<c->vertices.count;i++)
2700 verts_out[i] = float3(c->vertices[i]);
2702 c->vertices.count=c->vertices.array_size=0; c->vertices.element=NULL;
2707 int overhullv(float3 *verts, int verts_count,int maxplanes,
2708 float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate,float bevangle,int vlimit, Array<btHullTriangle*>& tris)
2710 if(!verts_count) return 0;
2711 extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris) ;
2712 Array<Plane> planes;
2713 int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle, tris) ;
2715 return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate);
2719 bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate, Array<btHullTriangle*>& arrtris)
2725 int verts_count_out;
2731 int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit, arrtris );
2732 if(!ret) return false;
2733 result.mIndexCount = (unsigned int) (tris_count*3);
2734 result.mFaceCount = (unsigned int) tris_count;
2735 result.mVertices = (float*) vertices;
2736 result.mVcount = (unsigned int) vcount;
2737 result.mIndices = (unsigned int *) tris_out;
2741 int ret = overhullv((float3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit, arrtris);
2742 if(!ret) return false;
2747 for(int i=0;i<n;i++)
2749 int pn = faces[k++];
2750 for(int j=2;j<pn;j++) tris.Add(int3(faces[k],faces[k+j-1],faces[k+j]));
2753 assert(tris.count == index_count-1-(n*3));
2755 result.mIndexCount = (unsigned int) (tris.count*3);
2756 result.mFaceCount = (unsigned int) tris.count;
2757 result.mVertices = (float*) verts_out;
2758 result.mVcount = (unsigned int) verts_count_out;
2759 result.mIndices = (unsigned int *) tris.element;
2760 tris.element=NULL; tris.count = tris.array_size=0;
2766 void ReleaseHull(PHullResult &result)
2768 if ( result.mIndices )
2770 free(result.mIndices);
2774 result.mIndexCount = 0;
2775 result.mIndices = 0;
2776 result.mVertices = 0;
2777 result.mIndices = 0;
2781 //*********************************************************************
2782 //*********************************************************************
2783 //******** HullLib header
2784 //*********************************************************************
2785 //*********************************************************************
2787 //*********************************************************************
2788 //*********************************************************************
2789 //******** HullLib implementation
2790 //*********************************************************************
2791 //*********************************************************************
2793 HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
2794 HullResult &result) // contains the resulst
2796 HullError ret = QE_FAIL;
2801 unsigned int vcount = desc.mVcount;
2802 if ( vcount < 8 ) vcount = 8;
2804 float *vsource = (float *) malloc( sizeof(float)*vcount*3);
2809 unsigned int ovcount;
2811 bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
2817 if ( 1 ) // scale vertices back to their original size.
2819 for (unsigned int i=0; i<ovcount; i++)
2821 float *v = &vsource[i*3];
2828 float skinwidth = 0;
2829 if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
2830 skinwidth = desc.mSkinWidth;
2832 Array<btHullTriangle*> tris;
2833 ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth, tris);
2838 // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
2839 float *vscratch = (float *) malloc( sizeof(float)*hr.mVcount*3);
2840 BringOutYourDead(hr.mVertices,hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount );
2844 if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
2846 result.mPolygons = false;
2847 result.mNumOutputVertices = ovcount;
2848 result.mOutputVertices = (float *)malloc( sizeof(float)*ovcount*3);
2849 result.mNumFaces = hr.mFaceCount;
2850 result.mNumIndices = hr.mIndexCount;
2852 result.mIndices = (unsigned int *) malloc( sizeof(unsigned int)*hr.mIndexCount);
2854 memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount );
2856 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
2859 const unsigned int *source = hr.mIndices;
2860 unsigned int *dest = result.mIndices;
2862 for (unsigned int i=0; i<hr.mFaceCount; i++)
2864 dest[0] = source[2];
2865 dest[1] = source[1];
2866 dest[2] = source[0];
2874 memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int)*hr.mIndexCount);
2879 result.mPolygons = true;
2880 result.mNumOutputVertices = ovcount;
2881 result.mOutputVertices = (float *)malloc( sizeof(float)*ovcount*3);
2882 result.mNumFaces = hr.mFaceCount;
2883 result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
2884 result.mIndices = (unsigned int *) malloc( sizeof(unsigned int)*result.mNumIndices);
2885 memcpy(result.mOutputVertices, vscratch, sizeof(float)*3*ovcount );
2889 const unsigned int *source = hr.mIndices;
2890 unsigned int *dest = result.mIndices;
2891 for (unsigned int i=0; i<hr.mFaceCount; i++)
2894 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
2896 dest[1] = source[2];
2897 dest[2] = source[1];
2898 dest[3] = source[0];
2902 dest[1] = source[0];
2903 dest[2] = source[1];
2904 dest[3] = source[2];
2931 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
2933 if ( result.mOutputVertices )
2935 free(result.mOutputVertices);
2936 result.mOutputVertices = 0;
2938 if ( result.mIndices )
2940 free(result.mIndices);
2941 result.mIndices = 0;
2947 static void addPoint(unsigned int &vcount,float *p,float x,float y,float z)
2949 float *dest = &p[vcount*3];
2957 float GetDist(float px,float py,float pz,const float *p2)
2960 float dx = px - p2[0];
2961 float dy = py - p2[1];
2962 float dz = pz - p2[2];
2964 return dx*dx+dy*dy+dz*dz;
2969 bool HullLibrary::CleanupVertices(unsigned int svcount,
2970 const float *svertices,
2971 unsigned int stride,
2972 unsigned int &vcount, // output number of vertices
2973 float *vertices, // location to store the results.
2974 float normalepsilon,
2977 if ( svcount == 0 ) return false;
2980 #define EPSILON 0.000001f /* close enough to consider two floating point numbers to be 'the same'. */
2993 float bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
2994 float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
2996 const char *vtx = (const char *) svertices;
3000 for (unsigned int i=0; i<svcount; i++)
3002 const float *p = (const float *) vtx;
3006 for (int j=0; j<3; j++)
3008 if ( p[j] < bmin[j] ) bmin[j] = p[j];
3009 if ( p[j] > bmax[j] ) bmax[j] = p[j];
3014 float dx = bmax[0] - bmin[0];
3015 float dy = bmax[1] - bmin[1];
3016 float dz = bmax[2] - bmin[2];
3020 center[0] = dx*0.5f + bmin[0];
3021 center[1] = dy*0.5f + bmin[1];
3022 center[2] = dz*0.5f + bmin[2];
3024 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
3027 float len = FLT_MAX;
3029 if ( dx > EPSILON && dx < len ) len = dx;
3030 if ( dy > EPSILON && dy < len ) len = dy;
3031 if ( dz > EPSILON && dz < len ) len = dz;
3033 if ( len == FLT_MAX )
3035 dx = dy = dz = 0.01f; // one centimeter
3039 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3040 if ( dy < EPSILON ) dy = len * 0.05f;
3041 if ( dz < EPSILON ) dz = len * 0.05f;
3044 float x1 = center[0] - dx;
3045 float x2 = center[0] + dx;
3047 float y1 = center[1] - dy;
3048 float y2 = center[1] + dy;
3050 float z1 = center[2] - dz;
3051 float z2 = center[2] + dz;
3053 addPoint(vcount,vertices,x1,y1,z1);
3054 addPoint(vcount,vertices,x2,y1,z1);
3055 addPoint(vcount,vertices,x2,y2,z1);
3056 addPoint(vcount,vertices,x1,y2,z1);
3057 addPoint(vcount,vertices,x1,y1,z2);
3058 addPoint(vcount,vertices,x2,y1,z2);
3059 addPoint(vcount,vertices,x2,y2,z2);
3060 addPoint(vcount,vertices,x1,y2,z2);
3062 return true; // return cube
3078 center[0]*=recip[0];
3079 center[1]*=recip[1];
3080 center[2]*=recip[2];
3088 vtx = (const char *) svertices;
3090 for (unsigned int i=0; i<svcount; i++)
3093 const float *p = (const float *)vtx;
3102 px = px*recip[0]; // normalize
3103 py = py*recip[1]; // normalize
3104 pz = pz*recip[2]; // normalize
3111 for (j=0; j<vcount; j++)
3113 float *v = &vertices[j*3];
3119 float dx = fabsf(x - px );
3120 float dy = fabsf(y - py );
3121 float dz = fabsf(z - pz );
3123 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
3125 // ok, it is close enough to the old one
3126 // now let us see if it is further from the center of the point cloud than the one we already recorded.
3127 // in which case we keep this one instead.
3129 float dist1 = GetDist(px,py,pz,center);
3130 float dist2 = GetDist(v[0],v[1],v[2],center);
3132 if ( dist1 > dist2 )
3145 float *dest = &vertices[vcount*3];
3154 // ok..now make sure we didn't prune so many vertices it is now invalid.
3157 float bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
3158 float bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
3160 for (unsigned int i=0; i<vcount; i++)
3162 const float *p = &vertices[i*3];
3163 for (int j=0; j<3; j++)
3165 if ( p[j] < bmin[j] ) bmin[j] = p[j];
3166 if ( p[j] > bmax[j] ) bmax[j] = p[j];
3170 float dx = bmax[0] - bmin[0];
3171 float dy = bmax[1] - bmin[1];
3172 float dz = bmax[2] - bmin[2];
3174 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
3176 float cx = dx*0.5f + bmin[0];
3177 float cy = dy*0.5f + bmin[1];
3178 float cz = dz*0.5f + bmin[2];
3180 float len = FLT_MAX;
3182 if ( dx >= EPSILON && dx < len ) len = dx;
3183 if ( dy >= EPSILON && dy < len ) len = dy;
3184 if ( dz >= EPSILON && dz < len ) len = dz;
3186 if ( len == FLT_MAX )
3188 dx = dy = dz = 0.01f; // one centimeter
3192 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge.
3193 if ( dy < EPSILON ) dy = len * 0.05f;
3194 if ( dz < EPSILON ) dz = len * 0.05f;
3206 vcount = 0; // add box
3208 addPoint(vcount,vertices,x1,y1,z1);
3209 addPoint(vcount,vertices,x2,y1,z1);
3210 addPoint(vcount,vertices,x2,y2,z1);
3211 addPoint(vcount,vertices,x1,y2,z1);
3212 addPoint(vcount,vertices,x1,y1,z2);
3213 addPoint(vcount,vertices,x2,y1,z2);
3214 addPoint(vcount,vertices,x2,y2,z2);
3215 addPoint(vcount,vertices,x1,y2,z2);
3224 void HullLibrary::BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
3226 unsigned int *used = (unsigned int *)malloc(sizeof(unsigned int)*vcount);
3227 memset(used,0,sizeof(unsigned int)*vcount);
3231 for (unsigned int i=0; i<indexcount; i++)
3233 unsigned int v = indices[i]; // original array index
3235 assert( v >= 0 && v < vcount );
3237 if ( used[v] ) // if already remapped
3239 indices[i] = used[v]-1; // index to new array
3244 indices[i] = ocount; // new index mapping
3246 overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array
3247 overts[ocount*3+1] = verts[v*3+1];
3248 overts[ocount*3+2] = verts[v*3+2];
3250 ocount++; // increment output vert count
3252 assert( ocount >=0 && ocount <= vcount );
3254 used[v] = ocount; // assign new index remapping