1// a set of routines that let you do common 3d math
2// operations without any vector, matrix, or quaternion
3// classes or templates.
4//
5// a vector (or point) is a 'float *' to 3 floating point numbers.
6// a matrix is a 'float *' to an array of 16 floating point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
7// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w
8//
9
10#ifdef _MSC_VER
11#pragma warning(disable:4996)
12#endif
13
14namespace FLOAT_MATH
15{
16
17void fm_inverseRT(const REAL matrix[16],const REAL pos[3],REAL t[3]) // inverse rotate translate the point.
18{
19
20 REAL _x = pos[0] - matrix[3*4+0];
21 REAL _y = pos[1] - matrix[3*4+1];
22 REAL _z = pos[2] - matrix[3*4+2];
23
24 // Multiply inverse-translated source vector by inverted rotation transform
25
26 t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
27 t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
28 t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
29
30}
31
32REAL fm_getDeterminant(const REAL matrix[16])
33{
34 REAL tempv[3];
35 REAL p0[3];
36 REAL p1[3];
37 REAL p2[3];
38
39
40 p0[0] = matrix[0*4+0];
41 p0[1] = matrix[0*4+1];
42 p0[2] = matrix[0*4+2];
43
44 p1[0] = matrix[1*4+0];
45 p1[1] = matrix[1*4+1];
46 p1[2] = matrix[1*4+2];
47
48 p2[0] = matrix[2*4+0];
49 p2[1] = matrix[2*4+1];
50 p2[2] = matrix[2*4+2];
51
52 fm_cross(tempv,p1,p2);
53
54 return fm_dot(p0,tempv);
55
56}
57
58REAL fm_squared(REAL x) { return x*x; };
59
60void fm_decomposeTransform(const REAL local_transform[16],REAL trans[3],REAL rot[4],REAL scale[3])
61{
62
63 trans[0] = local_transform[12];
64 trans[1] = local_transform[13];
65 trans[2] = local_transform[14];
66
67 scale[0] = (REAL)sqrt(fm_squared(local_transform[0*4+0]) + fm_squared(local_transform[0*4+1]) + fm_squared(local_transform[0*4+2]));
68 scale[1] = (REAL)sqrt(fm_squared(local_transform[1*4+0]) + fm_squared(local_transform[1*4+1]) + fm_squared(local_transform[1*4+2]));
69 scale[2] = (REAL)sqrt(fm_squared(local_transform[2*4+0]) + fm_squared(local_transform[2*4+1]) + fm_squared(local_transform[2*4+2]));
70
71 REAL m[16];
72 memcpy(m,local_transform,sizeof(REAL)*16);
73
74 REAL sx = 1.0f / scale[0];
75 REAL sy = 1.0f / scale[1];
76 REAL sz = 1.0f / scale[2];
77
78 m[0*4+0]*=sx;
79 m[0*4+1]*=sx;
80 m[0*4+2]*=sx;
81
82 m[1*4+0]*=sy;
83 m[1*4+1]*=sy;
84 m[1*4+2]*=sy;
85
86 m[2*4+0]*=sz;
87 m[2*4+1]*=sz;
88 m[2*4+2]*=sz;
89
90 fm_matrixToQuat(m,rot);
91
92}
93
94void fm_getSubMatrix(int32_t ki,int32_t kj,REAL pDst[16],const REAL matrix[16])
95{
96 int32_t row, col;
97 int32_t dstCol = 0, dstRow = 0;
98
99 for ( col = 0; col < 4; col++ )
100 {
101 if ( col == kj )
102 {
103 continue;
104 }
105 for ( dstRow = 0, row = 0; row < 4; row++ )
106 {
107 if ( row == ki )
108 {
109 continue;
110 }
111 pDst[dstCol*4+dstRow] = matrix[col*4+row];
112 dstRow++;
113 }
114 dstCol++;
115 }
116}
117
118void fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16])
119{
120 REAL determinant = fm_getDeterminant(matrix);
121 determinant = 1.0f / determinant;
122 for (int32_t i = 0; i < 4; i++ )
123 {
124 for (int32_t j = 0; j < 4; j++ )
125 {
126 int32_t sign = 1 - ( ( i + j ) % 2 ) * 2;
127 REAL subMat[16];
128 fm_identity(subMat);
129 fm_getSubMatrix( i, j, subMat, matrix );
130 REAL subDeterminant = fm_getDeterminant(subMat);
131 inverse_matrix[i*4+j] = ( subDeterminant * sign ) * determinant;
132 }
133 }
134}
135
136void fm_identity(REAL matrix[16]) // set 4x4 matrix to identity.
137{
138 matrix[0*4+0] = 1;
139 matrix[1*4+1] = 1;
140 matrix[2*4+2] = 1;
141 matrix[3*4+3] = 1;
142
143 matrix[1*4+0] = 0;
144 matrix[2*4+0] = 0;
145 matrix[3*4+0] = 0;
146
147 matrix[0*4+1] = 0;
148 matrix[2*4+1] = 0;
149 matrix[3*4+1] = 0;
150
151 matrix[0*4+2] = 0;
152 matrix[1*4+2] = 0;
153 matrix[3*4+2] = 0;
154
155 matrix[0*4+3] = 0;
156 matrix[1*4+3] = 0;
157 matrix[2*4+3] = 0;
158
159}
160
161void fm_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az)
162{
163 REAL x = quat[0];
164 REAL y = quat[1];
165 REAL z = quat[2];
166 REAL w = quat[3];
167
168 REAL sint = (2.0f * w * y) - (2.0f * x * z);
169 REAL cost_temp = 1.0f - (sint * sint);
170 REAL cost = 0;
171
172 if ( (REAL)fabs(cost_temp) > 0.001f )
173 {
174 cost = (REAL)sqrt( cost_temp );
175 }
176
177 REAL sinv, cosv, sinf, cosf;
178 if ( (REAL)fabs(cost) > 0.001f )
179 {
180 cost = 1.0f / cost;
181 sinv = ((2.0f * y * z) + (2.0f * w * x)) * cost;
182 cosv = (1.0f - (2.0f * x * x) - (2.0f * y * y)) * cost;
183 sinf = ((2.0f * x * y) + (2.0f * w * z)) * cost;
184 cosf = (1.0f - (2.0f * y * y) - (2.0f * z * z)) * cost;
185 }
186 else
187 {
188 sinv = (2.0f * w * x) - (2.0f * y * z);
189 cosv = 1.0f - (2.0f * x * x) - (2.0f * z * z);
190 sinf = 0;
191 cosf = 1.0f;
192 }
193
194 // compute output rotations
195 ax = (REAL)atan2( sinv, cosv );
196 ay = (REAL)atan2( sint, cost );
197 az = (REAL)atan2( sinf, cosf );
198
199}
200
201void fm_eulerToMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
202{
203 REAL quat[4];
204 fm_eulerToQuat(ax,ay,az,quat);
205 fm_quatToMatrix(quat,matrix);
206}
207
208void fm_getAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax)
209{
210
211 const uint8_t *source = (const uint8_t *) points;
212
213 bmin[0] = points[0];
214 bmin[1] = points[1];
215 bmin[2] = points[2];
216
217 bmax[0] = points[0];
218 bmax[1] = points[1];
219 bmax[2] = points[2];
220
221
222 for (uint32_t i=1; i<vcount; i++)
223 {
224 source+=pstride;
225 const REAL *p = (const REAL *) source;
226
227 if ( p[0] < bmin[0] ) bmin[0] = p[0];
228 if ( p[1] < bmin[1] ) bmin[1] = p[1];
229 if ( p[2] < bmin[2] ) bmin[2] = p[2];
230
231 if ( p[0] > bmax[0] ) bmax[0] = p[0];
232 if ( p[1] > bmax[1] ) bmax[1] = p[1];
233 if ( p[2] > bmax[2] ) bmax[2] = p[2];
234
235 }
236}
237
238void fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to quaternion.
239{
240 fm_eulerToQuat(euler[0],euler[1],euler[2],quat);
241}
242
243void fm_eulerToQuat(REAL roll,REAL pitch,REAL yaw,REAL *quat) // convert euler angles to quaternion.
244{
245 roll *= 0.5f;
246 pitch *= 0.5f;
247 yaw *= 0.5f;
248
249 REAL cr = (REAL)cos(roll);
250 REAL cp = (REAL)cos(pitch);
251 REAL cy = (REAL)cos(yaw);
252
253 REAL sr = (REAL)sin(roll);
254 REAL sp = (REAL)sin(pitch);
255 REAL sy = (REAL)sin(yaw);
256
257 REAL cpcy = cp * cy;
258 REAL spsy = sp * sy;
259 REAL spcy = sp * cy;
260 REAL cpsy = cp * sy;
261
262 quat[0] = ( sr * cpcy - cr * spsy);
263 quat[1] = ( cr * spcy + sr * cpsy);
264 quat[2] = ( cr * cpsy - sr * spcy);
265 quat[3] = cr * cpcy + sr * spsy;
266}
267
268void fm_quatToMatrix(const REAL *quat,REAL *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
269{
270
271 REAL xx = quat[0]*quat[0];
272 REAL yy = quat[1]*quat[1];
273 REAL zz = quat[2]*quat[2];
274 REAL xy = quat[0]*quat[1];
275 REAL xz = quat[0]*quat[2];
276 REAL yz = quat[1]*quat[2];
277 REAL wx = quat[3]*quat[0];
278 REAL wy = quat[3]*quat[1];
279 REAL wz = quat[3]*quat[2];
280
281 matrix[0*4+0] = 1 - 2 * ( yy + zz );
282 matrix[1*4+0] = 2 * ( xy - wz );
283 matrix[2*4+0] = 2 * ( xz + wy );
284
285 matrix[0*4+1] = 2 * ( xy + wz );
286 matrix[1*4+1] = 1 - 2 * ( xx + zz );
287 matrix[2*4+1] = 2 * ( yz - wx );
288
289 matrix[0*4+2] = 2 * ( xz - wy );
290 matrix[1*4+2] = 2 * ( yz + wx );
291 matrix[2*4+2] = 1 - 2 * ( xx + yy );
292
293 matrix[3*4+0] = matrix[3*4+1] = matrix[3*4+2] = (REAL) 0.0f;
294 matrix[0*4+3] = matrix[1*4+3] = matrix[2*4+3] = (REAL) 0.0f;
295 matrix[3*4+3] =(REAL) 1.0f;
296
297}
298
299
300void fm_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector directly by a quaternion.
301{
302 REAL left[4];
303
304 left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
305 left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
306 left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
307 left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
308
309 r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
310 r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
311 r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
312
313}
314
315
316void fm_getTranslation(const REAL *matrix,REAL *t)
317{
318 t[0] = matrix[3*4+0];
319 t[1] = matrix[3*4+1];
320 t[2] = matrix[3*4+2];
321}
322
323void fm_matrixToQuat(const REAL *matrix,REAL *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
324{
325
326 REAL tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
327
328 // check the diagonal
329
330 if (tr > 0.0f )
331 {
332 REAL s = (REAL) sqrt ( (double) (tr + 1.0f) );
333 quat[3] = s * 0.5f;
334 s = 0.5f / s;
335 quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
336 quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
337 quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
338
339 }
340 else
341 {
342 // diagonal is negative
343 int32_t nxt[3] = {1, 2, 0};
344 REAL qa[4];
345
346 int32_t i = 0;
347
348 if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
349 if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
350
351 int32_t j = nxt[i];
352 int32_t k = nxt[j];
353
354 REAL s = (REAL)sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
355
356 qa[i] = s * 0.5f;
357
358 if (s != 0.0f ) s = 0.5f / s;
359
360 qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
361 qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
362 qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
363
364 quat[0] = qa[0];
365 quat[1] = qa[1];
366 quat[2] = qa[2];
367 quat[3] = qa[3];
368 }
369// fm_normalizeQuat(quat);
370}
371
372
373REAL fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
374{
375 return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
376}
377
378
379REAL fm_cylinderVolume(REAL radius,REAL h)
380{
381 return FM_PI * radius * radius *h;
382}
383
384REAL fm_capsuleVolume(REAL radius,REAL h)
385{
386 REAL volume = fm_sphereVolume(radius); // volume of the sphere portion.
387 REAL ch = h-radius*2; // this is the cylinder length
388 if ( ch > 0 )
389 {
390 volume+=fm_cylinderVolume(radius,ch);
391 }
392 return volume;
393}
394
395void fm_transform(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point
396{
397 if ( matrix )
398 {
399 REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]) + matrix[3*4+0];
400 REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]) + matrix[3*4+1];
401 REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]) + matrix[3*4+2];
402 t[0] = tx;
403 t[1] = ty;
404 t[2] = tz;
405 }
406 else
407 {
408 t[0] = v[0];
409 t[1] = v[1];
410 t[2] = v[2];
411 }
412}
413
414void fm_rotate(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point
415{
416 if ( matrix )
417 {
418 REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] * v[2]);
419 REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] * v[2]);
420 REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] * v[2]);
421 t[0] = tx;
422 t[1] = ty;
423 t[2] = tz;
424 }
425 else
426 {
427 t[0] = v[0];
428 t[1] = v[1];
429 t[2] = v[2];
430 }
431}
432
433
434REAL fm_distance(const REAL *p1,const REAL *p2)
435{
436 REAL dx = p1[0] - p2[0];
437 REAL dy = p1[1] - p2[1];
438 REAL dz = p1[2] - p2[2];
439
440 return (REAL)sqrt( dx*dx + dy*dy + dz *dz );
441}
442
443REAL fm_distanceSquared(const REAL *p1,const REAL *p2)
444{
445 REAL dx = p1[0] - p2[0];
446 REAL dy = p1[1] - p2[1];
447 REAL dz = p1[2] - p2[2];
448
449 return dx*dx + dy*dy + dz *dz;
450}
451
452
453REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2)
454{
455 REAL dx = p1[0] - p2[0];
456 REAL dz = p1[2] - p2[2];
457
458 return dx*dx + dz *dz;
459}
460
461
462REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns D
463{
464 REAL vx = (B[0] - C[0]);
465 REAL vy = (B[1] - C[1]);
466 REAL vz = (B[2] - C[2]);
467
468 REAL wx = (A[0] - B[0]);
469 REAL wy = (A[1] - B[1]);
470 REAL wz = (A[2] - B[2]);
471
472 REAL vw_x = vy * wz - vz * wy;
473 REAL vw_y = vz * wx - vx * wz;
474 REAL vw_z = vx * wy - vy * wx;
475
476 REAL mag = (REAL)sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
477
478 if ( mag < 0.000001f )
479 {
480 mag = 0;
481 }
482 else
483 {
484 mag = 1.0f/mag;
485 }
486
487 REAL x = vw_x * mag;
488 REAL y = vw_y * mag;
489 REAL z = vw_z * mag;
490
491
492 REAL D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
493
494 n[0] = x;
495 n[1] = y;
496 n[2] = z;
497
498 return D;
499}
500
501REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of this point from the plane.
502{
503 return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
504}
505
506REAL fm_dot(const REAL *p1,const REAL *p2)
507{
508 return p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2];
509}
510
511void fm_cross(REAL *cross,const REAL *a,const REAL *b)
512{
513 cross[0] = a[1]*b[2] - a[2]*b[1];
514 cross[1] = a[2]*b[0] - a[0]*b[2];
515 cross[2] = a[0]*b[1] - a[1]*b[0];
516}
517
518REAL fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2)
519{
520 n[0] = p2[0] - p1[0];
521 n[1] = p2[1] - p1[1];
522 n[2] = p2[2] - p1[2];
523 return fm_normalize(n);
524}
525
526bool fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) // returns true if the triangle is clockwise.
527{
528 bool ret = false;
529
530 REAL v1[3];
531 REAL v2[3];
532
533 fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
534 fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
535
536 REAL cross[3];
537
538 fm_cross(cross, v1, v2 );
539 REAL ref[3] = { 1, 0, 0 };
540
541 REAL d = fm_dot( cross, ref );
542
543
544 if ( d <= 0 )
545 ret = false;
546 else
547 ret = true;
548
549 return ret;
550}
551
552REAL fm_normalize(REAL *n) // normalize this vector
553{
554 REAL dist = (REAL)sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
555 if ( dist > 0.0000001f )
556 {
557 REAL mag = 1.0f / dist;
558 n[0]*=mag;
559 n[1]*=mag;
560 n[2]*=mag;
561 }
562 else
563 {
564 n[0] = 1;
565 n[1] = 0;
566 n[2] = 0;
567 }
568
569 return dist;
570}
571
572
573void fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM)
574{
575#if 1
576
577 REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] + pA[0*4+3] * pB[3*4+0];
578 REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] + pA[0*4+3] * pB[3*4+1];
579 REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] + pA[0*4+3] * pB[3*4+2];
580 REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] + pA[0*4+3] * pB[3*4+3];
581
582 REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] + pA[1*4+3] * pB[3*4+0];
583 REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] + pA[1*4+3] * pB[3*4+1];
584 REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] + pA[1*4+3] * pB[3*4+2];
585 REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] + pA[1*4+3] * pB[3*4+3];
586
587 REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] + pA[2*4+3] * pB[3*4+0];
588 REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] + pA[2*4+3] * pB[3*4+1];
589 REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] + pA[2*4+3] * pB[3*4+2];
590 REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] + pA[2*4+3] * pB[3*4+3];
591
592 REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] + pA[3*4+3] * pB[3*4+0];
593 REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] + pA[3*4+3] * pB[3*4+1];
594 REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] + pA[3*4+3] * pB[3*4+2];
595 REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] + pA[3*4+3] * pB[3*4+3];
596
597 pM[0] = a;
598 pM[1] = b;
599 pM[2] = c;
600 pM[3] = d;
601
602 pM[4] = e;
603 pM[5] = f;
604 pM[6] = g;
605 pM[7] = h;
606
607 pM[8] = i;
608 pM[9] = j;
609 pM[10] = k;
610 pM[11] = l;
611
612 pM[12] = m;
613 pM[13] = n;
614 pM[14] = o;
615 pM[15] = p;
616
617
618#else
619 memset(pM, 0, sizeof(REAL)*16);
620 for(int32_t i=0; i<4; i++ )
621 for(int32_t j=0; j<4; j++ )
622 for(int32_t k=0; k<4; k++ )
623 pM[4*i+j] += pA[4*i+k] * pB[4*k+j];
624#endif
625}
626
627
628void fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to quaternion using the fucked up DirectX method
629{
630 REAL matrix[16];
631 fm_eulerToMatrix(x,y,z,matrix);
632 fm_matrixToQuat(matrix,quat);
633}
634
635// implementation copied from: http://blogs.msdn.com/mikepelton/archive/2004/10/29/249501.aspx
636void fm_eulerToMatrixDX(REAL x,REAL y,REAL z,REAL *matrix) // convert euler angles to quaternion using the fucked up DirectX method.
637{
638 fm_identity(matrix);
639 matrix[0*4+0] = (REAL)(cos(z)*cos(y) + sin(z)*sin(x)*sin(y));
640 matrix[0*4+1] = (REAL)(sin(z)*cos(x));
641 matrix[0*4+2] = (REAL)(cos(z)*-sin(y) + sin(z)*sin(x)*cos(y));
642
643 matrix[1*4+0] = (REAL)(-sin(z)*cos(y)+cos(z)*sin(x)*sin(y));
644 matrix[1*4+1] = (REAL)(cos(z)*cos(x));
645 matrix[1*4+2] = (REAL)(sin(z)*sin(y) +cos(z)*sin(x)*cos(y));
646
647 matrix[2*4+0] = (REAL)(cos(x)*sin(y));
648 matrix[2*4+1] = (REAL)(-sin(x));
649 matrix[2*4+2] = (REAL)(cos(x)*cos(y));
650}
651
652
653void fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix.
654{
655 fscale[0*4+0] = x;
656 fscale[1*4+1] = y;
657 fscale[2*4+2] = z;
658}
659
660
661void fm_composeTransform(const REAL *position,const REAL *quat,const REAL *scale,REAL *matrix)
662{
663 fm_identity(matrix);
664 fm_quatToMatrix(quat,matrix);
665
666 if ( scale && ( scale[0] != 1 || scale[1] != 1 || scale[2] != 1 ) )
667 {
668 REAL work[16];
669 memcpy(work,matrix,sizeof(REAL)*16);
670 REAL mscale[16];
671 fm_identity(mscale);
672 fm_scale(scale[0],scale[1],scale[2],mscale);
673 fm_matrixMultiply(work,mscale,matrix);
674 }
675
676 matrix[12] = position[0];
677 matrix[13] = position[1];
678 matrix[14] = position[2];
679}
680
681
682void fm_setTranslation(const REAL *translation,REAL *matrix)
683{
684 matrix[12] = translation[0];
685 matrix[13] = translation[1];
686 matrix[14] = translation[2];
687}
688
689static REAL enorm0_3d ( REAL x0, REAL y0, REAL z0, REAL x1, REAL y1, REAL z1 )
690
691/**********************************************************************/
692
693/*
694Purpose:
695
696ENORM0_3D computes the Euclidean norm of (P1-P0) in 3D.
697
698Modified:
699
70018 April 1999
701
702Author:
703
704John Burkardt
705
706Parameters:
707
708Input, REAL X0, Y0, Z0, X1, Y1, Z1, the coordinates of the points
709P0 and P1.
710
711Output, REAL ENORM0_3D, the Euclidean norm of (P1-P0).
712*/
713{
714 REAL value;
715
716 value = (REAL)sqrt (
717 ( x1 - x0 ) * ( x1 - x0 ) +
718 ( y1 - y0 ) * ( y1 - y0 ) +
719 ( z1 - z0 ) * ( z1 - z0 ) );
720
721 return value;
722}
723
724
725static REAL triangle_area_3d ( REAL x1, REAL y1, REAL z1, REAL x2,REAL y2, REAL z2, REAL x3, REAL y3, REAL z3 )
726
727 /**********************************************************************/
728
729 /*
730 Purpose:
731
732 TRIANGLE_AREA_3D computes the area of a triangle in 3D.
733
734 Modified:
735
736 22 April 1999
737
738 Author:
739
740 John Burkardt
741
742 Parameters:
743
744 Input, REAL X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (X,Y,Z)
745 coordinates of the corners of the triangle.
746
747 Output, REAL TRIANGLE_AREA_3D, the area of the triangle.
748 */
749{
750 REAL a;
751 REAL alpha;
752 REAL area;
753 REAL b;
754 REAL base;
755 REAL c;
756 REAL dot;
757 REAL height;
758 /*
759 Find the projection of (P3-P1) onto (P2-P1).
760 */
761 dot =
762 ( x2 - x1 ) * ( x3 - x1 ) +
763 ( y2 - y1 ) * ( y3 - y1 ) +
764 ( z2 - z1 ) * ( z3 - z1 );
765
766 base = enorm0_3d ( x1, y1, z1, x2, y2, z2 );
767 /*
768 The height of the triangle is the length of (P3-P1) after its
769 projection onto (P2-P1) has been subtracted.
770 */
771 if ( base == 0.0 ) {
772
773 height = 0.0;
774
775 }
776 else {
777
778 alpha = dot / ( base * base );
779
780 a = x3 - x1 - alpha * ( x2 - x1 );
781 b = y3 - y1 - alpha * ( y2 - y1 );
782 c = z3 - z1 - alpha * ( z2 - z1 );
783
784 height = (REAL)sqrt ( a * a + b * b + c * c );
785
786 }
787
788 area = 0.5f * base * height;
789
790 return area;
791}
792
793
794REAL fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3)
795{
796 REAL ret = 0;
797
798 ret = triangle_area_3d(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]);
799
800 return ret;
801}
802
803
804void fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue)
805{
806 dest[0] = ((p2[0] - p1[0])*lerpValue) + p1[0];
807 dest[1] = ((p2[1] - p1[1])*lerpValue) + p1[1];
808 dest[2] = ((p2[2] - p1[2])*lerpValue) + p1[2];
809}
810
811bool fm_pointTestXZ(const REAL *p,const REAL *i,const REAL *j)
812{
813 bool ret = false;
814
815 if (((( i[2] <= p[2] ) && ( p[2] < j[2] )) || (( j[2] <= p[2] ) && ( p[2] < i[2] ))) && ( p[0] < (j[0] - i[0]) * (p[2] - i[2]) / (j[2] - i[2]) + i[0]))
816 ret = true;
817
818 return ret;
819};
820
821
822bool fm_insideTriangleXZ(const REAL *p,const REAL *p1,const REAL *p2,const REAL *p3)
823{
824 bool ret = false;
825
826 int32_t c = 0;
827 if ( fm_pointTestXZ(p,p1,p2) ) c = !c;
828 if ( fm_pointTestXZ(p,p2,p3) ) c = !c;
829 if ( fm_pointTestXZ(p,p3,p1) ) c = !c;
830 if ( c ) ret = true;
831
832 return ret;
833}
834
835bool fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax)
836{
837 bool ret = false;
838
839 if ( pos[0] >= bmin[0] && pos[0] <= bmax[0] &&
840 pos[1] >= bmin[1] && pos[1] <= bmax[1] &&
841 pos[2] >= bmin[2] && pos[2] <= bmax[2] )
842 ret = true;
843
844 return ret;
845}
846
847
848uint32_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos)
849{
850 uint32_t ret = 0;
851
852 if ( pos[0] < bmin[0] )
853 ret|=FMCS_XMIN;
854 else if ( pos[0] > bmax[0] )
855 ret|=FMCS_XMAX;
856
857 if ( pos[1] < bmin[1] )
858 ret|=FMCS_YMIN;
859 else if ( pos[1] > bmax[1] )
860 ret|=FMCS_YMAX;
861
862 if ( pos[2] < bmin[2] )
863 ret|=FMCS_ZMIN;
864 else if ( pos[2] > bmax[2] )
865 ret|=FMCS_ZMAX;
866
867 return ret;
868}
869
870uint32_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) // only tests X and Z, not Y
871{
872 uint32_t ret = 0;
873
874 if ( pos[0] < bmin[0] )
875 ret|=FMCS_XMIN;
876 else if ( pos[0] > bmax[0] )
877 ret|=FMCS_XMAX;
878
879 if ( pos[2] < bmin[2] )
880 ret|=FMCS_ZMIN;
881 else if ( pos[2] > bmax[2] )
882 ret|=FMCS_ZMAX;
883
884 return ret;
885}
886
887uint32_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL *p2,const REAL *p3,uint32_t &andCode)
888{
889 uint32_t orCode = 0;
890
891 andCode = FMCS_XMIN | FMCS_XMAX | FMCS_YMIN | FMCS_YMAX | FMCS_ZMIN | FMCS_ZMAX;
892
893 uint32_t c = fm_clipTestPoint(bmin,bmax,p1);
894 orCode|=c;
895 andCode&=c;
896
897 c = fm_clipTestPoint(bmin,bmax,p2);
898 orCode|=c;
899 andCode&=c;
900
901 c = fm_clipTestPoint(bmin,bmax,p3);
902 orCode|=c;
903 andCode&=c;
904
905 return orCode;
906}
907
908bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL *time)
909{
910 REAL st,et,fst = 0,fet = 1;
911
912 for (int32_t i = 0; i < 3; i++)
913 {
914 if (*si < *ei)
915 {
916 if (*si > *bmax || *ei < *bmin)
917 return false;
918 REAL di = *ei - *si;
919 st = (*si < *bmin)? (*bmin - *si) / di: 0;
920 et = (*ei > *bmax)? (*bmax - *si) / di: 1;
921 }
922 else
923 {
924 if (*ei > *bmax || *si < *bmin)
925 return false;
926 REAL di = *ei - *si;
927 st = (*si > *bmax)? (*bmax - *si) / di: 0;
928 et = (*ei < *bmin)? (*bmin - *si) / di: 1;
929 }
930
931 if (st > fst) fst = st;
932 if (et < fet) fet = et;
933 if (fet < fst)
934 return false;
935 bmin++; bmax++;
936 si++; ei++;
937 }
938
939 *time = fst;
940 return true;
941}
942
943
944
945bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)
946{
947 bool sect = intersect(p1,p2,bmin,bmax,&time);
948 return sect;
949}
950
951
952bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)
953{
954 REAL _bmin[3];
955 REAL _bmax[3];
956
957 _bmin[0] = bmin[0];
958 _bmin[1] = -1e9;
959 _bmin[2] = bmin[2];
960
961 _bmax[0] = bmax[0];
962 _bmax[1] = 1e9;
963 _bmax[2] = bmax[2];
964
965 bool sect = intersect(p1,p2,_bmin,_bmax,&time);
966
967 return sect;
968}
969
970void fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max value
971{
972
973 if ( p[0] < bmin[0] ) bmin[0] = p[0];
974 if ( p[1] < bmin[1] ) bmin[1] = p[1];
975 if ( p[2] < bmin[2] ) bmin[2] = p[2];
976
977 if ( p[0] > bmax[0] ) bmax[0] = p[0];
978 if ( p[1] > bmax[1] ) bmax[1] = p[1];
979 if ( p[2] > bmax[2] ) bmax[2] = p[2];
980
981}
982
983REAL fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane equation and the other two components.
984{
985 REAL x = (y*plane[1]+z*plane[2]+plane[3]) / -plane[0];
986 return x;
987}
988
989REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane equation and the other two components.
990{
991 REAL y = (x*plane[0]+z*plane[2]+plane[3]) / -plane[1];
992 return y;
993}
994
995
996REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane equation and the other two components.
997{
998 REAL z = (x*plane[0]+y*plane[1]+plane[3]) / -plane[2];
999 return z;
1000}
1001
1002
1003void fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center)
1004{
1005 center[0] = (bmax[0]-bmin[0])*0.5f+bmin[0];
1006 center[1] = (bmax[1]-bmin[1])*0.5f+bmin[1];
1007 center[2] = (bmax[2]-bmin[2])*0.5f+bmin[2];
1008}
1009
1010FM_Axis fm_getDominantAxis(const REAL normal[3])
1011{
1012 FM_Axis ret = FM_XAXIS;
1013
1014 REAL x = (REAL)fabs(normal[0]);
1015 REAL y = (REAL)fabs(normal[1]);
1016 REAL z = (REAL)fabs(normal[2]);
1017
1018 if ( y > x && y > z )
1019 ret = FM_YAXIS;
1020 else if ( z > x && z > y )
1021 ret = FM_ZAXIS;
1022
1023 return ret;
1024}
1025
1026
1027bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const REAL *p2,REAL *intersect)
1028{
1029 bool ret = false;
1030
1031 REAL dir[3];
1032
1033 dir[0] = p2[0]-p1[0];
1034 dir[1] = p2[1]-p1[1];
1035 dir[2] = p2[2]-p1[2];
1036
1037 REAL distance = (REAL)sqrt( dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);
1038
1039 if ( distance > 0 )
1040 {
1041 REAL recip = 1.0f / distance;
1042 dir[0]*=recip;
1043 dir[1]*=recip;
1044 dir[2]*=recip;
1045 ret = fm_raySphereIntersect(center,radius,p1,dir,distance,intersect);
1046 }
1047 else
1048 {
1049 dir[0] = center[0]-p1[0];
1050 dir[1] = center[1]-p1[1];
1051 dir[2] = center[2]-p1[2];
1052 REAL d2 = dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2];
1053 REAL r2 = radius*radius;
1054 if ( d2 < r2 )
1055 {
1056 ret = true;
1057 if ( intersect )
1058 {
1059 intersect[0] = p1[0];
1060 intersect[1] = p1[1];
1061 intersect[2] = p1[2];
1062 }
1063 }
1064 }
1065 return ret;
1066}
1067
1068#define DOT(p1,p2) (p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2])
1069
1070bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const REAL *dir,REAL distance,REAL *intersect)
1071{
1072 bool ret = false;
1073
1074 REAL E0[3];
1075
1076 E0[0] = center[0] - pos[0];
1077 E0[1] = center[1] - pos[1];
1078 E0[2] = center[2] - pos[2];
1079
1080 REAL V[3];
1081
1082 V[0] = dir[0];
1083 V[1] = dir[1];
1084 V[2] = dir[2];
1085
1086
1087 REAL dist2 = E0[0]*E0[0] + E0[1]*E0[1] + E0[2] * E0[2];
1088 REAL radius2 = radius*radius; // radius squared..
1089
1090 // Bug Fix For Gem, if origin is *inside* the sphere, invert the
1091 // direction vector so that we get a valid intersection location.
1092 if ( dist2 < radius2 )
1093 {
1094 V[0]*=-1;
1095 V[1]*=-1;
1096 V[2]*=-1;
1097 }
1098
1099
1100 REAL v = DOT(E0,V);
1101
1102 REAL disc = radius2 - (dist2 - v*v);
1103
1104 if (disc > 0.0f)
1105 {
1106 if ( intersect )
1107 {
1108 REAL d = (REAL)sqrt(disc);
1109 REAL diff = v-d;
1110 if ( diff < distance )
1111 {
1112 intersect[0] = pos[0]+V[0]*diff;
1113 intersect[1] = pos[1]+V[1]*diff;
1114 intersect[2] = pos[2]+V[2]*diff;
1115 ret = true;
1116 }
1117 }
1118 }
1119
1120 return ret;
1121}
1122
1123
1124void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL *p3,const REAL *p4, const REAL s)
1125{
1126 REAL s_squared = s * s;
1127 REAL s_cubed = s_squared * s;
1128
1129 REAL coefficient_p1 = -s_cubed + 2*s_squared - s;
1130 REAL coefficient_p2 = 3 * s_cubed - 5 * s_squared + 2;
1131 REAL coefficient_p3 = -3 * s_cubed +4 * s_squared + s;
1132 REAL coefficient_p4 = s_cubed - s_squared;
1133
1134 out_vector[0] = (coefficient_p1 * p1[0] + coefficient_p2 * p2[0] + coefficient_p3 * p3[0] + coefficient_p4 * p4[0])*0.5f;
1135 out_vector[1] = (coefficient_p1 * p1[1] + coefficient_p2 * p2[1] + coefficient_p3 * p3[1] + coefficient_p4 * p4[1])*0.5f;
1136 out_vector[2] = (coefficient_p1 * p1[2] + coefficient_p2 * p2[2] + coefficient_p3 * p3[2] + coefficient_p4 * p4[2])*0.5f;
1137}
1138
1139bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const REAL *bmax2)
1140{
1141 if ((bmin1[0] > bmax2[0]) || (bmin2[0] > bmax1[0])) return false;
1142 if ((bmin1[1] > bmax2[1]) || (bmin2[1] > bmax1[1])) return false;
1143 if ((bmin1[2] > bmax2[2]) || (bmin2[2] > bmax1[2])) return false;
1144 return true;
1145
1146}
1147
1148bool fm_insideAABB(const REAL *obmin,const REAL *obmax,const REAL *tbmin,const REAL *tbmax) // test if bounding box tbmin/tmbax is fully inside obmin/obmax
1149{
1150 bool ret = false;
1151
1152 if ( tbmax[0] <= obmax[0] &&
1153 tbmax[1] <= obmax[1] &&
1154 tbmax[2] <= obmax[2] &&
1155 tbmin[0] >= obmin[0] &&
1156 tbmin[1] >= obmin[1] &&
1157 tbmin[2] >= obmin[2] ) ret = true;
1158
1159 return ret;
1160}
1161
1162
1163// Reference, from Stan Melax in Game Gems I
1164// Quaternion q;
1165// vector3 c = CrossProduct(v0,v1);
1166// REAL d = DotProduct(v0,v1);
1167// REAL s = (REAL)sqrt((1+d)*2);
1168// q.x = c.x / s;
1169// q.y = c.y / s;
1170// q.z = c.z / s;
1171// q.w = s /2.0f;
1172// return q;
1173void fm_rotationArc(const REAL *v0,const REAL *v1,REAL *quat)
1174{
1175 REAL cross[3];
1176
1177 fm_cross(cross,v0,v1);
1178 REAL d = fm_dot(v0,v1);
1179
1180 if( d<= -0.99999f ) // 180 about x axis
1181 {
1182 if ( fabsf((float)v0[0]) < 0.1f )
1183 {
1184 quat[0] = 0;
1185 quat[1] = v0[2];
1186 quat[2] = -v0[1];
1187 quat[3] = 0;
1188 }
1189 else
1190 {
1191 quat[0] = v0[1];
1192 quat[1] = -v0[0];
1193 quat[2] = 0;
1194 quat[3] = 0;
1195 }
1196 REAL magnitudeSquared = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
1197 REAL magnitude = sqrtf((float)magnitudeSquared);
1198 REAL recip = 1.0f / magnitude;
1199 quat[0]*=recip;
1200 quat[1]*=recip;
1201 quat[2]*=recip;
1202 quat[3]*=recip;
1203 }
1204 else
1205 {
1206 REAL s = (REAL)sqrt((1+d)*2);
1207 REAL recip = 1.0f / s;
1208
1209 quat[0] = cross[0] * recip;
1210 quat[1] = cross[1] * recip;
1211 quat[2] = cross[2] * recip;
1212 quat[3] = s * 0.5f;
1213 }
1214}
1215
1216
1217REAL fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL *LineEnd,REAL *intersection,LineSegmentType &type,REAL epsilon)
1218{
1219 REAL ret;
1220
1221 REAL LineMag = fm_distance( LineEnd, LineStart );
1222
1223 if ( LineMag > 0 )
1224 {
1225 REAL U = ( ( ( Point[0] - LineStart[0] ) * ( LineEnd[0] - LineStart[0] ) ) + ( ( Point[1] - LineStart[1] ) * ( LineEnd[1] - LineStart[1] ) ) + ( ( Point[2] - LineStart[2] ) * ( LineEnd[2] - LineStart[2] ) ) ) / ( LineMag * LineMag );
1226 if( U < 0.0f || U > 1.0f )
1227 {
1228 REAL d1 = fm_distanceSquared(Point,LineStart);
1229 REAL d2 = fm_distanceSquared(Point,LineEnd);
1230 if ( d1 <= d2 )
1231 {
1232 ret = (REAL)sqrt(d1);
1233 intersection[0] = LineStart[0];
1234 intersection[1] = LineStart[1];
1235 intersection[2] = LineStart[2];
1236 type = LS_START;
1237 }
1238 else
1239 {
1240 ret = (REAL)sqrt(d2);
1241 intersection[0] = LineEnd[0];
1242 intersection[1] = LineEnd[1];
1243 intersection[2] = LineEnd[2];
1244 type = LS_END;
1245 }
1246 }
1247 else
1248 {
1249 intersection[0] = LineStart[0] + U * ( LineEnd[0] - LineStart[0] );
1250 intersection[1] = LineStart[1] + U * ( LineEnd[1] - LineStart[1] );
1251 intersection[2] = LineStart[2] + U * ( LineEnd[2] - LineStart[2] );
1252
1253 ret = fm_distance(Point,intersection);
1254
1255 REAL d1 = fm_distanceSquared(intersection,LineStart);
1256 REAL d2 = fm_distanceSquared(intersection,LineEnd);
1257 REAL mag = (epsilon*2)*(epsilon*2);
1258
1259 if ( d1 < mag ) // if less than 1/100th the total distance, treat is as the 'start'
1260 {
1261 type = LS_START;
1262 }
1263 else if ( d2 < mag )
1264 {
1265 type = LS_END;
1266 }
1267 else
1268 {
1269 type = LS_MIDDLE;
1270 }
1271
1272 }
1273 }
1274 else
1275 {
1276 ret = LineMag;
1277 intersection[0] = LineEnd[0];
1278 intersection[1] = LineEnd[1];
1279 intersection[2] = LineEnd[2];
1280 type = LS_END;
1281 }
1282
1283 return ret;
1284}
1285
1286
1287#ifndef BEST_FIT_PLANE_H
1288
1289#define BEST_FIT_PLANE_H
1290
1291template <class Type> class Eigen
1292{
1293public:
1294
1295
1296 void DecrSortEigenStuff(void)
1297 {
1298 Tridiagonal(); //diagonalize the matrix.
1299 QLAlgorithm(); //
1300 DecreasingSort();
1301 GuaranteeRotation();
1302 }
1303
1304 void Tridiagonal(void)
1305 {
1306 Type fM00 = mElement[0][0];
1307 Type fM01 = mElement[0][1];
1308 Type fM02 = mElement[0][2];
1309 Type fM11 = mElement[1][1];
1310 Type fM12 = mElement[1][2];
1311 Type fM22 = mElement[2][2];
1312
1313 m_afDiag[0] = fM00;
1314 m_afSubd[2] = 0;
1315 if (fM02 != (Type)0.0)
1316 {
1317 Type fLength = (REAL)sqrt(fM01*fM01+fM02*fM02);
1318 Type fInvLength = ((Type)1.0)/fLength;
1319 fM01 *= fInvLength;
1320 fM02 *= fInvLength;
1321 Type fQ = ((Type)2.0)*fM01*fM12+fM02*(fM22-fM11);
1322 m_afDiag[1] = fM11+fM02*fQ;
1323 m_afDiag[2] = fM22-fM02*fQ;
1324 m_afSubd[0] = fLength;
1325 m_afSubd[1] = fM12-fM01*fQ;
1326 mElement[0][0] = (Type)1.0;
1327 mElement[0][1] = (Type)0.0;
1328 mElement[0][2] = (Type)0.0;
1329 mElement[1][0] = (Type)0.0;
1330 mElement[1][1] = fM01;
1331 mElement[1][2] = fM02;
1332 mElement[2][0] = (Type)0.0;
1333 mElement[2][1] = fM02;
1334 mElement[2][2] = -fM01;
1335 m_bIsRotation = false;
1336 }
1337 else
1338 {
1339 m_afDiag[1] = fM11;
1340 m_afDiag[2] = fM22;
1341 m_afSubd[0] = fM01;
1342 m_afSubd[1] = fM12;
1343 mElement[0][0] = (Type)1.0;
1344 mElement[0][1] = (Type)0.0;
1345 mElement[0][2] = (Type)0.0;
1346 mElement[1][0] = (Type)0.0;
1347 mElement[1][1] = (Type)1.0;
1348 mElement[1][2] = (Type)0.0;
1349 mElement[2][0] = (Type)0.0;
1350 mElement[2][1] = (Type)0.0;
1351 mElement[2][2] = (Type)1.0;
1352 m_bIsRotation = true;
1353 }
1354 }
1355
1356 bool QLAlgorithm(void)
1357 {
1358 const int32_t iMaxIter = 32;
1359
1360 for (int32_t i0 = 0; i0 <3; i0++)
1361 {
1362 int32_t i1;
1363 for (i1 = 0; i1 < iMaxIter; i1++)
1364 {
1365 int32_t i2;
1366 for (i2 = i0; i2 <= (3-2); i2++)
1367 {
1368 Type fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
1369 if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
1370 break;
1371 }
1372 if (i2 == i0)
1373 {
1374 break;
1375 }
1376
1377 Type fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((Type)2.0) * m_afSubd[i0]);
1378 Type fR = (REAL)sqrt(fG*fG+(Type)1.0);
1379 if (fG < (Type)0.0)
1380 {
1381 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
1382 }
1383 else
1384 {
1385 fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
1386 }
1387 Type fSin = (Type)1.0, fCos = (Type)1.0, fP = (Type)0.0;
1388 for (int32_t i3 = i2-1; i3 >= i0; i3--)
1389 {
1390 Type fF = fSin*m_afSubd[i3];
1391 Type fB = fCos*m_afSubd[i3];
1392 if (fabs(fF) >= fabs(fG))
1393 {
1394 fCos = fG/fF;
1395 fR = (REAL)sqrt(fCos*fCos+(Type)1.0);
1396 m_afSubd[i3+1] = fF*fR;
1397 fSin = ((Type)1.0)/fR;
1398 fCos *= fSin;
1399 }
1400 else
1401 {
1402 fSin = fF/fG;
1403 fR = (REAL)sqrt(fSin*fSin+(Type)1.0);
1404 m_afSubd[i3+1] = fG*fR;
1405 fCos = ((Type)1.0)/fR;
1406 fSin *= fCos;
1407 }
1408 fG = m_afDiag[i3+1]-fP;
1409 fR = (m_afDiag[i3]-fG)*fSin+((Type)2.0)*fB*fCos;
1410 fP = fSin*fR;
1411 m_afDiag[i3+1] = fG+fP;
1412 fG = fCos*fR-fB;
1413 for (int32_t i4 = 0; i4 < 3; i4++)
1414 {
1415 fF = mElement[i4][i3+1];
1416 mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
1417 mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
1418 }
1419 }
1420 m_afDiag[i0] -= fP;
1421 m_afSubd[i0] = fG;
1422 m_afSubd[i2] = (Type)0.0;
1423 }
1424 if (i1 == iMaxIter)
1425 {
1426 return false;
1427 }
1428 }
1429 return true;
1430 }
1431
1432 void DecreasingSort(void)
1433 {
1434 //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
1435 for (int32_t i0 = 0, i1; i0 <= 3-2; i0++)
1436 {
1437 // locate maximum eigenvalue
1438 i1 = i0;
1439 Type fMax = m_afDiag[i1];
1440 int32_t i2;
1441 for (i2 = i0+1; i2 < 3; i2++)
1442 {
1443 if (m_afDiag[i2] > fMax)
1444 {
1445 i1 = i2;
1446 fMax = m_afDiag[i1];
1447 }
1448 }
1449
1450 if (i1 != i0)
1451 {
1452 // swap eigenvalues
1453 m_afDiag[i1] = m_afDiag[i0];
1454 m_afDiag[i0] = fMax;
1455 // swap eigenvectors
1456 for (i2 = 0; i2 < 3; i2++)
1457 {
1458 Type fTmp = mElement[i2][i0];
1459 mElement[i2][i0] = mElement[i2][i1];
1460 mElement[i2][i1] = fTmp;
1461 m_bIsRotation = !m_bIsRotation;
1462 }
1463 }
1464 }
1465 }
1466
1467
1468 void GuaranteeRotation(void)
1469 {
1470 if (!m_bIsRotation)
1471 {
1472 // change sign on the first column
1473 for (int32_t iRow = 0; iRow <3; iRow++)
1474 {
1475 mElement[iRow][0] = -mElement[iRow][0];
1476 }
1477 }
1478 }
1479
1480 Type mElement[3][3];
1481 Type m_afDiag[3];
1482 Type m_afSubd[3];
1483 bool m_bIsRotation;
1484};
1485
1486#endif
1487
1488bool fm_computeBestFitPlane(uint32_t vcount,
1489 const REAL *points,
1490 uint32_t vstride,
1491 const REAL *weights,
1492 uint32_t wstride,
1493 REAL *plane,
1494 REAL *center)
1495{
1496 bool ret = false;
1497
1498 REAL kOrigin[3] = { 0, 0, 0 };
1499
1500 REAL wtotal = 0;
1501
1502 {
1503 const char *source = (const char *) points;
1504 const char *wsource = (const char *) weights;
1505
1506 for (uint32_t i=0; i<vcount; i++)
1507 {
1508
1509 const REAL *p = (const REAL *) source;
1510
1511 REAL w = 1;
1512
1513 if ( wsource )
1514 {
1515 const REAL *ws = (const REAL *) wsource;
1516 w = *ws; //
1517 wsource+=wstride;
1518 }
1519
1520 kOrigin[0]+=p[0]*w;
1521 kOrigin[1]+=p[1]*w;
1522 kOrigin[2]+=p[2]*w;
1523
1524 wtotal+=w;
1525
1526 source+=vstride;
1527 }
1528 }
1529
1530 REAL recip = 1.0f / wtotal; // reciprocol of total weighting
1531
1532 kOrigin[0]*=recip;
1533 kOrigin[1]*=recip;
1534 kOrigin[2]*=recip;
1535
1536 center[0] = kOrigin[0];
1537 center[1] = kOrigin[1];
1538 center[2] = kOrigin[2];
1539
1540
1541 REAL fSumXX=0;
1542 REAL fSumXY=0;
1543 REAL fSumXZ=0;
1544
1545 REAL fSumYY=0;
1546 REAL fSumYZ=0;
1547 REAL fSumZZ=0;
1548
1549
1550 {
1551 const char *source = (const char *) points;
1552 const char *wsource = (const char *) weights;
1553
1554 for (uint32_t i=0; i<vcount; i++)
1555 {
1556
1557 const REAL *p = (const REAL *) source;
1558
1559 REAL w = 1;
1560
1561 if ( wsource )
1562 {
1563 const REAL *ws = (const REAL *) wsource;
1564 w = *ws; //
1565 wsource+=wstride;
1566 }
1567
1568 REAL kDiff[3];
1569
1570 kDiff[0] = w*(p[0] - kOrigin[0]); // apply vertex weighting!
1571 kDiff[1] = w*(p[1] - kOrigin[1]);
1572 kDiff[2] = w*(p[2] - kOrigin[2]);
1573
1574 fSumXX+= kDiff[0] * kDiff[0]; // sume of the squares of the differences.
1575 fSumXY+= kDiff[0] * kDiff[1]; // sume of the squares of the differences.
1576 fSumXZ+= kDiff[0] * kDiff[2]; // sume of the squares of the differences.
1577
1578 fSumYY+= kDiff[1] * kDiff[1];
1579 fSumYZ+= kDiff[1] * kDiff[2];
1580 fSumZZ+= kDiff[2] * kDiff[2];
1581
1582
1583 source+=vstride;
1584 }
1585 }
1586
1587 fSumXX *= recip;
1588 fSumXY *= recip;
1589 fSumXZ *= recip;
1590 fSumYY *= recip;
1591 fSumYZ *= recip;
1592 fSumZZ *= recip;
1593
1594 // setup the eigensolver
1595 Eigen<REAL> kES;
1596
1597 kES.mElement[0][0] = fSumXX;
1598 kES.mElement[0][1] = fSumXY;
1599 kES.mElement[0][2] = fSumXZ;
1600
1601 kES.mElement[1][0] = fSumXY;
1602 kES.mElement[1][1] = fSumYY;
1603 kES.mElement[1][2] = fSumYZ;
1604
1605 kES.mElement[2][0] = fSumXZ;
1606 kES.mElement[2][1] = fSumYZ;
1607 kES.mElement[2][2] = fSumZZ;
1608
1609 // compute eigenstuff, smallest eigenvalue is in last position
1610 kES.DecrSortEigenStuff();
1611
1612 REAL kNormal[3];
1613
1614 kNormal[0] = kES.mElement[0][2];
1615 kNormal[1] = kES.mElement[1][2];
1616 kNormal[2] = kES.mElement[2][2];
1617
1618 // the minimum energy
1619 plane[0] = kNormal[0];
1620 plane[1] = kNormal[1];
1621 plane[2] = kNormal[2];
1622
1623 plane[3] = 0 - fm_dot(kNormal,kOrigin);
1624
1625 ret = true;
1626
1627 return ret;
1628}
1629
1630
1631bool fm_colinear(const REAL a1[3],const REAL a2[3],const REAL b1[3],const REAL b2[3],REAL epsilon) // true if these two line segments are co-linear.
1632{
1633 bool ret = false;
1634
1635 REAL dir1[3];
1636 REAL dir2[3];
1637
1638 dir1[0] = (a2[0] - a1[0]);
1639 dir1[1] = (a2[1] - a1[1]);
1640 dir1[2] = (a2[2] - a1[2]);
1641
1642 dir2[0] = (b2[0]-a1[0]) - (b1[0]-a1[0]);
1643 dir2[1] = (b2[1]-a1[1]) - (b1[1]-a1[1]);
1644 dir2[2] = (b2[2]-a2[2]) - (b1[2]-a2[2]);
1645
1646 fm_normalize(dir1);
1647 fm_normalize(dir2);
1648
1649 REAL dot = fm_dot(dir1,dir2);
1650
1651 if ( dot >= epsilon )
1652 {
1653 ret = true;
1654 }
1655
1656
1657 return ret;
1658}
1659
1660bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)
1661{
1662 bool ret = false;
1663
1664 REAL dir1[3];
1665 REAL dir2[3];
1666
1667 dir1[0] = p2[0] - p1[0];
1668 dir1[1] = p2[1] - p1[1];
1669 dir1[2] = p2[2] - p1[2];
1670
1671 dir2[0] = p3[0] - p2[0];
1672 dir2[1] = p3[1] - p2[1];
1673 dir2[2] = p3[2] - p2[2];
1674
1675 fm_normalize(dir1);
1676 fm_normalize(dir2);
1677
1678 REAL dot = fm_dot(dir1,dir2);
1679
1680 if ( dot >= epsilon )
1681 {
1682 ret = true;
1683 }
1684
1685
1686 return ret;
1687}
1688
1689void fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax)
1690{
1691 bmax[0] = bmin[0] = p[0];
1692 bmax[1] = bmin[1] = p[1];
1693 bmax[2] = bmin[2] = p[2];
1694}
1695
1696IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL *intersection)
1697{
1698 IntersectResult ret;
1699
1700 REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));
1701 REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));
1702 REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));
1703 if (denom == 0 )
1704 {
1705 if(nume_a == 0 && nume_b == 0)
1706 {
1707 ret = IR_COINCIDENT;
1708 }
1709 else
1710 {
1711 ret = IR_PARALLEL;
1712 }
1713 }
1714 else
1715 {
1716
1717 REAL recip = 1 / denom;
1718 REAL ua = nume_a * recip;
1719 REAL ub = nume_b * recip;
1720
1721 if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
1722 {
1723 // Get the intersection point.
1724 intersection[0] = a1[0] + ua*(a2[0] - a1[0]);
1725 intersection[1] = a1[1] + ua*(a2[1] - a1[1]);
1726 ret = IR_DO_INTERSECT;
1727 }
1728 else
1729 {
1730 ret = IR_DONT_INTERSECT;
1731 }
1732 }
1733 return ret;
1734}
1735
1736IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL &t1,REAL &t2)
1737{
1738 IntersectResult ret;
1739
1740 REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));
1741 REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));
1742 REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));
1743 if (denom == 0 )
1744 {
1745 if(nume_a == 0 && nume_b == 0)
1746 {
1747 ret = IR_COINCIDENT;
1748 }
1749 else
1750 {
1751 ret = IR_PARALLEL;
1752 }
1753 }
1754 else
1755 {
1756
1757 REAL recip = 1 / denom;
1758 REAL ua = nume_a * recip;
1759 REAL ub = nume_b * recip;
1760
1761 if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
1762 {
1763 t1 = ua;
1764 t2 = ub;
1765 ret = IR_DO_INTERSECT;
1766 }
1767 else
1768 {
1769 ret = IR_DONT_INTERSECT;
1770 }
1771 }
1772 return ret;
1773}
1774
1775//**** Plane Triangle Intersection
1776
1777
1778
1779
1780
1781// assumes that the points are on opposite sides of the plane!
1782bool fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL *plane)
1783{
1784
1785 REAL dp1 = fm_distToPlane(plane,p1);
1786 REAL dp2 = fm_distToPlane(plane, p2);
1787 if (dp1 <= 0 && dp2 <= 0)
1788 {
1789 return false;
1790 }
1791 if (dp1 >= 0 && dp2 >= 0)
1792 {
1793 return false;
1794 }
1795
1796 REAL dir[3];
1797
1798 dir[0] = p2[0] - p1[0];
1799 dir[1] = p2[1] - p1[1];
1800 dir[2] = p2[2] - p1[2];
1801
1802 REAL dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
1803 REAL dot2 = dp1 - plane[3];
1804
1805 REAL t = -(plane[3] + dot2 ) / dot1;
1806
1807 split[0] = (dir[0]*t)+p1[0];
1808 split[1] = (dir[1]*t)+p1[1];
1809 split[2] = (dir[2]*t)+p1[2];
1810
1811 return true;
1812}
1813
1814PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon)
1815{
1816 PlaneTriResult ret = PTR_ON_PLANE;
1817
1818 REAL d = fm_distToPlane(plane,p);
1819
1820 if ( d < -epsilon || d > epsilon )
1821 {
1822 if ( d > 0 )
1823 ret = PTR_FRONT; // it is 'in front' within the provided epsilon value.
1824 else
1825 ret = PTR_BACK;
1826 }
1827
1828 return ret;
1829}
1830
1831
1832
1833#ifndef PLANE_TRIANGLE_INTERSECTION_H
1834
1835#define PLANE_TRIANGLE_INTERSECTION_H
1836
1837#define MAXPTS 256
1838
1839template <class Type> class point
1840{
1841public:
1842
1843 void set(const Type *p)
1844 {
1845 x = p[0];
1846 y = p[1];
1847 z = p[2];
1848 }
1849
1850 Type x;
1851 Type y;
1852 Type z;
1853};
1854
1855template <class Type> class plane
1856{
1857public:
1858 plane(const Type *p)
1859 {
1860 normal.x = p[0];
1861 normal.y = p[1];
1862 normal.z = p[2];
1863 D = p[3];
1864 }
1865
1866 Type Classify_Point(const point<Type> &p)
1867 {
1868 return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;
1869 }
1870
1871 point<Type> normal;
1872 Type D;
1873};
1874
1875template <class Type> class polygon
1876{
1877public:
1878 polygon(void)
1879 {
1880 mVcount = 0;
1881 }
1882
1883 polygon(const Type *p1,const Type *p2,const Type *p3)
1884 {
1885 mVcount = 3;
1886 mVertices[0].set(p1);
1887 mVertices[1].set(p2);
1888 mVertices[2].set(p3);
1889 }
1890
1891
1892 int32_t NumVertices(void) const { return mVcount; };
1893
1894 const point<Type>& Vertex(int32_t index)
1895 {
1896 if ( index < 0 ) index+=mVcount;
1897 return mVertices[index];
1898 };
1899
1900
1901 void set(const point<Type> *pts,int32_t count)
1902 {
1903 for (int32_t i=0; i<count; i++)
1904 {
1905 mVertices[i] = pts[i];
1906 }
1907 mVcount = count;
1908 }
1909
1910
1911 void Split_Polygon(polygon<Type> *poly,plane<Type> *part, polygon<Type> &front, polygon<Type> &back)
1912 {
1913 int32_t count = poly->NumVertices ();
1914 int32_t out_c = 0, in_c = 0;
1915 point<Type> ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];
1916 Type sideA, sideB;
1917 ptA = poly->Vertex (count - 1);
1918 sideA = part->Classify_Point (ptA);
1919 for (int32_t i = -1; ++i < count;)
1920 {
1921 ptB = poly->Vertex(i);
1922 sideB = part->Classify_Point(ptB);
1923 if (sideB > 0)
1924 {
1925 if (sideA < 0)
1926 {
1927 point<Type> v;
1928 fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
1929 outpts[out_c++] = inpts[in_c++] = v;
1930 }
1931 outpts[out_c++] = ptB;
1932 }
1933 else if (sideB < 0)
1934 {
1935 if (sideA > 0)
1936 {
1937 point<Type> v;
1938 fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
1939 outpts[out_c++] = inpts[in_c++] = v;
1940 }
1941 inpts[in_c++] = ptB;
1942 }
1943 else
1944 outpts[out_c++] = inpts[in_c++] = ptB;
1945 ptA = ptB;
1946 sideA = sideB;
1947 }
1948
1949 front.set(&outpts[0], out_c);
1950 back.set(&inpts[0], in_c);
1951 }
1952
1953 int32_t mVcount;
1954 point<Type> mVertices[MAXPTS];
1955};
1956
1957
1958
1959#endif
1960
1961static inline void add(const REAL *p,REAL *dest,uint32_t tstride,uint32_t &pcount)
1962{
1963 char *d = (char *) dest;
1964 d = d + pcount*tstride;
1965 dest = (REAL *) d;
1966 dest[0] = p[0];
1967 dest[1] = p[1];
1968 dest[2] = p[2];
1969 pcount++;
1970 assert( pcount <= 4 );
1971}
1972
1973
1974PlaneTriResult fm_planeTriIntersection(const REAL *_plane, // the plane equation in Ax+By+Cz+D format
1975 const REAL *triangle, // the source triangle.
1976 uint32_t tstride, // stride in bytes of the input and output *vertices*
1977 REAL epsilon, // the co-planar epsilon value.
1978 REAL *front, // the triangle in front of the
1979 uint32_t &fcount, // number of vertices in the 'front' triangle
1980 REAL *back, // the triangle in back of the plane
1981 uint32_t &bcount) // the number of vertices in the 'back' triangle.
1982{
1983
1984 fcount = 0;
1985 bcount = 0;
1986
1987 const char *tsource = (const char *) triangle;
1988
1989 // get the three vertices of the triangle.
1990 const REAL *p1 = (const REAL *) (tsource);
1991 const REAL *p2 = (const REAL *) (tsource+tstride);
1992 const REAL *p3 = (const REAL *) (tsource+tstride*2);
1993
1994
1995 PlaneTriResult r1 = fm_getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on
1996 PlaneTriResult r2 = fm_getSidePlane(p2,_plane,epsilon);
1997 PlaneTriResult r3 = fm_getSidePlane(p3,_plane,epsilon);
1998
1999 // If any of the points lay right *on* the plane....
2000 if ( r1 == PTR_ON_PLANE || r2 == PTR_ON_PLANE || r3 == PTR_ON_PLANE )
2001 {
2002 // If the triangle is completely co-planar, then just treat it as 'front' and return!
2003 if ( r1 == PTR_ON_PLANE && r2 == PTR_ON_PLANE && r3 == PTR_ON_PLANE )
2004 {
2005 add(p1,front,tstride,fcount);
2006 add(p2,front,tstride,fcount);
2007 add(p3,front,tstride,fcount);
2008 return PTR_FRONT;
2009 }
2010 // Decide to place the co-planar points on the same side as the co-planar point.
2011 PlaneTriResult r= PTR_ON_PLANE;
2012 if ( r1 != PTR_ON_PLANE )
2013 r = r1;
2014 else if ( r2 != PTR_ON_PLANE )
2015 r = r2;
2016 else if ( r3 != PTR_ON_PLANE )
2017 r = r3;
2018
2019 if ( r1 == PTR_ON_PLANE ) r1 = r;
2020 if ( r2 == PTR_ON_PLANE ) r2 = r;
2021 if ( r3 == PTR_ON_PLANE ) r3 = r;
2022
2023 }
2024
2025 if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.
2026 {
2027 if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.
2028 {
2029 add(p1,front,tstride,fcount);
2030 add(p2,front,tstride,fcount);
2031 add(p3,front,tstride,fcount);
2032 }
2033 else
2034 {
2035 add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle.
2036 add(p2,back,tstride,bcount);
2037 add(p3,back,tstride,bcount);
2038 }
2039 return r1; // if all three points are on the same side of the plane return result
2040 }
2041
2042
2043 polygon<REAL> pi(p1,p2,p3);
2044 polygon<REAL> pfront,pback;
2045
2046 plane<REAL> part(_plane);
2047
2048 pi.Split_Polygon(&pi,&part,pfront,pback);
2049
2050 for (int32_t i=0; i<pfront.mVcount; i++)
2051 {
2052 add( &pfront.mVertices[i].x, front, tstride, fcount );
2053 }
2054
2055 for (int32_t i=0; i<pback.mVcount; i++)
2056 {
2057 add( &pback.mVertices[i].x, back, tstride, bcount );
2058 }
2059
2060 PlaneTriResult ret = PTR_SPLIT;
2061
2062 if ( fcount < 3 ) fcount = 0;
2063 if ( bcount < 3 ) bcount = 0;
2064
2065 if ( fcount == 0 && bcount )
2066 ret = PTR_BACK;
2067
2068 if ( bcount == 0 && fcount )
2069 ret = PTR_FRONT;
2070
2071
2072 return ret;
2073}
2074
2075// computes the OBB for this set of points relative to this transform matrix.
2076void computeOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix)
2077{
2078 const char *src = (const char *) points;
2079
2080 REAL bmin[3] = { 1e9, 1e9, 1e9 };
2081 REAL bmax[3] = { -1e9, -1e9, -1e9 };
2082
2083 for (uint32_t i=0; i<vcount; i++)
2084 {
2085 const REAL *p = (const REAL *) src;
2086 REAL t[3];
2087
2088 fm_inverseRT(matrix, p, t ); // inverse rotate translate
2089
2090 if ( t[0] < bmin[0] ) bmin[0] = t[0];
2091 if ( t[1] < bmin[1] ) bmin[1] = t[1];
2092 if ( t[2] < bmin[2] ) bmin[2] = t[2];
2093
2094 if ( t[0] > bmax[0] ) bmax[0] = t[0];
2095 if ( t[1] > bmax[1] ) bmax[1] = t[1];
2096 if ( t[2] > bmax[2] ) bmax[2] = t[2];
2097
2098 src+=pstride;
2099 }
2100
2101 REAL center[3];
2102
2103 sides[0] = bmax[0]-bmin[0];
2104 sides[1] = bmax[1]-bmin[1];
2105 sides[2] = bmax[2]-bmin[2];
2106
2107 center[0] = sides[0]*0.5f+bmin[0];
2108 center[1] = sides[1]*0.5f+bmin[1];
2109 center[2] = sides[2]*0.5f+bmin[2];
2110
2111 REAL ocenter[3];
2112
2113 fm_rotate(matrix,center,ocenter);
2114
2115 matrix[12]+=ocenter[0];
2116 matrix[13]+=ocenter[1];
2117 matrix[14]+=ocenter[2];
2118
2119}
2120
2121void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix,bool bruteForce)
2122{
2123 REAL plane[4];
2124 REAL center[3];
2125 fm_computeBestFitPlane(vcount,points,pstride,0,0,plane,center);
2126 fm_planeToMatrix(plane,matrix);
2127 computeOBB( vcount, points, pstride, sides, matrix );
2128
2129 REAL refmatrix[16];
2130 memcpy(refmatrix,matrix,16*sizeof(REAL));
2131
2132 REAL volume = sides[0]*sides[1]*sides[2];
2133 if ( bruteForce )
2134 {
2135 for (REAL a=10; a<180; a+=10)
2136 {
2137 REAL quat[4];
2138 fm_eulerToQuat(0,a*FM_DEG_TO_RAD,0,quat);
2139 REAL temp[16];
2140 REAL pmatrix[16];
2141 fm_quatToMatrix(quat,temp);
2142 fm_matrixMultiply(temp,refmatrix,pmatrix);
2143 REAL psides[3];
2144 computeOBB( vcount, points, pstride, psides, pmatrix );
2145 REAL v = psides[0]*psides[1]*psides[2];
2146 if ( v < volume )
2147 {
2148 volume = v;
2149 memcpy(matrix,pmatrix,sizeof(REAL)*16);
2150 sides[0] = psides[0];
2151 sides[1] = psides[1];
2152 sides[2] = psides[2];
2153 }
2154 }
2155 }
2156}
2157
2158void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos,REAL *quat,bool bruteForce)
2159{
2160 REAL matrix[16];
2161 fm_computeBestFitOBB(vcount,points,pstride,sides,matrix,bruteForce);
2162 fm_getTranslation(matrix,pos);
2163 fm_matrixToQuat(matrix,quat);
2164}
2165
2166void fm_computeBestFitABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos)
2167{
2168 REAL bmin[3];
2169 REAL bmax[3];
2170
2171 bmin[0] = points[0];
2172 bmin[1] = points[1];
2173 bmin[2] = points[2];
2174
2175 bmax[0] = points[0];
2176 bmax[1] = points[1];
2177 bmax[2] = points[2];
2178
2179 const char *cp = (const char *) points;
2180 for (uint32_t i=0; i<vcount; i++)
2181 {
2182 const REAL *p = (const REAL *) cp;
2183
2184 if ( p[0] < bmin[0] ) bmin[0] = p[0];
2185 if ( p[1] < bmin[1] ) bmin[1] = p[1];
2186 if ( p[2] < bmin[2] ) bmin[2] = p[2];
2187
2188 if ( p[0] > bmax[0] ) bmax[0] = p[0];
2189 if ( p[1] > bmax[1] ) bmax[1] = p[1];
2190 if ( p[2] > bmax[2] ) bmax[2] = p[2];
2191
2192 cp+=pstride;
2193 }
2194
2195
2196 sides[0] = bmax[0] - bmin[0];
2197 sides[1] = bmax[1] - bmin[1];
2198 sides[2] = bmax[2] - bmin[2];
2199
2200 pos[0] = bmin[0]+sides[0]*0.5f;
2201 pos[1] = bmin[1]+sides[1]*0.5f;
2202 pos[2] = bmin[2]+sides[2]*0.5f;
2203
2204}
2205
2206
2207void fm_planeToMatrix(const REAL *plane,REAL *matrix) // convert a plane equation to a 4x4 rotation matrix
2208{
2209 REAL ref[3] = { 0, 1, 0 };
2210 REAL quat[4];
2211 fm_rotationArc(ref,plane,quat);
2212 fm_quatToMatrix(quat,matrix);
2213 REAL origin[3] = { 0, -plane[3], 0 };
2214 REAL center[3];
2215 fm_transform(matrix,origin,center);
2216 fm_setTranslation(center,matrix);
2217}
2218
2219void fm_planeToQuat(const REAL *plane,REAL *quat,REAL *pos) // convert a plane equation to a quaternion and translation
2220{
2221 REAL ref[3] = { 0, 1, 0 };
2222 REAL matrix[16];
2223 fm_rotationArc(ref,plane,quat);
2224 fm_quatToMatrix(quat,matrix);
2225 REAL origin[3] = { 0, plane[3], 0 };
2226 fm_transform(matrix,origin,pos);
2227}
2228
2229void fm_eulerMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
2230{
2231 REAL quat[4];
2232 fm_eulerToQuat(ax,ay,az,quat);
2233 fm_quatToMatrix(quat,matrix);
2234}
2235
2236
2237//**********************************************************
2238//**********************************************************
2239//**** Vertex Welding
2240//**********************************************************
2241//**********************************************************
2242
2243#ifndef VERTEX_INDEX_H
2244
2245#define VERTEX_INDEX_H
2246
2247namespace VERTEX_INDEX
2248{
2249
2250class KdTreeNode;
2251
2252typedef std::vector< KdTreeNode * > KdTreeNodeVector;
2253
2254enum Axes
2255{
2256 X_AXIS = 0,
2257 Y_AXIS = 1,
2258 Z_AXIS = 2
2259};
2260
2261class KdTreeFindNode
2262{
2263public:
2264 KdTreeFindNode(void)
2265 {
2266 mNode = 0;
2267 mDistance = 0;
2268 }
2269 KdTreeNode *mNode;
2270 double mDistance;
2271};
2272
2273class KdTreeInterface
2274{
2275public:
2276 virtual const double * getPositionDouble(uint32_t index) const = 0;
2277 virtual const float * getPositionFloat(uint32_t index) const = 0;
2278};
2279
2280class KdTreeNode
2281{
2282public:
2283 KdTreeNode(void)
2284 {
2285 mIndex = 0;
2286 mLeft = 0;
2287 mRight = 0;
2288 }
2289
2290 KdTreeNode(uint32_t index)
2291 {
2292 mIndex = index;
2293 mLeft = 0;
2294 mRight = 0;
2295 };
2296
2297 ~KdTreeNode(void)
2298 {
2299 }
2300
2301
2302 void addDouble(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
2303 {
2304 const double *nodePosition = iface->getPositionDouble( node->mIndex );
2305 const double *position = iface->getPositionDouble( mIndex );
2306 switch ( dim )
2307 {
2308 case X_AXIS:
2309 if ( nodePosition[0] <= position[0] )
2310 {
2311 if ( mLeft )
2312 mLeft->addDouble(node,Y_AXIS,iface);
2313 else
2314 mLeft = node;
2315 }
2316 else
2317 {
2318 if ( mRight )
2319 mRight->addDouble(node,Y_AXIS,iface);
2320 else
2321 mRight = node;
2322 }
2323 break;
2324 case Y_AXIS:
2325 if ( nodePosition[1] <= position[1] )
2326 {
2327 if ( mLeft )
2328 mLeft->addDouble(node,Z_AXIS,iface);
2329 else
2330 mLeft = node;
2331 }
2332 else
2333 {
2334 if ( mRight )
2335 mRight->addDouble(node,Z_AXIS,iface);
2336 else
2337 mRight = node;
2338 }
2339 break;
2340 case Z_AXIS:
2341 if ( nodePosition[2] <= position[2] )
2342 {
2343 if ( mLeft )
2344 mLeft->addDouble(node,X_AXIS,iface);
2345 else
2346 mLeft = node;
2347 }
2348 else
2349 {
2350 if ( mRight )
2351 mRight->addDouble(node,X_AXIS,iface);
2352 else
2353 mRight = node;
2354 }
2355 break;
2356 }
2357
2358 }
2359
2360
2361 void addFloat(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
2362 {
2363 const float *nodePosition = iface->getPositionFloat( node->mIndex );
2364 const float *position = iface->getPositionFloat( mIndex );
2365 switch ( dim )
2366 {
2367 case X_AXIS:
2368 if ( nodePosition[0] <= position[0] )
2369 {
2370 if ( mLeft )
2371 mLeft->addFloat(node,Y_AXIS,iface);
2372 else
2373 mLeft = node;
2374 }
2375 else
2376 {
2377 if ( mRight )
2378 mRight->addFloat(node,Y_AXIS,iface);
2379 else
2380 mRight = node;
2381 }
2382 break;
2383 case Y_AXIS:
2384 if ( nodePosition[1] <= position[1] )
2385 {
2386 if ( mLeft )
2387 mLeft->addFloat(node,Z_AXIS,iface);
2388 else
2389 mLeft = node;
2390 }
2391 else
2392 {
2393 if ( mRight )
2394 mRight->addFloat(node,Z_AXIS,iface);
2395 else
2396 mRight = node;
2397 }
2398 break;
2399 case Z_AXIS:
2400 if ( nodePosition[2] <= position[2] )
2401 {
2402 if ( mLeft )
2403 mLeft->addFloat(node,X_AXIS,iface);
2404 else
2405 mLeft = node;
2406 }
2407 else
2408 {
2409 if ( mRight )
2410 mRight->addFloat(node,X_AXIS,iface);
2411 else
2412 mRight = node;
2413 }
2414 break;
2415 }
2416
2417 }
2418
2419
2420 uint32_t getIndex(void) const { return mIndex; };
2421
2422 void search(Axes axis,const double *pos,double radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
2423 {
2424
2425 const double *position = iface->getPositionDouble(mIndex);
2426
2427 double dx = pos[0] - position[0];
2428 double dy = pos[1] - position[1];
2429 double dz = pos[2] - position[2];
2430
2431 KdTreeNode *search1 = 0;
2432 KdTreeNode *search2 = 0;
2433
2434 switch ( axis )
2435 {
2436 case X_AXIS:
2437 if ( dx <= 0 ) // JWR if we are to the left
2438 {
2439 search1 = mLeft; // JWR then search to the left
2440 if ( -dx < radius ) // JWR if distance to the right is less than our search radius, continue on the right as well.
2441 search2 = mRight;
2442 }
2443 else
2444 {
2445 search1 = mRight; // JWR ok, we go down the left tree
2446 if ( dx < radius ) // JWR if the distance from the right is less than our search radius
2447 search2 = mLeft;
2448 }
2449 axis = Y_AXIS;
2450 break;
2451 case Y_AXIS:
2452 if ( dy <= 0 )
2453 {
2454 search1 = mLeft;
2455 if ( -dy < radius )
2456 search2 = mRight;
2457 }
2458 else
2459 {
2460 search1 = mRight;
2461 if ( dy < radius )
2462 search2 = mLeft;
2463 }
2464 axis = Z_AXIS;
2465 break;
2466 case Z_AXIS:
2467 if ( dz <= 0 )
2468 {
2469 search1 = mLeft;
2470 if ( -dz < radius )
2471 search2 = mRight;
2472 }
2473 else
2474 {
2475 search1 = mRight;
2476 if ( dz < radius )
2477 search2 = mLeft;
2478 }
2479 axis = X_AXIS;
2480 break;
2481 }
2482
2483 double r2 = radius*radius;
2484 double m = dx*dx+dy*dy+dz*dz;
2485
2486 if ( m < r2 )
2487 {
2488 switch ( count )
2489 {
2490 case 0:
2491 found[count].mNode = this;
2492 found[count].mDistance = m;
2493 break;
2494 case 1:
2495 if ( m < found[0].mDistance )
2496 {
2497 if ( maxObjects == 1 )
2498 {
2499 found[0].mNode = this;
2500 found[0].mDistance = m;
2501 }
2502 else
2503 {
2504 found[1] = found[0];
2505 found[0].mNode = this;
2506 found[0].mDistance = m;
2507 }
2508 }
2509 else if ( maxObjects > 1)
2510 {
2511 found[1].mNode = this;
2512 found[1].mDistance = m;
2513 }
2514 break;
2515 default:
2516 {
2517 bool inserted = false;
2518
2519 for (uint32_t i=0; i<count; i++)
2520 {
2521 if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...
2522 {
2523 // insertion sort...
2524 uint32_t scan = count;
2525 if ( scan >= maxObjects ) scan=maxObjects-1;
2526 for (uint32_t j=scan; j>i; j--)
2527 {
2528 found[j] = found[j-1];
2529 }
2530 found[i].mNode = this;
2531 found[i].mDistance = m;
2532 inserted = true;
2533 break;
2534 }
2535 }
2536
2537 if ( !inserted && count < maxObjects )
2538 {
2539 found[count].mNode = this;
2540 found[count].mDistance = m;
2541 }
2542 }
2543 break;
2544 }
2545 count++;
2546 if ( count > maxObjects )
2547 {
2548 count = maxObjects;
2549 }
2550 }
2551
2552
2553 if ( search1 )
2554 search1->search( axis, pos,radius, count, maxObjects, found, iface);
2555
2556 if ( search2 )
2557 search2->search( axis, pos,radius, count, maxObjects, found, iface);
2558
2559 }
2560
2561 void search(Axes axis,const float *pos,float radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
2562 {
2563
2564 const float *position = iface->getPositionFloat(mIndex);
2565
2566 float dx = pos[0] - position[0];
2567 float dy = pos[1] - position[1];
2568 float dz = pos[2] - position[2];
2569
2570 KdTreeNode *search1 = 0;
2571 KdTreeNode *search2 = 0;
2572
2573 switch ( axis )
2574 {
2575 case X_AXIS:
2576 if ( dx <= 0 ) // JWR if we are to the left
2577 {
2578 search1 = mLeft; // JWR then search to the left
2579 if ( -dx < radius ) // JWR if distance to the right is less than our search radius, continue on the right as well.
2580 search2 = mRight;
2581 }
2582 else
2583 {
2584 search1 = mRight; // JWR ok, we go down the left tree
2585 if ( dx < radius ) // JWR if the distance from the right is less than our search radius
2586 search2 = mLeft;
2587 }
2588 axis = Y_AXIS;
2589 break;
2590 case Y_AXIS:
2591 if ( dy <= 0 )
2592 {
2593 search1 = mLeft;
2594 if ( -dy < radius )
2595 search2 = mRight;
2596 }
2597 else
2598 {
2599 search1 = mRight;
2600 if ( dy < radius )
2601 search2 = mLeft;
2602 }
2603 axis = Z_AXIS;
2604 break;
2605 case Z_AXIS:
2606 if ( dz <= 0 )
2607 {
2608 search1 = mLeft;
2609 if ( -dz < radius )
2610 search2 = mRight;
2611 }
2612 else
2613 {
2614 search1 = mRight;
2615 if ( dz < radius )
2616 search2 = mLeft;
2617 }
2618 axis = X_AXIS;
2619 break;
2620 }
2621
2622 float r2 = radius*radius;
2623 float m = dx*dx+dy*dy+dz*dz;
2624
2625 if ( m < r2 )
2626 {
2627 switch ( count )
2628 {
2629 case 0:
2630 found[count].mNode = this;
2631 found[count].mDistance = m;
2632 break;
2633 case 1:
2634 if ( m < found[0].mDistance )
2635 {
2636 if ( maxObjects == 1 )
2637 {
2638 found[0].mNode = this;
2639 found[0].mDistance = m;
2640 }
2641 else
2642 {
2643 found[1] = found[0];
2644 found[0].mNode = this;
2645 found[0].mDistance = m;
2646 }
2647 }
2648 else if ( maxObjects > 1)
2649 {
2650 found[1].mNode = this;
2651 found[1].mDistance = m;
2652 }
2653 break;
2654 default:
2655 {
2656 bool inserted = false;
2657
2658 for (uint32_t i=0; i<count; i++)
2659 {
2660 if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...
2661 {
2662 // insertion sort...
2663 uint32_t scan = count;
2664 if ( scan >= maxObjects ) scan=maxObjects-1;
2665 for (uint32_t j=scan; j>i; j--)
2666 {
2667 found[j] = found[j-1];
2668 }
2669 found[i].mNode = this;
2670 found[i].mDistance = m;
2671 inserted = true;
2672 break;
2673 }
2674 }
2675
2676 if ( !inserted && count < maxObjects )
2677 {
2678 found[count].mNode = this;
2679 found[count].mDistance = m;
2680 }
2681 }
2682 break;
2683 }
2684 count++;
2685 if ( count > maxObjects )
2686 {
2687 count = maxObjects;
2688 }
2689 }
2690
2691
2692 if ( search1 )
2693 search1->search( axis, pos,radius, count, maxObjects, found, iface);
2694
2695 if ( search2 )
2696 search2->search( axis, pos,radius, count, maxObjects, found, iface);
2697
2698 }
2699
2700private:
2701
2702 void setLeft(KdTreeNode *left) { mLeft = left; };
2703 void setRight(KdTreeNode *right) { mRight = right; };
2704
2705 KdTreeNode *getLeft(void) { return mLeft; }
2706 KdTreeNode *getRight(void) { return mRight; }
2707
2708 uint32_t mIndex;
2709 KdTreeNode *mLeft;
2710 KdTreeNode *mRight;
2711};
2712
2713
2714#define MAX_BUNDLE_SIZE 1024 // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent.
2715
2716class KdTreeNodeBundle
2717{
2718public:
2719
2720 KdTreeNodeBundle(void)
2721 {
2722 mNext = 0;
2723 mIndex = 0;
2724 }
2725
2726 bool isFull(void) const
2727 {
2728 return (bool)( mIndex == MAX_BUNDLE_SIZE );
2729 }
2730
2731 KdTreeNode * getNextNode(void)
2732 {
2733 assert(mIndex<MAX_BUNDLE_SIZE);
2734 KdTreeNode *ret = &mNodes[mIndex];
2735 mIndex++;
2736 return ret;
2737 }
2738
2739 KdTreeNodeBundle *mNext;
2740 uint32_t mIndex;
2741 KdTreeNode mNodes[MAX_BUNDLE_SIZE];
2742};
2743
2744
2745typedef std::vector< double > DoubleVector;
2746typedef std::vector< float > FloatVector;
2747
2748class KdTree : public KdTreeInterface
2749{
2750public:
2751 KdTree(void)
2752 {
2753 mRoot = 0;
2754 mBundle = 0;
2755 mVcount = 0;
2756 mUseDouble = false;
2757 }
2758
2759 virtual ~KdTree(void)
2760 {
2761 reset();
2762 }
2763
2764 const double * getPositionDouble(uint32_t index) const
2765 {
2766 assert( mUseDouble );
2767 assert ( index < mVcount );
2768 return &mVerticesDouble[index*3];
2769 }
2770
2771 const float * getPositionFloat(uint32_t index) const
2772 {
2773 assert( !mUseDouble );
2774 assert ( index < mVcount );
2775 return &mVerticesFloat[index*3];
2776 }
2777
2778 uint32_t search(const double *pos,double radius,uint32_t maxObjects,KdTreeFindNode *found) const
2779 {
2780 assert( mUseDouble );
2781 if ( !mRoot ) return 0;
2782 uint32_t count = 0;
2783 mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
2784 return count;
2785 }
2786
2787 uint32_t search(const float *pos,float radius,uint32_t maxObjects,KdTreeFindNode *found) const
2788 {
2789 assert( !mUseDouble );
2790 if ( !mRoot ) return 0;
2791 uint32_t count = 0;
2792 mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
2793 return count;
2794 }
2795
2796 void reset(void)
2797 {
2798 mRoot = 0;
2799 mVerticesDouble.clear();
2800 mVerticesFloat.clear();
2801 KdTreeNodeBundle *bundle = mBundle;
2802 while ( bundle )
2803 {
2804 KdTreeNodeBundle *next = bundle->mNext;
2805 delete bundle;
2806 bundle = next;
2807 }
2808 mBundle = 0;
2809 mVcount = 0;
2810 }
2811
2812 uint32_t add(double x,double y,double z)
2813 {
2814 assert(mUseDouble);
2815 uint32_t ret = mVcount;
2816 mVerticesDouble.push_back(x);
2817 mVerticesDouble.push_back(y);
2818 mVerticesDouble.push_back(z);
2819 mVcount++;
2820 KdTreeNode *node = getNewNode(ret);
2821 if ( mRoot )
2822 {
2823 mRoot->addDouble(node,X_AXIS,this);
2824 }
2825 else
2826 {
2827 mRoot = node;
2828 }
2829 return ret;
2830 }
2831
2832 uint32_t add(float x,float y,float z)
2833 {
2834 assert(!mUseDouble);
2835 uint32_t ret = mVcount;
2836 mVerticesFloat.push_back(x);
2837 mVerticesFloat.push_back(y);
2838 mVerticesFloat.push_back(z);
2839 mVcount++;
2840 KdTreeNode *node = getNewNode(ret);
2841 if ( mRoot )
2842 {
2843 mRoot->addFloat(node,X_AXIS,this);
2844 }
2845 else
2846 {
2847 mRoot = node;
2848 }
2849 return ret;
2850 }
2851
2852 KdTreeNode * getNewNode(uint32_t index)
2853 {
2854 if ( mBundle == 0 )
2855 {
2856 mBundle = new KdTreeNodeBundle;
2857 }
2858 if ( mBundle->isFull() )
2859 {
2860 KdTreeNodeBundle *bundle = new KdTreeNodeBundle;
2861 mBundle->mNext = bundle;
2862 mBundle = bundle;
2863 }
2864 KdTreeNode *node = mBundle->getNextNode();
2865 new ( node ) KdTreeNode(index);
2866 return node;
2867 }
2868
2869 uint32_t getNearest(const double *pos,double radius,bool &_found) const // returns the nearest possible neighbor's index.
2870 {
2871 assert( mUseDouble );
2872 uint32_t ret = 0;
2873
2874 _found = false;
2875 KdTreeFindNode found[1];
2876 uint32_t count = search(pos,radius,1,found);
2877 if ( count )
2878 {
2879 KdTreeNode *node = found[0].mNode;
2880 ret = node->getIndex();
2881 _found = true;
2882 }
2883 return ret;
2884 }
2885
2886 uint32_t getNearest(const float *pos,float radius,bool &_found) const // returns the nearest possible neighbor's index.
2887 {
2888 assert( !mUseDouble );
2889 uint32_t ret = 0;
2890
2891 _found = false;
2892 KdTreeFindNode found[1];
2893 uint32_t count = search(pos,radius,1,found);
2894 if ( count )
2895 {
2896 KdTreeNode *node = found[0].mNode;
2897 ret = node->getIndex();
2898 _found = true;
2899 }
2900 return ret;
2901 }
2902
2903 const double * getVerticesDouble(void) const
2904 {
2905 assert( mUseDouble );
2906 const double *ret = 0;
2907 if ( !mVerticesDouble.empty() )
2908 {
2909 ret = &mVerticesDouble[0];
2910 }
2911 return ret;
2912 }
2913
2914 const float * getVerticesFloat(void) const
2915 {
2916 assert( !mUseDouble );
2917 const float * ret = 0;
2918 if ( !mVerticesFloat.empty() )
2919 {
2920 ret = &mVerticesFloat[0];
2921 }
2922 return ret;
2923 }
2924
2925 uint32_t getVcount(void) const { return mVcount; };
2926
2927 void setUseDouble(bool useDouble)
2928 {
2929 mUseDouble = useDouble;
2930 }
2931
2932private:
2933 bool mUseDouble;
2934 KdTreeNode *mRoot;
2935 KdTreeNodeBundle *mBundle;
2936 uint32_t mVcount;
2937 DoubleVector mVerticesDouble;
2938 FloatVector mVerticesFloat;
2939};
2940
2941}; // end of namespace VERTEX_INDEX
2942
2943class MyVertexIndex : public fm_VertexIndex
2944{
2945public:
2946 MyVertexIndex(double granularity,bool snapToGrid)
2947 {
2948 mDoubleGranularity = granularity;
2949 mFloatGranularity = (float)granularity;
2950 mSnapToGrid = snapToGrid;
2951 mUseDouble = true;
2952 mKdTree.setUseDouble(true);
2953 }
2954
2955 MyVertexIndex(float granularity,bool snapToGrid)
2956 {
2957 mDoubleGranularity = granularity;
2958 mFloatGranularity = (float)granularity;
2959 mSnapToGrid = snapToGrid;
2960 mUseDouble = false;
2961 mKdTree.setUseDouble(false);
2962 }
2963
2964 virtual ~MyVertexIndex(void)
2965 {
2966
2967 }
2968
2969
2970 double snapToGrid(double p)
2971 {
2972 double m = fmod(p,mDoubleGranularity);
2973 p-=m;
2974 return p;
2975 }
2976
2977 float snapToGrid(float p)
2978 {
2979 float m = fmodf(p,mFloatGranularity);
2980 p-=m;
2981 return p;
2982 }
2983
2984 uint32_t getIndex(const float *_p,bool &newPos) // get index for a vector float
2985 {
2986 uint32_t ret;
2987
2988 if ( mUseDouble )
2989 {
2990 double p[3];
2991 p[0] = _p[0];
2992 p[1] = _p[1];
2993 p[2] = _p[2];
2994 return getIndex(p,newPos);
2995 }
2996
2997 newPos = false;
2998
2999 float p[3];
3000
3001 if ( mSnapToGrid )
3002 {
3003 p[0] = snapToGrid(_p[0]);
3004 p[1] = snapToGrid(_p[1]);
3005 p[2] = snapToGrid(_p[2]);
3006 }
3007 else
3008 {
3009 p[0] = _p[0];
3010 p[1] = _p[1];
3011 p[2] = _p[2];
3012 }
3013
3014 bool found;
3015 ret = mKdTree.getNearest(p,mFloatGranularity,found);
3016 if ( !found )
3017 {
3018 newPos = true;
3019 ret = mKdTree.add(p[0],p[1],p[2]);
3020 }
3021
3022
3023 return ret;
3024 }
3025
3026 uint32_t getIndex(const double *_p,bool &newPos) // get index for a vector double
3027 {
3028 uint32_t ret;
3029
3030 if ( !mUseDouble )
3031 {
3032 float p[3];
3033 p[0] = (float)_p[0];
3034 p[1] = (float)_p[1];
3035 p[2] = (float)_p[2];
3036 return getIndex(p,newPos);
3037 }
3038
3039 newPos = false;
3040
3041 double p[3];
3042
3043 if ( mSnapToGrid )
3044 {
3045 p[0] = snapToGrid(_p[0]);
3046 p[1] = snapToGrid(_p[1]);
3047 p[2] = snapToGrid(_p[2]);
3048 }
3049 else
3050 {
3051 p[0] = _p[0];
3052 p[1] = _p[1];
3053 p[2] = _p[2];
3054 }
3055
3056 bool found;
3057 ret = mKdTree.getNearest(p,mDoubleGranularity,found);
3058 if ( !found )
3059 {
3060 newPos = true;
3061 ret = mKdTree.add(p[0],p[1],p[2]);
3062 }
3063
3064
3065 return ret;
3066 }
3067
3068 const float * getVerticesFloat(void) const
3069 {
3070 const float * ret = 0;
3071
3072 assert( !mUseDouble );
3073
3074 ret = mKdTree.getVerticesFloat();
3075
3076 return ret;
3077 }
3078
3079 const double * getVerticesDouble(void) const
3080 {
3081 const double * ret = 0;
3082
3083 assert( mUseDouble );
3084
3085 ret = mKdTree.getVerticesDouble();
3086
3087 return ret;
3088 }
3089
3090 const float * getVertexFloat(uint32_t index) const
3091 {
3092 const float * ret = 0;
3093 assert( !mUseDouble );
3094#ifdef _DEBUG
3095 uint32_t vcount = mKdTree.getVcount();
3096 assert( index < vcount );
3097#endif
3098 ret = mKdTree.getVerticesFloat();
3099 ret = &ret[index*3];
3100 return ret;
3101 }
3102
3103 const double * getVertexDouble(uint32_t index) const
3104 {
3105 const double * ret = 0;
3106 assert( mUseDouble );
3107#ifdef _DEBUG
3108 uint32_t vcount = mKdTree.getVcount();
3109 assert( index < vcount );
3110#endif
3111 ret = mKdTree.getVerticesDouble();
3112 ret = &ret[index*3];
3113
3114 return ret;
3115 }
3116
3117 uint32_t getVcount(void) const
3118 {
3119 return mKdTree.getVcount();
3120 }
3121
3122 bool isDouble(void) const
3123 {
3124 return mUseDouble;
3125 }
3126
3127
3128 bool saveAsObj(const char *fname,uint32_t tcount,uint32_t *indices)
3129 {
3130 bool ret = false;
3131
3132
3133 FILE *fph = fopen(fname,"wb");
3134 if ( fph )
3135 {
3136 ret = true;
3137
3138 uint32_t vcount = getVcount();
3139 if ( mUseDouble )
3140 {
3141 const double *v = getVerticesDouble();
3142 for (uint32_t i=0; i<vcount; i++)
3143 {
3144 fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", (float)v[0], (float)v[1], (float)v[2] );
3145 v+=3;
3146 }
3147 }
3148 else
3149 {
3150 const float *v = getVerticesFloat();
3151 for (uint32_t i=0; i<vcount; i++)
3152 {
3153 fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", v[0], v[1], v[2] );
3154 v+=3;
3155 }
3156 }
3157
3158 for (uint32_t i=0; i<tcount; i++)
3159 {
3160 uint32_t i1 = *indices++;
3161 uint32_t i2 = *indices++;
3162 uint32_t i3 = *indices++;
3163 fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );
3164 }
3165 fclose(fph);
3166 }
3167
3168 return ret;
3169 }
3170
3171private:
3172 bool mUseDouble:1;
3173 bool mSnapToGrid:1;
3174 double mDoubleGranularity;
3175 float mFloatGranularity;
3176 VERTEX_INDEX::KdTree mKdTree;
3177};
3178
3179fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid) // create an indexed vertex system for doubles
3180{
3181 MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);
3182 return static_cast< fm_VertexIndex *>(ret);
3183}
3184
3185fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid) // create an indexed vertext system for floats
3186{
3187 MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);
3188 return static_cast< fm_VertexIndex *>(ret);
3189}
3190
3191void fm_releaseVertexIndex(fm_VertexIndex *vindex)
3192{
3193 MyVertexIndex *m = static_cast< MyVertexIndex *>(vindex);
3194 delete m;
3195}
3196
3197#endif // END OF VERTEX WELDING CODE
3198
3199
3200REAL fm_computeBestFitAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) // returns the diagonal distance
3201{
3202
3203 const uint8_t *source = (const uint8_t *) points;
3204
3205 bmin[0] = points[0];
3206 bmin[1] = points[1];
3207 bmin[2] = points[2];
3208
3209 bmax[0] = points[0];
3210 bmax[1] = points[1];
3211 bmax[2] = points[2];
3212
3213
3214 for (uint32_t i=1; i<vcount; i++)
3215 {
3216 source+=pstride;
3217 const REAL *p = (const REAL *) source;
3218
3219 if ( p[0] < bmin[0] ) bmin[0] = p[0];
3220 if ( p[1] < bmin[1] ) bmin[1] = p[1];
3221 if ( p[2] < bmin[2] ) bmin[2] = p[2];
3222
3223 if ( p[0] > bmax[0] ) bmax[0] = p[0];
3224 if ( p[1] > bmax[1] ) bmax[1] = p[1];
3225 if ( p[2] > bmax[2] ) bmax[2] = p[2];
3226
3227 }
3228
3229 REAL dx = bmax[0] - bmin[0];
3230 REAL dy = bmax[1] - bmin[1];
3231 REAL dz = bmax[2] - bmin[2];
3232
3233 return (REAL) sqrt( dx*dx + dy*dy + dz*dz );
3234
3235}
3236
3237
3238
3239/* a = b - c */
3240#define vector(a,b,c) \
3241 (a)[0] = (b)[0] - (c)[0]; \
3242 (a)[1] = (b)[1] - (c)[1]; \
3243 (a)[2] = (b)[2] - (c)[2];
3244
3245
3246
3247#define innerProduct(v,q) \
3248 ((v)[0] * (q)[0] + \
3249 (v)[1] * (q)[1] + \
3250 (v)[2] * (q)[2])
3251
3252#define crossProduct(a,b,c) \
3253 (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
3254 (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
3255 (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
3256
3257
3258bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL *p1,const REAL *p2,const REAL *p3,REAL *sect)
3259{
3260 REAL dir[3];
3261
3262 dir[0] = rayEnd[0] - rayStart[0];
3263 dir[1] = rayEnd[1] - rayStart[1];
3264 dir[2] = rayEnd[2] - rayStart[2];
3265
3266 REAL d = (REAL)sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
3267 REAL r = 1.0f / d;
3268
3269 dir[0]*=r;
3270 dir[1]*=r;
3271 dir[2]*=r;
3272
3273
3274 REAL t;
3275
3276 bool ret = fm_rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
3277
3278 if ( ret )
3279 {
3280 if ( t > d )
3281 {
3282 sect[0] = rayStart[0] + dir[0]*t;
3283 sect[1] = rayStart[1] + dir[1]*t;
3284 sect[2] = rayStart[2] + dir[2]*t;
3285 }
3286 else
3287 {
3288 ret = false;
3289 }
3290 }
3291
3292 return ret;
3293}
3294
3295
3296
3297bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL *v1,const REAL *v2,REAL &t)
3298{
3299 REAL e1[3],e2[3],h[3],s[3],q[3];
3300 REAL a,f,u,v;
3301
3302 vector(e1,v1,v0);
3303 vector(e2,v2,v0);
3304 crossProduct(h,d,e2);
3305 a = innerProduct(e1,h);
3306
3307 if (a > -0.00001 && a < 0.00001)
3308 return(false);
3309
3310 f = 1/a;
3311 vector(s,p,v0);
3312 u = f * (innerProduct(s,h));
3313
3314 if (u < 0.0 || u > 1.0)
3315 return(false);
3316
3317 crossProduct(q,s,e1);
3318 v = f * innerProduct(d,q);
3319 if (v < 0.0 || u + v > 1.0)
3320 return(false);
3321 // at this stage we can compute t to find out where
3322 // the intersection point is on the line
3323 t = f * innerProduct(e2,q);
3324 if (t > 0) // ray intersection
3325 return(true);
3326 else // this means that there is a line intersection
3327 // but not a ray intersection
3328 return (false);
3329}
3330
3331
3332inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3)
3333{
3334 return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
3335}
3336
3337
3338REAL fm_computeMeshVolume(const REAL *vertices,uint32_t tcount,const uint32_t *indices)
3339{
3340 REAL volume = 0;
3341
3342 for (uint32_t i=0; i<tcount; i++,indices+=3)
3343 {
3344 const REAL *p1 = &vertices[ indices[0]*3 ];
3345 const REAL *p2 = &vertices[ indices[1]*3 ];
3346 const REAL *p3 = &vertices[ indices[2]*3 ];
3347 volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.
3348 }
3349
3350 volume*=(1.0f/6.0f);
3351 if ( volume < 0 )
3352 volume*=-1;
3353 return volume;
3354}
3355
3356
3357const REAL * fm_getPoint(const REAL *points,uint32_t pstride,uint32_t index)
3358{
3359 const uint8_t *scan = (const uint8_t *)points;
3360 scan+=(index*pstride);
3361 return (REAL *)scan;
3362}
3363
3364
3365bool fm_insideTriangle(REAL Ax, REAL Ay,
3366 REAL Bx, REAL By,
3367 REAL Cx, REAL Cy,
3368 REAL Px, REAL Py)
3369
3370{
3371 REAL ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
3372 REAL cCROSSap, bCROSScp, aCROSSbp;
3373
3374 ax = Cx - Bx; ay = Cy - By;
3375 bx = Ax - Cx; by = Ay - Cy;
3376 cx = Bx - Ax; cy = By - Ay;
3377 apx= Px - Ax; apy= Py - Ay;
3378 bpx= Px - Bx; bpy= Py - By;
3379 cpx= Px - Cx; cpy= Py - Cy;
3380
3381 aCROSSbp = ax*bpy - ay*bpx;
3382 cCROSSap = cx*apy - cy*apx;
3383 bCROSScp = bx*cpy - by*cpx;
3384
3385 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
3386}
3387
3388
3389REAL fm_areaPolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride)
3390{
3391 int32_t n = (int32_t)pcount;
3392
3393 REAL A=0.0f;
3394 for(int32_t p=n-1,q=0; q<n; p=q++)
3395 {
3396 const REAL *p1 = fm_getPoint(points,pstride,p);
3397 const REAL *p2 = fm_getPoint(points,pstride,q);
3398 A+= p1[0]*p2[1] - p2[0]*p1[1];
3399 }
3400 return A*0.5f;
3401}
3402
3403
3404bool fm_pointInsidePolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride,const REAL *point,uint32_t xindex,uint32_t yindex)
3405{
3406 uint32_t j = pcount-1;
3407 int32_t oddNodes = 0;
3408
3409 REAL x = point[xindex];
3410 REAL y = point[yindex];
3411
3412 for (uint32_t i=0; i<pcount; i++)
3413 {
3414 const REAL *p1 = fm_getPoint(points,pstride,i);
3415 const REAL *p2 = fm_getPoint(points,pstride,j);
3416
3417 REAL x1 = p1[xindex];
3418 REAL y1 = p1[yindex];
3419
3420 REAL x2 = p2[xindex];
3421 REAL y2 = p2[yindex];
3422
3423 if ( (y1 < y && y2 >= y) || (y2 < y && y1 >= y) )
3424 {
3425 if (x1+(y-y1)/(y2-y1)*(x2-x1)<x)
3426 {
3427 oddNodes = 1-oddNodes;
3428 }
3429 }
3430 j = i;
3431 }
3432
3433 return oddNodes ? true : false;
3434}
3435
3436
3437uint32_t fm_consolidatePolygon(uint32_t pcount,const REAL *points,uint32_t pstride,REAL *_dest,REAL epsilon) // collapses co-linear edges.
3438{
3439 uint32_t ret = 0;
3440
3441
3442 if ( pcount >= 3 )
3443 {
3444 const REAL *prev = fm_getPoint(points,pstride,pcount-1);
3445 const REAL *current = points;
3446 const REAL *next = fm_getPoint(points,pstride,1);
3447 REAL *dest = _dest;
3448
3449 for (uint32_t i=0; i<pcount; i++)
3450 {
3451
3452 next = (i+1)==pcount ? points : next;
3453
3454 if ( !fm_colinear(prev,current,next,epsilon) )
3455 {
3456 dest[0] = current[0];
3457 dest[1] = current[1];
3458 dest[2] = current[2];
3459
3460 dest+=3;
3461 ret++;
3462 }
3463
3464 prev = current;
3465 current+=3;
3466 next+=3;
3467
3468 }
3469 }
3470
3471 return ret;
3472}
3473
3474
3475#ifndef RECT3D_TEMPLATE
3476
3477#define RECT3D_TEMPLATE
3478
3479template <class T> class Rect3d
3480{
3481public:
3482 Rect3d(void) { };
3483
3484 Rect3d(const T *bmin,const T *bmax)
3485 {
3486
3487 mMin[0] = bmin[0];
3488 mMin[1] = bmin[1];
3489 mMin[2] = bmin[2];
3490
3491 mMax[0] = bmax[0];
3492 mMax[1] = bmax[1];
3493 mMax[2] = bmax[2];
3494
3495 }
3496
3497 void SetMin(const T *bmin)
3498 {
3499 mMin[0] = bmin[0];
3500 mMin[1] = bmin[1];
3501 mMin[2] = bmin[2];
3502 }
3503
3504 void SetMax(const T *bmax)
3505 {
3506 mMax[0] = bmax[0];
3507 mMax[1] = bmax[1];
3508 mMax[2] = bmax[2];
3509 }
3510
3511 void SetMin(T x,T y,T z)
3512 {
3513 mMin[0] = x;
3514 mMin[1] = y;
3515 mMin[2] = z;
3516 }
3517
3518 void SetMax(T x,T y,T z)
3519 {
3520 mMax[0] = x;
3521 mMax[1] = y;
3522 mMax[2] = z;
3523 }
3524
3525 T mMin[3];
3526 T mMax[3];
3527};
3528
3529#endif
3530
3531void splitRect(uint32_t axis,
3532 const Rect3d<REAL> &source,
3533 Rect3d<REAL> &b1,
3534 Rect3d<REAL> &b2,
3535 const REAL *midpoint)
3536{
3537 switch ( axis )
3538 {
3539 case 0:
3540 b1.SetMin(source.mMin);
3541 b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
3542
3543 b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
3544 b2.SetMax(source.mMax);
3545
3546 break;
3547 case 1:
3548 b1.SetMin(source.mMin);
3549 b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
3550
3551 b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
3552 b2.SetMax(source.mMax);
3553
3554 break;
3555 case 2:
3556 b1.SetMin(source.mMin);
3557 b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
3558
3559 b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
3560 b2.SetMax(source.mMax);
3561
3562 break;
3563 }
3564}
3565
3566bool fm_computeSplitPlane(uint32_t vcount,
3567 const REAL *vertices,
3568 uint32_t /* tcount */,
3569 const uint32_t * /* indices */,
3570 REAL *plane)
3571{
3572
3573 REAL sides[3];
3574 REAL matrix[16];
3575
3576 fm_computeBestFitOBB( vcount, vertices, sizeof(REAL)*3, sides, matrix );
3577
3578 REAL bmax[3];
3579 REAL bmin[3];
3580
3581 bmax[0] = sides[0]*0.5f;
3582 bmax[1] = sides[1]*0.5f;
3583 bmax[2] = sides[2]*0.5f;
3584
3585 bmin[0] = -bmax[0];
3586 bmin[1] = -bmax[1];
3587 bmin[2] = -bmax[2];
3588
3589
3590 REAL dx = sides[0];
3591 REAL dy = sides[1];
3592 REAL dz = sides[2];
3593
3594
3595 uint32_t axis = 0;
3596
3597 if ( dy > dx )
3598 {
3599 axis = 1;
3600 }
3601
3602 if ( dz > dx && dz > dy )
3603 {
3604 axis = 2;
3605 }
3606
3607 REAL p1[3];
3608 REAL p2[3];
3609 REAL p3[3];
3610
3611 p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
3612 p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
3613 p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
3614
3615 Rect3d<REAL> b(bmin,bmax);
3616
3617 Rect3d<REAL> b1,b2;
3618
3619 splitRect(axis,b,b1,b2,p1);
3620
3621
3622 switch ( axis )
3623 {
3624 case 0:
3625 p2[1] = bmin[1];
3626 p2[2] = bmin[2];
3627
3628 if ( dz > dy )
3629 {
3630 p3[1] = bmax[1];
3631 p3[2] = bmin[2];
3632 }
3633 else
3634 {
3635 p3[1] = bmin[1];
3636 p3[2] = bmax[2];
3637 }
3638
3639 break;
3640 case 1:
3641 p2[0] = bmin[0];
3642 p2[2] = bmin[2];
3643
3644 if ( dx > dz )
3645 {
3646 p3[0] = bmax[0];
3647 p3[2] = bmin[2];
3648 }
3649 else
3650 {
3651 p3[0] = bmin[0];
3652 p3[2] = bmax[2];
3653 }
3654
3655 break;
3656 case 2:
3657 p2[0] = bmin[0];
3658 p2[1] = bmin[1];
3659
3660 if ( dx > dy )
3661 {
3662 p3[0] = bmax[0];
3663 p3[1] = bmin[1];
3664 }
3665 else
3666 {
3667 p3[0] = bmin[0];
3668 p3[1] = bmax[1];
3669 }
3670
3671 break;
3672 }
3673
3674 REAL tp1[3];
3675 REAL tp2[3];
3676 REAL tp3[3];
3677
3678 fm_transform(matrix,p1,tp1);
3679 fm_transform(matrix,p2,tp2);
3680 fm_transform(matrix,p3,tp3);
3681
3682 plane[3] = fm_computePlane(tp1,tp2,tp3,plane);
3683
3684 return true;
3685
3686}
3687
3688#pragma warning(disable:4100)
3689
3690void fm_nearestPointInTriangle(const REAL * /*nearestPoint*/,const REAL * /*p1*/,const REAL * /*p2*/,const REAL * /*p3*/,REAL * /*nearest*/)
3691{
3692
3693}
3694
3695static REAL Partial(const REAL *a,const REAL *p)
3696{
3697 return (a[0]*p[1]) - (p[0]*a[1]);
3698}
3699
3700REAL fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2)
3701{
3702 REAL A = Partial(p0,p1);
3703 A+= Partial(p1,p2);
3704 A+= Partial(p2,p0);
3705 return A*0.5f;
3706}
3707
3708void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store the result in 'diff'
3709{
3710 diff[0] = A[0]-B[0];
3711 diff[1] = A[1]-B[1];
3712 diff[2] = A[2]-B[2];
3713}
3714
3715
3716void fm_multiplyTransform(const REAL *pA,const REAL *pB,REAL *pM)
3717{
3718
3719 REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] + pA[0*4+3] * pB[3*4+0];
3720 REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] + pA[0*4+3] * pB[3*4+1];
3721 REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] + pA[0*4+3] * pB[3*4+2];
3722 REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] + pA[0*4+3] * pB[3*4+3];
3723
3724 REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] + pA[1*4+3] * pB[3*4+0];
3725 REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] + pA[1*4+3] * pB[3*4+1];
3726 REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] + pA[1*4+3] * pB[3*4+2];
3727 REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] + pA[1*4+3] * pB[3*4+3];
3728
3729 REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] + pA[2*4+3] * pB[3*4+0];
3730 REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] + pA[2*4+3] * pB[3*4+1];
3731 REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] + pA[2*4+3] * pB[3*4+2];
3732 REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] + pA[2*4+3] * pB[3*4+3];
3733
3734 REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] + pA[3*4+3] * pB[3*4+0];
3735 REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] + pA[3*4+3] * pB[3*4+1];
3736 REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] + pA[3*4+3] * pB[3*4+2];
3737 REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] + pA[3*4+3] * pB[3*4+3];
3738
3739 pM[0] = a; pM[1] = b; pM[2] = c; pM[3] = d;
3740
3741 pM[4] = e; pM[5] = f; pM[6] = g; pM[7] = h;
3742
3743 pM[8] = i; pM[9] = j; pM[10] = k; pM[11] = l;
3744
3745 pM[12] = m; pM[13] = n; pM[14] = o; pM[15] = p;
3746}
3747
3748void fm_multiply(REAL *A,REAL scaler)
3749{
3750 A[0]*=scaler;
3751 A[1]*=scaler;
3752 A[2]*=scaler;
3753}
3754
3755void fm_add(const REAL *A,const REAL *B,REAL *sum)
3756{
3757 sum[0] = A[0]+B[0];
3758 sum[1] = A[1]+B[1];
3759 sum[2] = A[2]+B[2];
3760}
3761
3762void fm_copy3(const REAL *source,REAL *dest)
3763{
3764 dest[0] = source[0];
3765 dest[1] = source[1];
3766 dest[2] = source[2];
3767}
3768
3769
3770uint32_t fm_copyUniqueVertices(uint32_t vcount,const REAL *input_vertices,REAL *output_vertices,uint32_t tcount,const uint32_t *input_indices,uint32_t *output_indices)
3771{
3772 uint32_t ret = 0;
3773
3774 REAL *vertices = (REAL *)malloc(sizeof(REAL)*vcount*3);
3775 memcpy(vertices,input_vertices,sizeof(REAL)*vcount*3);
3776 REAL *dest = output_vertices;
3777
3778 uint32_t *reindex = (uint32_t *)malloc(sizeof(uint32_t)*vcount);
3779 memset(reindex,0xFF,sizeof(uint32_t)*vcount);
3780
3781 uint32_t icount = tcount*3;
3782
3783 for (uint32_t i=0; i<icount; i++)
3784 {
3785 uint32_t index = *input_indices++;
3786
3787 assert( index < vcount );
3788
3789 if ( reindex[index] == 0xFFFFFFFF )
3790 {
3791 *output_indices++ = ret;
3792 reindex[index] = ret;
3793 const REAL *pos = &vertices[index*3];
3794 dest[0] = pos[0];
3795 dest[1] = pos[1];
3796 dest[2] = pos[2];
3797 dest+=3;
3798 ret++;
3799 }
3800 else
3801 {
3802 *output_indices++ = reindex[index];
3803 }
3804 }
3805 free(vertices);
3806 free(reindex);
3807 return ret;
3808}
3809
3810bool fm_isMeshCoplanar(uint32_t tcount,const uint32_t *indices,const REAL *vertices,bool doubleSided) // returns true if this collection of indexed triangles are co-planar!
3811{
3812 bool ret = true;
3813
3814 if ( tcount > 0 )
3815 {
3816 uint32_t i1 = indices[0];
3817 uint32_t i2 = indices[1];
3818 uint32_t i3 = indices[2];
3819 const REAL *p1 = &vertices[i1*3];
3820 const REAL *p2 = &vertices[i2*3];
3821 const REAL *p3 = &vertices[i3*3];
3822 REAL plane[4];
3823 plane[3] = fm_computePlane(p1,p2,p3,plane);
3824 const uint32_t *scan = &indices[3];
3825 for (uint32_t i=1; i<tcount; i++)
3826 {
3827 i1 = *scan++;
3828 i2 = *scan++;
3829 i3 = *scan++;
3830 p1 = &vertices[i1*3];
3831 p2 = &vertices[i2*3];
3832 p3 = &vertices[i3*3];
3833 REAL _plane[4];
3834 _plane[3] = fm_computePlane(p1,p2,p3,_plane);
3835 if ( !fm_samePlane(plane,_plane,0.01f,0.001f,doubleSided) )
3836 {
3837 ret = false;
3838 break;
3839 }
3840 }
3841 }
3842 return ret;
3843}
3844
3845
3846bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL dEpsilon,bool doubleSided)
3847{
3848 bool ret = false;
3849
3850#if 0
3851 if (p1[0] == p2[0] &&
3852 p1[1] == p2[1] &&
3853 p1[2] == p2[2] &&
3854 p1[3] == p2[3])
3855 {
3856 ret = true;
3857 }
3858#else
3859 REAL diff = (REAL) fabs(p1[3]-p2[3]);
3860 if ( diff < dEpsilon ) // if the plane -d co-efficient is within our epsilon
3861 {
3862 REAL dot = fm_dot(p1,p2); // compute the dot-product of the vector normals.
3863 if ( doubleSided ) dot = (REAL)fabs(dot);
3864 REAL dmin = 1 - normalEpsilon;
3865 REAL dmax = 1 + normalEpsilon;
3866 if ( dot >= dmin && dot <= dmax )
3867 {
3868 ret = true; // then the plane equation is for practical purposes identical.
3869 }
3870 }
3871#endif
3872 return ret;
3873}
3874
3875
3876void fm_initMinMax(REAL bmin[3],REAL bmax[3])
3877{
3878 bmin[0] = FLT_MAX;
3879 bmin[1] = FLT_MAX;
3880 bmin[2] = FLT_MAX;
3881
3882 bmax[0] = -FLT_MAX;
3883 bmax[1] = -FLT_MAX;
3884 bmax[2] = -FLT_MAX;
3885}
3886
3887void fm_inflateMinMax(REAL bmin[3], REAL bmax[3], REAL ratio)
3888{
3889 REAL inflate = fm_distance(bmin, bmax)*0.5f*ratio;
3890
3891 bmin[0] -= inflate;
3892 bmin[1] -= inflate;
3893 bmin[2] -= inflate;
3894
3895 bmax[0] += inflate;
3896 bmax[1] += inflate;
3897 bmax[2] += inflate;
3898}
3899
3900#ifndef TESSELATE_H
3901
3902#define TESSELATE_H
3903
3904typedef std::vector< uint32_t > UintVector;
3905
3906class Myfm_Tesselate : public fm_Tesselate
3907{
3908public:
3909 virtual ~Myfm_Tesselate(void)
3910 {
3911
3912 }
3913
3914 const uint32_t * tesselate(fm_VertexIndex *vindex,uint32_t tcount,const uint32_t *indices,float longEdge,uint32_t maxDepth,uint32_t &outcount)
3915 {
3916 const uint32_t *ret = 0;
3917
3918 mMaxDepth = maxDepth;
3919 mLongEdge = longEdge*longEdge;
3920 mLongEdgeD = mLongEdge;
3921 mVertices = vindex;
3922
3923 if ( mVertices->isDouble() )
3924 {
3925 uint32_t vcount = mVertices->getVcount();
3926 double *vertices = (double *)malloc(sizeof(double)*vcount*3);
3927 memcpy(vertices,mVertices->getVerticesDouble(),sizeof(double)*vcount*3);
3928
3929 for (uint32_t i=0; i<tcount; i++)
3930 {
3931 uint32_t i1 = *indices++;
3932 uint32_t i2 = *indices++;
3933 uint32_t i3 = *indices++;
3934
3935 const double *p1 = &vertices[i1*3];
3936 const double *p2 = &vertices[i2*3];
3937 const double *p3 = &vertices[i3*3];
3938
3939 tesselate(p1,p2,p3,0);
3940
3941 }
3942 free(vertices);
3943 }
3944 else
3945 {
3946 uint32_t vcount = mVertices->getVcount();
3947 float *vertices = (float *)malloc(sizeof(float)*vcount*3);
3948 memcpy(vertices,mVertices->getVerticesFloat(),sizeof(float)*vcount*3);
3949
3950
3951 for (uint32_t i=0; i<tcount; i++)
3952 {
3953 uint32_t i1 = *indices++;
3954 uint32_t i2 = *indices++;
3955 uint32_t i3 = *indices++;
3956
3957 const float *p1 = &vertices[i1*3];
3958 const float *p2 = &vertices[i2*3];
3959 const float *p3 = &vertices[i3*3];
3960
3961 tesselate(p1,p2,p3,0);
3962
3963 }
3964 free(vertices);
3965 }
3966
3967 outcount = (uint32_t)(mIndices.size()/3);
3968 ret = &mIndices[0];
3969
3970
3971 return ret;
3972 }
3973
3974 void tesselate(const float *p1,const float *p2,const float *p3,uint32_t recurse)
3975 {
3976 bool split = false;
3977 float l1,l2,l3;
3978
3979 l1 = l2 = l3 = 0;
3980
3981 if ( recurse < mMaxDepth )
3982 {
3983 l1 = fm_distanceSquared(p1,p2);
3984 l2 = fm_distanceSquared(p2,p3);
3985 l3 = fm_distanceSquared(p3,p1);
3986
3987 if ( l1 > mLongEdge || l2 > mLongEdge || l3 > mLongEdge )
3988 split = true;
3989
3990 }
3991
3992 if ( split )
3993 {
3994 uint32_t edge;
3995
3996 if ( l1 >= l2 && l1 >= l3 )
3997 edge = 0;
3998 else if ( l2 >= l1 && l2 >= l3 )
3999 edge = 1;
4000 else
4001 edge = 2;
4002
4003 float splits[3];
4004
4005 switch ( edge )
4006 {
4007 case 0:
4008 {
4009 fm_lerp(p1,p2,splits,0.5f);
4010 tesselate(p1,splits,p3, recurse+1 );
4011 tesselate(splits,p2,p3, recurse+1 );
4012 }
4013 break;
4014 case 1:
4015 {
4016 fm_lerp(p2,p3,splits,0.5f);
4017 tesselate(p1,p2,splits, recurse+1 );
4018 tesselate(p1,splits,p3, recurse+1 );
4019 }
4020 break;
4021 case 2:
4022 {
4023 fm_lerp(p3,p1,splits,0.5f);
4024 tesselate(p1,p2,splits, recurse+1 );
4025 tesselate(splits,p2,p3, recurse+1 );
4026 }
4027 break;
4028 }
4029 }
4030 else
4031 {
4032 bool newp;
4033
4034 uint32_t i1 = mVertices->getIndex(p1,newp);
4035 uint32_t i2 = mVertices->getIndex(p2,newp);
4036 uint32_t i3 = mVertices->getIndex(p3,newp);
4037
4038 mIndices.push_back(i1);
4039 mIndices.push_back(i2);
4040 mIndices.push_back(i3);
4041 }
4042
4043 }
4044
4045 void tesselate(const double *p1,const double *p2,const double *p3,uint32_t recurse)
4046 {
4047 bool split = false;
4048 double l1,l2,l3;
4049
4050 l1 = l2 = l3 = 0;
4051
4052 if ( recurse < mMaxDepth )
4053 {
4054 l1 = fm_distanceSquared(p1,p2);
4055 l2 = fm_distanceSquared(p2,p3);
4056 l3 = fm_distanceSquared(p3,p1);
4057
4058 if ( l1 > mLongEdgeD || l2 > mLongEdgeD || l3 > mLongEdgeD )
4059 split = true;
4060
4061 }
4062
4063 if ( split )
4064 {
4065 uint32_t edge;
4066
4067 if ( l1 >= l2 && l1 >= l3 )
4068 edge = 0;
4069 else if ( l2 >= l1 && l2 >= l3 )
4070 edge = 1;
4071 else
4072 edge = 2;
4073
4074 double splits[3];
4075
4076 switch ( edge )
4077 {
4078 case 0:
4079 {
4080 fm_lerp(p1,p2,splits,0.5);
4081 tesselate(p1,splits,p3, recurse+1 );
4082 tesselate(splits,p2,p3, recurse+1 );
4083 }
4084 break;
4085 case 1:
4086 {
4087 fm_lerp(p2,p3,splits,0.5);
4088 tesselate(p1,p2,splits, recurse+1 );
4089 tesselate(p1,splits,p3, recurse+1 );
4090 }
4091 break;
4092 case 2:
4093 {
4094 fm_lerp(p3,p1,splits,0.5);
4095 tesselate(p1,p2,splits, recurse+1 );
4096 tesselate(splits,p2,p3, recurse+1 );
4097 }
4098 break;
4099 }
4100 }
4101 else
4102 {
4103 bool newp;
4104
4105 uint32_t i1 = mVertices->getIndex(p1,newp);
4106 uint32_t i2 = mVertices->getIndex(p2,newp);
4107 uint32_t i3 = mVertices->getIndex(p3,newp);
4108
4109 mIndices.push_back(i1);
4110 mIndices.push_back(i2);
4111 mIndices.push_back(i3);
4112 }
4113
4114 }
4115
4116private:
4117 float mLongEdge;
4118 double mLongEdgeD;
4119 fm_VertexIndex *mVertices;
4120 UintVector mIndices;
4121 uint32_t mMaxDepth;
4122};
4123
4124fm_Tesselate * fm_createTesselate(void)
4125{
4126 Myfm_Tesselate *m = new Myfm_Tesselate;
4127 return static_cast< fm_Tesselate * >(m);
4128}
4129
4130void fm_releaseTesselate(fm_Tesselate *t)
4131{
4132 Myfm_Tesselate *m = static_cast< Myfm_Tesselate *>(t);
4133 delete m;
4134}
4135
4136#endif
4137
4138
4139#ifndef RAY_ABB_INTERSECT
4140
4141#define RAY_ABB_INTERSECT
4142
4143//! Integer representation of a floating-point value.
4144#define IR(x) ((uint32_t&)x)
4145
4146///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
4147/**
4148* A method to compute a ray-AABB intersection.
4149* Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
4150* Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
4151* Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
4152*
4153* Hence this version is faster as well as more robust than the original one.
4154*
4155* Should work provided:
4156* 1) the integer representation of 0.0f is 0x00000000
4157* 2) the sign bit of the float is the most significant one
4158*
4159* Report bugs: p.terdiman@codercorner.com
4160*
4161* \param aabb [in] the axis-aligned bounding box
4162* \param origin [in] ray origin
4163* \param dir [in] ray direction
4164* \param coord [out] impact coordinates
4165* \return true if ray intersects AABB
4166*/
4167///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
4168#define RAYAABB_EPSILON 0.00001f
4169bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float origin[3],const float dir[3],float coord[3])
4170{
4171 bool Inside = true;
4172 float MaxT[3];
4173 MaxT[0]=MaxT[1]=MaxT[2]=-1.0f;
4174
4175 // Find candidate planes.
4176 for(uint32_t i=0;i<3;i++)
4177 {
4178 if(origin[i] < MinB[i])
4179 {
4180 coord[i] = MinB[i];
4181 Inside = false;
4182
4183 // Calculate T distances to candidate planes
4184 if(IR(dir[i])) MaxT[i] = (MinB[i] - origin[i]) / dir[i];
4185 }
4186 else if(origin[i] > MaxB[i])
4187 {
4188 coord[i] = MaxB[i];
4189 Inside = false;
4190
4191 // Calculate T distances to candidate planes
4192 if(IR(dir[i])) MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
4193 }
4194 }
4195
4196 // Ray origin inside bounding box
4197 if(Inside)
4198 {
4199 coord[0] = origin[0];
4200 coord[1] = origin[1];
4201 coord[2] = origin[2];
4202 return true;
4203 }
4204
4205 // Get largest of the maxT's for final choice of intersection
4206 uint32_t WhichPlane = 0;
4207 if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1;
4208 if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2;
4209
4210 // Check final candidate actually inside box
4211 if(IR(MaxT[WhichPlane])&0x80000000) return false;
4212
4213 for(uint32_t i=0;i<3;i++)
4214 {
4215 if(i!=WhichPlane)
4216 {
4217 coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
4218#ifdef RAYAABB_EPSILON
4219 if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON) return false;
4220#else
4221 if(coord[i] < MinB[i] || coord[i] > MaxB[i]) return false;
4222#endif
4223 }
4224 }
4225 return true; // ray hits box
4226}
4227
4228bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float p2[3],float intersect[3])
4229{
4230 bool ret = false;
4231
4232 float dir[3];
4233 dir[0] = p2[0] - p1[0];
4234 dir[1] = p2[1] - p1[1];
4235 dir[2] = p2[2] - p1[2];
4236 float dist = fm_normalize(dir);
4237 if ( dist > RAYAABB_EPSILON )
4238 {
4239 ret = fm_intersectRayAABB(bmin,bmax,p1,dir,intersect);
4240 if ( ret )
4241 {
4242 float d = fm_distanceSquared(p1,intersect);
4243 if ( d > (dist*dist) )
4244 {
4245 ret = false;
4246 }
4247 }
4248 }
4249 return ret;
4250}
4251
4252#endif
4253
4254#ifndef OBB_TO_AABB
4255
4256#define OBB_TO_AABB
4257
4258#pragma warning(disable:4100)
4259
4260void fm_OBBtoAABB(const float /*obmin*/[3],const float /*obmax*/[3],const float /*matrix*/[16],float /*abmin*/[3],float /*abmax*/[3])
4261{
4262 assert(0); // not yet implemented.
4263}
4264
4265
4266const REAL * computePos(uint32_t index,const REAL *vertices,uint32_t vstride)
4267{
4268 const char *tmp = (const char *)vertices;
4269 tmp+=(index*vstride);
4270 return (const REAL*)tmp;
4271}
4272
4273void computeNormal(uint32_t index,REAL *normals,uint32_t nstride,const REAL *normal)
4274{
4275 char *tmp = (char *)normals;
4276 tmp+=(index*nstride);
4277 REAL *dest = (REAL *)tmp;
4278 dest[0]+=normal[0];
4279 dest[1]+=normal[1];
4280 dest[2]+=normal[2];
4281}
4282
4283void fm_computeMeanNormals(uint32_t vcount, // the number of vertices
4284 const REAL *vertices, // the base address of the vertex position data.
4285 uint32_t vstride, // the stride between position data.
4286 REAL *normals, // the base address of the destination for mean vector normals
4287 uint32_t nstride, // the stride between normals
4288 uint32_t tcount, // the number of triangles
4289 const uint32_t *indices) // the triangle indices
4290{
4291
4292 // Step #1 : Zero out the vertex normals
4293 char *dest = (char *)normals;
4294 for (uint32_t i=0; i<vcount; i++)
4295 {
4296 REAL *n = (REAL *)dest;
4297 n[0] = 0;
4298 n[1] = 0;
4299 n[2] = 0;
4300 dest+=nstride;
4301 }
4302
4303 // Step #2 : Compute the face normals and accumulate them
4304 const uint32_t *scan = indices;
4305 for (uint32_t i=0; i<tcount; i++)
4306 {
4307
4308 uint32_t i1 = *scan++;
4309 uint32_t i2 = *scan++;
4310 uint32_t i3 = *scan++;
4311
4312 const REAL *p1 = computePos(i1,vertices,vstride);
4313 const REAL *p2 = computePos(i2,vertices,vstride);
4314 const REAL *p3 = computePos(i3,vertices,vstride);
4315
4316 REAL normal[3];
4317 fm_computePlane(p3,p2,p1,normal);
4318
4319 computeNormal(i1,normals,nstride,normal);
4320 computeNormal(i2,normals,nstride,normal);
4321 computeNormal(i3,normals,nstride,normal);
4322 }
4323
4324
4325 // Normalize the accumulated normals
4326 dest = (char *)normals;
4327 for (uint32_t i=0; i<vcount; i++)
4328 {
4329 REAL *n = (REAL *)dest;
4330 fm_normalize(n);
4331 dest+=nstride;
4332 }
4333
4334}
4335
4336#endif
4337
4338
4339#define BIGNUMBER 100000000.0 /* hundred million */
4340
4341static inline void Set(REAL *n,REAL x,REAL y,REAL z)
4342{
4343 n[0] = x;
4344 n[1] = y;
4345 n[2] = z;
4346};
4347
4348static inline void Copy(REAL *dest,const REAL *source)
4349{
4350 dest[0] = source[0];
4351 dest[1] = source[1];
4352 dest[2] = source[2];
4353}
4354
4355
4356REAL fm_computeBestFitSphere(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *center)
4357{
4358 REAL radius;
4359 REAL radius2;
4360
4361 REAL xmin[3];
4362 REAL xmax[3];
4363 REAL ymin[3];
4364 REAL ymax[3];
4365 REAL zmin[3];
4366 REAL zmax[3];
4367 REAL dia1[3];
4368 REAL dia2[3];
4369
4370 /* FIRST PASS: find 6 minima/maxima points */
4371 Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
4372 Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
4373 Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
4374 Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
4375 Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
4376 Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
4377
4378 {
4379 const char *scan = (const char *)points;
4380 for (uint32_t i=0; i<vcount; i++)
4381 {
4382 const REAL *caller_p = (const REAL *)scan;
4383 if (caller_p[0]<xmin[0])
4384 Copy(xmin,caller_p); /* New xminimum point */
4385 if (caller_p[0]>xmax[0])
4386 Copy(xmax,caller_p);
4387 if (caller_p[1]<ymin[1])
4388 Copy(ymin,caller_p);
4389 if (caller_p[1]>ymax[1])
4390 Copy(ymax,caller_p);
4391 if (caller_p[2]<zmin[2])
4392 Copy(zmin,caller_p);
4393 if (caller_p[2]>zmax[2])
4394 Copy(zmax,caller_p);
4395 scan+=pstride;
4396 }
4397 }
4398
4399 /* Set xspan = distance between the 2 points xmin & xmax (squared) */
4400 REAL dx = xmax[0] - xmin[0];
4401 REAL dy = xmax[1] - xmin[1];
4402 REAL dz = xmax[2] - xmin[2];
4403 REAL xspan = dx*dx + dy*dy + dz*dz;
4404
4405/* Same for y & z spans */
4406 dx = ymax[0] - ymin[0];
4407 dy = ymax[1] - ymin[1];
4408 dz = ymax[2] - ymin[2];
4409 REAL yspan = dx*dx + dy*dy + dz*dz;
4410
4411 dx = zmax[0] - zmin[0];
4412 dy = zmax[1] - zmin[1];
4413 dz = zmax[2] - zmin[2];
4414 REAL zspan = dx*dx + dy*dy + dz*dz;
4415
4416 /* Set points dia1 & dia2 to the maximally separated pair */
4417 Copy(dia1,xmin);
4418 Copy(dia2,xmax); /* assume xspan biggest */
4419 REAL maxspan = xspan;
4420
4421 if (yspan>maxspan)
4422 {
4423 maxspan = yspan;
4424 Copy(dia1,ymin);
4425 Copy(dia2,ymax);
4426 }
4427
4428 if (zspan>maxspan)
4429 {
4430 maxspan = zspan;
4431 Copy(dia1,zmin);
4432 Copy(dia2,zmax);
4433 }
4434
4435
4436 /* dia1,dia2 is a diameter of initial sphere */
4437 /* calc initial center */
4438 center[0] = (dia1[0]+dia2[0])*0.5f;
4439 center[1] = (dia1[1]+dia2[1])*0.5f;
4440 center[2] = (dia1[2]+dia2[2])*0.5f;
4441
4442 /* calculate initial radius**2 and radius */
4443
4444 dx = dia2[0]-center[0]; /* x component of radius vector */
4445 dy = dia2[1]-center[1]; /* y component of radius vector */
4446 dz = dia2[2]-center[2]; /* z component of radius vector */
4447
4448 radius2 = dx*dx + dy*dy + dz*dz;
4449 radius = REAL(sqrt(radius2));
4450
4451 /* SECOND PASS: increment current sphere */
4452 {
4453 const char *scan = (const char *)points;
4454 for (uint32_t i=0; i<vcount; i++)
4455 {
4456 const REAL *caller_p = (const REAL *)scan;
4457 dx = caller_p[0]-center[0];
4458 dy = caller_p[1]-center[1];
4459 dz = caller_p[2]-center[2];
4460 REAL old_to_p_sq = dx*dx + dy*dy + dz*dz;
4461 if (old_to_p_sq > radius2) /* do r**2 test first */
4462 { /* this point is outside of current sphere */
4463 REAL old_to_p = REAL(sqrt(old_to_p_sq));
4464 /* calc radius of new sphere */
4465 radius = (radius + old_to_p) * 0.5f;
4466 radius2 = radius*radius; /* for next r**2 compare */
4467 REAL old_to_new = old_to_p - radius;
4468 /* calc center of new sphere */
4469 REAL recip = 1.0f /old_to_p;
4470 REAL cx = (radius*center[0] + old_to_new*caller_p[0]) * recip;
4471 REAL cy = (radius*center[1] + old_to_new*caller_p[1]) * recip;
4472 REAL cz = (radius*center[2] + old_to_new*caller_p[2]) * recip;
4473 Set(center,cx,cy,cz);
4474 scan+=pstride;
4475 }
4476 }
4477 }
4478 return radius;
4479}
4480
4481
4482void fm_computeBestFitCapsule(uint32_t vcount,const REAL *points,uint32_t pstride,REAL &radius,REAL &height,REAL matrix[16],bool bruteForce)
4483{
4484 REAL sides[3];
4485 REAL omatrix[16];
4486 fm_computeBestFitOBB(vcount,points,pstride,sides,omatrix,bruteForce);
4487
4488 int32_t axis = 0;
4489 if ( sides[0] > sides[1] && sides[0] > sides[2] )
4490 axis = 0;
4491 else if ( sides[1] > sides[0] && sides[1] > sides[2] )
4492 axis = 1;
4493 else
4494 axis = 2;
4495
4496 REAL localTransform[16];
4497
4498 REAL maxDist = 0;
4499 REAL maxLen = 0;
4500
4501 switch ( axis )
4502 {
4503 case 0:
4504 {
4505 fm_eulerMatrix(0,0,FM_PI/2,localTransform);
4506 fm_matrixMultiply(localTransform,omatrix,matrix);
4507
4508 const uint8_t *scan = (const uint8_t *)points;
4509 for (uint32_t i=0; i<vcount; i++)
4510 {
4511 const REAL *p = (const REAL *)scan;
4512 REAL t[3];
4513 fm_inverseRT(omatrix,p,t);
4514 REAL dist = t[1]*t[1]+t[2]*t[2];
4515 if ( dist > maxDist )
4516 {
4517 maxDist = dist;
4518 }
4519 REAL l = (REAL) fabs(t[0]);
4520 if ( l > maxLen )
4521 {
4522 maxLen = l;
4523 }
4524 scan+=pstride;
4525 }
4526 }
4527 height = sides[0];
4528 break;
4529 case 1:
4530 {
4531 fm_eulerMatrix(0,FM_PI/2,0,localTransform);
4532 fm_matrixMultiply(localTransform,omatrix,matrix);
4533
4534 const uint8_t *scan = (const uint8_t *)points;
4535 for (uint32_t i=0; i<vcount; i++)
4536 {
4537 const REAL *p = (const REAL *)scan;
4538 REAL t[3];
4539 fm_inverseRT(omatrix,p,t);
4540 REAL dist = t[0]*t[0]+t[2]*t[2];
4541 if ( dist > maxDist )
4542 {
4543 maxDist = dist;
4544 }
4545 REAL l = (REAL) fabs(t[1]);
4546 if ( l > maxLen )
4547 {
4548 maxLen = l;
4549 }
4550 scan+=pstride;
4551 }
4552 }
4553 height = sides[1];
4554 break;
4555 case 2:
4556 {
4557 fm_eulerMatrix(FM_PI/2,0,0,localTransform);
4558 fm_matrixMultiply(localTransform,omatrix,matrix);
4559
4560 const uint8_t *scan = (const uint8_t *)points;
4561 for (uint32_t i=0; i<vcount; i++)
4562 {
4563 const REAL *p = (const REAL *)scan;
4564 REAL t[3];
4565 fm_inverseRT(omatrix,p,t);
4566 REAL dist = t[0]*t[0]+t[1]*t[1];
4567 if ( dist > maxDist )
4568 {
4569 maxDist = dist;
4570 }
4571 REAL l = (REAL) fabs(t[2]);
4572 if ( l > maxLen )
4573 {
4574 maxLen = l;
4575 }
4576 scan+=pstride;
4577 }
4578 }
4579 height = sides[2];
4580 break;
4581 }
4582 radius = (REAL)sqrt(maxDist);
4583 height = (maxLen*2)-(radius*2);
4584}
4585
4586
4587//************* Triangulation
4588
4589#ifndef TRIANGULATE_H
4590
4591#define TRIANGULATE_H
4592
4593typedef uint32_t TU32;
4594
4595class TVec
4596{
4597public:
4598 TVec(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
4599 TVec(void) { };
4600
4601 double x;
4602 double y;
4603 double z;
4604};
4605
4606typedef std::vector< TVec > TVecVector;
4607typedef std::vector< TU32 > TU32Vector;
4608
4609class CTriangulator
4610{
4611public:
4612 /// Default constructor
4613 CTriangulator();
4614
4615 /// Default destructor
4616 virtual ~CTriangulator();
4617
4618 /// Triangulates the contour
4619 void triangulate(TU32Vector &indices);
4620
4621 /// Returns the given point in the triangulator array
4622 inline TVec get(const TU32 id) { return mPoints[id]; }
4623
4624 virtual void reset(void)
4625 {
4626 mInputPoints.clear();
4627 mPoints.clear();
4628 mIndices.clear();
4629 }
4630
4631 virtual void addPoint(double x,double y,double z)
4632 {
4633 TVec v(x,y,z);
4634 // update bounding box...
4635 if ( mInputPoints.empty() )
4636 {
4637 mMin = v;
4638 mMax = v;
4639 }
4640 else
4641 {
4642 if ( x < mMin.x ) mMin.x = x;
4643 if ( y < mMin.y ) mMin.y = y;
4644 if ( z < mMin.z ) mMin.z = z;
4645
4646 if ( x > mMax.x ) mMax.x = x;
4647 if ( y > mMax.y ) mMax.y = y;
4648 if ( z > mMax.z ) mMax.z = z;
4649 }
4650 mInputPoints.push_back(v);
4651 }
4652
4653 // Triangulation happens in 2d. We could inverse transform the polygon around the normal direction, or we just use the two most signficant axes
4654 // Here we find the two longest axes and use them to triangulate. Inverse transforming them would introduce more doubleing point error and isn't worth it.
4655 virtual uint32_t * triangulate(uint32_t &tcount,double epsilon)
4656 {
4657 uint32_t *ret = 0;
4658 tcount = 0;
4659 mEpsilon = epsilon;
4660
4661 if ( !mInputPoints.empty() )
4662 {
4663 mPoints.clear();
4664
4665 double dx = mMax.x - mMin.x; // locate the first, second and third longest edges and store them in i1, i2, i3
4666 double dy = mMax.y - mMin.y;
4667 double dz = mMax.z - mMin.z;
4668
4669 uint32_t i1,i2,i3;
4670
4671 if ( dx > dy && dx > dz )
4672 {
4673 i1 = 0;
4674 if ( dy > dz )
4675 {
4676 i2 = 1;
4677 i3 = 2;
4678 }
4679 else
4680 {
4681 i2 = 2;
4682 i3 = 1;
4683 }
4684 }
4685 else if ( dy > dx && dy > dz )
4686 {
4687 i1 = 1;
4688 if ( dx > dz )
4689 {
4690 i2 = 0;
4691 i3 = 2;
4692 }
4693 else
4694 {
4695 i2 = 2;
4696 i3 = 0;
4697 }
4698 }
4699 else
4700 {
4701 i1 = 2;
4702 if ( dx > dy )
4703 {
4704 i2 = 0;
4705 i3 = 1;
4706 }
4707 else
4708 {
4709 i2 = 1;
4710 i3 = 0;
4711 }
4712 }
4713
4714 uint32_t pcount = (uint32_t)mInputPoints.size();
4715 const double *points = &mInputPoints[0].x;
4716 for (uint32_t i=0; i<pcount; i++)
4717 {
4718 TVec v( points[i1], points[i2], points[i3] );
4719 mPoints.push_back(v);
4720 points+=3;
4721 }
4722
4723 mIndices.clear();
4724 triangulate(mIndices);
4725 tcount = (uint32_t)mIndices.size()/3;
4726 if ( tcount )
4727 {
4728 ret = &mIndices[0];
4729 }
4730 }
4731 return ret;
4732 }
4733
4734 virtual const double * getPoint(uint32_t index)
4735 {
4736 return &mInputPoints[index].x;
4737 }
4738
4739
4740private:
4741 double mEpsilon;
4742 TVec mMin;
4743 TVec mMax;
4744 TVecVector mInputPoints;
4745 TVecVector mPoints;
4746 TU32Vector mIndices;
4747
4748 /// Tests if a point is inside the given triangle
4749 bool _insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P);
4750
4751 /// Returns the area of the contour
4752 double _area();
4753
4754 bool _snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V);
4755
4756 /// Processes the triangulation
4757 void _process(TU32Vector &indices);
4758
4759};
4760
4761/// Default constructor
4762CTriangulator::CTriangulator(void)
4763{
4764}
4765
4766/// Default destructor
4767CTriangulator::~CTriangulator()
4768{
4769}
4770
4771/// Triangulates the contour
4772void CTriangulator::triangulate(TU32Vector &indices)
4773{
4774 _process(indices);
4775}
4776
4777/// Processes the triangulation
4778void CTriangulator::_process(TU32Vector &indices)
4779{
4780 const int32_t n = (const int32_t)mPoints.size();
4781 if (n < 3)
4782 return;
4783 int32_t *V = (int32_t *)malloc(sizeof(int32_t)*n);
4784
4785 bool flipped = false;
4786
4787 if (0.0f < _area())
4788 {
4789 for (int32_t v = 0; v < n; v++)
4790 V[v] = v;
4791 }
4792 else
4793 {
4794 flipped = true;
4795 for (int32_t v = 0; v < n; v++)
4796 V[v] = (n - 1) - v;
4797 }
4798
4799 int32_t nv = n;
4800 int32_t count = 2 * nv;
4801 for (int32_t m = 0, v = nv - 1; nv > 2;)
4802 {
4803 if (0 >= (count--))
4804 return;
4805
4806 int32_t u = v;
4807 if (nv <= u)
4808 u = 0;
4809 v = u + 1;
4810 if (nv <= v)
4811 v = 0;
4812 int32_t w = v + 1;
4813 if (nv <= w)
4814 w = 0;
4815
4816 if (_snip(u, v, w, nv, V))
4817 {
4818 int32_t a, b, c, s, t;
4819 a = V[u];
4820 b = V[v];
4821 c = V[w];
4822 if ( flipped )
4823 {
4824 indices.push_back(a);
4825 indices.push_back(b);
4826 indices.push_back(c);
4827 }
4828 else
4829 {
4830 indices.push_back(c);
4831 indices.push_back(b);
4832 indices.push_back(a);
4833 }
4834 m++;
4835 for (s = v, t = v + 1; t < nv; s++, t++)
4836 V[s] = V[t];
4837 nv--;
4838 count = 2 * nv;
4839 }
4840 }
4841
4842 free(V);
4843}
4844
4845/// Returns the area of the contour
4846double CTriangulator::_area()
4847{
4848 int32_t n = (uint32_t)mPoints.size();
4849 double A = 0.0f;
4850 for (int32_t p = n - 1, q = 0; q < n; p = q++)
4851 {
4852 const TVec &pval = mPoints[p];
4853 const TVec &qval = mPoints[q];
4854 A += pval.x * qval.y - qval.x * pval.y;
4855 }
4856 A*=0.5f;
4857 return A;
4858}
4859
4860bool CTriangulator::_snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V)
4861{
4862 int32_t p;
4863
4864 const TVec &A = mPoints[ V[u] ];
4865 const TVec &B = mPoints[ V[v] ];
4866 const TVec &C = mPoints[ V[w] ];
4867
4868 if (mEpsilon > (((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))) )
4869 return false;
4870
4871 for (p = 0; p < n; p++)
4872 {
4873 if ((p == u) || (p == v) || (p == w))
4874 continue;
4875 const TVec &P = mPoints[ V[p] ];
4876 if (_insideTriangle(A, B, C, P))
4877 return false;
4878 }
4879 return true;
4880}
4881
4882/// Tests if a point is inside the given triangle
4883bool CTriangulator::_insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P)
4884{
4885 double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
4886 double cCROSSap, bCROSScp, aCROSSbp;
4887
4888 ax = C.x - B.x; ay = C.y - B.y;
4889 bx = A.x - C.x; by = A.y - C.y;
4890 cx = B.x - A.x; cy = B.y - A.y;
4891 apx = P.x - A.x; apy = P.y - A.y;
4892 bpx = P.x - B.x; bpy = P.y - B.y;
4893 cpx = P.x - C.x; cpy = P.y - C.y;
4894
4895 aCROSSbp = ax * bpy - ay * bpx;
4896 cCROSSap = cx * apy - cy * apx;
4897 bCROSScp = bx * cpy - by * cpx;
4898
4899 return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
4900}
4901
4902class Triangulate : public fm_Triangulate
4903{
4904public:
4905 Triangulate(void)
4906 {
4907 mPointsFloat = 0;
4908 mPointsDouble = 0;
4909 }
4910
4911 virtual ~Triangulate(void)
4912 {
4913 reset();
4914 }
4915 void reset(void)
4916 {
4917 free(mPointsFloat);
4918 free(mPointsDouble);
4919 mPointsFloat = 0;
4920 mPointsDouble = 0;
4921 }
4922
4923 virtual const double * triangulate3d(uint32_t pcount,
4924 const double *_points,
4925 uint32_t vstride,
4926 uint32_t &tcount,
4927 bool consolidate,
4928 double epsilon)
4929 {
4930 reset();
4931
4932 double *points = (double *)malloc(sizeof(double)*pcount*3);
4933 if ( consolidate )
4934 {
4935 pcount = fm_consolidatePolygon(pcount,_points,vstride,points,1-epsilon);
4936 }
4937 else
4938 {
4939 double *dest = points;
4940 for (uint32_t i=0; i<pcount; i++)
4941 {
4942 const double *src = fm_getPoint(_points,vstride,i);
4943 dest[0] = src[0];
4944 dest[1] = src[1];
4945 dest[2] = src[2];
4946 dest+=3;
4947 }
4948 vstride = sizeof(double)*3;
4949 }
4950
4951 if ( pcount >= 3 )
4952 {
4953 CTriangulator ct;
4954 for (uint32_t i=0; i<pcount; i++)
4955 {
4956 const double *src = fm_getPoint(points,vstride,i);
4957 ct.addPoint( src[0], src[1], src[2] );
4958 }
4959 uint32_t _tcount;
4960 uint32_t *indices = ct.triangulate(_tcount,epsilon);
4961 if ( indices )
4962 {
4963 tcount = _tcount;
4964 mPointsDouble = (double *)malloc(sizeof(double)*tcount*3*3);
4965 double *dest = mPointsDouble;
4966 for (uint32_t i=0; i<tcount; i++)
4967 {
4968 uint32_t i1 = indices[i*3+0];
4969 uint32_t i2 = indices[i*3+1];
4970 uint32_t i3 = indices[i*3+2];
4971 const double *p1 = ct.getPoint(i1);
4972 const double *p2 = ct.getPoint(i2);
4973 const double *p3 = ct.getPoint(i3);
4974
4975 dest[0] = p1[0];
4976 dest[1] = p1[1];
4977 dest[2] = p1[2];
4978
4979 dest[3] = p2[0];
4980 dest[4] = p2[1];
4981 dest[5] = p2[2];
4982
4983 dest[6] = p3[0];
4984 dest[7] = p3[1];
4985 dest[8] = p3[2];
4986 dest+=9;
4987 }
4988 }
4989 }
4990 free(points);
4991
4992 return mPointsDouble;
4993 }
4994
4995 virtual const float * triangulate3d(uint32_t pcount,
4996 const float *points,
4997 uint32_t vstride,
4998 uint32_t &tcount,
4999 bool consolidate,
5000 float epsilon)
5001 {
5002 reset();
5003
5004 double *temp = (double *)malloc(sizeof(double)*pcount*3);
5005 double *dest = temp;
5006 for (uint32_t i=0; i<pcount; i++)
5007 {
5008 const float *p = fm_getPoint(points,vstride,i);
5009 dest[0] = p[0];
5010 dest[1] = p[1];
5011 dest[2] = p[2];
5012 dest+=3;
5013 }
5014 const double *results = triangulate3d(pcount,temp,sizeof(double)*3,tcount,consolidate,epsilon);
5015 if ( results )
5016 {
5017 uint32_t fcount = tcount*3*3;
5018 mPointsFloat = (float *)malloc(sizeof(float)*tcount*3*3);
5019 for (uint32_t i=0; i<fcount; i++)
5020 {
5021 mPointsFloat[i] = (float) results[i];
5022 }
5023 free(mPointsDouble);
5024 mPointsDouble = 0;
5025 }
5026 free(temp);
5027
5028 return mPointsFloat;
5029 }
5030
5031private:
5032 float *mPointsFloat;
5033 double *mPointsDouble;
5034};
5035
5036fm_Triangulate * fm_createTriangulate(void)
5037{
5038 Triangulate *t = new Triangulate;
5039 return static_cast< fm_Triangulate *>(t);
5040}
5041
5042void fm_releaseTriangulate(fm_Triangulate *t)
5043{
5044 Triangulate *tt = static_cast< Triangulate *>(t);
5045 delete tt;
5046}
5047
5048#endif
5049
5050bool validDistance(const REAL *p1,const REAL *p2,REAL epsilon)
5051{
5052 bool ret = true;
5053
5054 REAL dx = p1[0] - p2[0];
5055 REAL dy = p1[1] - p2[1];
5056 REAL dz = p1[2] - p2[2];
5057 REAL dist = dx*dx+dy*dy+dz*dz;
5058 if ( dist < (epsilon*epsilon) )
5059 {
5060 ret = false;
5061 }
5062 return ret;
5063}
5064
5065bool fm_isValidTriangle(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)
5066{
5067 bool ret = false;
5068
5069 if ( validDistance(p1,p2,epsilon) &&
5070 validDistance(p1,p3,epsilon) &&
5071 validDistance(p2,p3,epsilon) )
5072 {
5073
5074 REAL area = fm_computeArea(p1,p2,p3);
5075 if ( area > epsilon )
5076 {
5077 REAL _vertices[3*3],vertices[64*3];
5078
5079 _vertices[0] = p1[0];
5080 _vertices[1] = p1[1];
5081 _vertices[2] = p1[2];
5082
5083 _vertices[3] = p2[0];
5084 _vertices[4] = p2[1];
5085 _vertices[5] = p2[2];
5086
5087 _vertices[6] = p3[0];
5088 _vertices[7] = p3[1];
5089 _vertices[8] = p3[2];
5090
5091 uint32_t pcount = fm_consolidatePolygon(3,_vertices,sizeof(REAL)*3,vertices,1-epsilon);
5092 if ( pcount == 3 )
5093 {
5094 ret = true;
5095 }
5096 }
5097 }
5098 return ret;
5099}
5100
5101
5102void fm_multiplyQuat(const REAL *left,const REAL *right,REAL *quat)
5103{
5104 REAL a,b,c,d;
5105
5106 a = left[3]*right[3] - left[0]*right[0] - left[1]*right[1] - left[2]*right[2];
5107 b = left[3]*right[0] + right[3]*left[0] + left[1]*right[2] - right[1]*left[2];
5108 c = left[3]*right[1] + right[3]*left[1] + left[2]*right[0] - right[2]*left[0];
5109 d = left[3]*right[2] + right[3]*left[2] + left[0]*right[1] - right[0]*left[1];
5110
5111 quat[3] = a;
5112 quat[0] = b;
5113 quat[1] = c;
5114 quat[2] = d;
5115}
5116
5117bool fm_computeCentroid(uint32_t vcount, // number of input data points
5118 const REAL *points, // starting address of points array.
5119 REAL *center)
5120
5121{
5122 bool ret = false;
5123 if ( vcount )
5124 {
5125 center[0] = 0;
5126 center[1] = 0;
5127 center[2] = 0;
5128 const REAL *p = points;
5129 for (uint32_t i=0; i<vcount; i++)
5130 {
5131 center[0]+=p[0];
5132 center[1]+=p[1];
5133 center[2]+=p[2];
5134 p += 3;
5135 }
5136 REAL recip = 1.0f / (REAL)vcount;
5137 center[0]*=recip;
5138 center[1]*=recip;
5139 center[2]*=recip;
5140 ret = true;
5141 }
5142 return ret;
5143}
5144
5145bool fm_computeCentroid(uint32_t vcount, // number of input data points
5146 const REAL *points, // starting address of points array.
5147 uint32_t triCount,
5148 const uint32_t *indices,
5149 REAL *center)
5150
5151{
5152 bool ret = false;
5153 if (vcount)
5154 {
5155 center[0] = 0;
5156 center[1] = 0;
5157 center[2] = 0;
5158
5159 REAL numerator[3] = { 0, 0, 0 };
5160 REAL denomintaor = 0;
5161
5162 for (uint32_t i = 0; i < triCount; i++)
5163 {
5164 uint32_t i1 = indices[i * 3 + 0];
5165 uint32_t i2 = indices[i * 3 + 1];
5166 uint32_t i3 = indices[i * 3 + 2];
5167
5168 const REAL *p1 = &points[i1 * 3];
5169 const REAL *p2 = &points[i2 * 3];
5170 const REAL *p3 = &points[i3 * 3];
5171
5172 // Compute the sum of the three positions
5173 REAL sum[3];
5174 sum[0] = p1[0] + p2[0] + p3[0];
5175 sum[1] = p1[1] + p2[1] + p3[1];
5176 sum[2] = p1[2] + p2[2] + p3[2];
5177
5178 // Compute the average of the three positions
5179 sum[0] = sum[0] / 3;
5180 sum[1] = sum[1] / 3;
5181 sum[2] = sum[2] / 3;
5182
5183 // Compute the area of this triangle
5184 REAL area = fm_computeArea(p1, p2, p3);
5185
5186 numerator[0]+= (sum[0] * area);
5187 numerator[1]+= (sum[1] * area);
5188 numerator[2]+= (sum[2] * area);
5189
5190 denomintaor += area;
5191
5192 }
5193 REAL recip = 1 / denomintaor;
5194 center[0] = numerator[0] * recip;
5195 center[1] = numerator[1] * recip;
5196 center[2] = numerator[2] * recip;
5197 ret = true;
5198 }
5199 return ret;
5200}
5201
5202
5203#ifndef TEMPLATE_VEC3
5204#define TEMPLATE_VEC3
5205template <class Type> class Vec3
5206{
5207public:
5208 Vec3(void)
5209 {
5210
5211 }
5212 Vec3(Type _x,Type _y,Type _z)
5213 {
5214 x = _x;
5215 y = _y;
5216 z = _z;
5217 }
5218 Type x;
5219 Type y;
5220 Type z;
5221};
5222#endif
5223
5224void fm_transformAABB(const REAL bmin[3],const REAL bmax[3],const REAL matrix[16],REAL tbmin[3],REAL tbmax[3])
5225{
5226 Vec3<REAL> box[8];
5227 box[0] = Vec3< REAL >( bmin[0], bmin[1], bmin[2] );
5228 box[1] = Vec3< REAL >( bmax[0], bmin[1], bmin[2] );
5229 box[2] = Vec3< REAL >( bmax[0], bmax[1], bmin[2] );
5230 box[3] = Vec3< REAL >( bmin[0], bmax[1], bmin[2] );
5231 box[4] = Vec3< REAL >( bmin[0], bmin[1], bmax[2] );
5232 box[5] = Vec3< REAL >( bmax[0], bmin[1], bmax[2] );
5233 box[6] = Vec3< REAL >( bmax[0], bmax[1], bmax[2] );
5234 box[7] = Vec3< REAL >( bmin[0], bmax[1], bmax[2] );
5235 // transform all 8 corners of the box and then recompute a new AABB
5236 for (unsigned int i=0; i<8; i++)
5237 {
5238 Vec3< REAL > &p = box[i];
5239 fm_transform(matrix,&p.x,&p.x);
5240 if ( i == 0 )
5241 {
5242 tbmin[0] = tbmax[0] = p.x;
5243 tbmin[1] = tbmax[1] = p.y;
5244 tbmin[2] = tbmax[2] = p.z;
5245 }
5246 else
5247 {
5248 if ( p.x < tbmin[0] ) tbmin[0] = p.x;
5249 if ( p.y < tbmin[1] ) tbmin[1] = p.y;
5250 if ( p.z < tbmin[2] ) tbmin[2] = p.z;
5251 if ( p.x > tbmax[0] ) tbmax[0] = p.x;
5252 if ( p.y > tbmax[1] ) tbmax[1] = p.y;
5253 if ( p.z > tbmax[2] ) tbmax[2] = p.z;
5254 }
5255 }
5256}
5257
5258REAL fm_normalizeQuat(REAL n[4]) // normalize this quat
5259{
5260 REAL dx = n[0]*n[0];
5261 REAL dy = n[1]*n[1];
5262 REAL dz = n[2]*n[2];
5263 REAL dw = n[3]*n[3];
5264
5265 REAL dist = dx*dx+dy*dy+dz*dz+dw*dw;
5266
5267 dist = (REAL)sqrt(dist);
5268
5269 REAL recip = 1.0f / dist;
5270
5271 n[0]*=recip;
5272 n[1]*=recip;
5273 n[2]*=recip;
5274 n[3]*=recip;
5275
5276 return dist;
5277}
5278
5279
5280}; // end of namespace
5281