分离轴定理应对凸多边形的碰撞检测

分离轴定理

AABB

void PerceptionVlb::Test() {
    MLOG(relative_map, INFO) << "jianglu test";
    haomo::hidelivery::math::Vec2d a1(0.0, 3.0);
    haomo::hidelivery::math::Vec2d a2(4.0, 3.0);
    haomo::hidelivery::math::Vec2d a3(4.0, 1.0);
    haomo::hidelivery::math::Vec2d a4(0.0, 1.0);
    haomo::hidelivery::math::Vec2d b1(2.0, 2.0);
    haomo::hidelivery::math::Vec2d b2(3.0, 3.0);
    haomo::hidelivery::math::Vec2d b3(4.0, 2.0);
    haomo::hidelivery::math::Vec2d b4(3.0, 1.0);

    // 1. 构建向量
    std::vector<haomo::hidelivery::math::Vec2d> vec_a;
    std::vector<haomo::hidelivery::math::Vec2d> vec_b;

    haomo::hidelivery::math::Vec2d vec_a1(a2 - a1);
    haomo::hidelivery::math::Vec2d vec_a2(a3 - a2);
    haomo::hidelivery::math::Vec2d vec_a3(a4 - a3);
    haomo::hidelivery::math::Vec2d vec_a4(a1 - a4);
    vec_a.push_back(vec_a1);
    vec_a.push_back(vec_a2);
    vec_a.push_back(vec_a3);
    vec_a.push_back(vec_a4);
    haomo::hidelivery::math::Vec2d vec_b1(b2 - b1);
    haomo::hidelivery::math::Vec2d vec_b2(b3 - b2);
    haomo::hidelivery::math::Vec2d vec_b3(b4 - b3);
    haomo::hidelivery::math::Vec2d vec_b4(b1 - b4);
    vec_b.push_back(vec_b1);
    vec_b.push_back(vec_b2);
    vec_b.push_back(vec_b3);
    vec_b.push_back(vec_b4);

    std::vector<haomo::hidelivery::math::Vec2d> vec;
    vec.assign(vec_a.begin(), vec_a.end());
    vec.assign(vec_b.begin(), vec_b.end());

    std::vector<haomo::hidelivery::math::Vec2d> a;
    a.push_back(a1);
    a.push_back(a2);
    a.push_back(a3);
    a.push_back(a4);
    a.push_back(a1);
    std::vector<haomo::hidelivery::math::Vec2d> b;
    b.push_back(b1);
    b.push_back(b2);
    b.push_back(b3);
    b.push_back(b4);
    b.push_back(b1);

    // 2. 循环
    for (auto& v: vec) {
        std::remove_reference<decltype(v)>::type vertical_v{v.y(), -v.x()};
        double min_a = std::numeric_limits<double>::max();
        double max_a = std::numeric_limits<double>::min();
        double min_b = std::numeric_limits<double>::max();
        double max_b = std::numeric_limits<double>::min();
        for (size_t i = 0; i < a.size() -1; ++i) {
            haomo::hidelivery::math::Vec2d vec_a(a.at(i+1) - a.at(i));
            double val = a.at(i).x() + vertical_v.InnerProd(vec_a) / vertical_v.Length();
            if (min_a > val) {
                min_a = val;
            }
            if (max_a < val) {
                max_a = val;
            }
        }
        for (size_t i = 0; i < b.size() -1; ++i) {
            haomo::hidelivery::math::Vec2d vec_b(b.at(i+1) - b.at(i));
            double val = b.at(i).x() + vertical_v.InnerProd(vec_b) / vertical_v.Length();
            if (min_b > val) {
                min_b = val;
            }
            if (max_b < val) {
                max_b = val;
            }
        }
        // 
        MLOG(relative_map, INFO) << "min_a=" << min_a << ", max_a=" << max_a;
        MLOG(relative_map, INFO) << "min_b=" << min_b << ", max_b=" << max_b;
        if ((max_a < min_b) || (min_a > max_b)) {
            MLOG(relative_map, INFO) << "no collision, return false";
            // return false;
            break;
        }
    }
    MLOG(relative_map, INFO) << "collision, return true";
    // return true;
}

向量点乘叉乘如下

/**
 * @class Vec2d
 *
 * @brief Implements a class of 2-dimensional vectors.
 */
class Vec2d {
 public:
  //! Constructor which takes x- and y-coordinates.
  constexpr Vec2d(const double x, const double y) noexcept : x_(x), y_(y) {}

  //! Constructor returning the zero vector.
  constexpr Vec2d() noexcept : Vec2d(0, 0) {}

  //! Creates a unit-vector with a given angle to the positive x semi-axis
  static Vec2d CreateUnitVec2d(const double angle);

  //! Getter for x component
  double x() const { return x_; }

  //! Getter for y component
  double y() const { return y_; }

  //! Setter for x component
  void set_x(const double x) { x_ = x; }

  //! Setter for y component
  void set_y(const double y) { y_ = y; }

  //! Gets the length of the vector
  double Length() const;

  //! Gets the squared length of the vector
  double LengthSquare() const;

  //! Gets the angle between the vector and the positive x semi-axis
  double Angle() const;

  //! Returns the unit vector that is co-linear with this vector
  void Normalize();

  //! Returns the distance to the given vector
  double DistanceTo(const Vec2d &other) const;

  //! Returns the squared distance to the given vector
  double DistanceSquareTo(const Vec2d &other) const;

  //! Returns the "cross" product between these two Vec2d (non-standard).
  double CrossProd(const Vec2d &other) const;

  //! Returns the inner product between these two Vec2d.
  double InnerProd(const Vec2d &other) const;

  //! rotate the vector by angle.
  Vec2d rotate(const double angle) const;

  //! rotate the vector itself by angle.
  void SelfRotate(const double angle);

  //! Sums two Vec2d
  Vec2d operator+(const Vec2d &other) const;

  //! Subtracts two Vec2d
  Vec2d operator-(const Vec2d &other) const;

  //! Multiplies Vec2d by a scalar
  Vec2d operator*(const double ratio) const;

  //! Divides Vec2d by a scalar
  Vec2d operator/(const double ratio) const;

  //! Sums another Vec2d to the current one
  Vec2d &operator+=(const Vec2d &other);

  //! Subtracts another Vec2d to the current one
  Vec2d &operator-=(const Vec2d &other);

  //! Multiplies this Vec2d by a scalar
  Vec2d &operator*=(const double ratio);

  //! Divides this Vec2d by a scalar
  Vec2d &operator/=(const double ratio);

  //! Compares two Vec2d
  bool operator==(const Vec2d &other) const;

  //! Returns a human-readable string representing this object
  std::string DebugString() const;

 protected:
  double x_ = 0.0;
  double y_ = 0.0;
};

cpp文件如下

Vec2d Vec2d::CreateUnitVec2d(const double angle) {
  return Vec2d(std::cos(angle), std::sin(angle));
}

double Vec2d::Length() const { return std::hypot(x_, y_); }

double Vec2d::LengthSquare() const { return x_ * x_ + y_ * y_; }

double Vec2d::Angle() const { return std::atan2(y_, x_); }

void Vec2d::Normalize() {
  const double l = Length();
  if (l > kMathEpsilon) {
    x_ /= l;
    y_ /= l;
  }
}

double Vec2d::DistanceTo(const Vec2d &other) const {
  return std::hypot(x_ - other.x_, y_ - other.y_);
}

double Vec2d::DistanceSquareTo(const Vec2d &other) const {
  const double dx = x_ - other.x_;
  const double dy = y_ - other.y_;
  return dx * dx + dy * dy;
}

double Vec2d::CrossProd(const Vec2d &other) const {
  return x_ * other.y() - y_ * other.x();
}

double Vec2d::InnerProd(const Vec2d &other) const {
  return x_ * other.x() + y_ * other.y();
}

Vec2d Vec2d::rotate(const double angle) const {
  return Vec2d(x_ * cos(angle) - y_ * sin(angle),
               x_ * sin(angle) + y_ * cos(angle));
}

void Vec2d::SelfRotate(const double angle) {
  double tmp_x = x_;
  x_ = x_ * cos(angle) - y_ * sin(angle);
  y_ = tmp_x * sin(angle) + y_ * cos(angle);
}

Vec2d Vec2d::operator+(const Vec2d &other) const {
  return Vec2d(x_ + other.x(), y_ + other.y());
}

Vec2d Vec2d::operator-(const Vec2d &other) const {
  return Vec2d(x_ - other.x(), y_ - other.y());
}

Vec2d Vec2d::operator*(const double ratio) const {
  return Vec2d(x_ * ratio, y_ * ratio);
}

Vec2d Vec2d::operator/(const double ratio) const {
  //CHECK_GT(std::abs(ratio), kMathEpsilon);
  return Vec2d(x_ / ratio, y_ / ratio);
}

Vec2d &Vec2d::operator+=(const Vec2d &other) {
  x_ += other.x();
  y_ += other.y();
  return *this;
}

Vec2d &Vec2d::operator-=(const Vec2d &other) {
  x_ -= other.x();
  y_ -= other.y();
  return *this;
}

Vec2d &Vec2d::operator*=(const double ratio) {
  x_ *= ratio;
  y_ *= ratio;
  return *this;
}

Vec2d &Vec2d::operator/=(const double ratio) {
  //CHECK_GT(std::abs(ratio), kMathEpsilon);
  x_ /= ratio;
  y_ /= ratio;
  return *this;
}

bool Vec2d::operator==(const Vec2d &other) const {
  return (std::abs(x_ - other.x()) < kMathEpsilon &&
          std::abs(y_ - other.y()) < kMathEpsilon);
}

Vec2d operator*(const double ratio, const Vec2d &vec) { return vec * ratio; }

std::string Vec2d::DebugString() const {
  std::string msg = "vec2d ( x = " + std::to_string(x_) + "  y = " + std::to_string(y_) + ")";
  return msg;
}