src/math/geometry.cpp

   1 #include "geometry.hpp"
   2
   3
   4 namespace blobs {
   5 namespace math {
   6
   7 bool Intersect(
   8         const AABB &a_box,
   9         const glm::dmat4 &a_m,
  10         const AABB &b_box,
  11         const glm::dmat4 &b_m,
  12         glm::dvec3 &normal,
  13         double &depth
  14 ) noexcept {
  15         glm::dvec3 a_corners[8] = {
  16                 glm::dvec3(a_m * glm::dvec4(a_box.min.x, a_box.min.y, a_box.min.z, 1.0)),
  17                 glm::dvec3(a_m * glm::dvec4(a_box.min.x, a_box.min.y, a_box.max.z, 1.0)),
  18                 glm::dvec3(a_m * glm::dvec4(a_box.min.x, a_box.max.y, a_box.min.z, 1.0)),
  19                 glm::dvec3(a_m * glm::dvec4(a_box.min.x, a_box.max.y, a_box.max.z, 1.0)),
  20                 glm::dvec3(a_m * glm::dvec4(a_box.max.x, a_box.min.y, a_box.min.z, 1.0)),
  21                 glm::dvec3(a_m * glm::dvec4(a_box.max.x, a_box.min.y, a_box.max.z, 1.0)),
  22                 glm::dvec3(a_m * glm::dvec4(a_box.max.x, a_box.max.y, a_box.min.z, 1.0)),
  23                 glm::dvec3(a_m * glm::dvec4(a_box.max.x, a_box.max.y, a_box.max.z, 1.0)),
  24         };
  25
  26         glm::dvec3 b_corners[8] = {
  27                 glm::dvec3(b_m * glm::dvec4(b_box.min.x, b_box.min.y, b_box.min.z, 1.0)),
  28                 glm::dvec3(b_m * glm::dvec4(b_box.min.x, b_box.min.y, b_box.max.z, 1.0)),
  29                 glm::dvec3(b_m * glm::dvec4(b_box.min.x, b_box.max.y, b_box.min.z, 1.0)),
  30                 glm::dvec3(b_m * glm::dvec4(b_box.min.x, b_box.max.y, b_box.max.z, 1.0)),
  31                 glm::dvec3(b_m * glm::dvec4(b_box.max.x, b_box.min.y, b_box.min.z, 1.0)),
  32                 glm::dvec3(b_m * glm::dvec4(b_box.max.x, b_box.min.y, b_box.max.z, 1.0)),
  33                 glm::dvec3(b_m * glm::dvec4(b_box.max.x, b_box.max.y, b_box.min.z, 1.0)),
  34                 glm::dvec3(b_m * glm::dvec4(b_box.max.x, b_box.max.y, b_box.max.z, 1.0)),
  35         };
  36
  37         glm::dvec3 axes[15] = {
  38                 glm::dvec3(a_m[0]),
  39                 glm::dvec3(a_m[1]),
  40                 glm::dvec3(a_m[2]),
  41                 glm::dvec3(b_m[0]),
  42                 glm::dvec3(b_m[1]),
  43                 glm::dvec3(b_m[2]),
  44                 normalize(cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[0]))),
  45                 normalize(cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[1]))),
  46                 normalize(cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[2]))),
  47                 normalize(cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[0]))),
  48                 normalize(cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[1]))),
  49                 normalize(cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[2]))),
  50                 normalize(cross(glm::dvec3(a_m[2]), glm::dvec3(b_m[0]))),
  51                 normalize(cross(glm::dvec3(a_m[2]), glm::dvec3(b_m[1]))),
  52                 normalize(cross(glm::dvec3(a_m[2]), glm::dvec3(b_m[2]))),
  53         };
  54
  55         depth = std::numeric_limits<double>::infinity();
  56         int min_axis = 0;
  57
  58         int cur_axis = 0;
  59         for (const glm::dvec3 &axis : axes) {
  60                 if (any(isnan(axis))) {
  61                         // can result from the cross products if A and B have parallel axes
  62                         ++cur_axis;
  63                         continue;
  64                 }
  65                 double a_min = std::numeric_limits<double>::infinity();
  66                 double a_max = -std::numeric_limits<double>::infinity();
  67                 for (const glm::dvec3 &corner : a_corners) {
  68                         double val = dot(corner, axis);
  69                         a_min = std::min(a_min, val);
  70                         a_max = std::max(a_max, val);
  71                 }
  72
  73                 double b_min = std::numeric_limits<double>::infinity();
  74                 double b_max = -std::numeric_limits<double>::infinity();
  75                 for (const glm::dvec3 &corner : b_corners) {
  76                         double val = dot(corner, axis);
  77                         b_min = std::min(b_min, val);
  78                         b_max = std::max(b_max, val);
  79                 }
  80
  81                 if (a_max < b_min || b_max < a_min) return false;
  82
  83                 double overlap = std::min(a_max, b_max) - std::max(a_min, b_min);
  84                 if (overlap < depth) {
  85                         depth = overlap;
  86                         min_axis = cur_axis;
  87                 }
  88
  89                 ++cur_axis;
  90         }
  91
  92         normal = axes[min_axis];
  93         return true;
  94 }
  95
  96
  97 }
  98 }