00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <assert.h>
00005 #include <math.h>
00006
00007 #include "float_math.h"
00008
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083 namespace ConvexDecomposition
00084 {
00085
00086 void fm_inverseRT(const double *matrix,const double *pos,double *t)
00087 {
00088
00089 double _x = pos[0] - matrix[3*4+0];
00090 double _y = pos[1] - matrix[3*4+1];
00091 double _z = pos[2] - matrix[3*4+2];
00092
00093
00094
00095 t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
00096 t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
00097 t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
00098
00099 }
00100
00101
00102 void fm_identity(double *matrix)
00103 {
00104 matrix[0*4+0] = 1;
00105 matrix[1*4+1] = 1;
00106 matrix[2*4+2] = 1;
00107 matrix[3*4+3] = 1;
00108
00109 matrix[1*4+0] = 0;
00110 matrix[2*4+0] = 0;
00111 matrix[3*4+0] = 0;
00112
00113 matrix[0*4+1] = 0;
00114 matrix[2*4+1] = 0;
00115 matrix[3*4+1] = 0;
00116
00117 matrix[0*4+2] = 0;
00118 matrix[1*4+2] = 0;
00119 matrix[3*4+2] = 0;
00120
00121 matrix[0*4+3] = 0;
00122 matrix[1*4+3] = 0;
00123 matrix[2*4+3] = 0;
00124
00125 }
00126
00127 void fm_eulerMatrix(double ax,double ay,double az,double *matrix)
00128 {
00129 double quat[4];
00130 fm_eulerToQuat(ax,ay,az,quat);
00131 fm_quatToMatrix(quat,matrix);
00132 }
00133
00134 void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax)
00135 {
00136
00137 const unsigned char *source = (const unsigned char *) points;
00138
00139 bmin[0] = points[0];
00140 bmin[1] = points[1];
00141 bmin[2] = points[2];
00142
00143 bmax[0] = points[0];
00144 bmax[1] = points[1];
00145 bmax[2] = points[2];
00146
00147
00148 for (unsigned int i=1; i<vcount; i++)
00149 {
00150 source+=pstride;
00151 const double *p = (const double *) source;
00152
00153 if ( p[0] < bmin[0] ) bmin[0] = p[0];
00154 if ( p[1] < bmin[1] ) bmin[1] = p[1];
00155 if ( p[2] < bmin[2] ) bmin[2] = p[2];
00156
00157 if ( p[0] > bmax[0] ) bmax[0] = p[0];
00158 if ( p[1] > bmax[1] ) bmax[1] = p[1];
00159 if ( p[2] > bmax[2] ) bmax[2] = p[2];
00160
00161 }
00162 }
00163
00164
00165 void fm_eulerToQuat(double roll,double pitch,double yaw,double *quat)
00166 {
00167 roll *= 0.5f;
00168 pitch *= 0.5f;
00169 yaw *= 0.5f;
00170
00171 double cr = cos(roll);
00172 double cp = cos(pitch);
00173 double cy = cos(yaw);
00174
00175 double sr = sin(roll);
00176 double sp = sin(pitch);
00177 double sy = sin(yaw);
00178
00179 double cpcy = cp * cy;
00180 double spsy = sp * sy;
00181 double spcy = sp * cy;
00182 double cpsy = cp * sy;
00183
00184 quat[0] = ( sr * cpcy - cr * spsy);
00185 quat[1] = ( cr * spcy + sr * cpsy);
00186 quat[2] = ( cr * cpsy - sr * spcy);
00187 quat[3] = cr * cpcy + sr * spsy;
00188 }
00189
00190 void fm_quatToMatrix(const double *quat,double *matrix)
00191 {
00192
00193 double xx = quat[0]*quat[0];
00194 double yy = quat[1]*quat[1];
00195 double zz = quat[2]*quat[2];
00196 double xy = quat[0]*quat[1];
00197 double xz = quat[0]*quat[2];
00198 double yz = quat[1]*quat[2];
00199 double wx = quat[3]*quat[0];
00200 double wy = quat[3]*quat[1];
00201 double wz = quat[3]*quat[2];
00202
00203 matrix[0*4+0] = 1 - 2 * ( yy + zz );
00204 matrix[1*4+0] = 2 * ( xy - wz );
00205 matrix[2*4+0] = 2 * ( xz + wy );
00206
00207 matrix[0*4+1] = 2 * ( xy + wz );
00208 matrix[1*4+1] = 1 - 2 * ( xx + zz );
00209 matrix[2*4+1] = 2 * ( yz - wx );
00210
00211 matrix[0*4+2] = 2 * ( xz - wy );
00212 matrix[1*4+2] = 2 * ( yz + wx );
00213 matrix[2*4+2] = 1 - 2 * ( xx + yy );
00214
00215 matrix[3*4+0] = 0.0f;
00216 matrix[3*4+1] = 0.0f;
00217 matrix[3*4+2] = 0.0f;
00218
00219 matrix[0*4+3] = 0.0f;
00220 matrix[1*4+3] = 0.0f;
00221 matrix[2*4+3] = 0.0f;
00222
00223 matrix[3*4+3] =(double) 1.0f;
00224
00225 }
00226
00227
00228 void fm_quatRotate(const double *quat,const double *v,double *r)
00229 {
00230 double left[4];
00231
00232 left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
00233 left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
00234 left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
00235 left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
00236
00237 r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
00238 r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
00239 r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
00240
00241 }
00242
00243
00244 void fm_getTranslation(const double *matrix,double *t)
00245 {
00246 t[0] = matrix[3*4+0];
00247 t[1] = matrix[3*4+1];
00248 t[2] = matrix[3*4+2];
00249 }
00250
00251 void fm_matrixToQuat(const double *matrix,double *quat)
00252 {
00253
00254 double tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
00255
00256
00257
00258 if (tr > 0.0f )
00259 {
00260 double s = (double) sqrt ( (double) (tr + 1.0f) );
00261 quat[3] = s * 0.5f;
00262 s = 0.5f / s;
00263 quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
00264 quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
00265 quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
00266
00267 }
00268 else
00269 {
00270
00271 int nxt[3] = {1, 2, 0};
00272 double qa[4];
00273
00274 int i = 0;
00275
00276 if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
00277 if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
00278
00279 int j = nxt[i];
00280 int k = nxt[j];
00281
00282 double s = sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
00283
00284 qa[i] = s * 0.5f;
00285
00286 if (s != 0.0f ) s = 0.5f / s;
00287
00288 qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
00289 qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
00290 qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
00291
00292 quat[0] = qa[0];
00293 quat[1] = qa[1];
00294 quat[2] = qa[2];
00295 quat[3] = qa[3];
00296 }
00297
00298
00299 }
00300
00301
00302 double fm_sphereVolume(double radius)
00303 {
00304 return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
00305 }
00306
00307
00308 double fm_cylinderVolume(double radius,double h)
00309 {
00310 return FM_PI * radius * radius *h;
00311 }
00312
00313 double fm_capsuleVolume(double radius,double h)
00314 {
00315 double volume = fm_sphereVolume(radius);
00316 double ch = h-radius*2;
00317 if ( ch > 0 )
00318 {
00319 volume+=fm_cylinderVolume(radius,ch);
00320 }
00321 return volume;
00322 }
00323
00324 void fm_transform(const double *matrix,const double *v,double *t)
00325 {
00326 t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0];
00327 t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1];
00328 t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2];
00329 }
00330
00331 void fm_rotate(const double *matrix,const double *v,double *t)
00332 {
00333 t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]);
00334 t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]);
00335 t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]);
00336 }
00337
00338
00339 double fm_distance(const double *p1,const double *p2)
00340 {
00341 double dx = p1[0] - p2[0];
00342 double dy = p1[1] - p2[1];
00343 double dz = p1[2] - p2[2];
00344
00345 return sqrt( dx*dx + dy*dy + dz *dz );
00346 }
00347
00348 double fm_distanceSquared(const double *p1,const double *p2)
00349 {
00350 double dx = p1[0] - p2[0];
00351 double dy = p1[1] - p2[1];
00352 double dz = p1[2] - p2[2];
00353
00354 return dx*dx + dy*dy + dz *dz;
00355 }
00356
00357
00358 double fm_computePlane(const double *A,const double *B,const double *C,double *n)
00359 {
00360 double vx = (B[0] - C[0]);
00361 double vy = (B[1] - C[1]);
00362 double vz = (B[2] - C[2]);
00363
00364 double wx = (A[0] - B[0]);
00365 double wy = (A[1] - B[1]);
00366 double wz = (A[2] - B[2]);
00367
00368 double vw_x = vy * wz - vz * wy;
00369 double vw_y = vz * wx - vx * wz;
00370 double vw_z = vx * wy - vy * wx;
00371
00372 double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
00373
00374 if ( mag < 0.000001f )
00375 {
00376 mag = 0;
00377 }
00378 else
00379 {
00380 mag = 1.0f/mag;
00381 }
00382
00383 double x = vw_x * mag;
00384 double y = vw_y * mag;
00385 double z = vw_z * mag;
00386
00387
00388 double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
00389
00390 n[0] = x;
00391 n[1] = y;
00392 n[2] = z;
00393
00394 return D;
00395 }
00396
00397 double fm_distToPlane(const double *plane,const double *p)
00398 {
00399 return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
00400 }
00401
00402 double fm_dot(const double *p1,const double *p2)
00403 {
00404 return p1[0]*p2[0]+p1[1]*p2[2]+p1[2]*p2[2];
00405 }
00406
00407 void fm_cross(double *cross,const double *a,const double *b)
00408 {
00409 cross[0] = a[1]*b[2] - a[2]*b[1];
00410 cross[1] = a[2]*b[0] - a[0]*b[2];
00411 cross[2] = a[0]*b[1] - a[1]*b[0];
00412 }
00413
00414 void fm_computeNormalVector(double *n,const double *p1,const double *p2)
00415 {
00416 n[0] = p2[0] - p1[0];
00417 n[1] = p2[1] - p1[1];
00418 n[2] = p2[2] - p1[2];
00419 fm_normalize(n);
00420 }
00421
00422 bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3)
00423 {
00424 bool ret = false;
00425
00426 double v1[3];
00427 double v2[3];
00428
00429 fm_computeNormalVector(v1,p1,p2);
00430 fm_computeNormalVector(v2,p1,p3);
00431
00432 double cross[3];
00433
00434 fm_cross(cross, v1, v2 );
00435 double ref[3] = { 1, 0, 0 };
00436
00437 double d = fm_dot( cross, ref );
00438
00439
00440 if ( d <= 0 )
00441 ret = false;
00442 else
00443 ret = true;
00444
00445 return ret;
00446 }
00447
00448 void fm_normalize(double *n)
00449 {
00450
00451 double dist = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
00452 double mag = 0;
00453
00454 if ( dist > 0.0000001f )
00455 mag = 1.0f / sqrt(dist);
00456
00457 n[0]*=mag;
00458 n[1]*=mag;
00459 n[2]*=mag;
00460
00461 }
00462
00463 };