float_math.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <assert.h>
5 #include <math.h>
6 
7 #include "float_math.h"
8 
65 // a set of routines that let you do common 3d math
66 // operations without any vector, matrix, or quaternion
67 // classes or templates.
68 //
69 // a vector (or point) is a 'double *' to 3 doubleing point numbers.
70 // a matrix is a 'double *' to an array of 16 doubleing point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
71 // a quaternion is a 'double *' to 4 doubles representing a quaternion x,y,z,w
72 //
73 //
74 //
75 // Please email bug fixes or improvements to John W. Ratcliff at mailto:jratcliff@infiniplex.net
76 //
77 // If you find this source code useful donate a couple of bucks to my kid's fund raising website at
78 // www.amillionpixels.us
79 //
80 // More snippets at: www.codesuppository.com
81 //
82 
83 namespace ConvexDecomposition
84 {
85 
86 void fm_inverseRT(const double *matrix,const double *pos,double *t) // inverse rotate translate the point.
87 {
88 
89  double _x = pos[0] - matrix[3*4+0];
90  double _y = pos[1] - matrix[3*4+1];
91  double _z = pos[2] - matrix[3*4+2];
92 
93  // Multiply inverse-translated source vector by inverted rotation transform
94 
95  t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
96  t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
97  t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
98 
99 }
100 
101 
102 void fm_identity(double *matrix) // set 4x4 matrix to identity.
103 {
104  matrix[0*4+0] = 1;
105  matrix[1*4+1] = 1;
106  matrix[2*4+2] = 1;
107  matrix[3*4+3] = 1;
108 
109  matrix[1*4+0] = 0;
110  matrix[2*4+0] = 0;
111  matrix[3*4+0] = 0;
112 
113  matrix[0*4+1] = 0;
114  matrix[2*4+1] = 0;
115  matrix[3*4+1] = 0;
116 
117  matrix[0*4+2] = 0;
118  matrix[1*4+2] = 0;
119  matrix[3*4+2] = 0;
120 
121  matrix[0*4+3] = 0;
122  matrix[1*4+3] = 0;
123  matrix[2*4+3] = 0;
124 
125 }
126 
127 void fm_eulerMatrix(double ax,double ay,double az,double *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
128 {
129  double quat[4];
130  fm_eulerToQuat(ax,ay,az,quat);
131  fm_quatToMatrix(quat,matrix);
132 }
133 
134 void fm_getAABB(unsigned int vcount,const double *points,unsigned int pstride,double *bmin,double *bmax)
135 {
136 
137  const unsigned char *source = (const unsigned char *) points;
138 
139  bmin[0] = points[0];
140  bmin[1] = points[1];
141  bmin[2] = points[2];
142 
143  bmax[0] = points[0];
144  bmax[1] = points[1];
145  bmax[2] = points[2];
146 
147 
148  for (unsigned int i=1; i<vcount; i++)
149  {
150  source+=pstride;
151  const double *p = (const double *) source;
152 
153  if ( p[0] < bmin[0] ) bmin[0] = p[0];
154  if ( p[1] < bmin[1] ) bmin[1] = p[1];
155  if ( p[2] < bmin[2] ) bmin[2] = p[2];
156 
157  if ( p[0] > bmax[0] ) bmax[0] = p[0];
158  if ( p[1] > bmax[1] ) bmax[1] = p[1];
159  if ( p[2] > bmax[2] ) bmax[2] = p[2];
160 
161  }
162 }
163 
164 
165 void fm_eulerToQuat(double roll,double pitch,double yaw,double *quat) // convert euler angles to quaternion.
166 {
167  roll *= 0.5f;
168  pitch *= 0.5f;
169  yaw *= 0.5f;
170 
171  double cr = cos(roll);
172  double cp = cos(pitch);
173  double cy = cos(yaw);
174 
175  double sr = sin(roll);
176  double sp = sin(pitch);
177  double sy = sin(yaw);
178 
179  double cpcy = cp * cy;
180  double spsy = sp * sy;
181  double spcy = sp * cy;
182  double cpsy = cp * sy;
183 
184  quat[0] = ( sr * cpcy - cr * spsy);
185  quat[1] = ( cr * spcy + sr * cpsy);
186  quat[2] = ( cr * cpsy - sr * spcy);
187  quat[3] = cr * cpcy + sr * spsy;
188 }
189 
190 void fm_quatToMatrix(const double *quat,double *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
191 {
192 
193  double xx = quat[0]*quat[0];
194  double yy = quat[1]*quat[1];
195  double zz = quat[2]*quat[2];
196  double xy = quat[0]*quat[1];
197  double xz = quat[0]*quat[2];
198  double yz = quat[1]*quat[2];
199  double wx = quat[3]*quat[0];
200  double wy = quat[3]*quat[1];
201  double wz = quat[3]*quat[2];
202 
203  matrix[0*4+0] = 1 - 2 * ( yy + zz );
204  matrix[1*4+0] = 2 * ( xy - wz );
205  matrix[2*4+0] = 2 * ( xz + wy );
206 
207  matrix[0*4+1] = 2 * ( xy + wz );
208  matrix[1*4+1] = 1 - 2 * ( xx + zz );
209  matrix[2*4+1] = 2 * ( yz - wx );
210 
211  matrix[0*4+2] = 2 * ( xz - wy );
212  matrix[1*4+2] = 2 * ( yz + wx );
213  matrix[2*4+2] = 1 - 2 * ( xx + yy );
214 
215  matrix[3*4+0] = 0.0f;
216  matrix[3*4+1] = 0.0f;
217  matrix[3*4+2] = 0.0f;
218 
219  matrix[0*4+3] = 0.0f;
220  matrix[1*4+3] = 0.0f;
221  matrix[2*4+3] = 0.0f;
222 
223  matrix[3*4+3] =(double) 1.0f;
224 
225 }
226 
227 
228 void fm_quatRotate(const double *quat,const double *v,double *r) // rotate a vector directly by a quaternion.
229 {
230  double left[4];
231 
232  left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
233  left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
234  left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
235  left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
236 
237  r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
238  r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
239  r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
240 
241 }
242 
243 
244 void fm_getTranslation(const double *matrix,double *t)
245 {
246  t[0] = matrix[3*4+0];
247  t[1] = matrix[3*4+1];
248  t[2] = matrix[3*4+2];
249 }
250 
251 void fm_matrixToQuat(const double *matrix,double *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
252 {
253 
254  double tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
255 
256  // check the diagonal
257 
258  if (tr > 0.0f )
259  {
260  double s = (double) sqrt ( (double) (tr + 1.0f) );
261  quat[3] = s * 0.5f;
262  s = 0.5f / s;
263  quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
264  quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
265  quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
266 
267  }
268  else
269  {
270  // diagonal is negative
271  int nxt[3] = {1, 2, 0};
272  double qa[4];
273 
274  int i = 0;
275 
276  if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
277  if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
278 
279  int j = nxt[i];
280  int k = nxt[j];
281 
282  double s = sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
283 
284  qa[i] = s * 0.5f;
285 
286  if (s != 0.0f ) s = 0.5f / s;
287 
288  qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
289  qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
290  qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
291 
292  quat[0] = qa[0];
293  quat[1] = qa[1];
294  quat[2] = qa[2];
295  quat[3] = qa[3];
296  }
297 
298 
299 }
300 
301 
302 double fm_sphereVolume(double radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
303 {
304  return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
305 }
306 
307 
308 double fm_cylinderVolume(double radius,double h)
309 {
310  return FM_PI * radius * radius *h;
311 }
312 
313 double fm_capsuleVolume(double radius,double h)
314 {
315  double volume = fm_sphereVolume(radius); // volume of the sphere portion.
316  double ch = h-radius*2; // this is the cylinder length
317  if ( ch > 0 )
318  {
319  volume+=fm_cylinderVolume(radius,ch);
320  }
321  return volume;
322 }
323 
324 void fm_transform(const double *matrix,const double *v,double *t) // rotate and translate this point
325 {
326  t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0];
327  t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1];
328  t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2];
329 }
330 
331 void fm_rotate(const double *matrix,const double *v,double *t) // rotate and translate this point
332 {
333  t[0] = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]);
334  t[1] = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]);
335  t[2] = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]);
336 }
337 
338 
339 double fm_distance(const double *p1,const double *p2)
340 {
341  double dx = p1[0] - p2[0];
342  double dy = p1[1] - p2[1];
343  double dz = p1[2] - p2[2];
344 
345  return sqrt( dx*dx + dy*dy + dz *dz );
346 }
347 
348 double fm_distanceSquared(const double *p1,const double *p2)
349 {
350  double dx = p1[0] - p2[0];
351  double dy = p1[1] - p2[1];
352  double dz = p1[2] - p2[2];
353 
354  return dx*dx + dy*dy + dz *dz;
355 }
356 
357 
358 double fm_computePlane(const double *A,const double *B,const double *C,double *n) // returns D
359 {
360  double vx = (B[0] - C[0]);
361  double vy = (B[1] - C[1]);
362  double vz = (B[2] - C[2]);
363 
364  double wx = (A[0] - B[0]);
365  double wy = (A[1] - B[1]);
366  double wz = (A[2] - B[2]);
367 
368  double vw_x = vy * wz - vz * wy;
369  double vw_y = vz * wx - vx * wz;
370  double vw_z = vx * wy - vy * wx;
371 
372  double mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
373 
374  if ( mag < 0.000001f )
375  {
376  mag = 0;
377  }
378  else
379  {
380  mag = 1.0f/mag;
381  }
382 
383  double x = vw_x * mag;
384  double y = vw_y * mag;
385  double z = vw_z * mag;
386 
387 
388  double D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
389 
390  n[0] = x;
391  n[1] = y;
392  n[2] = z;
393 
394  return D;
395 }
396 
397 double fm_distToPlane(const double *plane,const double *p) // computes the distance of this point from the plane.
398 {
399  return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
400 }
401 
402 double fm_dot(const double *p1,const double *p2)
403 {
404  return p1[0]*p2[0]+p1[1]*p2[2]+p1[2]*p2[2];
405 }
406 
407 void fm_cross(double *cross,const double *a,const double *b)
408 {
409  cross[0] = a[1]*b[2] - a[2]*b[1];
410  cross[1] = a[2]*b[0] - a[0]*b[2];
411  cross[2] = a[0]*b[1] - a[1]*b[0];
412 }
413 
414 void fm_computeNormalVector(double *n,const double *p1,const double *p2)
415 {
416  n[0] = p2[0] - p1[0];
417  n[1] = p2[1] - p1[1];
418  n[2] = p2[2] - p1[2];
419  fm_normalize(n);
420 }
421 
422 bool fm_computeWindingOrder(const double *p1,const double *p2,const double *p3) // returns true if the triangle is clockwise.
423 {
424  bool ret = false;
425 
426  double v1[3];
427  double v2[3];
428 
429  fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
430  fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
431 
432  double cross[3];
433 
434  fm_cross(cross, v1, v2 );
435  double ref[3] = { 1, 0, 0 };
436 
437  double d = fm_dot( cross, ref );
438 
439 
440  if ( d <= 0 )
441  ret = false;
442  else
443  ret = true;
444 
445  return ret;
446 }
447 
448 void fm_normalize(double *n) // normalize this vector
449 {
450 
451  double dist = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
452  double mag = 0;
453 
454  if ( dist > 0.0000001f )
455  mag = 1.0f / sqrt(dist);
456 
457  n[0]*=mag;
458  n[1]*=mag;
459  n[2]*=mag;
460 
461 }
462 
463 }; // end of namespace
ConvexDecomposition::fm_matrixToQuat
void fm_matrixToQuat(const double *matrix, double *quat)
Definition: float_math.cpp:251
ConvexDecomposition::fm_cross
void fm_cross(double *cross, const double *a, const double *b)
Definition: float_math.cpp:407
ConvexDecomposition::fm_getTranslation
void fm_getTranslation(const double *matrix, double *t)
Definition: float_math.cpp:244
ConvexDecomposition::fm_normalize
void fm_normalize(double *n)
Definition: float_math.cpp:448
ConvexDecomposition::fm_distToPlane
double fm_distToPlane(const double *plane, const double *p)
Definition: float_math.cpp:397
ConvexDecomposition::fm_quatRotate
void fm_quatRotate(const double *quat, const double *v, double *r)
Definition: float_math.cpp:228
ConvexDecomposition::fm_rotate
void fm_rotate(const double *matrix, const double *v, double *t)
Definition: float_math.cpp:331
ConvexDecomposition::fm_quatToMatrix
void fm_quatToMatrix(const double *quat, double *matrix)
Definition: float_math.cpp:190
ConvexDecomposition::fm_sphereVolume
double fm_sphereVolume(double radius)
Definition: float_math.cpp:302
float_math.h
ConvexDecomposition::FM_PI
const double FM_PI
Definition: float_math.h:82
ConvexDecomposition
Definition: bestfit.cpp:75
ConvexDecomposition::fm_dot
double fm_dot(const double *p1, const double *p2)
Definition: float_math.cpp:402
ConvexDecomposition::fm_inverseRT
void fm_inverseRT(const double *matrix, const double *pos, double *t)
Definition: float_math.cpp:86
ConvexDecomposition::fm_identity
void fm_identity(double *matrix)
Definition: float_math.cpp:102
ConvexDecomposition::fm_distance
double fm_distance(const double *p1, const double *p2)
Definition: float_math.cpp:339
ConvexDecomposition::fm_computeNormalVector
void fm_computeNormalVector(double *n, const double *p1, const double *p2)
Definition: float_math.cpp:414
ConvexDecomposition::fm_transform
void fm_transform(const double *matrix, const double *v, double *t)
Definition: float_math.cpp:324
ConvexDecomposition::fm_getAABB
void fm_getAABB(unsigned int vcount, const double *points, unsigned int pstride, double *bmin, double *bmax)
Definition: float_math.cpp:134
ConvexDecomposition::fm_cylinderVolume
double fm_cylinderVolume(double radius, double h)
Definition: float_math.cpp:308
ConvexDecomposition::cross
double3 cross(const double3 &a, const double3 &b)
Definition: cd_hull.cpp:672
ConvexDecomposition::fm_eulerMatrix
void fm_eulerMatrix(double ax, double ay, double az, double *matrix)
Definition: float_math.cpp:127
ConvexDecomposition::fm_eulerToQuat
void fm_eulerToQuat(double roll, double pitch, double yaw, double *quat)
Definition: float_math.cpp:165
ConvexDecomposition::plane
Definition: planetri.cpp:182
ConvexDecomposition::fm_computePlane
double fm_computePlane(const double *A, const double *B, const double *C, double *n)
Definition: float_math.cpp:358
ConvexDecomposition::fm_distanceSquared
double fm_distanceSquared(const double *p1, const double *p2)
Definition: float_math.cpp:348
ConvexDecomposition::fm_capsuleVolume
double fm_capsuleVolume(double radius, double h)
Definition: float_math.cpp:313
ConvexDecomposition::fm_computeWindingOrder
bool fm_computeWindingOrder(const double *p1, const double *p2, const double *p3)
Definition: float_math.cpp:422


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Wed Mar 2 2022 00:04:59