IVT
Math3d.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: Math3d.cpp
37 // Author: Pedram Azad
38 // Date: 2004
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "Math3d.h"
49 #include "Helpers/BasicFileIO.h"
50 
51 #include <math.h>
52 
53 
54 
55 // ****************************************************************************
56 // Variables
57 // ****************************************************************************
58 
59 const Vec3d Math3d::zero_vec = { 0, 0, 0 };
60 const Mat3d Math3d::unit_mat = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
61 const Mat3d Math3d::zero_mat = { 0, 0, 0, 0, 0, 0, 0, 0 ,0 };
62 
63 
64 // ****************************************************************************
65 // Functions
66 // ****************************************************************************
67 
68 bool Math3d::LoadFromFile(Vec3d &vector, const char *pFilePath)
69 {
70  // not using fscanf, since it is buggy on some platforms
71  FILE *f = fopen(pFilePath, "r");
72  if (!f)
73  return false;
74 
75  const int nFileSize = CBasicFileIO::GetFileSize(f);
76 
77  if (nFileSize > 1024)
78  {
79  fclose(f);
80  return false;
81  }
82 
83  char *pBuffer = new char[nFileSize];
84  if (fread(pBuffer, nFileSize, 1, f) != 1)
85  {
86  fclose(f);
87  delete [] pBuffer;
88  return false;
89  }
90 
91  fclose(f);
92 
93  if (sscanf(pBuffer, "%f%f%f", &vector.x, &vector.y, &vector.z) != 3)
94  {
95  delete [] pBuffer;
96  return false;
97  }
98 
99  delete [] pBuffer;
100 
101  return true;
102 }
103 
104 bool Math3d::LoadFromFile(Mat3d &matrix, const char *pFilePath)
105 {
106  // not using fscanf, since it is buggy on some platforms
107  FILE *f = fopen(pFilePath, "r");
108  if (!f)
109  return false;
110 
111  const int nFileSize = CBasicFileIO::GetFileSize(f);
112 
113  if (nFileSize > 1024)
114  {
115  fclose(f);
116  return false;
117  }
118 
119  char *pBuffer = new char[nFileSize];
120  if (fread(pBuffer, nFileSize, 1, f) != 1)
121  {
122  fclose(f);
123  delete [] pBuffer;
124  return false;
125  }
126 
127  fclose(f);
128 
129  if (sscanf(pBuffer, "%f%f%f%f%f%f%f%f%f",
130  &matrix.r1, &matrix.r2, &matrix.r3,
131  &matrix.r4, &matrix.r5, &matrix.r6,
132  &matrix.r7, &matrix.r8, &matrix.r9) != 9)
133  {
134  delete [] pBuffer;
135  return false;
136  }
137 
138  delete [] pBuffer;
139 
140  return true;
141 }
142 
143 bool Math3d::LoadFromFile(Transformation3d &transformation, const char *pFilePath)
144 {
145  // not using fscanf, since it is buggy on some platforms
146  FILE *f = fopen(pFilePath, "r");
147  if (!f)
148  return false;
149 
150  const int nFileSize = CBasicFileIO::GetFileSize(f);
151 
152  if (nFileSize > 1024)
153  {
154  fclose(f);
155  return false;
156  }
157 
158  char *pBuffer = new char[nFileSize];
159  if (fread(pBuffer, nFileSize, 1, f) != 1)
160  {
161  fclose(f);
162  delete [] pBuffer;
163  return false;
164  }
165 
166  fclose(f);
167 
168  if (sscanf(pBuffer, "%f%f%f%f%f%f%f%f%f%f%f%f",
169  &transformation.rotation.r1, &transformation.rotation.r2, &transformation.rotation.r3,
170  &transformation.rotation.r4, &transformation.rotation.r5, &transformation.rotation.r6,
171  &transformation.rotation.r7, &transformation.rotation.r8, &transformation.rotation.r9,
172  &transformation.translation.x, &transformation.translation.y, &transformation.translation.z) != 12)
173  {
174  delete [] pBuffer;
175  return false;
176  }
177 
178  delete [] pBuffer;
179 
180  return true;
181 }
182 
183 bool Math3d::SaveToFile(const Vec3d &vector, const char *pFilePath)
184 {
185  FILE *f = fopen(pFilePath, "w");
186  if (!f)
187  return false;
188 
189  if (fprintf(f, "%.10f %.10f %.10f", vector.x, vector.y, vector.z) <= 0)
190  {
191  fclose(f);
192  return false;
193  }
194 
195  fclose(f);
196 
197  return true;
198 }
199 
200 bool Math3d::SaveToFile(const Mat3d &matrix, const char *pFilePath)
201 {
202  FILE *f = fopen(pFilePath, "w");
203  if (!f)
204  return false;
205 
206  if (fprintf(f, "%.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f",
207  matrix.r1, matrix.r2, matrix.r3,
208  matrix.r4, matrix.r5, matrix.r6,
209  matrix.r7, matrix.r8, matrix.r9) <= 0)
210  {
211  fclose(f);
212  return false;
213  }
214 
215  fclose(f);
216 
217  return true;
218 }
219 
220 bool Math3d::SaveToFile(const Transformation3d &transformation, const char *pFilePath)
221 {
222  FILE *f = fopen(pFilePath, "w");
223  if (!f)
224  return false;
225 
226  if (fprintf(f, "%.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f %.10f\n%.10f %.10f %.10f",
227  transformation.rotation.r1, transformation.rotation.r2, transformation.rotation.r3,
228  transformation.rotation.r4, transformation.rotation.r5, transformation.rotation.r6,
229  transformation.rotation.r7, transformation.rotation.r8, transformation.rotation.r9,
230  transformation.translation.x, transformation.translation.y, transformation.translation.z) <= 0)
231  {
232  fclose(f);
233  return false;
234  }
235 
236  fclose(f);
237 
238  return true;
239 }
240 
241 
242 
243 void Math3d::SetVec(Vec3d &vec, float x, float y, float z)
244 {
245  vec.x = x;
246  vec.y = y;
247  vec.z = z;
248 }
249 
250 void Math3d::SetVec(Vec3d &vec, const Vec3d &sourceVector)
251 {
252  vec.x = sourceVector.x;
253  vec.y = sourceVector.y;
254  vec.z = sourceVector.z;
255 }
256 
257 void Math3d::SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
258 {
259  matrix.r1 = r1;
260  matrix.r2 = r2;
261  matrix.r3 = r3;
262  matrix.r4 = r4;
263  matrix.r5 = r5;
264  matrix.r6 = r6;
265  matrix.r7 = r7;
266  matrix.r8 = r8;
267  matrix.r9 = r9;
268 }
269 
270 void Math3d::SetMat(Mat3d &matrix, const Mat3d &sourceMatrix)
271 {
272  matrix.r1 = sourceMatrix.r1;
273  matrix.r2 = sourceMatrix.r2;
274  matrix.r3 = sourceMatrix.r3;
275  matrix.r4 = sourceMatrix.r4;
276  matrix.r5 = sourceMatrix.r5;
277  matrix.r6 = sourceMatrix.r6;
278  matrix.r7 = sourceMatrix.r7;
279  matrix.r8 = sourceMatrix.r8;
280  matrix.r9 = sourceMatrix.r9;
281 }
282 
283 void Math3d::SetRotationMat(Mat3d &matrix, const Vec3d &rotation)
284 {
285  const float alpha = rotation.x;
286  const float beta = rotation.y;
287  const float gamma = rotation.z;
288 
289  const float sinfalpha = sinf(alpha);
290  const float cosfalpha = cosf(alpha);
291  const float sinfbeta = sinf(beta);
292  const float cosfbeta = cosf(beta);
293  const float sinfgamma = sinf(gamma);
294  const float cosfgamma = cosf(gamma);
295 
296  matrix.r1 = cosfbeta * cosfgamma;
297  matrix.r2 = - cosfbeta * sinfgamma;
298  matrix.r3 = sinfbeta;
299  matrix.r4 = cosfalpha * sinfgamma + sinfalpha * sinfbeta * cosfgamma;
300  matrix.r5 = cosfalpha * cosfgamma - sinfalpha * sinfbeta * sinfgamma;
301  matrix.r6 = - sinfalpha * cosfbeta;
302  matrix.r7 = sinfalpha * sinfgamma - cosfalpha * sinfbeta * cosfgamma;
303  matrix.r8 = sinfalpha * cosfgamma + cosfalpha * sinfbeta * sinfgamma;
304  matrix.r9 = cosfalpha * cosfbeta;
305 }
306 
307 void Math3d::SetRotationMat(Mat3d &matrix, float alpha, float beta, float gamma)
308 {
309  const float sinfalpha = sinf(alpha);
310  const float cosfalpha = cosf(alpha);
311  const float sinfbeta = sinf(beta);
312  const float cosfbeta = cosf(beta);
313  const float sinfgamma = sinf(gamma);
314  const float cosfgamma = cosf(gamma);
315 
316  matrix.r1 = cosfbeta * cosfgamma;
317  matrix.r2 = - cosfbeta * sinfgamma;
318  matrix.r3 = sinfbeta;
319  matrix.r4 = cosfalpha * sinfgamma + sinfalpha * sinfbeta * cosfgamma;
320  matrix.r5 = cosfalpha * cosfgamma - sinfalpha * sinfbeta * sinfgamma;
321  matrix.r6 = - sinfalpha * cosfbeta;
322  matrix.r7 = sinfalpha * sinfgamma - cosfalpha * sinfbeta * cosfgamma;
323  matrix.r8 = sinfalpha * cosfgamma + cosfalpha * sinfbeta * sinfgamma;
324  matrix.r9 = cosfalpha * cosfbeta;
325 }
326 
327 void Math3d::SetRotationMatYZX(Mat3d &matrix, const Vec3d &rotation)
328 {
329  Mat3d temp;
330 
331  SetRotationMatY(matrix, rotation.y);
332 
333  SetRotationMatZ(temp, rotation.z);
334  MulMatMat(temp, matrix, matrix);
335 
336  SetRotationMatX(temp, rotation.x);
337  MulMatMat(temp, matrix, matrix);
338 }
339 
340 void Math3d::SetRotationMatX(Mat3d &matrix, float theta)
341 {
342  matrix.r1 = 1;
343  matrix.r2 = matrix.r3 = matrix.r4 = matrix.r7 = 0;
344  matrix.r5 = matrix.r9 = cosf(theta);
345  matrix.r6 = matrix.r8 = sinf(theta);
346  matrix.r6 = -matrix.r6;
347 }
348 
349 void Math3d::SetRotationMatY(Mat3d &matrix, float theta)
350 {
351  matrix.r5 = 1;
352  matrix.r2 = matrix.r4 = matrix.r6 = matrix.r8 = 0;
353  matrix.r1 = matrix.r9 = cosf(theta);
354  matrix.r3 = matrix.r7 = sinf(theta);
355  matrix.r7 = -matrix.r7;
356 }
357 
358 void Math3d::SetRotationMatZ(Mat3d &matrix, float theta)
359 {
360  matrix.r9 = 1;
361  matrix.r3 = matrix.r6 = matrix.r7 = matrix.r8 = 0;
362  matrix.r1 = matrix.r5 = cosf(theta);
363  matrix.r2 = matrix.r4 = sinf(theta);
364  matrix.r2 = -matrix.r2;
365 }
366 
367 void Math3d::SetRotationMat(Mat3d &matrix, const Vec3d &axis, float theta)
368 {
369  const float length = Length(axis);
370  const float v1 = axis.x / length;
371  const float v2 = axis.y / length;
372  const float v3 = axis.z / length;
373 
374  const float t1 = cosf(theta);
375  const float t2 = 1 - t1;
376  const float t3 = v1 * v1;
377  const float t6 = t2 * v1;
378  const float t7 = t6 * v2;
379  const float t8 = sinf(theta);
380  const float t9 = t8 * v3;
381  const float t11 = t6 * v3;
382  const float t12 = t8 * v2;
383  const float t15 = v2 * v2;
384  const float t19 = t2 * v2 * v3;
385  const float t20 = t8 * v1;
386  const float t24 = v3 * v3;
387 
388  matrix.r1 = t1 + t2 * t3;
389  matrix.r2 = t7 - t9;
390  matrix.r3 = t11 + t12;
391  matrix.r4 = t7 + t9;
392  matrix.r5 = t1 + t2 * t15;
393  matrix.r6 = t19 - t20;
394  matrix.r7 = t11 - t12;
395  matrix.r8 = t19 + t20;
396  matrix.r9 = t1 + t2 * t24;
397 }
398 
399 void Math3d::SetRotationMatAxis(Mat3d &matrix, const Vec3d &axis, float theta)
400 {
401  const float length = Length(axis);
402  const float x = axis.x / length;
403  const float y = axis.y / length;
404  const float z = axis.z / length;
405 
406  const float s = sinf(theta);
407  const float c = cosf(theta);
408  const float t = 1.0f - c;
409 
410  matrix.r1 = t * x * x + c;
411  matrix.r2 = t * x * y - s * z;
412  matrix.r3 = t * x * z + s * y;
413  matrix.r4 = t * x * y + s * z;
414  matrix.r5 = t * y * y + c;
415  matrix.r6 = t * y * z - s * x;
416  matrix.r7 = t * x * z - s * y;
417  matrix.r8 = t * y * z + s * x;
418  matrix.r9 = t * z * z + c;
419 }
420 
421 
422 void Math3d::MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
423 {
424  const float x = vec.x;
425  const float y = vec.y;
426  const float z = vec.z;
427 
428  result.x = matrix.r1 * x + matrix.r2 * y + matrix.r3 * z;
429  result.y = matrix.r4 * x + matrix.r5 * y + matrix.r6 * z;
430  result.z = matrix.r7 * x + matrix.r8 * y + matrix.r9 * z;
431 }
432 
433 void Math3d::MulMatVec(const Mat3d &matrix, const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
434 {
435  const float x = vector1.x;
436  const float y = vector1.y;
437  const float z = vector1.z;
438 
439  result.x = matrix.r1 * x + matrix.r2 * y + matrix.r3 * z + vector2.x;
440  result.y = matrix.r4 * x + matrix.r5 * y + matrix.r6 * z + vector2.y;
441  result.z = matrix.r7 * x + matrix.r8 * y + matrix.r9 * z + vector2.z;
442 }
443 
444 void Math3d::MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
445 {
446  const float x1 = matrix1.r1 * matrix2.r1 + matrix1.r2 * matrix2.r4 + matrix1.r3 * matrix2.r7;
447  const float x2 = matrix1.r1 * matrix2.r2 + matrix1.r2 * matrix2.r5 + matrix1.r3 * matrix2.r8;
448  const float x3 = matrix1.r1 * matrix2.r3 + matrix1.r2 * matrix2.r6 + matrix1.r3 * matrix2.r9;
449  const float x4 = matrix1.r4 * matrix2.r1 + matrix1.r5 * matrix2.r4 + matrix1.r6 * matrix2.r7;
450  const float x5 = matrix1.r4 * matrix2.r2 + matrix1.r5 * matrix2.r5 + matrix1.r6 * matrix2.r8;
451  const float x6 = matrix1.r4 * matrix2.r3 + matrix1.r5 * matrix2.r6 + matrix1.r6 * matrix2.r9;
452  const float x7 = matrix1.r7 * matrix2.r1 + matrix1.r8 * matrix2.r4 + matrix1.r9 * matrix2.r7;
453  const float x8 = matrix1.r7 * matrix2.r2 + matrix1.r8 * matrix2.r5 + matrix1.r9 * matrix2.r8;
454  const float x9 = matrix1.r7 * matrix2.r3 + matrix1.r8 * matrix2.r6 + matrix1.r9 * matrix2.r9;
455 
456  result.r1 = x1;
457  result.r2 = x2;
458  result.r3 = x3;
459  result.r4 = x4;
460  result.r5 = x5;
461  result.r6 = x6;
462  result.r7 = x7;
463  result.r8 = x8;
464  result.r9 = x9;
465 }
466 
467 void Math3d::MulVecTransposedVec(const Vec3d &vector1, const Vec3d &vector2, Mat3d &result)
468 {
469  result.r1 = vector1.x * vector2.x;
470  result.r2 = vector1.x * vector2.y;
471  result.r3 = vector1.x * vector2.z;
472  result.r4 = vector1.y * vector2.x;
473  result.r5 = vector1.y * vector2.y;
474  result.r6 = vector1.y * vector2.z;
475  result.r7 = vector1.z * vector2.x;
476  result.r8 = vector1.z * vector2.y;
477  result.r9 = vector1.z * vector2.z;
478 }
479 
480 
481 void Math3d::AddToVec(Vec3d &vec, const Vec3d &vectorToAdd)
482 {
483  vec.x += vectorToAdd.x;
484  vec.y += vectorToAdd.y;
485  vec.z += vectorToAdd.z;
486 }
487 
488 void Math3d::SubtractFromVec(Vec3d &vec, const Vec3d &vectorToSubtract)
489 {
490  vec.x -= vectorToSubtract.x;
491  vec.y -= vectorToSubtract.y;
492  vec.z -= vectorToSubtract.z;
493 }
494 
495 void Math3d::AddVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
496 {
497  result.x = vector1.x + vector2.x;
498  result.y = vector1.y + vector2.y;
499  result.z = vector1.z + vector2.z;
500 }
501 
502 void Math3d::MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
503 {
504  result.x = scalar * vec.x;
505  result.y = scalar * vec.y;
506  result.z = scalar * vec.z;
507 }
508 
509 void Math3d::MulMatScalar(const Mat3d &matrix, float scalar, Mat3d &result)
510 {
511  result.r1 = scalar * matrix.r1;
512  result.r2 = scalar * matrix.r2;
513  result.r3 = scalar * matrix.r3;
514  result.r4 = scalar * matrix.r4;
515  result.r5 = scalar * matrix.r5;
516  result.r6 = scalar * matrix.r6;
517  result.r7 = scalar * matrix.r7;
518  result.r8 = scalar * matrix.r8;
519  result.r9 = scalar * matrix.r9;
520 }
521 
522 void Math3d::SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
523 {
524  result.x = vector1.x - vector2.x;
525  result.y = vector1.y - vector2.y;
526  result.z = vector1.z - vector2.z;
527 }
528 
529 
530 void Math3d::RotateVec(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
531 {
532  Mat3d matrix;
533  SetRotationMat(matrix, rotation);
534  MulMatVec(matrix, vec, result);
535 }
536 
537 void Math3d::TransformVec(const Vec3d &vec, const Vec3d &rotation, const Vec3d &translation, Vec3d &result)
538 {
539  Mat3d matrix;
540  SetRotationMat(matrix, rotation);
541  MulMatVec(matrix, vec, translation, result);
542 }
543 
544 void Math3d::RotateVecYZX(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
545 {
546  Mat3d matrix;
547  SetRotationMatYZX(matrix, rotation);
548  MulMatVec(matrix, vec, result);
549 }
550 
551 void Math3d::TransformVecYZX(const Vec3d &vec, const Vec3d &rotation, const Vec3d &translation, Vec3d &result)
552 {
553  Mat3d matrix;
554  SetRotationMatYZX(matrix, rotation);
555  MulMatVec(matrix, vec, translation, result);
556 }
557 
558 
559 float Math3d::ScalarProduct(const Vec3d &vector1, const Vec3d &vector2)
560 {
561  return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
562 }
563 
564 void Math3d::CrossProduct(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
565 {
566  const float x = vector1.y * vector2.z - vector1.z * vector2.y;
567  const float y = vector1.z * vector2.x - vector1.x * vector2.z;
568  result.z = vector1.x * vector2.y - vector1.y * vector2.x;
569  result.x = x;
570  result.y = y;
571 }
572 
574 {
575  const float length = sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
576 
577  if (length != 0.0f)
578  {
579  vec.x /= length;
580  vec.y /= length;
581  vec.z /= length;
582  }
583 }
584 
585 float Math3d::Length(const Vec3d &vec)
586 {
587  return sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
588 }
589 
590 float Math3d::SquaredLength(const Vec3d &vec)
591 {
592  return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
593 }
594 
595 float Math3d::Distance(const Vec3d &vector1, const Vec3d &vector2)
596 {
597  const float x1 = vector1.x - vector2.x;
598  const float x2 = vector1.y - vector2.y;
599  const float x3 = vector1.z - vector2.z;
600 
601  return sqrtf(x1 * x1 + x2 * x2 + x3 * x3);
602 }
603 
604 float Math3d::SquaredDistance(const Vec3d &vector1, const Vec3d &vector2)
605 {
606  const float x1 = vector1.x - vector2.x;
607  const float x2 = vector1.y - vector2.y;
608  const float x3 = vector1.z - vector2.z;
609 
610  return x1 * x1 + x2 * x2 + x3 * x3;
611 }
612 
613 float Math3d::Angle(const Vec3d &vector1, const Vec3d &vector2)
614 {
615  const float sp = vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
616  const float l1 = sqrtf(vector1.x * vector1.x + vector1.y * vector1.y + vector1.z * vector1.z);
617  const float l2 = sqrtf(vector2.x * vector2.x + vector2.y * vector2.y + vector2.z * vector2.z);
618 
619  // added this. In some cases angle was numerically unstable
620  float r = sp / (l1 * l2);
621  if (r > 1.0) r = 1.0;
622  if (r < -1.0) r = -1.0;
623  return acosf(r);
624 }
625 
626 float Math3d::EvaluateForm(const Vec3d &matrix1, const Mat3d &matrix2)
627 {
628  const float t0 = matrix1.x * matrix2.r1 + matrix1.y * matrix2.r4 + matrix1.z * matrix2.r7;
629  const float t1 = matrix1.x * matrix2.r2 + matrix1.y * matrix2.r5 + matrix1.z * matrix2.r8;
630  const float t2 = matrix1.x * matrix2.r3 + matrix1.y * matrix2.r6 + matrix1.z * matrix2.r9;
631 
632  return t0 * matrix1.x + t1 * matrix1.y + t2 * matrix1.z;
633 }
634 
635 void Math3d::Transpose(const Mat3d &matrix, Mat3d &result)
636 {
637  float temp;
638 
639  result.r1 = matrix.r1;
640  result.r5 = matrix.r5;
641  result.r9 = matrix.r9;
642 
643  temp = matrix.r4;
644  result.r4 = matrix.r2;
645  result.r2 = temp;
646 
647  temp = matrix.r3;
648  result.r3 = matrix.r7;
649  result.r7 = temp;
650 
651  temp = matrix.r6;
652  result.r6 = matrix.r8;
653  result.r8 = temp;
654 
655 }
656 
657 void Math3d::Invert(const Mat3d &matrix, Mat3d &result)
658 {
659  const float a = matrix.r1;
660  const float b = matrix.r2;
661  const float c = matrix.r3;
662  const float d = matrix.r4;
663  const float e = matrix.r5;
664  const float f = matrix.r6;
665  const float g = matrix.r7;
666  const float h = matrix.r8;
667  const float i = matrix.r9;
668 
669  float det_inverse = 1 / (-c * e * g + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i);
670 
671  result.r1 = (-f * h + e * i) * det_inverse;
672  result.r2 = (c * h - b * i) * det_inverse;
673  result.r3 = (-c * e + b * f) * det_inverse;
674  result.r4 = (f * g - d * i) * det_inverse;
675  result.r5 = (-c * g + a * i) * det_inverse;
676  result.r6 = (c * d - a * f) * det_inverse;
677  result.r7 = (-e * g + d * h) * det_inverse;
678  result.r8 = (b * g - a * h) * det_inverse;
679  result.r9 = (-b * d + a * e) * det_inverse;
680 }
681 
682 void Math3d::AddMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &matrix)
683 {
684  matrix.r1 = matrix1.r1 + matrix2.r1;
685  matrix.r2 = matrix1.r2 + matrix2.r2;
686  matrix.r3 = matrix1.r3 + matrix2.r3;
687  matrix.r4 = matrix1.r4 + matrix2.r4;
688  matrix.r5 = matrix1.r5 + matrix2.r5;
689  matrix.r6 = matrix1.r6 + matrix2.r6;
690  matrix.r7 = matrix1.r7 + matrix2.r7;
691  matrix.r8 = matrix1.r8 + matrix2.r8;
692  matrix.r9 = matrix1.r9 + matrix2.r9;
693 }
694 
695 void Math3d::AddToMat(Mat3d &matrix, const Mat3d &matrixToAdd)
696 {
697  matrix.r1 += matrixToAdd.r1;
698  matrix.r2 += matrixToAdd.r2;
699  matrix.r3 += matrixToAdd.r3;
700  matrix.r4 += matrixToAdd.r4;
701  matrix.r5 += matrixToAdd.r5;
702  matrix.r6 += matrixToAdd.r6;
703  matrix.r7 += matrixToAdd.r7;
704  matrix.r8 += matrixToAdd.r8;
705  matrix.r9 += matrixToAdd.r9;
706 }
707 
708 void Math3d::SubtractMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
709 {
710  result.r1 = matrix1.r1 - matrix2.r1;
711  result.r2 = matrix1.r2 - matrix2.r2;
712  result.r3 = matrix1.r3 - matrix2.r3;
713  result.r4 = matrix1.r4 - matrix2.r4;
714  result.r5 = matrix1.r5 - matrix2.r5;
715  result.r6 = matrix1.r6 - matrix2.r6;
716  result.r7 = matrix1.r7 - matrix2.r7;
717  result.r8 = matrix1.r8 - matrix2.r8;
718  result.r9 = matrix1.r9 - matrix2.r9;
719 }
720 
721 float Math3d::Det(const Mat3d &matrix)
722 {
723  return matrix.r1 * matrix.r5 * matrix.r9
724  + matrix.r2 * matrix.r6 * matrix.r7
725  + matrix.r3 * matrix.r4 * matrix.r8
726  - matrix.r3 * matrix.r5 * matrix.r7
727  - matrix.r1 * matrix.r6 * matrix.r8
728  - matrix.r2 * matrix.r4 * matrix.r9;
729 }
730 
731 
732 void Math3d::SetTransformation(Transformation3d &transformation, const Vec3d &rotation, const Vec3d &translation)
733 {
734  Math3d::SetRotationMat(transformation.rotation, rotation);
735  Math3d::SetVec(transformation.translation, translation);
736 }
737 
738 void Math3d::SetTransformation(Transformation3d &transformation, const Transformation3d &sourceTransformation)
739 {
740  Math3d::SetMat(transformation.rotation, sourceTransformation.rotation);
741  Math3d::SetVec(transformation.translation, sourceTransformation.translation);
742 }
743 
745 {
746  Math3d::Invert(input.rotation, result.rotation);
747  Math3d::MulMatVec(result.rotation, input.translation, result.translation);
748  Math3d::MulVecScalar(result.translation, -1, result.translation);
749 }
750 
751 void Math3d::MulTransTrans(const Transformation3d &transformation1, const Transformation3d &transformation2, Transformation3d &result)
752 {
753  Math3d::MulMatVec(transformation1.rotation, transformation2.translation, transformation1.translation, result.translation);
754  Math3d::MulMatMat(transformation1.rotation, transformation2.rotation, result.rotation);
755 }
756 
757 void Math3d::MulTransVec(const Transformation3d &transformation, const Vec3d &vec, Vec3d &result)
758 {
759  Math3d::MulMatVec(transformation.rotation, vec, transformation.translation, result);
760 }
761 
762 
763 void Math3d::MulQuatQuat(const Quaternion &quat1, const Quaternion &quat2, Quaternion &result)
764 {
765  Vec3d v;
766 
767  CrossProduct(quat1.v, quat2.v, v);
768  v.x += quat1.w * quat2.v.x + quat1.w * quat2.v.x;
769  v.y += quat1.w * quat2.v.y + quat1.w * quat2.v.y;
770  v.z += quat1.w * quat2.v.z + quat1.w * quat2.v.z;
771 
772  result.w = quat1.w * quat2.w - ScalarProduct(quat1.v, quat2.v);
773  SetVec(result.v, v);
774 }
775 
776 
777 void Math3d::RotateVecQuaternion(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
778 {
779  /*const float st = sinf(theta * 0.5f);
780  const float ct = cosf(theta * 0.5f);
781 
782  Quaternion a, q, r;
783  Vec3d u;
784 
785  // u = normalized axis vector
786  SetVec(u, axis);
787  NormalizeVec(u);
788 
789  // set a
790  SetVec(a.v, vec);
791  a.w = 0;
792 
793  // set q
794  q.v.x = st * u.x;
795  q.v.y = st * u.y;
796  q.v.z = st * u.z;
797  q.w = ct;
798 
799  // calculate q * a
800  MulQuatQuat(q, a, r);
801 
802  // calculate conjugate quaternion of q
803  q.v.x = -q.v.x;
804  q.v.y = -q.v.y;
805  q.v.z = -q.v.z;
806 
807  // calculate q * a * q^
808  MulQuatQuat(r, q, r);*/
809 
810  const float length = Length(axis);
811  const float a = axis.x / length;
812  const float b = axis.y / length;
813  const float c = axis.z / length;
814  const float d = theta;
815 
816  const float v1 = vec.x;
817  const float v2 = vec.y;
818  const float v3 = vec.z;
819 
820  const float t2 = a * b;
821  const float t3 = a * c;
822  const float t4 = a * d;
823  const float t5 = -b * b;
824  const float t6 = b * c;
825  const float t7 = b * d;
826  const float t8 = -c * c;
827  const float t9 = c * d;
828  const float t10 = -d * d;
829 
830  result.x = 2 * ((t8 + t10) * v1 + (t6 - t4) * v2 + (t3 + t7) * v3 ) + v1;
831  result.y = 2 * ((t4 + t6) * v1 + (t5 + t10) * v2 + (t9 - t2) * v3 ) + v2;
832  result.z = 2 * ((t7 - t3) * v1 + (t2 + t9) * v2 + (t5 + t8) * v3 ) + v3;
833 }
834 
835 void Math3d::RotateVecAngleAxis(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
836 {
837  Mat3d m;
838  SetRotationMatAxis(m, axis, theta);
839  MulMatVec(m, vec, result);
840 }
841 
842 float Math3d::Angle(const Vec3d &vector1, const Vec3d &vector2, const Vec3d &axis)
843 {
844  Vec3d u1, u2, n, temp;
845  Mat3d testMatrix;
846 
847  Math3d::SetVec(n, axis);
849 
850  Math3d::SetVec(u1, vector1);
852  Math3d::SubtractFromVec(u1, temp);
854 
855  Math3d::SetVec(u2, vector2);
857  Math3d::SubtractFromVec(u2, temp);
859 
860  // test which one of the two solutions is the right one
861  Math3d::SetRotationMatAxis(testMatrix, n, Math3d::Angle(u1, u2));
862  Math3d::MulMatVec(testMatrix, u1, temp);
863  const float d1 = Math3d::Distance(temp, u2);
864 
865  Math3d::SetRotationMatAxis(testMatrix, n, -Math3d::Angle(u1, u2));
866  Math3d::MulMatVec(testMatrix, u1, temp);
867  const float d2 = Math3d::Distance(temp, u2);
868 
869  return d1 < d2 ? Math3d::Angle(u1, u2) : -Math3d::Angle(u1, u2);
870 }
871 
872 void Math3d::GetAxisAndAngle(const Mat3d &R, Vec3d &axis, float &angle)
873 {
874  const float x = R.r8 - R.r6;
875  const float y = R.r3 - R.r7;
876  const float z = R.r4 - R.r2;
877 
878  const float r = sqrtf(x * x + y * y + z * z);
879  const float t = R.r1 + R.r5 + R.r9;
880 
881  angle = atan2(r, t - 1);
882 
883  Math3d::SetVec(axis, x, y, z);
884 }
885 
886 void Math3d::Average(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
887 {
888  result.x = 0.5f * (vector1.x + vector2.x);
889  result.y = 0.5f * (vector1.y + vector2.y);
890  result.z = 0.5f * (vector1.z + vector2.z);
891 }
892 
893 void Math3d::Mean(const CVec3dArray &vectorList, Vec3d &result)
894 {
895  Mean(vectorList.GetElements(), vectorList.GetSize(), result);
896 }
897 
898 void Math3d::Mean(const Vec3d *pVectors, int nVectors, Vec3d &result)
899 {
900  if (nVectors == 0)
901  {
902  result = zero_vec;
903  return;
904  }
905 
906  float sum_x = 0.0f, sum_y = 0.0f, sum_z = 0.0f;
907 
908  for (int i = 0; i < nVectors; i++)
909  {
910  sum_x += pVectors[i].x;
911  sum_y += pVectors[i].y;
912  sum_z += pVectors[i].z;
913  }
914 
915  result.x = sum_x / float(nVectors);
916  result.y = sum_y / float(nVectors);
917  result.z = sum_z / float(nVectors);
918 }
void RotateVecAngleAxis(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
Definition: Math3d.cpp:835
void Mean(const CVec3dArray &vectorList, Vec3d &result)
Definition: Math3d.cpp:893
void Invert(const Mat3d &matrix, Mat3d &result)
Definition: Math3d.cpp:657
float y
Definition: Math3d.h:75
void RotateVecQuaternion(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
Definition: Math3d.cpp:777
void MulMatMat(const CFloatMatrix *A, const CFloatMatrix *B, CFloatMatrix *pResultMatrix, bool bTransposeB=false)
float r4
Definition: Math3d.h:95
float r3
Definition: Math3d.h:95
const Mat3d zero_mat
Definition: Math3d.cpp:61
void SetRotationMatZ(Mat3d &matrix, float theta)
Definition: Math3d.cpp:358
float w
Definition: Math3d.h:123
void AddToMat(Mat3d &matrix, const Mat3d &matrixToAdd)
Definition: Math3d.cpp:695
void TransformVecYZX(const Vec3d &vec, const Vec3d &rotation, const Vec3d &translation, Vec3d &result)
Definition: Math3d.cpp:551
void MulTransVec(const Transformation3d &transformation, const Vec3d &vec, Vec3d &result)
Definition: Math3d.cpp:757
void SetRotationMatAxis(Mat3d &matrix, const Vec3d &axis, float theta)
Definition: Math3d.cpp:399
float r1
Definition: Math3d.h:95
float r7
Definition: Math3d.h:95
float EvaluateForm(const Vec3d &matrix1, const Mat3d &matrix2)
Definition: Math3d.cpp:626
float ScalarProduct(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:559
float x
Definition: Math3d.h:75
float r2
Definition: Math3d.h:95
const Mat3d unit_mat
Definition: Math3d.cpp:60
float Length(const Vec3d &vec)
Definition: Math3d.cpp:585
Vec3d translation
Definition: Math3d.h:108
bool SaveToFile(const Vec3d &vector, const char *pFilePath)
Definition: Math3d.cpp:183
void RotateVecYZX(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
Definition: Math3d.cpp:544
float Det(const Mat3d &matrix)
Definition: Math3d.cpp:721
double Mean(CByteImage *pImage1, CByteImage *pImage2)
Deprecated.
static int GetFileSize(FILE *fp)
void MulVecTransposedVec(const Vec3d &vector1, const Vec3d &vector2, Mat3d &result)
Definition: Math3d.cpp:467
float r8
Definition: Math3d.h:95
void NormalizeVec(Vec3d &vec)
Definition: Math3d.cpp:573
float Distance(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:595
Vec3d v
Definition: Math3d.h:121
void SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:522
void MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:444
void Average(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:886
float ScalarProduct(const Vec2d &vector1, const Vec2d &vector2)
Definition: Math2d.cpp:155
bool LoadFromFile(Vec3d &vector, const char *pFilePath)
Definition: Math3d.cpp:68
Mat3d rotation
Definition: Math3d.h:107
const Vec3d zero_vec
Definition: Math3d.cpp:59
void GetAxisAndAngle(const Mat3d &R, Vec3d &axis, float &angle)
Definition: Math3d.cpp:872
void TransformVec(const Vec3d &vec, const Vec3d &rotation, const Vec3d &translation, Vec3d &result)
Definition: Math3d.cpp:537
void SetTransformation(Transformation3d &transformation, const Vec3d &rotation, const Vec3d &translation)
Definition: Math3d.cpp:732
const T * GetElements() const
void MulTransTrans(const Transformation3d &transformation1, const Transformation3d &transformation2, Transformation3d &result)
Definition: Math3d.cpp:751
void SetRotationMatY(Mat3d &matrix, float theta)
Definition: Math3d.cpp:349
float Length(const Vec2d &vec)
Definition: Math2d.cpp:171
void SetRotationMat(Mat3d &matrix, const Vec3d &axis, float theta)
Definition: Math3d.cpp:367
void SetRotationMat(Mat2d &matrix, float angle)
Definition: Math2d.cpp:99
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
void Transpose(const Mat3d &matrix, Mat3d &result)
Definition: Math3d.cpp:635
void MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
Definition: Math3d.cpp:422
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
void AddMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &matrix)
Definition: Math3d.cpp:682
void MulQuatQuat(const Quaternion &quat1, const Quaternion &quat2, Quaternion &result)
Definition: Math3d.cpp:763
float z
Definition: Math3d.h:75
float SquaredLength(const Vec3d &vec)
Definition: Math3d.cpp:590
Data structure for the representation of a quaternion.
Definition: Math3d.h:118
const Vec2d zero_vec
Definition: Math2d.cpp:61
float r6
Definition: Math3d.h:95
void SubtractFromVec(Vec3d &vec, const Vec3d &vectorToSubtract)
Definition: Math3d.cpp:488
void AddVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:495
float SquaredDistance(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:604
void AddToVec(Vec3d &vec, const Vec3d &vectorToAdd)
Definition: Math3d.cpp:481
void SubtractMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:708
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
void MulMatScalar(const Mat3d &matrix, float scalar, Mat3d &result)
Definition: Math3d.cpp:509
void SetVec(Vec2d &vec, float x, float y)
Definition: Math2d.cpp:68
float r9
Definition: Math3d.h:95
void MulMatVec(const CFloatMatrix *pMatrix, const CFloatVector *pVector, CFloatVector *pResultVector)
void SetRotationMatX(Mat3d &matrix, float theta)
Definition: Math3d.cpp:340
void MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
Definition: Math3d.cpp:502
void RotateVec(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
Definition: Math3d.cpp:530
Data structure for the representation of a 3D rigid body transformation.
Definition: Math3d.h:105
void SetRotationMatYZX(Mat3d &matrix, const Vec3d &rotation)
Definition: Math3d.cpp:327
float r5
Definition: Math3d.h:95
void SetVec(Vec3d &vec, float x, float y, float z)
Definition: Math3d.cpp:243
float Angle(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:613
void CrossProduct(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:564