Open Source Tomb Raider Engine
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

math.cpp 2.1KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293
  1. /*!
  2. *
  3. * \file src/math/math.cpp
  4. * \brief Vector and Matrix math
  5. *
  6. * \author Mongoose
  7. * \author xythobuz
  8. */
  9. #include <stdlib.h>
  10. #include <math.h>
  11. #include <float.h>
  12. #include <assert.h>
  13. #include "math/Vector3d.h"
  14. #include "math/Matrix.h"
  15. #include "math/math.h"
  16. bool equalEpsilon(vec_t a, vec_t b) {
  17. vec_t epsilon = FLT_EPSILON;
  18. if (fabs(a - b) <= (fmax(fabs(a), fabs(b)) * epsilon))
  19. return true;
  20. return false;
  21. }
  22. int intersectionLinePolygon(vec3_t intersect,
  23. vec3_t p1, vec3_t p2, vec3_t *polygon) {
  24. assert(polygon != NULL);
  25. // vec3_t normal, a, b;
  26. Vector3d a, b, normal, pA, pB;
  27. vec_t d, denominator, mu;
  28. pA = Vector3d(p1);
  29. pB = Vector3d(p2);
  30. // Find normal
  31. a = Vector3d(polygon[1]) - Vector3d(polygon[0]);
  32. b = Vector3d(polygon[2]) - Vector3d(polygon[0]);
  33. normal = Vector3d::cross(a, b);
  34. normal.normalize();
  35. // find D
  36. d = (normal.mVec[0] * polygon[0][0] -
  37. normal.mVec[1] * polygon[0][1] -
  38. normal.mVec[2] * polygon[0][2]);
  39. // line segment parallel to plane?
  40. a = pB - pA;
  41. denominator = Vector3d::dot(normal, a);
  42. if (denominator > 0.0)
  43. return 0;
  44. // Line segment contains intercept point?
  45. mu = -((d + Vector3d::dot(normal, pA)) / denominator);
  46. if (mu < 0.0 || mu > 1.0)
  47. return 0;
  48. b = pA + (a * mu);
  49. intersect[0] = b.mVec[0];
  50. intersect[1] = b.mVec[1];
  51. intersect[2] = b.mVec[2];
  52. // See if the intercept is bound by polygon by winding number
  53. // assume convex polygons here for sure
  54. double theta = Vector3d::dot(b - Vector3d(polygon[0]), normal); // b = intersect
  55. if (theta >= 90.0) // Yeah I know
  56. return 0;
  57. return 1;
  58. }
  59. vec_t distance(const vec3_t a, const vec3_t b) {
  60. return sqrtf(((b[0] - a[0]) * (b[0] - a[0])) +
  61. ((b[1] - a[1]) * (b[1] - a[1])) +
  62. ((b[2] - a[2]) * (b[2] - a[2])));
  63. }
  64. void midpoint(const vec3_t a, const vec3_t b, vec3_t mid) {
  65. mid[0] = (a[0] + b[0]) / 2.0f;
  66. mid[1] = (a[1] + b[1]) / 2.0f;
  67. mid[2] = (a[2] + b[2]) / 2.0f;
  68. }
  69. vec_t randomNum(vec_t from, vec_t to) {
  70. return from + ((to - from) * rand() / (RAND_MAX + 1.0f));
  71. }