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                 glm::normalize(glm::cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[0]))),
  45                 glm::normalize(glm::cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[1]))),
  46                 glm::normalize(glm::cross(glm::dvec3(a_m[0]), glm::dvec3(b_m[2]))),
  47                 glm::normalize(glm::cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[0]))),
  48                 glm::normalize(glm::cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[1]))),
  49                 glm::normalize(glm::cross(glm::dvec3(a_m[1]), glm::dvec3(b_m[2]))),
  50                 glm::normalize(glm::cross(glm::dvec3(a_m[2]), glm::dvec3(b_m[0]))),
  51                 glm::normalize(glm::cross(glm::dvec3(a_m[2]), glm::dvec3(b_m[1]))),
  52                 glm::normalize(glm::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 (glm::any(glm::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 = glm::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 = glm::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 bool Intersect(
  98         const Ray &ray,
  99         const AABB &aabb,
 100         const glm::dmat4 &M,
 101         glm::dvec3 &normal,
 102         double &dist
 103 ) noexcept {
 104         double t_min = 0.0;
 105         double t_max = std::numeric_limits<double>::infinity();
 106         const glm::dvec3 aabb_pos(M[3].x, M[3].y, M[3].z);
 107         const glm::dvec3 delta = aabb_pos - ray.Origin();
 108
 109         glm::dvec3 t1(t_min, t_min, t_min), t2(t_max, t_max, t_max);
 110
 111         for (int i = 0; i < 3; ++i) {
 112                 const glm::dvec3 axis(M[i].x, M[i].y, M[i].z);
 113                 const double e = glm::dot(axis, delta);
 114                 const double f = glm::dot(axis, ray.Direction());
 115
 116                 if (std::abs(f) > std::numeric_limits<double>::epsilon()) {
 117                         t1[i] = (e + aabb.min[i]) / f;
 118                         t2[i] = (e + aabb.max[i]) / f;
 119
 120                         t_min = std::max(t_min, std::min(t1[i], t2[i]));
 121                         t_max = std::min(t_max, std::max(t1[i], t2[i]));
 122
 123                         if (t_max < t_min) {
 124                                 return false;
 125                         }
 126                 } else {
 127                         if (aabb.min[i] - e > 0.0 || aabb.max[i] - e < 0.0) {
 128                                 return false;
 129                         }
 130                 }
 131         }
 132
 133         dist = t_min;
 134
 135         glm::dvec3 min_all(glm::min(t1, t2));
 136         if (min_all.x > min_all.y) {
 137                 if (min_all.x > min_all.z) {
 138                         normal = glm::dvec3(t2.x < t1.x ? 1 : -1, 0, 0);
 139                 } else {
 140                         normal = glm::dvec3(0, 0, t2.z < t1.z ? 1 : -1);
 141                 }
 142         } else if (min_all.y > min_all.z) {
 143                 normal = glm::dvec3(0, t2.y < t1.y ? 1 : -1, 0);
 144         } else {
 145                 normal = glm::dvec3(0, 0, t2.z < t1.z ? 1 : -1);
 146         }
 147
 148         return true;
 149 }
 150
 151 }
 152 }