光线投射（Raycasting）原理详解
首先光线投射（Raycasting）是一种基本的计算机图形学技术，用于检测和处理三维空间中的对象。它通过在场景中投射一条或多条光线，确定这些光线与场景中物体的交点，从而实现渲染、碰撞检测、路径规划等任务。下面将详细介绍光线投射的原理及其在各种应用中的实现方法。
基本原理
光线投射的核心思想是从视点（或相机）发射光线，沿着光线方向检查它们是否与障碍物点重合，可以用做Line of sight的判断。
碰撞检测
光线投射在碰撞检测中用于检测光线与物体的相交情况，以确定物体之间是否发生碰撞。以下是光线投射碰撞检测的具体步骤：
- 光线定义：
光线由一个起点和一个方向向量定义。数学表达式为：R(t) = O + t * D
R(t) 是光线的点。
O是光线的起点。
D是光线的方向向量。
t是参数，表示光线在方向向量上的位移。 - 物体表示：
使用几何体（如球体、平面、AABB等）表示场景中的物体。 - 相交检测算法：
对每个物体，使用相应的相交检测算法，判断光线是否与物体相交。
常用的相交检测算法包括：
光线与球体相交检测。
光线与平面相交检测。
光线与AABB相交检测。 - 碰撞处理：
如果光线与物体相交，记录相交点和表面法线。
根据碰撞情况，执行相应的处理操作，如反弹、阻挡等。
这是定义raycast类的头文件，后面会着重介绍一下里面的几个函数
raycast.h
#ifndef RAYCAST_H_
#define RAYCAST_H_
#include <Eigen/Eigen>
#include <vector>
//符号函数，用于返回x的符号（正数返回1，负数返回-1，零返回0）。
double signum(double x);
//模运算函数，返回value除以modulus的余数，处理了负数情况。
double mod(double value, double modulus);
//计算并返回沿某一方向（由ds指示）从s出发，到达下一个整数边界所需的距离。
double intbound(double s, double ds);
//这里提供了两个重载的射线投射函数，用于计算从start到end点的射线与由min和max定义的轴对齐边界框（Axis-Aligned Bounding Box, AABB）的交点。
//第一个函数将交点存储在预分配的数组中，而第二个函数使用std::vector<Eigen::Vector3d>动态存储交点。
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output);
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output);
/*
这个头文件定义了用于射线投射（Raycasting）的一系列函数和RayCaster类。
射线投射是计算机图形学和机器人领域中的一个常用技术，它模拟从一个起点沿特定方向发射的“射线”，并计算这条射线与场景中对象的交点。
这个技术在视觉模拟、地形分析、碰撞检测等多个领域都有应用。以下是对提供的代码及其功能的详细解释：*/
class RayCaster {
private:
/* data */
//包括射线的起点、终点、方向，以及遍历过程中的当前位置、步长、最大距离等。
Eigen::Vector3d start_;
Eigen::Vector3d end_;
Eigen::Vector3d direction_;
Eigen::Vector3d min_;
Eigen::Vector3d max_;
int x_;
int y_;
int z_;
int endX_;
int endY_;
int endZ_;
double maxDist_;
double dx_;
double dy_;
double dz_;
int stepX_;
int stepY_;
int stepZ_;
double tMaxX_;
double tMaxY_;
double tMaxZ_;
double tDeltaX_;
double tDeltaY_;
double tDeltaZ_;
double dist_;
int step_num_;
// 分辨率和偏移量
double resolution_;
Eigen::Vector3d offset_;
Eigen::Vector3d half_;
public:
RayCaster(/* args */) {
}
~RayCaster() {
}
//设置射线投射的参数，如分辨率和原点。
void setParams(const double& res, const Eigen::Vector3d& origin);
// 输入射线的起点和终点，准备进行射线投射。
bool input(const Eigen::Vector3d& start, const Eigen::Vector3d& end);
// 获取射线路径上下一个格子的索引。
bool nextId(Eigen::Vector3i& idx);
// 获取射线路径上下一个位置的坐标。
bool nextPos(Eigen::Vector3d& pos);
};
#endif // RAYCAST_H_
raycast类一共就有4个核心函数，分别是setParams，input，nextId和nextPos。还有3个运算的辅助函数signum，mod和intbound。
首先先介绍3个运算的辅助函数，因为核心函数里面会调用到这3个辅助函数，所以就先介绍了。
1.signum 函数
signum 函数用于确定一个整数的符号。
int signum(int x) {
return x == 0 ? 0 : x < 0 ? -1 : 1;
}
其实就是判断一个数的正负，这也用于判断一个向量单一维度的方向。
如果 x 为 0，返回 0。
如果 x 为负数，返回 -1。
如果 x 为正数，返回 1。
2.mod 函数
mod 函数用于计算浮点数的模数（取余数）。
double mod(double value, double modulus) {
return fmod(fmod(value, modulus) + modulus, modulus);
}
输入：value：被取模数。 modulus：模数。
输出：value 对 modulus 取模的结果。
这里需要注意的是fmod函数的原理和使用！！
（1）fmod 函数：
fmod(x, y) 返回 x 除以 y 的余数，其符号与 x 相同。
例如，fmod(5.5, 3.0) 返回 2.5，fmod(-5.5, 3.0) 返回 -2.5。
（2）双重 fmod 操作：
第一次 fmod 操作可能会返回负值，因此我们加上 modulus，确保结果为正。
再次对结果进行 fmod 操作，确保结果在 [0, modulus) 范围内。
3.intbound 函数
intbound 函数用于计算光线从当前位置到达下一个整数边界所需的步长。
double intbound(double s, double ds) {
// Find the smallest positive t such that s+t*ds is an integer.
// 找到最小的正t，使s+t*ds为整数。
if (ds < 0) {
return intbound(-s, -ds);
} else {
s = mod(s, 1);
// problem is now s+t*ds = 1
// 现在的问题是s+t*ds=1
return (1 - s) / ds;
}
}
输入：s：光线当前的位置。ds：光线在该轴上的步长。
输出：光线从 s 位置到达下一个整数边界所需的步长 t。
具体的步骤：
（1）处理负步长：
如果 ds 为负数，将 s 和 ds 取反，转换为正数处理。
这是因为从负方向穿越整数边界与从正方向穿越是对称的。
（2）模数运算：
通过 mod(s, 1)，将 s 转换到 [0, 1) 范围内。
例如，如果 s 为 2.7，mod(s, 1) 返回 0.7。
（3）计算步长 t：
现在的问题转换为：找到最小的正 t 使得 s + t * ds 为整数。
由于 s 已经在 [0, 1) 范围内，我们需要 s + t * ds = 1。
解方程得到 t = (1 - s) / ds。
好处：intbound 函数在光线投射算法中用于计算光线到达下一个体素边界所需的时间增量。这样可以确定光线在三维空间中穿越体素的路径。
4个核心函数
1.setParams
RayCaster::setParams 函数的作用是设置光线投射算法的参数，包括分辨率和原点的偏移量。具体来说，这个函数对光线投射的分辨率和原点坐标进行初始化和计算，以便在后续的光线投射过程中使用。
void RayCaster::setParams(const double& res, const Eigen::Vector3d& origin) {
// 获取光线投射的分辨率
resolution_ = res;
// 创建一个三维向量 half_，每个分量都为 0.5。这通常用于将坐标对齐到体素的中心，因为体素的边界在整数坐标上，而其中心则在 0.5 处。
half_ = Eigen::Vector3d(0.5, 0.5, 0.5);
offset_ = half_ - origin / resolution_;
}
RayCaster::setParams 函数主要完成以下任务：
设置光线投射的分辨率 resolution_。
计算并设置用于对齐体素中心的偏移量 offset_。
2.input
RayCaster::input 函数的作用是初始化光线投射（ray casting）过程中所需的各种参数，为接下来的 3D 数字微分分析（3D-DDA）算法做准备。该函数接收光线的起点和终点，并计算初始的步长、步数和其他辅助变量。
bool RayCaster::input(const Eigen::Vector3d& start, const Eigen::Vector3d& end) {
// 将起点和终点坐标除以分辨率 resolution_，归一化到栅格坐标系中。
start_ = start / resolution_;
end_ = end / resolution_;
// 获取起点和终点的整型坐标
x_ = (int)std::floor(start_.x());
y_ = (int)std::floor(start_.y());
z_ = (int)std::floor(start_.z());
endX_ = (int)std::floor(end_.x());
endY_ = (int)std::floor(end_.y());
endZ_ = (int)std::floor(end_.z());
// 获取起点指向终点的方向向量
direction_ = (end_ - start_);
// 计算两点的最大距离
maxDist_ = direction_.squaredNorm();
// Break out direction vector.
// 计算在 x、y、z 方向上的步长 dx_, dy_, dz_，即起点和终点在每个轴上的差值。
dx_ = endX_ - x_;
dy_ = endY_ - y_;
dz_ = endZ_ - z_;
// Direction to increment x,y,z when stepping.
// 使用 signum 函数确定在 x、y、z 轴上的步长方向（+1 或 -1）。
stepX_ = (int)signum((int)dx_);
stepY_ = (int)signum((int)dy_);
stepZ_ = (int)signum((int)dz_);
// See description above. The initial values depend on the fractional
// part of the origin.
tMaxX_ = intbound(start_.x(), dx_);
tMaxY_ = intbound(start_.y(), dy_);
tMaxZ_ = intbound(start_.z(), dz_);
// The change in t when taking a step (always positive).
tDeltaX_ = ((double)stepX_) / dx_;
tDeltaY_ = ((double)stepY_) / dy_;
tDeltaZ_ = ((double)stepZ_) / dz_;
dist_ = 0;
step_num_ = 0;
// Avoids an infinite loop.
if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
return false;
else
return true;
}
RayCaster::input 函数通过接收光线的起点和终点，计算并初始化了一系列参数，这些参数包括光线的方向、步长、边界穿越时间等。函数最后返回一个布尔值，表示初始化是否成功。如果光线的步长为零（即起点和终点相同），函数返回 false 以避免无限循环。否则，函数返回 true，表示初始化成功，可以继续进行光线投射过程。
3.nextId
RayCaster::nextId 函数实现了一个三维数字微分分析（3D Digital Differential Analyzer, 3D-DDA）算法，用于在三维栅格空间中沿着给定的方向逐步推进，找到光线穿过的每一个体素（voxel）。
// 该函数在每次调用时会更新当前光线的位置和下一次穿越体素边界的时间（t 值），并返回当前光线所在的体素索引。
bool RayCaster::nextId(Eigen::Vector3i& idx) {
// 获取起点的位置
auto tmp = Eigen::Vector3d(x_, y_, z_);
// 将当前光线位置 tmp 加上偏移量 offset_，并将结果转换为整数类型，存储在 idx 中。这一步确定了当前光线所在的体素索引。
idx = (tmp + offset_).cast<int>();
// 如果起点等于终点，则不需要进行投射
if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
return false;
}
// tMaxX stores the t-value at which we cross a cube boundary along the
// X axis, and similarly for Y and Z. Therefore, choosing the least tMax
// chooses the closest cube boundary. Only the first case of the four
// has been commented in detail.
/*
//tMaxX存储我们沿以下方向穿过立方体边界时的t值
//X轴，Y和Z轴也是如此。因此，选择最小的tMax
//选择最近的立方体边界。仅四例中的第一例
//已详细评论。
*/
//这部分代码是 3D-DDA 算法的核心，通过比较 tMaxX_, tMaxY_, tMaxZ_ 的值，选择光线下一步将穿越的体素边界。
// 三维的投射：图片可以
/*
**0
***
***
***
***
0 *为需要检查的体素块，每个体素块的方向需要增加tDeltaX_
*/
if (tMaxX_ < tMaxY_) {
if (tMaxX_ < tMaxZ_) {
// Update which cube we are now in.
// 如果 tMaxX_ 最小，说明光线将首先穿越 x 方向的体素边界，因此更新 x 位置，并增加 tMaxX_。
x_ += stepX_;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX_ += tDeltaX_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
} else {
if (tMaxY_ < tMaxZ_) {
// 如果 tMaxY_ 最小，说明光线将首先穿越 y 方向的体素边界，因此更新 y 位置，并增加 tMaxY_。
y_ += stepY_;
tMaxY_ += tDeltaY_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
}
return true;
}
其实上面这段代码的基本逻辑和下面这张图的逻辑是一样的，只不过是从二维转换成3维了。
代码的核心有两个部分，第一是体素块的索引是怎么确定的，第二如何得到直线所经过的栅格块。
idx = (tmp + offset_).cast<int>();
这个解决了第一个问题，通过增加偏移量获取对应体素点的索引
if (tMaxX_ < tMaxY_) {
if (tMaxX_ < tMaxZ_) {
// Update which cube we are now in.
// 如果 tMaxX_ 最小，说明光线将首先穿越 x 方向的体素边界，因此更新 x 位置，并增加 tMaxX_。
x_ += stepX_;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX_ += tDeltaX_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
} else {
if (tMaxY_ < tMaxZ_) {
// 如果 tMaxY_ 最小，说明光线将首先穿越 y 方向的体素边界，因此更新 y 位置，并增加 tMaxY_。
y_ += stepY_;
tMaxY_ += tDeltaY_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
}
这个解决了第二个问题，通过各个轴的最大时间tMax判断边界（时间最小最靠近边界），并更新光线的位置和各个轴的最大时间tMax（将上面的二维图的x，y边界对比着理解）。
4.nextPos
这个函数和nextId的作用类似，只不过这个是获取当前光线的位置点对应的体素点的中心。
bool RayCaster::nextPos(Eigen::Vector3d& pos) {
// 将当前光线在栅格坐标系中的位置 tmp 加上半体素偏移量 half_（通常为(0.5, 0.5, 0.5)），然后乘以分辨率 resolution_，得到实际空间中的光线位置并存储在 pos 中。
auto tmp = Eigen::Vector3d(x_, y_, z_);
// 这一步将光线的位置对齐到体素的中心，以便后续的计算。
pos = (tmp + half_) * resolution_;
// 检查当前光线的位置是否已经到达终点。如果当前坐标等于终点坐标，则返回 false，表示光线投射已经完成。
if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
return false;
}
// tMaxX stores the t-value at which we cross a cube boundary along the
// X axis, and similarly for Y and Z. Therefore, choosing the least tMax
// chooses the closest cube boundary. Only the first case of the four
// has been commented in detail.
/*
//tMaxX存储我们沿以下方向穿过立方体边界时的t值
//X轴，Y和Z轴也是如此。因此，选择最小的tMax
//选择最近的立方体边界。仅四例中的第一例
//已详细评论。
*/
// 这部分代码是 3D-DDA 算法的核心，通过比较 tMaxX_, tMaxY_, tMaxZ_ 的值，选择光线下一步将穿越的体素边界。
if (tMaxX_ < tMaxY_) {
if (tMaxX_ < tMaxZ_) {
// 如果 tMaxX_ 最小，说明光线将首先穿越 x 方向的体素边界，因此更新 x 位置，并增加 tMaxX_。
// Update which cube we are now in.
x_ += stepX_;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX_ += tDeltaX_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
} else {
if (tMaxY_ < tMaxZ_) {
// 如果 tMaxY_ 最小，说明光线将首先穿越 y 方向的体素边界，因此更新 y 位置，并增加 tMaxY_。
y_ += stepY_;
tMaxY_ += tDeltaY_;
} else {
// 如果 tMaxZ_ 最小，说明光线将首先穿越 z 方向的体素边界，因此更新 z 位置，并增加 tMaxZ_。
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
}
return true;
}
Line of sight
使用光线投影进行Line of sight的具体步骤如下：
（1）先通过setParams函数去确定光线投射的分辨率以及偏移量。
（2）然后通过input函数确定光线的起点和终点。
（3）接着通过nextId去搜索光线上的每一个栅格点，并获取栅格点的索引idx。
（4）最后通过对栅格地图的障碍物判断函数getOccupancy(idx)检索idx是否为障碍物。
（5）如果存在idx为障碍物，则该光线不可视，如果光线经过的所有栅格点都不是障碍物，则该光线可视。
当然raycasting不仅这一种用法，而line of sight的判断也不止这种方法，后续如果有更优的方法也会在这片博客上进行补充！！
扫一扫
2533
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
