9
3

position.cpp 7.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255
  1. #include "position.h"
  2. #include <algorithm>
  3. #include <string>
  4. #include <cmath>
  5. #include "../../common/string_util.h"
  6. static const float position_eps = 0.0001f;
  7. std::string to_string(const glm::vec4 &position) {
  8. return StringFormat("(%.3f, %.3f, %.3f, %.3f)", position.x,position.y,position.z,position.w);
  9. }
  10. std::string to_string(const glm::vec3 &position){
  11. return StringFormat("(%.3f, %.3f, %.3f)", position.x,position.y,position.z);
  12. }
  13. std::string to_string(const glm::vec2 &position){
  14. return StringFormat("(%.3f, %.3f)", position.x,position.y);
  15. }
  16. bool IsOrigin(const glm::vec2 &position) {
  17. return glm::dot(position, position) == 0;
  18. }
  19. bool IsOrigin(const glm::vec3 &position) {
  20. return glm::dot(position, position) == 0;
  21. }
  22. bool IsOrigin(const glm::vec4 &position) {
  23. return IsOrigin(glm::vec3(position));
  24. }
  25. /**
  26. * Produces the non square root'ed distance between the two points within the XY plane.
  27. */
  28. float DistanceSquared(const glm::vec2& point1, const glm::vec2& point2) {
  29. auto diff = point1 - point2;
  30. return glm::dot(diff, diff);
  31. }
  32. /**
  33. * Produces the distance between the two points on the XY plane.
  34. */
  35. float Distance(const glm::vec2& point1, const glm::vec2& point2) {
  36. return std::sqrt(DistanceSquared(point1, point2));
  37. }
  38. /**
  39. * Produces the non square root'ed distance between the two points.
  40. */
  41. float DistanceSquared(const glm::vec3& point1, const glm::vec3& point2) {
  42. auto diff = point1 - point2;
  43. return glm::dot(diff, diff);
  44. }
  45. /**
  46. * Produces the non square root'ed distance between the two points.
  47. */
  48. float DistanceSquared(const glm::vec4& point1, const glm::vec4& point2) {
  49. return DistanceSquared(static_cast<glm::vec3>(point1), static_cast<glm::vec3>(point2));
  50. }
  51. /**
  52. * Produces the distance between the two points.
  53. */
  54. float Distance(const glm::vec3& point1, const glm::vec3& point2) {
  55. return std::sqrt(DistanceSquared(point1, point2));
  56. }
  57. /**
  58. * Produces the distance between the two points.
  59. */
  60. float Distance(const glm::vec4& point1, const glm::vec4& point2) {
  61. return Distance(static_cast<glm::vec3>(point1), static_cast<glm::vec3>(point2));
  62. }
  63. /**
  64. * Produces the distance between the two points within the XY plane.
  65. */
  66. float DistanceNoZ(const glm::vec3& point1, const glm::vec3& point2) {
  67. return Distance(static_cast<glm::vec2>(point1),static_cast<glm::vec2>(point2));
  68. }
  69. /**
  70. * Produces the distance between the two points within the XY plane.
  71. */
  72. float DistanceNoZ(const glm::vec4& point1, const glm::vec4& point2) {
  73. return Distance(static_cast<glm::vec2>(point1),static_cast<glm::vec2>(point2));
  74. }
  75. /**
  76. * Produces the non square root'ed distance between the two points within the XY plane.
  77. */
  78. float DistanceSquaredNoZ(const glm::vec3& point1, const glm::vec3& point2) {
  79. return DistanceSquared(static_cast<glm::vec2>(point1),static_cast<glm::vec2>(point2));
  80. }
  81. /**
  82. * Produces the non square root'ed distance between the two points within the XY plane.
  83. */
  84. float DistanceSquaredNoZ(const glm::vec4& point1, const glm::vec4& point2) {
  85. return DistanceSquared(static_cast<glm::vec2>(point1),static_cast<glm::vec2>(point2));
  86. }
  87. /**
  88. * Determines if 'position' is within (inclusive) the axis aligned
  89. * box (3 dimensional) formed from the points minimum and maximum.
  90. */
  91. bool IsWithinAxisAlignedBox(const glm::vec3 &position, const glm::vec3 &minimum, const glm::vec3 &maximum) {
  92. auto actualMinimum = glm::vec3(std::min(minimum.x, maximum.x), std::min(minimum.y, maximum.y),std::min(minimum.z, maximum.z));
  93. auto actualMaximum = glm::vec3(std::max(minimum.x, maximum.x), std::max(minimum.y, maximum.y),std::max(minimum.z, maximum.z));
  94. bool xcheck = position.x >= actualMinimum.x && position.x <= actualMaximum.x;
  95. bool ycheck = position.y >= actualMinimum.y && position.y <= actualMaximum.y;
  96. bool zcheck = position.z >= actualMinimum.z && position.z <= actualMaximum.z;
  97. return xcheck && ycheck && zcheck;
  98. }
  99. /**
  100. * Determines if 'position' is within (inclusive) the axis aligned
  101. * box (2 dimensional) formed from the points minimum and maximum.
  102. */
  103. bool IsWithinAxisAlignedBox(const glm::vec2 &position, const glm::vec2 &minimum, const glm::vec2 &maximum) {
  104. auto actualMinimum = glm::vec2(std::min(minimum.x, maximum.x), std::min(minimum.y, maximum.y));
  105. auto actualMaximum = glm::vec2(std::max(minimum.x, maximum.x), std::max(minimum.y, maximum.y));
  106. bool xcheck = position.x >= actualMinimum.x && position.x <= actualMaximum.x;
  107. bool ycheck = position.y >= actualMinimum.y && position.y <= actualMaximum.y;
  108. return xcheck && ycheck;
  109. }
  110. /**
  111. * Gives the heading directly 180 degrees from the
  112. * current heading.
  113. * Takes the EQfloat from the glm::vec4 and returns
  114. * an EQFloat.
  115. */
  116. float GetReciprocalHeading(const glm::vec4& point1) {
  117. return GetReciprocalHeading(point1.w);
  118. }
  119. /**
  120. * Gives the heading directly 180 degrees from the
  121. * current heading.
  122. * Takes an EQfloat and returns an EQFloat.
  123. */
  124. float GetReciprocalHeading(const float heading)
  125. {
  126. float result = 0;
  127. // Convert to radians
  128. float h = (heading / 512.0f) * 6.283184f;
  129. // Calculate the reciprocal heading in radians
  130. result = h + 3.141592f;
  131. // Convert back to eq heading from radians
  132. result = (result / 6.283184f) * 512.0f;
  133. return result;
  134. }
  135. bool IsHeadingEqual(const float h1, const float h2)
  136. {
  137. return std::abs(h2 - h1) < 0.01f;
  138. }
  139. bool IsPositionEqual(const glm::vec2 &p1, const glm::vec2 &p2)
  140. {
  141. return std::abs(p1.x - p2.x) < position_eps && std::abs(p1.y - p2.y) < position_eps;
  142. }
  143. bool IsPositionEqual(const glm::vec3 &p1, const glm::vec3 &p2)
  144. {
  145. return std::abs(p1.x - p2.x) < position_eps && std::abs(p1.y - p2.y) < position_eps && std::abs(p1.z - p2.z) < position_eps;
  146. }
  147. bool IsPositionEqual(const glm::vec4 &p1, const glm::vec4 &p2)
  148. {
  149. return std::abs(p1.x - p2.x) < position_eps && std::abs(p1.y - p2.y) < position_eps && std::abs(p1.z - p2.z) < position_eps;
  150. }
  151. bool IsPositionEqualWithinCertainZ(const glm::vec3 &p1, const glm::vec3 &p2, float z_eps) {
  152. return std::abs(p1.x - p2.x) < position_eps && std::abs(p1.y - p2.y) < position_eps && std::abs(p1.z - p2.z) < z_eps;
  153. }
  154. bool IsPositionEqualWithinCertainZ(const glm::vec4 &p1, const glm::vec4 &p2, float z_eps) {
  155. return std::abs(p1.x - p2.x) < position_eps && std::abs(p1.y - p2.y) < position_eps && std::abs(p1.z - p2.z) < z_eps;
  156. }
  157. bool IsPositionWithinSimpleCylinder(const glm::vec3 &p1, const glm::vec3 &cylinder_center, float cylinder_radius, float cylinder_height)
  158. {
  159. //If we're outside the height of cylinder then we're not in it (duh)
  160. auto d = std::abs(p1.z - cylinder_center.z);
  161. if (d > cylinder_height / 2.0) {
  162. return false;
  163. }
  164. glm::vec2 p1d(p1.x, p1.y);
  165. glm::vec2 ccd(cylinder_center.x, cylinder_center.y);
  166. //If we're outside the radius of the cylinder then we're not in it (also duh)
  167. d = Distance(p1d, ccd);
  168. if (d > cylinder_radius) {
  169. return false;
  170. }
  171. return true;
  172. }
  173. bool IsPositionWithinSimpleCylinder(const glm::vec4 &p1, const glm::vec4 &cylinder_center, float cylinder_radius, float cylinder_height)
  174. {
  175. //If we're outside the height of cylinder then we're not in it (duh)
  176. auto d = std::abs(p1.z - cylinder_center.z);
  177. if (d > cylinder_height / 2.0) {
  178. return false;
  179. }
  180. glm::vec2 p1d(p1.x, p1.y);
  181. glm::vec2 ccd(cylinder_center.x, cylinder_center.y);
  182. //If we're outside the radius of the cylinder then we're not in it (also duh)
  183. d = Distance(p1d, ccd);
  184. if (d > cylinder_radius) {
  185. return false;
  186. }
  187. return true;
  188. }
  189. float CalculateHeadingAngleBetweenPositions(float x1, float y1, float x2, float y2)
  190. {
  191. float y_diff = std::abs(y1 - y2);
  192. float x_diff = std::abs(x1 - x2);
  193. if (y_diff < 0.0000009999999974752427)
  194. y_diff = 0.0000009999999974752427;
  195. float angle = atan2(x_diff, y_diff) * 180.0f * 0.3183099014828645f; // angle, nice "pi"
  196. // return the right thing based on relative quadrant
  197. // I'm sure this could be improved for readability, but whatever
  198. if (y1 >= y2) {
  199. if (x2 >= x1)
  200. return (90.0f - angle + 90.0f) * 511.5f * 0.0027777778f;
  201. if (x2 <= x1)
  202. return (angle + 180.0f) * 511.5f * 0.0027777778f;
  203. }
  204. if (y1 > y2 || x2 > x1)
  205. return angle * 511.5f * 0.0027777778f;
  206. else
  207. return (90.0f - angle + 270.0f) * 511.5f * 0.0027777778f;
  208. }