reactphysics3d入门与探索

简介

ReactPhysics3D is a C++ physics engine library that can be used in 3D simulations and games.

主要想尝试一下RL，想先弄个物理引擎，这样方便模拟场景。看了一圈，虽然也有不少Python的，但是好多都要自己写update函数，各种运动要自己算，想想还是算了吧，希望搞一个比较General的物理引擎，这样搭复杂模型会很简单，大不了做个interprocess communication (ipc) 接口，用Python搭RL模型控制下完事。
所以想要物理引擎尽可能light weight，库小一些，feature不那么多，能模拟就行，document好一些，方便自己修改。
最后看上了 ReactPhysics3D。
feature list，我比较看重的特性都粗体了：

• Rigid body dynamics
• Discrete collision detection
• Collision shapes (Sphere, Box, Capsule, Convex Mesh, Static Concave Mesh, Height Field)
• Multiple collision shapes per body
• Broadphase collision detection (Dynamic AABB tree)
• Narrowphase collision detection (SAT/GJK)
• Collision response and friction (Sequential Impulses Solver)
• Joints (Ball and Socket, Hinge, Slider, Fixed)
• Collision filtering with categories
• Ray casting
• Sleeping technique for inactive bodies
• Multi-platform (Windows, Linux, Mac OS X)
• No external libraries (do not use STL containers)
• Documentation (user manual and Doxygen API)
• Testbed application with demos
• Integrated Profiler
• Logs
• Unit tests

初步了解项目运行过程

看了下代码，不多，比较规整，有一个完整的看起来很方便改的Demo APP。看起来很完美的符合我的需求了。

CMakeLists.txt

构建方式cmake，直接搞到clion里面。root的cmakelist.txt就270行，包含了众多header文件和cpp文件，实际结构非常简单。构建目标主要就是 ADD_LIBRARY(reactphysics3d ${REACTPHYSICS3D_HEADERS} ${REACTPHYSICS3D_SOURCES})。包含了两个子目录：

IF(RP3D_COMPILE_TESTS)
   add_subdirectory(test/)
ENDIF()

IF(RP3D_COMPILE_TESTBED)
  	add_subdirectory(testbed/)
ENDIF()

显然一个测试一个是feature list 里面提及的 Demo。
去掉demo的 IF，直接编译运行，demo 没问题。

Main函数

下面可以开始读代码理解他的逻辑了。
main 入口函数很简单：

// Libraries
#include "TestbedApplication.h"
#include "nanogui/nanogui.h"

using namespace nanogui;

// Main function
int main(int argc, char** argv) {

    nanogui::init();

    {
        // Create and start the testbed application
        bool isFullscreen = false;
        nanogui::ref<TestbedApplication> application = new TestbedApplication(isFullscreen);
        application->setVisible(true);

        nanogui::mainloop();
    }

    nanogui::shutdown();

    return 0;
}

实例化了一下TestbedApplication，然后进 mainloop。

GUI框架概览

class TestbedApplication : public Screen {
...
}

查看TestbedApplication的定义，可以看到继承了Screen，下面是精简的Screen的定义，列出了主要的修改、初始化Screen的函数，Screen中protected以及OpenGL相关的函数都略去了。

class NANOGUI_EXPORT Screen : public Widget {
    friend class Widget;
    friend class Window;
public:
    Screen(const Vector2i &size, const std::string &caption,
           bool resizable = true, bool fullscreen = false, int colorBits = 8,
           int alphaBits = 8, int depthBits = 24, int stencilBits = 8,
           int nSamples = 0,
           unsigned int glMajor = 3, unsigned int glMinor = 3);
    const std::string &caption() const { return mCaption; }
    void setCaption(const std::string &caption);
    const Color &background() const { return mBackground; }
    void setBackground(const Color &background) { mBackground = background; }
    void setVisible(bool visible);
    void setSize(const Vector2i& size);
    virtual void drawAll();
    virtual void drawContents() { /* To be overridden */ }
    float pixelRatio() const { return mPixelRatio; }
    virtual bool dropEvent(const std::vector<std::string> & /* filenames */) { return false; /* To be overridden */ }
    virtual bool keyboardEvent(int key, int scancode, int action, int modifiers);
    virtual bool keyboardCharacterEvent(unsigned int codepoint);
    virtual bool resizeEvent(const Vector2i& size);
    std::function<void(Vector2i)> resizeCallback() const { return mResizeCallback; }
    void setResizeCallback(const std::function<void(Vector2i)> &callback) { mResizeCallback = callback; }
    Vector2i mousePos() const { return mMousePos; }
    GLFWwindow *glfwWindow() { return mGLFWWindow; }
    NVGcontext *nvgContext() { return mNVGContext; }
    void setShutdownGLFWOnDestruct(bool v) { mShutdownGLFWOnDestruct = v; }
    bool shutdownGLFWOnDestruct() { return mShutdownGLFWOnDestruct; }
    void performLayout() {
        Widget::performLayout(mNVGContext);
    }
    ......
    ......
};

可以看到Screen继承了Widget，在Doc中作者说明了:

src/screen.cpp – Top-level widget and interface between NanoGUI and GLFW

所以Screen是在NanoGUI和OpenGL的中间层，也就是Widget处理各种交互过程，而Screen调用OpenGL，拿到用户输入传递给对应的Widget，可以看下mouseButtonCallbackEvent的代码:

bool Screen::mouseButtonCallbackEvent(int button, int action, int modifiers) {
    mModifiers = modifiers;
    mLastInteraction = glfwGetTime();
    try {
        if (mFocusPath.size() > 1) {
            const Window *window =
                dynamic_cast<Window *>(mFocusPath[mFocusPath.size() - 2]);
            if (window && window->modal()) {
                if (!window->contains(mMousePos))
                    return false;
            }
        }

        if (action == GLFW_PRESS)
            mMouseState |= 1 << button;
        else
            mMouseState &= ~(1 << button);

        auto dropWidget = findWidget(mMousePos);
        if (mDragActive && action == GLFW_RELEASE &&
            dropWidget != mDragWidget)
            mDragWidget->mouseButtonEvent(
                mMousePos - mDragWidget->parent()->absolutePosition(), button,
                false, mModifiers);

        if (dropWidget != nullptr && dropWidget->cursor() != mCursor) {
            mCursor = dropWidget->cursor();
            glfwSetCursor(mGLFWWindow, mCursors[(int) mCursor]);
        }

        if (action == GLFW_PRESS && (button == GLFW_MOUSE_BUTTON_1 || button == GLFW_MOUSE_BUTTON_2)) {
            mDragWidget = findWidget(mMousePos);
            if (mDragWidget == this)
                mDragWidget = nullptr;
            mDragActive = mDragWidget != nullptr;
            if (!mDragActive)
                updateFocus(nullptr);
        } else {
            mDragActive = false;
            mDragWidget = nullptr;
        }

        return mouseButtonEvent(mMousePos, button, action == GLFW_PRESS,
                                mModifiers);
    } catch (const std::exception &e) {
        std::cerr << "Caught exception in event handler: " << e.what() << std::endl;
        return false;
    }
}

逻辑很清楚，更新Focus,处理拖动，通过findWidget函数处理拖动，然后调用继承自Widget的mouseButtonEvent函数，这样Widget就成了OpenGL无关的GUI部分，通过Screen的连接，实现了接耦合。

DemoAPP解析

由于本次探索重点不是GUI部分，主要是如何构建场景，把实验跑起来，所以不在继续深入GUI部分，回去看TestbedApplication中如何加入场景。

Demo中是可以选择场景的，作者已经写好了7个Demo场景，找到对应的代码就可以知道如何使用API了。在TestbedApplication中可以看到这样的初始化代码：

void TestbedApplication::init() {

    // Create all the scenes
    createScenes();

    // Initialize the GUI
    mGui.init();

    mTimer.start();

    mIsInitialized = true;
}

接着找 createScenes函数：

void TestbedApplication::createScenes() {

    // Cubes scene
    CubesScene* cubeScene = new CubesScene("Cubes", mEngineSettings);
    mScenes.push_back(cubeScene);

    // Cube Stack scene
    CubeStackScene* cubeStackScene = new CubeStackScene("Cube Stack", mEngineSettings);
    mScenes.push_back(cubeStackScene);

    // Joints scene
    JointsScene* jointsScene = new JointsScene("Joints", mEngineSettings);
    mScenes.push_back(jointsScene);

    // Collision shapes scene
    CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes", mEngineSettings);
    mScenes.push_back(collisionShapesScene);

    // Heightfield shape scene
    HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield", mEngineSettings);
    mScenes.push_back(heightFieldScene);

    // Raycast scene
    RaycastScene* raycastScene = new RaycastScene("Raycast", mEngineSettings);
    mScenes.push_back(raycastScene);

    // Collision Detection scene
    CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection", mEngineSettings);
    mScenes.push_back(collisionDetectionScene);

    // Concave Mesh scene
    ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh", mEngineSettings);
    mScenes.push_back(concaveMeshScene);

    assert(mScenes.size() > 0);

    const int firstSceneIndex = 0;

    switchScene(mScenes[firstSceneIndex]);
}

逻辑很明确了，依次创建7个场景，加入到mScenes中，调用switchScene切换场景。下面看下switchScene：

void TestbedApplication::switchScene(Scene* newScene) {

    if (newScene == mCurrentScene) return;

    mCurrentScene = newScene;

    // Reset the scene
    mCurrentScene->reset();

    mCurrentScene->updateEngineSettings();

    resizeEvent(Vector2i(0, 0));
}

代码非常简单，更新下成员变量mCurrentScene，然后调用reset和updateEngineSettings，字面理解用于初始化场景，接着发一个resizeEvent，接着看下这个resizeEvent:

bool TestbedApplication::resizeEvent(const Vector2i& size) {

    if (!mIsInitialized) return false;

    // Get the framebuffer dimension
    int width, height;
    glfwGetFramebufferSize(mGLFWWindow, &width, &height);

    // Resize the camera viewport
    mCurrentScene->reshape(width, height);

    // Update the window size of the scene
    int windowWidth, windowHeight;
    glfwGetWindowSize(mGLFWWindow, &windowWidth, &windowHeight);
    mCurrentScene->setWindowDimension(windowWidth, windowHeight);

    return true;
}

可以看到和场景相关的部分就是mCurrentScene->reshape;和mCurrentScene->setWindowDimension这两个函数调用，字面理解为设置渲染窗口大小，和构建模型没什么关系。
总结：
到目前位置，基本弄清了整个流程。

1. TestbedApplication是入口，在初始化过程中调用createScenes成员函数
2. createScenes创建场景实例，然后调用switchScene切换场景
3. witchScene调用场景的reset和updateEngineSettings两个成员函数初始化场景。然后场景就载入了。

弄清了基础流程就可以开始看场景构建了，先从最简单的CubesScene开始。

场景创建

testbed目录树如下

.
├── CMakeLists.txt
├── scenes
│   ├── collisiondetection
│   │   ├── CollisionDetectionScene.cpp
│   │   └── CollisionDetectionScene.h
│   ├── collisionshapes
│   │   ├── CollisionShapesScene.cpp
│   │   └── CollisionShapesScene.h
│   ├── concavemesh
│   │   ├── ConcaveMeshScene.cpp
│   │   └── ConcaveMeshScene.h
│   ├── cubes
│   │   ├── CubesScene.cpp
│   │   └── CubesScene.h
│   ├── cubestack
│   │   ├── CubeStackScene.cpp
│   │   └── CubeStackScene.h
│   ├── heightfield
│   │   ├── HeightFieldScene.cpp
│   │   └── HeightFieldScene.h
│   ├── joints
│   │   ├── JointsScene.cpp
│   │   └── JointsScene.h
│   └── raycast
│       ├── RaycastScene.cpp
│       └── RaycastScene.h
├── src
│   ├── Gui.cpp
│   ├── Gui.h
│   ├── Main.cpp
│   ├── Scene.cpp
│   ├── SceneDemo.cpp
│   ├── SceneDemo.h
│   ├── Scene.h
│   ├── TestbedApplication.cpp
│   ├── TestbedApplication.h
│   ├── Timer.cpp
│   └── Timer.h
└── VisualStudioUserTemplate.user

所有场景都在scenes下面。先从CubesScene开始。

class CubesScene : public SceneDemo {
    protected :
        // -------------------- Attributes -------------------- //
        /// All the boxes of the scene
        std::vector<Box*> mBoxes;
        /// Box for the floor
        Box* mFloor;
    public:
        // -------------------- Methods -------------------- //
        /// Constructor
        CubesScene(const std::string& name, EngineSettings& settings);
        /// Destructor
        ~CubesScene() override;
        /// Reset the scene
        void reset() override;
        /// Return all the contact points of the scene
        std::vector<ContactPoint> getContactPoints() override;
};

从前面的探索中可知，主要的工作在reset函数中完成。

CubesScene::CubesScene(const std::string& name, EngineSettings& settings)
      : SceneDemo(name, settings, SCENE_RADIUS) {
......
    // Create the dynamics world for the physics simulation
    mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);

    // Create all the cubes of the scene
    for (int i=0; i<NB_CUBES; i++) {

        // Create a cube and a corresponding rigid in the dynamics world
        Box* cube = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);

        // Set the box color
        cube->setColor(mDemoColors[i % mNbDemoColors]);
        cube->setSleepingColor(mRedColorDemo);

        // Change the material properties of the rigid body
        rp3d::Material& material = cube->getRigidBody()->getMaterial();
        material.setBounciness(rp3d::decimal(0.4));

        // Add the box the list of box in the scene
        mBoxes.push_back(cube);
        mPhysicsObjects.push_back(cube);
    }
......
}

在CubesScene的构造函数中创建了若干Box并存储在mBoxes成员变量中，并创建了mPhysicsWorld，从文档中看到，需要首先创建DynamicsWorld，然后使用DynamicsWorld的api可以进行模拟，DynamicsWorld会计算碰撞，受力等，并根据创建的约束更新场景。下面再看下reset函数：

void CubesScene::reset() {
    float radius = 2.0f;
    // Create all the cubes of the scene
    std::vector<Box*>::iterator it;
    int i = 0;
    for (it = mBoxes.begin(); it != mBoxes.end(); ++it) {
        // Position of the cubes
       float angle = i * 1.0f;
       rp3d::Vector3 position(radius * std::cos(angle),
                              10 + i * (BOX_SIZE.y + 0.3f),
                              radius * std::sin(angle));
       (*it)->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));

       i++;
    }
    mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));
}

reset函数中使用setTransform给每个Box新的位置。代码很简单。其他函数，比如更新更新物体等，应该在他是父类SceneDemo中定义。接下来可以看一下SceneDemo都做了什么。

class SceneDemo : public Scene {
    protected:
        // -------------------- Attributes -------------------- //
        ......
		std::vector<PhysicsObject*> mPhysicsObjects;
		rp3d::CollisionWorld* mPhysicsWorld;
        // -------------------- Methods -------------------- //
		......
        // Update the contact points
        void updateContactPoints();
        // Render the contact points
        void renderContactPoints(openglframework::Shader& shader,
                                 const openglframework::Matrix4& worldToCameraMatrix);
        /// Remove all contact points
        void removeAllContactPoints();
		......
    public:
        // -------------------- Methods -------------------- //
        /// Constructor
        SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled = true);
        ...
        /// Update the scene
        virtual void update() override;
		/// Update the physics world (take a simulation step)
		/// Can be called several times per frame
		virtual void updatePhysics() override;
        /// Render the scene (possibly in multiple passes for shadow mapping)
        virtual void render() override;
        /// Update the engine settings
        virtual void updateEngineSettings() override;
        /// Render the scene in a single pass
        virtual void renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
        /// Return all the contact points of the scene
        std::vector<ContactPoint> computeContactPointsOfWorld(reactphysics3d::DynamicsWorld *world);
};

首先是protected成员变量，定义了很多OpenGL相关的变量，包括各种参数和shader，此处略去了，和引擎相关的有两个变量：

		std::vector<PhysicsObject*> mPhysicsObjects;
        rp3d::CollisionWorld* mPhysicsWorld;

返回前面CubesScene的初始化代码可以看到这两个变量是在父累中定义，子类中初始化。去除setter和一些OpenGL的函数后，可以看到SceneDemo定义中剩下了由数个 update 函数构成，其中和物理引擎交互的部分应该主要为updatePhysics。为了快速了解什么时候调用了物理引擎模拟函数，直接在SceneDemo.cpp文件里面搜索上面的两个变量，其中mPhysicsObjects出现在了数个render函数中，略去不看，而mPhysicsObjects仅出现在了updatePhysics函数中，看来和物理相关的部分在updatePhysics中。

// Update the physics world (take a simulation step)
// Can be called several times per frame
void SceneDemo::updatePhysics() {

    if (getDynamicsWorld() != nullptr) {

        // Take a simulation step
        getDynamicsWorld()->update(mEngineSettings.timeStep);
    }
}

函数很简单，检查非空指针后调用了update函数让物理引擎进行一个步长的更新。再次搜索updatePhysics发现这个函数并没有在DemoScene中调用。继续查看父类Scene。
可以看到在Scene.cpp中可以看到大量处理鼠标和摄像机的代码，而Scene没有父类。所以到目前位置，Scene这一分支已经探索完毕：

1. 父类Scene，定义各种protected变量，以及众多setter，由于所有场景都使用鼠标拖动进行摄像机、视角等参数的变换，所以Scene作为所有场景的父类，处理鼠标操作和视野变换。
2. SceneDemo为Demo定制版场景。由于demo中使用相同的渲染、更新方法，SceneDemo重载了render和各种update函。
3. CubesScene针对具体场景，负责创建物体，动态世界，以及重置物体。

总结：
这一部分主要了解了这些内容：

1. 创建场景的流程：
   1. 实例化DynamicsWorld，
   2. 创建物体，创建物体的过程中，之前实例化的DynamicsWorld会被作为参数传入，以关联物体和创建的世界。
2. 大致弄懂了APP的初始化流程。

下一步探索 mainLoop中如何更新物理环境，如何与物理引擎交互。下一步探索 mainLoop中如何更新物理环境，如何与物理引擎交互。

mainLoop与物理引擎

ReactPhysics3D的GUI部分使用的是nanoGUI，查看nanoGUI的文档：

Function Documentation
void nanogui::mainloop(int refresh = 50)
Enter the application main loop.
Parameters
refresh: NanoGUI issues a redraw call whenever an keyboard/mouse/… event is received. In the absence of any external events, it enforces a redraw once every refresh milliseconds. To disable the refresh timer, specify a negative value here.

根据文档，DEMO中直接用了不带参数的nanogui::mainloop();，在没有输入的情况下，每50ms会重新更新画面。查看TestbedApplication的定义，TestbedApplication只重载了drawContents函数，这个函数在Screen的drawAll函数中被调用，而drawAll在mainloop里面调用，所以TestbedApplication中的drawContents每帧画面会调用一次。

void TestbedApplication::drawContents() {
    update();
    int bufferWidth, bufferHeight;
    glfwMakeContextCurrent(mGLFWWindow);
    glfwGetFramebufferSize(mGLFWWindow, &bufferWidth, &bufferHeight);
    // Set the viewport of the scene
    mCurrentScene->setViewport(0, 0, bufferWidth, bufferHeight);
    // Render the scene
    mCurrentScene->render();
    // Check the OpenGL errors
    checkOpenGLErrors();
    mGui.update();
    // Compute the current framerate
    computeFPS();
}

代码中除了GUI部分，还调用了update：

void TestbedApplication::update() {
    double currentTime = glfwGetTime();
    // Update the physics
    if (mSinglePhysicsStepEnabled && !mSinglePhysicsStepDone) {
        updateSinglePhysicsStep();
        mSinglePhysicsStepDone = true;
    }
    else {
        updatePhysics();
    }
    // Compute the physics update time
    mPhysicsTime = glfwGetTime() - currentTime;
    // Compute the interpolation factor
    float factor = mTimer.computeInterpolationFactor(mEngineSettings.timeStep);
    assert(factor >= 0.0f && factor <= 1.0f);
    // Notify the scene about the interpolation factor
    mCurrentScene->setInterpolationFactor(factor);
    // Enable/Disable shadow mapping
    mCurrentScene->setIsShadowMappingEnabled(mIsShadowMappingEnabled);
    // Display/Hide contact points
    mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed);
    // Display/Hide the AABBs
    mCurrentScene->setIsAABBsDisplayed(mIsAABBsDisplayed);
    // Enable/Disable wireframe mode
    mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled);
    // Update the scene
    mCurrentScene->update();
}

可以看到两个物理引擎相关的调用：

1. updateSinglePhysicsStep
2. updatePhysics

// Update the physics of the current scene
void TestbedApplication::updatePhysics() {

    // Update the elapsed time
    mEngineSettings.elapsedTime = mTimer.getPhysicsTime();

    if (mTimer.isRunning()) {

        // Compute the time since the last update() call and update the timer
        mTimer.update();

        // While the time accumulator is not empty
        while(mTimer.isPossibleToTakeStep(mEngineSettings.timeStep)) {

            // Take a physics simulation step
            mCurrentScene->updatePhysics();

            // Update the timer
            mTimer.nextStep(mEngineSettings.timeStep);
        }
    }
}
void TestbedApplication::updateSinglePhysicsStep() {

    assert(!mTimer.isRunning());

    mCurrentScene->updatePhysics();
}

查看timer定义可以看到，mTimer.nextStep(mEngineSettings.timeStep);会从当前时间累加器中减去mEngineSettings.timeStep，而mTimer.update();会给时间累加器中加上两次update的时间差，所以当累加器大于0 的时候，说明两次update时间差大于物理引擎的更新步长，此时应该不断更新物理引擎，直到物理引擎的模拟时间和实际时间一直。这样的更新机制分离了渲染的更新步长和引擎的更新步长，让两者之间没有任何关联但却能保持同步。

至此，已经弄懂了Demo的运行机制，下一步，为了方便快速开发，我准备重用这一部分代码，增加一个自己的场景，主要的工作如下：

1. 构建一个单自由度倒立摆模型，
2. 从物理引擎中拿到倒立摆的偏移角度，
3. 开发控制接口，让控制参数能够影响物理模型，
4. 用PID算法控制倒立摆，
5. 训练RL模型控制倒立摆。