$search
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 //*** DARRAY.H 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 // operator ArrayRet<Type> &() { return *(ArrayRet<Type> *)this;} // this worked but i suspect could be dangerous 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); // assert object t is in the array. 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); // to allocate space 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 //** VECMATH.H 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 //-------- 2D -------- 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 //--------- 3D --------- 00378 00379 class double3 // 3D 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 //operator double *() { return &x;}; 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 // due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb. 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; // the 3 rows of the Matrix 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); // its just 3x3 so we simply do that cofactor method 00448 00449 00450 //-------- 4D Math -------- 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 //operator double *() { return &x;}; 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; // the 4 rows 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); // Turns a 3D double3 4D vector4 by appending w 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 //-------- Quaternion ------------ 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 ); // returns quat q where q*v0=v1 00537 Quaternion Inverse(const Quaternion &q); 00538 double4x4 MatrixFromQuatVec(const Quaternion &q, const double3 &v); 00539 00540 00541 //------ Euler Angle ----- 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 //------- Plane ---------- 00552 00553 class Plane 00554 { 00555 public: 00556 double3 normal; 00557 double dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 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 //--------- Utility Functions ------ 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); // projects a onto infinite line p0p1 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 // ** VECMATH.CPP 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 //------------ double3 (3D) -------------- 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 // this routine, normalize, is ok, provided magnitude works!! 00734 double d=magnitude(v); 00735 if (d==0) 00736 { 00737 printf("Cant normalize ZERO vector\n"); 00738 assert(0);// yes this could go here 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 // the statement v1*v2 is ambiguous since there are 3 types 00775 // of vector multiplication 00776 // - componantwise (for example combining colors) 00777 // - dot product 00778 // - cross product 00779 // Therefore we never declare/implement this function. 00780 // So we will never see: double3 operator*(double3 a,double3 b) 00781 00782 00783 00784 00785 //------------ double3x3 --------------- 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 // reverse indexs i&j to take transpose 00806 b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d; 00807 } 00808 } 00809 // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it) 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 //--------------- 4D ---------------- 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; // yes this actually works 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 // Dont implement m*v for now, since that might confuse us 00900 // All our transforms are based on multiplying the "row" vector on the left 00901 //double4 operator*(const double4x4& m , const double4& v ) 00902 //{ 00903 // return double4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w)); 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); // view space height 00972 double w = h / aspect ; // view space width 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]; /* temp array for pairs */ 01028 double src[16]; /* array of transpose source matrix */ 01029 double det; /* determinant */ 01030 /* transpose matrix */ 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 /* calculate pairs for first 8 elements (cofactors) */ 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 /* calculate first 8 elements (cofactors) */ 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 /* calculate pairs for second 8 elements (cofactors) */ 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 /* calculate second 8 elements (cofactors) */ 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 /* calculate determinant */ 01098 det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3]; 01099 /* calculate matrix inverse */ 01100 det = 1/det; 01101 for ( int j = 0; j < 16; j++) 01102 dst[j] *= det; 01103 return d; 01104 } 01105 01106 01107 //--------- Quaternion -------------- 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 // The following is equivalent to: 01152 //return (q.getmatrix() * v); 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); // must multiply with the quat on the left 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 //------------- Plane -------------- 01262 01263 01264 void Plane::Transform(const double3 &position, const Quaternion &orientation) { 01265 // Transforms the plane to the space defined by the 01266 // given position/orientation. 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 //--------- utility functions ------------- 01281 01282 // RotationArc() 01283 // Given two vectors v0 and v1 this function 01284 // returns quaternion q where q*v0==v1. 01285 // Routine taken from game programming gems. 01286 Quaternion RotationArc(double3 v0,double3 v1){ 01287 static Quaternion q; 01288 v0 = normalize(v0); // Comment these two lines out if you know its not needed. 01289 v1 = normalize(v1); // If vector is already unit length then why do it again? 01290 double3 c = cross(v0,v1); 01291 double d = dot(v0,v1); 01292 if(d<=-1.0f) { return Quaternion(1,0,0,0);} // 180 about x axis 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 // builds a 4x4 transformation matrix based on orientation q and translation v 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 // returns the point where the line p0-p1 intersects the plane n&d 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 // return the normal of the triangle 01373 // inscribed by v0, v1, and v2 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 //v.x = bmin.x; 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 //v.x = bmax.x; 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 //v.y = bmin.y; 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 // vy = bmax.y; 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 // v.z = bmin.z; 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 // v.z = bmax.z; 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 // routine taken from game programming gems. 01514 // Implement track ball functionality to spin stuf on the screen 01515 // cop center of projection 01516 // cor center of rotation 01517 // dir1 old mouse direction 01518 // dir2 new mouse direction 01519 // pretend there is a sphere around cor. Then find the points 01520 // where dir1 and dir2 intersect that sphere. Find the 01521 // rotation that takes the first point to the second. 01522 double m; 01523 // compute plane 01524 double3 nrml = cor - cop; 01525 double fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop 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 // By using the cached plane distances d0 and d1 01584 // we can optimize the following: 01585 // the_point = planelineintersection(nrml,dist,v0,v1); 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 // let inside = 0 if outside 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 // HULL.H source code goes here 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 // HULL.cpp source code goes here 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; // the other half of the edge (index into edges list) 01662 unsigned char v; // the vertex at the start of this edge (index into vertices list) 01663 unsigned char p; // the facet on which this edge lies (index into facets list) 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; // i sliced tangent to an edge and created 2 meaningless edges 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 //assert(dot(localnormal,convex.facets[convex.edges[i].p].normal)>0);//Commented out on Stan Melax' advice 01781 if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0; 01782 } 01783 return 1; 01784 } 01785 01786 // back to back quads 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; // existing vertex members of convex that are coplanar 01900 vertscoplanar.count=0; 01901 static Array<int> edgesplit; // existing edges that members of convex that cross the splitplane 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 // do the side-of-plane tests 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 // ? vertscoplanar.Add(i); 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; // for debugging purposes 01930 } 01931 } 01932 int vertcountunderold = vertcountunder; // for debugging only 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 //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { 01961 // assert(ecop==-1); 01962 // ecop=e; 01963 //} 01964 01965 01966 if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){ 01967 // both endpoints over plane 01968 edgeflag[e0].undermap = -1; 01969 } 01970 else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER) { 01971 // at least one endpoint under, the other coplanar or under 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 // connect the neighbors 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 // both endpoints coplanar 01986 // must check a 3rd point to see if UNDER 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 // make sure this edge is added to the "coplanar" list 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 // first is under 2nd is over 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 // connect the neighbors 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 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); 02027 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); 02028 vout = vertcountunder++; 02029 } 02030 under_edge_count++; 02032 // wheter or not we know v-in yet. ok i;ll try this now: 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 // we previously processed an edge where we came under 02041 // now we know about vout as well 02042 02043 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! 02044 } 02045 02046 } 02047 else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) { 02048 // first is coplanar 2nd is over 02049 02050 edgeflag[e0].undermap = -1; 02051 vout = vertflag[edge0.v].undermap; 02052 // I hate this but i have to make sure part of this face is UNDER before ouputting this vert 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; // hmmm should make a note of the edge # for later on 02064 under_edge_count++; 02065 02066 } 02067 } 02068 else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == UNDER) { 02069 // first is over next is under 02070 // new vertex!!! 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 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); 02077 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); 02078 vin = vertcountunder++; 02079 } 02080 else { 02081 // find the new vertex that was created by edge[edge0.ea] 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); // for debugging only 02087 } 02088 if(vout!=-1) { 02089 // we previously processed an edge where we went over 02090 // now we know vin too 02091 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! 02092 } 02093 // output edge 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 // connect the neighbors 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 // first is over next is coplanar 02108 02109 edgeflag[e0].undermap = -1; 02110 vin = vertflag[edge1.v].undermap; 02111 if (vin==-1) return NULL; 02112 if(vout!=-1) { 02113 // we previously processed an edge where we came under 02114 // now we know both endpoints 02115 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! 02116 } 02117 02118 } 02119 else { 02120 assert(0); 02121 } 02122 02123 02124 e0=e1; 02125 e1++; // do the modulo at the beginning of the loop 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 // add the new plane to the mix: 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 // assert(j<coplanaredges_num); 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; // in degrees - result wont have two adjacent facets within this angle of each other. 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; // shouldn't happen. 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; // so this plane wont get selected. 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) // Yuck - this is really ugly 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); // 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; // djs: For heaven's sake!!!! 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) // Almost collinear or otherwise very close to each other 02537 return false; 02538 double result = dot(normalize(result3), verts[p3]-verts[p0]); 02539 return (result > VOLUME_EPSILON || result < -VOLUME_EPSILON); // Returns true iff volume is significantly non-zero 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; // simplex failed 02592 02593 02594 02595 double3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // a valid interior point 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]); // wtf we've already done this vertex 02626 isextreme[v]=1; 02627 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already 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 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle 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; // already done that vertex - algorithm needs to be able to terminate. 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 // please reset here, otherwise, we get a nice virtual function call (R6025) error with NxCooking library 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; // default is 3.0f; // in degrees - result wont have two adjacent facets within this angle of each other. 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 // somebody has to die, keep the biggest triangle 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; //bad place to do the tris.SetSize(0) fix, this line is executed many times, and will result in a whole lot of allocations if the array is totally cleared here 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 // inflate *=diameter; // RELATIVE INFLATION 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 // 2 is from the formula: 02790 // D = d*|n1+n2|/(1-n1 dot n2), where d is "inflate" and 02791 // n1 and n2 are the normals of two planes at bevelAngle to each other 02792 // for 120 degrees, D is 2d 02793 02794 //bmin -= double3(inflate,inflate,inflate); 02795 //bmax += double3(inflate,inflate,inflate); 02796 for(i=0;i<planes_count;i++) 02797 { 02798 planes[i].dist -= inflate; 02799 } 02800 double3 emin = bmin; // VectorMin(bmin,double3(0,0,0)); 02801 double3 emax = bmax; // VectorMax(bmax,double3(0,0,0)); 02802 double epsilon = 0.01f; // size of object is taken into account within candidate plane function. Used to multiply here by magnitude(emax-emin) 02803 planetestepsilon = magnitude(emax-emin) * PAPERWIDTH; 02804 // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. 02805 // ConvexH *convex = ConvexHMakeCube(bmin - double3(diameter,diameter,diameter),bmax+double3(diameter,diameter,diameter)); 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;} // might want to debug this case better!!! 02827 if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!! 02828 delete tmp; 02829 } 02830 02831 assert(AssertIntact(*c)); 02832 //return c; 02833 faces_out = (int*)NX_ALLOC(sizeof(int)*(1+c->facets.count+c->edges.count), CONVEX_TEMP); // new int[1+c->facets.count+c->edges.count]; 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; // number of faces. 02851 assert(k==c->facets.count); 02852 assert(faces_count_out == 1+c->facets.count+c->edges.count); 02853 verts_out = c->vertices.element; // new double3[c->vertices.count]; 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); //have to set the size to 0 in order to protect from a "pure virtual function call" problem 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); // PT: I added that. Is it ok ? 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); //have to set the size to 0 in order to protect from a "pure virtual function call" problem 02927 02928 return true; 02929 } 02930 02931 02932 void ReleaseHull(PHullResult &result) 02933 { 02934 NX_FREE(result.mIndices); // PT: I added that. Is it ok ? 02935 NX_FREE(result.mVertices); // PT: I added that. Is it ok ? 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 //****** HULLLIB source code 02946 02947 02948 HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request 02949 HullResult &result) // contains the resulst 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 ); // normalize point cloud, remove duplicates! 02967 02968 if ( ok ) 02969 { 02970 double bmin[3]; 02971 double bmax[3]; 02972 02973 02974 if ( 1 ) // scale vertices back to their original size. 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 ) // if it is a negative skinwidth we shrink the hull points relative to the center. 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; // make it positive... 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 // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. 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) ) // if he wants the results as triangle! 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 // ReleaseHull frees memory for hr.mVertices, which can be the 03125 // same pointer as vsource, so be sure to set it to NULL if necessary 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 // this pointer is usually freed in ReleaseHull() 03138 if ( vsource ) 03139 { 03140 NX_FREE(vsource); 03141 } 03142 03143 03144 return ret; 03145 } 03146 03147 03148 03149 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. 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, // output number of vertices 03191 double *vertices, // location to store the results. 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; // one centimeter 03256 } 03257 else 03258 { 03259 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. 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; // return cube 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]; // normalize 03323 py = py*recip[1]; // normalize 03324 pz = pz*recip[2]; // normalize 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 // ok, it is close enough to the old one 03346 // now let us see if it is further from the center of the point cloud than the one we already recorded. 03347 // in which case we keep this one instead. 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 // ok..now make sure we didn't prune so many vertices it is now invalid. 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; // one centimeter 03409 } 03410 else 03411 { 03412 if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. 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; // add box 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]; // original array index 03454 03455 assert( v >= 0 && v < vcount ); 03456 03457 if ( used[v] ) // if already remapped 03458 { 03459 indices[i] = used[v]-1; // index to new array 03460 } 03461 else 03462 { 03463 03464 indices[i] = ocount; // new index mapping 03465 03466 overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array 03467 overts[ocount*3+1] = verts[v*3+1]; 03468 overts[ocount*3+2] = verts[v*3+2]; 03469 03470 ocount++; // increment output vert count 03471 03472 assert( ocount >=0 && ocount <= vcount ); 03473 03474 used[v] = ocount; // assign new index remapping 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 };