CARLA
 
载入中...
搜索中...
未找到
Rotation.h
浏览该文件的文档.
1// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2// de Barcelona (UAB).
3//
4// This work is licensed under the terms of the MIT license.
5// For a copy, see <https://opensource.org/licenses/MIT>.
6
7#pragma once
8
9// 引入Carla的消息打包相关头文件
10#include "carla/MsgPack.h"
11// 引入Carla的几何数学相关头文件
12#include "carla/geom/Math.h"
13// 引入Carla的三维向量相关头文件
14#include "carla/geom/Vector3D.h"
15
16// 如果是从UE4中包含此文件(定义了相应宏),则引入UE4相关的宏启用和禁用头文件以及UE4的旋转器相关头文件
17#ifdef LIBCARLA_INCLUDED_FROM_UE4 // 判断是否从UE4编译该文件
18#include <compiler/enable-ue4-macros.h> // 启用UE4相关的宏
19#include "Math/Rotator.h" // 引入UE4的旋转器类(FRotator)
20#include <compiler/disable-ue4-macros.h> // 禁用UE4相关的宏
21#endif // LIBCARLA_INCLUDED_FROM_UE4 // 结束条件编译
22
23namespace carla {
24namespace geom {
25
26 // 旋转类,用于表示物体的旋转状态,包含俯仰角(pitch)、偏航角(yaw)和翻滚角(roll)三个角度信息
27 class Rotation {
28 public:
29
30 // =========================================================================
31 // -- 公开数据成员 ----------------------------------------------------------
32 // =========================================================================
33
34 // 俯仰角,绕X轴旋转的角度,初始化为0.0f,单位通常为弧度(根据使用场景可能会有转换)
35 float pitch = 0.0f;
36
37 // 偏航角,绕Y轴旋转的角度,初始化为0.0f,单位通常为弧度(根据使用场景可能会有转换)
38 float yaw = 0.0f;
39
40 // 翻滚角,绕Z轴旋转的角度,初始化为0.0f,单位通常为弧度(根据使用场景可能会有转换)
41 float roll = 0.0f;
42
43 // 使用MsgPack定义如何对这个类的成员(pitch、yaw、roll)进行序列化和反序列化操作
45
46 // =========================================================================
47 // -- 构造函数 --------------------------------------------------------------
48 // =========================================================================
49
50 // 默认构造函数,使用默认初始化,三个角度成员都为默认值(0.0f)
51 Rotation() = default;
52
53 // 带参数的构造函数,用于根据给定的俯仰角、偏航角和翻滚角来初始化旋转对象
54 Rotation(float p, float y, float r)
55 : pitch(p),
56 yaw(y),
57 roll(r) {}
58
59 // =========================================================================
60 // -- 其他方法 --------------------------------------------------------------
61 // =========================================================================
62
63 // 获取代表物体“前向”方向的向量,通过调用Math类中的相关函数,基于当前旋转状态来计算得到
65 return Math::GetForwardVector(*this);
66 }
67
68 // 获取代表物体“右向”方向的向量,通过调用Math类中的相关函数,基于当前旋转状态来计算得到
70 return Math::GetRightVector(*this);
71 }
72
73 // 获取代表物体“上向”方向的向量,通过调用Math类中的相关函数,基于当前旋转状态来计算得到
75 return Math::GetUpVector(*this);
76 }
77
78 // 对输入的三维向量进行旋转操作,按照先绕X轴(roll)、再绕Y轴(pitch)、最后绕Z轴(yaw)的顺序进行旋转变换
79 // 具体的旋转计算是通过三角函数(正弦、余弦)以及相应的矩阵乘法规则来实现的
80 void RotateVector(Vector3D &in_point) const {
81 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续旋转计算(偏航角相关)
82 const float cy = std::cos(Math::ToRadians(yaw));
83 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续旋转计算(偏航角相关)
84 const float sy = std::sin(Math::ToRadians(yaw));
85 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续旋转计算(翻滚角相关)
86 const float cr = std::cos(Math::ToRadians(roll));
87 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续旋转计算(翻滚角相关)
88 const float sr = std::sin(Math::ToRadians(roll));
89 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续旋转计算(俯仰角相关)
90 const float cp = std::cos(Math::ToRadians(pitch));
91 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续旋转计算(俯仰角相关)
92 const float sp = std::sin(Math::ToRadians(pitch));
93
94 Vector3D out_point;
95 // 根据旋转矩阵乘法规则计算旋转后向量的x坐标分量
96 out_point.x =
97 in_point.x * (cp * cy) +
98 in_point.y * (cy * sp * sr - sy * cr) +
99 in_point.z * (-cy * sp * cr - sy * sr);
100
101 // 根据旋转矩阵乘法规则计算旋转后向量的y坐标分量
102 out_point.y =
103 in_point.x * (cp * sy) +
104 in_point.y * (sy * sp * sr + cy * cr) +
105 in_point.z * (-sy * sp * cr + cy * sr);
106
107 // 根据旋转矩阵乘法规则计算旋转后向量的z坐标分量
108 out_point.z =
109 in_point.x * (sp) +
110 in_point.y * (-cp * sr) +
111 in_point.z * (cp * cr);
112
113 in_point = out_point;
114 }
115
116 // 对输入的三维向量进行旋转操作,返回旋转后的新向量(内部调用了上面的RotateVector方法,先复制输入向量再进行旋转)
117 Vector3D RotateVector(const Vector3D& in_point) const {
118 Vector3D out_point = in_point;
119 RotateVector(out_point);
120 return out_point;
121 }
122
123 // 对输入的三维向量进行逆旋转操作,应用RotateVector函数中使用的旋转矩阵的转置来实现逆旋转效果
124 void InverseRotateVector(Vector3D &in_point) const {
125 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续逆旋转计算(偏航角相关)
126 const float cy = std::cos(Math::ToRadians(yaw));
127 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续逆旋转计算(偏航角相关)
128 const float sy = std::sin(Math::ToRadians(yaw));
129 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续逆旋转计算(翻滚角相关)
130 const float cr = std::cos(Math::ToRadians(roll));
131 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续逆旋转计算(翻滚角相关)
132 const float sr = std::sin(Math::ToRadians(roll));
133 // 将角度从度数转换为弧度后获取对应的余弦值,用于后续逆旋转计算(俯仰角相关)
134 const float cp = std::cos(Math::ToRadians(pitch));
135 // 将角度从度数转换为弧度后获取对应的正弦值,用于后续逆旋转计算(俯仰角相关)
136 const float sp = std::sin(Math::ToRadians(pitch));
137
138 Vector3D out_point;
139 // 根据逆旋转矩阵乘法规则计算逆旋转后向量的x坐标分量
140 out_point.x =
141 in_point.x * (cp * cy) +
142 in_point.y * (cp * sy) +
143 in_point.z * (sp);
144
145 // 根据逆旋转矩阵乘法规则计算逆旋转后向量的y坐标分量
146 out_point.y =
147 in_point.x * (cy * sp * sr - sy * cr) +
148 in_point.y * (sy * sp * sr + cy * cr) +
149 in_point.z * (-cp * sr);
150
151 // 根据逆旋转矩阵乘法规则计算逆旋转后向量的z坐标分量
152 out_point.z =
153 in_point.x * (-cy * sp * cr - sy * sr) +
154 in_point.y * (-sy * sp * cr + cy * sr) +
155 in_point.z * (cp * cr);
156
157 in_point = out_point;
158 }
159
160 // =========================================================================
161 // -- 比较运算符 ------------------------------------------------------------
162 // =========================================================================
163
164 // 重载相等运算符,用于比较两个Rotation对象是否相等,即三个角度成员(pitch、yaw、roll)是否都相等
165 bool operator==(const Rotation &rhs) const {
166 return (pitch == rhs.pitch) && (yaw == rhs.yaw) && (roll == rhs.roll);
167 }
168
169 // 重载不等运算符,通过对相等运算符取反来判断两个Rotation对象是否不相等
170 bool operator!=(const Rotation &rhs) const {
171 return!(*this == rhs);
172 }
173
174 // =========================================================================
175 // -- 转换为 UE4 类型 -------------------------------------------------------
176 // =========================================================================
177
178#ifdef LIBCARLA_INCLUDED_FROM_UE4
179 // 从UE4的FRotator类型构造Rotation对象,将FRotator的三个角度成员赋值给当前Rotation对象的对应角度成员
180 Rotation(const FRotator &rotator)
181 : Rotation(rotator.Pitch, rotator.Yaw, rotator.Roll) {}
182
183 // 类型转换运算符重载,将Rotation对象转换为UE4的FRotator类型,直接返回一个由当前Rotation对象的三个角度成员构造的FRotator对象
184 operator FRotator() const {
185 return FRotator{ pitch, yaw, roll };
186 }
187#endif // LIBCARLA_INCLUDED_FROM_UE4
188 };
189
190} // namespace geom
191} // namespace carla
static Vector3D GetRightVector(const Rotation &rotation)
计算指向 rotation 的 Y 轴的单位向量
Definition Math.cpp:158
static constexpr T ToRadians(T deg)
Definition Math.h:47
static Vector3D GetUpVector(const Rotation &rotation)
计算指向 rotation 的 Z 轴的单位向量
Definition Math.cpp:173
static Vector3D GetForwardVector(const Rotation &rotation)
计算指向 rotation 的 X 轴的单位向量
Definition Math.cpp:147
Vector3D RotateVector(const Vector3D &in_point) const
Definition Rotation.h:117
Rotation(const FRotator &rotator)
Definition Rotation.h:180
MSGPACK_DEFINE_ARRAY(pitch, yaw, roll)
bool operator==(const Rotation &rhs) const
Definition Rotation.h:165
Vector3D GetUpVector() const
Definition Rotation.h:74
Vector3D GetRightVector() const
Definition Rotation.h:69
void InverseRotateVector(Vector3D &in_point) const
Definition Rotation.h:124
bool operator!=(const Rotation &rhs) const
Definition Rotation.h:170
void RotateVector(Vector3D &in_point) const
Definition Rotation.h:80
Vector3D GetForwardVector() const
Definition Rotation.h:64
Rotation(float p, float y, float r)
Definition Rotation.h:54
Carlaе·عܵռ
CARLA模拟器的主命名空间。
Definition Carla.cpp:139