$search
00001 #ifndef TRIUTIL_H 00002 #define TRIUTIL_H 00003 /* 00004 Szymon Rusinkiewicz 00005 Stanford Graphics Lab 00006 00007 triutil.h 00008 Various utility functions for dealing with vectors, triangles, and quaternions 00009 */ 00010 00011 #include <math.h> 00012 #include <string.h> 00013 #include <algorithm> 00014 using std::min; 00015 using std::max; 00016 using std::swap; 00017 00018 #define TRACKBALL_R 0.8f 00019 00020 namespace trimesh { 00021 00022 00023 #ifdef WIN32 00024 static double drand48() 00025 { 00026 return rand() / RAND_MAX; 00027 } 00028 #endif 00029 00030 // Utility functions for square and cube 00031 template <class T> 00032 static inline T sqr(const T &x) 00033 { 00034 return x*x; 00035 } 00036 00037 template <class T> 00038 static inline T cube(const T &x) 00039 { 00040 return x*x*x; 00041 } 00042 00043 00044 // Dot product of two 3-vectors 00045 template <class T> 00046 static inline T Dot(const T *x, const T *y) 00047 { 00048 return x[0]*y[0] + x[1]*y[1] + x[2]*y[2]; 00049 } 00050 00051 00052 // Squared length of a 3-vector 00053 template <class T> 00054 static inline T Len2(const T *x) 00055 { 00056 return sqr(x[0]) + sqr(x[1]) + sqr(x[2]); 00057 } 00058 00059 00060 // Length of a 3-vector 00061 template <class T> 00062 static inline T Len(const T *x) 00063 { 00064 return sqrt(Len2(x)); 00065 } 00066 00067 00068 // Squared distance between two points in 3-space 00069 template <class T> 00070 static inline T Dist2(const T *x1, const T *x2) 00071 { 00072 T dx = x2[0] - x1[0]; 00073 T dy = x2[1] - x1[1]; 00074 T dz = x2[2] - x1[2]; 00075 00076 return sqr(dx) + sqr(dy) + sqr(dz); 00077 } 00078 00079 00080 // Distance between two points in 3-space 00081 template <class T> 00082 static inline T Dist(const T *x1, const T *x2) 00083 { 00084 return sqrt(Dist2(x1,x2)); 00085 } 00086 00087 00088 // (a,b,c) cross (d,e,f) -> (g,h,i) */ 00089 template <class T> 00090 static inline void CrossProd(const T &a, const T &b, const T &c, 00091 const T &d, const T &e, const T &f, 00092 T &g, T &h, T &i) 00093 { 00094 g = b*f-c*e; 00095 h = c*d-a*f; 00096 i = a*e-b*d; 00097 } 00098 00099 00100 // x cross y -> z 00101 template <class T> 00102 static inline void CrossProd(const T *x, const T *y, T *z) 00103 { 00104 z[0] = x[1]*y[2] - x[2]*y[1]; 00105 z[1] = x[2]*y[0] - x[0]*y[2]; 00106 z[2] = x[0]*y[1] - x[1]*y[0]; 00107 } 00108 00109 00110 // Normal of a triangle with vertices p1, p2, p3 00111 // NOTE: not unit-length! 00112 template <class T> 00113 static inline void FindNormal(const T *p1, const T *p2, 00114 const T *p3, T *n) 00115 { 00116 T u1 = p2[0]-p1[0], u2 = p2[1]-p1[1], u3 = p2[2]-p1[2]; 00117 T v1 = p3[0]-p1[0], v2 = p3[1]-p1[1], v3 = p3[2]-p1[2]; 00118 00119 CrossProd(u1,u2,u3, v1,v2,v3, n[0], n[1], n[2]); 00120 } 00121 00122 00123 // Normalize a 3-vector (make it unit length) 00124 template <class T> 00125 static inline void Normalize(T *v) 00126 { 00127 T x = Len(v); 00128 if (x == T(0)) { 00129 v[0] = v[1] = T(0); 00130 v[2] = T(1); 00131 return; 00132 } 00133 00134 x = T(1) / x; 00135 v[0] *= x; 00136 v[1] *= x; 00137 v[2] *= x; 00138 } 00139 00140 00141 // Multiply matrices in OpenGL (column-major) order 00142 template <class T> 00143 static inline void MMult(const T *M1, 00144 const T *M2, 00145 T *Mout) 00146 { 00147 Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; 00148 Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; 00149 Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; 00150 Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; 00151 Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; 00152 Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; 00153 Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; 00154 Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; 00155 Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; 00156 Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; 00157 Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; 00158 Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; 00159 Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; 00160 Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; 00161 Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; 00162 Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; 00163 } 00164 00165 00166 // Multiply a vector by a matrix 00167 template <class T> 00168 static inline void MVMult(const T *M, 00169 const T *V, 00170 T *Vout, 00171 bool apply_trans = true) 00172 { 00173 Vout[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2]; 00174 Vout[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2]; 00175 Vout[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2]; 00176 if (apply_trans) { 00177 Vout[0] += M[12]; 00178 Vout[1] += M[13]; 00179 Vout[2] += M[14]; 00180 } 00181 } 00182 00183 00184 // Basically a replacement for gluProject, with an additional z offset 00185 template <class T> 00186 static inline void Project(const T *P, 00187 const T *M, 00188 const T *V, 00189 const T *vert, 00190 T *coord, 00191 T zoffset=0) 00192 { 00193 T x = vert[0]; 00194 T y = vert[1]; 00195 T z = vert[2]; 00196 00197 T x1 = M[0]*x+M[4]*y+M[8]*z+M[12]; 00198 T y1 = M[1]*x+M[5]*y+M[9]*z+M[13]; 00199 T z1 = M[2]*x+M[6]*y+M[10]*z+M[14]; 00200 T w1 = M[3]*x+M[7]*y+M[11]*z+M[15]; 00201 00202 z1 += zoffset*w1; 00203 00204 T x2 = P[0]*x1+P[4]*y1+P[8]*z1+P[12]*w1; 00205 T y2 = P[1]*x1+P[5]*y1+P[9]*z1+P[13]*w1; 00206 T z2 = P[2]*x1+P[6]*y1+P[10]*z1+P[14]*w1; 00207 T w2 = P[3]*x1+P[7]*y1+P[11]*z1+P[15]*w1; 00208 00209 coord[0] = V[0] + V[2] * T(0.5) * (x2/w2 + T(1)); 00210 coord[1] = V[1] + V[3] * T(0.5) * (y2/w2 + T(1)); 00211 coord[2] = T(0.5) * (z2/w2 + T(1)); 00212 } 00213 00214 00215 // Precompute some quantities for FastProject 00216 template <class T> 00217 static inline void FastProjectPrecompute(T *F, 00218 const T *P, 00219 const T *M, 00220 const T *V, 00221 T zoffset=0) 00222 { 00223 T tmp[16]; 00224 memcpy(tmp, M, 16*sizeof(T)); 00225 00226 if (zoffset) { 00227 tmp[ 2] += zoffset * tmp[ 3]; 00228 tmp[ 6] += zoffset * tmp[ 7]; 00229 tmp[10] += zoffset * tmp[11]; 00230 tmp[14] += zoffset * tmp[15]; 00231 } 00232 00233 MMult(P, tmp, F); 00234 00235 F[ 0] = T(0.5) * V[2] * (F[ 0]+F[ 3]) + V[0] * F[ 3]; 00236 F[ 4] = T(0.5) * V[2] * (F[ 4]+F[ 7]) + V[0] * F[ 7]; 00237 F[ 8] = T(0.5) * V[2] * (F[ 8]+F[11]) + V[0] * F[11]; 00238 F[12] = T(0.5) * V[2] * (F[12]+F[15]) + V[0] * F[15]; 00239 F[ 1] = T(0.5) * V[3] * (F[ 1]+F[ 3]) + V[1] * F[ 3]; 00240 F[ 5] = T(0.5) * V[3] * (F[ 5]+F[ 7]) + V[1] * F[ 7]; 00241 F[ 9] = T(0.5) * V[3] * (F[ 9]+F[11]) + V[1] * F[11]; 00242 F[13] = T(0.5) * V[3] * (F[13]+F[15]) + V[1] * F[15]; 00243 F[ 2] = T(0.5) * (F[ 2]+F[ 3]); 00244 F[ 6] = T(0.5) * (F[ 6]+F[ 7]); 00245 F[10] = T(0.5) * (F[10]+F[11]); 00246 F[14] = T(0.5) * (F[14]+F[15]); 00247 } 00248 00249 00250 // Same as Project (above), but faster. Uses the "F" precomputed 00251 // by FastProjectPrecompute 00252 template <class T> 00253 static inline void FastProject(const T *F, 00254 T x, T y, T z, 00255 T &xout, T &yout, T &zout) 00256 { 00257 T w_rec = T(1) / (F[ 3]*x + F[ 7]*y + F[11]*z + F[15]); 00258 xout = w_rec * (F[ 0]*x + F[ 4]*y + F[ 8]*z + F[12]); 00259 yout = w_rec * (F[ 1]*x + F[ 5]*y + F[ 9]*z + F[13]); 00260 zout = w_rec * (F[ 2]*x + F[ 6]*y + F[10]*z + F[14]); 00261 } 00262 00263 template <class T> 00264 static inline void FastProject(const T *F, 00265 const T *vert, 00266 T *coord) 00267 { 00268 FastProject(F, vert[0], vert[1], vert[2], coord[0], coord[1], coord[2]); 00269 } 00270 00271 00272 // Same as above, but even faster - doesn't bother to compute Z. 00273 // In theory, if you use FastProject but don't use zout, the compiler 00274 // should optimize its computation away. In practice... 00275 template <class T> 00276 static inline void FastProjectNoZ(const T *F, 00277 T x, T y, T z, 00278 T &xout, T &yout) 00279 { 00280 T w_rec = T(1) / (F[ 3]*x + F[ 7]*y + F[11]*z + F[15]); 00281 xout = w_rec * (F[ 0]*x + F[ 4]*y + F[ 8]*z + F[12]); 00282 yout = w_rec * (F[ 1]*x + F[ 5]*y + F[ 9]*z + F[13]); 00283 } 00284 00285 template <class T> 00286 static inline void FastProjectNoZ(const T *F, 00287 const T *vert, 00288 T *coord) 00289 { 00290 FastProjectNoZ(F, vert[0], vert[1], vert[2], coord[0], coord[1]); 00291 } 00292 00293 00294 // Invert a 4x4 matrix (in OpenGL order) that represents a rigid-body 00295 // (just rotation+translation) transformation 00296 template <class T> 00297 static inline void FastInvert(T *matrix) 00298 { 00299 swap(matrix[1], matrix[4]); 00300 swap(matrix[2], matrix[8]); 00301 swap(matrix[6], matrix[9]); 00302 00303 T tx = -matrix[12]; 00304 T ty = -matrix[13]; 00305 T tz = -matrix[14]; 00306 00307 matrix[12] = tx*matrix[0] + ty*matrix[4] + tz*matrix[8]; 00308 matrix[13] = tx*matrix[1] + ty*matrix[5] + tz*matrix[9]; 00309 matrix[14] = tx*matrix[2] + ty*matrix[6] + tz*matrix[10]; 00310 } 00311 00312 00313 // Find a sphere that encloses the given triangle 00314 // Based on GGems III V.1, by Fernando Lopez-Lopez 00315 // and GGems I "Triangles" by Ronald Goldman 00316 template <class T> 00317 static inline void TriBoundingSphere(const T *p1, 00318 const T *p2, 00319 const T *p3, 00320 T *cent, 00321 T &r) 00322 { 00323 T a[] = { p2[0] - p3[0], p2[1] - p3[1], p2[2] - p3[2] }; 00324 T b[] = { p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2] }; 00325 T c[] = { p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2] }; 00326 00327 T d1 = -Dot(b,c); 00328 T d2 = -Dot(c,a); 00329 T d3 = -Dot(a,b); 00330 00331 // If triangle is obtuse, just want midpt of longest side 00332 if (d1 <= 0) { 00333 00334 cent[0] = T(0.5) * (p2[0] + p3[0]); 00335 cent[1] = T(0.5) * (p2[1] + p3[1]); 00336 cent[2] = T(0.5) * (p2[2] + p3[2]); 00337 r = T(0.5) * Len(a); 00338 return; 00339 00340 } else if (d2 <= 0) { 00341 00342 cent[0] = T(0.5) * (p3[0] + p1[0]); 00343 cent[1] = T(0.5) * (p3[1] + p1[1]); 00344 cent[2] = T(0.5) * (p3[2] + p1[2]); 00345 r = T(0.5) * Len(b); 00346 return; 00347 00348 } else if (d3 <= 0) { 00349 00350 cent[0] = T(0.5) * (p1[0] + p2[0]); 00351 cent[1] = T(0.5) * (p1[1] + p2[1]); 00352 cent[2] = T(0.5) * (p1[2] + p2[2]); 00353 r = T(0.5) * Len(c); 00354 return; 00355 00356 } 00357 00358 // Else compute circumcircle 00359 T e1 = d2*d3; 00360 T e2 = d3*d1; 00361 T e3 = d1*d2; 00362 T e = e1+e2+e3; 00363 T tmp = T(1) / (T(2) * e); 00364 cent[0] = ((e2+e3)*p1[0] + (e3+e1)*p2[0] + (e1+e2)*p3[0]) * tmp; 00365 cent[1] = ((e2+e3)*p1[1] + (e3+e1)*p2[1] + (e1+e2)*p3[1]) * tmp; 00366 cent[2] = ((e2+e3)*p1[2] + (e3+e1)*p2[2] + (e1+e2)*p3[2]) * tmp; 00367 r = T(0.5) * sqrt((d1+d2)*(d2+d3)*(d3+d1)/e); 00368 } 00369 00370 00371 00372 // Normalize a quaternion (or any 4-vector) 00373 template <class T> 00374 static inline void QNorm(T *q) 00375 { 00376 T x = sqrt(sqr(q[0]) + sqr(q[1]) + sqr(q[2]) + sqr(q[3])); 00377 if (x == T(0)) { 00378 q[0] = T(1); 00379 q[1] = q[2] = q[3] = T(0); 00380 return; 00381 } 00382 00383 x = T(1) / x; 00384 q[0] *= x; 00385 q[1] *= x; 00386 q[2] *= x; 00387 q[3] *= x; 00388 } 00389 00390 00391 // Compose two quaternions 00392 template <class T> 00393 static inline void QCompose(const T *q1, const T *q2, T *q3) 00394 { 00395 // Separate var to hold output, so you can say QCompose(q1, q2, q2); 00396 T qout[4]; 00397 00398 qout[0] = q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]; 00399 qout[1] = q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2]; 00400 qout[2] = q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3]; 00401 qout[3] = q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1]; 00402 00403 QNorm(qout); 00404 q3[0] = qout[0]; q3[1] = qout[1]; q3[2] = qout[2]; q3[3] = qout[3]; 00405 } 00406 00407 00408 // Compute a quaternion from a rotation and axis 00409 template <class T> 00410 static inline void RotAndAxis2Q(T rot, const T *rotaxis, T *q) 00411 { 00412 T c2 = cos(T(0.5)*rot); 00413 T s2 = sin(T(0.5)*rot); 00414 00415 T x = s2 / Len(rotaxis); 00416 00417 q[0] = c2; 00418 q[1] = rotaxis[0] * x; 00419 q[2] = rotaxis[1] * x; 00420 q[3] = rotaxis[2] * x; 00421 } 00422 00423 00424 // Compute a rotation and axis from a quaternion 00425 template <class T> 00426 static inline void Q2RotAndAxis(const T *q, T &rot, T *rotaxis) 00427 { 00428 rot = T(2) * acos(q[0]); 00429 00430 if (rot == T(0)) { 00431 rotaxis[0] = rotaxis[1] = T(0); 00432 rotaxis[2] = T(1); 00433 return; 00434 } 00435 00436 rotaxis[0] = q[1]; 00437 rotaxis[1] = q[2]; 00438 rotaxis[2] = q[3]; 00439 Normalize(rotaxis); 00440 } 00441 00442 00443 // Convert an (x,y) normalized mouse position to a position on the trackball 00444 template <class T> 00445 static inline void Mouse2TrackballPos(T x, T y, T *pos) 00446 { 00447 T r2 = sqr(x) + sqr(y); 00448 T t = T(0.5) * sqr(TRACKBALL_R); 00449 00450 pos[0] = x; 00451 pos[1] = y; 00452 if (r2 < t) 00453 pos[2] = sqrt(T(2)*t - r2); 00454 else 00455 pos[2] = t / sqrt(r2); 00456 00457 Normalize(pos); 00458 } 00459 00460 00461 // Takes normalized mouse positions (x1, y1) and (x2, y2) and returns a 00462 // quaternion representing a rotation on the trackball 00463 template <class T> 00464 static inline void Mouse2Q(T x1, T y1, T x2, T y2, T *q) 00465 { 00466 if ((x1 == x2) && (y1 == y2)) { 00467 q[0] = T(1); 00468 q[1] = q[2] = q[3] = T(0); 00469 return; 00470 } 00471 00472 T pos1[3], pos2[3]; 00473 Mouse2TrackballPos(x1, y1, pos1); 00474 Mouse2TrackballPos(x2, y2, pos2); 00475 00476 T rotaxis[3]; 00477 CrossProd(pos1, pos2, rotaxis); 00478 Normalize(rotaxis); 00479 00480 T rotangle = TRACKBALL_R * sqrt(sqr(x2-x1) + sqr(y2-y1)); 00481 00482 RotAndAxis2Q(rotangle, rotaxis, q); 00483 } 00484 00485 00486 // Rotate point x by quaternion q 00487 template <class T> 00488 static inline void QRotate(T *x, const T *q) 00489 { 00490 T xlen = Len(x); 00491 if (xlen == T(0)) 00492 return; 00493 00494 T p[4] = { 0, x[0], x[1], x[2] }; 00495 T qbar[4] = { q[0], -q[1], -q[2], -q[3] }; 00496 T qtmp[4]; 00497 QCompose(q, p, qtmp); 00498 QCompose(qtmp, qbar, qtmp); 00499 00500 x[0] = qtmp[1]; x[1] = qtmp[2]; x[2] = qtmp[3]; 00501 Normalize(x); 00502 x[0] *= xlen; x[1] *= xlen; x[2] *= xlen; 00503 } 00504 00505 #ifndef M_PI 00506 #define M_PI 3.14159265358979323846 00507 #endif 00508 00509 #ifndef M_SQRT1_2 00510 #define M_SQRT1_2 0.70710678118654752440 00511 #endif 00512 00513 } // namespace trimesh 00514 00515 #endif 00516