012-碰撞及刚体动力学

碰撞及刚体动力学

物理系统是现代游戏引擎的核心组成部分，为游戏世界提供真实感和交互性。本章将深入探讨碰撞检测和刚体动力学的基本原理、实现方法以及如何将它们整合到游戏引擎中。

12.1 你想在游戏中加入物理吗？

12.1.1 物理系统的作用与价值

物理系统能够为游戏带来以下好处：

真实感：使游戏世界中的物体表现符合现实世界的物理规律
交互性：允许玩家以自然直观的方式与游戏世界互动
创造性：为玩家提供基于物理的创造性解决问题的机会
自动化行为：减少预先编写的动画需求，让物体自然地响应外力

然而，物理系统也带来了一些挑战：

性能开销：物理计算通常很耗费计算资源
复杂性：物理系统的实现和调试较为复杂
不可预测性：物理模拟可能导致不可预测的结果，影响游戏设计
调教难度：需要精细调节以平衡现实性和游戏性

12.1.2 物理引擎的核心组件

一个完整的物理引擎通常包含以下核心组件：

碰撞检测系统：检测物体之间的交叉和接触
刚体动力学系统：模拟物体的运动和旋转
约束求解器：处理物体之间的各种约束
材质系统：定义物体的物理属性，如摩擦力和弹性
积分器：推进物理世界的时间

下面是一个简单的物理引擎基础架构示例：

cpp

// 物理引擎基础架构
class PhysicsEngine {
public:
    PhysicsEngine();
    ~PhysicsEngine();
    
    // 初始化物理引擎
    bool Initialize();
    
    // 更新物理世界
    void Update(float deltaTime);
    
    // 创建刚体
    RigidBody* CreateRigidBody(const RigidBodyDesc& desc);
    
    // 创建碰撞形状
    CollisionShape* CreateShape(const ShapeDesc& desc);
    
    // 射线检测
    bool RayCast(const Ray& ray, RayCastResult& result);
    
    // 设置重力
    void SetGravity(const Vector3& gravity);
    
    // 获取重力
    Vector3 GetGravity() const;
    
    // 清理资源
    void Cleanup();
    
private:
    // 重力向量
    Vector3 gravity;
    
    // 刚体列表
    std::vector<RigidBody*> rigidBodies;
    
    // 碰撞形状列表
    std::vector<CollisionShape*> collisionShapes;
    
    // 碰撞检测系统
    CollisionSystem* collisionSystem;
    
    // 动力学求解器
    DynamicsSolver* dynamicsSolver;
    
    // 约束求解器
    ConstraintSolver* constraintSolver;
    
    // 物理世界的其他属性
    float fixedTimeStep;
    int maxSubSteps;
};

12.1.3 物理引擎的设计理念

设计一个高效的物理引擎需要考虑以下几点：

准确性与性能的权衡：物理模拟的准确性通常与性能成反比
离散时间步长：使用固定的时间步长可以提高稳定性
迭代求解方法：许多物理问题通过迭代方法而非直接求解
缓存友好的数据结构：设计结构时考虑CPU缓存和内存局部性
可调节的精度：根据不同物体的重要性调整模拟精度

下面是一个物理引擎更新循环的示例：

cpp

void PhysicsEngine::Update(float deltaTime) {
    // 使用固定时间步长进行子步更新
    float timeLeft = deltaTime;
    int subSteps = 0;
    
    while (timeLeft > 0.0f && subSteps < maxSubSteps) {
        float dt = std::min(timeLeft, fixedTimeStep);
        
        // 1. 更新所有刚体的状态
        for (auto body : rigidBodies) {
            if (body->IsActive()) {
                body->IntegrateForces(dt);
            }
        }
        
        // 2. 执行碰撞检测
        collisionSystem->DetectCollisions();
        
        // 3. 解决碰撞约束
        constraintSolver->SolveConstraints(dt);
        
        // 4. 更新所有刚体的位置
        for (auto body : rigidBodies) {
            if (body->IsActive()) {
                body->IntegrateVelocities(dt);
            }
        }
        
        timeLeft -= dt;
        subSteps++;
    }
}

12.2 碰撞/物理中间件

许多游戏开发者选择使用现有的物理中间件而非从头开发物理引擎。主流的物理中间件包括PhysX、Havok、Bullet Physics等。

12.2.1 为什么使用物理中间件？

使用物理中间件的主要优势：

减少开发时间：无需从头开发复杂的物理系统
优化的性能：商业中间件通常经过广泛优化，支持多线程和SIMD指令
跨平台支持：主流物理中间件支持多种平台
成熟的功能集：提供全面的物理功能，如碰撞检测、约束、布料和流体等
可靠性：经过大量商业游戏的验证和测试

12.2.2 物理中间件的集成

以下是将Bullet物理引擎集成到游戏引擎的示例：

cpp

// Bullet物理引擎包装器
class BulletPhysicsEngine : public IPhysicsEngine {
public:
    BulletPhysicsEngine() : 
        collisionConfiguration(nullptr),
        dispatcher(nullptr),
        broadphase(nullptr),
        solver(nullptr),
        dynamicsWorld(nullptr) {}
    
    ~BulletPhysicsEngine() {
        Cleanup();
    }
    
    bool Initialize() override {
        // 创建碰撞配置
        collisionConfiguration = new btDefaultCollisionConfiguration();
        
        // 创建碰撞调度器
        dispatcher = new btCollisionDispatcher(collisionConfiguration);
        
        // 创建宽阶段碰撞检测
        broadphase = new btDbvtBroadphase();
        
        // 创建约束求解器
        solver = new btSequentialImpulseConstraintSolver();
        
        // 创建动力学世界
        dynamicsWorld = new btDiscreteDynamicsWorld(
            dispatcher, broadphase, solver, collisionConfiguration);
        
        // 设置重力
        dynamicsWorld->setGravity(btVector3(0, -9.81f, 0));
        
        return true;
    }
    
    void Update(float deltaTime) override {
        if (dynamicsWorld) {
            dynamicsWorld->stepSimulation(deltaTime, 10);
        }
    }
    
    RigidBody* CreateRigidBody(const RigidBodyDesc& desc) override {
        // 创建Bullet刚体
        btCollisionShape* shape = FindOrCreateShape(desc.shapeDesc);
        
        btVector3 localInertia(0, 0, 0);
        if (desc.mass > 0.0f) {
            shape->calculateLocalInertia(desc.mass, localInertia);
        }
        
        btTransform transform;
        transform.setIdentity();
        transform.setOrigin(ConvertToBullet(desc.position));
        transform.setRotation(ConvertToBullet(desc.rotation));
        
        btDefaultMotionState* motionState = 
            new btDefaultMotionState(transform);
        
        btRigidBody::btRigidBodyConstructionInfo rbInfo(
            desc.mass, motionState, shape, localInertia);
        
        btRigidBody* bulletBody = new btRigidBody(rbInfo);
        dynamicsWorld->addRigidBody(bulletBody);
        
        // 创建我们的RigidBody包装器
        BulletRigidBody* body = new BulletRigidBody(bulletBody);
        rigidBodies.push_back(body);
        
        return body;
    }
    
    // 其他接口方法实现...
    
    void Cleanup() override {
        // 释放刚体
        for (auto body : rigidBodies) {
            dynamicsWorld->removeRigidBody(static_cast<BulletRigidBody*>(body)->GetBulletBody());
            delete body;
        }
        rigidBodies.clear();
        
        // 释放碰撞形状
        for (auto& pair : shapes) {
            delete pair.second;
        }
        shapes.clear();
        
        // 释放Bullet组件
        delete dynamicsWorld;
        delete solver;
        delete broadphase;
        delete dispatcher;
        delete collisionConfiguration;
        
        dynamicsWorld = nullptr;
        solver = nullptr;
        broadphase = nullptr;
        dispatcher = nullptr;
        collisionConfiguration = nullptr;
    }
    
private:
    // Bullet组件
    btDefaultCollisionConfiguration* collisionConfiguration;
    btCollisionDispatcher* dispatcher;
    btBroadphaseInterface* broadphase;
    btSequentialImpulseConstraintSolver* solver;
    btDiscreteDynamicsWorld* dynamicsWorld;
    
    // 刚体列表
    std::vector<RigidBody*> rigidBodies;
    
    // 碰撞形状缓存
    std::unordered_map<ShapeDesc, btCollisionShape*, ShapeDescHash> shapes;
    
    // 辅助函数：查找或创建碰撞形状
    btCollisionShape* FindOrCreateShape(const ShapeDesc& desc) {
        auto it = shapes.find(desc);
        if (it != shapes.end()) {
            return it->second;
        }
        
        btCollisionShape* shape = nullptr;
        
        switch (desc.type) {
            case ShapeType::Box:
                shape = new btBoxShape(ConvertToBullet(desc.box.halfExtents));
                break;
            case ShapeType::Sphere:
                shape = new btSphereShape(desc.sphere.radius);
                break;
            case ShapeType::Capsule:
                shape = new btCapsuleShape(desc.capsule.radius, desc.capsule.height);
                break;
            // 其他形状类型...
        }
        
        if (shape) {
            shapes[desc] = shape;
        }
        
        return shape;
    }
    
    // 坐标转换辅助函数
    static btVector3 ConvertToBullet(const Vector3& v) {
        return btVector3(v.x, v.y, v.z);
    }
    
    static btQuaternion ConvertToBullet(const Quaternion& q) {
        return btQuaternion(q.x, q.y, q.z, q.w);
    }
};

12.2.3 自定义物理与中间件的集成

有时候需要将自定义物理逻辑与中间件集成。这里是一个例子，展示如何将自定义运动控制器与物理中间件结合：

cpp

// 角色控制器，集成物理中间件
class CharacterController {
public:
    CharacterController(PhysicsEngine* engine, const Vector3& position, float radius, float height) {
        // 创建胶囊体形状
        ShapeDesc shapeDesc;
        shapeDesc.type = ShapeType::Capsule;
        shapeDesc.capsule.radius = radius;
        shapeDesc.capsule.height = height;
        
        // 创建刚体描述
        RigidBodyDesc bodyDesc;
        bodyDesc.mass = 80.0f;  // 80kg
        bodyDesc.position = position;
        bodyDesc.rotation = Quaternion::Identity();
        bodyDesc.shapeDesc = shapeDesc;
        
        // 创建刚体
        rigidBody = engine->CreateRigidBody(bodyDesc);
        
        // 配置刚体属性
        rigidBody->SetFriction(0.5f);
        rigidBody->SetRestitution(0.0f);
        
        // 禁用刚体旋转，保持角色直立
        rigidBody->SetAngularFactor(Vector3(0, 0, 0));
        
        this->radius = radius;
        this->height = height;
        this->physicsEngine = engine;
    }
    
    void Update(float deltaTime, const Vector3& moveDirection, bool jump) {
        // 获取当前状态
        Vector3 position = rigidBody->GetPosition();
        Vector3 velocity = rigidBody->GetLinearVelocity();
        
        // 检查是否接地
        bool grounded = CheckGrounded();
        
        // 处理移动
        if (moveDirection.LengthSquared() > 0.01f) {
            Vector3 targetVelocity = moveDirection * moveSpeed;
            
            // 只改变水平速度，保留垂直速度
            Vector3 velocityChange;
            velocityChange.x = targetVelocity.x - velocity.x;
            velocityChange.z = targetVelocity.z - velocity.z;
            
            // 施加水平力
            rigidBody->ApplyImpulse(velocityChange * rigidBody->GetMass());
        }
        
        // 处理跳跃
        if (jump && grounded) {
            Vector3 jumpImpulse(0, jumpForce * rigidBody->GetMass(), 0);
            rigidBody->ApplyImpulse(jumpImpulse);
        }
    }
    
    Vector3 GetPosition() const {
        return rigidBody->GetPosition();
    }
    
    Vector3 GetVelocity() const {
        return rigidBody->GetLinearVelocity();
    }
    
private:
    bool CheckGrounded() {
        // 执行射线检测确定角色是否接地
        Vector3 start = rigidBody->GetPosition();
        Vector3 end = start - Vector3(0, radius + 0.1f, 0);
        
        RayCastResult result;
        if (physicsEngine->RayCast(Ray(start, end), result)) {
            // 忽略与自身的碰撞
            if (result.body != rigidBody) {
                return true;
            }
        }
        
        return false;
    }
    
    RigidBody* rigidBody;
    PhysicsEngine* physicsEngine;
    float radius;
    float height;
    
    float moveSpeed = 5.0f;
    float jumpForce = 5.0f;
};

12.3 碰撞检测系统

碰撞检测是物理引擎的基础，负责确定物体之间是否发生接触以及接触的具体情况。

12.3.1 碰撞检测的基本阶段

碰撞检测通常分为三个阶段：

宽阶段（Broad Phase）：快速筛选可能碰撞的物体对
中阶段（Mid Phase）：对复杂物体进行更精确的包围体检测
窄阶段（Narrow Phase）：精确计算碰撞点、法线和穿透深度

以下是一个简单的碰撞检测系统实现：

cpp

// 碰撞检测系统
class CollisionSystem {
public:
    CollisionSystem() : broadPhase(nullptr) {}
    
    bool Initialize() {
        // 创建宽阶段算法（这里使用动态边界体积树）
        broadPhase = new DynamicAABBTree();
        return true;
    }
    
    void AddCollider(Collider* collider) {
        colliders.push_back(collider);
        
        // 将碰撞体添加到宽阶段
        if (broadPhase) {
            AABB aabb = collider->ComputeAABB();
            int proxyId = broadPhase->CreateProxy(aabb, collider);
            colliderProxyMap[collider] = proxyId;
        }
    }
    
    void RemoveCollider(Collider* collider) {
        auto it = std::find(colliders.begin(), colliders.end(), collider);
        if (it != colliders.end()) {
            colliders.erase(it);
            
            // 从宽阶段移除
            auto proxyIt = colliderProxyMap.find(collider);
            if (proxyIt != colliderProxyMap.end()) {
                broadPhase->DestroyProxy(proxyIt->second);
                colliderProxyMap.erase(proxyIt);
            }
        }
    }
    
    void UpdateCollider(Collider* collider) {
        // 更新宽阶段中的AABB
        auto proxyIt = colliderProxyMap.find(collider);
        if (proxyIt != colliderProxyMap.end()) {
            AABB aabb = collider->ComputeAABB();
            broadPhase->UpdateProxy(proxyIt->second, aabb);
        }
    }
    
    void DetectCollisions() {
        // 清除上一帧的碰撞结果
        collisionPairs.clear();
        
        // 宽阶段碰撞检测
        BroadPhaseCollisions();
        
        // 窄阶段碰撞检测
        NarrowPhaseCollisions();
    }
    
    // 获取碰撞结果
    const std::vector<CollisionPair>& GetCollisionPairs() const {
        return collisionPairs;
    }
    
    // 射线检测
    bool RayCast(const Ray& ray, RayCastResult& result) {
        bool hit = false;
        float closestHit = std::numeric_limits<float>::max();
        
        // 使用宽阶段加速射线检测
        broadPhase->RayCast(ray, [&](int proxyId) {
            Collider* collider = static_cast<Collider*>(broadPhase->GetUserData(proxyId));
            
            RayCastResult tempResult;
            if (collider->RayCast(ray, tempResult) && tempResult.distance < closestHit) {
                closestHit = tempResult.distance;
                result = tempResult;
                hit = true;
            }
            
            // 继续射线检测
            return true;
        });
        
        return hit;
    }
    
    // 清理资源
    void Cleanup() {
        delete broadPhase;
        broadPhase = nullptr;
        
        colliders.clear();
        colliderProxyMap.clear();
        collisionPairs.clear();
    }
    
private:
    // 宽阶段碰撞检测
    void BroadPhaseCollisions() {
        // 使用宽阶段算法找出潜在的碰撞对
        broadPhase->ComputeOverlappingPairs([this](int proxyIdA, int proxyIdB) {
            Collider* colliderA = static_cast<Collider*>(broadPhase->GetUserData(proxyIdA));
            Collider* colliderB = static_cast<Collider*>(broadPhase->GetUserData(proxyIdB));
            
            // 将潜在碰撞对加入待检测列表
            potentialPairs.emplace_back(colliderA, colliderB);
        });
    }
    
    // 窄阶段碰撞检测
    void NarrowPhaseCollisions() {
        for (const auto& pair : potentialPairs) {
            Collider* colliderA = pair.first;
            Collider* colliderB = pair.second;
            
            // 跳过不应碰撞的物体对
            if (!ShouldCollide(colliderA, colliderB)) {
                continue;
            }
            
            // 窄阶段碰撞检测
            CollisionInfo info;
            if (DetectCollision(colliderA, colliderB, info)) {
                CollisionPair collisionPair;
                collisionPair.colliderA = colliderA;
                collisionPair.colliderB = colliderB;
                collisionPair.info = info;
                
                collisionPairs.push_back(collisionPair);
                
                // 触发碰撞回调
                colliderA->OnCollision(colliderB, info);
                colliderB->OnCollision(colliderA, info);
            }
        }
        
        // 清除潜在碰撞对列表，为下一帧做准备
        potentialPairs.clear();
    }
    
    // 判断两个碰撞体是否应该检测碰撞
    bool ShouldCollide(Collider* colliderA, Collider* colliderB) {
        // 检查碰撞过滤器
        return colliderA->GetCollisionFilter().ShouldCollide(colliderB->GetCollisionFilter());
    }
    
    // 检测两个碰撞体的具体碰撞情况
    bool DetectCollision(Collider* colliderA, Collider* colliderB, CollisionInfo& info) {
        // 根据碰撞体类型选择适当的碰撞检测算法
        if (colliderA->GetType() == ColliderType::Sphere && 
            colliderB->GetType() == ColliderType::Sphere) {
            return DetectSphereSphereCollision(
                static_cast<SphereCollider*>(colliderA),
                static_cast<SphereCollider*>(colliderB),
                info);
        }
        else if (colliderA->GetType() == ColliderType::Box && 
                 colliderB->GetType() == ColliderType::Box) {
            return DetectBoxBoxCollision(
                static_cast<BoxCollider*>(colliderA),
                static_cast<BoxCollider*>(colliderB),
                info);
        }
        // 其他碰撞类型检测...
        
        return false;
    }
    
    // 球体与球体碰撞检测
    bool DetectSphereSphereCollision(SphereCollider* sphereA, SphereCollider* sphereB, 
                                    CollisionInfo& info) {
        Vector3 posA = sphereA->GetWorldPosition();
        Vector3 posB = sphereB->GetWorldPosition();
        
        float radiusA = sphereA->GetRadius();
        float radiusB = sphereB->GetRadius();
        
        Vector3 ab = posB - posA;
        float distance = ab.Length();
        
        // 检测球体是否相交
        float sumRadius = radiusA + radiusB;
        if (distance >= sumRadius) {
            return false;
        }
        
        // 计算碰撞信息
        Vector3 normal = distance > 0.0001f ? ab / distance : Vector3(0, 1, 0);
        float penetration = sumRadius - distance;
        
        // 填充碰撞信息
        info.normal = normal;
        info.penetration = penetration;
        info.contactPoint = posA + normal * (radiusA - penetration * 0.5f);
        
        return true;
    }
    
    // 盒子与盒子碰撞检测 (SAT算法)
    bool DetectBoxBoxCollision(BoxCollider* boxA, BoxCollider* boxB, 
                              CollisionInfo& info) {
        // SAT (分离轴定理) 实现
        // ...
        
        return false; // 简化实现
    }
    
    // 碰撞体列表
    std::vector<Collider*> colliders;
    
    // 宽阶段算法
    DynamicAABBTree* broadPhase;
    
    // 碰撞体与宽阶段代理ID的映射
    std::unordered_map<Collider*, int> colliderProxyMap;
    
    // 宽阶段产生的潜在碰撞对
    std::vector<std::pair<Collider*, Collider*>> potentialPairs;
    
    // 最终的碰撞结果
    std::vector<CollisionPair> collisionPairs;
};

12.3.2 宽阶段碰撞检测

宽阶段碰撞检测使用简化的边界体积快速排除不可能碰撞的物体对。常见的宽阶段算法包括：

暴力检测：O(n²)复杂度，适用于物体数量少的情况
空间分割：如网格、八叉树等
扫描与剪裁：沿一个轴排序物体
动态边界体积树：自适应树结构，常用于动态场景

以下是一个简化的动态AABB树实现：

cpp

// 动态AABB树节点
struct AABBTreeNode {
    AABB aabb;       // 节点的边界框
    void* userData;  // 用户数据（通常是碰撞体指针）
    
    union {
        int parent;  // 父节点索引
        int next;    // 自由列表中的下一个节点
    };
    
    int left;        // 左子节点索引
    int right;       // 右子节点索引
    
    // 是否是叶节点
    bool IsLeaf() const {
        return left == -1;
    }
};

// 动态AABB树实现
class DynamicAABBTree {
public:
    DynamicAABBTree(int initialCapacity = 16) : 
        nodeCapacity(initialCapacity),
        nodeCount(0),
        rootIndex(-1),
        freeListIndex(-1) {
        
        // 分配节点内存
        nodes = new AABBTreeNode[nodeCapacity];
        
        // 初始化自由列表
        for (int i = 0; i < nodeCapacity - 1; ++i) {
            nodes[i].next = i + 1;
            nodes[i].height = -1;
        }
        nodes[nodeCapacity - 1].next = -1;
        nodes[nodeCapacity - 1].height = -1;
        
        freeListIndex = 0;
    }
    
    ~DynamicAABBTree() {
        delete[] nodes;
    }
    
    // 创建代理
    int CreateProxy(const AABB& aabb, void* userData) {
        int proxyId = AllocateNode();
        
        // 初始化节点
        nodes[proxyId].aabb = aabb;
        nodes[proxyId].userData = userData;
        nodes[proxyId].height = 0;
        nodes[proxyId].left = -1;  // 标记为叶节点
        
        // 插入到树中
        InsertLeaf(proxyId);
        
        return proxyId;
    }
    
    // 销毁代理
    void DestroyProxy(int proxyId) {
        assert(0 <= proxyId && proxyId < nodeCapacity);
        assert(nodes[proxyId].IsLeaf());
        
        RemoveLeaf(proxyId);
        FreeNode(proxyId);
    }
    
    // 更新代理
    void UpdateProxy(int proxyId, const AABB& aabb) {
        assert(0 <= proxyId && proxyId < nodeCapacity);
        assert(nodes[proxyId].IsLeaf());
        
        RemoveLeaf(proxyId);
        
        nodes[proxyId].aabb = aabb;
        
        InsertLeaf(proxyId);
    }
    
    // 获取用户数据
    void* GetUserData(int proxyId) const {
        assert(0 <= proxyId && proxyId < nodeCapacity);
        return nodes[proxyId].userData;
    }
    
    // 计算重叠的对
    template<typename Callback>
    void ComputeOverlappingPairs(Callback callback) {
        // 使用栈来避免递归
        std::vector<std::pair<int, int>> stack;
        
        if (rootIndex == -1) {
            return;
        }
        
        // 从根节点开始遍历
        for (int i = 0; i < nodeCount; ++i) {
            if (nodes[i].IsLeaf()) {
                // 跳过已处理的节点
                for (int j = i + 1; j < nodeCount; ++j) {
                    if (nodes[j].IsLeaf()) {
                        if (AABB::TestOverlap(nodes[i].aabb, nodes[j].aabb)) {
                            callback(i, j);
                        }
                    }
                }
            }
        }
    }
    
    // 射线检测
    template<typename Callback>
    void RayCast(const Ray& ray, Callback callback) {
        if (rootIndex == -1) {
            return;
        }
        
        // 射线数据
        Vector3 p1 = ray.origin;
        Vector3 p2 = ray.origin + ray.direction * ray.maxDistance;
        
        // 使用栈来避免递归
        std::vector<int> stack;
        stack.push_back(rootIndex);
        
        while (!stack.empty()) {
            int nodeIndex = stack.back();
            stack.pop_back();
            
            // 检测射线是否与AABB相交
            if (!nodes[nodeIndex].aabb.RayCast(ray)) {
                continue;
            }
            
            if (nodes[nodeIndex].IsLeaf()) {
                // 叶节点，调用回调函数
                bool proceed = callback(nodeIndex);
                if (!proceed) {
                    break;
                }
            } else {
                // 内部节点，继续遍历子节点
                stack.push_back(nodes[nodeIndex].left);
                stack.push_back(nodes[nodeIndex].right);
            }
        }
    }
    
private:
    // 分配节点
    int AllocateNode() {
        // 扩展节点数组（如果需要）
        if (freeListIndex == -1) {
            assert(nodeCount == nodeCapacity);
            
            // 扩大容量
            nodeCapacity *= 2;
            AABBTreeNode* newNodes = new AABBTreeNode[nodeCapacity];
            
            // 复制旧节点
            memcpy(newNodes, nodes, nodeCount * sizeof(AABBTreeNode));
            
            // 删除旧数组
            delete[] nodes;
            nodes = newNodes;
            
            // 构建新的自由列表
            for (int i = nodeCount; i < nodeCapacity - 1; ++i) {
                nodes[i].next = i + 1;
                nodes[i].height = -1;
            }
            nodes[nodeCapacity - 1].next = -1;
            nodes[nodeCapacity - 1].height = -1;
            
            freeListIndex = nodeCount;
        }
        
        // 从自由列表中获取节点
        int nodeIndex = freeListIndex;
        freeListIndex = nodes[nodeIndex].next;
        nodes[nodeIndex].parent = -1;
        nodes[nodeIndex].left = -1;
        nodes[nodeIndex].right = -1;
        nodes[nodeIndex].height = 0;
        nodes[nodeIndex].userData = nullptr;
        ++nodeCount;
        
        return nodeIndex;
    }
    
    // 释放节点
    void FreeNode(int nodeIndex) {
        assert(0 <= nodeIndex && nodeIndex < nodeCapacity);
        assert(0 < nodeCount);
        
        nodes[nodeIndex].next = freeListIndex;
        nodes[nodeIndex].height = -1;
        freeListIndex = nodeIndex;
        --nodeCount;
    }
    
    // 插入叶节点
    void InsertLeaf(int leafIndex) {
        // 如果树为空，设置根节点
        if (rootIndex == -1) {
            rootIndex = leafIndex;
            nodes[rootIndex].parent = -1;
            return;
        }
        
        // 找到最佳插入位置
        AABB leafAABB = nodes[leafIndex].aabb;
        int index = rootIndex;
        
        while (!nodes[index].IsLeaf()) {
            int left = nodes[index].left;
            int right = nodes[index].right;
            
            float area = nodes[index].aabb.GetPerimeter();
            
            AABB combinedAABB = AABB::Combine(nodes[index].aabb, leafAABB);
            float combinedArea = combinedAABB.GetPerimeter();
            
            // 计算代价函数
            float cost = 2.0f * combinedArea;
            float inheritanceCost = 2.0f * (combinedArea - area);
            
            // 计算下降到子节点的代价
            float costLeft;
            if (nodes[left].IsLeaf()) {
                AABB aabb = AABB::Combine(leafAABB, nodes[left].aabb);
                costLeft = aabb.GetPerimeter() + inheritanceCost;
            } else {
                AABB aabb = AABB::Combine(leafAABB, nodes[left].aabb);
                float oldArea = nodes[left].aabb.GetPerimeter();
                float newArea = aabb.GetPerimeter();
                costLeft = (newArea - oldArea) + inheritanceCost;
            }
            
            float costRight;
            if (nodes[right].IsLeaf()) {
                AABB aabb = AABB::Combine(leafAABB, nodes[right].aabb);
                costRight = aabb.GetPerimeter() + inheritanceCost;
            } else {
                AABB aabb = AABB::Combine(leafAABB, nodes[right].aabb);
                float oldArea = nodes[right].aabb.GetPerimeter();
                float newArea = aabb.GetPerimeter();
                costRight = (newArea - oldArea) + inheritanceCost;
            }
            
            // 确定下降方向
            if (cost < costLeft && cost < costRight) {
                break;
            }
            
            // 下降到子节点
            if (costLeft < costRight) {
                index = left;
            } else {
                index = right;
            }
        }
        
        int sibling = index;
        
        // 创建新的父节点
        int oldParent = nodes[sibling].parent;
        int newParent = AllocateNode();
        nodes[newParent].parent = oldParent;
        nodes[newParent].userData = nullptr;
        nodes[newParent].aabb = AABB::Combine(leafAABB, nodes[sibling].aabb);
        nodes[newParent].height = nodes[sibling].height + 1;
        
        if (oldParent != -1) {
            // 兄弟节点不是根节点
            if (nodes[oldParent].left == sibling) {
                nodes[oldParent].left = newParent;
            } else {
                nodes[oldParent].right = newParent;
            }
            
            nodes[newParent].left = sibling;
            nodes[newParent].right = leafIndex;
            nodes[sibling].parent = newParent;
            nodes[leafIndex].parent = newParent;
        } else {
            // 兄弟节点是根节点
            nodes[newParent].left = sibling;
            nodes[newParent].right = leafIndex;
            nodes[sibling].parent = newParent;
            nodes[leafIndex].parent = newParent;
            rootIndex = newParent;
        }
        
        // 向上更新AABB和高度
        UpdateUpward(nodes[leafIndex].parent);
    }
    
    // 移除叶节点
    void RemoveLeaf(int leafIndex) {
        if (leafIndex == rootIndex) {
            rootIndex = -1;
            return;
        }
        
        int parentIndex = nodes[leafIndex].parent;
        int grandParentIndex = nodes[parentIndex].parent;
        int siblingIndex;
        
        if (nodes[parentIndex].left == leafIndex) {
            siblingIndex = nodes[parentIndex].right;
        } else {
            siblingIndex = nodes[parentIndex].left;
        }
        
        if (grandParentIndex != -1) {
            // 将兄弟节点连接到祖父节点
            if (nodes[grandParentIndex].left == parentIndex) {
                nodes[grandParentIndex].left = siblingIndex;
            } else {
                nodes[grandParentIndex].right = siblingIndex;
            }
            nodes[siblingIndex].parent = grandParentIndex;
            
            // 释放父节点
            FreeNode(parentIndex);
            
            // 向上更新AABB和高度
            UpdateUpward(grandParentIndex);
        } else {
            // 父节点是根节点，使兄弟节点成为新的根节点
            rootIndex = siblingIndex;
            nodes[siblingIndex].parent = -1;
            
            // 释放父节点
            FreeNode(parentIndex);
        }
    }
    
    // 向上更新AABB和高度
    void UpdateUpward(int index) {
        while (index != -1) {
            index = BalanceNode(index);
            
            int left = nodes[index].left;
            int right = nodes[index].right;
            
            nodes[index].height = 1 + std::max(nodes[left].height, nodes[right].height);
            nodes[index].aabb = AABB::Combine(nodes[left].aabb, nodes[right].aabb);
            
            index = nodes[index].parent;
        }
    }
    
    // 平衡节点（AVL树旋转）
    int BalanceNode(int index) {
        if (nodes[index].IsLeaf() || nodes[index].height < 2) {
            return index;
        }
        
        int left = nodes[index].left;
        int right = nodes[index].right;
        
        int balance = nodes[right].height - nodes[left].height;
        
        // 右子树过高
        if (balance > 1) {
            int rightLeft = nodes[right].left;
            int rightRight = nodes[right].right;
            
            // 右-左旋转
            if (nodes[rightLeft].height > nodes[rightRight].height) {
                // 先右旋
                // ...
                
                // 再左旋
                // ...
                
                return rightLeft;
            }
            // 左旋转
            // ...
            
            return right;
        }
        
        // 左子树过高
        if (balance < -1) {
            int leftLeft = nodes[left].left;
            int leftRight = nodes[left].right;
            
            // 左-右旋转
            if (nodes[leftRight].height > nodes[leftLeft].height) {
                // 先左旋
                // ...
                
                // 再右旋
                // ...
                
                return leftRight;
            }
            // 右旋转
            // ...
            
            return left;
        }
        
        return index;
    }
    
    AABBTreeNode* nodes;
    int nodeCapacity;
    int nodeCount;
    int rootIndex;
    int freeListIndex;
};

12.3.3 窄阶段碰撞检测

窄阶段碰撞检测负责精确计算两个物体之间的碰撞情况。以下是一些常见的基本碰撞检测算法：

球体与球体碰撞

cpp

bool TestSphereSphereCollision(const Sphere& sphereA, const Sphere& sphereB, 
                              CollisionInfo& info) {
    Vector3 posA = sphereA.center;
    Vector3 posB = sphereB.center;
    
    float radiusA = sphereA.radius;
    float radiusB = sphereB.radius;
    
    Vector3 ab = posB - posA;
    float distanceSquared = ab.LengthSquared();
    float sumRadius = radiusA + radiusB;
    
    // 快速平方检测
    if (distanceSquared >= sumRadius * sumRadius) {
        return false;  // 没有碰撞
    }
    
    // 计算实际距离
    float distance = sqrt(distanceSquared);
    
    // 填充碰撞信息
    Vector3 normal = distance > 0.0001f ? ab / distance : Vector3(0, 1, 0);
    float penetration = sumRadius - distance;
    
    info.normal = normal;
    info.penetration = penetration;
    info.contactPoint = posA + normal * (radiusA - penetration * 0.5f);
    
    return true;
}

分离轴定理(SAT)用于凸多面体

cpp

bool TestBoxBoxCollisionSAT(const Box& boxA, const Box& boxB, CollisionInfo& info) {
    // 获取两个盒子的变换
    const Transform& transformA = boxA.transform;
    const Transform& transformB = boxB.transform;
    
    // 计算盒子A的局部坐标轴（在世界空间中）
    Vector3 axisA[3] = {
        transformA.TransformDirection(Vector3(1, 0, 0)),
        transformA.TransformDirection(Vector3(0, 1, 0)),
        transformA.TransformDirection(Vector3(0, 0, 1))
    };
    
    // 计算盒子B的局部坐标轴（在世界空间中）
    Vector3 axisB[3] = {
        transformB.TransformDirection(Vector3(1, 0, 0)),
        transformB.TransformDirection(Vector3(0, 1, 0)),
        transformB.TransformDirection(Vector3(0, 0, 1))
    };
    
    // 盒子中心点
    Vector3 centerA = transformA.position;
    Vector3 centerB = transformB.position;
    
    // 盒子半尺寸
    Vector3 halfSizeA = boxA.halfSize;
    Vector3 halfSizeB = boxB.halfSize;
    
    // 盒子中心距离向量
    Vector3 t = centerB - centerA;
    
    // 最小渗透深度和对应的轴
    float minPenetration = std::numeric_limits<float>::max();
    Vector3 bestAxis;
    
    // 测试盒子A的3个轴
    for (int i = 0; i < 3; ++i) {
        float penetration = TestAxis(axisA[i], t, axisA, axisB, halfSizeA, halfSizeB);
        if (penetration < 0) {
            return false;  // 找到分离轴，无碰撞
        }
        
        if (penetration < minPenetration) {
            minPenetration = penetration;
            bestAxis = axisA[i];
        }
    }
    
    // 测试盒子B的3个轴
    for (int i = 0; i < 3; ++i) {
        float penetration = TestAxis(axisB[i], t, axisA, axisB, halfSizeA, halfSizeB);
        if (penetration < 0) {
            return false;  // 找到分离轴，无碰撞
        }
        
        if (penetration < minPenetration) {
            minPenetration = penetration;
            bestAxis = axisB[i];
        }
    }
    
    // 测试9个叉积轴
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            Vector3 axis = Vector3::Cross(axisA[i], axisB[j]);
            float length = axis.Length();
            
            // 跳过平行轴
            if (length < 0.0001f) {
                continue;
            }
            
            axis = axis / length;  // 归一化
            
            float penetration = TestAxis(axis, t, axisA, axisB, halfSizeA, halfSizeB);
            if (penetration < 0) {
                return false;  // 找到分离轴，无碰撞
            }
            
            if (penetration < minPenetration) {
                minPenetration = penetration;
                bestAxis = axis;
            }
        }
    }
    
    // 确保法线指向从A到B
    Vector3 normal = bestAxis;
    if (Vector3::Dot(t, normal) < 0) {
        normal = -normal;
    }
    
    // 填充碰撞信息
    info.normal = normal;
    info.penetration = minPenetration;
    
    // 计算接触点（简化版本）
    info.contactPoint = centerA + t * 0.5f;
    
    return true;
}

// 在给定轴上测试两个盒子的投影
float TestAxis(const Vector3& axis, const Vector3& t, 
              const Vector3 axisA[3], const Vector3 axisB[3],
              const Vector3& halfSizeA, const Vector3& halfSizeB) {
    // 计算盒子A在axis上的投影半长度
    float projA = 
        halfSizeA.x * std::abs(Vector3::Dot(axis, axisA[0])) +
        halfSizeA.y * std::abs(Vector3::Dot(axis, axisA[1])) +
        halfSizeA.z * std::abs(Vector3::Dot(axis, axisA[2]));
    
    // 计算盒子B在axis上的投影半长度
    float projB = 
        halfSizeB.x * std::abs(Vector3::Dot(axis, axisB[0])) +
        halfSizeB.y * std::abs(Vector3::Dot(axis, axisB[1])) +
        halfSizeB.z * std::abs(Vector3::Dot(axis, axisB[2]));
    
    // 计算两个中心在axis上的距离
    float distance = std::abs(Vector3::Dot(t, axis));
    
    // 计算重叠
    return projA + projB - distance;
}

12.3.4 碰撞过滤

碰撞过滤允许开发者控制哪些物体可以相互碰撞：

cpp

// 碰撞过滤器
struct CollisionFilter {
    uint32_t categoryBits;   // 物体所属的类别位掩码
    uint32_t maskBits;       // 物体可以碰撞的类别位掩码
    int16_t groupIndex;      // 组索引（负值表示不碰撞）
    
    CollisionFilter() : 
        categoryBits(0x0001), maskBits(0xFFFF), groupIndex(0) {}
    
    // 判断两个过滤器是否应该碰撞
    bool ShouldCollide(const CollisionFilter& other) const {
        // 负组索引表示同组物体不碰撞
        if (groupIndex != 0 && groupIndex == other.groupIndex) {
            return groupIndex > 0;
        }
        
        // 检查类别和掩码
        return (categoryBits & other.maskBits) != 0 && 
               (maskBits & other.categoryBits) != 0;
    }
};

// 预定义的碰撞类别（示例）
namespace CollisionCategory {
    constexpr uint32_t Default = 0x0001;
    constexpr uint32_t Player = 0x0002;
    constexpr uint32_t Enemy = 0x0004;
    constexpr uint32_t Projectile = 0x0008;
    constexpr uint32_t Sensor = 0x0010;
    constexpr uint32_t Environment = 0x0020;
    constexpr uint32_t Trigger = 0x0040;
    constexpr uint32_t All = 0xFFFF;
}

12.4 刚体动力学

刚体动力学负责模拟物体的运动，包括平移和旋转。

12.4.1 刚体运动方程

刚体运动由以下方程描述：

线性运动：

F = ma（牛顿第二定律）
v = v₀ + at（速度更新）
p = p₀ + vt + ½at²（位置更新）

角运动：

τ = Iα（转矩等于转动惯量乘以角加速度）
ω = ω₀ + αt（角速度更新）
θ = θ₀ + ωt + ½αt²（角度更新）

以下是一个简单的刚体实现：

cpp

// 刚体类
class RigidBody {
public:
    RigidBody() : 
        mass(1.0f), 
        inverseMass(1.0f),
        inertia(Vector3(1.0f, 1.0f, 1.0f)),
        inverseInertia(Vector3(1.0f, 1.0f, 1.0f)),
        linearDamping(0.01f),
        angularDamping(0.01f),
        restitution(0.5f),
        friction(0.5f),
        position(Vector3::Zero()),
        rotation(Quaternion::Identity()),
        linearVelocity(Vector3::Zero()),
        angularVelocity(Vector3::Zero()),
        force(Vector3::Zero()),
        torque(Vector3::Zero()),
        isStatic(false),
        isActive(true) {}
    
    // 更新力和转矩（施加外力）
    void ApplyForce(const Vector3& worldForce) {
        if (isStatic) return;
        force += worldForce;
    }
    
    void ApplyForceAtPoint(const Vector3& worldForce, const Vector3& worldPoint) {
        if (isStatic) return;
        force += worldForce;
        torque += Vector3::Cross(worldPoint - position, worldForce);
    }
    
    void ApplyTorque(const Vector3& worldTorque) {
        if (isStatic) return;
        torque += worldTorque;
    }
    
    void ApplyImpulse(const Vector3& impulse) {
        if (isStatic) return;
        linearVelocity += impulse * inverseMass;
    }
    
    void ApplyImpulseAtPoint(const Vector3& impulse, const Vector3& worldPoint) {
        if (isStatic) return;
        linearVelocity += impulse * inverseMass;
        
        Vector3 relativePos = worldPoint - position;
        Vector3 angularImpulse = Vector3::Cross(relativePos, impulse);
        
        // 将世界空间的角冲量转换为本地空间
        Matrix3x3 invInertiaTensor = GetInverseInertiaTensorWorld();
        Vector3 deltaOmega = invInertiaTensor * angularImpulse;
        
        angularVelocity += deltaOmega;
    }
    
    // 积分更新力（第一阶段）
    void IntegrateForces(float dt) {
        if (isStatic || !isActive) return;
        
        // 更新线性速度
        Vector3 linearAcceleration = force * inverseMass;
        linearVelocity += linearAcceleration * dt;
        
        // 应用线性阻尼
        linearVelocity *= pow(1.0f - linearDamping, dt);
        
        // 更新角速度
        Matrix3x3 invInertiaTensor = GetInverseInertiaTensorWorld();
        Vector3 angularAcceleration = invInertiaTensor * torque;
        angularVelocity += angularAcceleration * dt;
        
        // 应用角阻尼
        angularVelocity *= pow(1.0f - angularDamping, dt);
        
        // 重置力和转矩
        force = Vector3::Zero();
        torque = Vector3::Zero();
    }
    
    // 积分更新位置（第二阶段）
    void IntegrateVelocities(float dt) {
        if (isStatic || !isActive) return;
        
        // 更新位置
        position += linearVelocity * dt;
        
        // 更新旋转（使用四元数微分方程）
        Quaternion spin(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0.0f);
        Quaternion velocityQ = spin * rotation * 0.5f;
        
        rotation += velocityQ * dt;
        rotation = Quaternion::Normalize(rotation);
    }
    
    // 获取世界空间中的逆惯性张量
    Matrix3x3 GetInverseInertiaTensorWorld() const {
        // 从本地惯性张量构建对角矩阵
        Matrix3x3 localInvInertia = Matrix3x3::Zero();
        localInvInertia.m[0][0] = inverseInertia.x;
        localInvInertia.m[1][1] = inverseInertia.y;
        localInvInertia.m[2][2] = inverseInertia.z;
        
        // 构建旋转矩阵
        Matrix3x3 rotMatrix = Matrix3x3::CreateFromQuaternion(rotation);
        
        // 转换到世界空间：R * I^-1 * R^T
        Matrix3x3 result = rotMatrix * localInvInertia * Matrix3x3::Transpose(rotMatrix);
        return result;
    }
    
    // 设置质量和惯性张量
    void SetMass(float m) {
        if (m <= 0.0f) {
            // 质量为0表示静态物体
            mass = 0.0f;
            inverseMass = 0.0f;
            isStatic = true;
        } else {
            mass = m;
            inverseMass = 1.0f / m;
            isStatic = false;
        }
    }
    
    void SetInertia(const Vector3& i) {
        inertia = i;
        inverseInertia = Vector3(
            i.x != 0.0f ? 1.0f / i.x : 0.0f,
            i.y != 0.0f ? 1.0f / i.y : 0.0f,
            i.z != 0.0f ? 1.0f / i.z : 0.0f
        );
    }
    
    // 计算盒体的惯性张量
    void ComputeBoxInertia(const Vector3& boxSize) {
        float m = mass;
        float x2 = boxSize.x * boxSize.x;
        float y2 = boxSize.y * boxSize.y;
        float z2 = boxSize.z * boxSize.z;
        
        inertia.x = (1.0f / 12.0f) * m * (y2 + z2);
        inertia.y = (1.0f / 12.0f) * m * (x2 + z2);
        inertia.z = (1.0f / 12.0f) * m * (x2 + y2);
        
        inverseInertia.x = inertia.x != 0.0f ? 1.0f / inertia.x : 0.0f;
        inverseInertia.y = inertia.y != 0.0f ? 1.0f / inertia.y : 0.0f;
        inverseInertia.z = inertia.z != 0.0f ? 1.0f / inertia.z : 0.0f;
    }
    
    // 活动状态控制
    void SetActive(bool active) {
        isActive = active;
    }
    
    bool IsActive() const {
        return isActive;
    }
    
    bool IsStatic() const {
        return isStatic;
    }
    
    // 获取和设置物理属性
    float GetMass() const { return mass; }
    float GetInverseMass() const { return inverseMass; }
    
    void SetLinearDamping(float damping) { linearDamping = damping; }
    float GetLinearDamping() const { return linearDamping; }
    
    void SetAngularDamping(float damping) { angularDamping = damping; }
    float GetAngularDamping() const { return angularDamping; }
    
    void SetRestitution(float r) { restitution = r; }
    float GetRestitution() const { return restitution; }
    
    void SetFriction(float f) { friction = f; }
    float GetFriction() const { return friction; }
    
    // 运动学状态访问器
    Vector3 GetPosition() const { return position; }
    void SetPosition(const Vector3& pos) { position = pos; }
    
    Quaternion GetRotation() const { return rotation; }
    void SetRotation(const Quaternion& rot) { rotation = rot; }
    
    Vector3 GetLinearVelocity() const { return linearVelocity; }
    void SetLinearVelocity(const Vector3& vel) { linearVelocity = vel; }
    
    Vector3 GetAngularVelocity() const { return angularVelocity; }
    void SetAngularVelocity(const Vector3& vel) { angularVelocity = vel; }
    
    // 坐标转换
    Vector3 GetPointVelocity(const Vector3& worldPoint) const {
        Vector3 relativePos = worldPoint - position;
        return linearVelocity + Vector3::Cross(angularVelocity, relativePos);
    }
    
    Vector3 WorldToLocalPoint(const Vector3& worldPoint) const {
        Vector3 relativePos = worldPoint - position;
        Quaternion invRotation = Quaternion::Inverse(rotation);
        return Quaternion::RotateVector(invRotation, relativePos);
    }
    
    Vector3 LocalToWorldPoint(const Vector3& localPoint) const {
        return position + Quaternion::RotateVector(rotation, localPoint);
    }
    
    Vector3 WorldToLocalDirection(const Vector3& worldDir) const {
        Quaternion invRotation = Quaternion::Inverse(rotation);
        return Quaternion::RotateVector(invRotation, worldDir);
    }
    
    Vector3 LocalToWorldDirection(const Vector3& localDir) const {
        return Quaternion::RotateVector(rotation, localDir);
    }
    
private:
    // 物理属性
    float mass;
    float inverseMass;
    Vector3 inertia;
    Vector3 inverseInertia;
    float linearDamping;
    float angularDamping;
    float restitution;
    float friction;
    
    // 运动学状态
    Vector3 position;
    Quaternion rotation;
    Vector3 linearVelocity;
    Vector3 angularVelocity;
    
    // 累积的力和转矩
    Vector3 force;
    Vector3 torque;
    
    // 标志
    bool isStatic;
    bool isActive;
};

12.4.2 约束解算

约束求解器负责处理碰撞约束和用户定义的约束。以下是一个简单的顺序冲量约束求解器：

cpp

// 碰撞约束
struct ContactConstraint {
    RigidBody* bodyA;
    RigidBody* bodyB;
    Vector3 contactPoint;
    Vector3 normal;
    float penetration;
    float restitution;
    float friction;
    
    // 用于解算的数据
    Vector3 relPosA;
    Vector3 relPosB;
    float normalMass;
    Vector3 tangentMass;
    float bias;
    float normalImpulse;
    Vector3 tangentImpulse;
};

// 约束求解器
class ConstraintSolver {
public:
    ConstraintSolver() : velocityIterations(10), positionIterations(4) {}
    
    void SetIterations(int velocityIter, int positionIter) {
        velocityIterations = velocityIter;
        positionIterations = positionIter;
    }
    
    // 解决碰撞约束
    void SolveConstraints(const std::vector<ContactConstraint>& constraints, float dt) {
        // 预计算约束参数
        PrepareConstraints(constraints, dt);
        
        // 速度迭代
        for (int i = 0; i < velocityIterations; ++i) {
            SolveVelocityConstraints(constraints);
        }
        
        // 位置迭代
        for (int i = 0; i < positionIterations; ++i) {
            SolvePositionConstraints(constraints);
        }
    }
    
private:
    int velocityIterations;
    int positionIterations;
    
    // 预计算约束参数
    void PrepareConstraints(const std::vector<ContactConstraint>& constraints, float dt) {
        const float baumgarte = 0.2f;  // Baumgarte因子
        const float allowedPenetration = 0.01f;  // 允许的穿透深度
        
        for (auto& constraint : const_cast<std::vector<ContactConstraint>&>(constraints)) {
            RigidBody* bodyA = constraint.bodyA;
            RigidBody* bodyB = constraint.bodyB;
            
            // 相对接触点
            constraint.relPosA = constraint.contactPoint - bodyA->GetPosition();
            constraint.relPosB = constraint.contactPoint - bodyB->GetPosition();
            
            // 计算法向质量
            Vector3 raCrossN = Vector3::Cross(constraint.relPosA, constraint.normal);
            Vector3 rbCrossN = Vector3::Cross(constraint.relPosB, constraint.normal);
            
            Matrix3x3 invInertiaA = bodyA->GetInverseInertiaTensorWorld();
            Matrix3x3 invInertiaB = bodyB->GetInverseInertiaTensorWorld();
            
            Vector3 angularFactorA = invInertiaA * raCrossN;
            Vector3 angularFactorB = invInertiaB * rbCrossN;
            
            float invMassA = bodyA->GetInverseMass();
            float invMassB = bodyB->GetInverseMass();
            
            float invMassSum = invMassA + invMassB;
            
            // 计算角因子的点积
            float angularMass = Vector3::Dot(raCrossN, angularFactorA) + 
                               Vector3::Dot(rbCrossN, angularFactorB);
            
            float totalMass = invMassSum + angularMass;
            
            constraint.normalMass = totalMass > 0.0f ? 1.0f / totalMass : 0.0f;
            
            // 计算切向质量
            // ... (类似的计算)
            
            // 计算速度偏移（弹性）
            Vector3 velA = bodyA->GetLinearVelocity() + 
                          Vector3::Cross(bodyA->GetAngularVelocity(), constraint.relPosA);
            Vector3 velB = bodyB->GetLinearVelocity() + 
                          Vector3::Cross(bodyB->GetAngularVelocity(), constraint.relPosB);
            
            Vector3 relVel = velB - velA;
            float normalVel = Vector3::Dot(relVel, constraint.normal);
            
            // 计算位置偏移（穿透修正）
            float penetration = constraint.penetration - allowedPenetration;
            float positionCorrection = baumgarte * penetration / dt;
            
            constraint.bias = -constraint.restitution * normalVel + positionCorrection;
            
            // 清空累积冲量
            constraint.normalImpulse = 0.0f;
            constraint.tangentImpulse = Vector3::Zero();
        }
    }
    
    // 解决速度约束
    void SolveVelocityConstraints(const std::vector<ContactConstraint>& constraints) {
        for (auto& constraint : const_cast<std::vector<ContactConstraint>&>(constraints)) {
            RigidBody* bodyA = constraint.bodyA;
            RigidBody* bodyB = constraint.bodyB;
            
            // 法向约束求解
            {
                // 计算相对速度
                Vector3 velA = bodyA->GetLinearVelocity() + 
                              Vector3::Cross(bodyA->GetAngularVelocity(), constraint.relPosA);
                Vector3 velB = bodyB->GetLinearVelocity() + 
                              Vector3::Cross(bodyB->GetAngularVelocity(), constraint.relPosB);
                
                Vector3 relVel = velB - velA;
                float normalVel = Vector3::Dot(relVel, constraint.normal);
                
                // 计算冲量
                float impulseMagnitude = constraint.normalMass * (-normalVel + constraint.bias);
                
                // 累积冲量约束
                float oldImpulse = constraint.normalImpulse;
                constraint.normalImpulse = std::max(oldImpulse + impulseMagnitude, 0.0f);
                impulseMagnitude = constraint.normalImpulse - oldImpulse;
                
                // 应用冲量
                Vector3 impulse = constraint.normal * impulseMagnitude;
                
                bodyA->ApplyImpulse(-impulse);
                bodyA->ApplyTorque(-Vector3::Cross(constraint.relPosA, impulse));
                
                bodyB->ApplyImpulse(impulse);
                bodyB->ApplyTorque(Vector3::Cross(constraint.relPosB, impulse));
            }
            
            // 摩擦约束求解
            // ... (类似的处理)
        }
    }
    
    // 解决位置约束
    void SolvePositionConstraints(const std::vector<ContactConstraint>& constraints) {
        const float allowedPenetration = 0.01f;
        
        for (auto& constraint : const_cast<std::vector<ContactConstraint>&>(constraints)) {
            RigidBody* bodyA = constraint.bodyA;
            RigidBody* bodyB = constraint.bodyB;
            
            Vector3 posA = bodyA->GetPosition();
            Quaternion rotA = bodyA->GetRotation();
            
            Vector3 posB = bodyB->GetPosition();
            Quaternion rotB = bodyB->GetRotation();
            
            // 计算当前接触点位置
            Vector3 relPosA = Quaternion::RotateVector(rotA, 
                                                      bodyA->WorldToLocalPoint(constraint.contactPoint));
            Vector3 relPosB = Quaternion::RotateVector(rotB, 
                                                      bodyB->WorldToLocalPoint(constraint.contactPoint));
            
            Vector3 worldPosA = posA + Quaternion::RotateVector(rotA, relPosA);
            Vector3 worldPosB = posB + Quaternion::RotateVector(rotB, relPosB);
            
            // 计算当前穿透
            Vector3 penetrationVector = worldPosB - worldPosA;
            float penetration = Vector3::Dot(penetrationVector, constraint.normal);
            
            // 如果穿透小于允许值，跳过
            if (penetration >= -allowedPenetration) {
                continue;
            }
            
            // 计算质量
            float invMassA = bodyA->GetInverseMass();
            float invMassB = bodyB->GetInverseMass();
            
            // 计算冲量
            float totalMass = invMassA + invMassB;
            float positionCorrection = totalMass > 0.0f ? 
                                      -penetration / totalMass : 0.0f;
            
            // 应用位置修正
            Vector3 correction = constraint.normal * positionCorrection;
            
            bodyA->SetPosition(posA - correction * invMassA);
            bodyB->SetPosition(posB + correction * invMassB);
        }
    }
};

12.4.3 积分方法

在物理模拟中，积分器用于推进物体的状态。常见的积分方法包括：

显式欧拉积分：最简单但稳定性较差
半隐式欧拉积分：更稳定，常用于游戏物理
Verlet积分：保持能量更好，适合约束系统
Runge-Kutta积分：更高精度，但计算开销更大

以下是半隐式欧拉积分器的实现：

cpp

// 半隐式欧拉积分器
class SemiImplicitEulerIntegrator {
public:
    // 更新刚体状态
    void Integrate(RigidBody* body, float dt) {
        if (body->IsStatic() || !body->IsActive()) {
            return;
        }
        
        // 获取当前状态
        Vector3 position = body->GetPosition();
        Quaternion rotation = body->GetRotation();
        Vector3 linearVelocity = body->GetLinearVelocity();
        Vector3 angularVelocity = body->GetAngularVelocity();
        
        // 获取力和转矩
        Vector3 force = body->GetForce();
        Vector3 torque = body->GetTorque();
        
        // 计算加速度
        float inverseMass = body->GetInverseMass();
        Vector3 linearAcceleration = force * inverseMass;
        
        // 更新线性速度（先速度，再位置 - 半隐式）
        linearVelocity += linearAcceleration * dt;
        
        // 应用线性阻尼
        float linearDamping = body->GetLinearDamping();
        linearVelocity *= pow(1.0f - linearDamping, dt);
        
        // 更新位置
        position += linearVelocity * dt;
        
        // 计算角加速度
        Matrix3x3 invInertiaTensor = body->GetInverseInertiaTensorWorld();
        Vector3 angularAcceleration = invInertiaTensor * torque;
        
        // 更新角速度
        angularVelocity += angularAcceleration * dt;
        
        // 应用角阻尼
        float angularDamping = body->GetAngularDamping();
        angularVelocity *= pow(1.0f - angularDamping, dt);
        
        // 更新旋转（使用四元数微分方程）
        Quaternion spin(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0.0f);
        Quaternion velocityQ = spin * rotation * 0.5f;
        
        rotation += velocityQ * dt;
        rotation = Quaternion::Normalize(rotation);
        
        // 更新刚体状态
        body->SetPosition(position);
        body->SetRotation(rotation);
        body->SetLinearVelocity(linearVelocity);
        body->SetAngularVelocity(angularVelocity);
        
        // 清除力和转矩
        body->ClearForces();
    }
};

12.5 整合物理引擎至游戏

将物理引擎与游戏逻辑集成是一个关键挑战。

12.5.1 物理与游戏对象同步

cpp

// 物理组件
class PhysicsComponent {
public:
    PhysicsComponent(GameObject* gameObj, PhysicsEngine* engine) : 
        gameObject(gameObj),
        physicsEngine(engine),
        rigidBody(nullptr) {}
    
    ~PhysicsComponent() {
        if (rigidBody) {
            // 从物理引擎中移除刚体
            physicsEngine->DestroyRigidBody(rigidBody);
        }
    }
    
    // 初始化物理组件
    void Initialize(const RigidBodyDesc& desc) {
        // 创建刚体
        rigidBody = physicsEngine->CreateRigidBody(desc);
        
        if (rigidBody) {
            // 设置用户数据指向游戏对象
            rigidBody->SetUserData(gameObject);
            
            // 同步初始变换
            SyncGameObjectToPhysics();
        }
    }
    
    // 更新组件
    void Update() {
        if (rigidBody && gameObject) {
            // 从物理引擎同步变换到游戏对象
            SyncPhysicsToGameObject();
        }
    }
    
    // 从游戏对象同步到物理
    void SyncGameObjectToPhysics() {
        if (rigidBody && gameObject) {
            Transform transform = gameObject->GetTransform();
            rigidBody->SetPosition(transform.position);
            rigidBody->SetRotation(transform.rotation);
        }
    }
    
    // 从物理同步到游戏对象
    void SyncPhysicsToGameObject() {
        if (rigidBody && gameObject) {
            Vector3 position = rigidBody->GetPosition();
            Quaternion rotation = rigidBody->GetRotation();
            
            Transform transform = gameObject->GetTransform();
            transform.position = position;
            transform.rotation = rotation;
            gameObject->SetTransform(transform);
        }
    }
    
    // 获取刚体
    RigidBody* GetRigidBody() const {
        return rigidBody;
    }
    
    // 应用力
    void ApplyForce(const Vector3& force) {
        if (rigidBody) {
            rigidBody->ApplyForce(force);
        }
    }
    
    // 应用力矩
    void ApplyTorque(const Vector3& torque) {
        if (rigidBody) {
            rigidBody->ApplyTorque(torque);
        }
    }
    
    // 设置线性速度
    void SetLinearVelocity(const Vector3& velocity) {
        if (rigidBody) {
            rigidBody->SetLinearVelocity(velocity);
        }
    }
    
    // 设置角速度
    void SetAngularVelocity(const Vector3& velocity) {
        if (rigidBody) {
            rigidBody->SetAngularVelocity(velocity);
        }
    }
    
private:
    GameObject* gameObject;
    PhysicsEngine* physicsEngine;
    RigidBody* rigidBody;
};

12.5.2 物理回调与事件处理

cpp

// 碰撞回调接口
class CollisionListener {
public:
    virtual ~CollisionListener() {}
    
    // 碰撞开始
    virtual void OnCollisionEnter(GameObject* self, GameObject* other) {}
    
    // 碰撞持续
    virtual void OnCollisionStay(GameObject* self, GameObject* other) {}
    
    // 碰撞结束
    virtual void OnCollisionExit(GameObject* self, GameObject* other) {}
    
    // 触发器进入
    virtual void OnTriggerEnter(GameObject* self, GameObject* other) {}
    
    // 触发器持续
    virtual void OnTriggerStay(GameObject* self, GameObject* other) {}
    
    // 触发器退出
    virtual void OnTriggerExit(GameObject* self, GameObject* other) {}
};

// 碰撞事件管理器
class CollisionEventManager {
public:
    void RegisterListener(GameObject* gameObject, CollisionListener* listener) {
        listeners[gameObject] = listener;
    }
    
    void UnregisterListener(GameObject* gameObject) {
        listeners.erase(gameObject);
    }
    
    // 处理当前帧的碰撞
    void ProcessCollisions(const std::vector<CollisionPair>& collisions) {
        // 记录当前帧的碰撞
        std::unordered_map<std::pair<GameObject*, GameObject*>, bool, PairHash> currentCollisions;
        
        // 处理当前帧的每个碰撞
        for (const auto& pair : collisions) {
            GameObject* objectA = static_cast<GameObject*>(pair.colliderA->GetUserData());
            GameObject* objectB = static_cast<GameObject*>(pair.colliderB->GetUserData());
            
            if (!objectA || !objectB) continue;
            
            // 生成唯一的碰撞对ID
            auto collisionPair = std::make_pair(objectA, objectB);
            auto collisionPairReverse = std::make_pair(objectB, objectA);
            
            // 标记为当前帧的碰撞
            currentCollisions[collisionPair] = true;
            
            // 检查是否是触发器碰撞
            bool isTrigger = pair.colliderA->IsTrigger() || pair.colliderB->IsTrigger();
            
            // 查找上一帧的碰撞状态
            bool isNewCollision = prevCollisions.find(collisionPair) == prevCollisions.end() &&
                                 prevCollisions.find(collisionPairReverse) == prevCollisions.end();
            
            // 通知监听器
            if (isTrigger) {
                // 触发器事件
                NotifyTrigger(objectA, objectB, isNewCollision);
                NotifyTrigger(objectB, objectA, isNewCollision);
            } else {
                // 碰撞事件
                NotifyCollision(objectA, objectB, isNewCollision);
                NotifyCollision(objectB, objectA, isNewCollision);
            }
        }
        
        // 检查退出的碰撞
        for (const auto& pair : prevCollisions) {
            if (currentCollisions.find(pair.first) == currentCollisions.end()) {
                // 碰撞已经退出
                GameObject* objectA = pair.first.first;
                GameObject* objectB = pair.first.second;
                
                // 检查是否是触发器
                Collider* colliderA = objectA->GetComponent<ColliderComponent>()->GetCollider();
                Collider* colliderB = objectB->GetComponent<ColliderComponent>()->GetCollider();
                
                bool isTrigger = colliderA->IsTrigger() || colliderB->IsTrigger();
                
                if (isTrigger) {
                    NotifyTriggerExit(objectA, objectB);
                } else {
                    NotifyCollisionExit(objectA, objectB);
                }
            }
        }
        
        // 更新上一帧的碰撞
        prevCollisions = currentCollisions;
    }
    
private:
    // 用于哈希std::pair的辅助结构
    struct PairHash {
        template <typename T1, typename T2>
        std::size_t operator () (const std::pair<T1, T2>& p) const {
            auto h1 = std::hash<T1>{}(p.first);
            auto h2 = std::hash<T2>{}(p.second);
            return h1 ^ h2;
        }
    };
    
    // 通知碰撞事件
    void NotifyCollision(GameObject* self, GameObject* other, bool isNew) {
        auto it = listeners.find(self);
        if (it != listeners.end() && it->second) {
            if (isNew) {
                it->second->OnCollisionEnter(self, other);
            } else {
                it->second->OnCollisionStay(self, other);
            }
        }
    }
    
    // 通知碰撞退出
    void NotifyCollisionExit(GameObject* self, GameObject* other) {
        auto it = listeners.find(self);
        if (it != listeners.end() && it->second) {
            it->second->OnCollisionExit(self, other);
        }
    }
    
    // 通知触发器事件
    void NotifyTrigger(GameObject* self, GameObject* other, bool isNew) {
        auto it = listeners.find(self);
        if (it != listeners.end() && it->second) {
            if (isNew) {
                it->second->OnTriggerEnter(self, other);
            } else {
                it->second->OnTriggerStay(self, other);
            }
        }
    }
    
    // 通知触发器退出
    void NotifyTriggerExit(GameObject* self, GameObject* other) {
        auto it = listeners.find(self);
        if (it != listeners.end() && it->second) {
            it->second->OnTriggerExit(self, other);
        }
    }
    
    std::unordered_map<GameObject*, CollisionListener*> listeners;
    std::unordered_map<std::pair<GameObject*, GameObject*>, bool, PairHash> prevCollisions;
};

12.5.3 射线检测与拾取

cpp

// 射线检测辅助类
class PhysicsRaycaster {
public:
    PhysicsRaycaster(PhysicsEngine* engine) : physicsEngine(engine) {}
    
    // 执行射线检测
    bool Raycast(const Ray& ray, RaycastHit& hit, float maxDistance = 1000.0f) {
        RayCastResult result;
        
        // 创建带有最大距离的射线
        Ray limitedRay = ray;
        limitedRay.maxDistance = maxDistance;
        
        // 调用物理引擎的射线检测
        if (physicsEngine->RayCast(limitedRay, result)) {
            // 填充命中信息
            hit.point = result.point;
            hit.normal = result.normal;
            hit.distance = result.distance;
            hit.collider = result.collider;
            
            // 获取相关的游戏对象
            if (hit.collider) {
                hit.gameObject = static_cast<GameObject*>(hit.collider->GetUserData());
            }
            
            return true;
        }
        
        return false;
    }
    
    // 执行球体检测
    bool SphereCast(const Ray& ray, float radius, RaycastHit& hit, float maxDistance = 1000.0f) {
        // 球体检测实现
        // ...
        
        return false;
    }
    
    // 获取一条射线经过的所有物体
    std::vector<RaycastHit> RaycastAll(const Ray& ray, float maxDistance = 1000.0f) {
        std::vector<RaycastHit> hits;
        
        // 实现多目标射线检测
        // ...
        
        return hits;
    }
    
private:
    PhysicsEngine* physicsEngine;
};

// 物体拾取示例
class ObjectPicker {
public:
    ObjectPicker(PhysicsRaycaster* raycaster) : raycaster(raycaster) {}
    
    // 从屏幕坐标拾取物体
    GameObject* PickObjectFromScreen(int screenX, int screenY, Camera* camera) {
        // 将屏幕坐标转换为射线
        Ray ray = camera->ScreenPointToRay(screenX, screenY);
        
        // 执行射线检测
        RaycastHit hit;
        if (raycaster->Raycast(ray, hit)) {
            return hit.gameObject;
        }
        
        return nullptr;
    }
    
    // 拖动物体
    void DragObject(GameObject* object, const Ray& ray, float distance) {
        if (!object) return;
        
        // 获取物体的物理组件
        PhysicsComponent* physics = object->GetComponent<PhysicsComponent>();
        if (!physics) return;
        
        // 计算目标位置
        Vector3 targetPosition = ray.origin + ray.direction * distance;
        
        // 对于非运动学刚体，可以施加力来移动
        RigidBody* rigidBody = physics->GetRigidBody();
        if (rigidBody && !rigidBody->IsKinematic()) {
            // 计算所需的力
            Vector3 currentPos = rigidBody->GetPosition();
            Vector3 velocity = rigidBody->GetLinearVelocity();
            
            // 使用弹簧公式计算力
            Vector3 direction = targetPosition - currentPos;
            float springStiffness = 50.0f;
            float dampingFactor = 5.0f;
            
            Vector3 force = direction * springStiffness - velocity * dampingFactor;
            
            // 应用力
            rigidBody->ApplyForce(force);
        } else if (rigidBody && rigidBody->IsKinematic()) {
            // 对于运动学刚体，直接设置位置
            rigidBody->SetPosition(targetPosition);
        }
    }
    
private:
    PhysicsRaycaster* raycaster;
};

12.6 展望：高级物理功能

现代物理引擎支持许多高级功能，包括但不限于：

12.6.1 布料模拟

cpp

// 布料模拟点
struct ClothVertex {
    Vector3 position;        // 当前位置
    Vector3 oldPosition;     // 上一帧位置
    Vector3 acceleration;    // 加速度
    float mass;              // 质量
    bool pinned;             // 是否固定
};

// 布料约束
struct ClothConstraint {
    int particleA;           // 粒子A索引
    int particleB;           // 粒子B索引
    float restLength;        // 约束长度
    float stiffness;         // 约束刚度
};

// 简单的布料模拟器
class ClothSimulator {
public:
    ClothSimulator(int width, int height, float particleDistance) :
        width(width), height(height), gravity(0, -9.8f, 0) {
        
        // 创建粒子网格
        particles.resize(width * height);
        
        // 初始化粒子位置
        for (int y = 0; y < height; ++y) {
            for (int x = 0; x < width; ++x) {
                int index = y * width + x;
                
                ClothVertex& vertex = particles[index];
                vertex.position = Vector3(x * particleDistance, 0, y * particleDistance);
                vertex.oldPosition = vertex.position;
                vertex.acceleration = Vector3::Zero();
                vertex.mass = 1.0f;
                vertex.pinned = false;
            }
        }
        
        // 固定顶部两角
        particles[0].pinned = true;
        particles[width - 1].pinned = true;
        
        // 创建结构约束
        for (int y = 0; y < height; ++y) {
            for (int x = 0; x < width; ++x) {
                int index = y * width + x;
                
                // 水平约束
                if (x < width - 1) {
                    AddConstraint(index, index + 1, particleDistance);
                }
                
                // 垂直约束
                if (y < height - 1) {
                    AddConstraint(index, index + width, particleDistance);
                }
                
                // 对角约束（增加稳定性）
                if (x < width - 1 && y < height - 1) {
                    AddConstraint(index, index + width + 1, particleDistance * 1.414f);
                    AddConstraint(index + 1, index + width, particleDistance * 1.414f);
                }
            }
        }
    }
    
    // 添加约束
    void AddConstraint(int particleA, int particleB, float restLength) {
        ClothConstraint constraint;
        constraint.particleA = particleA;
        constraint.particleB = particleB;
        constraint.restLength = restLength;
        constraint.stiffness = 0.9f;
        
        constraints.push_back(constraint);
    }
    
    // 更新模拟
    void Update(float deltaTime) {
        // 积分次数（越多越稳定，但计算量增加）
        const int iterations = 5;
        float dt = deltaTime / iterations;
        
        for (int iter = 0; iter < iterations; ++iter) {
            // 累积力
            AccumulateForces();
            
            // Verlet积分
            Integrate(dt);
            
            // 约束求解
            SolveConstraints();
            
            // 碰撞处理
            HandleCollisions();
        }
    }
    
    // 获取粒子位置
    const std::vector<ClothVertex>& GetParticles() const {
        return particles;
    }
    
private:
    // 累积力
    void AccumulateForces() {
        for (auto& particle : particles) {
            // 重置加速度
            particle.acceleration = Vector3::Zero();
            
            // 应用重力
            particle.acceleration += gravity;
        }
    }
    
    // Verlet积分
    void Integrate(float dt) {
        for (auto& particle : particles) {
            if (particle.pinned) continue;
            
            // 保存当前位置
            Vector3 temp = particle.position;
            
            // Verlet积分更新位置
            Vector3 velocity = particle.position - particle.oldPosition;
            particle.position += velocity + particle.acceleration * dt * dt;
            
            // 更新旧位置
            particle.oldPosition = temp;
        }
    }
    
    // 约束求解
    void SolveConstraints() {
        for (const auto& constraint : constraints) {
            ClothVertex& particleA = particles[constraint.particleA];
            ClothVertex& particleB = particles[constraint.particleB];
            
            // 计算当前长度
            Vector3 delta = particleB.position - particleA.position;
            float currentLength = delta.Length();
            
            if (currentLength < 0.0001f) continue;  // 避免除零
            
            // 计算校正
            float diff = (currentLength - constraint.restLength) / currentLength;
            Vector3 correction = delta * diff * 0.5f * constraint.stiffness;
            
            // 应用校正
            if (!particleA.pinned) {
                particleA.position += correction;
            }
            
            if (!particleB.pinned) {
                particleB.position -= correction;
            }
        }
    }
    
    // 碰撞处理
    void HandleCollisions() {
        // 简单的地面碰撞示例
        const float groundY = -5.0f;
        
        for (auto& particle : particles) {
            if (particle.pinned) continue;
            
            // 地面碰撞
            if (particle.position.y < groundY) {
                particle.position.y = groundY;
                
                // 摩擦效果
                Vector3 velocity = particle.position - particle.oldPosition;
                velocity.x *= 0.9f;  // 减少水平速度
                velocity.z *= 0.9f;
                
                particle.oldPosition = particle.position - velocity;
            }
        }
    }
    
    int width;
    int height;
    Vector3 gravity;
    std::vector<ClothVertex> particles;
    std::vector<ClothConstraint> constraints;
};

12.6.2 软体模拟

cpp

// 软体顶点
struct SoftBodyVertex {
    Vector3 position;
    Vector3 velocity;
    float mass;
    bool fixed;
};

// 软体弹簧
struct SoftBodySpring {
    int vertexA;
    int vertexB;
    float restLength;
    float stiffness;
    float damping;
};

// 简单的软体模拟器
class SoftBodySimulator {
public:
    SoftBodySimulator() : gravity(0, -9.8f, 0) {}
    
    // 添加顶点
    int AddVertex(const Vector3& position, float mass = 1.0f, bool fixed = false) {
        SoftBodyVertex vertex;
        vertex.position = position;
        vertex.velocity = Vector3::Zero();
        vertex.mass = mass;
        vertex.fixed = fixed;
        
        vertices.push_back(vertex);
        return vertices.size() - 1;
    }
    
    // 添加弹簧
    void AddSpring(int vertexA, int vertexB, float stiffness = 100.0f, float damping = 0.5f) {
        if (vertexA >= vertices.size() || vertexB >= vertices.size()) {
            return;
        }
        
        SoftBodySpring spring;
        spring.vertexA = vertexA;
        spring.vertexB = vertexB;
        spring.restLength = (vertices[vertexA].position - vertices[vertexB].position).Length();
        spring.stiffness = stiffness;
        spring.damping = damping;
        
        springs.push_back(spring);
    }
    
    // 更新模拟
    void Update(float deltaTime) {
        // 累积力
        std::vector<Vector3> forces(vertices.size(), Vector3::Zero());
        
        // 应用重力
        for (size_t i = 0; i < vertices.size(); ++i) {
            forces[i] += gravity * vertices[i].mass;
        }
        
        // 应用弹簧力
        for (const auto& spring : springs) {
            SoftBodyVertex& vertexA = vertices[spring.vertexA];
            SoftBodyVertex& vertexB = vertices[spring.vertexB];
            
            // 计算弹簧向量
            Vector3 delta = vertexB.position - vertexA.position;
            float length = delta.Length();
            
            if (length < 0.0001f) continue;  // 避免除零
            
            // 计算弹簧力
            Vector3 direction = delta / length;
            float extension = length - spring.restLength;
            
            // 胡克定律：F = -k * x
            Vector3 springForce = direction * extension * spring.stiffness;
            
            // 阻尼力：F = -c * v
            Vector3 relativeVelocity = vertexB.velocity - vertexA.velocity;
            Vector3 dampingForce = direction * Vector3::Dot(relativeVelocity, direction) * spring.damping;
            
            Vector3 totalForce = springForce + dampingForce;
            
            // 应用力
            if (!vertexA.fixed) {
                forces[spring.vertexA] += totalForce;
            }
            
            if (!vertexB.fixed) {
                forces[spring.vertexB] -= totalForce;
            }
        }
        
        // 积分更新位置和速度
        for (size_t i = 0; i < vertices.size(); ++i) {
            SoftBodyVertex& vertex = vertices[i];
            
            if (vertex.fixed) continue;
            
            // F = ma => a = F/m
            Vector3 acceleration = forces[i] / vertex.mass;
            
            // 更新速度（半隐式欧拉）
            vertex.velocity += acceleration * deltaTime;
            
            // 简单阻尼
            vertex.velocity *= 0.99f;
            
            // 更新位置
            vertex.position += vertex.velocity * deltaTime;
        }
        
        // 碰撞处理
        HandleCollisions();
    }
    
    // 获取顶点位置
    const std::vector<SoftBodyVertex>& GetVertices() const {
        return vertices;
    }
    
    // 获取弹簧
    const std::vector<SoftBodySpring>& GetSprings() const {
        return springs;
    }
    
private:
    // 碰撞处理
    void HandleCollisions() {
        // 简单的地面碰撞示例
        const float groundY = -5.0f;
        
        for (auto& vertex : vertices) {
            if (vertex.fixed) continue;
            
            // 地面碰撞
            if (vertex.position.y < groundY) {
                vertex.position.y = groundY;
                
                // 反弹
                if (vertex.velocity.y < 0) {
                    vertex.velocity.y = -vertex.velocity.y * 0.5f;  // 反弹系数
                    
                    // 摩擦效果
                    vertex.velocity.x *= 0.9f;
                    vertex.velocity.z *= 0.9f;
                }
            }
        }
    }
    
    Vector3 gravity;
    std::vector<SoftBodyVertex> vertices;
    std::vector<SoftBodySpring> springs;
};

12.6.3 车辆物理

cpp

// 车轮模拟
struct Wheel {
    Vector3 localPosition;   // 相对于车体的位置
    float radius;            // 轮胎半径
    float suspensionLength;  // 悬挂长度
    float suspensionStiffness; // 悬挂刚度
    float suspensionDamping;   // 悬挂阻尼
    float lateralStiffness;    // 侧向刚度（防滑）
    
    // 运行时数据
    float suspensionCompression; // 当前悬挂压缩量
    bool isGrounded;             // 是否接地
    Vector3 contactPoint;        // 接触点
    Vector3 contactNormal;       // 接触法线
    float wheelRotation;         // 轮胎旋转角度
    float steerAngle;            // 转向角度
};

// 车辆物理模拟
class VehiclePhysics {
public:
    VehiclePhysics(PhysicsEngine* engine, RigidBody* chassisBody) :
        physicsEngine(engine),
        chassisBody(chassisBody),
        engineForce(0.0f),
        brakeForce(0.0f),
        steeringAngle(0.0f) {}
    
    // 添加车轮
    void AddWheel(const Wheel& wheel) {
        wheels.push_back(wheel);
    }
    
    // 设置引擎力
    void SetEngineForce(float force) {
        engineForce = force;
    }
    
    // 设置刹车力
    void SetBrakeForce(float force) {
        brakeForce = force;
    }
    
    // 设置转向角度
    void SetSteeringAngle(float angle) {
        steeringAngle = angle;
    }
    
    // 更新车辆物理
    void Update(float deltaTime) {
        // 对每个车轮进行物理计算
        for (auto& wheel : wheels) {
            // 射线检测悬挂
            UpdateSuspension(wheel);
            
            // 应用悬挂力
            ApplySuspensionForce(wheel, deltaTime);
            
            // 更新轮胎旋转
            UpdateWheelRotation(wheel, deltaTime);
            
            // 处理转向
            if (wheel.localPosition.x < 0) {  // 前轮
                wheel.steerAngle = steeringAngle;
            } else {
                wheel.steerAngle = 0.0f;
            }
        }
    }
    
private:
    // 更新悬挂
    void UpdateSuspension(Wheel& wheel) {
        // 计算轮胎的世界位置
        Vector3 wheelPos = chassisBody->LocalToWorldPoint(wheel.localPosition);
        
        // 计算悬挂方向（默认向下）
        Vector3 suspensionDir = chassisBody->LocalToWorldDirection(Vector3(0, -1, 0));
        
        // 射线检测
        Ray ray(wheelPos, suspensionDir);
        ray.maxDistance = wheel.suspensionLength + wheel.radius;
        
        RayCastResult result;
        if (physicsEngine->RayCast(ray, result)) {
            wheel.isGrounded = true;
            wheel.contactPoint = result.point;
            wheel.contactNormal = result.normal;
            
            // 计算悬挂压缩量
            float rayLength = result.distance;
            wheel.suspensionCompression = wheel.suspensionLength + wheel.radius - rayLength;
        } else {
            wheel.isGrounded = false;
            wheel.suspensionCompression = 0.0f;
        }
    }
    
    // 应用悬挂力
    void ApplySuspensionForce(Wheel& wheel, float deltaTime) {
        if (!wheel.isGrounded) return;
        
        // 计算悬挂力（弹簧-阻尼模型）
        float springForce = wheel.suspensionCompression * wheel.suspensionStiffness;
        
        // 计算轮胎在世界空间中的速度
        Vector3 wheelWorldPos = chassisBody->LocalToWorldPoint(wheel.localPosition);
        Vector3 wheelVelocity = chassisBody->GetPointVelocity(wheelWorldPos);
        
        // 计算沿悬挂方向的速度分量
        Vector3 suspensionDir = chassisBody->LocalToWorldDirection(Vector3(0, -1, 0));
        float suspensionVelocity = Vector3::Dot(wheelVelocity, suspensionDir);
        
        // 阻尼力
        float dampingForce = suspensionVelocity * wheel.suspensionDamping;
        
        // 总悬挂力
        float suspensionForce = springForce - dampingForce;
        
        // 应用悬挂力到车体
        chassisBody->ApplyForceAtPoint(suspensionDir * suspensionForce, wheelWorldPos);
        
        // 处理引擎力和刹车力
        if (wheel.localPosition.z < 0) {  // 后轮驱动
            // 计算车轮前进方向
            Vector3 wheelForwardDir = chassisBody->LocalToWorldDirection(
                Quaternion::RotateVector(
                    Quaternion::CreateFromAxisAngle(Vector3(0, 1, 0), wheel.steerAngle),
                    Vector3(0, 0, 1)
                )
            );
            
            // 应用引擎力
            chassisBody->ApplyForceAtPoint(wheelForwardDir * engineForce, wheelWorldPos);
            
            // 应用刹车力
            Vector3 forwardVelocity = wheelForwardDir * Vector3::Dot(wheelVelocity, wheelForwardDir);
            if (forwardVelocity.LengthSquared() > 0.01f) {
                Vector3 brakeDir = -forwardVelocity.Normalized();
                chassisBody->ApplyForceAtPoint(brakeDir * brakeForce, wheelWorldPos);
            }
        }
        
        // 侧向防滑力（横向摩擦）
        Vector3 wheelRightDir = chassisBody->LocalToWorldDirection(
            Quaternion::RotateVector(
                Quaternion::CreateFromAxisAngle(Vector3(0, 1, 0), wheel.steerAngle),
                Vector3(1, 0, 0)
            )
        );
        
        // 计算横向速度
        float lateralVelocity = Vector3::Dot(wheelVelocity, wheelRightDir);
        
        // 应用侧向力
        chassisBody->ApplyForceAtPoint(wheelRightDir * -lateralVelocity * wheel.lateralStiffness, 
                                     wheelWorldPos);
    }
    
    // 更新轮胎旋转
    void UpdateWheelRotation(Wheel& wheel, float deltaTime) {
        if (!wheel.isGrounded) return;
        
        // 计算轮胎在世界空间中的速度
        Vector3 wheelWorldPos = chassisBody->LocalToWorldPoint(wheel.localPosition);
        Vector3 wheelVelocity = chassisBody->GetPointVelocity(wheelWorldPos);
        
        // 计算沿轮胎前进方向的速度分量
        Vector3 wheelForwardDir = chassisBody->LocalToWorldDirection(
            Quaternion::RotateVector(
                Quaternion::CreateFromAxisAngle(Vector3(0, 1, 0), wheel.steerAngle),
                Vector3(0, 0, 1)
            )
        );
        
        float forwardVelocity = Vector3::Dot(wheelVelocity, wheelForwardDir);
        
        // 计算轮胎旋转速度
        float rotationSpeed = forwardVelocity / wheel.radius;
        
        // 更新轮胎旋转角度
        wheel.wheelRotation += rotationSpeed * deltaTime;
        
        // 规范化到0-2π
        wheel.wheelRotation = fmod(wheel.wheelRotation, 2.0f * 3.14159f);
    }
    
    PhysicsEngine* physicsEngine;
    RigidBody* chassisBody;
    std::vector<Wheel> wheels;
    
    float engineForce;
    float brakeForce;
    float steeringAngle;
};

游戏物理是一个深入且广泛的领域，本章只是介绍了其基础概念和实现方法。随着游戏技术的发展，物理系统将继续演进，提供更真实、更高效的模拟效果，为玩家创造更具沉浸感的游戏体验。