飞行器空间模拟-物理引擎

1. 物理引擎源码详解

MathUtils

实现3D物理引擎核心数学工具集，包含坐标系转换、欧拉角处理和正交向量计算

核心功能：
– 向量/矩阵的基础运算
– 欧拉角与旋转矩阵的相互转换
– 正交坐标系生成
– 四元数相关运算

函数分类及说明：

【正交坐标系生成】
1. getOrthoUnits(Vector3 v, Vector3 &p, Vector3 &q)
– 输入：法向量v
– 输出：与v正交的单位向量p和q(p×q=v)
– 特点：自动选择数值稳定的计算路径

2. getMatrixFromAxisX(Vector3 x_dir, Vector3 y_sugg)
– 输入：X轴方向和建议Y轴
– 输出：完整的正交矩阵[X,Y,Z]
– 特点：自动处理退化情况

【欧拉角转换】(6种顺序)
matrixToEuler[XYZ|XZY|YXZ|YZX|ZXY|ZYX]():
– 通用处理流程：
1. 检查万向锁情况(阈值0.99)
2. 常规情况计算三个角度
3. 万向锁时返回简化解
– 返回值：是否唯一解

【特殊矩阵生成】
1. getCrossMatrix(Vector3 v)
– 生成叉乘矩阵[v×]

2. 四元数相关矩阵：
– getStarMatrix44(Quatern q)：四元数乘法矩阵(4×4)
– getGlMatrix34(Quatern q)：四元数转旋转矩阵辅助矩阵
– getFpMatrix34/getFmMatrix34：四元数导数计算矩阵

【关键算法细节】
* 欧拉角转换统一模式：
if (接近万向锁) {
返回退化解 + false
} else {
计算完整角度 + true
}

* 正交基生成策略：
if (Z分量主导) {
在YZ平面构建p
} else {
在XY平面构建p
}

* 四元数矩阵特性：
StarMatrix44(q)*v = q⊗v
GlMatrix34(q)用于ω→q’转换
============================

[典型应用场景]
1. 碰撞检测：
– 使用getOrthoUnits生成接触切平面
– 用getMatrixFromAxisX构建碰撞坐标系

2. 关节约束：
– 欧拉角转换用于限制旋转自由度
– 叉乘矩阵用于计算力矩臂

3. 四元数运算：
– StarMatrix用于四元数插值
– GlMatrix用于角速度积分

[注意事项]
1. 欧拉角转换在万向锁位置：
– 返回的角度组合不唯一
– 需检查返回值判断是否唯一解

2. 正交基生成：
– 建议Y轴不平行于X轴时最稳定
– 自动处理零向量输入

3. 四元数矩阵：
– Fp/Fm矩阵分别对应前后乘导数
– 注意四元数顺序(w,x,y,z)
============================

示例代码：

cpp
// 构建正交坐标系
Vector3 normal(1,2,3);
Vector3 tangent, binormal;
getOrthoUnits(normal, tangent, binormal);

// 从旋转矩阵获取ZYX欧拉角
Matrix3x3 rot = …;
Vector3 euler;
if(matrixToEulerZYX(rot, euler)) {
// 非万向锁情况
} else {
// 处理万向锁
}

// 计算角速度引起的四元数变化率
Matrix<3,4> G = getGlMatrix34(orientation);
Quatern q_dot = 0.5 * G.transpose() * angular_velocity;
“`

补充说明：

所有函数设计为无动态内存分配
使用显式分支避免数值误差
提供完整的欧拉角顺序支持
阈值(0.99)可根据需要调整

BodyBase.cpp

作用介绍： 该文件实现了物理体基类（BodyBase）的核心功能。BodyBase是物理引擎中所有物理对象的基础类，负责管理物理体的状态（静态、运动学、动态、休眠等）、碰撞忽略列表、包围盒更新以及休眠机制。它为刚体、软体等具体物理对象提供通用的状态管理和基础功能。

所有函数大致作用介绍：

isKinematic() – 检查物理体是否为运动学类型（受外部控制，不受物理力影响）
isStatic() – 检查物理体是否为静态类型（完全不移动的物体）
isSleep() – 检查物理体是否处于休眠状态（暂停物理计算以优化性能）
setSleep() – 将物理体设置为休眠状态，暂停其物理更新
setKinematic() – 将物理体设置为运动学类型，清除休眠状态并设置运动学标志
setStatic() – 将物理体设置为静态类型，清除休眠状态并设置静态标志
setDynamic() – 将物理体设置为动态类型，仅保留休眠标志，清除其他类型标志
setAwake() – 唤醒物理体，清除休眠标志并重置休眠计时器
updateSleepTime() – 更新休眠计时器，累加时间并返回当前休眠时间
resetSleepTime() – 重置休眠计时器为零
setToWake() – 标记物理体需要在下一帧被唤醒
clearToWake() – 清除待唤醒标志
isToWake() – 检查物理体是否被标记为待唤醒
updateAABB() – 根据当前世界变换和形状更新轴对齐包围盒
addIgnoreCollision() – 添加需要忽略碰撞的物体ID到忽略列表
ignoreCollision() – 检查指定ID的物体是否在碰撞忽略列表中

AABBBox.cpp

作用介绍： 该文件实现了轴对齐包围盒（Axis-Aligned Bounding Box, AABB）类的核心功能。AABB是物理引擎中用于碰撞检测和空间分割的重要几何体，通过最小点和最大点定义一个与坐标轴对齐的矩形包围盒，广泛用于快速碰撞检测的粗略阶段。

函数介绍：

AABBBox构造函数 – 使用最小点和最大点初始化AABB包围盒对象
getExtent() – 计算并返回包围盒的尺寸向量（长、宽、高），通过最大点减去最小点得到
getVolume() – 计算并返回包围盒的体积，通过三个维度的尺寸相乘得到
isContain() – 判断当前包围盒是否完全包含另一个包围盒，检查所有六个面的包含关系
testCollision() – 快速碰撞检测函数，使用分离轴定理判断两个AABB是否相交，通过检查三个轴向的重叠情况
mergeAABB() – 将另一个AABB合并到当前包围盒中，扩展当前包围盒以包含两个盒子的并集
isOverlap() – 静态函数，检测两个AABB是否重叠，并计算重叠区域的最小点和最大点，返回重叠状态和重叠区域信息

BoxShape.cpp

作用介绍： 该文件实现了盒子形状（BoxShape）类的功能。BoxShape是物理引擎中的凸形状之一，用于表示长方体碰撞体。它继承自ConvexShape，提供了盒子的惯性张量计算、支撑顶点查找等功能，是物理引擎中最常用的碰撞形状之一，广泛用于建筑物、箱子等矩形物体的物理模拟。

所有函数大致作用介绍：

BoxShape构造函数 – 使用半尺寸向量初始化盒子形状，设置形状类型标志为eBox，并存储盒子的半尺寸参数
getInertia() – 计算并返回盒子的惯性张量矩阵。根据盒子的几何特性，计算绕三个坐标轴的转动惯量，用于物理模拟中的旋转运动计算
getHalfExtentsWithMargin() – 返回包含碰撞边距的半尺寸向量。在原始半尺寸基础上加上边距值，用于碰撞检测中的容错处理
localGetSupportingVertex() – 在本地坐标系中查找指定方向上的支撑顶点。这是GJK碰撞检测算法的核心函数，根据查询方向返回盒子表面最远的顶点坐标，包含容差处理

CapsuleShape.cpp

作用介绍： 该文件实现了胶囊形状（CapsuleShape）类的功能。CapsuleShape是物理引擎中的凸形状之一，由一个圆柱体和两个半球组成，形似胶囊或药丸。它继承自ConvexShape，广泛用于角色控制器、柱状物体等需要平滑碰撞表面的场景，能够提供良好的碰撞响应和滚动特性。

所有函数大致作用介绍：

CapsuleShape构造函数 – 使用半径和半高度初始化胶囊形状，设置形状类型标志为eCapsule，存储胶囊的几何参数
getInertia() – 计算并返回胶囊的惯性张量矩阵。胶囊的惯性计算较为复杂，需要考虑圆柱部分和两个半球部分的贡献：
- Y轴（胶囊长轴）的转动惯量较小
- X轴和Z轴的转动惯量相等且较大，包含了圆柱和球体部分的组合惯性
localGetSupportingVertex() – 在本地坐标系中查找指定方向上的支撑顶点。这是GJK碰撞检测算法的核心函数：
- 根据查询方向的Y分量判断应该从上半球还是下半球查找支撑点
- 将规范化的方向向量乘以半径（加容差）得到球面上的支撑点
- 加上对应半球中心的偏移量得到最终的支撑顶点坐标

CompoundShape.cpp

该文件实现了复合形状类 CompoundShape，用于将多个基础几何形状组合成一个复杂的复合形状。这在物理引擎中常用于创建由多个简单形状组成的复杂物体，如汽车、建筑物等。该类继承自 ShapeBase，并使用边界体积层次结构（BVH）来优化碰撞检测性能。

所有函数大致作用介绍

构造函数 CompoundShape(ObjectArray<ShapeEle> shapes)

初始化复合形状对象
设置形状标志为复合类型
调用setShapes方法设置子形状数组

setShapes(ObjectArray<ShapeEle> shapes)

设置复合形状包含的子形状数组
根据形状数量计算BVH节点大小（最少8191个形状per节点）
初始化BVH结构用于加速碰撞检测

getShapes()

返回复合形状中所有子形状的引用
提供对内部形状数组的访问接口

getAABBBox(const Coordsys &transform)

计算复合形状在给定变换下的轴对齐包围盒（AABB）
遍历所有子形状，获取每个形状变换后的AABB
将所有子形状的AABB合并成一个总的包围盒

getInertia()

计算复合形状的惯性张量
使用平行轴定理计算每个子形状对总惯性的贡献
考虑子形状的局部坐标系、旋转和质量比例
返回归一化后的总惯性张量

getIntersectShapes(const AABBBox &aabb, ObjectArray<int> &intersect)

查找与给定AABB相交的子形状索引
使用BVH结构进行快速空间查询
将相交的形状索引存储在intersect数组中

setCenter(const Vector3 &center)

设置复合形状的中心点
通过调整所有子形状的局部坐标系位置来实现重心调整
将所有子形状相对于新中心重新定位

ConvexShape.cpp

作用介绍

该文件实现了凸形状基类 ConvexShape 的核心功能。凸形状是物理引擎中一个重要的几何形状抽象，所有凸几何体（如球体、盒子、胶囊体等）都继承自这个类。该文件主要实现了凸形状的轴对齐包围盒（AABB）计算功能，这是碰撞检测系统中宽相位检测的基础。

所有函数大致作用介绍

getAABBBox(const Coordsys &transform)

计算凸形状在给定变换下的轴对齐包围盒（AABB）
参数说明：
- transform：形状的世界坐标变换（包含位置和旋转）
算法流程：
- 提取变换中的旋转矩阵
- 对于X、Y、Z三个坐标轴方向：
  - 创建正方向单位向量，变换到物体局部坐标系
  - 调用 localGetSupportingVertex 获取该方向上的支撑顶点
  - 将支撑顶点变换到世界坐标系
  - 加上形状边距（margin）得到AABB最大值
  - 创建负方向单位向量，重复上述过程得到AABB最小值
返回由最小和最大坐标构成的AABB包围盒
这种方法利用了凸形状的支撑函数特性，能够精确计算任意凸形状的紧致包围盒
考虑了形状的边距（mMargin），确保包围盒包含完整的碰撞区域

该函数是凸形状碰撞检测流水线中的关键组件

CylinderShape.cpp

实现圆柱体碰撞形状的属性和方法

函数列表：

CylinderShape() – 构造函数，初始化圆柱体尺寸
getInertia() – 计算圆柱体的惯性张量
localGetSupportingVertex() – 计算给定方向上的支撑顶点

主要功能：

存储圆柱体的半高和半径
提供物理计算所需的几何特性
支持碰撞检测的支撑点计算

`MeshShape` 简略解析

功能

将 .ply 或 .obj 模型文件解析为一组三角形，用于物理仿真（如碰撞检测）。

构造方式

传入顶点 + 面索引 cpp复制编辑MeshShape(vertices, faces, size);
读取模型文件 cpp复制编辑MeshShape("model.obj", size);
- .ply: 支持 ASCII / 二进制
- .obj: 读取 v 顶点和 f 三角面

核心逻辑：setMesh(...)

遍历所有面
构造三角形（TriangleShape）
根据面积设定质量比例
添加到 CompoundShape 中

依赖类

Vector3: 三维点
Vector<3, int>: 三角面索引
TriangleShape: 三角形物理形状
CompoundShape: 可组合的复合体

注意

.obj 顶点索引从 1 开始，需减1
只支持 三角形面
.ply 仅解析几何，不含纹理/法线

`SphereShape` 类说明

功能

表示一个球体的几何形状，继承自 ConvexShape。

主要成员与方法

构造函数 SphereShape(Real radius) 初始化球体半径 mRadius，并将类型标记为 ShapeFlag::eSphere。
惯性矩计算 Matrix3x3 getInertia() const 返回球体的惯性矩（简化为对角矩阵）： I=0.4×r2×I3I = 0.4 \times r^2 \times I_3 其中 I3I_3 是单位矩阵。
支持点计算Vector3 localGetSupportingVertex(const Vector3 &position, const Real &tolerance) const 计算在某方向 position 上的支持点（Minkowski差中用于GJK等算法）：
- r = mRadius + tolerance
- 归一化方向向量 position 并乘以半径，返回球面上的点。

代码示例总结

// 构造球体，设置类型和半径
SphereShape::SphereShape(Real radius) : ConvexShape() {
    mType |= ShapeFlag::eSphere;
    mRadius = radius;
}

// 计算惯性矩，简化为对角矩阵
Matrix3x3 SphereShape::getInertia() const {
    Real element = mRadius * mRadius * 0.4;
    return Matrix3x3::Identity() * element;
}

// 计算给定方向的支持点

下面是你给出的 TriangleShape 类的简略解析笔记：

TriangleShape（三角形形状）类

表示物理引擎中一个三角形的碰撞形状，继承自 ConvexShape，用来参与碰撞检测和物理计算。

主要成员变量

mVertices[3]：三角形的三个顶点坐标（局部空间）

关键方法

1. 构造函数 `TriangleShape(Vector3 p1, Vector3 p2, Vector3 p3)`

初始化三角形的三个顶点
标记类型为三角形（ShapeFlag::eTriangle）

2. `getInertia()`

计算三角形形状的惯性张量（3×3矩阵）
通过累加顶点坐标的平方和乘积计算惯性张量分量
对称矩阵赋值（非对角元素相等）
结果用于物理模拟中刚体的旋转惯量计算

3. `localGetSupportingVertex(const Vector3 &position, const Real &tolerance)`

给定一个方向向量 position，计算支持顶点（用于GJK等碰撞检测算法）
计算所有顶点在该方向上的点积，返回最大点积对应的顶点加上方向单位向量乘以容差值
返回形状在该方向的“最远点”

4. `getVertex(int index)`

访问三角形的指定顶点
通过断言保证索引合法

设计亮点

简单明了，适用于三角网格碰撞形状
支持惯性计算和支持顶点查询，为物理引擎提供关键接口
支持容差调节，增加数值稳定性

BVH.cpp

作用介绍： 该文件实现了层次包围盒（Bounding Volume Hierarchy, BVH）数据结构，用于加速复合形状（CompoundShape）的空间查询和碰撞检测。BVH通过递归地将形状元素分组到层次化的包围盒中，可以快速剪除不相交的区域，显著提升复杂几何体的碰撞检测和空间查询性能。

所有函数大致作用介绍：

cmp() – 全局比较函数，用于按指定轴向对复合形状的子形状元素进行排序，比较各自包围盒中心点在指定轴上的坐标值
BVH构造函数 – 初始化BVH树，调用buildBVH递归构建整个层次结构，传入复合形状和每个叶节点的最大元素数量
BVH析构函数 – 清理BVH树的所有节点内存
buildBVH() – 递归构建BVH树的核心算法：
- 计算当前范围内所有形状的联合包围盒
- 如果形状数量小于等于阈值，创建叶节点并存储形状索引
- 否则选择最长的轴向进行排序分割，递归构建左右子树
- 更新节点的子树大小统计信息
clear() – 使用非递归方式（栈）遍历并删除BVH树的所有节点，避免深度递归导致的栈溢出
setCompoundShape() – 重新设置复合形状并重建BVH树，先清理原有树结构再构建新的层次结构
search() – 空间查询函数，在BVH树中搜索与给定AABB包围盒相交的所有形状：
- 使用非递归遍历（栈）提高性能
- 对每个节点进行包围盒相交测试进行剪枝
- 对叶节点收集相交的形状索引
- 对内部节点继续遍历其子节点

Axle.cpp

作用介绍： 该文件实现了车辆轴系统（Axle）类的功能。轴系统是轮式车辆物理模拟中的核心组件，负责管理一组车轮、悬挂系统和与底盘的连接约束。它处理车轮的转向、油门控制，以及悬挂系统的物理模拟，是连接底盘和车轮的重要中间层。

所有函数大致作用介绍：

Axle构造函数 – 初始化轴系统，设置关联的底盘引用以及该轴是否支持转向和油门控制的标志位
injectWorld() – 将轴系统中的所有车轮刚体和约束链接注入到物理世界中，用于物理引擎的统一管理
addWheel() – 向轴系统添加车轮，同时创建对应的悬挂系统和通用链接约束。设置悬挂的刚度和阻尼参数，并确保悬挂方向垂直
getLeftSpeed() – 计算并返回左侧车轮的平均转速，将角速度转换为RPM（转/分钟）单位
getRightSpeed() – 计算并返回右侧车轮的平均转速，将角速度转换为RPM单位
getSpeed() – 计算整个轴系统的平均转速，取左右两侧车轮转速的平均值
setWorldTransform() – 设置轴系统中所有车轮的世界坐标变换，用于更新车轮在世界空间中的位置和朝向
update() – 更新轴系统状态，遍历所有悬挂系统并调用其更新函数，处理悬挂的物理计算

Brake.cpp

作用介绍： 该文件实现了刹车系统（Brake）类的功能。Brake类负责车辆刹车机制的物理模拟，通过施加与车轮旋转方向相反的制动扭矩来实现刹车效果。它是车辆物理系统中的重要组件，为车轮提供可控的制动力，实现车辆的减速和停止功能。

所有函数大致作用介绍：

Brake构造函数 – 初始化刹车系统，设置最大制动扭矩值，该值决定了刹车系统的制动能力上限
applyBrake() – 对指定车轮施加刹车力。根据车轮当前的角速度方向计算制动扭矩方向，然后施加与旋转方向相反的扭矩，扭矩大小由最大制动扭矩和刹车输入值（0-1）的乘积决定

BroadPhaseBase.cpp

作用介绍： 该文件实现了广相碰撞检测基类（BroadPhaseBase）的功能。广相碰撞检测是物理引擎碰撞检测流水线的第一阶段，负责快速筛选出可能发生碰撞的物体对，过滤掉明显不会碰撞的物体组合，为后续的精确碰撞检测（窄相检测）提供候选对象，大幅提升碰撞检测的整体性能。

所有函数大致作用介绍：

getOverlappingPairs() – 返回当前检测到的重叠物体对列表的常量引用，供窄相碰撞检测阶段使用
clear() – 清空重叠物体对列表，通常在每帧开始时调用，重置碰撞检测状态
defaultValidatePairFunc() – 默认的物体对验证函数，用于过滤无效的碰撞对：
- 静态物体之间不检测碰撞
- 运动学物体之间不检测碰撞
- 休眠物体之间不检测碰撞
- 被标记为忽略碰撞的物体对不检测碰撞
- 通过验证的物体对才会进入后续的精确碰撞检测流程

BroadPhaseSimple.cpp

作用介绍： 该文件实现了简单广相碰撞检测（BroadPhaseSimple）类的功能。这是广相碰撞检测的一个基础实现，使用朴素的O(n²)算法对所有物体进行两两AABB包围盒重叠检测。虽然性能较低，但实现简单、稳定可靠，适用于物体数量较少的场景或作为其他高级广相算法的参考实现。

所有函数大致作用介绍：

calculateOverlappingPairs() – 计算所有重叠的物体对的核心函数：
- 首先清空之前的重叠对标志
- 使用双重循环遍历所有刚体对象（避免重复检测相同的对）
- 对每个物体对调用验证函数，过滤掉无效的碰撞组合
- 获取两个物体的AABB包围盒并进行重叠检测
- 如果AABB重叠，则将该物体对添加到重叠对列表中
- 最后移除未使用的重叠对，清理过期的碰撞对信息

BroadPhaseSweepAndPrune.cpp

作用介绍： 该文件实现了扫描与剪枝广相碰撞检测（BroadPhaseSweepAndPrune）类的功能。这是一种高效的广相碰撞检测算法，通过在某个坐标轴上对物体进行排序，然后扫描排序后的列表来快速识别可能碰撞的物体对。相比简单的O(n²)算法，该算法在物体分布较为均匀时能显著提升性能，并且能够自适应选择最优的排序轴。

所有函数大致作用介绍：

BroadPhaseSweepAndPrune构造函数 – 初始化扫描剪枝广相检测器，设置目标轴为X轴（索引0）
calculateOverlappingPairs() – 计算重叠物体对的核心算法：
- 清空之前的重叠对标志，处理少于2个物体的边界情况
- 按照当前目标轴的最小值对所有碰撞对象进行排序
- 扫描排序后的数组，对每个物体与其后续物体进行碰撞检测
- 在扫描过程中累计物体中心点的统计信息（和、平方和）
- 利用排序特性进行剪枝优化：如果后续物体的最小值超过当前物体的最大值，则跳出内层循环
- 对重叠的AABB执行精确重叠检测并添加到重叠对列表
- 清理未使用的重叠对
- 根据统计的方差信息自适应更新排序轴，选择方差最大的轴作为下一帧的目标轴，以获得更好的剪枝效果

BroadPhaseUtils.cpp

作用介绍： 该文件实现了广相碰撞检测的工具类，主要包含基于哈希表的重叠对容器（HashedBroadphaseOverlappingPairContainer）。该容器负责高效地管理和存储广相碰撞检测阶段发现的物体重叠对，使用哈希表优化查找性能，并提供了添加、删除、查找重叠对等核心功能，同时处理物体的休眠唤醒逻辑。

所有函数大致作用介绍：

HashedBroadphaseOverlappingPairContainer构造函数 – 初始化哈希重叠对容器
析构函数 – 清理容器资源，调用clear()函数
findPair() – 查找指定的两个物体是否存在重叠对，通过规范化物体顺序（较小ID在前）后调用内部查找函数
removePair() – 从容器中移除指定的重叠对，使用哈希查找定位后删除，并通过交换最后一个元素来避免数组空洞
clear() – 清空所有重叠对数据，重置哈希表和数组
clearPairFlag() – 清除所有重叠对的使用标志，为新一轮广相检测做准备
addPair() – 添加新的重叠对到容器中，如果已存在则更新使用标志，否则创建新的重叠对记录
removeUnusedPair() – 移除未使用的重叠对，同时处理物体休眠逻辑：当上方物体休眠而下方物体活跃时，唤醒上方物体以处理重力影响
getOverlappingPairs() – 返回当前所有重叠对的常量引用
internalFindPair() (重载1) – 内部查找函数，根据物体对计算哈希值后调用重载版本
internalFindPair() (重载2) – 内部查找函数的核心实现，在哈希表中查找匹配的重叠对
computeHash() – 计算两个物体ID的哈希值，使用boost风格的哈希组合算法，确保相同的物体对总是产生相同的哈希值

CompoundCompoundCollisionAlgorithm.cpp

作用介绍： 该文件实现了复合形状之间的碰撞检测算法（CompoundCompoundCollisionAlgorithm）。当两个复合形状（CompoundShape）发生碰撞时，该算法负责处理复杂的层次化碰撞检测，通过空间剪枝和递归分派来高效地检测所有可能的子形状碰撞组合，避免不必要的计算开销。

所有函数大致作用介绍：

processCollision() – 处理两个复合形状之间碰撞检测的核心函数：
- 将输入的形状强制转换为CompoundShape类型
- 获取两个复合形状的世界变换信息
- 使用空间查询优化：将第二个复合形状的AABB变换到第一个复合形状的本地坐标系中
- 调用第一个复合形状的getIntersectShapes()方法，获取所有可能与第二个复合形状相交的子形状索引列表
- 遍历相交的子形状，对每个子形状与第二个完整复合形状进行递归碰撞检测
- 通过NarrowPhaseDispatcher分派具体的碰撞算法，处理子形状与复合形状的碰撞
- 返回最终检测到的碰撞点数量

该算法通过层次化处理和空间剪枝大幅减少了复合形状碰撞检测的计算复杂度。

CompoundConvexCollisionAlgorithm.cpp

作用介绍： 该文件实现了复合形状与凸形状之间的碰撞检测算法（CompoundConvexCollisionAlgorithm）。当复合形状（CompoundShape）与凸形状（ConvexShape）发生碰撞时，该算法负责高效地处理这种混合类型的碰撞检测，通过空间剪枝技术只检测复合形状中可能与凸形状相交的子形状，避免全量检测的性能开销。

所有函数大致作用介绍：

processCollision() – 处理复合形状与凸形状之间碰撞检测的核心函数：
- 将输入的形状分别强制转换为CompoundShape和ConvexShape类型
- 获取复合形状和凸形状的世界变换信息
- 执行空间查询优化：将凸形状的AABB包围盒变换到复合形状的本地坐标系中
- 调用复合形状的getIntersectShapes()方法，快速筛选出可能与凸形状相交的子形状索引
- 遍历筛选出的相交子形状，对每个子形状与凸形状进行精确碰撞检测
- 通过NarrowPhaseDispatcher分派合适的碰撞算法来处理子形状与凸形状的具体碰撞
- 累积所有子碰撞的结果到输出参数中
- 返回检测到的碰撞接触点总数

该算法通过BVH或类似的空间数据结构实现高效的空间剪枝，显著提升了复合形状与凸形状碰撞检测的性能。

ConstraintUtils.cpp

该文件实现了物理引擎中约束系统的核心工具类，包含约束方程（ConstraintEquation）和约束元组（ConstraintTuple）两个主要类。这些类用于处理刚体间的各种约束关系，如关节、碰撞约束等，是物理仿真中约束求解器的基础组件。

ConstraintEquation 类函数

构造函数 ConstraintEquation()

初始化约束方程对象
设置默认参数：激活状态为true，模式为LOCK（锁定模式）
初始化拉格朗日乘数、偏置、对角线元素等为0

violation(Real mc_i)

计算约束违反程度
对于单向约束（UNILATERAL），如果mc_i > 0则返回0（无违反）
对于其他约束类型直接返回mc_i值

defaultProject()

执行默认的约束投影操作
对于单向约束，确保拉格朗日乘数l_i不为负数
用于约束求解过程中的投影步骤

ConstraintTuple 类函数

构造函数 ConstraintTuple()

初始化约束元组对象
将刚体指针设为空，雅可比矩阵Cq和辅助矩阵Eq设为零

updateAuxiliary(Real &g_i)

更新约束的辅助变量
计算Eq矩阵：前3个元素为质量逆矩阵乘以雅可比矩阵的线性部分
后3个元素为惯性逆矩阵乘以雅可比矩阵的角度部分
更新对角线元素g_i（用于约束求解的系数矩阵）

computeJacobianTimesState()

计算雅可比矩阵与当前状态向量的点积
状态向量包含线性速度和局部坐标系下的角速度
返回J·v的计算结果（约束速度）

incrementState(Real deltal)

根据约束求解结果增量更新刚体状态
更新线性速度：v += Eq_linear * deltal
更新角速度：ω += R * Eq_angular * deltal（R为旋转矩阵）
用于约束求解迭代过程中的状态更新

ConvexConvexCollisionAlgorithm.cpp

该文件实现了凸形状之间的碰撞检测算法类 ConvexConvexCollisionAlgorithm。这是物理引擎中窄相位碰撞检测系统的核心组件，专门用于处理两个凸几何形状（如球体、盒子、胶囊等）之间的精确碰撞检测和穿透深度计算。该算法使用EPA（Expanding Polytope Algorithm）来计算碰撞细节。

所有函数大致作用介绍

processCollision(const Ref<ShapeBase> &shape0, const Ref<ShapeBase> &shape1, const Coordsys &trans0, const Coordsys &trans1, CollisionResult &resultOut)

处理两个凸形状之间的碰撞检测主函数
参数说明：
- shape0, shape1：两个待检测的形状对象
- trans0, trans1：两个形状的世界坐标变换
- resultOut：输出碰撞结果的引用
将输入的基础形状转换为凸形状类型
创建EPA算法实例进行穿透深度计算
使用EPA算法的GetPenetrationDepth方法检测是否相交
如果发生碰撞：
- 计算碰撞法向量（取v1的负方向并归一化）
- 计算接触点位置（考虑形状边距margin）
- 计算穿透深度（考虑两个形状的边距）
- 将碰撞信息添加到结果中
返回值：1表示有碰撞，0表示无碰撞

该算法是物理引擎中处理凸形状碰撞的标准方法，EPA算法能够提供准确的穿透深度和接触信息，这对于碰撞响应和约束求解至关重要，为宽相位碰撞检测提供高效的空间划分基础。

CylinderBoxCollisionAlgorithm.cpp

实现圆柱体(Cylinder)与盒子(Box)之间的碰撞检测算法

函数列表：

PointInsideBox() – 检测点是否在盒子内部
addContactPoint() – 添加盒子与点的碰撞接触点(两个重载版本)
FindClosestBoxFace() – 找到离给定位置最近的盒子面
IntersectLinePlane() – 计算直线与平面的交点
IntersectSegmentBox() – 检测线段与盒子的相交
IntersectSegmentCylinder() – 检测线段与圆柱体的相交
processCollision() – 主处理函数，执行圆柱体与盒子的碰撞检测

主要算法流程：

将圆柱体转换到盒子坐标系
检查圆柱体侧面与盒子面的碰撞
检查盒子边与圆柱体的碰撞
生成碰撞接触点信息

Driveline.cpp

============================

实现轮式车辆传动系统的扭矩分配逻辑（模拟限滑差速器）

核心功能：
- 根据车轮转速差动态分配扭矩
- 模拟Torsten限滑差速器特性
- 支持可配置的最大扭矩偏置比

函数列表：
1. Driveline() - 构造函数，初始化最大扭矩偏置比
2. applyTorque() - 主逻辑函数，执行扭矩分配算法

算法流程：
1. 获取左右车轮转速差
2. 根据转速差计算动态扭矩偏置比：
   - 转速差≤0.25时，偏置比为1:1（完全开放差速器）
   - 转速差≥0.5时，采用最大偏置比(mMaxBias:1)
   - 中间状态线性过渡
3. 将输入扭矩分为快慢两部分：
   - 慢轮获得较大扭矩(alpha)
   - 快轮获得较小扭矩(1-alpha)
4. 根据实际转速确定左右轮的快慢状态
5. 将计算后的扭矩施加到对应车轮

技术特点：
- 使用四段式线性模型模拟限滑特性
- 扭矩分配考虑车轮世界坐标系旋转
- 支持多轮轴配置（通过Axle类管理）
============================

关键参数说明：

mMaxBias：最大扭矩偏置比（如设置为4表示最多4:1的扭矩分配）
diff：归一化的车轮转速差（|左速 – 右速|）
bias：动态计算的当前扭矩偏置比

典型应用场景：

// 创建传动系统（最大偏置比4:1）
auto driveline = CreateRef<Driveline>(4.0f); 

// 对某车轴施加1000Nm扭矩
driveline->applyTorque(axle, 1000.0f);

注意：实际实现中扭矩方向需要考虑车轮局部坐标系到世界坐标系的旋转变换（通过getWorldTransform()处理）

FrictionContactConstraint.cpp


实现带摩擦的接触约束求解器，采用顺序脉冲(Sequential Impulse)方法处理刚体碰撞

核心功能：
- 处理碰撞点法向和切向约束
- 支持静摩擦和动摩擦模型
- 实现Anitescu-Tasora投影算法
- 支持恢复系数(弹性)和穿透修正

关键参数：
- restitutionVelocityThreshold (0.1)：触发弹性碰撞的最小相对速度
- penetrationThreshold (0.3)：最大允许穿透深度
- kerp (0.2)：穿透修正系数

主要函数：
1. initSequentialImpulse() - 初始化约束信息
   - 计算法向和切向约束的有效质量
   - 设置约束RHS项
   - 处理弹性碰撞和穿透修正

2. iterateSequentialImpulse() - 执行顺序脉冲迭代
   - 计算法向冲量(考虑穿透恢复)
   - 计算切向冲量(基于库伦摩擦)
   - 应用冲量到刚体

3. computeJacobianForContactPart() - 计算接触约束雅可比矩阵
   - 构建法向(N)和切向(U,V)约束空间
   - 处理双刚体的相对运动

4. project() - 执行摩擦锥投影
   - 将解投影到摩擦锥约束空间
   - 处理静摩擦和动摩擦转换

技术特点：
- 采用有效质量(Effective Mass)简化计算
- 支持同时处理多个接触点
- 使用冲量-速度公式而非力-加速度
- 实现非穿透约束和摩擦约束的耦合求解
============================

[约束处理流程]
1. 初始化阶段：
   - 从碰撞结果提取接触点信息
   - 计算每个接触点的：
     * 相对位置向量(rA, rB)
     * 法向量和切平面
     * 有效质量项(nDenomInv, tDenomInv)

2. 迭代求解阶段：
   while (!converged) {
     - 计算当前相对速度
     - 求解法向冲量(考虑穿透和恢复)
     - 求解切向冲量(受法向力约束)
     - 应用冲量更新刚体速度
   }

3. 投影阶段：
   - 确保解满足摩擦锥约束
   - 处理静/动摩擦转换
   - 维持非穿透条件

[关键算法细节]
* 法向约束处理：
  λₙ = max(0, (Δv + e·v⁻)/nDenomInv)

* 切向约束处理：
  λₜ ∈ [-μλₙ, +μλₙ]

* 有效质量计算：
  1/M_eff = 1/m₁ + 1/m₂ + (r₁×n)ᵀI₁⁻¹(r₁×n) + (r₂×n)ᵀI₂⁻¹(r₂×n)

* Anitescu-Tasora投影：
  if (fₜ² > (μfₙ)²) {
    fₙ' = (fₜ/μ + fₙ)/(1 + μ²)
    fₜ' = μfₙ'
  }
============================

补充说明：

该实现特别适合处理堆叠物体和滑动摩擦等复杂接触情况
kerp参数控制穿透修正的刚度，值越大修正越激进
摩擦系数通过sqrt转换实现更平滑的静动摩擦过渡
约束雅可比矩阵的计算考虑了刚体的旋转效应
支持运动学刚体(kinematic)的特殊处理

GenericLinkConstraint.cpp

实现通用刚体链接约束，支持6自由度(D6)的位姿约束

核心功能：

连接两个刚体并约束其相对运动
支持独立控制各平移/旋转自由度的约束状态
采用顺序脉冲法求解约束
提供完整的雅可比矩阵计算

关键参数：

kerp (0.2)：位置修正刚度系数
damping (1.0)：速度阻尼系数
mFrameInA/mFrameInB：两刚体的局部连接坐标系

主要函数：

setLimit() – 设置各自由度约束状态

控制X/Y/Z平移和旋转的约束开关
初始化约束方程数组

calculateTransforms() – 计算相对位姿

处理欧拉角万向锁问题
计算质量加权因子(mFactA/B)

initSequentialImpulse() – 初始化约束求解

计算有效质量(m_jacDiagABInv)
设置约束RHS项(含弹性修正)

iterateSequentialImpulse() – 执行约束迭代

计算冲量增量并更新刚体速度

UpdateState() – 更新约束状态

计算完整雅可比矩阵(Cq1/Cq2)
处理旋转导数的四元数转换

技术特点：

支持混合约束模式(双边/单边)
使用四元数避免旋转奇异性
考虑静态刚体的特殊处理
提供约束力反馈(react向量)

[约束坐标系体系]
BodyA BodyB
┌───────┐ ┌───────┐
│ FrameInA │ FrameInB
│ (mA) │ (mB)
└───────┘ └───────┘
│ │
│ relative transform │
└───────────────┬───────────────┘
↓
relM / relM_dt

[核心算法流程]

初始化阶段：

设置约束自由度(setLimit)
计算初始相对位姿(calculateTransforms)

每帧更新：

更新标记点坐标(UpdateRelMarkerCoords)
计算约束状态(UpdateState)
转换雅可比矩阵(UpdateCqw)

约束求解：

计算有效质量(initSequentialImpulse)
迭代求解速度约束(iterateSequentialImpulse)

[雅可比矩阵结构]
Cq1 (对Body1的导数) = [ CqxT | -CqxT·R1·P1*·G1 ]
[ 0 | S(q2⁻¹q1) ]

Cq2 (对Body2的导数) = [ -CqxT | CqxT·R2·Q2·G2 + Aqᵀ·(PQw)·G2 ]
[ 0 | S(q1⁻¹q2) ]
其中：

CqxT = Aqᵀ·Ao2ᵀ
P1/Q2 为位置向量的叉乘矩阵
G1/G2 为四元数转换矩阵
S() 为四元数星型矩阵

[顺序脉冲求解]
λ = (J·M⁻¹·Jᵀ)⁻¹·(β·C/Δt + J·v)
其中：

β = kerp/Δt (位置修正增益)
C 为约束误差
– v 为相对速度

补充说明：

该约束可用于实现各种机械连接，如：

铰链关节(约束5个自由度)
滑块关节(约束5个自由度)
球窝关节(约束3个自由度)

特殊处理包括：

欧拉角一致性修正(防止万向锁跳变)
静态刚体的质量加权优化
四元数导数的高效计算

性能优化点：

使用SIMD加速矩阵运算
稀疏雅可比矩阵存储
热启动冲量缓存

示例用法：
“`cpp
// 创建两个刚体的通用链接约束
auto constraint = CreateRef(bodyA, bodyB, frameA, frameB);

// 设置为滑块关节(只允许X轴平移)
constraint->setLimit(true, false, false, // X平移自由，Y/Z固定
true, true, true); // 所有旋转固定

`NarrowPhaseDispatcher`

功能

处理刚体碰撞的“窄相位碰撞检测”，根据物体形状类型调用具体算法进行精确碰撞计算。

主要结构

构造函数

NarrowPhaseDispatcher();    // 初始化
~NarrowPhaseDispatcher();   // 自动清理缓存

主要函数：

int calculateCollisionResults(const ObjectArray<BroadphaseOverlappingPair>& overlappingPairs);

输入：来自粗阶段的重叠物体对
并行处理每对物体
调用 dispatch(...) 判定具体碰撞类型
输出：发生真实碰撞的结果列表

派发器核心：

dispatch(shape0, shape1, trans0, trans1, resultOut);

根据 ShapeFlag 判断形状组合
调用对应的 xxxCollisionAlgorithm 执行实际碰撞计算
如：
- BoxBoxCollisionAlgorithm
- SphereSphereCollisionAlgorithm
- ConvexConvexCollisionAlgorithm 等

支持的碰撞类型示意

类型组合	算法
Box vs Box	`BoxBoxCollisionAlgorithm`
Sphere vs Sphere	`SphereSphereCollisionAlgorithm`
Sphere vs Box	`SphereBoxCollisionAlgorithm`
Sphere vs Cylinder	`SphereCylinderCollisionAlgorithm`
Convex vs Convex	`ConvexConvexCollisionAlgorithm`
Compound vs Convex	`CompoundConvexCollisionAlgorithm`
Compound vs Compound	`CompoundCompoundCollisionAlgorithm`

Cylinder vs Box 被注释掉，标注有 @bug

特别说明

使用线程池 ThreadPool::parallelFor 并行处理碰撞对
自动处理需要交换物体顺序的情况（setSwapFlag）
支持动态拓展更多碰撞算法

`PSORSolver` 简略笔记

功能

基于 投影超松弛迭代（PSOR）法 解决刚体约束系统中的拉格朗日乘子，进而计算约束反力（例如：碰撞、摩擦等）。

参数简述

参数名	含义
`mIteration`	最大迭代次数（默认 200）
`mOmega`	松弛因子，控制迭代步长
`mShlambda`	平滑系数（1 表示无平滑）
`mTolerance`	收敛容差，控制退出条件

主函数：`solve(dt, rigidBodies, constraints)`

初始化阶段
- 每个约束准备辅助变量（如舒尔补 g_i、刚度 Cq*invM*CqT）
- 将复合摩擦约束分组平均化 g_i
主循环（迭代求解）
对所有约束方程进行迭代：
- 计算残差 r = J*v + b + C*λ
- 更新 λ（拉格朗日乘子）
- 投影 λ 到合法范围（如不可为负）
- 应用平滑（如果启用）
- 更新物体状态（通过 incrementState(...)）
特殊处理：摩擦约束是三元组
- N、U、V 三个方向约束一起处理
- 平滑与投影合并处理
- 维护 lambda_friction[3] 更新幅度

关键类/结构

ConstraintBase：约束基类
ConstraintEquation：线性化约束项（一个 λ）
incrementState(...)：把 λ 转化为速度影响
mProjectFunc()：将 λ 投影到物理允许区（比如只能推动不能拉）

支持热启动代码已注释（即可以从上一步结果继续迭代）
摩擦项成组处理，必须正确配对
投影和平滑是收敛与稳定的关键机制

注意事项

`RenderBody3D` 简略笔记

功能

将刚体 (BodyBase) 对象转化为可渲染的三角网格数据（顶点 + 面），用于 3D 可视化（如 OpenGL 或 GUI 显示）。

构造函数：`RenderBody3D(const Ref<BodyBase>&)`

获取刚体的形状 ShapeBase
如果是复合体（CompoundShape）：
- 遍历子形状
- 分别生成网格，应用局部变换
- 合并所有 mesh
如果是简单形状：
- 直接生成默认网格

关键函数

`getVertices() → Eigen::MatrixXd`

返回世界坐标系下的所有顶点
应用物体的旋转和平移变换

`setDefalutMesh(...)`

根据物体的形状类型生成对应的默认三角网格：

形状类型	网格来源
Box	`BoxMesh`
Sphere	`IcosphereMesh`
Cylinder	`CylinderMesh`
Capsule	Cylinder + 上下球
Triangle	3 顶点组成的面

内部数据结构

成员名	作用
`mVertices`	所有顶点（局部）
`mFaces`	面索引（三角形）
`mBodyBase`	关联的物理刚体

顶点转换流程

当调用 getVertices() 时，执行变换流程：

局部坐标 → 世界坐标
v_world = R^T * v_local + position

注意事项

复合体形状逐个子形状累加网格
所有 mesh 数据都来自 DefaultMesh（预设模型）
Capsule 由 Cylinder + 两个 Sphere 拼接生成

这类渲染模块通常只用于可视化展示，不参与物理计算。

这是 SequentialImpulseConstraintSolver 的简要解析笔记：

`SequentialImpulseConstraintSolver` 简略笔记

📌 功能

实现经典的Sequential Impulse（逐次冲量）方法，用于刚体动力学中求解约束反力（如碰撞和关节）。

结构简述

构造函数

SequentialImpulseConstraintSolver() {
    mIteration = 10;  // 默认迭代次数
}

主函数

void solve(Real dt, const ObjectArray<Ref<RigidBody>>& rigidBodies,
           const ObjectArray<Ref<ConstraintBase>>& constraints)

解算流程

初始化冲量约束 constraint->initSequentialImpulse(dt); 每个约束根据当前状态准备迭代所需的数据。
逐次迭代冲量 for (int iter = 0; iter < mIteration; ++iter) { for (auto &constraint : constraints) { constraint->iterateSequentialImpulse(); } } 多轮迭代（类似高斯-赛德尔），每次更新一个约束的冲量，立即影响刚体状态。

注：注释中提到并行执行 iterateSequentialImpulse() 是不正确的（依赖前一步的刚体状态），虽然视觉效果“看起来”没问题。

依赖接口（由约束类实现）

ConstraintBase::initSequentialImpulse(dt)
初始化阶段计算系数、偏差等
ConstraintBase::iterateSequentialImpulse()
实际冲量计算与应用阶段

特点与应用

更快、更简单、适合实时仿真
通常用于游戏引擎中的稳定刚体求解器（如 Bullet）

`SimulatorGUI` 简略笔记

功能概览

这是一个用于图形化展示和控制物理仿真的GUI 控制器类，基于 libigl + ImGui 搭建。
提供：

动画展示
控制面板（暂停、单步、重置）
Debug 碰撞点可视化

类结构核心

成员变量	说明
`mViewer`	`libigl::Viewer` 实例
`mSimThread`	后台仿真线程
`mIsPaused`	是否暂停仿真
`mTime`, `mFrame`	仿真时间与帧数
`mRenders`	每个刚体对应的渲染器（`RenderBody3D`）
`mColorMap`	物体状态的颜色映射（静止、运动、控制）

启动流程

`run()`

初始化物理世界 (init())
创建刚体网格 (initMeshes())
启动 Viewer 渲染 (mViewer.launch())

图形显示：`render()`

对所有物体调用：
- getVertices()：变换后的顶点
- getFaces()：三角面索引
- 根据物体状态着色（如：静止蓝色）
Debug 可视化：
- 显示碰撞点：红色为点 A，蓝色为点 B

🎮 用户交互

`onGUI()`

通过 ImGui 绘制菜单：

显示帧数、FPS、时间等统计信息
控制仿真状态（运行/暂停/单步/重置）
设置时间步长 dt

`onKeyPressed()`

空格键 → 切换暂停/播放

仿真线程：`runSimThread()`

后台线程不断执行 step()，前提是未暂停。

`step()` 执行流程：

update(mDt);                  // 执行物理模拟一步
统计时间与帧率；
std::this_thread::sleep_for(); // 控制仿真速度

初始化渲染模型：`initMeshes()`

遍历 RigidWorld 中的刚体
为每个刚体构造 RenderBody3D 并建立 Viewer 网格

重置功能：`reset()`

停止线程
重置统计值
清空世界，重新初始化仿真与 mesh
重新创建仿真线程

技术栈概览

依赖库	用途
`libigl`	网格渲染 + Viewer 控制
`ImGui`	GUI 控制面板
`Eigen`	矩阵、向量操作
`std::thread`	后台仿真执行线程

`SphereBoxCollisionAlgorithm` 简要解析

功能概览

该类负责处理球体（Sphere）与盒子（Box）之间的碰撞检测与求解。核心逻辑是：

将球中心转换到盒子局部空间
找到球中心到盒子最近表面的点
根据球半径与盒子 margin 判断是否发生碰撞
若碰撞，计算法线、穿透深度、接触点，并填入 CollisionResult

🔧 核心函数解析

`getSphereDistance(...)`

判断球体是否与盒子发生接触，并计算接触信息

输入：

sphereCenter：球心（世界坐标系）
fRadius：球体总半径（包含 margin）
boxShape 和 transBox：盒子形状与变换信息

输出：

pointOnBox：接触点（在盒子表面）
normal：接触法线（从盒子指向球）
penetrationDepth：穿透深度（负值表示重叠）

主要步骤：

把球心变换到盒子局部坐标系 sphereRelPos
找出球心在盒子内部/外部的最接近点 closestPoint
判断距离平方 dist2 是否小于 (r + margin)^2：若否，没碰撞
若球心在盒子内，则调用 getSpherePenetration() 获取最浅穿透方向
否则，计算穿透深度与法线

`getSpherePenetration(...)`

在球心已在盒子内部的情况下，找出最浅的穿透面、法线与穿透深度

逻辑是逐个比较球心距离 6 个面的位置，找到最近的面方向作为法线。

`processCollision(...)`

调度上述两个函数，若有碰撞则写入 resultOut

类型转换成具体 SphereShape / BoxShape
调用 getSphereDistance()
若发生碰撞，添加接触点信息到 resultOut

resultOut.addCollisionPoint(
    法线, 
    接触点（去掉 box margin）, 
    穿透深度（含 sphere & box margin）
);

输入输出样例说明（示意）

输入：
- 球半径 r=1，位置在盒子边缘附近
- 盒子边长 2x2x2，位于原点
输出：
- 是否碰撞
- 接触点（在盒子边缘）
- 法线（从盒子指向球）
- 穿透深度：距离 - (r + margin)

特点总结

特性	描述
支持 margin	可避免零穿透误差问题
处理内部/外部两种情况	用不同策略求法线
转换到局部空间计算	简化几何判断逻辑

`SphereCylinderCollisionAlgorithm` 结构解析

功能概览

该类负责处理球体（Sphere）与圆柱体（Cylinder）之间的窄相位碰撞检测，根据球中心相对于圆柱位置，分为 三种情况（Case A, B, C）判断是否接触并记录碰撞点。

`processCollision(...)` 核心逻辑

输入：

shape0: 球体（SphereShape）
shape1: 圆柱体（CylinderShape）
trans0, trans1: 各自的世界坐标变换
resultOut: 输出碰撞点容器

📐 局部变量含义

变量	说明
`diff`	球心相对于圆柱局部空间的位置
`r1`	`diff` 的水平（x-z）投影
`r1_len`	水平投影长度（球心到圆柱轴线的距离）
`y1`	球心在圆柱局部空间的 y 坐标
`H1`	圆柱体的半高 + margin
`radius0`, `radius1`	球体/圆柱体的半径，均含 margin

三种碰撞情况判断

Case A（球心在柱体高度范围内）

if (y1 ∈ [-H1, H1])

说明：球心在柱体中间部分 → 柱面碰撞

判断：水平距离是否小于 r0 + r1
正常求圆周点法线与深度
法线方向：从柱体指向球
接触点在柱面上

Case B（球心在柱顶/底外侧）

if (r1_len > radius1 && y1 超出上下界)

说明：球心靠近圆柱边缘的侧边 → 球面和柱圆边缘的角点碰撞

碰撞点为柱顶/底的边缘
法线方向为两点之间向量归一化
深度为球心与边缘点距离 – 球半径

Case C（球心靠近柱顶/底正上方/下方）

else // r1_len <= radius1 且 y1 超出上下界

说明：球靠近圆柱顶面或底面中央 → 柱盖碰撞

法线方向沿柱体 y 轴方向
接触点在柱顶/底正中心附近

最后

统一将结果通过：

resultOut.addCollisionPoint(normalOnSurfaceB, pos1 - normalOnSurfaceB * cylinderShape->getMargin(), ...);

记录到 CollisionResult 中。

特点总结

特性	描述
分三种区域处理	准确匹配不同几何部位（侧面、边缘、盖面）
使用局部空间判断	降低计算复杂度
法线和穿透深度处理完备	兼顾几何正确性和数值鲁棒性

Vector3 SphereShape::localGetSupportingVertex(const Vector3 &position, const Real &tolerance) const {
Real r = mRadius + tolerance;
Real l = position.norm();
return position * (r / l);
}

`SphereSphereCollisionAlgorithm`

核心逻辑

获取球心差向量与距离 Vector3 diff = trans0.getPosition() - trans1.getPosition(); Real len = diff.norm(); 计算两个球体中心点的位置差和距离。
计算两个球体的“有效半径” Real radius0 = sphere0->getRadius() + sphere0->getMargin(); Real radius1 = sphere1->getRadius() + sphere1->getMargin(); 这里半径加上了各自的碰撞边界余量（margin），用于更稳定的碰撞检测。
判断是否碰撞 if (len > (radius0 + radius1)) return 0; 如果两球心距离大于两球有效半径之和，说明没有碰撞，直接返回。
计算穿透深度（负值表示穿透） Real dist = len - (radius0 + radius1);
计算碰撞法线 Vector3 normalOnSurfaceB(1, 0, 0); if (len > REAL_EPSILON) { normalOnSurfaceB = diff / len; } 如果两个球中心点重合，使用默认法线 (1,0,0)，否则法线指向从球1指向球0的方向。
计算碰撞点 Vector3 pos1 = trans1.getPosition() + radius1 * normalOnSurfaceB; 碰撞点位于球1表面，沿法线方向偏移半径。
添加碰撞点 resultOut.addCollisionPoint(normalOnSurfaceB, pos1 - normalOnSurfaceB * sphere1->getMargin(), dist + sphere0->getMargin() + sphere1->getMargin());

总结

该算法用简单的几何距离判断两球碰撞。
处理了碰撞边界余量以提高稳定性。
支持穿透深度的计算。
返回碰撞点与法线，供后续物理响应使用。

Suspension 车辆悬挂系统类 — 简要解析

功能概述：
该类用于模拟车辆悬挂系统中弹簧和阻尼的力学行为，将车轮和车体通过弹簧-阻尼系统连接，实现悬挂效果。

主要成员变量

mChassis：车体对象引用
mWheel：车轮对象引用
mSpringStiffness：弹簧刚度系数
mSpringDamping：弹簧阻尼系数
mIsIdler：布尔值，标记是否是惰轮（非驱动轮，不参与动力传递）
mWheelRelLoc：车轮相对车体位置（初始化时获取）

核心方法

`update(Real dt)`

条件判断： 忽略惰轮（mIsIdler == false 时才计算）
计算弹簧变形向量：
dir = 当前车轮位置 - 车体乘以初始相对车轮位置
代表弹簧拉伸或压缩的方向和长度。
弹簧力计算：
force = - stiffness * dir （弹簧力与变形成反比，方向相反）
获取弹簧相对车体位置的速度 velSpring 和车轮速度 velSpindle
计算阻尼力和弹簧力的合力，并分别对车轮和车体施加冲量
- 车轮加：force * dt + damping * (velSpring - velSpindle)
- 车体加：相反方向的力（遵循牛顿第三定律）

总结

该类模拟了悬挂弹簧的弹性恢复力和阻尼阻力，动态调整车轮和车体的受力，保持悬挂系统的稳定和现实感。

TerrainMaterial 地形材质类 — 简要解析

功能：
管理地形表面的摩擦系数和弹性（恢复系数）数据，基于多张“权重贴图”计算材质参数，支持地形不同区域材质表现差异。

核心流程

构造函数加载权重贴图
- 输入：地形包围盒（AABBBox）和多个权重贴图（WeightMap数组）
- 使用 stbi_load 加载每张权重贴图的像素数据
- 记录每张贴图的宽、高、通道数，以及地形的最大宽高
- 如果加载失败，抛出异常
计算地形材质参数
- 对地形覆盖区域的每个像素点，计算该点摩擦力和弹性：
- 遍历所有权重贴图，根据贴图对应位置的灰度值（0~~255转为0~~1浮点数），乘以各自的摩擦权重和弹性权重
- 累加所有贴图贡献，并根据权重归一化，得到该点最终的摩擦力和弹性值
析构函数
- 释放动态分配的摩擦力和弹性数组内存

材质查询接口

getFriction(const Vector3 &pos)
根据给定世界坐标pos，计算其在地形纹理中的UV坐标，返回对应点的摩擦力数据。
getRestitution(const Vector3 &pos)
同理，返回对应点的弹性恢复系数。

Terrain 地形类 — 简要解析

成员变量：
- mTerrainFile：地形模型文件路径
- mSize：地形尺寸（3D尺寸）
构造函数：
- 标记地形为运动学刚体（Kinematic，不受力学影响）
- 使用地形文件和尺寸创建网格碰撞体（MeshShape）
- 设置质量（10000，较大数值以固定地形）
- 创建对应的地形材质实例，用于摩擦和弹性计算

总结

该模块负责加载和管理地形物理属性，允许基于多种权重贴图定义地面不同区域的摩擦和弹性差异，从而实现更真实的地面交互效果。地形本身作为运动学刚体存在，既有碰撞检测能力，又有丰富的材质响应。

下面是你给出的 TrackAssembly 及相关轨迹类代码的简略解析笔记：

TrackAssembly 和轨迹类简要解析

核心功能

该模块模拟履带车辆的履带系统，构建履带轨迹，管理履带节（Track Shoe）和轮子之间的连接、运动与约束。

主要类和职责

1. `Trajectory`（轨迹基类）

表示一段路径区间 [minT, maxT]
提供接口：
- getPos(t)：给定参数t，返回轨迹上的位置（Vector3）
- getRot(t)：给定t，返回轨迹上对应的旋转（Quaternion）

2. `TrackLine`（轨迹直线段）

继承自Trajectory
以起点start和角度angle定义直线段轨迹
计算沿直线的点位置和旋转

3. `TrackArc`（轨迹弧线段）

继承自Trajectory
以圆心center、半径radius和起始角度startAngle定义弧线段轨迹
计算弧线上的点位置和旋转

`TrackAssembly`（履带组件）

关键成员

mChassis：车辆底盘
mWheels：履带轮子集合
mShoes：履带节（履带板）集合
mLinks：连接履带节与底盘、履带节间的约束
mSuspensions：悬挂系统列表
mShoeLength，mShoeThickness，mShoeMass：履带节尺寸与质量参数

关键方法

`injectWorld`

将所有轮子、履带节及约束链接注入世界刚体和约束列表，供物理系统管理

`initShoes`

根据轮子位置初始化履带轨迹和履带节
计算轮子之间的角度和轨迹区间（用二维几何计算轮子位置和角度关系）
构建由直线段(TrackLine)和弧线段(TrackArc)组成的履带轨迹
沿轨迹均匀布置履带节，调用addShoe创建
生成履带节之间的约束，保证履带闭环和运动稳定

`addWheel`

向组件添加轮子，并创建悬挂系统和与底盘的连接约束
确保悬挂方向垂直

`setWorldTransform`

批量设置轮子和履带节的世界坐标系（位置和旋转）

`getSpeed`

计算所有轮子的平均角速度，转化为速度值

`update`

更新所有悬挂系统的物理状态（弹簧、阻尼力）

设计亮点

轨迹由多段弧线与直线拼接组成，精确模拟履带绕轮子运动路径
通过二维几何计算和角度关系，动态构建轨迹段
利用多约束系统连接履带节与底盘及彼此，保证物理稳定性
支持悬挂物理反馈，提升模拟真实感

TrackedVehicle（履带式车辆）类简要解析

作用

管理整个履带车辆，包括底盘（chassis）、履带组件（TrackAssembly）、动力传输（engine & transmission）、操控输入（油门、转向、刹车）等，整合各部分完成物理模拟和控制。

主要成员

mChassis：车辆底盘刚体
mTracks：多个履带组件（TrackAssembly）
mEngine：发动机模型（提供扭矩输出）
mTransmission：变速箱模型（提供档位比）
mThrottleValue：油门输入值
mSteeringValue：转向输入值
mBrakeValue：刹车输入值
mBrake：刹车控制模块

关键方法

1. `injectWorld`

将底盘和所有履带组件注入物理世界刚体和约束集合
方便物理引擎统一管理

2. `setWorldTransform`

设置车辆及所有履带组件的世界坐标系（位置与旋转）
使车辆及履带位置保持同步

3. `update(dt)`

油门与转向控制
- 对每条履带计算当前轮速rpm
- 根据变速箱档位比计算实际扭矩
- 根据转向值调整左右履带扭矩方向（左右履带反向转动实现转向）
- 将扭矩施加到每个轮子上，驱动轮子旋转
刹车控制
- 对所有履带上的轮子应用刹车力，减速
履带组件物理更新
- 调用所有TrackAssembly的update，更新悬挂、履带节等物理状态

设计亮点

通过油门和转向输入，直接计算并作用扭矩，模拟驱动力和转向
左右履带扭矩符号相反实现差速转向（履带车经典转向方式）
结构清晰，分层调用，方便扩展和维护

VehicleEngine（车辆引擎）类简要解析

作用

模拟车辆引擎的扭矩输出，通过给定的转速-扭矩曲线（torque curve）计算对应转速下的扭矩值。

主要成员变量

mTorqueCurve：一个转速-扭矩点（TNPoint）的数组，表示引擎的扭矩曲线数据点。
- TNPoint 结构包含：
  - N：转速（rpm）
  - T：对应转速下的扭矩

关键方法

1. 构造函数 `VehicleEngine(const ObjectArray<TNPoint> &torqueCurve)`

初始化引擎扭矩曲线，存储一组转速-扭矩点

2. `getTorque(Real rpm) const`

根据输入的转速 rpm 计算对应的扭矩输出
逻辑：
- 若 rpm 低于曲线最小转速，返回曲线起点扭矩
- 若 rpm 高于曲线最大转速，返回曲线终点扭矩
- 否则，使用二分查找找到曲线上紧邻的两个点
- 在这两个点之间线性插值计算扭矩值，保证扭矩输出连续平滑

设计亮点

使用标准库的 std::lower_bound 快速定位插值区间，效率高
支持任意形状的扭矩曲线，只需提供离散点数据即可
线性插值简单有效，适合实时计算

VehicleTransmission（车辆变速箱）类简要解析

作用

管理车辆的档位和齿轮比，支持手动和自动变速，通过当前发动机转速调整档位并返回当前齿轮比。

主要成员变量

mGearRatios：齿轮比数组，对应每个档位的传动比。
mCurrentGear：当前档位索引，初始为 -1（空档）。
mType：变速箱类型，支持手动（MANUAL）和自动（AUTOMATIC）。
mShiftPoints：换挡点数组，定义自动换挡时的转速阈值（上下换挡临界转速区间）。

关键方法

构造函数 `VehicleTransmission(ObjectArray<Real> gearRatios, ObjectArray<ShiftPoint> shiftPoints)`

初始化齿轮比、换挡点，默认设为手动档。

`getGearRatio(Real rpm)`

传入当前发动机转速 rpm，返回当前档位的齿轮比。
自动变速逻辑（如果是自动档且档位有效）：
- 如果转速超过当前档位上换挡转速，自动升档。
- 如果转速低于当前档位下换挡转速，自动降档。
返回当前档位对应的齿轮比，若档位无效则返回0。

设计亮点

支持自动和手动两种变速方式。
自动换挡根据转速区间灵活调整档位。
简洁易扩展的接口，方便与引擎和动力系统衔接。

Wheel（车轮）类简要解析

作用

表示车辆的车轮，定义车轮的几何形状、质量、局部和世界坐标变换。

主要成员变量

mRadius：车轮半径。
mWidth：车轮宽度。
mLocalTransform：车轮相对于车辆的局部坐标变换（位置+旋转）。
mWorldTransform：车轮的世界坐标变换（位置+旋转）。
mShapeBase：车轮的碰撞形状，这里用的是圆柱体（CylinderShape）。

关键方法

构造函数 `Wheel(const Coordsys &localCoordsys, Real wheelRadius, Real wheelWidth, Real wheelMass)`

初始化车轮的局部变换、半径、宽度。
创建对应的圆柱形状作为车轮的碰撞形状。
设置车轮质量。
初始化世界变换为局部变换。

`setWorldTransform(const Coordsys &coordsys)`

根据传入的世界坐标变换，结合局部变换计算车轮的最终世界变换。
作用是让车轮随车辆整体变换同步移动旋转。

设计亮点

使用 CylinderShape 来表示车轮的物理形状，方便与物理引擎交互。
通过局部与世界坐标变换的组合，便于管理车轮相对于车辆的定位和车辆整体移动。

WheeledVehicle（轮式车辆）类简要解析

作用

表示带轮轴（Axle）和轮子的车辆，负责车辆动力学和控制（油门、转向、刹车）更新，管理底盘和轮轴物理刚体及连接。

主要成员

mChassis：车辆底盘刚体。
mAxles：车辆所有轮轴集合，每个轮轴包含轮子和悬挂等。
mEngine：车辆发动机，负责输出转矩。
mTransmission：变速箱，提供档位和传动比。
mDriveline：传动系统，负责将发动机扭矩传递到轮轴。
mBrake：刹车系统。
mThrottleValue、mSteeringValue、mBrakeValue：输入控制值（油门、方向盘、刹车）。

关键方法

`injectWorld`

将车辆底盘和所有轮轴的刚体及约束加入物理世界，方便物理仿真。

`setWorldTransform`

设置车辆的整体世界变换，更新底盘和所有轮轴的世界坐标。

`update`

油门控制：对每个受油门控制的轮轴，根据发动机扭矩和变速箱传动比计算扭矩，传递给驱动轮轴。
转向控制：对受转向控制的轮轴，计算各轮子的旋转和转向角度，调整轮子关节的约束以实现转向效果。
刹车控制：对所有轮子应用刹车扭矩。
更新轮轴：调用所有轮轴的更新函数，推动物理模拟。

设计要点

结合发动机、变速箱、传动系统实现真实动力传递。
转向部分用关节约束动态调整，实现灵活转向。
结构清晰，职责分明，便于扩展轮轴和车辆控制逻辑。

1. 物理引擎源码详解

MathUtils

BodyBase.cpp

AABBBox.cpp

BoxShape.cpp

CapsuleShape.cpp

CompoundShape.cpp

ConvexShape.cpp

CylinderShape.cpp

MeshShape 简略解析

SphereShape 类说明

功能

主要成员与方法

代码示例总结

TriangleShape（三角形形状）类

主要成员变量

关键方法

1. 构造函数 TriangleShape(Vector3 p1, Vector3 p2, Vector3 p3)

2. getInertia()

3. localGetSupportingVertex(const Vector3 &position, const Real &tolerance)

4. getVertex(int index)

设计亮点

BVH.cpp

Axle.cpp

Brake.cpp

BroadPhaseBase.cpp

BroadPhaseSimple.cpp

BroadPhaseSweepAndPrune.cpp

BroadPhaseUtils.cpp

CompoundCompoundCollisionAlgorithm.cpp

CompoundConvexCollisionAlgorithm.cpp

ConstraintUtils.cpp

ConstraintEquation 类函数

ConstraintTuple 类函数

ConvexConvexCollisionAlgorithm.cpp

所有函数大致作用介绍

CylinderBoxCollisionAlgorithm.cpp

Driveline.cpp

FrictionContactConstraint.cpp

GenericLinkConstraint.cpp

NarrowPhaseDispatcher

功能

主要结构

构造函数

主要函数：

派发器核心：

支持的碰撞类型示意

特别说明

PSORSolver 简略笔记

功能

参数简述

主函数：solve(dt, rigidBodies, constraints)

关键类/结构

注意事项

RenderBody3D 简略笔记

功能

构造函数：RenderBody3D(const Ref<BodyBase>&)

关键函数

getVertices() → Eigen::MatrixXd

setDefalutMesh(...)

内部数据结构

顶点转换流程

注意事项

SequentialImpulseConstraintSolver 简略笔记

📌 功能

结构简述

构造函数

主函数

解算流程

依赖接口（由约束类实现）

特点与应用

SimulatorGUI 简略笔记

功能概览

类结构核心

启动流程

run()

图形显示：render()

🎮 用户交互

onGUI()

onKeyPressed()

`MeshShape` 简略解析

`SphereShape` 类说明

1. 构造函数 `TriangleShape(Vector3 p1, Vector3 p2, Vector3 p3)`

2. `getInertia()`

3. `localGetSupportingVertex(const Vector3 &position, const Real &tolerance)`

4. `getVertex(int index)`

`NarrowPhaseDispatcher`

`PSORSolver` 简略笔记

主函数：`solve(dt, rigidBodies, constraints)`

`RenderBody3D` 简略笔记

构造函数：`RenderBody3D(const Ref<BodyBase>&)`

`getVertices() → Eigen::MatrixXd`

`setDefalutMesh(...)`

`SequentialImpulseConstraintSolver` 简略笔记

`SimulatorGUI` 简略笔记

`run()`

图形显示：`render()`

`onGUI()`

`onKeyPressed()`

仿真线程：`runSimThread()`

`step()` 执行流程：

初始化渲染模型：`initMeshes()`

重置功能：`reset()`

`SphereBoxCollisionAlgorithm` 简要解析

`getSphereDistance(...)`

`getSpherePenetration(...)`

`processCollision(...)`

`SphereCylinderCollisionAlgorithm` 结构解析

`processCollision(...)` 核心逻辑

`SphereSphereCollisionAlgorithm`

`update(Real dt)`

1. `Trajectory`（轨迹基类）

2. `TrackLine`（轨迹直线段）

3. `TrackArc`（轨迹弧线段）

`TrackAssembly`（履带组件）

`injectWorld`

`initShoes`

`addWheel`

`setWorldTransform`

`getSpeed`

`update`

1. `injectWorld`

2. `setWorldTransform`

3. `update(dt)`