float_math.cpp
Go to the documentation of this file.
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 // a set of routines that let you do common 3d math
00066 // operations without any vector, matrix, or quaternion
00067 // classes or templates.
00068 //
00069 // a vector (or point) is a 'double *' to 3 doubleing point numbers.
00070 // a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
00071 // a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
00072 //
00073 //
00074 //
00075 // Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
00076 //
00077 // If you find this source code useful donate a couple of bucks to my kid's fund raising website at
00078 //  www.amillionpixels.us
00079 //
00080 // More snippets at: www.codesuppository.com
00081 //
00082 
00083 namespace ConvexDecomposition
00084 {
00085 
00086 void fm_inverseRT(const double *matrix,const double *pos,double *t) // inverse rotate translate the point.
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         // Multiply inverse-translated source vector by inverted rotation transform
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) // set 4x4 matrix to identity.
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) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
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) // convert euler angles to quaternion.
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) // convert quaterinion rotation to matrix, zeros out the translation component.
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) // rotate a vector directly by a quaternion.
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) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
00252 {
00253 
00254         double tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
00255 
00256         // check the diagonal
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                 // diagonal is negative
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) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
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); // volume of the sphere portion.
00316         double ch = h-radius*2; // this is the cylinder length
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) // rotate and translate this point
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) // rotate and translate this point
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) // returns D
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) // computes the distance of this point from the plane.
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) // returns true if the triangle is clockwise.
00423 {
00424   bool ret = false;
00425 
00426   double v1[3];
00427   double v2[3];
00428 
00429   fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
00430   fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
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) // normalize this vector
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 }; // end of namespace


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Sat Jun 8 2019 20:01:17