bmtk: bmtk: triutil.h Source File

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