前言
最近在写个小需求，需要判断障碍物在自车前后左右哪个方向，然后再进行下一步的逻辑处理。当然有很多种方法，这里就做个记录。
思路
画了张图，假设A点为自车，需要判断B点在A点的哪个方向（注意我这里前向是x）。
有以下公式：
所以，为B点在A点前方，为B点在A点左方。
代码如下：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
| #include <cmath>
#include <cstdio>
#include <iostream>
#include <vector>
using namespace std;
class Vec2d {
public:
constexpr Vec2d(const double x, const double y) noexcept : x_(x), y_(y) {}
constexpr Vec2d() noexcept : Vec2d(0, 0) {}
void SelfRotate(const double angle) {
double tmp_x = x_;
x_ = x_ * cos(angle) - y_ * sin(angle);
y_ = tmp_x * sin(angle) + y_ * cos(angle);
}
double x() const { return x_; }
double y() const { return y_; }
void set_x(const double x) { x_ = x; }
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;
}
}
|
