判断同一坐标系下物体方向

前言

最近在写个小需求，需要判断障碍物在自车前后左右哪个方向，然后再进行下一步的逻辑处理。当然有很多种方法，这里就做个记录。

思路

画了张图，假设A点为自车，需要判断B点在A点的哪个方向（注意我这里前向是x）。

有以下公式：

所以，为B点在A点前方，为B点在A点左方。

代码如下：

#include <cmath>
#include <cstdio>
#include <iostream>
#include <vector>

using namespace std;

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) {}

  //! rotate the vector itself by angle.
  void SelfRotate(const double angle) {
    double tmp_x = x_;
    x_ = x_ * cos(angle) - y_ * sin(angle);
    y_ = tmp_x * sin(angle) + y_ * cos(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; }

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

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

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

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

int main() {
  double front_edge_to_center = 3.89;
  double back_edge_to_center = 1.043;

  vector<double> track_pos = {5974.055909431665, 219.0300190522155, 0};
  vector<double> ego_pos = {5921.028659216853, 219.4455434876256, 0};
  double ego_yaw = 3.139378753137839;

  Vec2d inner_u(track_pos[0] - ego_pos[0], track_pos[1] - ego_pos[1]);
  Vec2d inner_v = Vec2d::CreateUnitVec2d(ego_yaw);
  double inner_value = inner_u.InnerProd(inner_v);
  if(inner_value > 0) {
    std::cout << "track is on the front" << std::endl;
  } else {
    std::cout << "track is on the back" << std::endl;
  }

  Vec2d cross_u(track_pos[0] - ego_pos[0], track_pos[1] - ego_pos[1]);
  Vec2d cross_v = Vec2d::CreateUnitVec2d(ego_yaw);
  double cross_value = cross_u.CrossProd(cross_v);
  if(cross_value > 0) {
    std::cout << "track is on the right" << std::endl;
  } else {
    std::cout << "track is on the left" << std::endl;
  }
}