00001
00056 #include <stdio.h>
00057 #include <stdlib.h>
00058 #include <string.h>
00059 #include <assert.h>
00060 #include <math.h>
00061 #include <float.h>
00062
00063
00064 #include <stdarg.h>
00065 #include <setjmp.h>
00066
00067 #include "cd_hull.h"
00068
00069 #define STANDALONE 1 // This #define is used when tranferring this source code to other projects
00070
00071 #if STANDALONE
00072
00073 #undef NX_ALLOC
00074 #undef NX_FREE
00075
00076 #define NX_ALLOC(x,y) malloc(x)
00077 #define NX_FREE(x) free(x)
00078
00079 #else
00080 #include "Allocateable.h"
00081 #endif
00082
00083 namespace ConvexDecomposition
00084 {
00085
00086
00087
00088
00089
00090 template <class Type> class ArrayRet;
00091 template <class Type> class Array
00092 {
00093 public:
00094 Array(int s=0);
00095 Array(Array<Type> &array);
00096 Array(ArrayRet<Type> &array);
00097 ~Array();
00098 void allocate(int s);
00099 void SetSize(int s);
00100 void Pack();
00101 Type& Add(Type);
00102 void AddUnique(Type);
00103 int Contains(Type);
00104 void Insert(Type,int);
00105 int IndexOf(Type);
00106 void Remove(Type);
00107 void DelIndex(int i);
00108 Type * element;
00109 int count;
00110 int array_size;
00111 const Type &operator[](int i) const { assert(i>=0 && i<count); return element[i]; }
00112 Type &operator[](int i) { assert(i>=0 && i<count); return element[i]; }
00113 Type &Pop() { assert(count); count--; return element[count]; }
00114 Array<Type> &operator=(Array<Type> &array);
00115 Array<Type> &operator=(ArrayRet<Type> &array);
00116
00117 };
00118
00119 template <class Type> class ArrayRet:public Array<Type>
00120 {
00121 };
00122
00123 template <class Type> Array<Type>::Array(int s)
00124 {
00125 count=0;
00126 array_size = 0;
00127 element = NULL;
00128 if(s)
00129 {
00130 allocate(s);
00131 }
00132 }
00133
00134
00135 template <class Type> Array<Type>::Array(Array<Type> &array)
00136 {
00137 count=0;
00138 array_size = 0;
00139 element = NULL;
00140 for(int i=0;i<array.count;i++)
00141 {
00142 Add(array[i]);
00143 }
00144 }
00145
00146
00147 template <class Type> Array<Type>::Array(ArrayRet<Type> &array)
00148 {
00149 *this = array;
00150 }
00151 template <class Type> Array<Type> &Array<Type>::operator=(ArrayRet<Type> &array)
00152 {
00153 count=array.count;
00154 array_size = array.array_size;
00155 element = array.element;
00156 array.element=NULL;
00157 array.count=0;
00158 array.array_size=0;
00159 return *this;
00160 }
00161
00162
00163 template <class Type> Array<Type> &Array<Type>::operator=(Array<Type> &array)
00164 {
00165 count=0;
00166 for(int i=0;i<array.count;i++)
00167 {
00168 Add(array[i]);
00169 }
00170 return *this;
00171 }
00172
00173 template <class Type> Array<Type>::~Array()
00174 {
00175 if (element != NULL)
00176 {
00177 NX_FREE(element);
00178 }
00179 count=0;array_size=0;element=NULL;
00180 }
00181
00182 template <class Type> void Array<Type>::allocate(int s)
00183 {
00184 assert(s>0);
00185 assert(s>=count);
00186 Type *old = element;
00187 array_size =s;
00188 element = (Type *) NX_ALLOC( sizeof(Type)*array_size, CONVEX_TEMP );
00189 assert(element);
00190 for(int i=0;i<count;i++)
00191 {
00192 element[i]=old[i];
00193 }
00194 if(old)
00195 {
00196 NX_FREE(old);
00197 }
00198 }
00199
00200 template <class Type> void Array<Type>::SetSize(int s)
00201 {
00202 if(s==0)
00203 {
00204 if(element)
00205 {
00206 NX_FREE(element);
00207 element = NULL;
00208 }
00209 array_size = s;
00210 }
00211 else
00212 {
00213 allocate(s);
00214 }
00215 count=s;
00216 }
00217
00218 template <class Type> void Array<Type>::Pack()
00219 {
00220 allocate(count);
00221 }
00222
00223 template <class Type> Type& Array<Type>::Add(Type t)
00224 {
00225 assert(count<=array_size);
00226 if(count==array_size)
00227 {
00228 allocate((array_size)?array_size *2:16);
00229 }
00230 element[count++] = t;
00231 return element[count-1];
00232 }
00233
00234 template <class Type> int Array<Type>::Contains(Type t)
00235 {
00236 int i;
00237 int found=0;
00238 for(i=0;i<count;i++)
00239 {
00240 if(element[i] == t) found++;
00241 }
00242 return found;
00243 }
00244
00245 template <class Type> void Array<Type>::AddUnique(Type t)
00246 {
00247 if(!Contains(t)) Add(t);
00248 }
00249
00250
00251 template <class Type> void Array<Type>::DelIndex(int i)
00252 {
00253 assert(i<count);
00254 count--;
00255 while(i<count)
00256 {
00257 element[i] = element[i+1];
00258 i++;
00259 }
00260 }
00261
00262 template <class Type> void Array<Type>::Remove(Type t)
00263 {
00264 int i;
00265 for(i=0;i<count;i++)
00266 {
00267 if(element[i] == t)
00268 {
00269 break;
00270 }
00271 }
00272 assert(i<count);
00273 DelIndex(i);
00274 for(i=0;i<count;i++)
00275 {
00276 assert(element[i] != t);
00277 }
00278 }
00279
00280 template <class Type> void Array<Type>::Insert(Type t,int k)
00281 {
00282 int i=count;
00283 Add(t);
00284 while(i>k)
00285 {
00286 element[i]=element[i-1];
00287 i--;
00288 }
00289 assert(i==k);
00290 element[k]=t;
00291 }
00292
00293
00294 template <class Type> int Array<Type>::IndexOf(Type t)
00295 {
00296 int i;
00297 for(i=0;i<count;i++)
00298 {
00299 if(element[i] == t)
00300 {
00301 return i;
00302 }
00303 }
00304 assert(0);
00305 return -1;
00306 }
00307
00308
00309
00310
00311 #ifndef PLUGIN_3DSMAX
00312 #define PI (3.1415926535897932384626433832795f)
00313 #endif
00314
00315 #define DEG2RAD (PI / 180.0f)
00316 #define RAD2DEG (180.0f / PI)
00317 #define SQRT_OF_2 (1.4142135f)
00318 #define OFFSET(Class,Member) (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL))
00319
00320
00321
00322 int argmin(double a[],int n);
00323 double sqr(double a);
00324 double clampf(double a) ;
00325 double Round(double a,double precision);
00326 double Interpolate(const double &f0,const double &f1,double alpha) ;
00327
00328 template <class T>
00329 void Swap(T &a,T &b)
00330 {
00331 T tmp = a;
00332 a=b;
00333 b=tmp;
00334 }
00335
00336
00337
00338 template <class T>
00339 T Max(const T &a,const T &b)
00340 {
00341 return (a>b)?a:b;
00342 }
00343
00344 template <class T>
00345 T Min(const T &a,const T &b)
00346 {
00347 return (a<b)?a:b;
00348 }
00349
00350
00351
00352 class int3
00353 {
00354 public:
00355 int x,y,z;
00356 int3(){};
00357 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
00358 const int& operator[](int i) const {return (&x)[i];}
00359 int& operator[](int i) {return (&x)[i];}
00360 };
00361
00362
00363
00364
00365 class double2
00366 {
00367 public:
00368 double x,y;
00369 double2(){x=0;y=0;};
00370 double2(double _x,double _y){x=_x;y=_y;}
00371 double& operator[](int i) {assert(i>=0&&i<2);return ((double*)this)[i];}
00372 const double& operator[](int i) const {assert(i>=0&&i<2);return ((double*)this)[i];}
00373 };
00374 inline double2 operator-( const double2& a, const double2& b ){return double2(a.x-b.x,a.y-b.y);}
00375 inline double2 operator+( const double2& a, const double2& b ){return double2(a.x+b.x,a.y+b.y);}
00376
00377
00378
00379 class double3
00380 {
00381 public:
00382 double x,y,z;
00383 double3(){x=0;y=0;z=0;};
00384 double3(double _x,double _y,double _z){x=_x;y=_y;z=_z;};
00385
00386 double& operator[](int i) {assert(i>=0&&i<3);return ((double*)this)[i];}
00387 const double& operator[](int i) const {assert(i>=0&&i<3);return ((double*)this)[i];}
00388 # ifdef PLUGIN_3DSMAX
00389 double3(const Point3 &p):x(p.x),y(p.y),z(p.z){}
00390 operator Point3(){return *((Point3*)this);}
00391 # endif
00392 };
00393
00394
00395 double3& operator+=( double3 &a, const double3& b );
00396 double3& operator-=( double3 &a ,const double3& b );
00397 double3& operator*=( double3 &v ,const double s );
00398 double3& operator/=( double3 &v, const double s );
00399
00400 double magnitude( const double3& v );
00401 double3 normalize( const double3& v );
00402 double3 safenormalize(const double3 &v);
00403 double3 vabs(const double3 &v);
00404 double3 operator+( const double3& a, const double3& b );
00405 double3 operator-( const double3& a, const double3& b );
00406 double3 operator-( const double3& v );
00407 double3 operator*( const double3& v, const double s );
00408 double3 operator*( const double s, const double3& v );
00409 double3 operator/( const double3& v, const double s );
00410 inline int operator==( const double3 &a, const double3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); }
00411 inline int operator!=( const double3 &a, const double3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); }
00412
00413 double dot( const double3& a, const double3& b );
00414 double3 cmul( const double3 &a, const double3 &b);
00415 double3 cross( const double3& a, const double3& b );
00416 double3 Interpolate(const double3 &v0,const double3 &v1,double alpha);
00417 double3 Round(const double3& a,double precision);
00418 double3 VectorMax(const double3 &a, const double3 &b);
00419 double3 VectorMin(const double3 &a, const double3 &b);
00420
00421
00422
00423 class double3x3
00424 {
00425 public:
00426 double3 x,y,z;
00427 double3x3(){}
00428 double3x3(double xx,double xy,double xz,double yx,double yy,double yz,double zx,double zy,double zz):x(xx,xy,xz),y(yx,yy,yz),z(zx,zy,zz){}
00429 double3x3(double3 _x,double3 _y,double3 _z):x(_x),y(_y),z(_z){}
00430 double3& operator[](int i) {assert(i>=0&&i<3);return (&x)[i];}
00431 const double3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];}
00432 double& operator()(int r, int c) {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
00433 const double& operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
00434 };
00435 double3x3 Transpose( const double3x3& m );
00436 double3 operator*( const double3& v , const double3x3& m );
00437 double3 operator*( const double3x3& m , const double3& v );
00438 double3x3 operator*( const double3x3& m , const double& s );
00439 double3x3 operator*( const double3x3& ma, const double3x3& mb );
00440 double3x3 operator/( const double3x3& a, const double& s ) ;
00441 double3x3 operator+( const double3x3& a, const double3x3& b );
00442 double3x3 operator-( const double3x3& a, const double3x3& b );
00443 double3x3 &operator+=( double3x3& a, const double3x3& b );
00444 double3x3 &operator-=( double3x3& a, const double3x3& b );
00445 double3x3 &operator*=( double3x3& a, const double& s );
00446 double Determinant(const double3x3& m );
00447 double3x3 Inverse(const double3x3& a);
00448
00449
00450
00451
00452 class double4
00453 {
00454 public:
00455 double x,y,z,w;
00456 double4(){x=0;y=0;z=0;w=0;};
00457 double4(double _x,double _y,double _z,double _w){x=_x;y=_y;z=_z;w=_w;}
00458 double4(const double3 &v,double _w){x=v.x;y=v.y;z=v.z;w=_w;}
00459
00460 double& operator[](int i) {assert(i>=0&&i<4);return ((double*)this)[i];}
00461 const double& operator[](int i) const {assert(i>=0&&i<4);return ((double*)this)[i];}
00462 const double3& xyz() const { return *((double3*)this);}
00463 double3& xyz() { return *((double3*)this);}
00464 };
00465
00466
00467 struct D3DXMATRIX;
00468
00469 class double4x4
00470 {
00471 public:
00472 double4 x,y,z,w;
00473 double4x4(){}
00474 double4x4(const double4 &_x, const double4 &_y, const double4 &_z, const double4 &_w):x(_x),y(_y),z(_z),w(_w){}
00475 double4x4(double m00, double m01, double m02, double m03,
00476 double m10, double m11, double m12, double m13,
00477 double m20, double m21, double m22, double m23,
00478 double m30, double m31, double m32, double m33 )
00479 :x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){}
00480 double& operator()(int r, int c) {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
00481 const double& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
00482 operator double* () {return &x.x;}
00483 operator const double* () const {return &x.x;}
00484 operator struct D3DXMATRIX* () { return (struct D3DXMATRIX*) this;}
00485 operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;}
00486 };
00487
00488
00489 int operator==( const double4 &a, const double4 &b );
00490 double4 Homogenize(const double3 &v3,const double &w=1.0f);
00491 double4 cmul( const double4 &a, const double4 &b);
00492 double4 operator*( const double4 &v, double s);
00493 double4 operator*( double s, const double4 &v);
00494 double4 operator+( const double4 &a, const double4 &b);
00495 double4 operator-( const double4 &a, const double4 &b);
00496 double4x4 operator*( const double4x4& a, const double4x4& b );
00497 double4 operator*( const double4& v, const double4x4& m );
00498 double4x4 Inverse(const double4x4 &m);
00499 double4x4 MatrixRigidInverse(const double4x4 &m);
00500 double4x4 MatrixTranspose(const double4x4 &m);
00501 double4x4 MatrixPerspectiveFov(double fovy, double Aspect, double zn, double zf );
00502 double4x4 MatrixTranslation(const double3 &t);
00503 double4x4 MatrixRotationZ(const double angle_radians);
00504 double4x4 MatrixLookAt(const double3& eye, const double3& at, const double3& up);
00505 int operator==( const double4x4 &a, const double4x4 &b );
00506
00507
00508
00509
00510 class Quaternion :public double4
00511 {
00512 public:
00513 Quaternion() { x = y = z = 0.0f; w = 1.0f; }
00514 Quaternion( double3 v, double t ) { v = normalize(v); w = cos(t/2.0f); v = v*sin(t/2.0f); x = v.x; y = v.y; z = v.z; }
00515 Quaternion(double _x, double _y, double _z, double _w){x=_x;y=_y;z=_z;w=_w;}
00516 double angle() const { return acos(w)*2.0f; }
00517 double3 axis() const { double3 a(x,y,z); if(fabs(angle())<0.0000001f) return double3(1,0,0); return a*(1/sin(angle()/2.0f)); }
00518 double3 xdir() const { return double3( 1-2*(y*y+z*z), 2*(x*y+w*z), 2*(x*z-w*y) ); }
00519 double3 ydir() const { return double3( 2*(x*y-w*z),1-2*(x*x+z*z), 2*(y*z+w*x) ); }
00520 double3 zdir() const { return double3( 2*(x*z+w*y), 2*(y*z-w*x),1-2*(x*x+y*y) ); }
00521 double3x3 getmatrix() const { return double3x3( xdir(), ydir(), zdir() ); }
00522 operator double3x3() { return getmatrix(); }
00523 void Normalize();
00524 };
00525
00526 Quaternion& operator*=(Quaternion& a, double s );
00527 Quaternion operator*( const Quaternion& a, double s );
00528 Quaternion operator*( const Quaternion& a, const Quaternion& b);
00529 Quaternion operator+( const Quaternion& a, const Quaternion& b );
00530 Quaternion normalize( Quaternion a );
00531 double dot( const Quaternion &a, const Quaternion &b );
00532 double3 operator*( const Quaternion& q, const double3& v );
00533 double3 operator*( const double3& v, const Quaternion& q );
00534 Quaternion slerp( Quaternion a, const Quaternion& b, double interp );
00535 Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,double alpha);
00536 Quaternion RotationArc(double3 v0, double3 v1 );
00537 Quaternion Inverse(const Quaternion &q);
00538 double4x4 MatrixFromQuatVec(const Quaternion &q, const double3 &v);
00539
00540
00541
00542
00543 Quaternion YawPitchRoll( double yaw, double pitch, double roll );
00544 double Yaw( const Quaternion& q );
00545 double Pitch( const Quaternion& q );
00546 double Roll( Quaternion q );
00547 double Yaw( const double3& v );
00548 double Pitch( const double3& v );
00549
00550
00551
00552
00553 class Plane
00554 {
00555 public:
00556 double3 normal;
00557 double dist;
00558 Plane(const double3 &n,double d):normal(n),dist(d){}
00559 Plane():normal(),dist(0){}
00560 void Transform(const double3 &position, const Quaternion &orientation);
00561 };
00562
00563 inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);}
00564 inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
00565 inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); }
00566
00567
00568
00569
00570 double3 PlaneLineIntersection(const Plane &plane, const double3 &p0, const double3 &p1);
00571 double3 PlaneProject(const Plane &plane, const double3 &point);
00572 double3 LineProject(const double3 &p0, const double3 &p1, const double3 &a);
00573 double LineProjectTime(const double3 &p0, const double3 &p1, const double3 &a);
00574 double3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2);
00575 int PolyHit(const double3 *vert,const int n,const double3 &v0, const double3 &v1, double3 *impact=NULL, double3 *normal=NULL);
00576 int BoxInside(const double3 &p,const double3 &bmin, const double3 &bmax) ;
00577 int BoxIntersect(const double3 &v0, const double3 &v1, const double3 &bmin, const double3 &bmax, double3 *impact);
00578 double DistanceBetweenLines(const double3 &ustart, const double3 &udir, const double3 &vstart, const double3 &vdir, double3 *upoint=NULL, double3 *vpoint=NULL);
00579 double3 TriNormal(const double3 &v0, const double3 &v1, const double3 &v2);
00580 double3 NormalOf(const double3 *vert, const int n);
00581 Quaternion VirtualTrackBall(const double3 &cop, const double3 &cor, const double3 &dir0, const double3 &dir1);
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591 double sqr(double a) {return a*a;}
00592 double clampf(double a) {return Min(1.0,Max(0.0,a));}
00593
00594
00595 double Round(double a,double precision)
00596 {
00597 return floor(0.5f+a/precision)*precision;
00598 }
00599
00600
00601 double Interpolate(const double &f0,const double &f1,double alpha)
00602 {
00603 return f0*(1-alpha) + f1*alpha;
00604 }
00605
00606
00607 int argmin(double a[],int n)
00608 {
00609 int r=0;
00610 for(int i=1;i<n;i++)
00611 {
00612 if(a[i]<a[r])
00613 {
00614 r = i;
00615 }
00616 }
00617 return r;
00618 }
00619
00620
00621
00622
00623
00624
00625
00626 double3 operator+( const double3& a, const double3& b )
00627 {
00628 return double3(a.x+b.x, a.y+b.y, a.z+b.z);
00629 }
00630
00631
00632 double3 operator-( const double3& a, const double3& b )
00633 {
00634 return double3( a.x-b.x, a.y-b.y, a.z-b.z );
00635 }
00636
00637
00638 double3 operator-( const double3& v )
00639 {
00640 return double3( -v.x, -v.y, -v.z );
00641 }
00642
00643
00644 double3 operator*( const double3& v, double s )
00645 {
00646 return double3( v.x*s, v.y*s, v.z*s );
00647 }
00648
00649
00650 double3 operator*( double s, const double3& v )
00651 {
00652 return double3( v.x*s, v.y*s, v.z*s );
00653 }
00654
00655
00656 double3 operator/( const double3& v, double s )
00657 {
00658 return v*(1.0f/s);
00659 }
00660
00661 double dot( const double3& a, const double3& b )
00662 {
00663 return a.x*b.x + a.y*b.y + a.z*b.z;
00664 }
00665
00666 double3 cmul( const double3 &v1, const double3 &v2)
00667 {
00668 return double3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z);
00669 }
00670
00671
00672 double3 cross( const double3& a, const double3& b )
00673 {
00674 return double3( a.y*b.z - a.z*b.y,
00675 a.z*b.x - a.x*b.z,
00676 a.x*b.y - a.y*b.x );
00677 }
00678
00679
00680
00681
00682 double3& operator+=( double3& a , const double3& b )
00683 {
00684 a.x += b.x;
00685 a.y += b.y;
00686 a.z += b.z;
00687 return a;
00688 }
00689
00690
00691 double3& operator-=( double3& a , const double3& b )
00692 {
00693 a.x -= b.x;
00694 a.y -= b.y;
00695 a.z -= b.z;
00696 return a;
00697 }
00698
00699
00700 double3& operator*=(double3& v , double s )
00701 {
00702 v.x *= s;
00703 v.y *= s;
00704 v.z *= s;
00705 return v;
00706 }
00707
00708
00709 double3& operator/=(double3& v , double s )
00710 {
00711 double sinv = 1.0f / s;
00712 v.x *= sinv;
00713 v.y *= sinv;
00714 v.z *= sinv;
00715 return v;
00716 }
00717
00718 double3 vabs(const double3 &v)
00719 {
00720 return double3(fabs(v.x),fabs(v.y),fabs(v.z));
00721 }
00722
00723
00724 double magnitude( const double3& v )
00725 {
00726 return sqrt(sqr(v.x) + sqr( v.y)+ sqr(v.z));
00727 }
00728
00729
00730
00731 double3 normalize( const double3 &v )
00732 {
00733
00734 double d=magnitude(v);
00735 if (d==0)
00736 {
00737 printf("Cant normalize ZERO vector\n");
00738 assert(0);
00739 d=0.1f;
00740 }
00741 d = 1/d;
00742 return double3(v.x*d,v.y*d,v.z*d);
00743 }
00744
00745 double3 safenormalize(const double3 &v)
00746 {
00747 if(magnitude(v)<=0.0f)
00748 {
00749 return double3(1,0,0);
00750 }
00751 return normalize(v);
00752 }
00753
00754 double3 Round(const double3 &a,double precision)
00755 {
00756 return double3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
00757 }
00758
00759
00760 double3 Interpolate(const double3 &v0,const double3 &v1,double alpha)
00761 {
00762 return v0*(1-alpha) + v1*alpha;
00763 }
00764
00765 double3 VectorMin(const double3 &a,const double3 &b)
00766 {
00767 return double3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));
00768 }
00769 double3 VectorMax(const double3 &a,const double3 &b)
00770 {
00771 return double3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));
00772 }
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786 double Determinant(const double3x3 &m)
00787 {
00788 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
00789 -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 ;
00790 }
00791
00792 double3x3 Inverse(const double3x3 &a)
00793 {
00794 double3x3 b;
00795 double d=Determinant(a);
00796 assert(d!=0);
00797 for(int i=0;i<3;i++)
00798 {
00799 for(int j=0;j<3;j++)
00800 {
00801 int i1=(i+1)%3;
00802 int i2=(i+2)%3;
00803 int j1=(j+1)%3;
00804 int j2=(j+2)%3;
00805
00806 b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d;
00807 }
00808 }
00809
00810 return b;
00811 }
00812
00813
00814 double3x3 Transpose( const double3x3& m )
00815 {
00816 return double3x3( double3(m.x.x,m.y.x,m.z.x),
00817 double3(m.x.y,m.y.y,m.z.y),
00818 double3(m.x.z,m.y.z,m.z.z));
00819 }
00820
00821
00822 double3 operator*(const double3& v , const double3x3 &m ) {
00823 return double3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z),
00824 (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z),
00825 (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z));
00826 }
00827 double3 operator*(const double3x3 &m,const double3& v ) {
00828 return double3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
00829 }
00830
00831
00832 double3x3 operator*( const double3x3& a, const double3x3& b )
00833 {
00834 return double3x3(a.x*b,a.y*b,a.z*b);
00835 }
00836
00837 double3x3 operator*( const double3x3& a, const double& s )
00838 {
00839 return double3x3(a.x*s, a.y*s ,a.z*s);
00840 }
00841 double3x3 operator/( const double3x3& a, const double& s )
00842 {
00843 double t=1/s;
00844 return double3x3(a.x*t, a.y*t ,a.z*t);
00845 }
00846 double3x3 operator+( const double3x3& a, const double3x3& b )
00847 {
00848 return double3x3(a.x+b.x, a.y+b.y, a.z+b.z);
00849 }
00850 double3x3 operator-( const double3x3& a, const double3x3& b )
00851 {
00852 return double3x3(a.x-b.x, a.y-b.y, a.z-b.z);
00853 }
00854 double3x3 &operator+=( double3x3& a, const double3x3& b )
00855 {
00856 a.x+=b.x;
00857 a.y+=b.y;
00858 a.z+=b.z;
00859 return a;
00860 }
00861 double3x3 &operator-=( double3x3& a, const double3x3& b )
00862 {
00863 a.x-=b.x;
00864 a.y-=b.y;
00865 a.z-=b.z;
00866 return a;
00867 }
00868 double3x3 &operator*=( double3x3& a, const double& s )
00869 {
00870 a.x*=s;
00871 a.y*=s;
00872 a.z*=s;
00873 return a;
00874 }
00875
00876
00877
00878 double3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2){
00879 double3x3 mp =Transpose(double3x3(p0.normal,p1.normal,p2.normal));
00880 double3x3 mi = Inverse(mp);
00881 double3 b(p0.dist,p1.dist,p2.dist);
00882 return -b * mi;
00883 }
00884
00885
00886
00887
00888 double4 operator*( const double4& v, const double4x4& m )
00889 {
00890 return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w;
00891 }
00892
00893 int operator==( const double4 &a, const double4 &b )
00894 {
00895 return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
00896 }
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908 double4 cmul( const double4 &a, const double4 &b)
00909 {
00910 return double4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w);
00911 }
00912
00913
00914 double4 operator*( const double4 &v, double s)
00915 {
00916 return double4(v.x*s,v.y*s,v.z*s,v.w*s);
00917 }
00918
00919
00920 double4 operator*( double s, const double4 &v)
00921 {
00922 return double4(v.x*s,v.y*s,v.z*s,v.w*s);
00923 }
00924
00925
00926 double4 operator+( const double4 &a, const double4 &b)
00927 {
00928 return double4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);
00929 }
00930
00931
00932
00933 double4 operator-( const double4 &a, const double4 &b)
00934 {
00935 return double4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);
00936 }
00937
00938
00939 double4 Homogenize(const double3 &v3,const double &w)
00940 {
00941 return double4(v3.x,v3.y,v3.z,w);
00942 }
00943
00944
00945
00946 double4x4 operator*( const double4x4& a, const double4x4& b )
00947 {
00948 return double4x4(a.x*b,a.y*b,a.z*b,a.w*b);
00949 }
00950
00951 double4x4 MatrixTranspose(const double4x4 &m)
00952 {
00953 return double4x4(
00954 m.x.x, m.y.x, m.z.x, m.w.x,
00955 m.x.y, m.y.y, m.z.y, m.w.y,
00956 m.x.z, m.y.z, m.z.z, m.w.z,
00957 m.x.w, m.y.w, m.z.w, m.w.w );
00958 }
00959
00960 double4x4 MatrixRigidInverse(const double4x4 &m)
00961 {
00962 double4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
00963 double4x4 rot = m;
00964 rot.w = double4(0,0,0,1);
00965 return trans_inverse * MatrixTranspose(rot);
00966 }
00967
00968
00969 double4x4 MatrixPerspectiveFov(double fovy, double aspect, double zn, double zf )
00970 {
00971 double h = 1.0f/tan(fovy/2.0f);
00972 double w = h / aspect ;
00973 return double4x4(
00974 w, 0, 0 , 0,
00975 0, h, 0 , 0,
00976 0, 0, zf/(zn-zf) , -1,
00977 0, 0, zn*zf/(zn-zf) , 0 );
00978 }
00979
00980
00981
00982 double4x4 MatrixLookAt(const double3& eye, const double3& at, const double3& up)
00983 {
00984 double4x4 m;
00985 m.w.w = 1.0f;
00986 m.w.xyz() = eye;
00987 m.z.xyz() = normalize(eye-at);
00988 m.x.xyz() = normalize(cross(up,m.z.xyz()));
00989 m.y.xyz() = cross(m.z.xyz(),m.x.xyz());
00990 return MatrixRigidInverse(m);
00991 }
00992
00993
00994 double4x4 MatrixTranslation(const double3 &t)
00995 {
00996 return double4x4(
00997 1, 0, 0, 0,
00998 0, 1, 0, 0,
00999 0, 0, 1, 0,
01000 t.x,t.y,t.z,1 );
01001 }
01002
01003
01004 double4x4 MatrixRotationZ(const double angle_radians)
01005 {
01006 double s = sin(angle_radians);
01007 double c = cos(angle_radians);
01008 return double4x4(
01009 c, s, 0, 0,
01010 -s, c, 0, 0,
01011 0, 0, 1, 0,
01012 0, 0, 0, 1 );
01013 }
01014
01015
01016
01017 int operator==( const double4x4 &a, const double4x4 &b )
01018 {
01019 return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
01020 }
01021
01022
01023 double4x4 Inverse(const double4x4 &m)
01024 {
01025 double4x4 d;
01026 double *dst = &d.x.x;
01027 double tmp[12];
01028 double src[16];
01029 double det;
01030
01031 for ( int i = 0; i < 4; i++) {
01032 src[i] = m(i,0) ;
01033 src[i + 4] = m(i,1);
01034 src[i + 8] = m(i,2);
01035 src[i + 12] = m(i,3);
01036 }
01037
01038 tmp[0] = src[10] * src[15];
01039 tmp[1] = src[11] * src[14];
01040 tmp[2] = src[9] * src[15];
01041 tmp[3] = src[11] * src[13];
01042 tmp[4] = src[9] * src[14];
01043 tmp[5] = src[10] * src[13];
01044 tmp[6] = src[8] * src[15];
01045 tmp[7] = src[11] * src[12];
01046 tmp[8] = src[8] * src[14];
01047 tmp[9] = src[10] * src[12];
01048 tmp[10] = src[8] * src[13];
01049 tmp[11] = src[9] * src[12];
01050
01051 dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
01052 dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
01053 dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
01054 dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
01055 dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
01056 dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
01057 dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
01058 dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
01059 dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
01060 dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
01061 dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
01062 dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
01063 dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
01064 dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
01065 dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
01066 dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
01067
01068 tmp[0] = src[2]*src[7];
01069 tmp[1] = src[3]*src[6];
01070 tmp[2] = src[1]*src[7];
01071 tmp[3] = src[3]*src[5];
01072 tmp[4] = src[1]*src[6];
01073 tmp[5] = src[2]*src[5];
01074 tmp[6] = src[0]*src[7];
01075 tmp[7] = src[3]*src[4];
01076 tmp[8] = src[0]*src[6];
01077 tmp[9] = src[2]*src[4];
01078 tmp[10] = src[0]*src[5];
01079 tmp[11] = src[1]*src[4];
01080
01081 dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
01082 dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
01083 dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
01084 dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
01085 dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
01086 dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
01087 dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
01088 dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
01089 dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
01090 dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
01091 dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
01092 dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
01093 dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
01094 dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
01095 dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
01096 dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
01097
01098 det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
01099
01100 det = 1/det;
01101 for ( int j = 0; j < 16; j++)
01102 dst[j] *= det;
01103 return d;
01104 }
01105
01106
01107
01108
01109 Quaternion operator*( const Quaternion& a, const Quaternion& b )
01110 {
01111 Quaternion c;
01112 c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
01113 c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
01114 c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
01115 c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
01116 return c;
01117 }
01118
01119
01120 Quaternion operator*( const Quaternion& a, double b )
01121 {
01122 return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b);
01123 }
01124
01125 Quaternion Inverse(const Quaternion &q)
01126 {
01127 return Quaternion(-q.x,-q.y,-q.z,q.w);
01128 }
01129
01130 Quaternion& operator*=( Quaternion& q, const double s )
01131 {
01132 q.x *= s;
01133 q.y *= s;
01134 q.z *= s;
01135 q.w *= s;
01136 return q;
01137 }
01138 void Quaternion::Normalize()
01139 {
01140 double m = sqrt(sqr(w)+sqr(x)+sqr(y)+sqr(z));
01141 if(m<0.000000001f) {
01142 w=1.0f;
01143 x=y=z=0.0f;
01144 return;
01145 }
01146 (*this) *= (1.0f/m);
01147 }
01148
01149 double3 operator*( const Quaternion& q, const double3& v )
01150 {
01151
01152
01153 double qx2 = q.x*q.x;
01154 double qy2 = q.y*q.y;
01155 double qz2 = q.z*q.z;
01156
01157 double qxqy = q.x*q.y;
01158 double qxqz = q.x*q.z;
01159 double qxqw = q.x*q.w;
01160 double qyqz = q.y*q.z;
01161 double qyqw = q.y*q.w;
01162 double qzqw = q.z*q.w;
01163 return double3(
01164 (1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z ,
01165 (2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z ,
01166 (2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z );
01167 }
01168
01169 double3 operator*( const double3& v, const Quaternion& q )
01170 {
01171 assert(0);
01172 return double3(0.0f,0.0f,0.0f);
01173 }
01174
01175 Quaternion operator+( const Quaternion& a, const Quaternion& b )
01176 {
01177 return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w);
01178 }
01179
01180 double dot( const Quaternion &a,const Quaternion &b )
01181 {
01182 return (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z);
01183 }
01184
01185 Quaternion normalize( Quaternion a )
01186 {
01187 double m = sqrt(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z));
01188 if(m<0.000000001)
01189 {
01190 a.w=1;
01191 a.x=a.y=a.z=0;
01192 return a;
01193 }
01194 return a * (1/m);
01195 }
01196
01197 Quaternion slerp( Quaternion a, const Quaternion& b, double interp )
01198 {
01199 if(dot(a,b) <0.0)
01200 {
01201 a.w=-a.w;
01202 a.x=-a.x;
01203 a.y=-a.y;
01204 a.z=-a.z;
01205 }
01206 double d = dot(a,b);
01207 if(d>=1.0) {
01208 return a;
01209 }
01210 double theta = acos(d);
01211 if(theta==0.0f) { return(a);}
01212 return a*(sin(theta-interp*theta)/sin(theta)) + b*(sin(interp*theta)/sin(theta));
01213 }
01214
01215
01216 Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,double alpha) {
01217 return slerp(q0,q1,alpha);
01218 }
01219
01220
01221 Quaternion YawPitchRoll( double yaw, double pitch, double roll )
01222 {
01223 roll *= DEG2RAD;
01224 yaw *= DEG2RAD;
01225 pitch *= DEG2RAD;
01226 return Quaternion(double3(0.0f,0.0f,1.0f),yaw)*Quaternion(double3(1.0f,0.0f,0.0f),pitch)*Quaternion(double3(0.0f,1.0f,0.0f),roll);
01227 }
01228
01229 double Yaw( const Quaternion& q )
01230 {
01231 static double3 v;
01232 v=q.ydir();
01233 return (v.y==0.0&&v.x==0.0) ? 0.0: atan2(-v.x,v.y)*RAD2DEG;
01234 }
01235
01236 double Pitch( const Quaternion& q )
01237 {
01238 static double3 v;
01239 v=q.ydir();
01240 return atan2(v.z,sqrt(sqr(v.x)+sqr(v.y)))*RAD2DEG;
01241 }
01242
01243 double Roll( Quaternion q )
01244 {
01245 q = Quaternion(double3(0.0f,0.0f,1.0f),-Yaw(q)*DEG2RAD) *q;
01246 q = Quaternion(double3(1.0f,0.0f,0.0f),-Pitch(q)*DEG2RAD) *q;
01247 return atan2(-q.xdir().z,q.xdir().x)*RAD2DEG;
01248 }
01249
01250 double Yaw( const double3& v )
01251 {
01252 return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2(-v.x,v.y)*RAD2DEG;
01253 }
01254
01255 double Pitch( const double3& v )
01256 {
01257 return atan2(v.z,sqrt(sqr(v.x)+sqr(v.y)))*RAD2DEG;
01258 }
01259
01260
01261
01262
01263
01264 void Plane::Transform(const double3 &position, const Quaternion &orientation) {
01265
01266
01267 static double3 newnormal;
01268 static double3 origin;
01269
01270 newnormal = Inverse(orientation)*normal;
01271 origin = Inverse(orientation)*(-normal*dist - position);
01272
01273 normal = newnormal;
01274 dist = -dot(newnormal, origin);
01275 }
01276
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286 Quaternion RotationArc(double3 v0,double3 v1){
01287 static Quaternion q;
01288 v0 = normalize(v0);
01289 v1 = normalize(v1);
01290 double3 c = cross(v0,v1);
01291 double d = dot(v0,v1);
01292 if(d<=-1.0f) { return Quaternion(1,0,0,0);}
01293 double s = sqrt((1+d)*2);
01294 q.x = c.x / s;
01295 q.y = c.y / s;
01296 q.z = c.z / s;
01297 q.w = s /2.0f;
01298 return q;
01299 }
01300
01301
01302 double4x4 MatrixFromQuatVec(const Quaternion &q, const double3 &v)
01303 {
01304
01305 double qx2 = q.x*q.x;
01306 double qy2 = q.y*q.y;
01307 double qz2 = q.z*q.z;
01308
01309 double qxqy = q.x*q.y;
01310 double qxqz = q.x*q.z;
01311 double qxqw = q.x*q.w;
01312 double qyqz = q.y*q.z;
01313 double qyqw = q.y*q.w;
01314 double qzqw = q.z*q.w;
01315
01316 return double4x4(
01317 1-2*(qy2+qz2),
01318 2*(qxqy+qzqw),
01319 2*(qxqz-qyqw),
01320 0 ,
01321 2*(qxqy-qzqw),
01322 1-2*(qx2+qz2),
01323 2*(qyqz+qxqw),
01324 0 ,
01325 2*(qxqz+qyqw),
01326 2*(qyqz-qxqw),
01327 1-2*(qx2+qy2),
01328 0 ,
01329 v.x ,
01330 v.y ,
01331 v.z ,
01332 1.0f );
01333 }
01334
01335
01336 double3 PlaneLineIntersection(const Plane &plane, const double3 &p0, const double3 &p1)
01337 {
01338
01339 static double3 dif;
01340 dif = p1-p0;
01341 double dn= dot(plane.normal,dif);
01342 double t = -(plane.dist+dot(plane.normal,p0) )/dn;
01343 return p0 + (dif*t);
01344 }
01345
01346 double3 PlaneProject(const Plane &plane, const double3 &point)
01347 {
01348 return point - plane.normal * (dot(point,plane.normal)+plane.dist);
01349 }
01350
01351 double3 LineProject(const double3 &p0, const double3 &p1, const double3 &a)
01352 {
01353 double3 w;
01354 w = p1-p0;
01355 double t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
01356 return p0+ w*t;
01357 }
01358
01359
01360 double LineProjectTime(const double3 &p0, const double3 &p1, const double3 &a)
01361 {
01362 double3 w;
01363 w = p1-p0;
01364 double t= dot(w,(a-p0)) / (sqr(w.x)+sqr(w.y)+sqr(w.z));
01365 return t;
01366 }
01367
01368
01369
01370 double3 TriNormal(const double3 &v0, const double3 &v1, const double3 &v2)
01371 {
01372
01373
01374 double3 cp=cross(v1-v0,v2-v1);
01375 double m=magnitude(cp);
01376 if(m==0) return double3(1,0,0);
01377 return cp*(1.0f/m);
01378 }
01379
01380
01381
01382 int BoxInside(const double3 &p, const double3 &bmin, const double3 &bmax)
01383 {
01384 return (p.x >= bmin.x && p.x <=bmax.x &&
01385 p.y >= bmin.y && p.y <=bmax.y &&
01386 p.z >= bmin.z && p.z <=bmax.z );
01387 }
01388
01389
01390 int BoxIntersect(const double3 &v0, const double3 &v1, const double3 &bmin, const double3 &bmax,double3 *impact)
01391 {
01392 if(BoxInside(v0,bmin,bmax))
01393 {
01394 *impact=v0;
01395 return 1;
01396 }
01397 if(v0.x<=bmin.x && v1.x>=bmin.x)
01398 {
01399 double a = (bmin.x-v0.x)/(v1.x-v0.x);
01400
01401 double vy = (1-a) *v0.y + a*v1.y;
01402 double vz = (1-a) *v0.z + a*v1.z;
01403 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
01404 {
01405 impact->x = bmin.x;
01406 impact->y = vy;
01407 impact->z = vz;
01408 return 1;
01409 }
01410 }
01411 else if(v0.x >= bmax.x && v1.x <= bmax.x)
01412 {
01413 double a = (bmax.x-v0.x)/(v1.x-v0.x);
01414
01415 double vy = (1-a) *v0.y + a*v1.y;
01416 double vz = (1-a) *v0.z + a*v1.z;
01417 if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
01418 {
01419 impact->x = bmax.x;
01420 impact->y = vy;
01421 impact->z = vz;
01422 return 1;
01423 }
01424 }
01425 if(v0.y<=bmin.y && v1.y>=bmin.y)
01426 {
01427 double a = (bmin.y-v0.y)/(v1.y-v0.y);
01428 double vx = (1-a) *v0.x + a*v1.x;
01429
01430 double vz = (1-a) *v0.z + a*v1.z;
01431 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
01432 {
01433 impact->x = vx;
01434 impact->y = bmin.y;
01435 impact->z = vz;
01436 return 1;
01437 }
01438 }
01439 else if(v0.y >= bmax.y && v1.y <= bmax.y)
01440 {
01441 double a = (bmax.y-v0.y)/(v1.y-v0.y);
01442 double vx = (1-a) *v0.x + a*v1.x;
01443
01444 double vz = (1-a) *v0.z + a*v1.z;
01445 if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
01446 {
01447 impact->x = vx;
01448 impact->y = bmax.y;
01449 impact->z = vz;
01450 return 1;
01451 }
01452 }
01453 if(v0.z<=bmin.z && v1.z>=bmin.z)
01454 {
01455 double a = (bmin.z-v0.z)/(v1.z-v0.z);
01456 double vx = (1-a) *v0.x + a*v1.x;
01457 double vy = (1-a) *v0.y + a*v1.y;
01458
01459 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
01460 {
01461 impact->x = vx;
01462 impact->y = vy;
01463 impact->z = bmin.z;
01464 return 1;
01465 }
01466 }
01467 else if(v0.z >= bmax.z && v1.z <= bmax.z)
01468 {
01469 double a = (bmax.z-v0.z)/(v1.z-v0.z);
01470 double vx = (1-a) *v0.x + a*v1.x;
01471 double vy = (1-a) *v0.y + a*v1.y;
01472
01473 if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
01474 {
01475 impact->x = vx;
01476 impact->y = vy;
01477 impact->z = bmax.z;
01478 return 1;
01479 }
01480 }
01481 return 0;
01482 }
01483
01484
01485 double DistanceBetweenLines(const double3 &ustart, const double3 &udir, const double3 &vstart, const double3 &vdir, double3 *upoint, double3 *vpoint)
01486 {
01487 static double3 cp;
01488 cp = normalize(cross(udir,vdir));
01489
01490 double distu = -dot(cp,ustart);
01491 double distv = -dot(cp,vstart);
01492 double dist = (double)fabs(distu-distv);
01493 if(upoint)
01494 {
01495 Plane plane;
01496 plane.normal = normalize(cross(vdir,cp));
01497 plane.dist = -dot(plane.normal,vstart);
01498 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
01499 }
01500 if(vpoint)
01501 {
01502 Plane plane;
01503 plane.normal = normalize(cross(udir,cp));
01504 plane.dist = -dot(plane.normal,ustart);
01505 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
01506 }
01507 return dist;
01508 }
01509
01510
01511 Quaternion VirtualTrackBall(const double3 &cop, const double3 &cor, const double3 &dir1, const double3 &dir2)
01512 {
01513
01514
01515
01516
01517
01518
01519
01520
01521
01522 double m;
01523
01524 double3 nrml = cor - cop;
01525 double fudgefactor = 1.0f/(magnitude(nrml) * 0.25f);
01526 nrml = normalize(nrml);
01527 double dist = -dot(nrml,cor);
01528 double3 u= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir1);
01529 u=u-cor;
01530 u=u*fudgefactor;
01531 m= magnitude(u);
01532 if(m>1)
01533 {
01534 u/=m;
01535 }
01536 else
01537 {
01538 u=u - (nrml * sqrt(1-m*m));
01539 }
01540 double3 v= PlaneLineIntersection(Plane(nrml,dist),cop,cop+dir2);
01541 v=v-cor;
01542 v=v*fudgefactor;
01543 m= magnitude(v);
01544 if(m>1)
01545 {
01546 v/=m;
01547 }
01548 else
01549 {
01550 v=v - (nrml * sqrt(1-m*m));
01551 }
01552 return RotationArc(u,v);
01553 }
01554
01555
01556 int countpolyhit=0;
01557 int PolyHit(const double3 *vert, const int n, const double3 &v0, const double3 &v1, double3 *impact, double3 *normal)
01558 {
01559 countpolyhit++;
01560 int i;
01561 double3 nrml(0,0,0);
01562 for(i=0;i<n;i++)
01563 {
01564 int i1=(i+1)%n;
01565 int i2=(i+2)%n;
01566 nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]);
01567 }
01568
01569 double m = magnitude(nrml);
01570 if(m==0.0)
01571 {
01572 return 0;
01573 }
01574 nrml = nrml * (1.0f/m);
01575 double dist = -dot(nrml,vert[0]);
01576 double d0,d1;
01577 if((d0=dot(v0,nrml)+dist) <0 || (d1=dot(v1,nrml)+dist) >0)
01578 {
01579 return 0;
01580 }
01581
01582 static double3 the_point;
01583
01584
01585
01586 double a = d0/(d0-d1);
01587 the_point = v0*(1-a) + v1*a;
01588
01589
01590 int inside=1;
01591 for(int j=0;inside && j<n;j++)
01592 {
01593
01594 double3 pp1,pp2,side;
01595 pp1 = vert[j] ;
01596 pp2 = vert[(j+1)%n];
01597 side = cross((pp2-pp1),(the_point-pp1));
01598 inside = (dot(nrml,side) >= 0.0);
01599 }
01600 if(inside)
01601 {
01602 if(normal){*normal=nrml;}
01603 if(impact){*impact=the_point;}
01604 }
01605 return inside;
01606 }
01607
01608
01609
01610
01611 class PHullResult
01612 {
01613 public:
01614
01615 PHullResult(void)
01616 {
01617 mVcount = 0;
01618 mIndexCount = 0;
01619 mFaceCount = 0;
01620 mVertices = 0;
01621 mIndices = 0;
01622 }
01623
01624 unsigned int mVcount;
01625 unsigned int mIndexCount;
01626 unsigned int mFaceCount;
01627 double *mVertices;
01628 unsigned int *mIndices;
01629 };
01630
01631 bool ComputeHull(unsigned int vcount,const double *vertices,PHullResult &result,unsigned int maxverts,double inflate);
01632 void ReleaseHull(PHullResult &result);
01633
01634
01635
01636
01637
01638
01639 #define REAL3 double3
01640 #define REAL double
01641
01642 #define COPLANAR (0)
01643 #define UNDER (1)
01644 #define OVER (2)
01645 #define SPLIT (OVER|UNDER)
01646 #define PAPERWIDTH (0.001f)
01647 #define VOLUME_EPSILON (1e-20f)
01648
01649 double planetestepsilon = PAPERWIDTH;
01650
01651 #if STANDALONE
01652 class ConvexH
01653 #else
01654 class ConvexH : public NxFoundation::NxAllocateable
01655 #endif
01656 {
01657 public:
01658 class HalfEdge
01659 {
01660 public:
01661 short ea;
01662 unsigned char v;
01663 unsigned char p;
01664 HalfEdge(){}
01665 HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
01666 };
01667 Array<REAL3> vertices;
01668 Array<HalfEdge> edges;
01669 Array<Plane> facets;
01670 ConvexH(int vertices_size,int edges_size,int facets_size);
01671 };
01672
01673 typedef ConvexH::HalfEdge HalfEdge;
01674
01675 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
01676 :vertices(vertices_size)
01677 ,edges(edges_size)
01678 ,facets(facets_size)
01679 {
01680 vertices.count=vertices_size;
01681 edges.count = edges_size;
01682 facets.count = facets_size;
01683 }
01684
01685 ConvexH *ConvexHDup(ConvexH *src)
01686 {
01687 #if STANDALONE
01688 ConvexH *dst = new ConvexH(src->vertices.count,src->edges.count,src->facets.count);
01689 #else
01690 ConvexH *dst = NX_NEW_MEM(ConvexH(src->vertices.count,src->edges.count,src->facets.count), CONVEX_TEMP);
01691 #endif
01692
01693 memcpy(dst->vertices.element,src->vertices.element,sizeof(double3)*src->vertices.count);
01694 memcpy(dst->edges.element,src->edges.element,sizeof(HalfEdge)*src->edges.count);
01695 memcpy(dst->facets.element,src->facets.element,sizeof(Plane)*src->facets.count);
01696 return dst;
01697 }
01698
01699
01700 int PlaneTest(const Plane &p, const REAL3 &v) {
01701 REAL a = dot(v,p.normal)+p.dist;
01702 int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
01703 return flag;
01704 }
01705
01706 int SplitTest(ConvexH &convex,const Plane &plane) {
01707 int flag=0;
01708 for(int i=0;i<convex.vertices.count;i++) {
01709 flag |= PlaneTest(plane,convex.vertices[i]);
01710 }
01711 return flag;
01712 }
01713
01714 class VertFlag
01715 {
01716 public:
01717 unsigned char planetest;
01718 unsigned char junk;
01719 unsigned char undermap;
01720 unsigned char overmap;
01721 };
01722 class EdgeFlag
01723 {
01724 public:
01725 unsigned char planetest;
01726 unsigned char fixes;
01727 short undermap;
01728 short overmap;
01729 };
01730 class PlaneFlag
01731 {
01732 public:
01733 unsigned char undermap;
01734 unsigned char overmap;
01735 };
01736 class Coplanar{
01737 public:
01738 unsigned short ea;
01739 unsigned char v0;
01740 unsigned char v1;
01741 };
01742
01743 int AssertIntact(ConvexH &convex) {
01744 int i;
01745 int estart=0;
01746 for(i=0;i<convex.edges.count;i++) {
01747 if(convex.edges[estart].p!= convex.edges[i].p) {
01748 estart=i;
01749 }
01750 int inext = i+1;
01751 if(inext>= convex.edges.count || convex.edges[inext].p != convex.edges[i].p) {
01752 inext = estart;
01753 }
01754 assert(convex.edges[inext].p == convex.edges[i].p);
01755 HalfEdge &edge = convex.edges[i];
01756 int nb = convex.edges[i].ea;
01757 assert(nb!=255);
01758 if(nb==255 || nb==-1) return 0;
01759 assert(nb!=-1);
01760 assert(i== convex.edges[nb].ea);
01761 }
01762 for(i=0;i<convex.edges.count;i++) {
01763 assert(COPLANAR==PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v]));
01764 if(COPLANAR!=PlaneTest(convex.facets[convex.edges[i].p],convex.vertices[convex.edges[i].v])) return 0;
01765 if(convex.edges[estart].p!= convex.edges[i].p) {
01766 estart=i;
01767 }
01768 int i1 = i+1;
01769 if(i1>= convex.edges.count || convex.edges[i1].p != convex.edges[i].p) {
01770 i1 = estart;
01771 }
01772 int i2 = i1+1;
01773 if(i2>= convex.edges.count || convex.edges[i2].p != convex.edges[i].p) {
01774 i2 = estart;
01775 }
01776 if(i==i2) continue;
01777 REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i ].v],
01778 convex.vertices[convex.edges[i1].v],
01779 convex.vertices[convex.edges[i2].v]);
01780
01781 if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0;
01782 }
01783 return 1;
01784 }
01785
01786
01787 ConvexH *test_btbq()
01788 {
01789
01790 #if STANDALONE
01791 ConvexH *convex = new ConvexH(4,8,2);
01792 #else
01793 ConvexH *convex = NX_NEW_MEM(ConvexH(4,8,2), CONVEX_TEMP);
01794 #endif
01795
01796 convex->vertices[0] = REAL3(0,0,0);
01797 convex->vertices[1] = REAL3(1,0,0);
01798 convex->vertices[2] = REAL3(1,1,0);
01799 convex->vertices[3] = REAL3(0,1,0);
01800 convex->facets[0] = Plane(REAL3(0,0,1),0);
01801 convex->facets[1] = Plane(REAL3(0,0,-1),0);
01802 convex->edges[0] = HalfEdge(7,0,0);
01803 convex->edges[1] = HalfEdge(6,1,0);
01804 convex->edges[2] = HalfEdge(5,2,0);
01805 convex->edges[3] = HalfEdge(4,3,0);
01806
01807 convex->edges[4] = HalfEdge(3,0,1);
01808 convex->edges[5] = HalfEdge(2,3,1);
01809 convex->edges[6] = HalfEdge(1,2,1);
01810 convex->edges[7] = HalfEdge(0,1,1);
01811 AssertIntact(*convex);
01812 return convex;
01813 }
01814
01815 ConvexH *test_cube()
01816 {
01817 #if STANDALONE
01818 ConvexH *convex = new ConvexH(8,24,6);
01819 #else
01820 ConvexH *convex = NX_NEW_MEM(ConvexH(8,24,6), CONVEX_TEMP);
01821 #endif
01822 convex->vertices[0] = REAL3(0,0,0);
01823 convex->vertices[1] = REAL3(0,0,1);
01824 convex->vertices[2] = REAL3(0,1,0);
01825 convex->vertices[3] = REAL3(0,1,1);
01826 convex->vertices[4] = REAL3(1,0,0);
01827 convex->vertices[5] = REAL3(1,0,1);
01828 convex->vertices[6] = REAL3(1,1,0);
01829 convex->vertices[7] = REAL3(1,1,1);
01830
01831 convex->facets[0] = Plane(REAL3(-1,0,0),0);
01832 convex->facets[1] = Plane(REAL3(1,0,0),-1);
01833 convex->facets[2] = Plane(REAL3(0,-1,0),0);
01834 convex->facets[3] = Plane(REAL3(0,1,0),-1);
01835 convex->facets[4] = Plane(REAL3(0,0,-1),0);
01836 convex->facets[5] = Plane(REAL3(0,0,1),-1);
01837
01838 convex->edges[0 ] = HalfEdge(11,0,0);
01839 convex->edges[1 ] = HalfEdge(23,1,0);
01840 convex->edges[2 ] = HalfEdge(15,3,0);
01841 convex->edges[3 ] = HalfEdge(16,2,0);
01842
01843 convex->edges[4 ] = HalfEdge(13,6,1);
01844 convex->edges[5 ] = HalfEdge(21,7,1);
01845 convex->edges[6 ] = HalfEdge( 9,5,1);
01846 convex->edges[7 ] = HalfEdge(18,4,1);
01847
01848 convex->edges[8 ] = HalfEdge(19,0,2);
01849 convex->edges[9 ] = HalfEdge( 6,4,2);
01850 convex->edges[10] = HalfEdge(20,5,2);
01851 convex->edges[11] = HalfEdge( 0,1,2);
01852
01853 convex->edges[12] = HalfEdge(22,3,3);
01854 convex->edges[13] = HalfEdge( 4,7,3);
01855 convex->edges[14] = HalfEdge(17,6,3);
01856 convex->edges[15] = HalfEdge( 2,2,3);
01857
01858 convex->edges[16] = HalfEdge( 3,0,4);
01859 convex->edges[17] = HalfEdge(14,2,4);
01860 convex->edges[18] = HalfEdge( 7,6,4);
01861 convex->edges[19] = HalfEdge( 8,4,4);
01862
01863 convex->edges[20] = HalfEdge(10,1,5);
01864 convex->edges[21] = HalfEdge( 5,5,5);
01865 convex->edges[22] = HalfEdge(12,7,5);
01866 convex->edges[23] = HalfEdge( 1,3,5);
01867
01868
01869 return convex;
01870 }
01871 ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) {
01872 ConvexH *convex = test_cube();
01873 convex->vertices[0] = REAL3(bmin.x,bmin.y,bmin.z);
01874 convex->vertices[1] = REAL3(bmin.x,bmin.y,bmax.z);
01875 convex->vertices[2] = REAL3(bmin.x,bmax.y,bmin.z);
01876 convex->vertices[3] = REAL3(bmin.x,bmax.y,bmax.z);
01877 convex->vertices[4] = REAL3(bmax.x,bmin.y,bmin.z);
01878 convex->vertices[5] = REAL3(bmax.x,bmin.y,bmax.z);
01879 convex->vertices[6] = REAL3(bmax.x,bmax.y,bmin.z);
01880 convex->vertices[7] = REAL3(bmax.x,bmax.y,bmax.z);
01881
01882 convex->facets[0] = Plane(REAL3(-1,0,0), bmin.x);
01883 convex->facets[1] = Plane(REAL3(1,0,0), -bmax.x);
01884 convex->facets[2] = Plane(REAL3(0,-1,0), bmin.y);
01885 convex->facets[3] = Plane(REAL3(0,1,0), -bmax.y);
01886 convex->facets[4] = Plane(REAL3(0,0,-1), bmin.z);
01887 convex->facets[5] = Plane(REAL3(0,0,1), -bmax.z);
01888 return convex;
01889 }
01890 ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
01891 {
01892 int i;
01893 int vertcountunder=0;
01894 int vertcountover =0;
01895 int edgecountunder=0;
01896 int edgecountover =0;
01897 int planecountunder=0;
01898 int planecountover =0;
01899 static Array<int> vertscoplanar;
01900 vertscoplanar.count=0;
01901 static Array<int> edgesplit;
01902 edgesplit.count=0;
01903
01904 assert(convex.edges.count<480);
01905
01906 EdgeFlag edgeflag[512];
01907 VertFlag vertflag[256];
01908 PlaneFlag planeflag[128];
01909 HalfEdge tmpunderedges[512];
01910 Plane tmpunderplanes[128];
01911 Coplanar coplanaredges[512];
01912 int coplanaredges_num=0;
01913
01914 Array<REAL3> createdverts;
01915
01916 for(i=0;i<convex.vertices.count;i++) {
01917 vertflag[i].planetest = PlaneTest(slice,convex.vertices[i]);
01918 if(vertflag[i].planetest == COPLANAR) {
01919
01920 vertflag[i].undermap = vertcountunder++;
01921 vertflag[i].overmap = vertcountover++;
01922 }
01923 else if(vertflag[i].planetest == UNDER) {
01924 vertflag[i].undermap = vertcountunder++;
01925 }
01926 else {
01927 assert(vertflag[i].planetest == OVER);
01928 vertflag[i].overmap = vertcountover++;
01929 vertflag[i].undermap = (unsigned char)-1;
01930 }
01931 }
01932 int vertcountunderold = vertcountunder;
01933
01934 int under_edge_count =0;
01935 int underplanescount=0;
01936 int e0=0;
01937
01938 for(int currentplane=0; currentplane<convex.facets.count; currentplane++) {
01939 int estart =e0;
01940 int enextface;
01941 int planeside = 0;
01942 int e1 = e0+1;
01943 int eus=-1;
01944 int ecop=-1;
01945 int vout=-1;
01946 int vin =-1;
01947 int coplanaredge = -1;
01948 do{
01949
01950 if(e1 >= convex.edges.count || convex.edges[e1].p!=currentplane) {
01951 enextface = e1;
01952 e1=estart;
01953 }
01954 HalfEdge &edge0 = convex.edges[e0];
01955 HalfEdge &edge1 = convex.edges[e1];
01956 HalfEdge &edgea = convex.edges[edge0.ea];
01957
01958
01959 planeside |= vertflag[edge0.v].planetest;
01960
01961
01962
01963
01964
01965
01966 if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){
01967
01968 edgeflag[e0].undermap = -1;
01969 }
01970 else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER) {
01971
01972
01973 edgeflag[e0].undermap = under_edge_count;
01974 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
01975 tmpunderedges[under_edge_count].p = underplanescount;
01976 if(edge0.ea < e0) {
01977
01978 assert(edgeflag[edge0.ea].undermap !=-1);
01979 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
01980 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
01981 }
01982 under_edge_count++;
01983 }
01984 else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == COPLANAR) {
01985
01986
01987 int e2 = e1+1;
01988 if(e2>=convex.edges.count || convex.edges[e2].p!=currentplane) {
01989 e2 = estart;
01990 }
01991 assert(convex.edges[e2].p==currentplane);
01992 HalfEdge &edge2 = convex.edges[e2];
01993 if(vertflag[edge2.v].planetest==UNDER) {
01994
01995 edgeflag[e0].undermap = under_edge_count;
01996 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
01997 tmpunderedges[under_edge_count].p = underplanescount;
01998 tmpunderedges[under_edge_count].ea = -1;
01999
02000 coplanaredge = under_edge_count;
02001 vout = vertflag[edge0.v].undermap;
02002 vin = vertflag[edge1.v].undermap;
02003 under_edge_count++;
02004 }
02005 else {
02006 edgeflag[e0].undermap = -1;
02007 }
02008 }
02009 else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) {
02010
02011
02012 edgeflag[e0].undermap = under_edge_count;
02013 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
02014 tmpunderedges[under_edge_count].p = underplanescount;
02015 if(edge0.ea < e0) {
02016 assert(edgeflag[edge0.ea].undermap !=-1);
02017
02018 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
02019 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
02020 vout = tmpunderedges[edgeflag[edge0.ea].undermap].v;
02021 }
02022 else {
02023 Plane &p0 = convex.facets[edge0.p];
02024 Plane &pa = convex.facets[edgea.p];
02025 createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
02026
02027
02028 vout = vertcountunder++;
02029 }
02030 under_edge_count++;
02032
02033 tmpunderedges[under_edge_count].v = vout;
02034 tmpunderedges[under_edge_count].p = underplanescount;
02035 tmpunderedges[under_edge_count].ea = -1;
02036 coplanaredge = under_edge_count;
02037 under_edge_count++;
02038
02039 if(vin!=-1) {
02040
02041
02042
02043
02044 }
02045
02046 }
02047 else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) {
02048
02049
02050 edgeflag[e0].undermap = -1;
02051 vout = vertflag[edge0.v].undermap;
02052
02053 int k=estart;
02054 assert(edge0.p == currentplane);
02055 while(!(planeside&UNDER) && k<convex.edges.count && convex.edges[k].p==edge0.p) {
02056 planeside |= vertflag[convex.edges[k].v].planetest;
02057 k++;
02058 }
02059 if(planeside&UNDER){
02060 tmpunderedges[under_edge_count].v = vout;
02061 tmpunderedges[under_edge_count].p = underplanescount;
02062 tmpunderedges[under_edge_count].ea = -1;
02063 coplanaredge = under_edge_count;
02064 under_edge_count++;
02065
02066 }
02067 }
02068 else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) {
02069
02070
02071 if (vin!=-1) return NULL;
02072 if(e0<edge0.ea) {
02073 Plane &p0 = convex.facets[edge0.p];
02074 Plane &pa = convex.facets[edgea.p];
02075 createdverts.Add(ThreePlaneIntersection(p0,pa,slice));
02076
02077
02078 vin = vertcountunder++;
02079 }
02080 else {
02081
02082 int nea = edgeflag[edge0.ea].undermap;
02083 assert(tmpunderedges[nea].p==tmpunderedges[nea+1].p);
02084 vin = tmpunderedges[nea+1].v;
02085 assert(vin < vertcountunder);
02086 assert(vin >= vertcountunderold);
02087 }
02088 if(vout!=-1) {
02089
02090
02091
02092 }
02093
02094 tmpunderedges[under_edge_count].v = vin;
02095 tmpunderedges[under_edge_count].p = underplanescount;
02096 edgeflag[e0].undermap = under_edge_count;
02097 if(e0>edge0.ea) {
02098 assert(edgeflag[edge0.ea].undermap !=-1);
02099
02100 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
02101 tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count;
02102 }
02103 assert(edgeflag[e0].undermap == under_edge_count);
02104 under_edge_count++;
02105 }
02106 else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) {
02107
02108
02109 edgeflag[e0].undermap = -1;
02110 vin = vertflag[edge1.v].undermap;
02111 if (vin==-1) return NULL;
02112 if(vout!=-1) {
02113
02114
02115
02116 }
02117
02118 }
02119 else {
02120 assert(0);
02121 }
02122
02123
02124 e0=e1;
02125 e1++;
02126
02127 } while(e0!=estart) ;
02128 e0 = enextface;
02129 if(planeside&UNDER) {
02130 planeflag[currentplane].undermap = underplanescount;
02131 tmpunderplanes[underplanescount] = convex.facets[currentplane];
02132 underplanescount++;
02133 }
02134 else {
02135 planeflag[currentplane].undermap = 0;
02136 }
02137 if(vout>=0 && (planeside&UNDER)) {
02138 assert(vin>=0);
02139 assert(coplanaredge>=0);
02140 assert(coplanaredge!=511);
02141 coplanaredges[coplanaredges_num].ea = coplanaredge;
02142 coplanaredges[coplanaredges_num].v0 = vin;
02143 coplanaredges[coplanaredges_num].v1 = vout;
02144 coplanaredges_num++;
02145 }
02146 }
02147
02148
02149 if(coplanaredges_num>0) {
02150 tmpunderplanes[underplanescount++]=slice;
02151 }
02152 for(i=0;i<coplanaredges_num-1;i++) {
02153 if(coplanaredges[i].v1 != coplanaredges[i+1].v0) {
02154 int j = 0;
02155 for(j=i+2;j<coplanaredges_num;j++) {
02156 if(coplanaredges[i].v1 == coplanaredges[j].v0) {
02157 Coplanar tmp = coplanaredges[i+1];
02158 coplanaredges[i+1] = coplanaredges[j];
02159 coplanaredges[j] = tmp;
02160 break;
02161 }
02162 }
02163 if(j>=coplanaredges_num)
02164 {
02165
02166 return NULL;
02167 }
02168 }
02169 }
02170 #if STANDALONE
02171 ConvexH *punder = new ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount);
02172 #else
02173 ConvexH *punder = NX_NEW_MEM(ConvexH(vertcountunder,under_edge_count+coplanaredges_num,underplanescount), CONVEX_TEMP);
02174 #endif
02175
02176 ConvexH &under = *punder;
02177 int k=0;
02178 for(i=0;i<convex.vertices.count;i++) {
02179 if(vertflag[i].planetest != OVER){
02180 under.vertices[k++] = convex.vertices[i];
02181 }
02182 }
02183 i=0;
02184 while(k<vertcountunder) {
02185 under.vertices[k++] = createdverts[i++];
02186 }
02187 assert(i==createdverts.count);
02188
02189 for(i=0;i<coplanaredges_num;i++) {
02190 under.edges[under_edge_count+i].p = underplanescount-1;
02191 under.edges[under_edge_count+i].ea = coplanaredges[i].ea;
02192 tmpunderedges[coplanaredges[i].ea].ea = under_edge_count+i;
02193 under.edges[under_edge_count+i].v = coplanaredges[i].v0;
02194 }
02195
02196 memcpy(under.edges.element,tmpunderedges,sizeof(HalfEdge)*under_edge_count);
02197 memcpy(under.facets.element,tmpunderplanes,sizeof(Plane)*underplanescount);
02198 return punder;
02199 }
02200
02201
02202 double minadjangle = 3.0f;
02203 static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,double epsilon)
02204 {
02205 int p =-1;
02206 REAL md=0;
02207 int i,j;
02208 double maxdot_minang = cos(DEG2RAD*minadjangle);
02209 for(i=0;i<planes_count;i++)
02210 {
02211 double d=0;
02212 double dmax=0;
02213 double dmin=0;
02214 for(j=0;j<convex->vertices.count;j++)
02215 {
02216 dmax = Max(dmax,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
02217 dmin = Min(dmin,dot(convex->vertices[j],planes[i].normal)+planes[i].dist);
02218 }
02219 double dr = dmax-dmin;
02220 if(dr<planetestepsilon) dr=1.0f;
02221 d = dmax /dr;
02222 if(d<=md) continue;
02223 for(j=0;j<convex->facets.count;j++)
02224 {
02225 if(planes[i]==convex->facets[j])
02226 {
02227 d=0;continue;
02228 }
02229 if(dot(planes[i].normal,convex->facets[j].normal)>maxdot_minang)
02230 {
02231 for(int k=0;k<convex->edges.count;k++)
02232 {
02233 if(convex->edges[k].p!=j) continue;
02234 if(dot(convex->vertices[convex->edges[k].v],planes[i].normal)+planes[i].dist<0)
02235 {
02236 d=0;
02237 break;
02238 }
02239 }
02240 }
02241 }
02242 if(d>md)
02243 {
02244 p=i;
02245 md=d;
02246 }
02247 }
02248 return (md>epsilon)?p:-1;
02249 }
02250
02251
02252
02253 template<class T>
02254 inline int maxdir(const T *p,int count,const T &dir)
02255 {
02256 assert(count);
02257 int m=0;
02258 for(int i=1;i<count;i++)
02259 {
02260 if(dot(p[i],dir)>dot(p[m],dir)) m=i;
02261 }
02262 return m;
02263 }
02264
02265
02266 template<class T>
02267 int maxdirfiltered(const T *p,int count,const T &dir,Array<int> &allow)
02268 {
02269 assert(count);
02270 int m=-1;
02271 for(int i=0;i<count;i++) if(allow[i])
02272 {
02273 if(m==-1 || dot(p[i],dir)>dot(p[m],dir)) m=i;
02274 }
02275 assert(m!=-1);
02276 return m;
02277 }
02278
02279 double3 orth(const double3 &v)
02280 {
02281 double3 a=cross(v,double3(0,0,1));
02282 double3 b=cross(v,double3(0,1,0));
02283 return normalize((magnitude(a)>magnitude(b))?a:b);
02284 }
02285
02286
02287 template<class T>
02288 int maxdirsterid(const T *p,int count,const T &dir,Array<int> &allow)
02289 {
02290 int m=-1;
02291 while(m==-1)
02292 {
02293 m = maxdirfiltered(p,count,dir,allow);
02294 if(allow[m]==3) return m;
02295 T u = orth(dir);
02296 T v = cross(u,dir);
02297 int ma=-1;
02298 for(double x = 0.0f ; x<= 360.0f ; x+= 45.0f)
02299 {
02300 double s = sin(DEG2RAD*(x));
02301 double c = cos(DEG2RAD*(x));
02302 int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
02303 if(ma==m && mb==m)
02304 {
02305 allow[m]=3;
02306 return m;
02307 }
02308 if(ma!=-1 && ma!=mb)
02309 {
02310 int mc = ma;
02311 for(double xx = x-40.0f ; xx <= x ; xx+= 5.0f)
02312 {
02313 double s = sin(DEG2RAD*(xx));
02314 double c = cos(DEG2RAD*(xx));
02315 int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow);
02316 if(mc==m && md==m)
02317 {
02318 allow[m]=3;
02319 return m;
02320 }
02321 mc=md;
02322 }
02323 }
02324 ma=mb;
02325 }
02326 allow[m]=0;
02327 m=-1;
02328 }
02329 assert(0);
02330 return m;
02331 }
02332
02333
02334
02335
02336 int operator ==(const int3 &a,const int3 &b)
02337 {
02338 for(int i=0;i<3;i++)
02339 {
02340 if(a[i]!=b[i]) return 0;
02341 }
02342 return 1;
02343 }
02344
02345 int3 roll3(int3 a)
02346 {
02347 int tmp=a[0];
02348 a[0]=a[1];
02349 a[1]=a[2];
02350 a[2]=tmp;
02351 return a;
02352 }
02353 int isa(const int3 &a,const int3 &b)
02354 {
02355 return ( a==b || roll3(a)==b || a==roll3(b) );
02356 }
02357 int b2b(const int3 &a,const int3 &b)
02358 {
02359 return isa(a,int3(b[2],b[1],b[0]));
02360 }
02361 int above(double3* vertices,const int3& t, const double3 &p, double epsilon)
02362 {
02363 double3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
02364 return (dot(n,p-vertices[t[0]]) > epsilon);
02365 }
02366 int hasedge(const int3 &t, int a,int b)
02367 {
02368 for(int i=0;i<3;i++)
02369 {
02370 int i1= (i+1)%3;
02371 if(t[i]==a && t[i1]==b) return 1;
02372 }
02373 return 0;
02374 }
02375 int hasvert(const int3 &t, int v)
02376 {
02377 return (t[0]==v || t[1]==v || t[2]==v) ;
02378 }
02379 int shareedge(const int3 &a,const int3 &b)
02380 {
02381 int i;
02382 for(i=0;i<3;i++)
02383 {
02384 int i1= (i+1)%3;
02385 if(hasedge(a,b[i1],b[i])) return 1;
02386 }
02387 return 0;
02388 }
02389
02390 class Tri;
02391
02392 static Array<Tri*> tris;
02393
02394 #if STANDALONE
02395 class Tri : public int3
02396 #else
02397 class Tri : public int3, public NxFoundation::NxAllocateable
02398 #endif
02399 {
02400 public:
02401 int3 n;
02402 int id;
02403 int vmax;
02404 double rise;
02405 Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
02406 {
02407 id = tris.count;
02408 tris.Add(this);
02409 vmax=-1;
02410 rise = 0.0f;
02411 }
02412 ~Tri()
02413 {
02414 assert(tris[id]==this);
02415 tris[id]=NULL;
02416 }
02417 int &neib(int a,int b);
02418 };
02419
02420
02421 int &Tri::neib(int a,int b)
02422 {
02423 static int er=-1;
02424 int i;
02425 for(i=0;i<3;i++)
02426 {
02427 int i1=(i+1)%3;
02428 int i2=(i+2)%3;
02429 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
02430 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
02431 }
02432 assert(0);
02433 return er;
02434 }
02435 void b2bfix(Tri* s,Tri*t)
02436 {
02437 int i;
02438 for(i=0;i<3;i++)
02439 {
02440 int i1=(i+1)%3;
02441 int i2=(i+2)%3;
02442 int a = (*s)[i1];
02443 int b = (*s)[i2];
02444 assert(tris[s->neib(a,b)]->neib(b,a) == s->id);
02445 assert(tris[t->neib(a,b)]->neib(b,a) == t->id);
02446 tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
02447 tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
02448 }
02449 }
02450
02451 void removeb2b(Tri* s,Tri*t)
02452 {
02453 b2bfix(s,t);
02454 delete s;
02455 delete t;
02456 }
02457
02458 void checkit(Tri *t)
02459 {
02460 int i;
02461 assert(tris[t->id]==t);
02462 for(i=0;i<3;i++)
02463 {
02464 int i1=(i+1)%3;
02465 int i2=(i+2)%3;
02466 int a = (*t)[i1];
02467 int b = (*t)[i2];
02468 assert(a!=b);
02469 assert( tris[t->n[i]]->neib(b,a) == t->id);
02470 }
02471 }
02472 void extrude(Tri *t0,int v)
02473 {
02474 int3 t= *t0;
02475 int n = tris.count;
02476 #if STANDALONE
02477 Tri* ta = new Tri(v,t[1],t[2]);
02478 #else
02479 Tri* ta = NX_NEW_MEM(Tri(v,t[1],t[2]), CONVEX_TEMP);
02480 #endif
02481 ta->n = int3(t0->n[0],n+1,n+2);
02482 tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
02483 #if STANDALONE
02484 Tri* tb = new Tri(v,t[2],t[0]);
02485 #else
02486 Tri* tb = NX_NEW_MEM(Tri(v,t[2],t[0]), CONVEX_TEMP);
02487 #endif
02488 tb->n = int3(t0->n[1],n+2,n+0);
02489 tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
02490 #if STANDALONE
02491 Tri* tc = new Tri(v,t[0],t[1]);
02492 #else
02493 Tri* tc = NX_NEW_MEM(Tri(v,t[0],t[1]), CONVEX_TEMP);
02494 #endif
02495 tc->n = int3(t0->n[2],n+0,n+1);
02496 tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
02497 checkit(ta);
02498 checkit(tb);
02499 checkit(tc);
02500 if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]);
02501 if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]);
02502 if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]);
02503 delete t0;
02504
02505 }
02506
02507 Tri *extrudable(double epsilon)
02508 {
02509 int i;
02510 Tri *t=NULL;
02511 for(i=0;i<tris.count;i++)
02512 {
02513 if(!t || (tris[i] && t->rise<tris[i]->rise))
02514 {
02515 t = tris[i];
02516 }
02517 }
02518 return (t->rise >epsilon)?t:NULL ;
02519 }
02520
02521 class int4
02522 {
02523 public:
02524 int x,y,z,w;
02525 int4(){};
02526 int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
02527 const int& operator[](int i) const {return (&x)[i];}
02528 int& operator[](int i) {return (&x)[i];}
02529 };
02530
02531
02532
02533 bool hasVolume(double3 *verts, int p0, int p1, int p2, int p3)
02534 {
02535 double3 result3 = cross(verts[p1]-verts[p0], verts[p2]-verts[p0]);
02536 if (magnitude(result3) < VOLUME_EPSILON && magnitude(result3) > -VOLUME_EPSILON)
02537 return false;
02538 double result = dot(normalize(result3), verts[p3]-verts[p0]);
02539 return (result > VOLUME_EPSILON || result < -VOLUME_EPSILON);
02540 }
02541
02542 int4 FindSimplex(double3 *verts,int verts_count,Array<int> &allow)
02543 {
02544 double3 basis[3];
02545 basis[0] = double3( 0.01f, 0.02f, 1.0f );
02546 int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
02547 int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
02548 basis[0] = verts[p0]-verts[p1];
02549 if(p0==p1 || basis[0]==double3(0,0,0))
02550 return int4(-1,-1,-1,-1);
02551 basis[1] = cross(double3( 1, 0.02f, 0),basis[0]);
02552 basis[2] = cross(double3(-0.02f, 1, 0),basis[0]);
02553 basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]);
02554 int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
02555 if(p2 == p0 || p2 == p1)
02556 {
02557 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
02558 }
02559 if(p2 == p0 || p2 == p1)
02560 return int4(-1,-1,-1,-1);
02561 basis[1] = verts[p2] - verts[p0];
02562 basis[2] = normalize(cross(basis[1],basis[0]));
02563 int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
02564 if(p3==p0||p3==p1||p3==p2||!hasVolume(verts, p0, p1, p2, p3)) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
02565 if(p3==p0||p3==p1||p3==p2)
02566 return int4(-1,-1,-1,-1);
02567 assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
02568 if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
02569 return int4(p0,p1,p2,p3);
02570 }
02571
02572 int calchullgen(double3 *verts,int verts_count, int vlimit)
02573 {
02574 if(verts_count <4) return 0;
02575 if(vlimit==0) vlimit=1000000000;
02576 int j;
02577 double3 bmin(*verts),bmax(*verts);
02578 Array<int> isextreme(verts_count);
02579 Array<int> allow(verts_count);
02580 for(j=0;j<verts_count;j++)
02581 {
02582 allow.Add(1);
02583 isextreme.Add(0);
02584 bmin = VectorMin(bmin,verts[j]);
02585 bmax = VectorMax(bmax,verts[j]);
02586 }
02587 double epsilon = magnitude(bmax-bmin) * 0.001f;
02588
02589
02590 int4 p = FindSimplex(verts,verts_count,allow);
02591 if(p.x==-1) return 0;
02592
02593
02594
02595 double3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f;
02596 #if STANDALONE
02597 Tri *t0 = new Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1);
02598 Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0);
02599 Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3);
02600 Tri *t3 = new Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2);
02601 #else
02602 Tri *t0 = NX_NEW_MEM(Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1), CONVEX_TEMP);
02603 Tri *t1 = NX_NEW_MEM(Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0), CONVEX_TEMP);
02604 Tri *t2 = NX_NEW_MEM(Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3), CONVEX_TEMP);
02605 Tri *t3 = NX_NEW_MEM(Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2), CONVEX_TEMP);
02606 #endif
02607 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
02608 checkit(t0);checkit(t1);checkit(t2);checkit(t3);
02609
02610 for(j=0;j<tris.count;j++)
02611 {
02612 Tri *t=tris[j];
02613 assert(t);
02614 assert(t->vmax<0);
02615 double3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02616 t->vmax = maxdirsterid(verts,verts_count,n,allow);
02617 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
02618 }
02619 Tri *te;
02620 vlimit-=4;
02621 while(vlimit >0 && (te=extrudable(epsilon)))
02622 {
02623 int3 ti=*te;
02624 int v=te->vmax;
02625 assert(!isextreme[v]);
02626 isextreme[v]=1;
02627
02628 j=tris.count;
02629 int newstart=j;
02630 while(j--) {
02631 if(!tris[j]) continue;
02632 int3 t=*tris[j];
02633 if(above(verts,t,verts[v],0.01f*epsilon))
02634 {
02635 extrude(tris[j],v);
02636 }
02637 }
02638
02639 j=tris.count;
02640 while(j--)
02641 {
02642 if(!tris[j]) continue;
02643 if(!hasvert(*tris[j],v)) break;
02644 int3 nt=*tris[j];
02645 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 )
02646 {
02647 Tri *nb = tris[tris[j]->n[0]];
02648 assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
02649 extrude(nb,v);
02650 j=tris.count;
02651 }
02652 }
02653 j=tris.count;
02654 while(j--)
02655 {
02656 Tri *t=tris[j];
02657 if(!t) continue;
02658 if(t->vmax>=0) break;
02659 double3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02660 t->vmax = maxdirsterid(verts,verts_count,n,allow);
02661 if(isextreme[t->vmax])
02662 {
02663 t->vmax=-1;
02664 }
02665 else
02666 {
02667 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
02668 }
02669 }
02670 vlimit --;
02671 }
02672 return 1;
02673 }
02674
02675 int calchull(double3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit)
02676 {
02677 int rc=calchullgen(verts,verts_count, vlimit) ;
02678 if(!rc) return 0;
02679 Array<int> ts;
02680 for(int i=0;i<tris.count;i++)if(tris[i])
02681 {
02682 for(int j=0;j<3;j++)ts.Add((*tris[i])[j]);
02683 delete tris[i];
02684 }
02685 tris_count = ts.count/3;
02686 tris_out = ts.element;
02687 ts.element=NULL; ts.count=ts.array_size=0;
02688
02689 tris.SetSize( 0 );
02690 return 1;
02691 }
02692
02693 static double area2(const double3 &v0,const double3 &v1,const double3 &v2)
02694 {
02695 double3 cp = cross(v0-v1,v2-v0);
02696 return dot(cp,cp);
02697 }
02698 int calchullpbev(double3 *verts,int verts_count,int vlimit, Array<Plane> &planes,double bevangle)
02699 {
02700 int i,j;
02701 Array<Plane> bplanes;
02702 planes.count=0;
02703 int rc = calchullgen(verts,verts_count,vlimit);
02704 if(!rc) return 0;
02705 extern double minadjangle;
02706 double maxdot_minang = cos(DEG2RAD*minadjangle);
02707 for(i=0;i<tris.count;i++)if(tris[i])
02708 {
02709 Plane p;
02710 Tri *t = tris[i];
02711 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02712 p.dist = -dot(p.normal, verts[(*t)[0]]);
02713 for(j=0;j<3;j++)
02714 {
02715 if(t->n[j]<t->id) continue;
02716 Tri *s = tris[t->n[j]];
02717 REAL3 snormal = TriNormal(verts[(*s)[0]],verts[(*s)[1]],verts[(*s)[2]]);
02718 if(dot(snormal,p.normal)>=cos(bevangle*DEG2RAD)) continue;
02719 REAL3 e = verts[(*t)[(j+2)%3]] - verts[(*t)[(j+1)%3]];
02720 REAL3 n = (e!=REAL3(0,0,0))? cross(snormal,e)+cross(e,p.normal) : snormal+p.normal;
02721 assert(n!=REAL3(0,0,0));
02722 if(n==REAL3(0,0,0)) return 0;
02723 n=normalize(n);
02724 bplanes.Add(Plane(n,-dot(n,verts[maxdir(verts,verts_count,n)])));
02725 }
02726 }
02727 for(i=0;i<tris.count;i++)if(tris[i])for(j=i+1;j<tris.count;j++)if(tris[i] && tris[j])
02728 {
02729 Tri *ti = tris[i];
02730 Tri *tj = tris[j];
02731 REAL3 ni = TriNormal(verts[(*ti)[0]],verts[(*ti)[1]],verts[(*ti)[2]]);
02732 REAL3 nj = TriNormal(verts[(*tj)[0]],verts[(*tj)[1]],verts[(*tj)[2]]);
02733 if(dot(ni,nj)>maxdot_minang)
02734 {
02735
02736 if( area2(verts[(*ti)[0]],verts[(*ti)[1]],verts[(*ti)[2]]) < area2(verts[(*tj)[0]],verts[(*tj)[1]],verts[(*tj)[2]]))
02737 {
02738 delete tris[i]; tris[i]=NULL;
02739 }
02740 else
02741 {
02742 delete tris[j]; tris[j]=NULL;
02743 }
02744 }
02745 }
02746 for(i=0;i<tris.count;i++)if(tris[i])
02747 {
02748 Plane p;
02749 Tri *t = tris[i];
02750 p.normal = TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
02751 p.dist = -dot(p.normal, verts[(*t)[0]]);
02752 planes.Add(p);
02753 }
02754 for(i=0;i<bplanes.count;i++)
02755 {
02756 for(j=0;j<planes.count;j++)
02757 {
02758 if(dot(bplanes[i].normal,planes[j].normal)>maxdot_minang) break;
02759 }
02760 if(j==planes.count)
02761 {
02762 planes.Add(bplanes[i]);
02763 }
02764 }
02765 for(i=0;i<tris.count;i++)if(tris[i])
02766 {
02767 delete tris[i];
02768 }
02769 tris.count = 0;
02770 return 1;
02771 }
02772
02773 static int overhull(Plane *planes,int planes_count,double3 *verts, int verts_count,int maxplanes,
02774 double3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,double inflate)
02775 {
02776 int i,j;
02777 if(verts_count <4) return NULL;
02778 maxplanes = Min(maxplanes,planes_count);
02779 double3 bmin(verts[0]),bmax(verts[0]);
02780 for(i=0;i<verts_count;i++)
02781 {
02782 bmin = VectorMin(bmin,verts[i]);
02783 bmax = VectorMax(bmax,verts[i]);
02784 }
02785 double diameter = magnitude(bmax-bmin);
02786
02787 bmin -= double3(inflate*2.5f,inflate*2.5f,inflate*2.5f);
02788 bmax += double3(inflate*2.5f,inflate*2.5f,inflate*2.5f);
02789
02790
02791
02792
02793
02794
02795
02796 for(i=0;i<planes_count;i++)
02797 {
02798 planes[i].dist -= inflate;
02799 }
02800 double3 emin = bmin;
02801 double3 emax = bmax;
02802 double epsilon = 0.01f;
02803 planetestepsilon = magnitude(emax-emin) * PAPERWIDTH;
02804
02805
02806 double maxdot_minang = cos(DEG2RAD*minadjangle);
02807 for(j=0;j<6;j++)
02808 {
02809 double3 n(0,0,0);
02810 n[j/2] = (j%2)? 1.0f : -1.0f;
02811 for(i=0;i<planes_count;i++)
02812 {
02813 if(dot(n,planes[i].normal)> maxdot_minang)
02814 {
02815 (*((j%2)?&bmax:&bmin)) += n * (diameter*0.5f);
02816 break;
02817 }
02818 }
02819 }
02820 ConvexH *c = ConvexHMakeCube(REAL3(bmin),REAL3(bmax));
02821 int k;
02822 while(maxplanes-- && (k=candidateplane(planes,planes_count,c,epsilon))>=0)
02823 {
02824 ConvexH *tmp = c;
02825 c = ConvexHCrop(*tmp,planes[k]);
02826 if(c==NULL) {c=tmp; break;}
02827 if(!AssertIntact(*c)) {c=tmp; break;}
02828 delete tmp;
02829 }
02830
02831 assert(AssertIntact(*c));
02832
02833 faces_out = (int*)NX_ALLOC(sizeof(int)*(1+c->facets.count+c->edges.count), CONVEX_TEMP);
02834 faces_count_out=0;
02835 i=0;
02836 faces_out[faces_count_out++]=-1;
02837 k=0;
02838 while(i<c->edges.count)
02839 {
02840 j=1;
02841 while(j+i<c->edges.count && c->edges[i].p==c->edges[i+j].p) { j++; }
02842 faces_out[faces_count_out++]=j;
02843 while(j--)
02844 {
02845 faces_out[faces_count_out++] = c->edges[i].v;
02846 i++;
02847 }
02848 k++;
02849 }
02850 faces_out[0]=k;
02851 assert(k==c->facets.count);
02852 assert(faces_count_out == 1+c->facets.count+c->edges.count);
02853 verts_out = c->vertices.element;
02854 verts_count_out = c->vertices.count;
02855 for(i=0;i<c->vertices.count;i++)
02856 {
02857 verts_out[i] = double3(c->vertices[i]);
02858 }
02859 c->vertices.count=c->vertices.array_size=0; c->vertices.element=NULL;
02860 delete c;
02861 return 1;
02862 }
02863
02864 static int overhullv(double3 *verts, int verts_count,int maxplanes,
02865 double3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,double inflate,double bevangle,int vlimit)
02866 {
02867 if(!verts_count) return 0;
02868 extern int calchullpbev(double3 *verts,int verts_count,int vlimit, Array<Plane> &planes,double bevangle) ;
02869 Array<Plane> planes;
02870 int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ;
02871 if(!rc) return 0;
02872 return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate);
02873 }
02874
02875
02876
02877
02878
02879
02880 bool ComputeHull(unsigned int vcount,const double *vertices,PHullResult &result,unsigned int vlimit,double inflate)
02881 {
02882
02883 int index_count;
02884 int *faces;
02885 double3 *verts_out;
02886 int verts_count_out;
02887
02888 if(inflate==0.0f)
02889 {
02890 int *tris_out;
02891 int tris_count;
02892 int ret = calchull( (double3 *) vertices, (int) vcount, tris_out, tris_count, vlimit );
02893 if(!ret) return false;
02894 result.mIndexCount = (unsigned int) (tris_count*3);
02895 result.mFaceCount = (unsigned int) tris_count;
02896 result.mVertices = (double*) vertices;
02897 result.mVcount = (unsigned int) vcount;
02898 result.mIndices = (unsigned int *) tris_out;
02899 return true;
02900 }
02901
02902 int ret = overhullv((double3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit);
02903 if(!ret) {
02904 tris.SetSize(0);
02905 return false;
02906 }
02907
02908 Array<int3> tris;
02909 int n=faces[0];
02910 int k=1;
02911 for(int i=0;i<n;i++)
02912 {
02913 int pn = faces[k++];
02914 for(int j=2;j<pn;j++) tris.Add(int3(faces[k],faces[k+j-1],faces[k+j]));
02915 k+=pn;
02916 }
02917 assert(tris.count == index_count-1-(n*3));
02918 NX_FREE(faces);
02919
02920 result.mIndexCount = (unsigned int) (tris.count*3);
02921 result.mFaceCount = (unsigned int) tris.count;
02922 result.mVertices = (double*) verts_out;
02923 result.mVcount = (unsigned int) verts_count_out;
02924 result.mIndices = (unsigned int *) tris.element;
02925 tris.element=NULL; tris.count = tris.array_size=0;
02926 ConvexDecomposition::tris.SetSize(0);
02927
02928 return true;
02929 }
02930
02931
02932 void ReleaseHull(PHullResult &result)
02933 {
02934 NX_FREE(result.mIndices);
02935 NX_FREE(result.mVertices);
02936 result.mVcount = 0;
02937 result.mIndexCount = 0;
02938 result.mIndices = 0;
02939 result.mVertices = 0;
02940 result.mIndices = 0;
02941 }
02942
02943
02944
02945
02946
02947
02948 HullError HullLibrary::CreateConvexHull(const HullDesc &desc,
02949 HullResult &result)
02950 {
02951 HullError ret = QE_FAIL;
02952
02953
02954 PHullResult hr;
02955
02956 unsigned int vcount = desc.mVcount;
02957 if ( vcount < 8 ) vcount = 8;
02958
02959 double *vsource = (double *) NX_ALLOC( sizeof(double)*vcount*3, CONVEX_TEMP );
02960
02961
02962 double scale[3];
02963
02964 unsigned int ovcount;
02965
02966 bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale );
02967
02968 if ( ok )
02969 {
02970 double bmin[3];
02971 double bmax[3];
02972
02973
02974 if ( 1 )
02975 {
02976
02977 for (unsigned int i=0; i<ovcount; i++)
02978 {
02979 double *v = &vsource[i*3];
02980 v[0]*=scale[0];
02981 v[1]*=scale[1];
02982 v[2]*=scale[2];
02983
02984 if ( i == 0 )
02985 {
02986 bmin[0] = bmax[0] = v[0];
02987 bmin[1] = bmax[1] = v[1];
02988 bmin[2] = bmax[2] = v[2];
02989 }
02990 else
02991 {
02992 if ( v[0] < bmin[0] ) bmin[0] = v[0];
02993 if ( v[1] < bmin[1] ) bmin[1] = v[1];
02994 if ( v[2] < bmin[2] ) bmin[2] = v[2];
02995 if ( v[0] > bmax[0] ) bmax[0] = v[0];
02996 if ( v[1] > bmax[1] ) bmax[1] = v[1];
02997 if ( v[2] > bmax[2] ) bmax[2] = v[2];
02998 }
02999
03000 }
03001 }
03002
03003 double skinwidth = 0;
03004
03005 if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
03006 {
03007 skinwidth = desc.mSkinWidth;
03008 if ( skinwidth < 0 )
03009 {
03010 double center[3];
03011
03012 center[0] = (bmax[0] - bmin[0])*0.5f + bmin[0];
03013 center[1] = (bmax[1] - bmin[1])*0.5f + bmin[1];
03014 center[2] = (bmax[2] - bmin[2])*0.5f + bmin[2];
03015
03016 double dx = (bmax[0]-bmin[0])*0.5f;
03017 double dy = (bmax[1]-bmin[1])*0.5f;
03018 double dz = (bmax[2]-bmin[2])*0.5f;
03019 double dist = sqrt(dx*dx+dy*dy+dz*dz);
03020
03021 skinwidth*=-1;
03022
03023 double scale = 1.0f - (skinwidth/dist);
03024 if ( scale < 0.3f ) scale = 0.3f;
03025 for (unsigned int i=0; i<ovcount; i++)
03026 {
03027 double *v = &vsource[i*3];
03028
03029 v[0]-=center[0];
03030 v[1]-=center[1];
03031 v[2]-=center[2];
03032
03033 v[0]*=scale;
03034 v[1]*=scale;
03035 v[2]*=scale;
03036
03037 v[0]+=center[0];
03038 v[1]+=center[1];
03039 v[2]+=center[2];
03040 }
03041 skinwidth = 0;
03042 }
03043 }
03044
03045 ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth);
03046
03047 if ( ok )
03048 {
03049
03050
03051 double *vscratch = (double *) NX_ALLOC( sizeof(double)*hr.mVcount*3, CONVEX_TEMP );
03052 BringOutYourDead(hr.mVertices,hr.mVcount, vscratch, ovcount, hr.mIndices, hr.mIndexCount );
03053
03054 ret = QE_OK;
03055
03056 if ( desc.HasHullFlag(QF_TRIANGLES) )
03057 {
03058 result.mPolygons = false;
03059 result.mNumOutputVertices = ovcount;
03060 result.mOutputVertices = (double *)NX_ALLOC( sizeof(double)*ovcount*3, CONVEX_TEMP );
03061 result.mNumFaces = hr.mFaceCount;
03062 result.mNumIndices = hr.mIndexCount;
03063
03064 result.mIndices = (unsigned int *) NX_ALLOC( sizeof(unsigned int)*hr.mIndexCount, CONVEX_TEMP );
03065
03066 memcpy(result.mOutputVertices, vscratch, sizeof(double)*3*ovcount );
03067
03068 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
03069 {
03070
03071 const unsigned int *source = hr.mIndices;
03072 unsigned int *dest = result.mIndices;
03073
03074 for (unsigned int i=0; i<hr.mFaceCount; i++)
03075 {
03076 dest[0] = source[2];
03077 dest[1] = source[1];
03078 dest[2] = source[0];
03079 dest+=3;
03080 source+=3;
03081 }
03082
03083 }
03084 else
03085 {
03086 memcpy(result.mIndices, hr.mIndices, sizeof(unsigned int)*hr.mIndexCount);
03087 }
03088 }
03089 else
03090 {
03091 result.mPolygons = true;
03092 result.mNumOutputVertices = ovcount;
03093 result.mOutputVertices = (double *)NX_ALLOC( sizeof(double)*ovcount*3, CONVEX_TEMP );
03094 result.mNumFaces = hr.mFaceCount;
03095 result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
03096 result.mIndices = (unsigned int *) NX_ALLOC( sizeof(unsigned int)*result.mNumIndices, CONVEX_TEMP );
03097 memcpy(result.mOutputVertices, vscratch, sizeof(double)*3*ovcount );
03098
03099 if ( 1 )
03100 {
03101 const unsigned int *source = hr.mIndices;
03102 unsigned int *dest = result.mIndices;
03103 for (unsigned int i=0; i<hr.mFaceCount; i++)
03104 {
03105 dest[0] = 3;
03106 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
03107 {
03108 dest[1] = source[2];
03109 dest[2] = source[1];
03110 dest[3] = source[0];
03111 }
03112 else
03113 {
03114 dest[1] = source[0];
03115 dest[2] = source[1];
03116 dest[3] = source[2];
03117 }
03118
03119 dest+=4;
03120 source+=3;
03121 }
03122 }
03123 }
03124
03125
03126 if ( hr.mVertices == vsource) vsource = NULL;
03127
03128 ReleaseHull(hr);
03129
03130 if ( vscratch )
03131 {
03132 NX_FREE(vscratch);
03133 }
03134 }
03135 }
03136
03137
03138 if ( vsource )
03139 {
03140 NX_FREE(vsource);
03141 }
03142
03143
03144 return ret;
03145 }
03146
03147
03148
03149 HullError HullLibrary::ReleaseResult(HullResult &result)
03150 {
03151 if ( result.mOutputVertices )
03152 {
03153 NX_FREE(result.mOutputVertices);
03154 result.mOutputVertices = 0;
03155 }
03156 if ( result.mIndices )
03157 {
03158 NX_FREE(result.mIndices);
03159 result.mIndices = 0;
03160 }
03161 return QE_OK;
03162 }
03163
03164
03165 static void AddPoint(unsigned int &vcount,double *p,double x,double y,double z)
03166 {
03167 double *dest = &p[vcount*3];
03168 dest[0] = x;
03169 dest[1] = y;
03170 dest[2] = z;
03171 vcount++;
03172 }
03173
03174
03175 double GetDist(double px,double py,double pz,const double *p2)
03176 {
03177
03178 double dx = px - p2[0];
03179 double dy = py - p2[1];
03180 double dz = pz - p2[2];
03181
03182 return dx*dx+dy*dy+dz*dz;
03183 }
03184
03185
03186
03187 bool HullLibrary::CleanupVertices(unsigned int svcount,
03188 const double *svertices,
03189 unsigned int stride,
03190 unsigned int &vcount,
03191 double *vertices,
03192 double normalepsilon,
03193 double *scale)
03194 {
03195 if ( svcount == 0 ) return false;
03196
03197
03198 #define EPSILON 0.000001f // close enough to consider two doubleing point numbers to be 'the same'.
03199
03200 bool ret = false;
03201
03202 vcount = 0;
03203
03204 double recip[3];
03205
03206 if ( scale )
03207 {
03208 scale[0] = 1;
03209 scale[1] = 1;
03210 scale[2] = 1;
03211 }
03212
03213 double bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
03214 double bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
03215
03216 const char *vtx = (const char *) svertices;
03217
03218 if ( 1 )
03219 {
03220 for (unsigned int i=0; i<svcount; i++)
03221 {
03222 const double *p = (const double *) vtx;
03223
03224 vtx+=stride;
03225
03226 for (int j=0; j<3; j++)
03227 {
03228 if ( p[j] < bmin[j] ) bmin[j] = p[j];
03229 if ( p[j] > bmax[j] ) bmax[j] = p[j];
03230 }
03231 }
03232 }
03233
03234 double dx = bmax[0] - bmin[0];
03235 double dy = bmax[1] - bmin[1];
03236 double dz = bmax[2] - bmin[2];
03237
03238 double center[3];
03239
03240 center[0] = dx*0.5f + bmin[0];
03241 center[1] = dy*0.5f + bmin[1];
03242 center[2] = dz*0.5f + bmin[2];
03243
03244 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
03245 {
03246
03247 double len = FLT_MAX;
03248
03249 if ( dx > EPSILON && dx < len ) len = dx;
03250 if ( dy > EPSILON && dy < len ) len = dy;
03251 if ( dz > EPSILON && dz < len ) len = dz;
03252
03253 if ( len == FLT_MAX )
03254 {
03255 dx = dy = dz = 0.01f;
03256 }
03257 else
03258 {
03259 if ( dx < EPSILON ) dx = len * 0.05f;
03260 if ( dy < EPSILON ) dy = len * 0.05f;
03261 if ( dz < EPSILON ) dz = len * 0.05f;
03262 }
03263
03264 double x1 = center[0] - dx;
03265 double x2 = center[0] + dx;
03266
03267 double y1 = center[1] - dy;
03268 double y2 = center[1] + dy;
03269
03270 double z1 = center[2] - dz;
03271 double z2 = center[2] + dz;
03272
03273 AddPoint(vcount,vertices,x1,y1,z1);
03274 AddPoint(vcount,vertices,x2,y1,z1);
03275 AddPoint(vcount,vertices,x2,y2,z1);
03276 AddPoint(vcount,vertices,x1,y2,z1);
03277 AddPoint(vcount,vertices,x1,y1,z2);
03278 AddPoint(vcount,vertices,x2,y1,z2);
03279 AddPoint(vcount,vertices,x2,y2,z2);
03280 AddPoint(vcount,vertices,x1,y2,z2);
03281
03282 return true;
03283
03284
03285 }
03286 else
03287 {
03288 if ( scale )
03289 {
03290 scale[0] = dx;
03291 scale[1] = dy;
03292 scale[2] = dz;
03293
03294 recip[0] = 1 / dx;
03295 recip[1] = 1 / dy;
03296 recip[2] = 1 / dz;
03297
03298 center[0]*=recip[0];
03299 center[1]*=recip[1];
03300 center[2]*=recip[2];
03301
03302 }
03303
03304 }
03305
03306
03307
03308 vtx = (const char *) svertices;
03309
03310 for (unsigned int i=0; i<svcount; i++)
03311 {
03312
03313 const double *p = (const double *)vtx;
03314 vtx+=stride;
03315
03316 double px = p[0];
03317 double py = p[1];
03318 double pz = p[2];
03319
03320 if ( scale )
03321 {
03322 px = px*recip[0];
03323 py = py*recip[1];
03324 pz = pz*recip[2];
03325 }
03326
03327 if ( 1 )
03328 {
03329 unsigned int j;
03330
03331 for (j=0; j<vcount; j++)
03332 {
03333 double *v = &vertices[j*3];
03334
03335 double x = v[0];
03336 double y = v[1];
03337 double z = v[2];
03338
03339 double dx = fabs(x - px );
03340 double dy = fabs(y - py );
03341 double dz = fabs(z - pz );
03342
03343 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
03344 {
03345
03346
03347
03348
03349 double dist1 = GetDist(px,py,pz,center);
03350 double dist2 = GetDist(v[0],v[1],v[2],center);
03351
03352 if ( dist1 > dist2 )
03353 {
03354 v[0] = px;
03355 v[1] = py;
03356 v[2] = pz;
03357 }
03358
03359 break;
03360 }
03361 }
03362
03363 if ( j == vcount )
03364 {
03365 double *dest = &vertices[vcount*3];
03366 dest[0] = px;
03367 dest[1] = py;
03368 dest[2] = pz;
03369 vcount++;
03370 }
03371 }
03372 }
03373
03374
03375 if ( 1 )
03376 {
03377 double bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
03378 double bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
03379
03380 for (unsigned int i=0; i<vcount; i++)
03381 {
03382 const double *p = &vertices[i*3];
03383 for (int j=0; j<3; j++)
03384 {
03385 if ( p[j] < bmin[j] ) bmin[j] = p[j];
03386 if ( p[j] > bmax[j] ) bmax[j] = p[j];
03387 }
03388 }
03389
03390 double dx = bmax[0] - bmin[0];
03391 double dy = bmax[1] - bmin[1];
03392 double dz = bmax[2] - bmin[2];
03393
03394 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
03395 {
03396 double cx = dx*0.5f + bmin[0];
03397 double cy = dy*0.5f + bmin[1];
03398 double cz = dz*0.5f + bmin[2];
03399
03400 double len = FLT_MAX;
03401
03402 if ( dx >= EPSILON && dx < len ) len = dx;
03403 if ( dy >= EPSILON && dy < len ) len = dy;
03404 if ( dz >= EPSILON && dz < len ) len = dz;
03405
03406 if ( len == FLT_MAX )
03407 {
03408 dx = dy = dz = 0.01f;
03409 }
03410 else
03411 {
03412 if ( dx < EPSILON ) dx = len * 0.05f;
03413 if ( dy < EPSILON ) dy = len * 0.05f;
03414 if ( dz < EPSILON ) dz = len * 0.05f;
03415 }
03416
03417 double x1 = cx - dx;
03418 double x2 = cx + dx;
03419
03420 double y1 = cy - dy;
03421 double y2 = cy + dy;
03422
03423 double z1 = cz - dz;
03424 double z2 = cz + dz;
03425
03426 vcount = 0;
03427
03428 AddPoint(vcount,vertices,x1,y1,z1);
03429 AddPoint(vcount,vertices,x2,y1,z1);
03430 AddPoint(vcount,vertices,x2,y2,z1);
03431 AddPoint(vcount,vertices,x1,y2,z1);
03432 AddPoint(vcount,vertices,x1,y1,z2);
03433 AddPoint(vcount,vertices,x2,y1,z2);
03434 AddPoint(vcount,vertices,x2,y2,z2);
03435 AddPoint(vcount,vertices,x1,y2,z2);
03436
03437 return true;
03438 }
03439 }
03440
03441 return true;
03442 }
03443
03444 void HullLibrary::BringOutYourDead(const double *verts,unsigned int vcount, double *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
03445 {
03446 unsigned int *used = (unsigned int *)NX_ALLOC(sizeof(unsigned int)*vcount, CONVEX_TEMP );
03447 memset(used,0,sizeof(unsigned int)*vcount);
03448
03449 ocount = 0;
03450
03451 for (unsigned int i=0; i<indexcount; i++)
03452 {
03453 unsigned int v = indices[i];
03454
03455 assert( v >= 0 && v < vcount );
03456
03457 if ( used[v] )
03458 {
03459 indices[i] = used[v]-1;
03460 }
03461 else
03462 {
03463
03464 indices[i] = ocount;
03465
03466 overts[ocount*3+0] = verts[v*3+0];
03467 overts[ocount*3+1] = verts[v*3+1];
03468 overts[ocount*3+2] = verts[v*3+2];
03469
03470 ocount++;
03471
03472 assert( ocount >=0 && ocount <= vcount );
03473
03474 used[v] = ocount;
03475 }
03476 }
03477
03478 NX_FREE(used);
03479 }
03480
03481
03482
03483 HullError HullLibrary::CreateTriangleMesh(HullResult &answer,ConvexHullTriangleInterface *iface)
03484 {
03485 HullError ret = QE_FAIL;
03486
03487
03488 const double *p = answer.mOutputVertices;
03489 const unsigned int *idx = answer.mIndices;
03490 unsigned int fcount = answer.mNumFaces;
03491
03492 if ( p && idx && fcount )
03493 {
03494 ret = QE_OK;
03495
03496 for (unsigned int i=0; i<fcount; i++)
03497 {
03498 unsigned int pcount = *idx++;
03499
03500 unsigned int i1 = *idx++;
03501 unsigned int i2 = *idx++;
03502 unsigned int i3 = *idx++;
03503
03504 const double *p1 = &p[i1*3];
03505 const double *p2 = &p[i2*3];
03506 const double *p3 = &p[i3*3];
03507
03508 AddConvexTriangle(iface,p1,p2,p3);
03509
03510 pcount-=3;
03511 while ( pcount )
03512 {
03513 i3 = *idx++;
03514 p2 = p3;
03515 p3 = &p[i3*3];
03516
03517 AddConvexTriangle(iface,p1,p2,p3);
03518 pcount--;
03519 }
03520
03521 }
03522 }
03523
03524 return ret;
03525 }
03526
03527
03528 void HullLibrary::AddConvexTriangle(ConvexHullTriangleInterface *callback,const double *p1,const double *p2,const double *p3)
03529 {
03530 ConvexHullVertex v1,v2,v3;
03531
03532 #define TSCALE1 (1.0f/4.0f)
03533
03534 v1.mPos[0] = p1[0];
03535 v1.mPos[1] = p1[1];
03536 v1.mPos[2] = p1[2];
03537
03538 v2.mPos[0] = p2[0];
03539 v2.mPos[1] = p2[1];
03540 v2.mPos[2] = p2[2];
03541
03542 v3.mPos[0] = p3[0];
03543 v3.mPos[1] = p3[1];
03544 v3.mPos[2] = p3[2];
03545
03546 double n[3];
03547 ComputeNormal(n,p1,p2,p3);
03548
03549 v1.mNormal[0] = n[0];
03550 v1.mNormal[1] = n[1];
03551 v1.mNormal[2] = n[2];
03552
03553 v2.mNormal[0] = n[0];
03554 v2.mNormal[1] = n[1];
03555 v2.mNormal[2] = n[2];
03556
03557 v3.mNormal[0] = n[0];
03558 v3.mNormal[1] = n[1];
03559 v3.mNormal[2] = n[2];
03560
03561 const double *tp1 = p1;
03562 const double *tp2 = p2;
03563 const double *tp3 = p3;
03564
03565 int i1 = 0;
03566 int i2 = 0;
03567
03568 double nx = fabs(n[0]);
03569 double ny = fabs(n[1]);
03570 double nz = fabs(n[2]);
03571
03572 if ( nx <= ny && nx <= nz )
03573 i1 = 0;
03574 if ( ny <= nx && ny <= nz )
03575 i1 = 1;
03576 if ( nz <= nx && nz <= ny )
03577 i1 = 2;
03578
03579 switch ( i1 )
03580 {
03581 case 0:
03582 if ( ny < nz )
03583 i2 = 1;
03584 else
03585 i2 = 2;
03586 break;
03587 case 1:
03588 if ( nx < nz )
03589 i2 = 0;
03590 else
03591 i2 = 2;
03592 break;
03593 case 2:
03594 if ( nx < ny )
03595 i2 = 0;
03596 else
03597 i2 = 1;
03598 break;
03599 }
03600
03601 v1.mTexel[0] = tp1[i1]*TSCALE1;
03602 v1.mTexel[1] = tp1[i2]*TSCALE1;
03603
03604 v2.mTexel[0] = tp2[i1]*TSCALE1;
03605 v2.mTexel[1] = tp2[i2]*TSCALE1;
03606
03607 v3.mTexel[0] = tp3[i1]*TSCALE1;
03608 v3.mTexel[1] = tp3[i2]*TSCALE1;
03609
03610 callback->ConvexHullTriangle(v3,v2,v1);
03611 }
03612
03613
03614 double HullLibrary::ComputeNormal(double *n,const double *A,const double *B,const double *C)
03615 {
03616 double vx,vy,vz,wx,wy,wz,vw_x,vw_y,vw_z,mag;
03617
03618 vx = (B[0] - C[0]);
03619 vy = (B[1] - C[1]);
03620 vz = (B[2] - C[2]);
03621
03622 wx = (A[0] - B[0]);
03623 wy = (A[1] - B[1]);
03624 wz = (A[2] - B[2]);
03625
03626 vw_x = vy * wz - vz * wy;
03627 vw_y = vz * wx - vx * wz;
03628 vw_z = vx * wy - vy * wx;
03629
03630 mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
03631
03632 if ( mag < 0.000001f )
03633 {
03634 mag = 0;
03635 }
03636 else
03637 {
03638 mag = 1.0f/mag;
03639 }
03640
03641 n[0] = vw_x * mag;
03642 n[1] = vw_y * mag;
03643 n[2] = vw_z * mag;
03644
03645 return mag;
03646 }
03647
03648
03649
03650 };