geom模块概述

carla::geom 是 CARLA 引擎中专门负责几何计算和坐标变换的模块。它提供了常用的几何基本类型（如二维/三维向量、整数向量）、位置（Location）、旋转（Rotation）和变换（Transform）等数据结构，以及相关的数学工具函数。该模块还实现了经纬度与世界坐标的转换（GeoLocation 和 Mercator 投影函数）、有向边界框（BoundingBox）计算、样条曲线（CubicPolynomial）等功能。此外，它包含网格（Mesh）和网格生成（MeshFactory）类用于道路和场景几何的构造，以及网格简化（Simplification）和基于 R 树的空间索引（PointCloudRtree、SegmentCloudRtree）等高级功能。总体而言，geom 模块负责车辆、自行车、路网等对象的几何表示及其间的空间运算与变换。

主要类与功能说明

• Vector2D/Vector3D/Vector3DInt：分别表示二维浮点向量、三维浮点向量和三维整型向量，支持算术运算（加减、点乘、叉乘等）、长度计算、归一化等常见操作。
• Location：三维位置类，继承自 Vector3D，并提供了位置之间的欧氏距离等额外功能。
• Rotation：旋转类（俯仰、偏航、横滚），可转换为对应的方向向量和旋转矩阵等。
• Transform：变换类，由位置（Location）和旋转（Rotation）构成，支持将点从本地坐标系转换到世界坐标系（以及逆变换）。
• Math：静态数学工具类，提供夹取、距离（3D/2D）、点到线段距离、向量夹角、线性插值、坐标旋转、经纬度转换（LatLonToMercator、MercatorToLatLon 等）等功能函数。
• GeoLocation：地理位置类，用于表示纬度、经度、高度，并可将世界坐标位置（Location）转换为对应的地理坐标。
• CubicPolynomial：三次多项式类，可表示形式为 a + b*x + c*x^2 + d*x^3 的函数，用于道路曲线拟合与计算。
• BoundingBox：有向边界框类，由中心位置、尺度和旋转构成，提供判断点是否在框内、计算八个顶点坐标（局部或世界空间）等方法。
• Mesh：网格容器类及其生成器。Mesh 用于存储顶点、法线、纹理坐标、索引和材质信息，可导出为 OBJ/PLY 等格式；
• Simplification：网格简化类，根据指定简化率对给定网格进行简化操作（使用第三方简化库）。
• RTree:是一种用于空间索引的树形结构,提供检索方法用于点云和线段云的快速空间查询（最近邻、相交判定等），主要使用在物体的碰撞使用。

其他模块的引用

• Carla 客户端（client）模块： 该模块大量使用几何类型来描述场景中对象的位置和姿态。例如，carla::client::World::SpawnActor 方法的接口中使用了 geom::Transform 作为参数carla.org；carla::client::TrafficLight 类中使用 geom::BoundingBox 表示灯组的遮挡盒（如 GetLightBoxes 返回多个 BoundingBox）carla.org，以及继承自 Actor 的 AddAngularImpulse(const Vector3D&) 方法使用了三维向量carla.org。这些用法说明客户端模块在创建和控制 actor（车辆、交通灯等）时依赖 geom 提供的几何表示与变换工具。

• Carla 路网（road）模块： 路网模块对道路、交叉口等结构建模时使用了边界框和几何计算。比如 carla::road::Map 在计算路口冲突、获取交叉口的 BoundingBox 列表时会用到 geom::BoundingBoxcarla.org；构建道路网格时也会涉及多边形与向量运算。此外，MapBuilder::SDFToMesh 等功能也参考了 BoundingBox 的顶点计算carla.org。

• Carla 导航（nav）模块： 该模块（包括 Navigation 和 WalkerManager）使用 geom::Vector3D 和 geom::BoundingBox 来管理交通参与者的路径和位置。例如 Navigation::AddOrUpdateVehicle、Navigation::AddWalker 等方法中频繁操作三维位置向量carla.org；同时，导航中的安全检查也会用到边界盒计算（如附近路段障碍物的边界框）。

• Carla RSS 模块： 路网安全（RSS）检查模块使用三维向量来存储车辆位置，并计算规划路线目标，比如 RssCheck::AppendRoutingTarget 中会使用 geom::Vector3Dcarla.org。

• Carla 交通管理（Traffic Manager）模块： 交通管理使用 geom 中的几何类型进行轨迹规划和位置更新。在多个阶段（如 MotionPlanStage::SafeAfterJunction、LocalizationStage::Update）中可见对 Vector3D 的调用carla.org，用于计算车辆运动和加速度等。

• 其他依赖： 还有一些功能模块也使用了 geom。例如，client::TrafficLight::GetStopWaypoints() 返回的是与交通灯相关的停车点（Waypoints），这些 Waypoint 本身也封装了 Location/Transform 等；client::TrafficLight 的触发体积（TriggerVolume）也是 geom::BoundingBox 类型carla.org。此外，路网构建和传感器处理等环节也可能间接使用几何变换方法。

下面是其主要类的代码说明文档。

Math

概述

carla::geom::Math 提供了一系列数学常量与常用数学运算函数，主要用于三维/二维向量运算、角度转换、距离计算、线性插值等。

模板常量

Pi

template <typename T>
static constexpr T Pi();

Pi2

template <typename T>
static constexpr T Pi2();

角度转换

ToDegrees

template <typename T>
static constexpr T ToDegrees(T rad);

ToRadians

template <typename T>
static constexpr T ToRadians(T deg);

范围与平方

Clamp

template <typename T>
static T Clamp(T a, T min = T(0), T max = T(1));

Square

template <typename T>
static T Square(const T &a);

向量运算

Cross

static auto Cross(const Vector3D &a, const Vector3D &b);

设$a=[x_1,y_1,z_1]$,$b=[x_2,y_2,z_2]$那么它们的叉积$a\times b$为： $$a\times b=\begin{pmatrix}y_1z_2-z_1y_2\\z_1x_2-x_1z_2\\x_1y_2-y_1x_2\end{pmatrix}$$

Dot

static auto Dot(const Vector3D &a, const Vector3D &b);

向量的点积计算公式为: $$a\cdot b=\sum_i^n a_ib_i$$

其中$n$是向量的维度。

Dot2D

static auto Dot2D(const Vector3D &a, const Vector3D &b);

距离计算

DistanceSquared

static auto DistanceSquared(const Vector3D &a, const Vector3D &b);

DistanceSquared2D

static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b);

Distance

static auto Distance(const Vector3D &a, const Vector3D &b);

Distance2D

static auto Distance2D(const Vector3D &a, const Vector3D &b);

插值

LinearLerp

static float LinearLerp(float a, float b, float f);

复杂几何运算

GetVectorAngle

static double GetVectorAngle(const Vector3D &a, const Vector3D &b);

DistanceSegmentToPoint

static std::pair<float, float> DistanceSegmentToPoint(
    const Vector3D &p,
    const Vector3D &v,
    const Vector3D &w);

设：

• 点 $p$
• 线段起点 $v$
• 线段终点 $w$

线段向量为：$\vec{d} = \vec{w} - \vec{v}$ 向量 $\vec{vp} = \vec{p} - \vec{v}$

1. 计算投影参数 $t$

该参数t 表示点 p 在 $\vec{d}$上的投影比例位置：

$$t = \frac{(\vec{p} - \vec{v}) \cdot (\vec{w} - \vec{v})}{\|\vec{w} - \vec{v}\|^2}$$

然后将 t限制在 [0,1] 范围内（因为只在线段上）：

t = Clamp(t, 0.0f, 1.0f);

2. 计算投影点坐标

$$\vec{proj} = \vec{v} + t \cdot (\vec{w} - \vec{v})$$

3. 返回结果

• 第一项：投影点到线段起点的距离（线段弧长）

$$\text{distance\_along\_segment} = t \cdot \|\vec{w} - \vec{v}\|$$

• 第二项：点 p 到投影点的欧氏距离

$$\text{distance\_to\_segment} = \|\vec{p} - \vec{proj}\|$$

DistanceArcToPoint

static std::pair<float, float> DistanceArcToPoint(
    Vector3D p,
    Vector3D start_pos,
    float length,
    float heading,
    float curvature);

1. 处理特殊情况

若曲率为 0，圆弧退化为直线段，调用 DistanceSegmentToPoint 即可：

if (std::abs(curvature) < 1e-6) return DistanceSegmentToPoint(...);

2. 圆心计算

设圆弧是逆时针方向（曲率为正），那么：

• 曲率半径 $R = \frac{1}{|curvature|}$
• 圆心在起点的垂直方向上：

$$center = start\_pos + R \cdot \begin{bmatrix} -\sin(heading) \\ \cos(heading) \end{bmatrix} \quad \text{（当 curvature > 0）}$$

3. 将点 p映射到极坐标下

• 向量 $\vec{v} = p - center$
• 距离为 $r = \|\vec{v}\|$
• 角度 $\theta_p = \arctan2(p_y - c_y, p_x - c_x)$

4. 计算圆弧起始角度 $\theta_0$

由于起始点位于弧上的起点，其角度为：

$$\theta_0 = \arctan2(start\_pos_y - c_y, start\_pos_x - c_x)$$

5. 弧长范围映射（0 到 total_arc_angle）

总的圆心角：

$$\Delta \theta = \frac{length}{R}$$

点到起点的圆心角（根据方向判断）：

$$\Delta \theta_p = \text{WrapAngle}(\theta_p - \theta_0)$$

• 如果 curvature > 0，逆时针方向；否则顺时针。

投影点是否在圆弧范围内需判断 $0 \leq \Delta \theta_p \leq \Delta \theta$

6. 计算投影点坐标 $$proj = center + R \cdot \begin{bmatrix} \cos(\theta_p) \\ \sin(\theta_p) \end{bmatrix}$$

7. 计算结果

• 沿弧长距离：

$$s = R \cdot \Delta \theta_p$$

• 欧氏距离：

$$d = \|\vec{p} - \vec{proj}\|$$

若超出圆弧范围，则与端点距离更近，返回：

Distance(p, start_pos) 或 Distance(p, arc_end)

RotatePointOnOrigin2D

static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle);

GetForwardVector

static Vector3D GetForwardVector(const Rotation &rotation);

GetRightVector

static Vector3D GetRightVector(const Rotation &rotation);

GetUpVector

static Vector3D GetUpVector(const Rotation &rotation);

范围生成

GenerateRange

static std::vector<int> GenerateRange(int a, int b);

Vector3D/Vector2D/Vector3DInt

概述

它们用于表示向量，包含坐标分量与丰富的向量运算接口，也支持与 UE4 的类型互转及 MsgPack 序列化。

Vector2D

SquaredLength

• 原型：float SquaredLength() const;
• 说明：返回向量长度的平方： $$x^2 + y^2$$
• 示例：
  

  Vector2D **<font color="#f8805a">v</font>**(3.0f,4.0f);
  float **<font color="#f8805a">sq</font>** = **<font color="#f8805a">v</font>**.SquaredLength(); // sq == 25
  

Length

• 原型：float Length() const;
• 说明：返回向量长度： $$\sqrt{x^2 + y^2}$$
• 示例：
  

  Vector2D **<font color="#f8805a">v</font>**(3.0f,4.0f);
  float **<font color="#f8805a">len</font>** = **<font color="#f8805a">v</font>**.Length(); // len == 5
  

MakeUnitVector

• 原型：Vector2D MakeUnitVector() const;
• 说明：返回归一化后的单位向量： $$\frac{1}{\|v\|}(x,y)$$
• 示例：
  

  Vector2D **<font color="#f8805a">v</font>**(0.0f,5.0f);
  Vector2D **<font color="#f8805a">u</font>** = **<font color="#f8805a">v</font>**.MakeUnitVector(); // u ≈ (0,1)
  

Vector3D

SquaredLength

• 原型：float SquaredLength() const;
• 说明：返回： $$x^2 + y^2 + z^2$$
• 示例：
  

  Vector3D **<font color="#f8805a">v3</font>**(1.0f,2.0f,2.0f);
  float **<font color="#f8805a">sq3</font>** = **<font color="#f8805a">v3</font>**.SquaredLength(); // sq3 == 9
  

Length

• 原型：float Length() const;
• 说明：返回： $$\sqrt{x^2 + y^2 + z^2}$$
• 示例：
  

  float **<font color="#f8805a">len3</font>** = **<font color="#f8805a">v3</font>**.Length(); // len3 == 3
  

SquaredLength2D / Length2D

• 原型：

• float SquaredLength2D() const; 返回 $$x^2 + y^2$$

• float Length2D() const; 返回 $$\sqrt{x^2 + y^2}$$
• 示例：
  

  Vector3D **<font color="#f8805a">vxy</font>**(3.0f,4.0f,7.0f);
  float **<font color="#f8805a">sq2d</font>** = **<font color="#f8805a">vxy</font>**.SquaredLength2D(); // 25
  float **<font color="#f8805a">len2d</font>** = **<font color="#f8805a">vxy</font>**.Length2D();        // 5
  

Abs

• 原型：Vector3D Abs() const;
• 说明：返回 $$(|x|,|y|,|z|)$$
• 示例：
  

  Vector3D **<font color="#f8805a">vneg</font>**(-1.0f,-2.0f,3.0f);
  Vector3D **<font color="#f8805a">apos</font>** = **<font color="#f8805a">vneg</font>**.Abs(); // (1,2,3)
  

MakeUnitVector

• 原型：Vector3D MakeUnitVector() const;
• 说明：返回标准归一化单位向量
• 示例：
  

  Vector3D **<font color="#f8805a">vnorm</font>**(0.0f,0.0f,5.0f);
  Vector3D **<font color="#f8805a">unorm</font>** = **<font color="#f8805a">vnorm</font>**.MakeUnitVector(); // (0,0,1)
  

MakeSafeUnitVector

• 原型：Vector3D MakeSafeUnitVector(float epsilon) const;
• 说明：长度小于 $$\epsilon$$ 时返回原向量，否则归一化
• 示例：
  

  Vector3D **<font color="#f8805a">vsafe</font>**(1e-4f,0,0);
  Vector3D **<font color="#f8805a">svec</font>** = **<font color="#f8805a">vsafe</font>**.MakeSafeUnitVector(1e-3f); // unchanged
  

Vector3DInt

SquaredLength

• 原型：int64_t SquaredLength() const;
• 说明：返回整数平方和： $$x^2 + y^2 + z^2$$
• 示例：
  

  Vector3DInt **<font color="#f8805a">vi</font>**(2,3,6);
  int64_t **<font color="#f8805a">sqint</font>** = **<font color="#f8805a">vi</font>**.SquaredLength(); // 49
  

Length

• 原型：double Length() const;
• 说明：返回： $$\sqrt{x^2 + y^2 + z^2}$$
• 示例：
  

  double **<font color="#f8805a">lint</font>** = **<font color="#f8805a">vi</font>**.Length(); // 7.0
  

UE4 互转（仅在 UE4 模式开启时）

Vector3D ToMeters() const;
Vector3D ToCentimeters() const;
FVector ToFVector() const;

Vector3D v(100,200,300);
FVector f = v.ToFVector();     // f = (100,200,300)
Vector3D m = v.ToMeters();     // m = (1,2,3)
Vector3D cm = m.ToCentimeters(); // cm = (100,200,300)

MsgPack 序列化

template <typename Packer>
void msgpack_pack(Packer& pk) const;

void msgpack_unpack(clmdep_msgpack::object const& o);

template <typename MSGPACK_OBJECT>
void msgpack_object(MSGPACK_OBJECT* o, clmdep_msgpack::zone& sneaky_variable_that_shadows_z) const;

// pack
clmdep_msgpack::sbuffer sbuf;
msgpack::pack(sbuf, Vector3D(1,2,3));
// unpack
auto obj = msgpack::unpack(sbuf.data(), sbuf.size()).get();
Vector3D v;
v.msgpack_unpack(obj);

Mesh

概述

carla::geom::Mesh 类是一个网格数据容器，提供验证、构建、导出以及与 UE4 类型互转的功能。支持顶点、法线、索引、UV 和材质分组的管理。

类型别名

• vertex_type：顶点类型，Vector3D
• normal_type：法线类型，Vector3D
• index_type：索引类型，size_t
• uv_type：纹理坐标类型，Vector2D
• material_type：材质类型，MeshMaterial

构造函数

Mesh(...)

Mesh(
  const std::vector<vertex_type> &vertices = {},
  const std::vector<normal_type> &normals = {},
  const std::vector<index_type> &indexes = {},
  const std::vector<uv_type> &uvs = {}
);

std::vector<Vector3D> verts = { {0,0,0}, {1,0,0}, {0,1,0} };
Mesh mesh(verts);

验证方法

IsValid

bool IsValid() const;

if (!mesh.IsValid()) {
  std::cerr << "Invalid mesh!" << std::endl;
}

网格构建方法

AddTriangleStrip

void AddTriangleStrip(const std::vector<vertex_type> &vertices);

mesh.AddTriangleStrip({{0,0,0},{1,0,0},{1,1,0},{0,1,0}});

AddTriangleFan

void AddTriangleFan(const std::vector<vertex_type> &vertices);

mesh.AddTriangleFan({{0,0,0},{1,0,0},{1,1,0},{0,1,0}});

AddVertex / AddVertices

void AddVertex(vertex_type vertex);
void AddVertices(const std::vector<vertex_type> &vertices);

mesh.AddVertex({0,0,1});
mesh.AddVertices({{1,0,1},{0,1,1}});

AddNormal

void AddNormal(normal_type normal);

AddIndex

void AddIndex(index_type index);

AddUV / AddUVs

void AddUV(uv_type uv);
void AddUVs(const std::vector<uv_type> &uvs);

AddMaterial / EndMaterial

void AddMaterial(const std::string &material_name);
void EndMaterial();

mesh.AddMaterial("matA");
// ... 添加三角面 ...
mesh.EndMaterial();

导出方法

GenerateOBJ

std::string GenerateOBJ() const;

GenerateOBJForRecast

std::string GenerateOBJForRecast() const;

GeneratePLY

std::string GeneratePLY() const;

访问方法

GetVertices / GetVerticesNum

const std::vector<vertex_type> &GetVertices() const;
size_t GetVerticesNum() const;

GetNormals

const std::vector<normal_type> &GetNormals() const;

GetIndexes / GetIndexesNum

const std::vector<index_type> &GetIndexes() const;
size_t GetIndexesNum() const;

GetUVs

const std::vector<uv_type> &GetUVs() const;

GetMaterials

const std::vector<material_type> &GetMaterials() const;

GetLastVertexIndex

size_t GetLastVertexIndex() const;

合并网格

ConcatMesh

Mesh& ConcatMesh(const Mesh &rhs, int num_vertices_to_link);

operator+= / operator+

Mesh& operator+=(const Mesh &rhs);
friend Mesh operator+(const Mesh &lhs, const Mesh &rhs);

UE4 类型转换

operator FProceduralCustomMesh() const

operator FProceduralCustomMesh() const;

FProceduralCustomMesh ue_mesh = mesh;

Simplification

概述

carla::geom::Simplification 类用于对 carla::geom::Mesh 网格模型执行简化操作，通过指定简化比率，将三角形数减少到原始数量的若干百分比，从而降低模型复杂度。

构造函数

Simplification()

Simplification() = default;

carla::geom::Simplification sim; // 使用默认简化率（未指定）

Simplification(float simplificationrate)

Simplification(float simplificationrate);

carla::geom::Simplification sim(0.25f); // 保留 25% 的三角形

公开数据成员

• simplification_percentage

• 存储简化比率（0–1 之间的浮点数）

• 示例：

  float rate = sim.simplification_percentage; // 获取当前简化比率
  

方法

Simplificate

void Simplificate(const std::unique_ptr<geom::Mesh>& pmesh);

• pmesh：指向待简化网格的智能指针引用
• 示例：

  #include "carla/geom/Simplification.h"
  #include "carla/geom/Mesh.h"
  
  // 假设已创建或加载了一个 Mesh 对象
  std::unique_ptr<carla::geom::Mesh> mesh = LoadYourMesh();
  carla::geom::Simplification sim(0.5f);    // 保留 50% 三角形
  sim.Simplificate(mesh);                   // 执行简化
  // 之后 mesh->GetVertices() 和 mesh->GetIndexes() 已被更新为简化后的数据
  

RTree

简介

R-树（R-Tree）是一种用于空间索引的树形结构，通过将空间中相近对象聚合成最小外接矩形（MBR）构建索引，广泛应用于点云、线段云等三维几何对象的快速邻近查询与碰撞检测。

RTree包含两个模板类： - PointCloudRtree<T>：支持对点云的插入、k-NN 查询、带过滤器查询； - SegmentCloudRtree<T>：支持对线段云的插入、k-NN 查询、交集查询。

欧几里得距离公式

对任意两个 $n$ 维点 $\mathbf{p}=(p_1,p_2,\dots,p_n)$ 和 $\mathbf{q}=(q_1,q_2,\dots,q_n)$，欧几里得距离定义如下：

$$d(\mathbf{p}, \mathbf{q}) = \sqrt{ \sum_{i=1}^n (p_i - q_i)^2 }$$

类定义：PointCloudRtree

template <typename T, size_t Dimension = 3>
class PointCloudRtree;

• T：元素类型；
• Dimension：空间维度，默认值为 3。

成员类型

using BPoint = boost::geometry::model::point<float, Dimension>;
using TreeElement = std::pair<BPoint, T>;

方法说明

InsertElement

void InsertElement(const BPoint &point, const T &element);
void InsertElement(const TreeElement &element);

• 插入一个点或点-元素对到 R 树中。

InsertElements

void InsertElements(const std::vector<TreeElement> &elements);

• 批量插入元素。

GetNearestNeighbours

std::vector<TreeElement> GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const;

• 返回距离给定点最近的 $k$ 个邻居（默认 $k=1$）。

GetNearestNeighboursWithFilter

template <typename Filter>
std::vector<TreeElement> GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, size_t number_neighbours = 1) const;

• 在返回最近邻的基础上，添加用户自定义过滤条件。

GetTreeSize

size_t GetTreeSize() const;

• 返回当前树中包含的元素数量。

类定义：SegmentCloudRtree

template <typename T, size_t Dimension = 3>
class SegmentCloudRtree;

成员类型

using BPoint = boost::geometry::model::point<float, Dimension>;
using BSegment = boost::geometry::model::segment<BPoint>;
using TreeElement = std::pair<BSegment, std::pair<T, T>>;

方法说明

InsertElement

void InsertElement(const BSegment &segment, const T &element_start, const T &element_end);
void InsertElement(const TreeElement &element);

• 插入一条线段以及其两端关联的数据。

InsertElements

void InsertElements(const std::vector<TreeElement> &elements);

• 批量插入线段。

GetNearestNeighbours

template <typename Geometry>
std::vector<TreeElement> GetNearestNeighbours(const Geometry &geometry, size_t number_neighbours = 1) const;

• 返回与几何体最邻近的线段。

GetNearestNeighboursWithFilter

template <typename Geometry, typename Filter>
std::vector<TreeElement> GetNearestNeighboursWithFilter(const Geometry &geometry, Filter filter, size_t number_neighbours = 1) const;

• 返回最邻近的线段，且满足过滤条件。

GetIntersections

template <typename Geometry>
std::vector<TreeElement> GetIntersections(const Geometry &geometry) const;

• 查询与几何体相交的线段集合。注意：Boost Geometry 暂不支持 3D 线段与线段精确交集判断，结果可能不完全准确。

GetTreeSize

size_t GetTreeSize() const;

• 返回线段树的大小。

Location

概述

carla::geom::Location 类继承自 Vector3D，表示三维空间中的位置点，提供与 Vector3DInt 间的互转、距离计算、算术与比较运算符重载，以及与 UE4 FVector 类型的互转。

主要成员属性

构造函数

Location()

• 原型：Location() = default;
• 说明：默认构造，坐标初始化为 (0, 0, 0)。
• 示例：
  

  Location **<font color="#f8805a">loc_default</font>**; // (0,0,0)
  

Location(float x, float y, float z)

(继承自 Vector3D)
- 说明：使用基类 Vector3D 构造函数，初始化三维坐标。
- 示例：

Location **<font color="#f8805a">loc_xyz</font>**(1.0f, 2.0f, 3.0f); // (1,2,3)

Location(const Vector3D &rhs)

• 原型：Location(const Vector3D &rhs);
• 说明：从 Vector3D 对象转换而来，拷贝坐标。
• 示例：
  

  Vector3D **<font color="#f8805a">v</font>**(4.0f,5.0f,6.0f);
  Location **<font color="#f8805a">loc_from_v</font>**(**<font color="#f8805a">v</font>**); // (4,5,6)
  

Location(const Vector3DInt &rhs)

• 原型：Location(const Vector3DInt &rhs);
• 说明：从整数坐标 Vector3DInt 转换而来，按比例（int→float）赋值。
• 示例：
  

  Vector3DInt **<font color="#f8805a">vi</font>**(7,8,9);
  Location **<font color="#f8805a">loc_from_vi</font>**(**<font color="#f8805a">vi</font>**); // (7,8,9)
  

距离计算方法

DistanceSquared

• 原型:
  

  auto DistanceSquared(const Location &loc) const;
  

• 说明：返回当前对象到 loc 的三维距离平方，计算公式：
  $$d^2 = (x - x')^2 + (y - y')^2 + (z - z')^2$$
• 示例：
  

  Location **<font color="#f8805a">a</font>**(0,0,0), **<font color="#f8805a">b</font>**(1,2,2);
  auto **<font color="#f8805a">d2</font>** = **<font color="#f8805a">a</font>**.DistanceSquared(**<font color="#f8805a">b</font>**); // d2 == 9
  

Distance

• 原型:
  

  auto Distance(const Location &loc) const;
  

• 说明：返回当前对象到 loc 的欧氏距离，等于 $$\sqrt{DistanceSquared(loc)}$$ 。
• 示例：
  

  auto **<font color="#f8805a">d</font>** = **<font color="#f8805a">a</font>**.Distance(**<font color="#f8805a">b</font>**); // d == 3
  

算术运算符

operator+=

• 原型:
  

  Location &operator+=(const Location &rhs);
  

• 说明：将 rhs 坐标加到当前对象。
• 示例:
  

  Location **<font color="#f8805a">p1</font>**(1,1,1), **<font color="#f8805a">p2</font>**(2,3,4);
  **<font color="#f8805a">p1</font>** += **<font color="#f8805a">p2</font>**; // p1 = (3,4,5)
  

operator+

• 原型:
  

  friend Location operator+(Location lhs, const Location &rhs);
  

• 说明：返回两坐标相加的新 Location。
• 示例:
  

  auto **<font color="#f8805a">sum</font>** = **<font color="#f8805a">p1</font>** + **<font color="#f8805a">p2</font>**; // sum = (3,4,5)
  

operator-= / operator-

• 原型:
  

  Location &operator-=(const Location &rhs);
  friend Location operator-(Location lhs, const Location &rhs);
  

• 说明：坐标差运算。
• 示例:
  

  Location **<font color="#f8805a">diff</font>** = **<font color="#f8805a">p2</font>** - **<font color="#f8805a">p1</font>**; // ( -1, -2, -3 )
  

比较运算符

operator== / operator!=

• 原型:
  

  bool operator==(const Location &rhs) const;
  bool operator!=(const Location &rhs) const;
  

• 说明：逐分量比较相等或不等。
• 示例:
  

  bool **<font color="#f8805a">eq</font>** = ( **<font color="#f8805a">p1</font>** == **<font color="#f8805a">p2</font>** );
  bool **<font color="#f8805a">ne</font>** = ( **<font color="#f8805a">p1</font>** != **<font color="#f8805a">p2</font>** );
  

UE4 类型互转 (仅在 LIBCARLA_INCLUDED_FROM_UE4 定义时)

Location(const FVector &vector)

• 说明：从 UE4 FVector（厘米）转换到米坐标。
• 示例:
  

  FVector **<font color="#f8805a">fv</font>**{100,200,300};
  Location **<font color="#f8805a">loc_ue</font>**(**<font color="#f8805a">fv</font>**); // (1,2,3)
  

operator FVector()

• 说明：将 Location（米）转换到 UE4 FVector（厘米）。
• 示例:
  

  FVector **<font color="#f8805a">fv2</font>** = **<font color="#f8805a">loc_xyz</font>**; // (100,200,300)
  

Rotation

概述

carla::geom::Rotation 用于表示三维空间中的旋转状态，包含俯仰（pitch）、偏航（yaw）、翻滚（roll）三个角度。提供方向向量计算、向量旋转/逆旋转，以及与 UE4 FRotator 的互转。

公开数据成员

• pitch：绕 X 轴的旋转角度，默认 0.0f
• yaw：绕 Y 轴的旋转角度，默认 0.0f
• roll：绕 Z 轴的旋转角度，默认 0.0f

构造函数

Rotation()

• 原型：Rotation() = default;
• 说明：默认构造，三角度均为 0。
• 示例：
  

  Rotation **<font color="#f8805a">r0</font>**; // pitch=0,yaw=0,roll=0
  

Rotation(float p, float y, float r)

• 原型:
  

  Rotation(float p, float y, float r);
  

• 参数
• p：俯仰角（pitch）
• y：偏航角（yaw）
• r：翻滚角（roll）
• 示例:
  

  Rotation **<font color="#f8805a">r1</font>**(10.0f, 20.0f, 30.0f);
  

方向向量方法

GetForwardVector

• 原型:
  

  Vector3D GetForwardVector() const;
  

• 说明：返回物体前方单位向量，计算公式：
  $$\begin{aligned} x &= \cos(\mathrm{yaw})\cos(\mathrm{pitch}),\\ y &= \sin(\mathrm{yaw})\cos(\mathrm{pitch}),\\ z &= \sin(\mathrm{pitch}). \end{aligned}$$
• 示例:
  

  Rotation r(0,90,0);
  Vector3D **<font color="#f8805a">fwd</font>** = r.GetForwardVector(); // ≈ (0,1,0)
  

GetRightVector

• 原型:
  

  Vector3D GetRightVector() const;
  

• 说明：返回物体右侧单位向量，具体计算见 Math::GetRightVector。
• 示例:
  

  Vector3D **<font color="#f8805a">right</font>** = r.GetRightVector();
  

GetUpVector

• 原型:
  

  Vector3D GetUpVector() const;
  

• 说明：返回物体上方单位向量，具体计算见 Math::GetUpVector。
• 示例:
  

  Vector3D **<font color="#f8805a">up</font>** = r.GetUpVector();
  

向量旋转方法

RotateVector

• 原型:
  

  void RotateVector(Vector3D &in_point) const;
  Vector3D RotateVector(const Vector3D &in_point) const;
  

• 说明：
  按顺序绕 Z（roll）、X（pitch）、Y（yaw）轴旋转：
  $$\mathbf{v}' = R_{\text{yaw}} R_{\text{roll}} R_{\text{pitch}}\,\mathbf{v}.$$
• 示例:
  

  Vector3D **<font color="#f8805a">pt</font>**(1,0,0);
  r.RotateVector(**<font color="#f8805a">pt</font>**);           // 原地旋转
  auto **<font color="#f8805a">pt2</font>** = r.RotateVector(pt); // 返回旋转后拷贝
  

InverseRotateVector

• 原型:
  

  void InverseRotateVector(Vector3D &in_point) const;
  

• 说明：按上述顺序的逆矩阵（转置）对向量做逆旋转。
• 示例:
  

  Vector3D **<font color="#f8805a">p3</font>** = pt2;
  r.InverseRotateVector(**<font color="#f8805a">p3</font>**); // 恢复原向量 (1,0,0)
  

比较运算符

operator== / operator!=

• 原型:
  

  bool operator==(const Rotation &rhs) const;
  bool operator!=(const Rotation &rhs) const;
  

• 说明：逐分量比较相等或不等。
• 示例:
  

  bool **<font color="#f8805a">same</font>** = (r == Rotation(10,20,30));
  

UE4 类型互转（LIBCARLA_INCLUDED_FROM_UE4）

Rotation(const FRotator &rotator)

• 说明：从 FRotator 构造。
• 示例:
  

  FRotator fr{15,25,35};
  Rotation **<font color="#f8805a">rue</font>**(fr);
  

operator FRotator()

• 说明：转换为 UE4 FRotator。
• 示例:
  

  FRotator **<font color="#f8805a">outFR</font>** = r;
  

Transform

概述

carla::geom::Transform 表示在三维空间中的位置与旋转变换，封装了 Location 和 Rotation 两部分。提供向量变换、点变换、逆变换及 4×4 矩阵生成，并支持与 UE4 FTransform 类型互转。

公开数据成员

• location：类型 Location，表示平移分量
• rotation：类型 Rotation，表示旋转分量

构造函数

Transform()

• 原型：Transform() = default;
• 说明：默认构造，location 和 rotation 均为默认值。
• 示例：
  

  Transform **<font color="#f8805a">t0</font>**; // location=(0,0,0)、rotation=(0,0,0)
  

Transform(const Location &in_location)

• 原型：Transform(const Location &in_location);
• 说明：只设置平移，旋转保持默认。
• 示例：
  

  Location **<font color="#f8805a">loc</font>**(1,2,3);
  Transform **<font color="#f8805a">t1</font>**(**<font color="#f8805a">loc</font>**); // rotation=(0,0,0)
  

Transform(const Location &in_location, const Rotation &in_rotation)

• 原型：Transform(const Location &in_location, const Rotation &in_rotation);
• 说明：同时设置平移与旋转。
• 示例：
  

  Rotation **<font color="#f8805a">r</font>**(10,20,30);
  Transform **<font color="#f8805a">t2</font>**(**<font color="#f8805a">loc</font>**, **<font color="#f8805a">r</font>**);
  

方向向量方法

GetForwardVector

• 原型:
  

  Vector3D GetForwardVector() const;
  

• 说明：基于 rotation 计算单位“前向”向量。
• 示例:
  

  Vector3D **<font color="#f8805a">fwd</font>** = **<font color="#f8805a">t2</font>**.GetForwardVector();
  

GetRightVector

• 原型:
  

  Vector3D GetRightVector() const;
  

• 说明：基于 rotation 计算单位“右向”向量。
• 示例:
  

  Vector3D **<font color="#f8805a">right</font>** = **<font color="#f8805a">t2</font>**.GetRightVector();
  

GetUpVector

• 原型:
  

  Vector3D GetUpVector() const;
  

• 说明：基于 rotation 计算单位“上向”向量.
• 示例:
  

  Vector3D **<font color="#f8805a">up</font>** = **<font color="#f8805a">t2</font>**.GetUpVector();
  

点/向量变换方法

TransformPoint

• 原型:
  

  void TransformPoint(Vector3D &in_point) const;
  

• 说明：先绕原点旋转，再平移。
• 示例:
  

  Vector3D **<font color="#f8805a">p</font>**(1,0,0);
  **<font color="#f8805a">t2</font>**.TransformPoint(**<font color="#f8805a">p</font>**);
  

TransformVector

• 原型:
  

  void TransformVector(Vector3D &in_vector) const;
  

• 说明：仅旋转向量，不平移。
• 示例:
  

  Vector3D **<font color="#f8805a">v</font>**(0,1,0);
  **<font color="#f8805a">t2</font>**.TransformVector(**<font color="#f8805a">v</font>**);
  

InverseTransformPoint

• 原型:
  

  void InverseTransformPoint(Vector3D &in_point) const;
  

• 说明：先平移逆变换，再旋转逆变换。
• 示例:
  

  Vector3D **<font color="#f8805a">p2</font>** = **<font color="#f8805a">p</font>**;
  **<font color="#f8805a">t2</font>**.InverseTransformPoint(**<font color="#f8805a">p2</font>**);
  

矩阵生成方法

GetMatrix

• 原型:
  

  std::array<float,16> GetMatrix() const;
  

• 说明：生成变换的 4×4 齐次矩阵，结合旋转与平移：
  $$M = \begin{pmatrix} R & t\\ 0 & 1 \end{pmatrix}$$
• 示例:
  

  auto **<font color="#f8805a">m</font>** = **<font color="#f8805a">t2</font>**.GetMatrix();
  

GetInverseMatrix

• 原型:
  

  std::array<float,16> GetInverseMatrix() const;
  

• 说明：生成逆变换矩阵 $M^{-1}$。
• 示例:
  

  auto **<font color="#f8805a">mi</font>** = **<font color="#f8805a">t2</font>**.GetInverseMatrix();
  

比较运算符

operator== / operator!=

• 原型:
  

  bool operator==(const Transform &rhs) const;
  bool operator!=(const Transform &rhs) const;
  

• 说明：位置与旋转均相等时才返回 true。
• 示例:
  

  bool **<font color="#f8805a">eq</font>** = (t1 == t2);
  

UE4 类型互转（LIBCARLA_INCLUDED_FROM_UE4）

Transform(const FTransform &transform)

• 说明：从 UE4 FTransform 构造。
• 示例:
  

  FTransform **<font color="#f8805a">ut</font>**;
  Transform **<font color="#f8805a">tue</font>**(**<font color="#f8805a">ut</font>**);
  

operator FTransform()

• 说明：转换为 UE4 FTransform。
• 示例:
  

  FTransform **<font color="#f8805a">ut2</font>** = t2;
  

CubicPolynomial

概述

carla::geom::CubicPolynomial 用于描述和操作三次多项式
$$f(x)=a + b\,x + c\,x^2 + d\,x^3$$
支持偏移量修正、求值、求切线、以及多项式加法和数乘。

构造函数

CubicPolynomial()

• 原型：CubicPolynomial() = default;
• 说明：默认构造，所有系数和偏移量初始化为 0。
• 示例:
  

  CubicPolynomial **<font color="#f8805a">p0</font>**; // a=b=c=d=s=0
  

CubicPolynomial(const value_type &a, const value_type &b, const value_type &c, const value_type &d)

• 原型:
  

  CubicPolynomial(const value_type &a,
                  const value_type &b,
                  const value_type &c,
                  const value_type &d);
  

• 说明：按系数 $(a,b,c,d)$ 构造，无偏移量。
• 示例:
  

  CubicPolynomial **<font color="#f8805a">p1</font>**(1.0, 2.0, 3.0, 4.0);
  

CubicPolynomial(const value_type &a, const value_type &b, const value_type &c, const value_type &d, const value_type &s)

• 原型:
  

  CubicPolynomial(const value_type &a,
                  const value_type &b,
                  const value_type &c,
                  const value_type &d,
                  const value_type &s);
  

• 说明：按系数与偏移量 $s$ 构造，实际存储系数已做偏移校正：
  $$\tilde a = a - b s + c s^2 - d s^3,\quad \tilde b = b -2c s +3d s^2,\quad \tilde c = c-3d s,\quad \tilde d = d$$
• 示例:
  

  CubicPolynomial **<font color="#f8805a">p2</font>**(1,2,3,4,0.5);
  

访问与修改

GetA/B/C/D

• 原型:
  

  value_type GetA() const;
  value_type GetB() const;
  value_type GetC() const;
  value_type GetD() const;
  

• 说明：分别返回当前存储的系数 $\tilde a,\tilde b,\tilde c,\tilde d$。
• 示例:
  

  auto **<font color="#f8805a">a</font>** = p2.GetA();
  

GetS

• 原型:
  

  value_type GetS() const;
  

• 说明：返回偏移量 $s$。
• 示例:
  

  auto **<font color="#f8805a">s</font>** = p2.GetS(); // 0.5
  

Set(a,b,c,d[,s])

• 原型:
  

  void Set(const value_type &a,
           const value_type &b,
           const value_type &c,
           const value_type &d);
  void Set(const value_type &a,
           const value_type &b,
           const value_type &c,
           const value_type &d,
           const value_type &s);
  

• 说明：重置系数（和可选偏移量），与构造逻辑一致。
• 示例:
  

  p2.Set(0,1,0,0);      // f(x)=x
  p2.Set(1,2,3,4, 0.2); // 带偏移修正
  

多项式运算

Evaluate

• 原型:
  

  value_type Evaluate(const value_type &x) const;
  

• 说明：计算 $$f(x)=\tilde a + \tilde b\,x + \tilde c\,x^2 + \tilde d\,x^3$$
• 示例:
  

  auto **<font color="#f8805a">y</font>** = p1.Evaluate(2.0); // 1+2*2+3*4+4*8
  

Tangent

• 原型:
  

  value_type Tangent(const value_type &x) const;
  

• 说明：计算导数 $$f'(x)=\tilde b + 2\tilde c\,x + 3\tilde d\,x^2$$ 。
• 示例:
  

  auto **<font color="#f8805a">m</font>** = p1.Tangent(2.0); // 2 + 2*3*2 + 3*4*4
  

复合运算符

operator+=

• 原型:
  

  CubicPolynomial &operator+=(const CubicPolynomial &rhs);
  

• 说明：系数逐项相加。
• 示例:
  

  p1 += p2;
  

operator+

• 原型:
  

  friend CubicPolynomial operator+(CubicPolynomial lhs, const CubicPolynomial &rhs);
  

• 说明：返回相加结果。
• 示例:
  

  auto **<font color="#f8805a">sum</font>** = p1 + p2;
  

operator*=

• 原型:
  

  CubicPolynomial &operator*=(const value_type &rhs);
  

• 说明：系数逐项乘以标量。
• 示例:
  

  p1 *= 2.0;
  

operator*

• 原型:
  

  friend CubicPolynomial operator*(CubicPolynomial lhs, const value_type &rhs);
  friend CubicPolynomial operator*(const value_type &lhs, CubicPolynomial rhs);
  

• 说明：返回乘标量结果，支持标量在左或右。
• 示例:
  

  auto **<font color="#f8805a">scaled</font>** = 0.5 * p1;
  

BoundingBox

概述

carla::geom::BoundingBox 表示三维空间中的轴对齐或旋转后的边界盒，包含中心位置、半尺寸（extent）和旋转信息。提供判断点包含、获取局部/世界顶点数组，以及与 UE4 类型互转。

构造函数

BoundingBox()

• 原型：BoundingBox() = default;
• 说明：默认构造，location=(0,0,0)，extent=(0,0,0)，rotation=(0,0,0)。

• 示例：
  

  BoundingBox **<font color="#f8805a">bb0</font>**; // 默认空盒
  

• 带参构造：

explicit BoundingBox(const Location &in_location,
                     const Vector3D &in_extent,
                     const Rotation &in_rotation);

Location **<font color="#f8805a">loc</font>**(1,2,3);
Vector3D **<font color="#f8805a">ext</font>**(0.5f,0.5f,1.0f);
Rotation **<font color="#f8805a">rot</font>**(0,45,0);
BoundingBox **<font color="#f8805a">bb1</font>**(**<font color="#f8805a">loc</font>**, **<font color="#f8805a">ext</font>**, **<font color="#f8805a">rot</font>**);

• 带参构造：

explicit BoundingBox(const Location &in_location,
                     const Vector3D &in_extent);

BoundingBox **<font color="#f8805a">bb2</font>**(**<font color="#f8805a">loc</font>**, **<font color="#f8805a">ext</font>**);

• 带参构造：

explicit BoundingBox(const Vector3D &in_extent);

BoundingBox **<font color="#f8805a">bb3</font>**(**<font color="#f8805a">ext</font>**);

核心方法

Contains

• 原型：
  

  bool Contains(const Location &in_world_point,
                const Transform &in_bbox_to_world_transform) const;
  

• 说明：判断世界空间点是否在边界盒内。
• 使用 inverseTransform 将世界点变换到盒子局部空间；
• 相对于 location 归一化；
• 判断坐标是否在 ±extent 范围内。
• 示例：
  

  Location **<font color="#f8805a">pt</font>**(1.1f,2.0f,3.0f);
  Transform **<font color="#f8805a">tr</font>**(...);
  bool **<font color="#f8805a">inside</font>** = bb1.Contains(**<font color="#f8805a">pt</font>**, **<font color="#f8805a">tr</font>**);
  

GetLocalVertices

• 原型：
  

  std::array<Location,8> GetLocalVertices() const;
  

• 说明：返回考虑旋转后的 8 个顶点在局部空间的位置：
  $$\displaystyle \text{vertex}_i = \text{location} + \text{rotation.RotateVector}(\pm extent.x,\pm extent.y,\pm extent.z).$$
• 示例：
  

  auto **<font color="#f8805a">verts</font>** = bb1.GetLocalVertices();
  

GetLocalVerticesNoRotation

• 原型：
  

  std::array<Location,8> GetLocalVerticesNoRotation() const;
  

• 说明：返回不考虑旋转，仅基于 location 与 extent 的 8 个顶点。
• 示例：
  

  auto **<font color="#f8805a">vertsNR</font>** = bb1.GetLocalVerticesNoRotation();
  

GetWorldVertices

• 原型：
  

  std::array<Location,8> GetWorldVertices(const Transform &in_bbox_to_world_tr) const;
  

• 说明：先调用 GetLocalVertices，再对每个顶点应用 TransformPoint，得到世界坐标顶点数组。
• 示例：
  

  auto **<font color="#f8805a">worldVerts</font>** = bb1.GetWorldVertices(**<font color="#f8805a">tr</font>**);
  

比较运算符

operator== / operator!=

• 原型：
  

  bool operator==(const BoundingBox &rhs) const;
  bool operator!=(const BoundingBox &rhs) const;
  

• 说明：当且仅当 location, extent, rotation 全部相等时才判定相等。
• 示例：
  

  bool **<font color="#f8805a">eq</font>** = (bb1 == bb2);
  

UE4 类型互转（LIBCARLA_INCLUDED_FROM_UE4）

BoundingBox(const FBoundingBox &Box)

• 说明：将 UE4 FBoundingBox 转换为本地类型，单位从厘米转米：
  $$\text{location}=Box.Origin,\quad \text{extent}=(Box.Extent/100).$$
• 示例：
  

  FBoundingBox **<font color="#f8805a">ubox</font>**;
  BoundingBox **<font color="#f8805a">bbU</font>**(**<font color="#f8805a">ubox</font>**);
  

GeoLocation

概述

carla::geom::GeoLocation 用于在真实地理坐标系（WGS84，经纬度+海拔）与仿真局部坐标系carla::geom::Location（CARLA 自定义坐标）之间进行转换。当 CARLA 场景与 OpenDRIVE 地图或真实地理数据（如地图对齐或高精地图）结合时，GeoLocation 能实现：设定仿真地图的锚点（anchor）；映射仿真场景中物体的位置到全球坐标；实现与外部设备或真实车辆的数据融合（如 GPS 传感器仿真）。

公共数据成员

• latitude (double)：纬度，单位度 (°)，初始值 0.0
• longitude (double)：经度，单位度 (°)，初始值 0.0
• altitude (double)：海拔，单位米 (m)，初始值 0.0

构造函数

GeoLocation()

• 原型：GeoLocation() = default;
• 说明：使用默认值构造。
• 示例：
  

  GeoLocation **<font color="#f8805a">g0</font>**; // (0,0,0)
  

GeoLocation(double latitude, double longitude, double altitude)

• 原型：
  

  GeoLocation(double latitude, double longitude, double altitude);
  

• 说明：按指定经纬度及海拔初始化。
• 示例：
  

  GeoLocation **<font color="#f8805a">g1</font>**(37.7749, -122.4194, 30.0);
  

核心方法

Transform

• 原型：
  

  GeoLocation Transform(const Location &location) const;
  

• 说明：
• 将当前经纬度和平面位移（location.x, location.y）转换为新的经纬度；
• altitude 累加 location.z。
• 示例：
  

  GeoLocation **<font color="#f8805a">geo</font>**(0.0, 0.0, 100.0);
  Location **<font color="#f8805a">loc</font>**(10.0f, 20.0f, 5.0f);
  GeoLocation **<font color="#f8805a">res</font>** = **<font color="#f8805a">geo</font>**.Transform(**<font color="#f8805a">loc</font>**);
  // 计算后 res.latitude, res.longitude 变化，res.altitude == 105.0
  

转换原理
- 利用墨卡托投影将 (latitude, longitude) 映射为 (mx, my)；
- 在投影平面上加上 dx = location.x、dy = -location.y（北向为正）；
- 反变换回经纬度；
- 海拔相加。

静态辅助函数

• LatToScale
  

  static double LatToScale(double lat);
  

• 说明：比例因子 $=\cos(\mathrm{toRadians}(lat))$。

• LatLonToMercator (模板)
  

  template<class T>
  static void LatLonToMercator(double lat, double lon, double scale, T &mx, T &my);
  

• 说明：
  $$mx = scale \times R \times \mathrm{toRadians}(lon),$$
  $$my = scale \times R \times \ln\!\tan\Bigl(\tfrac{90+lat}{360}\pi\Bigr).$$

• MercatorToLatLon
  

  static void MercatorToLatLon(double mx, double my, double scale, double &lat, double &lon);
  

• 说明：
  $$lon = mx\tfrac{180}{\pi R\,scale},\quad lat = 360\arctan\exp\bigl(\tfrac{my}{R\,scale}\bigr)/\pi -90.$$

• LatLonAddMeters
  

  static void LatLonAddMeters(
      double lat_start, double lon_start,
      double dx, double dy,
      double &lat_end, double &lon_end);
  

• 说明：在起始经纬度处，沿东(dx)/北(dy)方向（米）偏移后计算新的经纬度。