cpp-game-engine/math/src/common.cpp

//
//  common.cpp
//  math
//
//  Created by Sam Jaffe on 8/20/16.
//

#include "game/math/common.hpp"

#include <vector>

#include "game/math/angle.hpp"
#include "game/math/compare.hpp"
#include "game/math/shape.hpp"

namespace math {
  vec2 rotate(vec2 const & c, vec2 const & p, radian r) {
    vec2 trans = p - c;
    vec2 vcos = trans * static_cast<float>(cos(r));
    vec2 vsin = trans * static_cast<float>(sin(r));
    return {{
      vcos[0] - vsin[1] + c[0],
      vsin[0] - vcos[1] + c[1]
    }};
  }
  
  dim2::quad rotate(vec2 const & c, dim2::quad const & q, radian r) {
    return {
      rotate(c, q.ll, r),
      rotate(c, q.lr, r),
      rotate(c, q.ur, r),
      rotate(c, q.ul, r)
    };
  }
  
  enum orientation {
    colinear = 0, clockwise = 1, anticlockwise = 2
  };
  
  orientation orient(dim2::point const & p, dim2::point const & q,
                     dim2::point const & r) {
    float val = (q[1] - p[1]) * (r[0] - q[0]) - (q[0] - p[0]) * (r[1] - q[1]);
    
    if (val == 0) return colinear;
    return (val > 0) ? clockwise : anticlockwise;
  }
  
  bool contains(dim2::line const & ln, dim2::point const & pt) {
    auto xs = std::minmax(ln.first[0], ln.second[0], lessinf());
    auto ys = std::minmax(ln.first[1], ln.second[1], lessinf());
    return orient(ln.first, ln.second, pt) == colinear &&
        between(pt[0], xs.first, xs.second) &&
        between(pt[1], ys.first, ys.second);
  }
  
  bool contains(dim2::circle const & shape, dim2::point const & pt) {
    vec2 const delta = pt - shape.center;
    return delta.dot(delta) <= std::pow(shape.radius, 2);
  }
  
  static dim2::line ray_x(dim2::point const & pt, dim2::line const & l) {
    auto x_inf = std::max({l.first[0], l.second[0], pt[0]});
    return {pt, {{x_inf, pt[1]}}};
  }
  
  static bool contains(std::vector<dim2::line> const & segments,
                       dim2::point const & pt) {
    int hits = 0;
    for (auto l : segments) {
      if (contains(l, pt)) return true;
      else if (intersects(l, ray_x(pt, l))) ++hits;
    }
    return (hits & 1) == 1;
  }
  
  bool contains(dim2::quad const & shape, dim2::point const & pt) {
    return contains(shapes::segments(shape), pt);
  }
  
  bool intersects(dim2::line const & lhs, dim2::line const & rhs) {
    // Find the four orientations needed for general and
    // special cases
    orientation o1 = orient(lhs.first, lhs.second, rhs.first);
    orientation o2 = orient(lhs.first, lhs.second, rhs.second);
    orientation o3 = orient(rhs.first, rhs.second, lhs.first);
    orientation o4 = orient(rhs.first, rhs.second, lhs.second);
    
    // General case
    if (o1 != o2 && o3 != o4)
      return true;
    
    // Special Cases
    // p1, q1 and p2 are colinear and p2 lies on segment p1q1
    if (o1 == colinear && contains(lhs, rhs.second)) { return true; }
    // p1, q1 and q2 are colinear and q2 lies on segment p1q1
    if (o2 == colinear && contains(lhs, rhs.second)) { return true; }
    // p2, q2 and p1 are colinear and p1 lies on segment p2q2
    if (o3 == colinear && contains(rhs, lhs.first)) { return true; }
    // p2, q2 and q1 are colinear and q1 lies on segment p2q2
    if (o4 == colinear && contains(rhs, lhs.second)) { return true; }
    
    return false; // Doesn't fall in any of the above cases
  }
  
  bool intersects(dim2::line const & lhs, dim2::circle const & rhs) {
    dim2::line const orth = lines::orthogonal(lhs, rhs.center);
    vec2 const delta = orth.second - orth.first;
    return delta.dot(delta) <= std::pow(rhs.radius, 2);
  }
  
  bool intersects(dim2::line const & lhs, dim2::quad const & rhs) {
    std::vector<dim2::line> segments = shapes::segments(rhs);
    auto lhs_intersects = [&lhs](dim2::line const & ln) {
      return intersects(lhs, ln);
    };
    return std::any_of(segments.begin(), segments.end(), lhs_intersects) ||
        contains(segments, lhs.first) ||
        contains(segments, lhs.second);
  }
  
  bool intersects(dim2::quad const & lhs, dim2::circle const & rhs) {
    std::vector<dim2::line> segments = shapes::segments(lhs);
    auto rhs_intersects = [&rhs](dim2::line const & ln) {
      return intersects(ln, rhs);
    };
    return std::any_of(segments.begin(), segments.end(), rhs_intersects) ||
        contains(lhs, rhs.center);
  }
  
  bool intersects(dim2::quad const & lhs, dim2::quad const & rhs);
  
  bool intersects(dim2::circle const & lhs, dim2::circle const & rhs) {
    vec2 const delta = rhs.center - lhs.center;
    return delta.dot(delta) <= std::pow(lhs.radius + rhs.radius, 2);
  }
}