From e491e3814630d4371e411271812340653c3c18d6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 27 Jan 2017 20:26:56 +0100 Subject: [PATCH 001/105] Fix issue in collision detection --- src/collision/CollisionDetection.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 345c46bb..be17e4f7 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -605,7 +605,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo); } - return nullptr; + return narrowPhaseInfo; } // Report all the bodies that overlap with the aabb in parameter @@ -976,7 +976,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // For each possible collision pair of bodies map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { OverlappingPair* pair = it->second; From d1cb0d92756963afcb8b27fd8c39658c95cc03cb Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 28 Jan 2017 00:22:22 +0100 Subject: [PATCH 002/105] Refactor testbed application, Add collision detection scene --- testbed/CMakeLists.txt | 2 + testbed/common/Box.cpp | 161 +++---- testbed/common/Box.h | 30 +- testbed/common/Capsule.cpp | 10 +- testbed/common/Capsule.h | 2 +- testbed/common/ConcaveMesh.cpp | 10 +- testbed/common/ConcaveMesh.h | 2 +- testbed/common/Cone.cpp | 10 +- testbed/common/Cone.h | 2 +- testbed/common/ConvexMesh.cpp | 10 +- testbed/common/ConvexMesh.h | 2 +- testbed/common/Cylinder.cpp | 10 +- testbed/common/Cylinder.h | 2 +- testbed/common/Dumbbell.cpp | 10 +- testbed/common/Dumbbell.h | 2 +- testbed/common/HeightField.cpp | 10 +- testbed/common/HeightField.h | 2 +- testbed/common/Sphere.cpp | 11 +- testbed/common/Sphere.h | 2 +- testbed/meshes/cube.obj | 17 + .../CollisionDetectionScene.cpp | 359 +++++++++++++++ .../CollisionDetectionScene.h | 235 ++++++++++ .../collisionshapes/CollisionShapesScene.cpp | 20 +- .../scenes/concavemesh/ConcaveMeshScene.cpp | 6 +- testbed/scenes/cubes/CubesScene.cpp | 408 +++++++++--------- testbed/scenes/cubes/CubesScene.h | 192 ++++----- .../scenes/heightfield/HeightFieldScene.cpp | 6 +- testbed/scenes/joints/JointsScene.cpp | 28 +- testbed/scenes/raycast/RaycastScene.cpp | 20 +- testbed/src/Gui.cpp | 7 + testbed/src/Scene.cpp | 2 +- testbed/src/Scene.h | 19 + testbed/src/TestbedApplication.cpp | 12 +- testbed/src/TestbedApplication.h | 3 + 34 files changed, 1166 insertions(+), 458 deletions(-) create mode 100644 testbed/meshes/cube.obj create mode 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.cpp create mode 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.h diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index 808ded2f..8d402ad3 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -110,6 +110,8 @@ SET(SCENES_SOURCES scenes/raycast/RaycastScene.cpp scenes/collisionshapes/CollisionShapesScene.h scenes/collisionshapes/CollisionShapesScene.cpp + scenes/collisiondetection/CollisionDetectionScene.h + scenes/collisiondetection/CollisionDetectionScene.cpp scenes/concavemesh/ConcaveMeshScene.h scenes/concavemesh/ConcaveMeshScene.cpp scenes/heightfield/HeightFieldScene.h diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp index 549b86bb..6c8f052d 100644 --- a/testbed/common/Box.cpp +++ b/testbed/common/Box.cpp @@ -32,88 +32,21 @@ // Initialize static variables openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Box::mVBONormals(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject Box::mVBOTextureCoords(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject Box::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Box::mVAO; int Box::totalNbBoxes = 0; -GLfloat Box::mCubeVertices[108] = { - -1.0f,-1.0f,-1.0f, // triangle 1 : begin - -1.0f,-1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, // triangle 1 : end - 1.0f, 1.0f,-1.0f, // triangle 2 : begin - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, // triangle 2 : end - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f, 1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f -}; -GLfloat Box::mCubeNormals[108] = { - -1.0f, 0.0f, 0.0f, // triangle 1 : begin - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, // triangle 1 : end - 0.0f, 0.0f,-1.0f, // triangle 2 : begin - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, // triangle 2 : end - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f,// - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f,0.0f,// - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f// -}; + // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) - : openglframework::Object3D() { + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) + : openglframework::Mesh() { + + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); + + // Calculate the normals of the mesh + calculateNormals(); // Initialize the size of the box mSize[0] = size.x * 0.5f; @@ -161,8 +94,14 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world) - : openglframework::Object3D() { + float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) + : openglframework::Mesh() { + + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); + + // Calculate the normals of the mesh + calculateNormals(); // Initialize the size of the box mSize[0] = size.x * 0.5f; @@ -226,16 +165,15 @@ Box::~Box() { // Render the cube at the correct position and with the correct orientation void Box::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - // Bind the VAO - mVAO.bind(); + if (wireframe) { + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + } // Bind the shader shader.bind(); - mVBOVertices.bind(); - // Set the model to camera matrix shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); @@ -252,20 +190,27 @@ void Box::render(openglframework::Shader& shader, openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); shader.setVector4Uniform("vertexColor", color, false); + // Bind the VAO + mVAO.bind(); + + mVBOVertices.bind(); + // Get the location of shader attribute variables GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); mVBONormals.bind(); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); - // Draw the geometry of the box - glDrawArrays(GL_TRIANGLES, 0, 36); + // For each part of the mesh + for (unsigned int i=0; isetColor(mGreyColorDemo); + mSphere1->setSleepingColor(mRedColorDemo); + + // ---------- Sphere 2 ---------- // + openglframework::Vector3 position2(4, 0, 0); + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); + mAllShapesObjects.push_back(mSphere2); + mAllShapesPhysicsObjects.push_back(mSphere2); + + // Set the color + mSphere2->setColor(mGreyColorDemo); + mSphere2->setSleepingColor(mRedColorDemo); + + // ---------- Cone ---------- // + //openglframework::Vector3 position4(0, 0, 0); + + // Create a cone and a corresponding collision body in the dynamics world + //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, + // mMeshFolderPath); + + // Set the color + //mCone->setColor(mGreyColorDemo); + //mCone->setSleepingColor(mRedColorDemo); + + // ---------- Cylinder ---------- // + //openglframework::Vector3 position5(0, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, + // mCollisionWorld, mMeshFolderPath); + + // Set the color + //mCylinder->setColor(mGreyColorDemo); + //mCylinder->setSleepingColor(mRedColorDemo); + + // ---------- Capsule ---------- // + //openglframework::Vector3 position6(0, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + //mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , + // mCollisionWorld, mMeshFolderPath); + + // Set the color + //mCapsule->setColor(mGreyColorDemo); + //mCapsule->setSleepingColor(mRedColorDemo); + + // ---------- Convex Mesh ---------- // + //openglframework::Vector3 position7(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + + // Set the color + //mConvexMesh->setColor(mGreyColorDemo); + //mConvexMesh->setSleepingColor(mRedColorDemo); + + // ---------- Concave Mesh ---------- // + //openglframework::Vector3 position8(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + + // Set the color + //mConcaveMesh->setColor(mGreyColorDemo); + //mConcaveMesh->setSleepingColor(mRedColorDemo); + + // ---------- Heightfield ---------- // + //openglframework::Vector3 position9(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mHeightField = new HeightField(position9, mCollisionWorld); + + // Set the color + //mHeightField->setColor(mGreyColorDemo); + //mHeightField->setSleepingColor(mRedColorDemo); + + // Create the VBO and VAO to render the lines + createVBOAndVAO(mPhongShader); + + mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Reset the scene +void CollisionDetectionScene::reset() { + +} + +// Destructor +CollisionDetectionScene::~CollisionDetectionScene() { + + // Destroy the shader + mPhongShader.destroy(); + + // Destroy the box rigid body from the dynamics world + //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + //delete mBox; + + // Destroy the spheres + mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); + delete mSphere1; + + mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); + delete mSphere2; + + /* + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); + delete mCone; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); + + // Destroy the sphere + delete mCylinder; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + + // Destroy the sphere + delete mCapsule; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConvexMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + + // Destroy the dumbbell + delete mDumbbell; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConcaveMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + + // Destroy the convex mesh + delete mHeightField; + */ + + mContactManager.resetPoints(); + + // Destroy the static data for the visual contact points + VisualContactPoint::destroyStaticData(); + + // Destroy the collision world + delete mCollisionWorld; + + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVAO.destroy(); +} + +// Update the physics world (take a simulation step) +void CollisionDetectionScene::updatePhysics() { + + +} + +// Take a step for the simulation +void CollisionDetectionScene::update() { + + mContactManager.resetPoints(); + + mCollisionWorld->testCollision(&mContactManager); + + SceneDemo::update(); +} + +// Render the scene +void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) { + + /* + // Bind the VAO + mVAO.bind(); + + // Bind the shader + shader.bind(); + + mVBOVertices.bind(); + + // Set the model to camera matrix + const Matrix4 localToCameraMatrix = Matrix4::identity(); + shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + // Set the vertex color + openglframework::Vector4 color(1, 0, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + + // Draw the lines + glDrawArrays(GL_LINES, 0, NB_RAYS); + + glDisableVertexAttribArray(vertexPositionLoc); + + mVBOVertices.unbind(); + + // Unbind the VAO + mVAO.unbind(); + + shader.unbind(); + */ + + // Render the shapes + if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + + /* + if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); + if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); + if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); + if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); + if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); + if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); + if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); + */ + + shader.unbind(); +} + +// Create the Vertex Buffer Objects used to render with OpenGL. +/// We create two VBOs (one for vertices and one for indices) +void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { + + // Bind the shader + shader.bind(); + + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); + mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); + mVBOVertices.unbind(); + + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); + + // Bind the VBO of vertices + mVBOVertices.bind(); + + // Unbind the VAO + mVAO.unbind(); + + // Unbind the shader + shader.unbind(); +} + +void CollisionDetectionScene::selectNextShape() { + + int previousIndex = mSelectedShapeIndex; + mSelectedShapeIndex++; + if (mSelectedShapeIndex >= mAllShapesPhysicsObjects.size()) { + mSelectedShapeIndex = 0; + } + + mAllShapesPhysicsObjects[previousIndex]->setColor(mGreyColorDemo); + mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Called when a keyboard event occurs +bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { + + // If the space key has been pressed + if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { + selectNextShape(); + return true; + } + + if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapesPhysicsObjects[mSelectedShapeIndex]->getCollisionBody()->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(1, 0, 0)); + mAllShapesObjects[mSelectedShapeIndex]->translateWorld(openglframework::Vector3(1, 0, 0)); + } + + return false; +} diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h new file mode 100644 index 00000000..d77cee7f --- /dev/null +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -0,0 +1,235 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef COLLISION_DETECTION_SCENE_H +#define COLLISION_DETECTION_SCENE_H + +// Libraries +#define _USE_MATH_DEFINES +#include +#include "openglframework.h" +#include "reactphysics3d.h" +#include "SceneDemo.h" +#include "Sphere.h" +#include "Box.h" +#include "Cone.h" +#include "Cylinder.h" +#include "Capsule.h" +#include "Line.h" +#include "ConvexMesh.h" +#include "ConcaveMesh.h" +#include "HeightField.h" +#include "Dumbbell.h" +#include "VisualContactPoint.h" + +namespace collisiondetectionscene { + +// Constants +const float SCENE_RADIUS = 30.0f; +const openglframework::Vector3 BOX_SIZE(4, 2, 1); +const float SPHERE_RADIUS = 3.0f; +const float CONE_RADIUS = 3.0f; +const float CONE_HEIGHT = 5.0f; +const float CYLINDER_RADIUS = 3.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 3.0f; +const float CAPSULE_HEIGHT = 5.0f; +const float DUMBBELL_HEIGHT = 5.0f; +const int NB_RAYS = 100; +const float RAY_LENGTH = 30.0f; +const int NB_BODIES = 9; + +// Raycast manager +class ContactManager : public rp3d::CollisionCallback { + + private: + + /// All the visual contact points + std::vector mContactPoints; + + /// All the normals at contact points + std::vector mNormals; + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + public: + + ContactManager(openglframework::Shader& shader, + const std::string& meshFolderPath) + : mMeshFolderPath(meshFolderPath) { + + } + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { + + rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1)); + + rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2)); + + // Create a line to display the normal at hit point + rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position1, position1 + normal); + mNormals.push_back(normalLine); + } + + void resetPoints() { + + mContactPoints.clear(); + + // Destroy all the normals + for (std::vector::iterator it = mNormals.begin(); + it != mNormals.end(); ++it) { + delete (*it); + } + mNormals.clear(); + } + + std::vector getContactPoints() const { + return mContactPoints; + } +}; + +// Class CollisionDetectionScene +class CollisionDetectionScene : public SceneDemo { + + private : + + // -------------------- Attributes -------------------- // + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + /// Contact manager + ContactManager mContactManager; + + bool mAreNormalsDisplayed; + + /// All objects on the scene + //Box* mBox; + Sphere* mSphere1; + Sphere* mSphere2; + //Cone* mCone; + //Cylinder* mCylinder; + //Capsule* mCapsule; + //ConvexMesh* mConvexMesh; + //Dumbbell* mDumbbell; + //ConcaveMesh* mConcaveMesh; + //HeightField* mHeightField; + + std::vector mAllShapesObjects; + std::vector mAllShapesPhysicsObjects; + + int mSelectedShapeIndex; + + /// Collision world used for the physics simulation + rp3d::CollisionWorld* mCollisionWorld; + + /// All the points to render the lines + std::vector mLinePoints; + + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; + + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; + + /// Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(openglframework::Shader& shader); + + /// Select the next shape + void selectNextShape(); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CollisionDetectionScene(const std::string& name); + + /// Destructor + virtual ~CollisionDetectionScene() override; + + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics() override; + + /// Take a step for the simulation + virtual void update() override; + + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) override; + + /// Reset the scene + virtual void reset() override; + + /// Display or not the surface normals at hit points + void showHideNormals(); + + /// Called when a keyboard event occurs + virtual bool keyboardEvent(int key, int scancode, int action, int mods) override; + + /// Enabled/Disable the shadow mapping + virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; + + /// Display/Hide the contact points + virtual void setIsContactPointsDisplayed(bool display) override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Display or not the surface normals at hit points +inline void CollisionDetectionScene::showHideNormals() { + mAreNormalsDisplayed = !mAreNormalsDisplayed; +} + +// Enabled/Disable the shadow mapping +inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { + SceneDemo::setIsShadowMappingEnabled(false); +} + +// Display/Hide the contact points +inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { + SceneDemo::setIsContactPointsDisplayed(true); +} + +// Return all the contact points of the scene +inline std::vector CollisionDetectionScene::getContactPoints() const { + return mContactManager.getContactPoints(); +} + +} + +#endif diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index 5078cd14..e146bd6b 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -83,7 +83,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) radius * sin(angle)); // Create a sphere and a corresponding rigid in the dynamics world - Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); + Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color box->setColor(mDemoColors[i % mNbDemoColors]); @@ -235,7 +235,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // ---------- Create the floor --------- openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mFloor->setColor(mGreyColorDemo); @@ -461,43 +461,43 @@ void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader, // Render all the boxes of the scene for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the sphere of the scene for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the cones of the scene for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the cylinders of the scene for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the capsules of the scene for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the convex meshes of the scene for (std::vector::iterator it = mConvexMeshes.begin(); it != mConvexMeshes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render all the dumbbells of the scene for (std::vector::iterator it = mDumbbells.begin(); it != mDumbbells.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render the floor - mFloor->render(shader, worldToCameraMatrix); + mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); //mConcaveMesh->render(shader, worldToCameraMatrix); diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 41234f3f..9b65893e 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -58,7 +58,7 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) openglframework::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE); // Create a sphere and a corresponding rigid in the dynamics world - mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld); + mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld, mMeshFolderPath); // Set the sphere color mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]); @@ -160,10 +160,10 @@ void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::M // Bind the shader shader.bind(); - mConcaveMesh->render(shader, worldToCameraMatrix); + mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); for (int i=0; irender(shader, worldToCameraMatrix); + mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Unbind the shader diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp index f9c8ad77..17af45c1 100644 --- a/testbed/scenes/cubes/CubesScene.cpp +++ b/testbed/scenes/cubes/CubesScene.cpp @@ -1,195 +1,213 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CubesScene.h" - -// Namespaces -using namespace openglframework; -using namespace cubesscene; - -// Constructor -CubesScene::CubesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { - - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); - - // Set the center of the scene - setScenePosition(center, SCENE_RADIUS); - - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); - - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - - float radius = 2.0f; - - // Create all the cubes of the scene - for (int i=0; isetColor(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); - } - - // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); - mFloor->setColor(mGreyColorDemo); - mFloor->setSleepingColor(mGreyColorDemo); - - // The floor must be a static rigid body - mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); - - // Change the material properties of the floor rigid body - rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.3)); - - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); -} - -// Destructor -CubesScene::~CubesScene() { - - // Destroy all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the cube - delete (*it); - } - - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - - // Destroy the floor - delete mFloor; - - // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void CubesScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Update the scene -void CubesScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene in a single pass -void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix); - - // Unbind the shader - shader.unbind(); -} - -// Reset the scene -void CubesScene::reset() { - - float radius = 2.0f; - - for (int i=0; iresetTransform(transform); - } -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CubesScene.h" + +// Namespaces +using namespace openglframework; +using namespace cubesscene; + +// Constructor +CubesScene::CubesScene(const std::string& name) + : SceneDemo(name, SCENE_RADIUS) { + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + // Create the dynamics world for the physics simulation + mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + + float radius = 2.0f; + + //// Create all the cubes of the scene + //for (int i=0; isetColor(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); + //} + + // ------------------------- FLOOR ----------------------- // + + // Create the floor + openglframework::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); + + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); + + // Change the material properties of the floor rigid body + //rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); + //material.setBounciness(rp3d::decimal(0.3)); + + // ------------------------- BOX ----------------------- // + + // Create a cube and a corresponding rigid in the dynamics world + Box* cube = new Box(BOX_SIZE, Vector3(0, 10, 0), BOX_MASS, mDynamicsWorld, mMeshFolderPath); + + // Set the box color + cube->setColor(mDemoColors[0]); + 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); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); + rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); +} + +// Destructor +CubesScene::~CubesScene() { + + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Destroy the corresponding rigid body from the dynamics world + mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the cube + delete (*it); + } + + // Destroy the rigid body of the floor + mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + + // Destroy the floor + delete mFloor; + + // Destroy the dynamics world + delete mDynamicsWorld; +} + +// Update the physics world (take a simulation step) +void CubesScene::updatePhysics() { + + // Update the physics engine parameters + mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + mDynamicsWorld->setGravity(gravity); + mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); + mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + + // Take a simulation step + mDynamicsWorld->update(mEngineSettings.timeStep); +} + +// Update the scene +void CubesScene::update() { + + SceneDemo::update(); + + // Update the position and orientation of the boxes + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } + + mFloor->updateTransform(mInterpolationFactor); +} + +// Render the scene in a single pass +void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { + + // Bind the shader + shader.bind(); + + // Render all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + } + + // Render the floor + mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + + // Unbind the shader + shader.unbind(); +} + +// Reset the scene +void CubesScene::reset() { + + float radius = 2.0f; + + for (int i=0; iresetTransform(transform); + } +} diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h index a07c74d2..4ad57f77 100644 --- a/testbed/scenes/cubes/CubesScene.h +++ b/testbed/scenes/cubes/CubesScene.h @@ -1,96 +1,96 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CUBES_SCENE_H -#define CUBES_SCENE_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "Box.h" -#include "SceneDemo.h" - -namespace cubesscene { - -// Constants -const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters -const int NB_CUBES = 30; // Number of boxes in the scene -const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters -const float BOX_MASS = 1.0f; // Box mass in kilograms -const float FLOOR_MASS = 100.0f; // Floor mass in kilograms - -// Class CubesScene -class CubesScene : public SceneDemo { - - protected : - - // -------------------- Attributes -------------------- // - - /// All the boxes of the scene - std::vector mBoxes; - - /// Box for the floor - Box* mFloor; - - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - CubesScene(const std::string& name); - - /// Destructor - virtual ~CubesScene() override; - - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - - /// Reset the scene - virtual void reset() override; - - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; -}; - -// Return all the contact points of the scene -inline std::vector CubesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); -} - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CUBES_SCENE_H +#define CUBES_SCENE_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" +#include "Box.h" +#include "SceneDemo.h" + +namespace cubesscene { + +// Constants +const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters +const int NB_CUBES = 30; // Number of boxes in the scene +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(5, 5.0, 5); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms + +// Class CubesScene +class CubesScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// All the boxes of the scene + std::vector mBoxes; + + /// Box for the floor + Box* mFloor; + + /// Dynamics world used for the physics simulation + rp3d::DynamicsWorld* mDynamicsWorld; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CubesScene(const std::string& name); + + /// Destructor + virtual ~CubesScene() override; + + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics() override; + + /// Update the scene (take a simulation step) + virtual void update() override; + + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) override; + + /// Reset the scene + virtual void reset() override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Return all the contact points of the scene +inline std::vector CubesScene::getContactPoints() const { + return computeContactPointsOfWorld(mDynamicsWorld); +} + +} + +#endif diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 3395ac17..5b62f614 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -54,7 +54,7 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC openglframework::Vector3 position(15, 10 + 6 * i, 0); // Create a box and a corresponding rigid in the dynamics world - mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld); + mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld, mMeshFolderPath); // Set the box color mBoxes[i]->setColor(mDemoColors[2]); @@ -156,10 +156,10 @@ void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::M // Bind the shader shader.bind(); - mHeightField->render(shader, worldToCameraMatrix); + mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled); for (int i=0; irender(shader, worldToCameraMatrix); + mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Unbind the shader diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index b1891627..34bb1503 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -163,17 +163,17 @@ void JointsScene::renderSinglePass(openglframework::Shader& shader, shader.bind(); // Render all the boxes - mSliderJointBottomBox->render(shader, worldToCameraMatrix); - mSliderJointTopBox->render(shader, worldToCameraMatrix); - mPropellerBox->render(shader, worldToCameraMatrix); - mFixedJointBox1->render(shader, worldToCameraMatrix); - mFixedJointBox2->render(shader, worldToCameraMatrix); + mSliderJointBottomBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + mSliderJointTopBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + mPropellerBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + mFixedJointBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + mFixedJointBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); for (int i=0; irender(shader, worldToCameraMatrix); + mBallAndSocketJointChainBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } // Render the floor - mFloor->render(shader, worldToCameraMatrix); + mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); // Unbind the shader shader.unbind(); @@ -263,7 +263,7 @@ void JointsScene::createBallAndSocketJoints() { // Create a box and a corresponding rigid in the dynamics world mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass, - mDynamicsWorld); + mDynamicsWorld, mMeshFolderPath); // Set the box color mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]); @@ -312,7 +312,7 @@ void JointsScene::createSliderJoint() { // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box1Dimension(2, 4, 2); - mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mSliderJointBottomBox->setColor(mBlueColorDemo); @@ -332,7 +332,7 @@ void JointsScene::createSliderJoint() { // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); - mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld); + mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mSliderJointTopBox->setColor(mOrangeColorDemo); @@ -372,7 +372,7 @@ void JointsScene::createPropellerHingeJoint() { // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(10, 1, 1); - mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mPropellerBox->setColor(mYellowColorDemo); @@ -411,7 +411,7 @@ void JointsScene::createFixedJoints() { // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); - mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mFixedJointBox1->setColor(mPinkColorDemo); @@ -427,7 +427,7 @@ void JointsScene::createFixedJoints() { openglframework::Vector3 positionBox2(-5, 7, 0); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); + mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mFixedJointBox2->setColor(mBlueColorDemo); @@ -466,7 +466,7 @@ void JointsScene::createFloor() { // Create the floor openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); // Set the box color mFloor->setColor(mGreyColorDemo); diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index 479e0915..58b19055 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -61,7 +61,7 @@ RaycastScene::RaycastScene(const std::string& name) openglframework::Vector3 position2(0, 0, 0); // Create a box and a corresponding collision body in the dynamics world - mBox = new Box(BOX_SIZE, position2, mCollisionWorld); + mBox = new Box(BOX_SIZE, position2, mCollisionWorld, mMeshFolderPath); mBox->getCollisionBody()->setIsActive(false); // Set the box color @@ -376,15 +376,15 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, shader.unbind(); // Render the shapes - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); - if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); + if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled); shader.unbind(); } diff --git a/testbed/src/Gui.cpp b/testbed/src/Gui.cpp index 499f1fb3..cd9f8f4e 100644 --- a/testbed/src/Gui.cpp +++ b/testbed/src/Gui.cpp @@ -402,6 +402,13 @@ void Gui::createSettingsPanel() { mApp->mIsShadowMappingEnabled = value; }); + // Enable/Disable wireframe mode + CheckBox* checkboxWireframe = new CheckBox(mRenderingPanel, "Wireframe"); + checkboxWireframe->setChecked(mApp->mIsWireframeEnabled); + checkboxWireframe->setCallback([&](bool value) { + mApp->mIsWireframeEnabled = value; + }); + mPhysicsPanel->setVisible(true); mRenderingPanel->setVisible(false); } diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp index 8031c1f8..e872b3bc 100644 --- a/testbed/src/Scene.cpp +++ b/testbed/src/Scene.cpp @@ -33,7 +33,7 @@ using namespace openglframework; Scene::Scene(const std::string& name, bool isShadowMappingEnabled) : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled), - mIsContactPointsDisplayed(true) { + mIsContactPointsDisplayed(true), mIsWireframeEnabled(false) { } diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index bfac3ecb..d51ffec1 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -108,6 +108,9 @@ class Scene { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if we render shapes in wireframe mode + bool mIsWireframeEnabled; + // -------------------- Methods -------------------- // /// Set the scene position (where the camera needs to look at) @@ -199,6 +202,12 @@ class Scene { /// Display/Hide the contact points void virtual setIsContactPointsDisplayed(bool display); + /// Return true if wireframe rendering is enabled + bool getIsWireframeEnabled() const; + + /// Enable/disbale wireframe rendering + void setIsWireframeEnabled(bool isEnabled); + /// Return all the contact points of the scene std::vector virtual getContactPoints() const; }; @@ -267,6 +276,16 @@ inline void Scene::setIsContactPointsDisplayed(bool display) { mIsContactPointsDisplayed = display; } +// Return true if wireframe rendering is enabled +inline bool Scene::getIsWireframeEnabled() const { + return mIsWireframeEnabled; +} + +// Enable/disbale wireframe rendering +inline void Scene::setIsWireframeEnabled(bool isEnabled) { + mIsWireframeEnabled = isEnabled; +} + // Return all the contact points of the scene inline std::vector Scene::getContactPoints() const { diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index f57aa829..09f464f3 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -30,6 +30,7 @@ #include #include #include "cubes/CubesScene.h" +#include "collisiondetection/CollisionDetectionScene.h" #include "joints/JointsScene.h" #include "collisionshapes/CollisionShapesScene.h" #include "heightfield/HeightFieldScene.h" @@ -43,6 +44,7 @@ using namespace raycastscene; using namespace collisionshapesscene; using namespace trianglemeshscene; using namespace heightfieldscene; +using namespace collisiondetectionscene; // Initialization of static variables const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; @@ -63,6 +65,7 @@ TestbedApplication::TestbedApplication(bool isFullscreen) mIsShadowMappingEnabled = true; mIsVSyncEnabled = false; mIsContactPointsDisplayed = false; + mIsWireframeEnabled = false; init(); @@ -113,7 +116,11 @@ void TestbedApplication::createScenes() { RaycastScene* raycastScene = new RaycastScene("Raycast"); mScenes.push_back(raycastScene); - // Raycast scene + // Collision Detection scene + CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection"); + mScenes.push_back(collisionDetectionScene); + + // Concave Mesh scene ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh"); mScenes.push_back(concaveMeshScene); @@ -195,6 +202,9 @@ void TestbedApplication::update() { // Display/Hide contact points mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed); + // Enable/Disable wireframe mode + mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled); + // Update the scene mCurrentScene->update(); } diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h index dcff0598..93d85ea7 100644 --- a/testbed/src/TestbedApplication.h +++ b/testbed/src/TestbedApplication.h @@ -109,6 +109,9 @@ class TestbedApplication : public Screen { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if the wireframe rendering is enabled + bool mIsWireframeEnabled; + /// True if vsync is enabled bool mIsVSyncEnabled; From a9da0d6d3c7a68136864ebaf88c23e58230c194d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 28 Jan 2017 00:59:04 +0100 Subject: [PATCH 003/105] Refactor testbed application --- testbed/common/Box.cpp | 6 ++---- testbed/common/Box.h | 2 +- testbed/common/Capsule.cpp | 4 ++-- testbed/common/Capsule.h | 2 +- testbed/common/ConcaveMesh.cpp | 4 ++-- testbed/common/ConcaveMesh.h | 2 +- testbed/common/Cone.cpp | 4 ++-- testbed/common/Cone.h | 2 +- testbed/common/ConvexMesh.cpp | 4 ++-- testbed/common/ConvexMesh.h | 2 +- testbed/common/Cylinder.cpp | 4 ++-- testbed/common/Cylinder.h | 2 +- testbed/common/Dumbbell.cpp | 6 ++---- testbed/common/Dumbbell.h | 2 +- testbed/common/HeightField.cpp | 4 ++-- testbed/common/HeightField.h | 2 +- testbed/common/PhysicsObject.cpp | 2 +- testbed/common/PhysicsObject.h | 2 +- testbed/common/Sphere.cpp | 4 ++-- testbed/common/Sphere.h | 2 +- testbed/scenes/cubes/CubesScene.cpp | 24 ++++++++++++------------ 21 files changed, 41 insertions(+), 45 deletions(-) diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp index 6c8f052d..0720799e 100644 --- a/testbed/common/Box.cpp +++ b/testbed/common/Box.cpp @@ -39,8 +39,7 @@ int Box::totalNbBoxes = 0; // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh() { + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); @@ -94,8 +93,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh() { + float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); diff --git a/testbed/common/Box.h b/testbed/common/Box.h index 53da204e..0c21be36 100644 --- a/testbed/common/Box.h +++ b/testbed/common/Box.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Box -class Box : public openglframework::Mesh, public PhysicsObject { +class Box : public PhysicsObject { private : diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp index ba6a45b4..e9ae2b40 100644 --- a/testbed/common/Capsule.cpp +++ b/testbed/common/Capsule.cpp @@ -37,7 +37,7 @@ int Capsule::totalNbCapsules = 0; Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); @@ -86,7 +86,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h index 748fa46b..2a97b14b 100644 --- a/testbed/common/Capsule.h +++ b/testbed/common/Capsule.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Sphere -class Capsule : public openglframework::Mesh, public PhysicsObject { +class Capsule : public PhysicsObject { private : diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index c21732f8..2f0360ba 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -30,7 +30,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { @@ -87,7 +87,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h index 3c645ec8..e4b33308 100644 --- a/testbed/common/ConcaveMesh.h +++ b/testbed/common/ConcaveMesh.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class ConcaveMesh -class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { +class ConcaveMesh : public PhysicsObject { private : diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp index 07a825a8..96f52c54 100644 --- a/testbed/common/Cone.cpp +++ b/testbed/common/Cone.cpp @@ -37,7 +37,7 @@ int Cone::totalNbCones = 0; Cone::Cone(float radius, float height, const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); @@ -85,7 +85,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, Cone::Cone(float radius, float height, const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h index e4f7face..6fc35182 100644 --- a/testbed/common/Cone.h +++ b/testbed/common/Cone.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Cone -class Cone : public openglframework::Mesh, public PhysicsObject { +class Cone : public PhysicsObject { private : diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index ca372a1f..c107724f 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -30,7 +30,7 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { @@ -81,7 +81,7 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index a4a2ffab..19b39220 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class ConvexMesh -class ConvexMesh : public openglframework::Mesh, public PhysicsObject { +class ConvexMesh : public PhysicsObject { private : diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp index d5ec1964..9b520d5c 100644 --- a/testbed/common/Cylinder.cpp +++ b/testbed/common/Cylinder.cpp @@ -37,7 +37,7 @@ int Cylinder::totalNbCylinders = 0; Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); @@ -85,7 +85,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { + : mRadius(radius), mHeight(height) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h index eb7ba72d..4aab2dc9 100644 --- a/testbed/common/Cylinder.h +++ b/testbed/common/Cylinder.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Cylinder -class Cylinder : public openglframework::Mesh, public PhysicsObject { +class Cylinder : public PhysicsObject { private : diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp index e590d033..770f8ec1 100644 --- a/testbed/common/Dumbbell.cpp +++ b/testbed/common/Dumbbell.cpp @@ -35,8 +35,7 @@ int Dumbbell::totalNbDumbbells = 0; // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh() { + reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); @@ -106,8 +105,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh() { + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index 4d44a38b..5ce3ec95 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Sphere -class Dumbbell : public openglframework::Mesh, public PhysicsObject { +class Dumbbell : public PhysicsObject { private : diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp index 38eda261..55da4e1d 100644 --- a/testbed/common/HeightField.cpp +++ b/testbed/common/HeightField.cpp @@ -30,7 +30,7 @@ // Constructor HeightField::HeightField(const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { @@ -73,7 +73,7 @@ HeightField::HeightField(const openglframework::Vector3 &position, // Constructor HeightField::HeightField(const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h index d4a4895d..d9851992 100644 --- a/testbed/common/HeightField.h +++ b/testbed/common/HeightField.h @@ -33,7 +33,7 @@ // Class HeightField -class HeightField : public openglframework::Mesh, public PhysicsObject { +class HeightField : public PhysicsObject { private : diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp index c176bb5d..c69f9485 100644 --- a/testbed/common/PhysicsObject.cpp +++ b/testbed/common/PhysicsObject.cpp @@ -27,7 +27,7 @@ #include "PhysicsObject.h" /// Constructor -PhysicsObject::PhysicsObject() { +PhysicsObject::PhysicsObject() : openglframework::Mesh() { mColor = openglframework::Color(1, 1, 1, 1); mSleepingColor = openglframework::Color(1, 0, 0, 1); diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index 5a54d6d0..bc470091 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -31,7 +31,7 @@ #include "reactphysics3d.h" // Class PhysicsObject -class PhysicsObject { +class PhysicsObject : public openglframework::Mesh { protected: diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp index 236a81ad..93c48af2 100644 --- a/testbed/common/Sphere.cpp +++ b/testbed/common/Sphere.cpp @@ -37,7 +37,7 @@ int Sphere::totalNbSpheres = 0; Sphere::Sphere(float radius, const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { + : mRadius(radius) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); @@ -86,7 +86,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, Sphere::Sphere(float radius, const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { + : mRadius(radius) { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h index 5fdfcfe9..d941d984 100644 --- a/testbed/common/Sphere.h +++ b/testbed/common/Sphere.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Sphere -class Sphere : public openglframework::Mesh, public PhysicsObject { +class Sphere : public PhysicsObject { private : diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp index 17af45c1..81929e58 100644 --- a/testbed/scenes/cubes/CubesScene.cpp +++ b/testbed/scenes/cubes/CubesScene.cpp @@ -195,19 +195,19 @@ void CubesScene::reset() { float radius = 2.0f; - for (int i=0; iresetTransform(transform); - } +// mBoxes[i]->resetTransform(transform); +// } } From 0b0975769b1a408266cf3f23e1175709e48cfe77 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 28 Jan 2017 11:38:48 +0100 Subject: [PATCH 004/105] Refactor testbed application --- testbed/common/Box.cpp | 30 +++------------- testbed/common/Box.h | 3 -- testbed/common/Capsule.cpp | 34 ++---------------- testbed/common/Capsule.h | 3 -- testbed/common/ConcaveMesh.cpp | 34 ++---------------- testbed/common/ConcaveMesh.h | 3 -- testbed/common/Cone.cpp | 34 ++---------------- testbed/common/Cone.h | 3 -- testbed/common/ConvexMesh.cpp | 34 ++---------------- testbed/common/ConvexMesh.h | 3 -- testbed/common/Cylinder.cpp | 34 ++---------------- testbed/common/Cylinder.h | 3 -- testbed/common/Dumbbell.cpp | 36 +++---------------- testbed/common/Dumbbell.h | 3 -- testbed/common/HeightField.cpp | 18 ---------- testbed/common/HeightField.h | 3 -- testbed/common/PhysicsObject.cpp | 28 +++++++++++++++ testbed/common/PhysicsObject.h | 6 ++++ testbed/common/Sphere.cpp | 34 ++---------------- testbed/common/Sphere.h | 3 -- .../CollisionDetectionScene.h | 1 - 21 files changed, 54 insertions(+), 296 deletions(-) diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp index 0720799e..09dc388e 100644 --- a/testbed/common/Box.cpp +++ b/testbed/common/Box.cpp @@ -39,13 +39,8 @@ int Box::totalNbBoxes = 0; // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "cube.obj") { // Initialize the size of the box mSize[0] = size.x * 0.5f; @@ -93,7 +88,8 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p // Constructor Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) { + float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "cube.obj") { // Load the mesh from a file openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); @@ -284,24 +280,6 @@ void Box::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Box::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Box::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Box.h b/testbed/common/Box.h index 0c21be36..a1f05d5e 100644 --- a/testbed/common/Box.h +++ b/testbed/common/Box.h @@ -88,9 +88,6 @@ class Box : public PhysicsObject { /// Render the cube at the correct position and with the correct orientation void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp index e9ae2b40..413d6eb2 100644 --- a/testbed/common/Capsule.cpp +++ b/testbed/common/Capsule.cpp @@ -37,13 +37,7 @@ int Capsule::totalNbCapsules = 0; Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -86,13 +80,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -272,24 +260,6 @@ void Capsule::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Capsule::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Capsule::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h index 2a97b14b..f117984f 100644 --- a/testbed/common/Capsule.h +++ b/testbed/common/Capsule.h @@ -97,9 +97,6 @@ class Capsule : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index 2f0360ba..608e0020 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -30,16 +30,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) - : mVBOVertices(GL_ARRAY_BUFFER), + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - // Initialize the position where the sphere will be rendered translateWorld(position); @@ -87,16 +81,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) - : mVBOVertices(GL_ARRAY_BUFFER), + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - // Initialize the position where the sphere will be rendered translateWorld(position); @@ -285,24 +273,6 @@ void ConcaveMesh::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void ConcaveMesh::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h index e4b33308..afda3d45 100644 --- a/testbed/common/ConcaveMesh.h +++ b/testbed/common/ConcaveMesh.h @@ -90,9 +90,6 @@ class ConcaveMesh : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp index 96f52c54..bbd32ed9 100644 --- a/testbed/common/Cone.cpp +++ b/testbed/common/Cone.cpp @@ -37,13 +37,7 @@ int Cone::totalNbCones = 0; Cone::Cone(float radius, float height, const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -85,13 +79,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position, Cone::Cone(float radius, float height, const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -269,24 +257,6 @@ void Cone::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Cone::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Cone::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h index 6fc35182..8f71493c 100644 --- a/testbed/common/Cone.h +++ b/testbed/common/Cone.h @@ -96,9 +96,6 @@ class Cone : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index c107724f..7842c529 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -30,16 +30,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshPath) - : mVBOVertices(GL_ARRAY_BUFFER), + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - // Initialize the position where the sphere will be rendered translateWorld(position); @@ -81,16 +75,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) - : mVBOVertices(GL_ARRAY_BUFFER), + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - // Initialize the position where the sphere will be rendered translateWorld(position); @@ -266,24 +254,6 @@ void ConvexMesh::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void ConvexMesh::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void ConvexMesh::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index 19b39220..b6cb90c1 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -89,9 +89,6 @@ class ConvexMesh : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp index 9b520d5c..2943c3e8 100644 --- a/testbed/common/Cylinder.cpp +++ b/testbed/common/Cylinder.cpp @@ -37,13 +37,7 @@ int Cylinder::totalNbCylinders = 0; Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -85,13 +79,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -270,24 +258,6 @@ void Cylinder::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Cylinder::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Cylinder::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h index 4aab2dc9..3a94b443 100644 --- a/testbed/common/Cylinder.h +++ b/testbed/common/Cylinder.h @@ -96,9 +96,6 @@ class Cylinder : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp index 770f8ec1..03afd59b 100644 --- a/testbed/common/Dumbbell.cpp +++ b/testbed/common/Dumbbell.cpp @@ -35,13 +35,8 @@ int Dumbbell::totalNbDumbbells = 0; // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "dumbbell.obj") { // Identity scaling matrix mScalingMatrix.setToIdentity(); @@ -105,13 +100,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Constructor Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "dumbbell.obj"){ // Identity scaling matrix mScalingMatrix.setToIdentity(); @@ -309,24 +299,6 @@ void Dumbbell::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Dumbbell::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Dumbbell::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index 5ce3ec95..81020935 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -97,9 +97,6 @@ class Dumbbell : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp index 55da4e1d..377cab07 100644 --- a/testbed/common/HeightField.cpp +++ b/testbed/common/HeightField.cpp @@ -328,24 +328,6 @@ void HeightField::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void HeightField::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void HeightField::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h index d9851992..acb39163 100644 --- a/testbed/common/HeightField.h +++ b/testbed/common/HeightField.h @@ -103,9 +103,6 @@ class HeightField : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp index c69f9485..acba8e67 100644 --- a/testbed/common/PhysicsObject.cpp +++ b/testbed/common/PhysicsObject.cpp @@ -33,6 +33,16 @@ PhysicsObject::PhysicsObject() : openglframework::Mesh() { mSleepingColor = openglframework::Color(1, 0, 0, 1); } +/// Constructor +PhysicsObject::PhysicsObject(const std::string& meshPath) : PhysicsObject() { + + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + + // Calculate the normals of the mesh + calculateNormals(); +} + // Compute the new transform matrix openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFactor, const openglframework::Matrix4& scalingMatrix) { @@ -57,3 +67,21 @@ openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFact // Apply the scaling matrix to have the correct box dimensions return newMatrix * scalingMatrix; } + +// Reset the transform +void PhysicsObject::resetTransform(const rp3d::Transform& transform) { + + // Reset the transform + mBody->setTransform(transform); + + mBody->setIsSleeping(false); + + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != nullptr) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } + + updateTransform(1.0f); +} diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index bc470091..15f5cf80 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -56,6 +56,9 @@ class PhysicsObject : public openglframework::Mesh { /// Constructor PhysicsObject(); + /// Constructor + PhysicsObject(const std::string& meshPath); + /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor)=0; @@ -65,6 +68,9 @@ class PhysicsObject : public openglframework::Mesh { /// Set the sleeping color of the box void setSleepingColor(const openglframework::Color& color); + /// Set the position of the box + void resetTransform(const rp3d::Transform& transform); + /// Return a pointer to the collision body of the box reactphysics3d::CollisionBody* getCollisionBody(); diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp index 93c48af2..9eaf62c2 100644 --- a/testbed/common/Sphere.cpp +++ b/testbed/common/Sphere.cpp @@ -37,13 +37,7 @@ int Sphere::totalNbSpheres = 0; Sphere::Sphere(float radius, const openglframework::Vector3 &position, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : mRadius(radius) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -86,13 +80,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, Sphere::Sphere(float radius, const openglframework::Vector3 &position, float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) - : mRadius(radius) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -270,24 +258,6 @@ void Sphere::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Sphere::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Sphere::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h index d941d984..62f1809e 100644 --- a/testbed/common/Sphere.h +++ b/testbed/common/Sphere.h @@ -93,9 +93,6 @@ class Sphere : public PhysicsObject { void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index d77cee7f..06b426fe 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -27,7 +27,6 @@ #define COLLISION_DETECTION_SCENE_H // Libraries -#define _USE_MATH_DEFINES #include #include "openglframework.h" #include "reactphysics3d.h" From adeac945061b4b737dc08c3609982b2b554709da Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 28 Jan 2017 15:23:05 +0100 Subject: [PATCH 005/105] Working on collision detection scene --- testbed/common/PhysicsObject.cpp | 2 +- testbed/common/PhysicsObject.h | 12 ++- .../CollisionDetectionScene.cpp | 78 ++++++++++++++++--- .../CollisionDetectionScene.h | 3 +- .../collisionshapes/CollisionShapesScene.cpp | 14 ++-- .../scenes/concavemesh/ConcaveMeshScene.cpp | 4 +- .../scenes/heightfield/HeightFieldScene.cpp | 4 +- testbed/scenes/joints/JointsScene.cpp | 12 +-- testbed/src/TestbedApplication.h | 16 ---- 9 files changed, 96 insertions(+), 49 deletions(-) diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp index acba8e67..9955725c 100644 --- a/testbed/common/PhysicsObject.cpp +++ b/testbed/common/PhysicsObject.cpp @@ -69,7 +69,7 @@ openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFact } // Reset the transform -void PhysicsObject::resetTransform(const rp3d::Transform& transform) { +void PhysicsObject::setTransform(const rp3d::Transform& transform) { // Reset the transform mBody->setTransform(transform); diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index 15f5cf80..0da224c3 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -68,8 +68,11 @@ class PhysicsObject : public openglframework::Mesh { /// Set the sleeping color of the box void setSleepingColor(const openglframework::Color& color); - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + /// Get the transform + const rp3d::Transform& getTransform() const; + + /// Set the transform + void setTransform(const rp3d::Transform& transform); /// Return a pointer to the collision body of the box reactphysics3d::CollisionBody* getCollisionBody(); @@ -91,6 +94,11 @@ inline void PhysicsObject::setSleepingColor(const openglframework::Color& color) mSleepingColor = color; } +// Get the transform +inline const rp3d::Transform& PhysicsObject::getTransform() const { + return mBody->getTransform(); +} + // Return a pointer to the collision body of the box inline rp3d::CollisionBody* PhysicsObject::getCollisionBody() { return mBody; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 09544540..8aae0145 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -54,8 +54,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Create a sphere and a corresponding collision body in the dynamics world mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); - mAllShapesObjects.push_back(mSphere1); - mAllShapesPhysicsObjects.push_back(mSphere1); + mAllShapes.push_back(mSphere1); // Set the color mSphere1->setColor(mGreyColorDemo); @@ -66,8 +65,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Create a sphere and a corresponding collision body in the dynamics world mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); - mAllShapesObjects.push_back(mSphere2); - mAllShapesPhysicsObjects.push_back(mSphere2); + mAllShapes.push_back(mSphere2); // Set the color mSphere2->setColor(mGreyColorDemo); @@ -139,7 +137,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Create the VBO and VAO to render the lines createVBOAndVAO(mPhongShader); - mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo); + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); } // Reset the scene @@ -332,12 +330,12 @@ void CollisionDetectionScene::selectNextShape() { int previousIndex = mSelectedShapeIndex; mSelectedShapeIndex++; - if (mSelectedShapeIndex >= mAllShapesPhysicsObjects.size()) { + if (mSelectedShapeIndex >= mAllShapes.size()) { mSelectedShapeIndex = 0; } - mAllShapesPhysicsObjects[previousIndex]->setColor(mGreyColorDemo); - mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo); + mAllShapes[previousIndex]->setColor(mGreyColorDemo); + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); } // Called when a keyboard event occurs @@ -349,10 +347,68 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i return true; } + float stepDist = 0.5f; + float stepAngle = 20 * (3.14f / 180.0f); + if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapesPhysicsObjects[mSelectedShapeIndex]->getCollisionBody()->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(1, 0, 0)); - mAllShapesObjects[mSelectedShapeIndex]->translateWorld(openglframework::Vector3(1, 0, 0)); + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_H && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_A && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_D && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_W && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_S && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_F && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_G && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); } return false; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 06b426fe..b7f57c1d 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -146,8 +146,7 @@ class CollisionDetectionScene : public SceneDemo { //ConcaveMesh* mConcaveMesh; //HeightField* mHeightField; - std::vector mAllShapesObjects; - std::vector mAllShapesPhysicsObjects; + std::vector mAllShapes; int mSelectedShapeIndex; diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index e146bd6b..8db4908a 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -524,7 +524,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mDumbbells[i]->resetTransform(transform); + mDumbbells[i]->setTransform(transform); } // Create all the boxes of the scene @@ -542,7 +542,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mBoxes[i]->resetTransform(transform); + mBoxes[i]->setTransform(transform); } // Create all the spheres of the scene @@ -560,7 +560,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mSpheres[i]->resetTransform(transform); + mSpheres[i]->setTransform(transform); } // Create all the cones of the scene @@ -578,7 +578,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mCones[i]->resetTransform(transform); + mCones[i]->setTransform(transform); } // Create all the cylinders of the scene @@ -596,7 +596,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mCylinders[i]->resetTransform(transform); + mCylinders[i]->setTransform(transform); } // Create all the capsules of the scene @@ -614,7 +614,7 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mCapsules[i]->resetTransform(transform); + mCapsules[i]->setTransform(transform); } // Create all the convex meshes of the scene @@ -632,6 +632,6 @@ void CollisionShapesScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Reset the transform - mConvexMeshes[i]->resetTransform(transform); + mConvexMeshes[i]->setTransform(transform); } } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 9b65893e..11474819 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -175,7 +175,7 @@ void ConcaveMeshScene::reset() { // Reset the transform rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); - mConcaveMesh->resetTransform(transform); + mConcaveMesh->setTransform(transform); for (int i=0; iresetTransform(boxTransform); + mBoxes[i * NB_BOXES_Z + j]->setTransform(boxTransform); } } diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 5b62f614..3e4061c9 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -171,13 +171,13 @@ void HeightFieldScene::reset() { // Reset the transform rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - mHeightField->resetTransform(transform); + mHeightField->setTransform(transform); float heightFieldWidth = 10.0f; float stepDist = heightFieldWidth / (NB_BOXES + 1); for (int i=0; iresetTransform(boxTransform); + mBoxes[i]->setTransform(boxTransform); } } diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index 34bb1503..23bcdb93 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -193,7 +193,7 @@ void JointsScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mBallAndSocketJointChainBoxes[i]->resetTransform(transform); + mBallAndSocketJointChainBoxes[i]->setTransform(transform); positionBox.y -= boxDimension.y + 0.5f; } @@ -207,7 +207,7 @@ void JointsScene::reset() { rp3d::Transform transformBottomBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mSliderJointBottomBox->resetTransform(transformBottomBox); + mSliderJointBottomBox->setTransform(transformBottomBox); // Position of the box openglframework::Vector3 positionBox2(0, 4.2f, 0); @@ -216,7 +216,7 @@ void JointsScene::reset() { rp3d::Transform transformTopBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mSliderJointTopBox->resetTransform(transformTopBox); + mSliderJointTopBox->setTransform(transformTopBox); // --------------- Propeller Hinge joint --------------- // @@ -227,7 +227,7 @@ void JointsScene::reset() { rp3d::Transform transformHingeBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mPropellerBox->resetTransform(transformHingeBox); + mPropellerBox->setTransform(transformHingeBox); // --------------- Fixed joint --------------- // @@ -238,7 +238,7 @@ void JointsScene::reset() { rp3d::Transform transformFixedBox1(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox1->resetTransform(transformFixedBox1); + mFixedJointBox1->setTransform(transformFixedBox1); // Position of the box positionBox2 = openglframework::Vector3(-5, 7, 0); @@ -247,7 +247,7 @@ void JointsScene::reset() { rp3d::Transform transformFixedBox2(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2->resetTransform(transformFixedBox2); + mFixedJointBox2->setTransform(transformFixedBox2); } // Create the boxes and joints for the Ball-and-Socket joint example diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h index 93d85ea7..8790a1f7 100644 --- a/testbed/src/TestbedApplication.h +++ b/testbed/src/TestbedApplication.h @@ -162,12 +162,6 @@ class TestbedApplication : public Screen { /// Set the variable to know if we need to take a single physics step void toggleTakeSinglePhysicsStep(); - /// Enable/Disable shadow mapping - void enableShadows(bool enable); - - /// Display/Hide contact points - void displayContactPoints(bool display); - public : // -------------------- Methods -------------------- // @@ -252,16 +246,6 @@ inline void TestbedApplication::toggleTakeSinglePhysicsStep() { } } -// Enable/Disable shadow mapping -inline void TestbedApplication::enableShadows(bool enable) { - mIsShadowMappingEnabled = enable; -} - -/// Display/Hide contact points -inline void TestbedApplication::displayContactPoints(bool display) { - mIsContactPointsDisplayed = display; -} - // Enable/Disable Vertical synchronization inline void TestbedApplication::enableVSync(bool enable) { mIsVSyncEnabled = enable; From e9f2f94f6455ef366cd5ab33b457d007cd19a580 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 Feb 2017 22:58:40 +0100 Subject: [PATCH 006/105] Continue working on SAT, remove Cone and Cylinder shapes --- CMakeLists.txt | 22 +- src/collision/CollisionDetection.cpp | 35 +- src/collision/CollisionDetection.h | 26 +- src/collision/HalfEdgeStructure.cpp | 116 ++++ src/collision/HalfEdgeStructure.h | 142 +++++ src/collision/PolyhedronMesh.cpp | 62 +++ src/collision/PolyhedronMesh.h | 86 +++ src/collision/TriangleMesh.h | 2 +- ...tore.cpp => CapsuleVsCapsuleAlgorithm.cpp} | 12 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 71 +++ .../narrowphase/DefaultCollisionDispatch.cpp | 20 +- .../narrowphase/DefaultCollisionDispatch.h | 4 + .../narrowphase/EPA/EPAAlgorithm.cpp | 436 --------------- src/collision/narrowphase/EPA/EPAAlgorithm.h | 147 ----- src/collision/narrowphase/EPA/EdgeEPA.cpp | 125 ----- src/collision/narrowphase/EPA/TriangleEPA.cpp | 145 ----- src/collision/narrowphase/EPA/TriangleEPA.h | 197 ------- .../narrowphase/EPA/TrianglesStore.h | 139 ----- .../narrowphase/GJK/GJKAlgorithm.cpp | 119 +--- src/collision/narrowphase/GJK/GJKAlgorithm.h | 26 +- .../narrowphase/SAT/SATAlgorithm.cpp | 43 ++ .../{EPA/EdgeEPA.h => SAT/SATAlgorithm.h} | 89 +-- .../SphereVsConvexMeshAlgorithm.cpp | 62 +++ .../narrowphase/SphereVsConvexMeshAlgorithm.h | 71 +++ src/collision/shapes/CollisionShape.h | 3 +- src/collision/shapes/ConeShape.cpp | 209 ------- src/collision/shapes/ConeShape.h | 194 ------- src/collision/shapes/CylinderShape.cpp | 231 -------- src/collision/shapes/CylinderShape.h | 190 ------- src/engine/OverlappingPair.cpp | 4 +- src/mathematics/Quaternion.h | 2 +- src/reactphysics3d.h | 2 - test/tests/collision/TestCollisionWorld.h | 39 -- test/tests/collision/TestPointInside.h | 215 +------- test/tests/collision/TestRaycast.h | 508 +----------------- testbed/CMakeLists.txt | 4 - testbed/common/Cone.cpp | 272 ---------- testbed/common/Cone.h | 110 ---- testbed/common/Cylinder.cpp | 272 ---------- testbed/common/Cylinder.h | 111 ---- testbed/common/Dumbbell.cpp | 22 +- testbed/common/Dumbbell.h | 6 +- .../CollisionDetectionScene.h | 2 - .../collisionshapes/CollisionShapesScene.cpp | 136 ----- .../collisionshapes/CollisionShapesScene.h | 8 - testbed/scenes/raycast/RaycastScene.cpp | 50 +- testbed/scenes/raycast/RaycastScene.h | 4 - 47 files changed, 792 insertions(+), 3999 deletions(-) create mode 100644 src/collision/HalfEdgeStructure.cpp create mode 100644 src/collision/HalfEdgeStructure.h create mode 100644 src/collision/PolyhedronMesh.cpp create mode 100644 src/collision/PolyhedronMesh.h rename src/collision/narrowphase/{EPA/TrianglesStore.cpp => CapsuleVsCapsuleAlgorithm.cpp} (87%) create mode 100644 src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h delete mode 100644 src/collision/narrowphase/EPA/EPAAlgorithm.cpp delete mode 100644 src/collision/narrowphase/EPA/EPAAlgorithm.h delete mode 100644 src/collision/narrowphase/EPA/EdgeEPA.cpp delete mode 100644 src/collision/narrowphase/EPA/TriangleEPA.cpp delete mode 100644 src/collision/narrowphase/EPA/TriangleEPA.h delete mode 100644 src/collision/narrowphase/EPA/TrianglesStore.h create mode 100644 src/collision/narrowphase/SAT/SATAlgorithm.cpp rename src/collision/narrowphase/{EPA/EdgeEPA.h => SAT/SATAlgorithm.h} (51%) create mode 100644 src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp create mode 100644 src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h delete mode 100644 src/collision/shapes/ConeShape.cpp delete mode 100644 src/collision/shapes/ConeShape.h delete mode 100644 src/collision/shapes/CylinderShape.cpp delete mode 100644 src/collision/shapes/CylinderShape.h delete mode 100644 testbed/common/Cone.cpp delete mode 100644 testbed/common/Cone.h delete mode 100644 testbed/common/Cylinder.cpp delete mode 100644 testbed/common/Cylinder.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b3d37ed1..9b6af8c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,23 +67,21 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/CollisionDispatch.h" "src/collision/narrowphase/DefaultCollisionDispatch.h" "src/collision/narrowphase/DefaultCollisionDispatch.cpp" - "src/collision/narrowphase/EPA/EdgeEPA.h" - "src/collision/narrowphase/EPA/EdgeEPA.cpp" - "src/collision/narrowphase/EPA/EPAAlgorithm.h" - "src/collision/narrowphase/EPA/EPAAlgorithm.cpp" - "src/collision/narrowphase/EPA/TriangleEPA.h" - "src/collision/narrowphase/EPA/TriangleEPA.cpp" - "src/collision/narrowphase/EPA/TrianglesStore.h" - "src/collision/narrowphase/EPA/TrianglesStore.cpp" "src/collision/narrowphase/GJK/VoronoiSimplex.h" "src/collision/narrowphase/GJK/VoronoiSimplex.cpp" "src/collision/narrowphase/GJK/GJKAlgorithm.h" "src/collision/narrowphase/GJK/GJKAlgorithm.cpp" + "src/collision/narrowphase/SAT/SATAlgorithm.h" + "src/collision/narrowphase/SAT/SATAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseAlgorithm.h" "src/collision/narrowphase/SphereVsSphereAlgorithm.h" "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp" + "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h" + "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" + "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h" + "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" @@ -96,12 +94,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/shapes/CapsuleShape.cpp" "src/collision/shapes/CollisionShape.h" "src/collision/shapes/CollisionShape.cpp" - "src/collision/shapes/ConeShape.h" - "src/collision/shapes/ConeShape.cpp" "src/collision/shapes/ConvexMeshShape.h" "src/collision/shapes/ConvexMeshShape.cpp" - "src/collision/shapes/CylinderShape.h" - "src/collision/shapes/CylinderShape.cpp" "src/collision/shapes/SphereShape.h" "src/collision/shapes/SphereShape.cpp" "src/collision/shapes/TriangleShape.h" @@ -118,6 +112,10 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/TriangleVertexArray.cpp" "src/collision/TriangleMesh.h" "src/collision/TriangleMesh.cpp" + "src/collision/PolyhedronMesh.h" + "src/collision/PolyhedronMesh.cpp" + "src/collision/HalfEdgeStructure.h" + "src/collision/HalfEdgeStructure.cpp" "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" "src/collision/NarrowPhaseInfo.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index be17e4f7..161f6328 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -300,9 +300,7 @@ void CollisionDetection::computeNarrowPhase() { // Select the narrow phase algorithm to use according to the two collision shapes const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType(); - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is no collision algorithm between those two kinds of shapes, skip it if (narrowPhaseAlgorithm != nullptr) { @@ -462,6 +460,17 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; + // Make sure the shape with the smallest collision shape type comes first + const uint shape1TypeIndex = static_cast(shape1->getCollisionShape()->getType()); + const uint shape2TypeIndex = static_cast(shape2->getCollisionShape()->getType()); + if (shape2TypeIndex > shape1TypeIndex) { + + // Swap the two shapes + ProxyShape* temp = shape1; + shape1 = shape2; + shape2 = temp; + } + // Compute the overlapping pair ID overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); @@ -681,9 +690,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) if (!isColliding) { // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -771,9 +778,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl if (!isColliding) { // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -846,9 +851,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body while (narrowPhaseInfo != nullptr) { // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -926,9 +929,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c while (narrowPhaseInfo != nullptr) { // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -999,9 +1000,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { while (narrowPhaseInfo != nullptr) { // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 3bb27366..c06a6357 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -124,6 +124,10 @@ class CollisionDetection { /// Fill-in the collision detection matrix void fillInCollisionMatrix(); + /// Return the corresponding narrow-phase algorithm + NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, + const CollisionShapeType& shape2Type) const; + /// Add all the contact manifold of colliding pairs to their bodies void addAllContactManifoldsToBodies(); @@ -153,10 +157,6 @@ class CollisionDetection { /// Set the collision dispatch configuration void setCollisionDispatch(CollisionDispatch* collisionDispatch); - /// Return the Narrow-phase collision detection algorithm to use between two types of shapes - NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const; - /// Add a proxy collision shape to the collision detection void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); @@ -231,12 +231,6 @@ class CollisionDetection { friend class ConvexMeshShape; }; -// Return the Narrow-phase collision detection algorithm to use between two types of shapes -inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const { - return mCollisionMatrix[static_cast(shape1Type)][static_cast(shape2Type)]; -} - // Set the collision dispatch configuration inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { mCollisionDispatch = collisionDispatch; @@ -280,6 +274,18 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } +// Return the corresponding narrow-phase algorithm +inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, + const CollisionShapeType& shape2Type) const { + + const unsigned int shape1Index = static_cast(shape1Type); + const unsigned int shape2Index = static_cast(shape2Type); + + assert(shape1Index <= shape2Index); + + return mCollisionMatrix[shape1Index][shape2Index]; +} + // Ray casting method inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, const Ray& ray, diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp new file mode 100644 index 00000000..16a79704 --- /dev/null +++ b/src/collision/HalfEdgeStructure.cpp @@ -0,0 +1,116 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "HalfEdgeStructure.h" +#include + +using namespace reactphysics3d; + +// Initialize the structure +void HalfEdgeStructure::init(std::vector vertices, std::vector> faces) { + + using edgeKey = std::pair; + + std::map edges; + std::map nextEdges; + std::map mapEdgeToStartVertex; + + // For each vertices + for (uint v=0; v& faceVertices = faces[f]; + + std::vector currentFaceEdges; + + edgeKey firstEdgeKey; + + // For each edge of the face + for (uint e=0; e < faceVertices.size(); e++) { + uint v1Index = faceVertices[e]; + uint v2Index = faceVertices[e == (faceVertices.size() - 1) ? 0 : e + 1]; + + const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index); + + // Create a new half-edge + Edge edge; + edge.faceIndex = f; + edge.vertexIndex = v1Index; + if (e == 0) { + firstEdgeKey = pairV1V2; + } + else if (e >= 1) { + nextEdges.insert(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2); + } + if (e == (faceVertices.size() - 1)) { + nextEdges.insert(pairV1V2, firstEdgeKey); + } + edges.insert(pairV1V2, edge); + + const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index); + + mapEdgeToStartVertex.insert(pairV1V2, v1Index); + mapEdgeToStartVertex.insert(pairV2V1, v2Index); + + auto itEdge = edges.find(pairV2V1); + if (itEdge != edges.end()) { + + const uint edgeIndex = mEdges.size(); + mEdges.push_back(itEdge->second); + mEdges.push_back(edge); + + itEdge->second.twinEdgeIndex = edgeIndex + 1; + itEdge->second.nextEdgeIndex = nextEdges[pairV2V1]; + + edge.twinEdgeIndex = edgeIndex; + edge.nextEdgeIndex = edges[nextEdges[pairV1V2]].; + + mVertices[v1Index].edgeIndex = edgeIndex; + mVertices[v2Index].edgeIndex = edgeIndex + 1; + + face.edgeIndex = edgeIndex + 1; + } + + currentFaceEdges.push_back(pairV1V2); + } + + // For each edge of the face + for (uint e=0; e < currentFaceEdges.size(); e++) { + Edge& edge = currentFaceEdges[e]; + edge.nextEdgeIndex = + } + } +} diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h new file mode 100644 index 00000000..71b549d0 --- /dev/null +++ b/src/collision/HalfEdgeStructure.h @@ -0,0 +1,142 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H +#define REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H + +// Libraries +#include "mathematics/mathematics.h" +#include + +namespace reactphysics3d { + +// Class HalfEdgeStructure +/** + * This class describes a polyhedron mesh made of faces and vertices. + * The faces do not have to be triangle. Note that the half-edge structure + * is only valid if the mesh is closed (each edge has two adjacent faces). + */ +class HalfEdgeStructure { + + public: + + struct Edge { + uint vertexIndex; // Index of the vertex at the end of the edge + uint twinEdgeIndex; // Index of the twin edge + uint faceIndex; // Adjacent face index of the edge + uint nextEdgeIndex; // Index of the next edge + }; + + struct Face { + uint edgeIndex; // Index of an half-edge of the face + }; + + struct Vertex { + const Vector3 point; // Coordinates of the vertex + uint edgeIndex; // Index of one edge emanting from this vertex + + /// Constructor + Vertex(const Vector3& p) { point = p;} + }; + + private: + + /// All the faces + std::vector mFaces; + + /// All the vertices + std::vector mVertices; + + /// All the half-edges + std::vector mEdges; + + public: + + /// Constructor + HalfEdgeStructure() = default; + + /// Destructor + ~HalfEdgeStructure() = default; + + /// Initialize the structure + void init(std::vector vertices, std::vector> faces); + + /// Return the number of faces + uint getNbFaces() const; + + /// Return the number of edges + uint getNbEdges() const; + + /// Return the number of vertices + uint getNbVertices() const; + + /// Return a given face + Face getFace(uint index) const; + + /// Return a given edge + Edge getHalfEdge(uint index) const; + + /// Retunr a given vertex + Vertex getVertex(uint index) const; + +}; + +// Return the number of faces +inline uint HalfEdgeStructure::getNbFaces() const { + return mFaces.size(); +} + +// Return the number of edges +inline uint HalfEdgeStructure::getNbEdges() const { + return mEdges.size(); +} + +// Return the number of vertices +inline uint HalfEdgeStructure::getNbVertices() const { + return mVertices.size(); +} + +// Return a given face +inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const { + assert(index < mFaces.size()); + return mFaces[index]; +} + +// Return a given edge +inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const { + assert(index < mEdges.size()); + return mEdges[index]; +} + +// Retunr a given vertex +inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const { + assert(index < mVertices.size()); + return mVertices[index]; +} + +} + +#endif + diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp new file mode 100644 index 00000000..891a2f47 --- /dev/null +++ b/src/collision/PolyhedronMesh.cpp @@ -0,0 +1,62 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "PolyhedronMesh.h" + +using namespace reactphysics3d; + + +// Constructor +PolyhedronMesh::PolyhedronMesh() : mIsFinalized(false) { + +} + +// Add a vertex into the polyhedron. +// This method returns the index of the vertex that you need to use +// to add faces. +uint PolyhedronMesh::addVertex(const Vector3& vertex) { + mVertices.push_back(vertex); + return mVertices.size() - 1; +} + +// Add a face into the polyhedron. +// A face is a list of vertices indices (returned by addVertex() method). +// The order of the indices are important. You need to specify the vertices of +// of the face sorted counter-clockwise as seen from the outside of the polyhedron. +void PolyhedronMesh::addFace(std::vector faceVertices) { + mFaces.push_back(faceVertices); +} + +// Call this method when you are done adding vertices and faces +void PolyhedronMesh::finalize() { + + if (mIsFinalized) return; + + // Initialize the half-edge structure + mHalfEdgeStructure.init(mVertices, mFaces); + + mIsFinalized = true; +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h new file mode 100644 index 00000000..8f0e8abf --- /dev/null +++ b/src/collision/PolyhedronMesh.h @@ -0,0 +1,86 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_POLYHEDRON_MESH_H +#define REACTPHYSICS3D_POLYHEDRON_MESH_H + +// Libraries +#include "mathematics/mathematics.h" +#include "HalfEdgeStructure.h" +#include + +namespace reactphysics3d { + +// Class PolyhedronMesh +/** + * This class describes a polyhedron mesh made of faces and vertices. + * The faces do not have to be triangle + */ +class PolyhedronMesh { + + private: + + /// Half-edge structure of the mesh + HalfEdgeStructure mHalfEdgeStructure; + + /// True if the half-edge structure has been generated + bool mIsFinalized; + + /// All the vertices + std::vector mVertices; + + /// All the indexes of the face vertices + std::vector> mFaces; + + public: + + /// Constructor + PolyhedronMesh(); + + /// Destructor + ~PolyhedronMesh() = default; + + /// Add a vertex into the polyhedron + uint addVertex(const Vector3& vertex); + + /// Add a face into the polyhedron + void addFace(std::vector faceVertices); + + /// Call this method when you are done adding vertices and faces + void finalize(); + + /// Return the half-edge structure of the mesh + const HalfEdgeStructure& getHalfEdgeStructure() const; +}; + +// Return the half-edge structure of the mesh +inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { + return mHalfEdgeStructure; +} + +} + +#endif + diff --git a/src/collision/TriangleMesh.h b/src/collision/TriangleMesh.h index 936824e9..e33fad67 100644 --- a/src/collision/TriangleMesh.h +++ b/src/collision/TriangleMesh.h @@ -38,7 +38,7 @@ namespace reactphysics3d { * This class represents a mesh made of triangles. A TriangleMesh contains * one or several parts. Each part is a set of triangles represented in a * TriangleVertexArray object describing all the triangles vertices of the part. - * A TriangleMesh object is used to create a ConcaveMeshShape from a triangle + * A TriangleMesh object can be used to create a ConcaveMeshShape from a triangle * mesh for instance. */ class TriangleMesh { diff --git a/src/collision/narrowphase/EPA/TrianglesStore.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp similarity index 87% rename from src/collision/narrowphase/EPA/TrianglesStore.cpp rename to src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 93fb95a6..44badf67 100644 --- a/src/collision/narrowphase/EPA/TrianglesStore.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -24,12 +24,14 @@ ********************************************************************************/ // Libraries -#include "TrianglesStore.h" +#include "CapsuleVsCapsuleAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" -// We use the ReactPhysics3D namespace +// We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Constructor -TrianglesStore::TrianglesStore() : mNbTriangles(0) { - +bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { + + } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h new file mode 100644 index 00000000..6a48495e --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two capsule collision shapes. + */ +class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 35b1e50b..4864dbbe 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -33,18 +33,22 @@ using namespace reactphysics3d; // use between two types of collision shapes. NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { + CollisionShapeType shape1Type = static_cast(type1); CollisionShapeType shape2Type = static_cast(type2); // Convex vs Convex algorithm (GJK algorithm) - if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { - return &mGJKAlgorithm; - } - // Sphere vs Sphere algorithm - else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { - return &mSphereVsSphereAlgorithm; - } - else { + if (type1 > type2) { return nullptr; } + // Sphere vs Sphere algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { + return &mSphereVsSphereAlgorithm; + } + // Sphere vs Convex shapes (convex Mesh, BoxShape) algorithm + if (shape1Type == CollisionShapeType::SPHERE && CollisionShape::isConvex(shape2Type)) { + return &mSphereVsConvexMeshAlgorithm; + } + + return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index f62a6564..07a819c0 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -30,6 +30,7 @@ #include "CollisionDispatch.h" #include "ConcaveVsConvexAlgorithm.h" #include "SphereVsSphereAlgorithm.h" +#include "SphereVsConvexMeshAlgorithm.h" #include "GJK/GJKAlgorithm.h" namespace reactphysics3d { @@ -47,6 +48,9 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Sphere collision algorithm SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; + /// Sphere vs Convex Mesh collision algorithm + SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm; + /// GJK Algorithm GJKAlgorithm mGJKAlgorithm; diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp deleted file mode 100644 index 6afa7a10..00000000 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "EPAAlgorithm.h" -#include "engine/Profiler.h" -#include "collision/narrowphase//GJK/GJKAlgorithm.h" -#include "TrianglesStore.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Decide if the origin is in the tetrahedron. -/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of -/// the vertex that is wrong if the origin is not in the tetrahedron -int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const { - - // Check vertex 1 - Vector3 normal1 = (p2-p1).cross(p3-p1); - if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { - return 4; - } - - // Check vertex 2 - Vector3 normal2 = (p4-p2).cross(p3-p2); - if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { - return 1; - } - - // Check vertex 3 - Vector3 normal3 = (p4-p3).cross(p1-p3); - if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { - return 2; - } - - // Check vertex 4 - Vector3 normal4 = (p2-p4).cross(p1-p4); - if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { - return 3; - } - - // The origin is in the tetrahedron, we return 0 - return 0; -} - -// Compute the penetration depth with the EPA algorithm. -/// This method computes the penetration depth and contact points between two -/// enlarged objects (with margin) where the original objects (without margin) -/// intersect. An initial simplex that contains origin has been computed with -/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find -/// the correct penetration depth. This method returns true if the EPA penetration -/// depth computation has succeeded and false it has failed. -bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - const NarrowPhaseInfo* narrowPhaseInfo, - Vector3& v, - ContactPointInfo& contactPointInfo) { - - PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - - decimal gjkPenDepthSquare = v.lengthSquare(); - - assert(narrowPhaseInfo->collisionShape1->isConvex()); - assert(narrowPhaseInfo->collisionShape2->isConvex()); - - const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); - const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - - Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates - Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates - Vector3 points[MAX_SUPPORT_POINTS]; // Current points - TrianglesStore triangleStore; // Store the triangles - TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face - // candidate of the EPA algorithm - - // Transform a point from local space of body 2 to local - // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; - - // Matrix that transform a direction from local - // space of body 1 into local space of body 2 - Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() * - narrowPhaseInfo->shape1ToWorldTransform.getOrientation(); - - // Get the simplex computed previously by the GJK algorithm - int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); - - // Compute the tolerance - decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); - - // Number of triangles in the polytope - unsigned int nbTriangles = 0; - - // Clear the storing of triangles - triangleStore.clear(); - - // Select an action according to the number of points in the simplex - // computed with GJK algorithm in order to obtain an initial polytope for - // The EPA algorithm. - switch(nbVertices) { - case 1: - // Only one point in the simplex (which should be the origin). - // We have a touching contact with zero penetration depth. - // We drop that kind of contact. Therefore, we return false - return false; - - case 2: { - // The simplex returned by GJK is a line segment d containing the origin. - // We add two additional support points to construct a hexahedron (two tetrahedron - // glued together with triangle faces. The idea is to compute three different vectors - // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively - // rotated of 120 degree around the d segment. The the three new points to - // construct the polytope are the three support points in those three directions - // v1, v2 and v3. - - // Direction of the segment - Vector3 d = (points[1] - points[0]).getUnit(); - - // Choose the coordinate axis from the minimal absolute component of the vector d - int minAxis = d.getAbsoluteVector().getMinAxis(); - - // Compute sin(60) - const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); - - // Create a rotation quaternion to rotate the vector v1 to get the vectors - // v2 and v3 - Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); - - // Compute the vector v1, v2, v3 - Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); - Vector3 v2 = rotationQuat * v1; - Vector3 v3 = rotationQuat * v2; - - // Compute the support point in the direction of v1 - suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); - suppPointsB[2] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); - points[2] = suppPointsA[2] - suppPointsB[2]; - - // Compute the support point in the direction of v2 - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; - - // Compute the support point in the direction of v3 - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; - - // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the - // tetrahedron that contains the origin in order that the initial polytope of the - // EPA algorithm is a tetrahedron, which is simpler to deal with. - - // If the origin is in the tetrahedron of points 0, 2, 3, 4 - if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 1 for the initial tetrahedron - suppPointsA[1] = suppPointsA[4]; - suppPointsB[1] = suppPointsB[4]; - points[1] = points[4]; - } - // If the origin is in the tetrahedron of points 1, 2, 3, 4 - else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 0 for the initial tetrahedron - suppPointsA[0] = suppPointsA[4]; - suppPointsB[0] = suppPointsB[4]; - points[0] = points[4]; - } - else { - // The origin is not in the initial polytope - return false; - } - - // The polytope contains now 4 vertices - nbVertices = 4; - } - case 4: { - // The simplex computed by the GJK algorithm is a tetrahedron. Here we check - // if this tetrahedron contains the origin. If it is the case, we keep it and - // otherwise we remove the wrong vertex of the tetrahedron and go in the case - // where the GJK algorithm compute a simplex of three vertices. - - // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) - int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); - - // If the origin is in the tetrahedron - if (badVertex == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); - TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); - TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); - TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); - - // If the constructed tetrahedron is not correct - if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return false; - } - - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - - break; - } - - // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) - // Remove the wrong vertex and continue to the next case with the - // three remaining vertices - if (badVertex < 4) { - - suppPointsA[badVertex-1] = suppPointsA[3]; - suppPointsB[badVertex-1] = suppPointsB[3]; - points[badVertex-1] = points[3]; - } - - // We have removed the wrong vertex - nbVertices = 3; - } - case 3: { - // The GJK algorithm returned a triangle that contains the origin. - // We need two new vertices to create two tetrahedron. The two new - // vertices are the support points in the "n" and "-n" direction - // where "n" is the normal of the triangle. Then, we use only the - // tetrahedron that contains the origin. - - // Compute the normal of the triangle - Vector3 v1 = points[1] - points[0]; - Vector3 v2 = points[2] - points[0]; - Vector3 n = v1.cross(v2); - - // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; - - TriangleEPA* face0 = nullptr; - TriangleEPA* face1 = nullptr; - TriangleEPA* face2 = nullptr; - TriangleEPA* face3 = nullptr; - - // If the origin is in the first tetrahedron - if (isOriginInTetrahedron(points[0], points[1], - points[2], points[3]) == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 3, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 3); - face3 = triangleStore.newTriangle(points, 1, 3, 2); - } - else if (isOriginInTetrahedron(points[0], points[1], - points[2], points[4]) == 0) { - - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 4, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 4); - face3 = triangleStore.newTriangle(points, 1, 4, 2); - } - else { - return false; - } - - // If the constructed tetrahedron is not correct - if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return false; - } - - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - - nbVertices = 4; - - } - break; - } - - // At this point, we have a polytope that contains the origin. Therefore, we - // can run the EPA algorithm. - - if (nbTriangles == 0) { - return false; - } - - TriangleEPA* triangle = 0; - decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; - - do { - triangle = triangleHeap[0]; - - // Get the next candidate face (the face closest to the origin) - std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); - nbTriangles--; - - // If the candidate face in the heap is not obsolete - if (!triangle->getIsObsolete()) { - - // If we have reached the maximum number of support points - if (nbVertices == MAX_SUPPORT_POINTS) { - assert(false); - break; - } - - // Compute the support point of the Minkowski - // difference (A-B) in the closest point direction - suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( - triangle->getClosestPoint(), shape1CachedCollisionData); - suppPointsB[nbVertices] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * - (-triangle->getClosestPoint()), shape2CachedCollisionData); - points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; - - int indexNewVertex = nbVertices; - nbVertices++; - - // Update the upper bound of the penetration depth - decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); - - decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); - if (wDotVSquare < upperBoundSquarePenDepth) { - upperBoundSquarePenDepth = wDotVSquare; - } - - // Compute the error - decimal error = wDotv - triangle->getDistSquare(); - if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || - points[indexNewVertex] == points[(*triangle)[0]] || - points[indexNewVertex] == points[(*triangle)[1]] || - points[indexNewVertex] == points[(*triangle)[2]]) { - break; - } - - // Now, we compute the silhouette cast by the new vertex. The current triangle - // face will not be in the convex hull. We start the local recursive silhouette - // algorithm from the current triangle face. - int i = triangleStore.getNbTriangles(); - if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { - break; - } - - // Add all the new triangle faces computed with the silhouette algorithm - // to the candidates list of faces of the current polytope - while(i != triangleStore.getNbTriangles()) { - TriangleEPA* newTriangle = &triangleStore[i]; - addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); - i++; - } - } - } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); - - // Compute the contact info - v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint(); - Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - Vector3 normal = v.getUnit(); - decimal penetrationDepth = v.length(); - - // If the length of the normal vector is too small, skip this contact point - if (normal.lengthSquare() < MACHINE_EPSILON) { - return false; - } - - if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) { - - // Create the contact info object - contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal); - - return true; - } - - return false; -} diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h deleted file mode 100644 index 1d241540..00000000 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_EPA_ALGORITHM_H -#define REACTPHYSICS3D_EPA_ALGORITHM_H - -// Libraries -#include "collision/narrowphase/GJK/VoronoiSimplex.h" -#include "collision/shapes/CollisionShape.h" -#include "collision/NarrowPhaseInfo.h" -#include "constraint/ContactPoint.h" -#include "collision/narrowphase/NarrowPhaseAlgorithm.h" -#include "mathematics/mathematics.h" -#include "TriangleEPA.h" -#include "memory/PoolAllocator.h" -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// ---------- Constants ---------- // - -/// Maximum number of support points of the polytope -constexpr unsigned int MAX_SUPPORT_POINTS = 100; - -/// Maximum number of facets of the polytope -constexpr unsigned int MAX_FACETS = 200; - - -// Class TriangleComparison -/** - * This class allows the comparison of two triangles in the heap - * The comparison between two triangles is made using their square distance to the closest - * point to the origin. The goal is that in the heap, the first triangle is the one with the - * smallest square distance. - */ -class TriangleComparison { - - public: - - /// Comparison operator - bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { - return (face1->getDistSquare() > face2->getDistSquare()); - } -}; - - -// Class EPAAlgorithm -/** - * This class is the implementation of the Expanding Polytope Algorithm (EPA). - * The EPA algorithm computes the penetration depth and contact points between - * two enlarged objects (with margin) where the original objects (without margin) - * intersect. The penetration depth of a pair of intersecting objects A and B is - * the length of a point on the boundary of the Minkowski sum (A-B) closest to the - * origin. The goal of the EPA algorithm is to start with an initial simplex polytope - * that contains the origin and expend it in order to find the point on the boundary - * of (A-B) that is closest to the origin. An initial simplex that contains origin - * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex - * polytope to find the correct penetration depth. The implementation of the EPA - * algorithm is based on the book "Collision Detection in 3D Environments". - */ -class EPAAlgorithm { - - private: - - // -------------------- Attributes -------------------- // - - /// Triangle comparison operator - TriangleComparison mTriangleComparison; - - // -------------------- Methods -------------------- // - - /// Add a triangle face in the candidate triangle heap - void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles, - decimal upperBoundSquarePenDepth); - - /// Decide if the origin is in the tetrahedron. - int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - EPAAlgorithm() = default; - - /// Destructor - ~EPAAlgorithm() = default; - - /// Deleted copy-constructor - EPAAlgorithm(const EPAAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; - - /// Compute the penetration depth with EPA algorithm. - bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - const NarrowPhaseInfo* narrowPhaseInfo, - Vector3& v, - ContactPointInfo &contactPointInfo); -}; - -// Add a triangle face in the candidate triangle heap in the EPA algorithm -inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, - uint& nbTriangles, decimal upperBoundSquarePenDepth) { - - // If the closest point of the affine hull of triangle - // points is internal to the triangle and if the distance - // of the closest point from the origin is at most the - // penetration depth upper bound - if (triangle->isClosestPointInternalToTriangle() && - triangle->getDistSquare() <= upperBoundSquarePenDepth) { - - // Add the triangle face to the list of candidates - heap[nbTriangles] = triangle; - nbTriangles++; - std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison); - } -} - -} - -#endif - diff --git a/src/collision/narrowphase/EPA/EdgeEPA.cpp b/src/collision/narrowphase/EPA/EdgeEPA.cpp deleted file mode 100644 index dcbf61fd..00000000 --- a/src/collision/narrowphase/EPA/EdgeEPA.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "EdgeEPA.h" -#include "TriangleEPA.h" -#include "TrianglesStore.h" -#include - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index) - : mOwnerTriangle(ownerTriangle), mIndex(index) { - assert(index >= 0 && index < 3); -} - -// Copy-constructor -EdgeEPA::EdgeEPA(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; -} - -// Return the index of the source vertex of the edge (vertex starting the edge) -uint EdgeEPA::getSourceVertexIndex() const { - return (*mOwnerTriangle)[mIndex]; -} - -// Return the index of the target vertex of the edge (vertex ending the edge) -uint EdgeEPA::getTargetVertexIndex() const { - return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; -} - -// Execute the recursive silhouette algorithm from this edge -bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - // If the edge has not already been visited - if (!mOwnerTriangle->getIsObsolete()) { - - // If the triangle of this edge is not visible from the given point - if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - // If the triangle has been created - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - else { - - // The current triangle is visible and therefore obsolete - mOwnerTriangle->setIsObsolete(true); - - int backup = triangleStore.getNbTriangles(); - - if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - // If the triangle has been created - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - - triangleStore.setNbTriangles(backup); - - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - } - } - - return true; -} diff --git a/src/collision/narrowphase/EPA/TriangleEPA.cpp b/src/collision/narrowphase/EPA/TriangleEPA.cpp deleted file mode 100644 index c322059f..00000000 --- a/src/collision/narrowphase/EPA/TriangleEPA.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - - -// Libraries -#include "TriangleEPA.h" -#include "EdgeEPA.h" -#include "TrianglesStore.h" - -// We use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3) - : mIsObsolete(false) { - mIndicesVertices[0] = indexVertex1; - mIndicesVertices[1] = indexVertex2; - mIndicesVertices[2] = indexVertex3; -} - -// Compute the point v closest to the origin of this triangle -bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { - const Vector3& p0 = vertices[mIndicesVertices[0]]; - - Vector3 v1 = vertices[mIndicesVertices[1]] - p0; - Vector3 v2 = vertices[mIndicesVertices[2]] - p0; - decimal v1Dotv1 = v1.dot(v1); - decimal v1Dotv2 = v1.dot(v2); - decimal v2Dotv2 = v2.dot(v2); - decimal p0Dotv1 = p0.dot(v1); - decimal p0Dotv2 = p0.dot(v2); - - // Compute determinant - mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; - - // Compute lambda values - mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; - mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; - - // If the determinant is positive - if (mDet > 0.0) { - // Compute the closest point v - mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); - - // Compute the square distance of closest point to the origin - mDistSquare = mClosestPoint.dot(mClosestPoint); - - return true; - } - - return false; -} - -/// Link an edge with another one. It means that the current edge of a triangle will -/// be associated with the edge of another triangle in order that both triangles -/// are neighbour along both edges). -bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { - bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - - if (isPossible) { - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; - edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; - } - - return isPossible; -} - -/// Make an half link of an edge with another one from another triangle. An half-link -/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an -/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will -/// be made later. -void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { - assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - - // Link - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; -} - -// Execute the recursive silhouette algorithm from this triangle face. -/// The parameter "vertices" is an array that contains the vertices of the current polytope and the -/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the -/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore, -/// the triangle faces that are visible from the new vertex must be removed from the polytope and we -/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette. -/// The silhouette is the connected set of edges that are part of the border between faces that -/// are seen and faces that are not seen from the new vertex. This method starts from the nearest -/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in -/// order that we always have a convex polytope. The faces visible from the new vertex are set -/// obselete and will not be considered as being a candidate face in the future. -bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - - uint first = triangleStore.getNbTriangles(); - - // Mark the current triangle as obsolete because it - setIsObsolete(true); - - // Execute recursively the silhouette algorithm for the adjacent edges of neighboring - // triangles of the current triangle - bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); - - if (result) { - int i,j; - - // For each triangle face that contains the new vertex and an edge of the silhouette - for (i=first, j=triangleStore.getNbTriangles()-1; - i != triangleStore.getNbTriangles(); j = i++) { - TriangleEPA* triangle = &triangleStore[i]; - halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); - - if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { - return false; - } - } - - } - - return result; -} diff --git a/src/collision/narrowphase/EPA/TriangleEPA.h b/src/collision/narrowphase/EPA/TriangleEPA.h deleted file mode 100644 index 865c69d4..00000000 --- a/src/collision/narrowphase/EPA/TriangleEPA.h +++ /dev/null @@ -1,197 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_TRIANGLE_EPA_H -#define REACTPHYSICS3D_TRIANGLE_EPA_H - -// Libraries -#include "mathematics/mathematics.h" -#include "configuration.h" -#include "EdgeEPA.h" -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Prototypes -bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); -void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); - - -// Class TriangleEPA -/** - * This class represents a triangle face of the current polytope in the EPA algorithm. - */ -class TriangleEPA { - - private: - - // -------------------- Attributes -------------------- // - - /// Indices of the vertices y_i of the triangle - uint mIndicesVertices[3]; - - /// Three adjacent edges of the triangle (edges of other triangles) - EdgeEPA mAdjacentEdges[3]; - - /// True if the triangle face is visible from the new support point - bool mIsObsolete; - - /// Determinant - decimal mDet; - - /// Point v closest to the origin on the affine hull of the triangle - Vector3 mClosestPoint; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda1; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda2; - - /// Square distance of the point closest point v to the origin - decimal mDistSquare; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - TriangleEPA() = default; - - /// Constructor - TriangleEPA(uint v1, uint v2, uint v3); - - /// Destructor - ~TriangleEPA() = default; - - /// Deleted copy-constructor - TriangleEPA(const TriangleEPA& triangle) = delete; - - /// Deleted assignment operator - TriangleEPA& operator=(const TriangleEPA& triangle) = delete; - - /// Return an adjacent edge of the triangle - EdgeEPA& getAdjacentEdge(int index); - - /// Set an adjacent edge of the triangle - void setAdjacentEdge(int index, EdgeEPA& edge); - - /// Return the square distance of the closest point to origin - decimal getDistSquare() const; - - /// Set the isObsolete value - void setIsObsolete(bool isObsolete); - - /// Return true if the triangle face is obsolete - bool getIsObsolete() const; - - /// Return the point closest to the origin - const Vector3& getClosestPoint() const; - - // Return true if the closest point on affine hull is inside the triangle - bool isClosestPointInternalToTriangle() const; - - /// Return true if the triangle is visible from a given vertex - bool isVisibleFromVertex(const Vector3* vertices, uint index) const; - - /// Compute the point v closest to the origin of this triangle - bool computeClosestPoint(const Vector3* vertices); - - /// Compute the point of an object closest to the origin - Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; - - /// Execute the recursive silhouette algorithm from this triangle face. - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); - - /// Access operator - uint operator[](int i) const; - - /// Associate two edges - friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); - - /// Make a half-link between two edges - friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); -}; - -// Return an edge of the triangle -inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) { - assert(index >= 0 && index < 3); - return mAdjacentEdges[index]; -} - -// Set an adjacent edge of the triangle -inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { - assert(index >=0 && index < 3); - mAdjacentEdges[index] = edge; -} - -// Return the square distance of the closest point to origin -inline decimal TriangleEPA::getDistSquare() const { - return mDistSquare; -} - -// Set the isObsolete value -inline void TriangleEPA::setIsObsolete(bool isObsolete) { - mIsObsolete = isObsolete; -} - -// Return true if the triangle face is obsolete -inline bool TriangleEPA::getIsObsolete() const { - return mIsObsolete; -} - -// Return the point closest to the origin -inline const Vector3& TriangleEPA::getClosestPoint() const { - return mClosestPoint; -} - -// Return true if the closest point on affine hull is inside the triangle -inline bool TriangleEPA::isClosestPointInternalToTriangle() const { - return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet); -} - -// Return true if the triangle is visible from a given vertex -inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const { - Vector3 closestToVert = vertices[index] - mClosestPoint; - return (mClosestPoint.dot(closestToVert) > 0.0); -} - -// Compute the point of an object closest to the origin -inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ - const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; - return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + - mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); -} - -// Access operator -inline uint TriangleEPA::operator[](int i) const { - assert(i >= 0 && i <3); - return mIndicesVertices[i]; -} - -} - -#endif diff --git a/src/collision/narrowphase/EPA/TrianglesStore.h b/src/collision/narrowphase/EPA/TrianglesStore.h deleted file mode 100644 index d21ccd35..00000000 --- a/src/collision/narrowphase/EPA/TrianglesStore.h +++ /dev/null @@ -1,139 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H -#define REACTPHYSICS3D_TRIANGLES_STORE_H - -#include "TriangleEPA.h" - - -// Libraries -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Constants -constexpr unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles - -// Class TriangleStore -/** - * This class stores several triangles of the polytope in the EPA algorithm. - */ -class TrianglesStore { - - private: - - // -------------------- Attributes -------------------- // - - /// Triangles - TriangleEPA mTriangles[MAX_TRIANGLES]; - - /// Number of triangles - int mNbTriangles; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - TrianglesStore(); - - /// Destructor - ~TrianglesStore() = default; - - /// Deleted copy-constructor - TrianglesStore(const TrianglesStore& triangleStore) = delete; - - /// Deleted assignment operator - TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete; - - /// Clear all the storage - void clear(); - - /// Return the number of triangles - int getNbTriangles() const; - - /// Set the number of triangles - void setNbTriangles(int backup); - - /// Return the last triangle - TriangleEPA& last(); - - /// Create a new triangle - TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); - - /// Access operator - TriangleEPA& operator[](int i); -}; - -// Clear all the storage -inline void TrianglesStore::clear() { - mNbTriangles = 0; -} - -// Return the number of triangles -inline int TrianglesStore::getNbTriangles() const { - return mNbTriangles; -} - - -inline void TrianglesStore::setNbTriangles(int backup) { - mNbTriangles = backup; -} - -// Return the last triangle -inline TriangleEPA& TrianglesStore::last() { - assert(mNbTriangles > 0); - return mTriangles[mNbTriangles - 1]; -} - -// Create a new triangle -inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, - uint v0,uint v1, uint v2) { - TriangleEPA* newTriangle = nullptr; - - // If we have not reached the maximum number of triangles - if (mNbTriangles != MAX_TRIANGLES) { - newTriangle = &mTriangles[mNbTriangles++]; - new (newTriangle) TriangleEPA(v0, v1, v2); - if (!newTriangle->computeClosestPoint(vertices)) { - mNbTriangles--; - newTriangle = nullptr; - } - } - - // Return the new triangle - return newTriangle; -} - -// Access operator -inline TriangleEPA& TrianglesStore::operator[](int i) { - return mTriangles[i]; -} - -} - -#endif diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index e1649af0..cd84f5a0 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. -bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, +GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { PROFILE("GJKAlgorithm::testCollision()"); @@ -101,8 +101,7 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // Compute the support points for original objects (without margins) A and B suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * - shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); + suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -116,7 +115,7 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v); // No intersection, we return - return false; + return GJKResult::SEPARATED; } // If the objects intersect only in the margins @@ -135,7 +134,8 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // Contact point has been found contactFound = true; - break; + + return GJKResult::INTERPENETRATE; } // Compute the point of the simplex closest to the origin @@ -144,13 +144,14 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // Contact point has been found contactFound = true; - break; + + return GJKResult::INTERPENETRATE; } // Closest point is almost the origin, go to EPA algorithm // Vector v to small to continue computing support points if (v.lengthSquare() < MACHINE_EPSILON) { - break; + return GJKResult::INTERPENETRATE; } // Store and update the squared distance of the closest point @@ -173,20 +174,6 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); - // If no contact has been found (penetration case) - if (!contactFound) { - - // The objects (without margins) intersect. Therefore, we run the GJK algorithm - // again but on the enlarged objects to compute a simplex polytope that contains - // the origin. Then, we give that simplex polytope to the EPA algorithm to compute - // the correct penetration depth and contact points between the enlarged objects. - if(computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v)) { - - // A contact has been found with EPA algorithm, we return true - return true; - } - } - if (contactFound && distSquare > MACHINE_EPSILON) { // Compute the closet points of both objects (without the margins) @@ -205,103 +192,21 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // If the penetration depth is negative (due too numerical errors), there is no contact if (penetrationDepth <= decimal(0.0)) { - return false; + return GJKResult::INTERPENETRATE; } // Do not generate a contact point with zero normal length if (normal.lengthSquare() < MACHINE_EPSILON) { - return false; + return GJKResult::INTERPENETRATE; } // Create the contact info object contactPointInfo.init(normal, penetrationDepth, pA, pB); - return true; + return GJKResult::COLLIDE_IN_MARGIN; } - return false; -} - -/// This method runs the GJK algorithm on the two enlarged objects (with margin) -/// to compute a simplex polytope that contains the origin. The two objects are -/// assumed to intersect in the original objects (without margin). Therefore such -/// a polytope must exist. Then, we give that polytope to the EPA algorithm to -/// compute the correct penetration depth and contact points of the enlarged objects. -bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo, - Vector3& v) { - PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); - - VoronoiSimplex simplex; - Vector3 suppA; - Vector3 suppB; - Vector3 w; - decimal vDotw; - decimal distSquare = DECIMAL_LARGEST; - decimal prevDistSquare; - - assert(narrowPhaseInfo->collisionShape1->isConvex()); - assert(narrowPhaseInfo->collisionShape2->isConvex()); - - const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); - const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - - bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); - - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - - // Transform a point from local space of body 2 to local space - // of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; - - // Matrix that transform a direction from local space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() * - narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix(); - - do { - // Compute the support points for the enlarged object A and B - suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); - suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); - - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; - - vDotw = v.dot(w); - - // If the enlarge objects do not intersect - if (vDotw > decimal(0.0)) { - - // No intersection, we return - return false; - } - - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); - - if (simplex.isAffinelyDependent()) { - return false; - } - - if (!simplex.computeClosestPoint(v)) { - return false; - } - - // Store and update the square distance - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); - - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - return false; - } - - } while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() && - distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); - - // Give the simplex computed with GJK algorithm to the EPA algorithm - // which will compute the correct penetration depth and contact points - // between the two enlarged objects - return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo); + return GJKResult::SEPARATED; } // Use the GJK Algorithm to find if a point is inside a convex collision shape diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e0dad891..8ca25e33 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -27,11 +27,9 @@ #define REACTPHYSICS3D_GJK_ALGORITHM_H // Libraries -#include "collision/narrowphase/NarrowPhaseAlgorithm.h" #include "constraint/ContactPoint.h" #include "collision/shapes/ConvexShape.h" -#include "collision/narrowphase/EPA/EPAAlgorithm.h" - +#include "VoronoiSimplex.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -57,24 +55,22 @@ constexpr int MAX_ITERATIONS_GJK_RAYCAST = 32; * Polytope Algorithm) to compute the correct penetration depth between the * enlarged objects. */ -class GJKAlgorithm : public NarrowPhaseAlgorithm { +class GJKAlgorithm { private : // -------------------- Attributes -------------------- // - /// EPA Algorithm - EPAAlgorithm mAlgoEPA; - // -------------------- Methods -------------------- // - /// Compute the penetration depth for enlarged objects. - bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo, - Vector3& v); - public : + enum class GJKResult { + SEPARATED, // The two shapes are separated outside the margin + COLLIDE_IN_MARGIN, // The two shapes overlap only in the margin (shallow penetration) + INTERPENETRATE // The two shapes overlap event without the margin (deep penetration) + }; + // -------------------- Methods -------------------- // /// Constructor @@ -90,14 +86,16 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volumes collide. - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo); /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); + + }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp new file mode 100644 index 00000000..d74faf09 --- /dev/null +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -0,0 +1,43 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SATAlgorithm.h" +#include "constraint/ContactPoint.h" +#include "configuration.h" +#include "engine/Profiler.h" +#include +#include +#include +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { + + +} diff --git a/src/collision/narrowphase/EPA/EdgeEPA.h b/src/collision/narrowphase/SAT/SATAlgorithm.h similarity index 51% rename from src/collision/narrowphase/EPA/EdgeEPA.h rename to src/collision/narrowphase/SAT/SATAlgorithm.h index ba3dcfee..30156cc2 100644 --- a/src/collision/narrowphase/EPA/EdgeEPA.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -23,100 +23,45 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_EDGE_EPA_H -#define REACTPHYSICS3D_EDGE_EPA_H - +#ifndef REACTPHYSICS3D_SAT_ALGORITHM_H +#define REACTPHYSICS3D_SAT_ALGORITHM_H // Libraries -#include "mathematics/mathematics.h" +#include "constraint/ContactPoint.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Class declarations -class TriangleEPA; -class TrianglesStore; +// Class SATAlgorithm +class SATAlgorithm { -// Class EdgeEPA -/** - * This class represents an edge of the current polytope in the EPA algorithm. - */ -class EdgeEPA { - - private: + private : // -------------------- Attributes -------------------- // - /// Pointer to the triangle that contains this edge - TriangleEPA* mOwnerTriangle; + // -------------------- Methods -------------------- // - /// Index of the edge in the triangle (between 0 and 2). - /// The edge with index i connect triangle vertices i and (i+1 % 3) - int mIndex; - - public: + public : // -------------------- Methods -------------------- // /// Constructor - EdgeEPA() = default; - - /// Constructor - EdgeEPA(TriangleEPA* ownerTriangle, int index); - - /// Copy-constructor - EdgeEPA(const EdgeEPA& edge); + SATAlgorithm() = default; /// Destructor - ~EdgeEPA() = default; + ~SATAlgorithm() = default; - /// Return the pointer to the owner triangle - TriangleEPA* getOwnerTriangle() const; + /// Deleted copy-constructor + SATAlgorithm(const SATAlgorithm& algorithm) = delete; - /// Return the index of the edge in the triangle - int getIndex() const; + /// Deleted assignment operator + SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; - /// Return index of the source vertex of the edge - uint getSourceVertexIndex() const; - - /// Return the index of the target vertex of the edge - uint getTargetVertexIndex() const; - - /// Execute the recursive silhouette algorithm from this edge - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); - - /// Assignment operator - EdgeEPA& operator=(const EdgeEPA& edge); + /// Compute a contact info if the two bounding volumes collide. + bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo); }; -// Return the pointer to the owner triangle -inline TriangleEPA* EdgeEPA::getOwnerTriangle() const { - return mOwnerTriangle; -} - -// Return the edge index -inline int EdgeEPA::getIndex() const { - return mIndex; -} - -// Assignment operator -inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; - return *this; -} - -// Return the index of the next counter-clockwise edge of the ownver triangle -inline int indexOfNextCounterClockwiseEdge(int i) { - return (i + 1) % 3; -} - -// Return the index of the previous counter-clockwise edge of the ownver triangle -inline int indexOfPreviousCounterClockwiseEdge(int i) { - return (i + 2) % 3; -} - } #endif - diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp new file mode 100644 index 00000000..65371e4f --- /dev/null +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp @@ -0,0 +1,62 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SphereVsConvexMeshAlgorithm.h" +#include "SAT/SATAlgorithm.h" +#include "collision/shapes/SphereShape.h" +#include "collision/shapes/ConvexMeshShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { + + // Get the local-space to world-space transforms + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; + + // First, we run the GJK algorithm + GJKAlgorithm gjkAlgorithm; + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo); + + // If we have found a contact point inside the margins (shallow penetration) + if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + + // Return true + return true; + } + + // If we have overlap even without the margins (deep penetration) + if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm; + return satAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo); + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h new file mode 100644 index 00000000..448b8a5d --- /dev/null +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsConvexMeshAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a sphere and a convex mesh. + */ +class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsConvexMeshAlgorithm() = default; + + /// Destructor + virtual ~SphereVsConvexMeshAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsConvexMeshAlgorithm(const SphereVsConvexMeshAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsConvexMeshAlgorithm& operator=(const SphereVsConvexMeshAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index b33ed4f1..8a591614 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -40,8 +40,7 @@ namespace reactphysics3d { /// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, - CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; +enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; const int NB_COLLISION_SHAPE_TYPES = 9; // Declarations diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp deleted file mode 100644 index 4a1ece33..00000000 --- a/src/collision/shapes/ConeShape.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include -#include "configuration.h" -#include "ConeShape.h" -#include "collision/ProxyShape.h" - -using namespace reactphysics3d; - -// Constructor -/** - * @param radius Radius of the cone (in meters) - * @param height Height of the cone (in meters) - * @param margin Collision margin (in meters) around the collision shape - */ -ConeShape::ConeShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CollisionShapeType::CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) { - assert(mRadius > decimal(0.0)); - assert(mHalfHeight > decimal(0.0)); - - // Compute the sine of the semi-angle at the apex point - mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); -} - -// Return a local support point in a given direction without the object margin -Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - const Vector3& v = direction; - decimal sinThetaTimesLengthV = mSinTheta * v.length(); - Vector3 supportPoint; - - if (v.y > sinThetaTimesLengthV) { - supportPoint = Vector3(0.0, mHalfHeight, 0.0); - } - else { - decimal projectedLength = sqrt(v.x * v.x + v.z * v.z); - if (projectedLength > MACHINE_EPSILON) { - decimal d = mRadius / projectedLength; - supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); - } - else { - supportPoint = Vector3(0.0, -mHalfHeight, 0.0); - } - } - - return supportPoint; -} - -// Raycast method with feedback information -// This implementation is based on the technique described by David Eberly in the article -// "Intersection of a Line and a Cone" that can be found at -// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf -bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const Vector3 r = ray.point2 - ray.point1; - - const decimal epsilon = decimal(0.00001); - Vector3 V(0, mHalfHeight, 0); - Vector3 centerBase(0, -mHalfHeight, 0); - Vector3 axis(0, decimal(-1.0), 0); - decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; - decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); - decimal factor = decimal(1.0) - cosThetaSquare; - Vector3 delta = ray.point1 - V; - decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - - cosThetaSquare * delta.z * delta.z; - decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; - decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; - decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)}; - Vector3 localHitPoint[3]; - Vector3 localNormal[3]; - - // If c2 is different from zero - if (std::abs(c2) > MACHINE_EPSILON) { - decimal gamma = c1 * c1 - c0 * c2; - - // If there is no real roots in the quadratic equation - if (gamma < decimal(0.0)) { - return false; - } - else if (gamma > decimal(0.0)) { // The equation has two real roots - - // Compute two intersections - decimal sqrRoot = std::sqrt(gamma); - tHit[0] = (-c1 - sqrRoot) / c2; - tHit[1] = (-c1 + sqrRoot) / c2; - } - else { // If the equation has a single real root - - // Compute the intersection - tHit[0] = -c1 / c2; - } - } - else { // If c2 == 0 - - // If c2 = 0 and c1 != 0 - if (std::abs(c1) > MACHINE_EPSILON) { - tHit[0] = -c0 / (decimal(2.0) * c1); - } - else { // If c2 = c1 = 0 - - // If c0 is different from zero, no solution and if c0 = 0, we have a - // degenerate case, the whole ray is contained in the cone side - // but we return no hit in this case - return false; - } - } - - // If the origin of the ray is inside the cone, we return no hit - if (testPointInside(ray.point1, nullptr)) return false; - - localHitPoint[0] = ray.point1 + tHit[0] * r; - localHitPoint[1] = ray.point1 + tHit[1] * r; - - // Only keep hit points in one side of the double cone (the cone we are interested in) - if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { - tHit[0] = decimal(-1.0); - } - if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) { - tHit[1] = decimal(-1.0); - } - - // Only keep hit points that are within the correct height of the cone - if (localHitPoint[0].y < decimal(-mHalfHeight)) { - tHit[0] = decimal(-1.0); - } - if (localHitPoint[1].y < decimal(-mHalfHeight)) { - tHit[1] = decimal(-1.0); - } - - // If the ray is in direction of the base plane of the cone - if (r.y > epsilon) { - - // Compute the intersection with the base plane of the cone - tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); - - // Only keep this intersection if it is inside the cone radius - localHitPoint[2] = ray.point1 + tHit[2] * r; - - if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { - tHit[2] = decimal(-1.0); - } - - // Compute the normal direction - localNormal[2] = axis; - } - - // Find the smallest positive t value - int hitIndex = -1; - decimal t = DECIMAL_LARGEST; - for (int i=0; i<3; i++) { - if (tHit[i] < decimal(0.0)) continue; - if (tHit[i] < t) { - hitIndex = i; - t = tHit[hitIndex]; - } - } - - if (hitIndex < 0) return false; - if (t > ray.maxFraction) return false; - - // Compute the normal direction for hit against side of the cone - if (hitIndex != 2) { - decimal h = decimal(2.0) * mHalfHeight; - decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + - localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); - decimal rOverH = mRadius / h; - decimal value2 = decimal(1.0) + rOverH * rOverH; - decimal factor = decimal(1.0) / std::sqrt(value1 * value2); - decimal x = localHitPoint[hitIndex].x * factor; - decimal z = localHitPoint[hitIndex].z * factor; - localNormal[hitIndex].x = x; - localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; - localNormal[hitIndex].z = z; - } - - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint[hitIndex]; - raycastInfo.worldNormal = localNormal[hitIndex]; - - return true; -} diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h deleted file mode 100644 index 16102163..00000000 --- a/src/collision/shapes/ConeShape.h +++ /dev/null @@ -1,194 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CONE_SHAPE_H -#define REACTPHYSICS3D_CONE_SHAPE_H - -// Libraries -#include "ConvexShape.h" -#include "body/CollisionBody.h" -#include "mathematics/mathematics.h" - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class ConeShape -/** - * This class represents a cone collision shape centered at the - * origin and alligned with the Y axis. The cone is defined - * by its height and by the radius of its base. The center of the - * cone is at the half of the height. The "transform" of the - * corresponding rigid body gives an orientation and a position - * to the cone. This collision shape uses an extra margin distance around - * it for collision detection purpose. The default margin is 4cm (if your - * units are meters, which is recommended). In case, you want to simulate small - * objects (smaller than the margin distance), you might want to reduce the margin - * by specifying your own margin distance using the "margin" parameter in the - * constructor of the cone shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. - */ -class ConeShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - - /// Radius of the base - decimal mRadius; - - /// Half height of the cone - decimal mHalfHeight; - - /// sine of the semi angle at the apex point - decimal mSinTheta; - - // -------------------- Methods -------------------- // - - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const override; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN); - - /// Destructor - virtual ~ConeShape() override = default; - - /// Deleted copy-constructor - ConeShape(const ConeShape& shape) = delete; - - /// Deleted assignment operator - ConeShape& operator=(const ConeShape& shape) = delete; - - /// Return the radius - decimal getRadius() const; - - /// Return the height - decimal getHeight() const; - - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; -}; - -// Return the radius -/** - * @return Radius of the cone (in meters) - */ -inline decimal ConeShape::getRadius() const { - return mRadius; -} - -// Return the height -/** - * @return Height of the cone (in meters) - */ -inline decimal ConeShape::getHeight() const { - return decimal(2.0) * mHalfHeight; -} - -// Set the scaling vector of the collision shape -inline void ConeShape::setLocalScaling(const Vector3& scaling) { - - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -inline size_t ConeShape::getSizeInBytes() const { - return sizeof(ConeShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { - - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; - - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; -} - -// Return true if the collision shape is a polyhedron -inline bool ConeShape::isPolyhedron() const { - return false; -} - -// Return the local inertia tensor of the collision shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal rSquare = mRadius * mRadius; - decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); - tensor.setAllValues(diagXZ, 0.0, 0.0, - 0.0, decimal(0.3) * mass * rSquare, - 0.0, 0.0, 0.0, diagXZ); -} - -// Return true if a point is inside the collision shape -inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / - (mHalfHeight * decimal(2.0)); - return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && - (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); -} - -} - -#endif diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp deleted file mode 100644 index 8cb405f1..00000000 --- a/src/collision/shapes/CylinderShape.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CylinderShape.h" -#include "collision/ProxyShape.h" -#include "configuration.h" - -using namespace reactphysics3d; - -// Constructor -/** - * @param radius Radius of the cylinder (in meters) - * @param height Height of the cylinder (in meters) - * @param margin Collision margin (in meters) around the collision shape - */ -CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CollisionShapeType::CYLINDER, margin), mRadius(radius), - mHalfHeight(height/decimal(2.0)) { - assert(radius > decimal(0.0)); - assert(height > decimal(0.0)); -} - -// Return a local support point in a given direction without the object margin -Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - Vector3 supportPoint(0.0, 0.0, 0.0); - decimal uDotv = direction.y; - Vector3 w(direction.x, 0.0, direction.z); - decimal lengthW = std::sqrt(direction.x * direction.x + direction.z * direction.z); - - if (lengthW > MACHINE_EPSILON) { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - supportPoint += (mRadius / lengthW) * w; - } - else { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - } - - return supportPoint; -} - -// Raycast method with feedback information -/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by -/// Morgan Kaufmann. -bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const Vector3 n = ray.point2 - ray.point1; - - const decimal epsilon = decimal(0.01); - Vector3 p(decimal(0), -mHalfHeight, decimal(0)); - Vector3 q(decimal(0), mHalfHeight, decimal(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; - decimal t; - - decimal mDotD = m.dot(d); - decimal nDotD = n.dot(d); - decimal dDotD = d.dot(d); - - // Test if the segment is outside the cylinder - if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false; - if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; - - decimal nDotN = n.dot(n); - decimal mDotN = m.dot(n); - - decimal a = dDotD * nDotN - nDotD * nDotD; - decimal k = m.dot(m) - mRadius * mRadius; - decimal c = dDotD * k - mDotD * mDotD; - - // If the ray is parallel to the cylinder axis - if (std::abs(a) < epsilon) { - - // If the origin is outside the surface of the cylinder, we return no hit - if (c > decimal(0.0)) return false; - - // Here we know that the segment intersect an endcap of the cylinder - - // If the ray intersects with the "p" endcap of the cylinder - if (mDotD < decimal(0.0)) { - - t = -mDotN / nDotN; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder - - t = (nDotD - mDotN) / nDotN; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else { // If the origin is inside the cylinder, we return no hit - return false; - } - } - decimal b = dDotD * mDotN - nDotD * mDotD; - decimal discriminant = b * b - a * c; - - // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < decimal(0.0)) return false; - - // Compute the smallest root (first intersection along the ray) - decimal t0 = t = (-b - std::sqrt(discriminant)) / a; - - // If the intersection is outside the cylinder on "p" endcap side - decimal value = mDotD + t * nDotD; - if (value < decimal(0.0)) { - - // If the ray is pointing away from the "p" endcap, we return no hit - if (nDotD <= decimal(0.0)) return false; - - // Compute the intersection against the "p" endcap (intersection agains whole plane) - t = -mDotD / nDotD; - - // Keep the intersection if the it is inside the cylinder radius - if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side - - // If the ray is pointing away from the "q" endcap, we return no hit - if (nDotD >= decimal(0.0)) return false; - - // Compute the intersection against the "q" endcap (intersection against whole plane) - t = (dDotD - mDotD) / nDotD; - - // Keep the intersection if it is inside the cylinder radius - if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > - decimal(0.0)) return false; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - - t = t0; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)); - raycastInfo.worldNormal = normalDirection; - - return true; -} diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h deleted file mode 100644 index e0744a80..00000000 --- a/src/collision/shapes/CylinderShape.h +++ /dev/null @@ -1,190 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CYLINDER_SHAPE_H -#define REACTPHYSICS3D_CYLINDER_SHAPE_H - -// Libraries -#include "ConvexShape.h" -#include "body/CollisionBody.h" -#include "mathematics/mathematics.h" - - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class CylinderShape -/** - * This class represents a cylinder collision shape around the Y axis - * and centered at the origin. The cylinder is defined by its height - * and the radius of its base. The "transform" of the corresponding - * rigid body gives an orientation and a position to the cylinder. - * This collision shape uses an extra margin distance around it for collision - * detection purpose. The default margin is 4cm (if your units are meters, - * which is recommended). In case, you want to simulate small objects - * (smaller than the margin distance), you might want to reduce the margin by - * specifying your own margin distance using the "margin" parameter in the - * constructor of the cylinder shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. - */ -class CylinderShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - - /// Radius of the base - decimal mRadius; - - /// Half height of the cylinder - decimal mHalfHeight; - - // -------------------- Methods -------------------- // - - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override ; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const override; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN); - - /// Destructor - virtual ~CylinderShape() override = default; - - /// Deleted copy-constructor - CylinderShape(const CylinderShape& shape) = delete; - - /// Deleted assignment operator - CylinderShape& operator=(const CylinderShape& shape) = delete; - - /// Return the radius - decimal getRadius() const; - - /// Return the height - decimal getHeight() const; - - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; -}; - -// Return the radius -/** - * @return Radius of the cylinder (in meters) - */ -inline decimal CylinderShape::getRadius() const { - return mRadius; -} - -// Return the height -/** - * @return Height of the cylinder (in meters) - */ -inline decimal CylinderShape::getHeight() const { - return mHalfHeight + mHalfHeight; -} - -// Return true if the collision shape is a polyhedron -inline bool CylinderShape::isPolyhedron() const { - return false; -} - -// Set the scaling vector of the collision shape -inline void CylinderShape::setLocalScaling(const Vector3& scaling) { - - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -inline size_t CylinderShape::getSizeInBytes() const { - return sizeof(CylinderShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { - - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; - - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; -} - -// Return the local inertia tensor of the cylinder -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal height = decimal(2.0) * mHalfHeight; - decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setAllValues(diag, 0.0, 0.0, 0.0, - decimal(0.5) * mass * mRadius * mRadius, 0.0, - 0.0, 0.0, diag); -} - -// Return true if a point is inside the collision shape -inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{ - return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); -} - -} - -#endif - diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 48658f5c..6df4bf4e 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -24,15 +24,17 @@ ********************************************************************************/ // Libraries +#include #include "OverlappingPair.h" using namespace reactphysics3d; - // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int nbMaxContactManifolds, PoolAllocator& memoryAllocator) : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), mCachedSeparatingAxis(0.0, 1.0, 0.0) { + assert(static_cast(shape1->getCollisionShape()->getType()) <= + static_cast(shape2->getCollisionShape()->getType())); } diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 360cf229..28f7b636 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -199,7 +199,7 @@ inline Vector3 Quaternion::getVectorV() const { // Return the length of the quaternion (inline) inline decimal Quaternion::length() const { - return sqrt(x*x + y*y + z*z + w*w); + return std::sqrt(x*x + y*y + z*z + w*w); } // Return the square of the length of the quaternion diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 6f1e42f4..a1bef6dc 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -46,8 +46,6 @@ #include "collision/shapes/CollisionShape.h" #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" -#include "collision/shapes/ConeShape.h" -#include "collision/shapes/CylinderShape.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/ConcaveMeshShape.h" diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index 94d44a98..242ebcf9 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -48,8 +48,6 @@ class WorldCollisionCallback : public CollisionCallback public: bool boxCollideWithSphere1; - bool boxCollideWithCylinder; - bool sphere1CollideWithCylinder; bool sphere1CollideWithSphere2; CollisionBody* boxBody; @@ -65,8 +63,6 @@ class WorldCollisionCallback : public CollisionCallback void reset() { boxCollideWithSphere1 = false; - boxCollideWithCylinder = false; - sphere1CollideWithCylinder = false; sphere1CollideWithSphere2 = false; } @@ -76,12 +72,6 @@ class WorldCollisionCallback : public CollisionCallback if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) { boxCollideWithSphere1 = true; } - else if (isContactBetweenBodies(boxBody, cylinderBody, collisionCallbackInfo)) { - boxCollideWithCylinder = true; - } - else if (isContactBetweenBodies(sphere1Body, cylinderBody, collisionCallbackInfo)) { - sphere1CollideWithCylinder = true; - } else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) { sphere1CollideWithSphere2 = true; } @@ -118,7 +108,6 @@ class TestCollisionWorld : public Test { // Collision shapes BoxShape* mBoxShape; SphereShape* mSphereShape; - CylinderShape* mCylinderShape; // Proxy shapes ProxyShape* mBoxProxyShape; @@ -154,11 +143,6 @@ class TestCollisionWorld : public Test { mSphere2Body = mWorld->createCollisionBody(sphere2Transform); mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); - Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity()); - mCylinderBody = mWorld->createCollisionBody(cylinderTransform); - mCylinderShape = new CylinderShape(2, 5); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity()); - // Assign collision categories to proxy shapes mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); @@ -175,7 +159,6 @@ class TestCollisionWorld : public Test { ~TestCollisionWorld() { delete mBoxShape; delete mSphereShape; - delete mCylinderShape; } /// Run the tests @@ -189,8 +172,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); @@ -206,22 +187,16 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with sphere 2 @@ -230,22 +205,16 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with box @@ -260,8 +229,6 @@ class TestCollisionWorld : public Test { mSphere2Body->setIsActive(false); mWorld->testCollision(&mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); @@ -289,8 +256,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with sphere 2 @@ -299,8 +264,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(mCollisionCallback.sphere1CollideWithSphere2); mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); @@ -311,8 +274,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); test(!mCollisionCallback.sphere1CollideWithSphere2); // Move sphere 1 to collide with box diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 68345739..dd2a793b 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -31,9 +31,7 @@ #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/shapes/ConeShape.h" #include "collision/shapes/ConvexMeshShape.h" -#include "collision/shapes/CylinderShape.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -65,10 +63,8 @@ class TestPointInside : public Test { BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; ConvexMeshShape* mConvexMeshShape; ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; - CylinderShape* mCylinderShape; // Transform Transform mBodyTransform; @@ -128,9 +124,6 @@ class TestPointInside : public Test { mCapsuleShape = new CapsuleShape(2, 10); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); - mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, -4)); @@ -168,15 +161,12 @@ class TestPointInside : public Test { mConvexMeshShapeBodyEdgesInfo, mShapeTransform); - mCylinderShape = new CylinderShape(3, 8, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - - // Compound shape is a cylinder and a sphere + // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); } @@ -185,10 +175,8 @@ class TestPointInside : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConeShape; delete mConvexMeshShape; delete mConvexMeshShapeBodyEdgesInfo; - delete mCylinderShape; } /// Run the tests @@ -196,9 +184,7 @@ class TestPointInside : public Test { testBox(); testSphere(); testCapsule(); - testCone(); testConvexMesh(); - testCylinder(); testCompound(); } @@ -407,83 +393,6 @@ class TestPointInside : public Test { test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); } - /// Test the ProxyConeShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCone() { - - // Tests with CollisionBody - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - - // Tests with ProxyConeShape - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - } - /// Test the ProxyConvexMeshShape::testPointInside() and /// CollisionBody::testPointInside() methods void testConvexMesh() { @@ -597,127 +506,11 @@ class TestPointInside : public Test { test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); } - /// Test the ProxyCylinderShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCylinder() { - - // Tests with CollisionBody - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); - - // Tests with ProxyCylinderShape - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); - } - /// Test the CollisionBody::testPointInside() method void testCompound() { - // Points on the cylinder + // Points on the capsule + // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 7295a8d9..b39b751b 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -33,9 +33,7 @@ #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/shapes/ConeShape.h" #include "collision/shapes/ConvexMeshShape.h" -#include "collision/shapes/CylinderShape.h" #include "collision/shapes/TriangleShape.h" #include "collision/shapes/ConcaveMeshShape.h" #include "collision/shapes/HeightFieldShape.h" @@ -130,10 +128,8 @@ class TestRaycast : public Test { BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; ConvexMeshShape* mConvexMeshShape; ConvexMeshShape* mConvexMeshShapeEdgesInfo; - CylinderShape* mCylinderShape; TriangleShape* mTriangleShape; ConcaveShape* mConcaveMeshShape; HeightFieldShape* mHeightFieldShape; @@ -214,9 +210,6 @@ class TestRaycast : public Test { mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); - // Box of dimension (2, 3, 4) mConvexMeshShape = new ConvexMeshShape(0.0); mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); @@ -255,15 +248,12 @@ class TestRaycast : public Test { mConvexMeshShapeEdgesInfo, mShapeTransform); - mCylinderShape = new CylinderShape(2, 5, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); // Concave Mesh shape @@ -328,10 +318,8 @@ class TestRaycast : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConeShape; delete mConvexMeshShape; delete mConvexMeshShapeEdgesInfo; - delete mCylinderShape; delete mTriangleShape; delete mConcaveMeshShape; delete mHeightFieldShape; @@ -344,9 +332,7 @@ class TestRaycast : public Test { testBox(); testSphere(); testCapsule(); - testCone(); testConvexMesh(); - testCylinder(); testCompound(); testTriangle(); testConcaveMesh(); @@ -1313,253 +1299,6 @@ class TestRaycast : public Test { mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback); } - /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCone() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1); - - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0); - Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0); - - mCallback.shapeToTest = mConeProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mConeBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mConeBody); - test(raycastInfo2.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConeProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConeBody); - test(raycastInfo3.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - mCallback.reset(); - mWorld->raycast(rayBottom, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); - - // CollisionBody::raycast() - RaycastInfo raycastInfo5; - test(mConeBody->raycast(rayBottom, raycastInfo5)); - test(raycastInfo5.body == mConeBody); - test(raycastInfo5.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mConeProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mConeBody); - test(raycastInfo6.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1)); - Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74)); - Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27)); - Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30)); - - // ----- Test raycast miss ----- // - test(!mConeBody->raycast(ray1, raycastInfo3)); - test(!mConeProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray2, raycastInfo3)); - test(!mConeProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray3, raycastInfo3)); - test(!mConeProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray4, raycastInfo3)); - test(!mConeProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray5, raycastInfo3)); - test(!mConeProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray6, raycastInfo3)); - test(!mConeProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray7, raycastInfo3)); - test(!mConeProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray8, raycastInfo3)); - test(!mConeProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray9, raycastInfo3)); - test(!mConeProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray10, raycastInfo3)); - test(!mConeProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mConeBody->raycast(ray11, raycastInfo3)); - test(mConeProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray12, raycastInfo3)); - test(mConeProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray13, raycastInfo3)); - test(mConeProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray14, raycastInfo3)); - test(mConeProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray15, raycastInfo3)); - test(mConeProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray16, raycastInfo3)); - test(mConeProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - /// Test the ProxyConvexMeshShape::raycast(), CollisionBody::raycast() and /// CollisionWorld::raycast() methods. void testConvexMesh() { @@ -1824,248 +1563,6 @@ class TestRaycast : public Test { test(mCallback.isHit); } - /// Test the ProxyCylinderShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCylinder() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); - - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0); - Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(2.5), 0); - - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0); - Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-2.5), 0); - - mCallback.shapeToTest = mCylinderProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mCylinderBody); - test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mCylinderBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mCylinderBody); - test(raycastInfo2.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mCylinderProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mCylinderBody); - test(raycastInfo3.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mCylinderProxyShape->raycast(rayTop, raycastInfo5)); - test(raycastInfo5.body == mCylinderBody); - test(raycastInfo5.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mCylinderProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mCylinderBody); - test(raycastInfo6.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mCylinderBody->raycast(ray1, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray2, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray3, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray4, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray5, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray6, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray7, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray8, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray9, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray10, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mCylinderBody->raycast(ray11, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray12, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray13, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray14, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray15, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray16, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - /// Test the CollisionBody::raycast() and /// CollisionWorld::raycast() methods. void testCompound() { @@ -2141,7 +1638,8 @@ class TestRaycast : public Test { mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); test(mCallback.isHit); - // Raycast hit agains the cylinder shape + // Raycast hit agains the capsule shape + // TODO : Previous it was a cylinder, now it is a capsule shape, maybe those tests are wrong now Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2)); Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1)); Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index 8d402ad3..a5620028 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -74,8 +74,6 @@ SET(TESTBED_SOURCES SET(COMMON_SOURCES common/Box.h common/Box.cpp - common/Cone.h - common/Cone.cpp common/Sphere.h common/Sphere.cpp common/Line.h @@ -86,8 +84,6 @@ SET(COMMON_SOURCES common/ConvexMesh.cpp common/ConcaveMesh.h common/ConcaveMesh.cpp - common/Cylinder.h - common/Cylinder.cpp common/Dumbbell.h common/Dumbbell.cpp common/HeightField.h diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp deleted file mode 100644 index bbd32ed9..00000000 --- a/testbed/common/Cone.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "Cone.h" - -openglframework::VertexBufferObject Cone::mVBOVertices(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBONormals(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBOTextureCoords(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); -openglframework::VertexArrayObject Cone::mVAO; -int Cone::totalNbCones = 0; - -// Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) { - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cone will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cone shape) and do - // not forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; - - // Create a rigid body corresponding to the cone in the dynamics world - mBody = world->createCollisionBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity()); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } - - totalNbCones++; -} - -// Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) { - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cone will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cone shape) and do not - // forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - // Create a rigid body corresponding to the cone in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass); - - mBody = body; - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } - - totalNbCones++; -} - -// Destructor -Cone::~Cone() { - - if (totalNbCones == 1) { - // Destroy the mesh - destroy(); - - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mConeShape; - totalNbCones--; -} - -// Render the cone at the correct position and with the correct orientation -void Cone::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } - - // Bind the shader - shader.bind(); - - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); - - // Bind the VAO - mVAO.bind(); - - mVBOVertices.bind(); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - - mVBONormals.bind(); - - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - - // For each part of the mesh - for (unsigned int i=0; isetLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); -} - diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h deleted file mode 100644 index 8f71493c..00000000 --- a/testbed/common/Cone.h +++ /dev/null @@ -1,110 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CONE_H -#define CONE_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "PhysicsObject.h" - -// Class Cone -class Cone : public PhysicsObject { - - private : - - // -------------------- Attributes -------------------- // - - /// Radius of the cone - float mRadius; - - /// Height of the cone - float mHeight; - - /// Collision shape - rp3d::ConeShape* mConeShape; - rp3d::ProxyShape* mProxyShape; - - /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions) - openglframework::Matrix4 mScalingMatrix; - - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; - - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; - - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; - - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; - - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; - - // Total number of cones created - static int totalNbCones; - - // -------------------- Methods -------------------- // - - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); - - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); - - /// Destructor - ~Cone(); - - /// Render the cone at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor) override; - - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); -}; - -inline void Cone::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); -} - -#endif diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp deleted file mode 100644 index 2943c3e8..00000000 --- a/testbed/common/Cylinder.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "Cylinder.h" - -openglframework::VertexBufferObject Cylinder::mVBOVertices(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBONormals(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBOTextureCoords(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); -openglframework::VertexArrayObject Cylinder::mVAO; -int Cylinder::totalNbCylinders = 0; - -// Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) { - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cylinder will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cylinder shape) and do not - // forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; - - // Create a rigid body corresponding to the cylinder in the dynamics world - mBody = world->createCollisionBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity()); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } - - totalNbCylinders++; -} - -// Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) { - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cylinder will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cylinder shape) and do - // not forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - // Create a rigid body corresponding to the cylinder in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - mBody = body; - - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } - - totalNbCylinders++; -} - -// Destructor -Cylinder::~Cylinder() { - - if (totalNbCylinders == 1) { - - // Destroy the mesh - destroy(); - - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mCylinderShape; - totalNbCylinders--; -} - -// Render the cylinder at the correct position and with the correct orientation -void Cylinder::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } - - // Bind the shader - shader.bind(); - - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); - - // Bind the VAO - mVAO.bind(); - - mVBOVertices.bind(); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - - mVBONormals.bind(); - - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - - // For each part of the mesh - for (unsigned int i=0; isetLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); -} diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h deleted file mode 100644 index 3a94b443..00000000 --- a/testbed/common/Cylinder.h +++ /dev/null @@ -1,111 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CYLINDER_H -#define CYLINDER_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "PhysicsObject.h" - -// Class Cylinder -class Cylinder : public PhysicsObject { - - private : - - // -------------------- Attributes -------------------- // - - /// Radius of the cylinder - float mRadius; - - /// Height of the cylinder - float mHeight; - - /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions) - openglframework::Matrix4 mScalingMatrix; - - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; - - /// Collision shape - rp3d::CylinderShape* mCylinderShape; - rp3d::ProxyShape* mProxyShape; - - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; - - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; - - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; - - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; - - // Total number of capsules created - static int totalNbCylinders; - - // -------------------- Methods -------------------- // - - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string &meshFolderPath); - - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath); - - /// Destructor - ~Cylinder(); - - /// Render the cylinder at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); - - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor) override; - - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); -}; - -// Update the transform matrix of the object -inline void Cylinder::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); -} - -#endif diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp index 03afd59b..02afe091 100644 --- a/testbed/common/Dumbbell.cpp +++ b/testbed/common/Dumbbell.cpp @@ -53,13 +53,13 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, const rp3d::decimal massSphere = rp3d::decimal(2.0); mSphereShape = new rp3d::SphereShape(radiusSphere); - // Create a cylinder collision shape for the middle of the dumbbell + // Create a capsule collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); + const rp3d::decimal radiusCapsule = rp3d::decimal(0.5); + const rp3d::decimal heightCapsule = rp3d::decimal(7.0); const rp3d::decimal massCylinder = rp3d::decimal(1.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); + mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); @@ -84,7 +84,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere); mProxyShapeSphere2 = body->addCollisionShape(mSphereShape, transformSphereShape2, massSphere); - mProxyShapeCylinder = body->addCollisionShape(mCylinderShape, transformCylinderShape, massCylinder); + mProxyShapeCapsule = body->addCollisionShape(mCapsuleShape, transformCylinderShape, massCylinder); mBody = body; @@ -120,9 +120,9 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Create a cylinder collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); + const rp3d::decimal radiusCapsule = rp3d::decimal(0.5); + const rp3d::decimal heightCapsule = rp3d::decimal(7.0); + mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); @@ -145,7 +145,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1); mProxyShapeSphere2 = mBody->addCollisionShape(mSphereShape, transformSphereShape2); - mProxyShapeCylinder = mBody->addCollisionShape(mCylinderShape, transformCylinderShape); + mProxyShapeCapsule = mBody->addCollisionShape(mCapsuleShape, transformCylinderShape); mTransformMatrix = mTransformMatrix * mScalingMatrix; @@ -173,7 +173,7 @@ Dumbbell::~Dumbbell() { mVAO.destroy(); } delete mSphereShape; - delete mCylinderShape; + delete mCapsuleShape; totalNbDumbbells--; } @@ -304,7 +304,7 @@ void Dumbbell::setScaling(const openglframework::Vector3& scaling) { // Scale the collision shape rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z); - mProxyShapeCylinder->setLocalScaling(newScaling); + mProxyShapeCapsule->setLocalScaling(newScaling); mProxyShapeSphere1->setLocalScaling(newScaling); mProxyShapeSphere2->setLocalScaling(newScaling); diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index 81020935..ea2d3d88 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -31,7 +31,7 @@ #include "reactphysics3d.h" #include "PhysicsObject.h" -// Class Sphere +// Class Dumbbell class Dumbbell : public PhysicsObject { private : @@ -42,9 +42,9 @@ class Dumbbell : public PhysicsObject { float mDistanceBetweenSphere; /// Collision shapes - rp3d::CylinderShape* mCylinderShape; + rp3d::CapsuleShape* mCapsuleShape; rp3d::SphereShape* mSphereShape; - rp3d::ProxyShape* mProxyShapeCylinder; + rp3d::ProxyShape* mProxyShapeCapsule; rp3d::ProxyShape* mProxyShapeSphere1; rp3d::ProxyShape* mProxyShapeSphere2; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index b7f57c1d..d3a7de00 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -33,8 +33,6 @@ #include "SceneDemo.h" #include "Sphere.h" #include "Box.h" -#include "Cone.h" -#include "Cylinder.h" #include "Capsule.h" #include "Line.h" #include "ConvexMesh.h" diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index 8db4908a..746942ff 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -125,62 +125,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) mSpheres.push_back(sphere); } - // Create all the cones of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); - - // Set the box color - cone->setColor(mDemoColors[i % mNbDemoColors]); - cone->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = cone->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - - // Add the cone the list of sphere in the scene - mCones.push_back(cone); - } - - // Create all the cylinders of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); - - // Set the box color - cylinder->setColor(mDemoColors[i % mNbDemoColors]); - cylinder->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - - // Add the cylinder the list of sphere in the scene - mCylinders.push_back(cylinder); - } - // Create all the capsules of the scene for (int i=0; i::iterator it = mCones.begin(); it != mCones.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - // Destroy all the capsules of the scene for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { @@ -410,20 +334,6 @@ void CollisionShapesScene::update() { (*it)->updateTransform(mInterpolationFactor); } - // Update the position and orientation of the cones - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the cylinders - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - // Update the position and orientation of the capsules for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { @@ -469,16 +379,6 @@ void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader, (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); } - // Render all the cones of the scene - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - // Render all the capsules of the scene for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); @@ -563,42 +463,6 @@ void CollisionShapesScene::reset() { mSpheres[i]->setTransform(transform); } - // Create all the cones of the scene - for (int i=0; isetTransform(transform); - } - - // Create all the cylinders of the scene - for (int i=0; isetTransform(transform); - } - // Create all the capsules of the scene for (int i=0; i mSpheres; - std::vector mCones; - - std::vector mCylinders; - std::vector mCapsules; /// All the convex meshes of the scene diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index 58b19055..c403ebb2 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -79,28 +79,6 @@ RaycastScene::RaycastScene(const std::string& name) mSphere->setColor(mGreyColorDemo); mSphere->setSleepingColor(mRedColorDemo); - // ---------- Cone ---------- // - openglframework::Vector3 position4(0, 0, 0); - - // Create a cone and a corresponding collision body in the dynamics world - mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, - mMeshFolderPath); - - // Set the color - mCone->setColor(mGreyColorDemo); - mCone->setSleepingColor(mRedColorDemo); - - // ---------- Cylinder ---------- // - openglframework::Vector3 position5(0, 0, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, - mCollisionWorld, mMeshFolderPath); - - // Set the color - mCylinder->setColor(mGreyColorDemo); - mCylinder->setSleepingColor(mRedColorDemo); - // ---------- Capsule ---------- // openglframework::Vector3 position6(0, 0, 0); @@ -188,8 +166,6 @@ void RaycastScene::changeBody() { mSphere->getCollisionBody()->setIsActive(false); mBox->getCollisionBody()->setIsActive(false); - mCone->getCollisionBody()->setIsActive(false); - mCylinder->getCollisionBody()->setIsActive(false); mCapsule->getCollisionBody()->setIsActive(false); mConvexMesh->getCollisionBody()->setIsActive(false); mDumbbell->getCollisionBody()->setIsActive(false); @@ -201,19 +177,15 @@ void RaycastScene::changeBody() { break; case 1: mBox->getCollisionBody()->setIsActive(true); break; - case 2: mCone->getCollisionBody()->setIsActive(true); + case 2: mCapsule->getCollisionBody()->setIsActive(true); break; - case 3: mCylinder->getCollisionBody()->setIsActive(true); + case 3: mConvexMesh->getCollisionBody()->setIsActive(true); break; - case 4: mCapsule->getCollisionBody()->setIsActive(true); + case 4: mDumbbell->getCollisionBody()->setIsActive(true); break; - case 5: mConvexMesh->getCollisionBody()->setIsActive(true); + case 5: mConcaveMesh->getCollisionBody()->setIsActive(true); break; - case 6: mDumbbell->getCollisionBody()->setIsActive(true); - break; - case 7: mConcaveMesh->getCollisionBody()->setIsActive(true); - break; - case 8: mHeightField->getCollisionBody()->setIsActive(true); + case 6: mHeightField->getCollisionBody()->setIsActive(true); break; } @@ -238,16 +210,6 @@ RaycastScene::~RaycastScene() { mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody()); delete mSphere; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); - delete mCone; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); - - // Destroy the sphere - delete mCylinder; - // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); @@ -378,8 +340,6 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, // Render the shapes if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled); diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index 56ebb565..be698e53 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -34,8 +34,6 @@ #include "SceneDemo.h" #include "Sphere.h" #include "Box.h" -#include "Cone.h" -#include "Cylinder.h" #include "Capsule.h" #include "Line.h" #include "ConvexMesh.h" @@ -141,8 +139,6 @@ class RaycastScene : public SceneDemo { /// All objects on the scene Box* mBox; Sphere* mSphere; - Cone* mCone; - Cylinder* mCylinder; Capsule* mCapsule; ConvexMesh* mConvexMesh; Dumbbell* mDumbbell; From 30e0132e158accaa87a5eb21eeab6ab8b79fd476 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 Feb 2017 23:10:01 +0100 Subject: [PATCH 007/105] Add capsule/capsule and capsule/sphere collision algorithm --- CMakeLists.txt | 2 + .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 188 +++- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 142 +-- .../narrowphase/SphereVsSphereAlgorithm.cpp | 138 +-- .../narrowphase/SphereVsSphereAlgorithm.h | 142 +-- src/mathematics/mathematics_functions.cpp | 261 ++++-- src/mathematics/mathematics_functions.h | 190 ++-- .../mathematics/TestMathematicsFunctions.h | 312 ++++--- .../CollisionDetectionScene.cpp | 832 +++++++++--------- .../CollisionDetectionScene.h | 466 +++++----- 10 files changed, 1500 insertions(+), 1173 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b6af8c9..37b8febe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h" "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h" diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 44badf67..2f9f8703 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -1,37 +1,151 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CapsuleVsCapsuleAlgorithm.h" -#include "collision/shapes/CapsuleShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { - - -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CapsuleVsCapsuleAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { + + const decimal parallelEpsilon = decimal(0.001); + + // Get the capsule collision shapes + const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the transform from capsule 1 local-space to capsule 2 local-space + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse(); + + // Compute the end-points of the inner segment of the first capsule + Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0); + Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0); + capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; + capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; + + // Compute the end-points of the inner segment of the second capsule + const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0); + const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0); + + // The two inner capsule segments + const Vector3 seg1 = capsule1SegB - capsule1SegA; + const Vector3 seg2 = capsule2SegB - capsule2SegA; + + // Compute the sum of the radius of the two capsules (virtual spheres) + decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); + + // If the two capsules are parallel (we create two contact points) + if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) { + + // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) + const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); + if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) { + + // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. + // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. + return false; + } + + // Compute the planes that goes through the extrem points of the inner segment of capsule 1 + decimal d1 = seg1.dot(capsule1SegA); + decimal d2 = -seg1.dot(capsule1SegB); + + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + + bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid) + + // Clip the inner segment of capsule 2 + Vector3 clipPointA, clipPointB; + if (t1 >= decimal(0.0)) { + + if (t1 > decimal(1.0)) t1 = decimal(1.0); + clipPointA = capsule2SegB - t1 * seg2; + isClipValid = true; + } + if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) { + + if (t2 > decimal(1.0)) t2 = decimal(1.0); + clipPointB = capsule2SegA + t2 * seg2; + isClipValid = true; + } + + // If we have a valid clip segment + if (isClipValid) { + + Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA); + Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + + const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + + decimal penetrationDepth = sumRadius - segmentsDistance; + + // Create the contact info object + // TODO : Make sure we create two contact points at the same time (same method here) + contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local); + contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local); + } + } + + // Compute the closest points between the two inner capsule segments + Vector3 closestPointCapsule1Seg; + Vector3 closestPointCapsule2Seg; + computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, + closestPointCapsule1Seg, closestPointCapsule2Seg); + + // Compute the distance between the sphere center and the closest point on the segment + Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); + const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); + + // If the collision shapes overlap + if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { + + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; + + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + + decimal penetrationDepth = sumRadius - closestPointsDistance; + + // Create the contact info object + contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 6a48495e..7a46f73a 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -1,71 +1,71 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H -#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H - -// Libraries -#include "body/Body.h" -#include "constraint/ContactPoint.h" -#include "NarrowPhaseAlgorithm.h" - - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class CapsuleVsCapsuleAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between two capsule collision shapes. - */ -class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { - - protected : - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - CapsuleVsCapsuleAlgorithm() = default; - - /// Destructor - virtual ~CapsuleVsCapsuleAlgorithm() override = default; - - /// Deleted copy-constructor - CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; -}; - -} - -#endif - +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two capsules collision shapes. + */ +class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 6b28372d..e80688d5 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -1,69 +1,69 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "SphereVsSphereAlgorithm.h" -#include "collision/shapes/SphereShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); - const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); - - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - - // Compute the distance between the centers - Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); - decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); - - // Compute the sum of the radius - decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); - - // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); - - // Create the contact info object - contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); - - return true; - } - - return false; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SphereVsSphereAlgorithm.h" +#include "collision/shapes/SphereShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) { + + // Get the sphere collision shapes + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the local-space to world-space transforms + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; + + // Compute the distance between the centers + Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + + // Compute the sum of the radius + decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + + // If the sphere collision shapes intersect + if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + Vector3 intersectionOnBody1 = sphereShape1->getRadius() * + centerSphere2InBody1LocalSpace.getUnit(); + Vector3 intersectionOnBody2 = sphereShape2->getRadius() * + centerSphere1InBody2LocalSpace.getUnit(); + decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + + // Create the contact info object + contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 24886f7f..be10d5a1 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -1,71 +1,71 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H -#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H - -// Libraries -#include "body/Body.h" -#include "constraint/ContactPoint.h" -#include "NarrowPhaseAlgorithm.h" - - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class SphereVsSphereAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between two sphere collision shapes. - */ -class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - - protected : - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - SphereVsSphereAlgorithm() = default; - - /// Destructor - virtual ~SphereVsSphereAlgorithm() override = default; - - /// Deleted copy-constructor - SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; -}; - -} - -#endif - +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsSphereAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ +class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsSphereAlgorithm() = default; + + /// Destructor + virtual ~SphereVsSphereAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 97bf6dcf..578cecdd 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -1,59 +1,202 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "mathematics_functions.h" -#include "Vector3.h" - -using namespace reactphysics3d; - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -/// This method uses the technique described in the book Real-Time collision detection by -/// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; - - decimal d00 = v0.dot(v0); - decimal d01 = v0.dot(v1); - decimal d11 = v1.dot(v1); - decimal d20 = v2.dot(v0); - decimal d21 = v2.dot(v1); - - decimal denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = decimal(1.0) - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; - } - return vector; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "mathematics_functions.h" +#include "Vector3.h" +#include + +using namespace reactphysics3d; + +/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +/// This method uses the technique described in the book Real-Time collision detection by +/// Christer Ericson. +void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w) { + const Vector3 v0 = b - a; + const Vector3 v1 = c - a; + const Vector3 v2 = p - a; + + decimal d00 = v0.dot(v0); + decimal d01 = v0.dot(v1); + decimal d11 = v1.dot(v1); + decimal d20 = v2.dot(v0); + decimal d21 = v2.dot(v1); + + decimal denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = decimal(1.0) - v - w; +} + +/// Clamp a vector such that it is no longer than a given maximum length +Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; +} + +/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { + + const Vector3 ab = segPointB - segPointA; + + decimal abLengthSquare = ab.lengthSquare(); + + // If the segment has almost zero length + if (abLengthSquare < MACHINE_EPSILON) { + + // Return one end-point of the segment as the closest point + return segPointA; + } + + // Project point C onto "AB" line + decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; + + // If projected point onto the line is outside the segment, clamp it to the segment + if (t < decimal(0.0)) t = decimal(0.0); + if (t > decimal(1.0)) t = decimal(1.0); + + // Return the closest point on the segment + return segPointA + t * ab; +} + +/// Compute the closest points between two segments +/// This method uses the technique described in the book Real-Time +/// collision detection by Christer Ericson. +void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2) { + + const Vector3 d1 = seg1PointB - seg1PointA; + const Vector3 d2 = seg2PointB - seg2PointA; + const Vector3 r = seg1PointA - seg2PointA; + decimal a = d1.lengthSquare(); + decimal e = d2.lengthSquare(); + decimal f = d2.dot(r); + decimal s, t; + + // If both segments degenerate into points + if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { + + closestPointSeg1 = seg1PointA; + closestPointSeg2 = seg2PointA; + return; + } + if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point + + s = decimal(0.0); + + // Compute the closest point on second segment + t = clamp(f / e, decimal(0.0), decimal(1.0)); + } + else { + + decimal c = d1.dot(r); + + // If the second segment degenerates into a point + if (e <= MACHINE_EPSILON) { + + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else { + + decimal b = d1.dot(d2); + decimal denom = a * e - b * b; + + // If the segments are not parallel + if (denom != decimal(0.0)) { + + // Compute the closest point on line 1 to line 2 and + // clamp to first segment. + s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); + } + else { + + // Pick an arbitrary point on first segment + s = decimal(0.0); + } + + // Compute the point on line 2 closest to the closest point + // we have just found + t = (b * s + f) / e; + + // If this closest point is inside second segment (t in [0, 1]), we are done. + // Otherwise, we clamp the point to the second segment and compute again the + // closest point on segment 1 + if (t < decimal(0.0)) { + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else if (t > decimal(1.0)) { + t = decimal(1.0); + s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); + } + } + } + + // Compute the closest points on both segments + closestPointSeg1 = seg1PointA + d1 * s; + closestPointSeg2 = seg2PointA + d2 * t; +} + +/// Compute the intersection between a plane and a segment +// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method +// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such +// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, +// there is no intersection between the plane and the segment. +decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { + + const decimal parallelEpsilon = decimal(0.0001); + decimal t = decimal(-1); + + // Segment AB + const Vector3 ab = segB - segA; + + decimal nDotAB = planeNormal.dot(ab); + + // If the segment is not parallel to the plane + if (nDotAB > parallelEpsilon) { + t = (planeD - planeNormal.dot(segA)) / nDotAB; + } + + return t; +} + +/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { + + decimal distAB = (linePointB - linePointA).length(); + + if (distAB < MACHINE_EPSILON) { + return (point - linePointA).length(); + } + + return ((point - linePointA).cross(point - linePointB)).length() / distAB; +} + + diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 7efe6d44..315de65c 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -1,87 +1,103 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H -#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H - -// Libraries -#include "configuration.h" -#include "decimal.h" -#include -#include -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -struct Vector3; - -// ---------- Mathematics functions ---------- // - -/// Function to test if two real numbers are (almost) equal -/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline int clamp(int value, int lowerLimit, int upperLimit) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); -} - -/// Return the minimum value among three values -inline decimal min3(decimal a, decimal b, decimal c) { - return std::min(std::min(a, b), c); -} - -/// Return the maximum value among three values -inline decimal max3(decimal a, decimal b, decimal c) { - return std::max(std::max(a, b), c); -} - -/// Return true if two values have the same sign -inline bool sameSign(decimal a, decimal b) { - return a * b >= decimal(0.0); -} - -/// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal maxLength); - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w); - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H +#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "configuration.h" +#include "decimal.h" +#include +#include +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +struct Vector3; + +// ---------- Mathematics functions ---------- // + +/// Function to test if two real numbers are (almost) equal +/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] +inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { + return (std::fabs(a - b) < epsilon); +} + +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline int clamp(int value, int lowerLimit, int upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + +/// Return the minimum value among three values +inline decimal min3(decimal a, decimal b, decimal c) { + return std::min(std::min(a, b), c); +} + +/// Return the maximum value among three values +inline decimal max3(decimal a, decimal b, decimal c) { + return std::max(std::max(a, b), c); +} + +/// Return true if two values have the same sign +inline bool sameSign(decimal a, decimal b) { + return a * b >= decimal(0.0); +} + +/// Clamp a vector such that it is no longer than a given maximum length +Vector3 clamp(const Vector3& vector, decimal maxLength); + +// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC); + +// Compute the closest points between two segments +void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2); + +/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w); + +/// Compute the intersection between a plane and a segment +decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); + +/// Compute the distance between a point and a line +decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); + +} + + + +#endif diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 5041fd48..32e19aad 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -1,133 +1,179 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef TEST_MATHEMATICS_FUNCTIONS_H -#define TEST_MATHEMATICS_FUNCTIONS_H - -// Libraries -#include "Test.h" -#include "mathematics/mathematics_functions.h" - -/// Reactphysics3D namespace -namespace reactphysics3d { - -// Class TestMathematicsFunctions -/** - * Unit test for mathematics functions - */ -class TestMathematicsFunctions : public Test { - - private : - - // ---------- Atributes ---------- // - - - - public : - - // ---------- Methods ---------- // - - /// Constructor - TestMathematicsFunctions(const std::string& name): Test(name) {} - - /// Run the tests - void run() { - - // Test approxEqual() - test(approxEqual(2, 7, 5.2)); - test(approxEqual(7, 2, 5.2)); - test(approxEqual(6, 6)); - test(!approxEqual(1, 5)); - test(!approxEqual(1, 5, 3)); - test(approxEqual(-2, -2)); - test(approxEqual(-2, -7, 6)); - test(!approxEqual(-2, 7, 2)); - test(approxEqual(-3, 8, 12)); - test(!approxEqual(-3, 8, 6)); - - // Test clamp() - test(clamp(4, -3, 5) == 4); - test(clamp(-3, 1, 8) == 1); - test(clamp(45, -6, 7) == 7); - test(clamp(-5, -2, -1) == -2); - test(clamp(-5, -9, -1) == -5); - test(clamp(6, 6, 9) == 6); - test(clamp(9, 6, 9) == 9); - test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4)); - test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1)); - test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7)); - test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2)); - test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5)); - test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6)); - test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9)); - - // Test min3() - test(min3(1, 5, 7) == 1); - test(min3(-4, 2, 4) == -4); - test(min3(-1, -5, -7) == -7); - test(min3(13, 5, 47) == 5); - test(min3(4, 4, 4) == 4); - - // Test max3() - test(max3(1, 5, 7) == 7); - test(max3(-4, 2, 4) == 4); - test(max3(-1, -5, -7) == -1); - test(max3(13, 5, 47) == 47); - test(max3(4, 4, 4) == 4); - - // Test sameSign() - test(sameSign(4, 53)); - test(sameSign(-4, -8)); - test(!sameSign(4, -7)); - test(!sameSign(-4, 53)); - - // Test computeBarycentricCoordinatesInTriangle() - Vector3 a(0, 0, 0); - Vector3 b(5, 0, 0); - Vector3 c(0, 0, 5); - Vector3 testPoint(4, 0, 1); - decimal u,v,w; - computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); - test(approxEqual(u, 1.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 1.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 1.0, 0.000001)); - - computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); - test(approxEqual(u + v + w, 1.0, 0.000001)); - } - - }; - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_MATHEMATICS_FUNCTIONS_H +#define TEST_MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "Test.h" +#include "mathematics/mathematics_functions.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMathematicsFunctions +/** + * Unit test for mathematics functions + */ +class TestMathematicsFunctions : public Test { + + private : + + // ---------- Atributes ---------- // + + + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMathematicsFunctions(const std::string& name): Test(name) {} + + /// Run the tests + void run() { + + // Test approxEqual() + test(approxEqual(2, 7, 5.2)); + test(approxEqual(7, 2, 5.2)); + test(approxEqual(6, 6)); + test(!approxEqual(1, 5)); + test(!approxEqual(1, 5, 3)); + test(approxEqual(-2, -2)); + test(approxEqual(-2, -7, 6)); + test(!approxEqual(-2, 7, 2)); + test(approxEqual(-3, 8, 12)); + test(!approxEqual(-3, 8, 6)); + + // Test clamp() + test(clamp(4, -3, 5) == 4); + test(clamp(-3, 1, 8) == 1); + test(clamp(45, -6, 7) == 7); + test(clamp(-5, -2, -1) == -2); + test(clamp(-5, -9, -1) == -5); + test(clamp(6, 6, 9) == 6); + test(clamp(9, 6, 9) == 9); + test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4)); + test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1)); + test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7)); + test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2)); + test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5)); + test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6)); + test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9)); + + // Test min3() + test(min3(1, 5, 7) == 1); + test(min3(-4, 2, 4) == -4); + test(min3(-1, -5, -7) == -7); + test(min3(13, 5, 47) == 5); + test(min3(4, 4, 4) == 4); + + // Test max3() + test(max3(1, 5, 7) == 7); + test(max3(-4, 2, 4) == 4); + test(max3(-1, -5, -7) == -1); + test(max3(13, 5, 47) == 47); + test(max3(4, 4, 4) == 4); + + // Test sameSign() + test(sameSign(4, 53)); + test(sameSign(-4, -8)); + test(!sameSign(4, -7)); + test(!sameSign(-4, 53)); + + // Test computeBarycentricCoordinatesInTriangle() + Vector3 a(0, 0, 0); + Vector3 b(5, 0, 0); + Vector3 c(0, 0, 5); + Vector3 testPoint(4, 0, 1); + decimal u,v,w; + computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); + test(approxEqual(u, 1.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 1.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 1.0, 0.000001)); + + computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); + test(approxEqual(u + v + w, 1.0, 0.000001)); + + // Test computeClosestPointBetweenTwoSegments() + Vector3 closestSeg1, closestSeg2; + computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 6.0, 0.000001)); + test(approxEqual(closestSeg1.y, 0.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 0.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 4.0, 0.000001)); + test(approxEqual(closestSeg1.y, 6.0, 0.000001)); + test(approxEqual(closestSeg1.z, 5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, -9.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 0.0, 0.000001)); + test(approxEqual(closestSeg1.y, 3.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 6.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 1.0, 0.000001)); + test(approxEqual(closestSeg1.y, 5.0, 0.000001)); + test(approxEqual(closestSeg1.z, -5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 1.0, 0.000001)); + test(approxEqual(closestSeg2.y, 5.0, 0.000001)); + test(approxEqual(closestSeg2.z, -5.0, 0.000001)); + + // Test computePlaneSegmentIntersection(); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001)); + decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0)); + test(tIntersect < 0.0 && tIntersect > 1.0); + + // Test computeDistancePointToLineDistance() + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); + test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); + } + + }; + +} + +#endif diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 8aae0145..3d692501 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -1,415 +1,417 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "CollisionDetectionScene.h" - -// Namespaces -using namespace openglframework; -using namespace collisiondetectionscene; - -// Constructor -CollisionDetectionScene::CollisionDetectionScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), - mContactManager(mPhongShader, mMeshFolderPath), - mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { - - mSelectedShapeIndex = 0; - mIsContactPointsDisplayed = true; - mIsWireframeEnabled = true; - - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 0, 0); - - // Set the center of the scene - setScenePosition(center, SCENE_RADIUS); - - // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); - - // ---------- Sphere 1 ---------- // - openglframework::Vector3 position1(0, 0, 0); - - // Create a sphere and a corresponding collision body in the dynamics world - mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mSphere1); - - // Set the color - mSphere1->setColor(mGreyColorDemo); - mSphere1->setSleepingColor(mRedColorDemo); - - // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(4, 0, 0); - - // Create a sphere and a corresponding collision body in the dynamics world - mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mSphere2); - - // Set the color - mSphere2->setColor(mGreyColorDemo); - mSphere2->setSleepingColor(mRedColorDemo); - - // ---------- Cone ---------- // - //openglframework::Vector3 position4(0, 0, 0); - - // Create a cone and a corresponding collision body in the dynamics world - //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, - // mMeshFolderPath); - - // Set the color - //mCone->setColor(mGreyColorDemo); - //mCone->setSleepingColor(mRedColorDemo); - - // ---------- Cylinder ---------- // - //openglframework::Vector3 position5(0, 0, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, - // mCollisionWorld, mMeshFolderPath); - - // Set the color - //mCylinder->setColor(mGreyColorDemo); - //mCylinder->setSleepingColor(mRedColorDemo); - - // ---------- Capsule ---------- // - //openglframework::Vector3 position6(0, 0, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - //mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , - // mCollisionWorld, mMeshFolderPath); - - // Set the color - //mCapsule->setColor(mGreyColorDemo); - //mCapsule->setSleepingColor(mRedColorDemo); - - // ---------- Convex Mesh ---------- // - //openglframework::Vector3 position7(0, 0, 0); - - // Create a convex mesh and a corresponding collision body in the dynamics world - //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); - - // Set the color - //mConvexMesh->setColor(mGreyColorDemo); - //mConvexMesh->setSleepingColor(mRedColorDemo); - - // ---------- Concave Mesh ---------- // - //openglframework::Vector3 position8(0, 0, 0); - - // Create a convex mesh and a corresponding collision body in the dynamics world - //mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); - - // Set the color - //mConcaveMesh->setColor(mGreyColorDemo); - //mConcaveMesh->setSleepingColor(mRedColorDemo); - - // ---------- Heightfield ---------- // - //openglframework::Vector3 position9(0, 0, 0); - - // Create a convex mesh and a corresponding collision body in the dynamics world - //mHeightField = new HeightField(position9, mCollisionWorld); - - // Set the color - //mHeightField->setColor(mGreyColorDemo); - //mHeightField->setSleepingColor(mRedColorDemo); - - // Create the VBO and VAO to render the lines - createVBOAndVAO(mPhongShader); - - mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); -} - -// Reset the scene -void CollisionDetectionScene::reset() { - -} - -// Destructor -CollisionDetectionScene::~CollisionDetectionScene() { - - // Destroy the shader - mPhongShader.destroy(); - - // Destroy the box rigid body from the dynamics world - //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); - //delete mBox; - - // Destroy the spheres - mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); - delete mSphere1; - - mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); - delete mSphere2; - - /* - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); - delete mCone; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); - - // Destroy the sphere - delete mCylinder; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); - - // Destroy the sphere - delete mCapsule; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConvexMesh; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); - - // Destroy the dumbbell - delete mDumbbell; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConcaveMesh; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); - - // Destroy the convex mesh - delete mHeightField; - */ - - mContactManager.resetPoints(); - - // Destroy the static data for the visual contact points - VisualContactPoint::destroyStaticData(); - - // Destroy the collision world - delete mCollisionWorld; - - // Destroy the VBOs and VAO - mVBOVertices.destroy(); - mVAO.destroy(); -} - -// Update the physics world (take a simulation step) -void CollisionDetectionScene::updatePhysics() { - - -} - -// Take a step for the simulation -void CollisionDetectionScene::update() { - - mContactManager.resetPoints(); - - mCollisionWorld->testCollision(&mContactManager); - - SceneDemo::update(); -} - -// Render the scene -void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - /* - // Bind the VAO - mVAO.bind(); - - // Bind the shader - shader.bind(); - - mVBOVertices.bind(); - - // Set the model to camera matrix - const Matrix4 localToCameraMatrix = Matrix4::identity(); - shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - - // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); - - glDisableVertexAttribArray(vertexPositionLoc); - - mVBOVertices.unbind(); - - // Unbind the VAO - mVAO.unbind(); - - shader.unbind(); - */ - - // Render the shapes - if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - /* - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); - */ - - shader.unbind(); -} - -// Create the Vertex Buffer Objects used to render with OpenGL. -/// We create two VBOs (one for vertices and one for indices) -void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { - - // Bind the shader - shader.bind(); - - // Create the VBO for the vertices data - mVBOVertices.create(); - mVBOVertices.bind(); - size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); - mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); - mVBOVertices.unbind(); - - // Create the VAO for both VBOs - mVAO.create(); - mVAO.bind(); - - // Bind the VBO of vertices - mVBOVertices.bind(); - - // Unbind the VAO - mVAO.unbind(); - - // Unbind the shader - shader.unbind(); -} - -void CollisionDetectionScene::selectNextShape() { - - int previousIndex = mSelectedShapeIndex; - mSelectedShapeIndex++; - if (mSelectedShapeIndex >= mAllShapes.size()) { - mSelectedShapeIndex = 0; - } - - mAllShapes[previousIndex]->setColor(mGreyColorDemo); - mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); -} - -// Called when a keyboard event occurs -bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { - - // If the space key has been pressed - if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { - selectNextShape(); - return true; - } - - float stepDist = 0.5f; - float stepAngle = 20 * (3.14f / 180.0f); - - if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_H && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_A && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_D && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_W && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_S && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_F && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - else if (key == GLFW_KEY_G && action == GLFW_PRESS) { - rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); - mAllShapes[mSelectedShapeIndex]->setTransform(transform); - } - - return false; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CollisionDetectionScene.h" + +// Namespaces +using namespace openglframework; +using namespace collisiondetectionscene; + +// Constructor +CollisionDetectionScene::CollisionDetectionScene(const std::string& name) + : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), + mContactManager(mPhongShader, mMeshFolderPath), + mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { + + mSelectedShapeIndex = 0; + mIsContactPointsDisplayed = true; + mIsWireframeEnabled = true; + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 0, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Create the dynamics world for the physics simulation + mCollisionWorld = new rp3d::CollisionWorld(); + + // ---------- Sphere 1 ---------- // + openglframework::Vector3 position1(0, 0, 0); + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere1); + + // Set the color + mSphere1->setColor(mGreyColorDemo); + mSphere1->setSleepingColor(mRedColorDemo); + + // ---------- Sphere 2 ---------- // + openglframework::Vector3 position2(4, 0, 0); + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere2); + + // Set the color + mSphere2->setColor(mGreyColorDemo); + mSphere2->setSleepingColor(mRedColorDemo); + + // ---------- Capsule 1 ---------- // + openglframework::Vector3 position3(4, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule1); + + // Set the color + mCapsule1->setColor(mGreyColorDemo); + mCapsule1->setSleepingColor(mRedColorDemo); + + // ---------- Cone ---------- // + //openglframework::Vector3 position4(0, 0, 0); + + // Create a cone and a corresponding collision body in the dynamics world + //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, + // mMeshFolderPath); + + // Set the color + //mCone->setColor(mGreyColorDemo); + //mCone->setSleepingColor(mRedColorDemo); + + // ---------- Cylinder ---------- // + //openglframework::Vector3 position5(0, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, + // mCollisionWorld, mMeshFolderPath); + + // Set the color + //mCylinder->setColor(mGreyColorDemo); + //mCylinder->setSleepingColor(mRedColorDemo); + + + // ---------- Convex Mesh ---------- // + //openglframework::Vector3 position7(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + + // Set the color + //mConvexMesh->setColor(mGreyColorDemo); + //mConvexMesh->setSleepingColor(mRedColorDemo); + + // ---------- Concave Mesh ---------- // + //openglframework::Vector3 position8(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + + // Set the color + //mConcaveMesh->setColor(mGreyColorDemo); + //mConcaveMesh->setSleepingColor(mRedColorDemo); + + // ---------- Heightfield ---------- // + //openglframework::Vector3 position9(0, 0, 0); + + // Create a convex mesh and a corresponding collision body in the dynamics world + //mHeightField = new HeightField(position9, mCollisionWorld); + + // Set the color + //mHeightField->setColor(mGreyColorDemo); + //mHeightField->setSleepingColor(mRedColorDemo); + + // Create the VBO and VAO to render the lines + //createVBOAndVAO(mPhongShader); + + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Reset the scene +void CollisionDetectionScene::reset() { + +} + +// Destructor +CollisionDetectionScene::~CollisionDetectionScene() { + + // Destroy the shader + mPhongShader.destroy(); + + // Destroy the box rigid body from the dynamics world + //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + //delete mBox; + + // Destroy the spheres + mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); + delete mSphere1; + + mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); + delete mSphere2; + + /* + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); + delete mCone; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); + + // Destroy the sphere + delete mCylinder; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + + // Destroy the sphere + delete mCapsule; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConvexMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + + // Destroy the dumbbell + delete mDumbbell; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + + // Destroy the convex mesh + delete mConcaveMesh; + + // Destroy the corresponding rigid body from the dynamics world + mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + + // Destroy the convex mesh + delete mHeightField; + */ + + mContactManager.resetPoints(); + + // Destroy the static data for the visual contact points + VisualContactPoint::destroyStaticData(); + + // Destroy the collision world + delete mCollisionWorld; + + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVAO.destroy(); +} + +// Update the physics world (take a simulation step) +void CollisionDetectionScene::updatePhysics() { + + +} + +// Take a step for the simulation +void CollisionDetectionScene::update() { + + mContactManager.resetPoints(); + + mCollisionWorld->testCollision(&mContactManager); + + SceneDemo::update(); +} + +// Render the scene +void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) { + + /* + // Bind the VAO + mVAO.bind(); + + // Bind the shader + shader.bind(); + + mVBOVertices.bind(); + + // Set the model to camera matrix + const Matrix4 localToCameraMatrix = Matrix4::identity(); + shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + // Set the vertex color + openglframework::Vector4 color(1, 0, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + + // Draw the lines + glDrawArrays(GL_LINES, 0, NB_RAYS); + + glDisableVertexAttribArray(vertexPositionLoc); + + mVBOVertices.unbind(); + + // Unbind the VAO + mVAO.unbind(); + + shader.unbind(); + */ + + // Render the shapes + if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + + /* + if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); + if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); + if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); + if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); + if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); + if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); + if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); + */ + + shader.unbind(); +} + +// Create the Vertex Buffer Objects used to render with OpenGL. +/// We create two VBOs (one for vertices and one for indices) +void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { + + // Bind the shader + shader.bind(); + + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); + mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); + mVBOVertices.unbind(); + + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); + + // Bind the VBO of vertices + mVBOVertices.bind(); + + // Unbind the VAO + mVAO.unbind(); + + // Unbind the shader + shader.unbind(); +} + +void CollisionDetectionScene::selectNextShape() { + + int previousIndex = mSelectedShapeIndex; + mSelectedShapeIndex++; + if (mSelectedShapeIndex >= mAllShapes.size()) { + mSelectedShapeIndex = 0; + } + + mAllShapes[previousIndex]->setColor(mGreyColorDemo); + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Called when a keyboard event occurs +bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { + + // If the space key has been pressed + if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { + selectNextShape(); + return true; + } + + float stepDist = 0.5f; + float stepAngle = 20 * (3.14f / 180.0f); + + if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_H && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_A && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_D && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_W && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_S && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_F && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_G && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + + return false; +} diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index d3a7de00..323b9c34 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -1,231 +1,235 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef COLLISION_DETECTION_SCENE_H -#define COLLISION_DETECTION_SCENE_H - -// Libraries -#include -#include "openglframework.h" -#include "reactphysics3d.h" -#include "SceneDemo.h" -#include "Sphere.h" -#include "Box.h" -#include "Capsule.h" -#include "Line.h" -#include "ConvexMesh.h" -#include "ConcaveMesh.h" -#include "HeightField.h" -#include "Dumbbell.h" -#include "VisualContactPoint.h" - -namespace collisiondetectionscene { - -// Constants -const float SCENE_RADIUS = 30.0f; -const openglframework::Vector3 BOX_SIZE(4, 2, 1); -const float SPHERE_RADIUS = 3.0f; -const float CONE_RADIUS = 3.0f; -const float CONE_HEIGHT = 5.0f; -const float CYLINDER_RADIUS = 3.0f; -const float CYLINDER_HEIGHT = 5.0f; -const float CAPSULE_RADIUS = 3.0f; -const float CAPSULE_HEIGHT = 5.0f; -const float DUMBBELL_HEIGHT = 5.0f; -const int NB_RAYS = 100; -const float RAY_LENGTH = 30.0f; -const int NB_BODIES = 9; - -// Raycast manager -class ContactManager : public rp3d::CollisionCallback { - - private: - - /// All the visual contact points - std::vector mContactPoints; - - /// All the normals at contact points - std::vector mNormals; - - /// Contact point mesh folder path - std::string mMeshFolderPath; - - public: - - ContactManager(openglframework::Shader& shader, - const std::string& meshFolderPath) - : mMeshFolderPath(meshFolderPath) { - - } - - /// This method will be called for each reported contact point - virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - - rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1)); - - rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2)); - - // Create a line to display the normal at hit point - rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position1, position1 + normal); - mNormals.push_back(normalLine); - } - - void resetPoints() { - - mContactPoints.clear(); - - // Destroy all the normals - for (std::vector::iterator it = mNormals.begin(); - it != mNormals.end(); ++it) { - delete (*it); - } - mNormals.clear(); - } - - std::vector getContactPoints() const { - return mContactPoints; - } -}; - -// Class CollisionDetectionScene -class CollisionDetectionScene : public SceneDemo { - - private : - - // -------------------- Attributes -------------------- // - - /// Contact point mesh folder path - std::string mMeshFolderPath; - - /// Contact manager - ContactManager mContactManager; - - bool mAreNormalsDisplayed; - - /// All objects on the scene - //Box* mBox; - Sphere* mSphere1; - Sphere* mSphere2; - //Cone* mCone; - //Cylinder* mCylinder; - //Capsule* mCapsule; - //ConvexMesh* mConvexMesh; - //Dumbbell* mDumbbell; - //ConcaveMesh* mConcaveMesh; - //HeightField* mHeightField; - - std::vector mAllShapes; - - int mSelectedShapeIndex; - - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; - - /// All the points to render the lines - std::vector mLinePoints; - - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; - - /// Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); - - /// Select the next shape - void selectNextShape(); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - CollisionDetectionScene(const std::string& name); - - /// Destructor - virtual ~CollisionDetectionScene() override; - - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Take a step for the simulation - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - - /// Reset the scene - virtual void reset() override; - - /// Display or not the surface normals at hit points - void showHideNormals(); - - /// Called when a keyboard event occurs - virtual bool keyboardEvent(int key, int scancode, int action, int mods) override; - - /// Enabled/Disable the shadow mapping - virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; - - /// Display/Hide the contact points - virtual void setIsContactPointsDisplayed(bool display) override; - - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; -}; - -// Display or not the surface normals at hit points -inline void CollisionDetectionScene::showHideNormals() { - mAreNormalsDisplayed = !mAreNormalsDisplayed; -} - -// Enabled/Disable the shadow mapping -inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { - SceneDemo::setIsShadowMappingEnabled(false); -} - -// Display/Hide the contact points -inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { - SceneDemo::setIsContactPointsDisplayed(true); -} - -// Return all the contact points of the scene -inline std::vector CollisionDetectionScene::getContactPoints() const { - return mContactManager.getContactPoints(); -} - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef COLLISION_DETECTION_SCENE_H +#define COLLISION_DETECTION_SCENE_H + +// Libraries +#include +#include "openglframework.h" +#include "reactphysics3d.h" +#include "SceneDemo.h" +#include "Sphere.h" +#include "Box.h" +#include "Cone.h" +#include "Cylinder.h" +#include "Capsule.h" +#include "Line.h" +#include "ConvexMesh.h" +#include "ConcaveMesh.h" +#include "HeightField.h" +#include "Dumbbell.h" +#include "VisualContactPoint.h" + +namespace collisiondetectionscene { + +// Constants +const float SCENE_RADIUS = 30.0f; +const openglframework::Vector3 BOX_SIZE(4, 2, 1); +const float SPHERE_RADIUS = 3.0f; +const float CONE_RADIUS = 3.0f; +const float CONE_HEIGHT = 5.0f; +const float CYLINDER_RADIUS = 3.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 3.0f; +const float CAPSULE_HEIGHT = 5.0f; +const float DUMBBELL_HEIGHT = 5.0f; +const int NB_RAYS = 100; +const float RAY_LENGTH = 30.0f; +const int NB_BODIES = 9; + +// Raycast manager +class ContactManager : public rp3d::CollisionCallback { + + private: + + /// All the visual contact points + std::vector mContactPoints; + + /// All the normals at contact points + std::vector mNormals; + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + public: + + ContactManager(openglframework::Shader& shader, + const std::string& meshFolderPath) + : mMeshFolderPath(meshFolderPath) { + + } + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { + + rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1)); + + rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2)); + + // Create a line to display the normal at hit point + rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position1, position1 + normal); + mNormals.push_back(normalLine); + } + + void resetPoints() { + + mContactPoints.clear(); + + // Destroy all the normals + for (std::vector::iterator it = mNormals.begin(); + it != mNormals.end(); ++it) { + delete (*it); + } + mNormals.clear(); + } + + std::vector getContactPoints() const { + return mContactPoints; + } +}; + +// Class CollisionDetectionScene +class CollisionDetectionScene : public SceneDemo { + + private : + + // -------------------- Attributes -------------------- // + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + /// Contact manager + ContactManager mContactManager; + + bool mAreNormalsDisplayed; + + /// All objects on the scene + //Box* mBox; + Sphere* mSphere1; + Sphere* mSphere2; + Capsule* mCapsule1; + Capsule* mCapsule2; + //Cone* mCone; + //Cylinder* mCylinder; + //Capsule* mCapsule; + //ConvexMesh* mConvexMesh; + //Dumbbell* mDumbbell; + //ConcaveMesh* mConcaveMesh; + //HeightField* mHeightField; + + std::vector mAllShapes; + + int mSelectedShapeIndex; + + /// Collision world used for the physics simulation + rp3d::CollisionWorld* mCollisionWorld; + + /// All the points to render the lines + std::vector mLinePoints; + + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVertices; + + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAO; + + /// Create the Vertex Buffer Objects used to render with OpenGL. + void createVBOAndVAO(openglframework::Shader& shader); + + /// Select the next shape + void selectNextShape(); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CollisionDetectionScene(const std::string& name); + + /// Destructor + virtual ~CollisionDetectionScene() override; + + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics() override; + + /// Take a step for the simulation + virtual void update() override; + + /// Render the scene in a single pass + virtual void renderSinglePass(openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) override; + + /// Reset the scene + virtual void reset() override; + + /// Display or not the surface normals at hit points + void showHideNormals(); + + /// Called when a keyboard event occurs + virtual bool keyboardEvent(int key, int scancode, int action, int mods) override; + + /// Enabled/Disable the shadow mapping + virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; + + /// Display/Hide the contact points + virtual void setIsContactPointsDisplayed(bool display) override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Display or not the surface normals at hit points +inline void CollisionDetectionScene::showHideNormals() { + mAreNormalsDisplayed = !mAreNormalsDisplayed; +} + +// Enabled/Disable the shadow mapping +inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { + SceneDemo::setIsShadowMappingEnabled(false); +} + +// Display/Hide the contact points +inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { + SceneDemo::setIsContactPointsDisplayed(true); +} + +// Return all the contact points of the scene +inline std::vector CollisionDetectionScene::getContactPoints() const { + return mContactManager.getContactPoints(); +} + +} + +#endif From 7a656aedc93e7931b0afca9983696e8f754f7a71 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 13 Feb 2017 22:38:47 +0100 Subject: [PATCH 008/105] Working on HalfEdgeStructure --- src/collision/HalfEdgeStructure.cpp | 42 +++++----- src/collision/HalfEdgeStructure.h | 6 +- src/collision/PolyhedronMesh.h | 2 +- .../narrowphase/DefaultCollisionDispatch.cpp | 11 ++- .../narrowphase/DefaultCollisionDispatch.h | 9 ++- .../narrowphase/GJK/GJKAlgorithm.cpp | 1 + .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 80 +++++++++++++++++++ .../narrowphase/SphereVsCapsuleAlgorithm.h | 71 ++++++++++++++++ src/collision/shapes/CollisionShape.h | 2 +- src/mathematics/mathematics_functions.cpp | 6 +- src/reactphysics3d.h | 1 + .../CollisionDetectionScene.h | 2 - 12 files changed, 197 insertions(+), 36 deletions(-) create mode 100644 src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp create mode 100644 src/collision/narrowphase/SphereVsCapsuleAlgorithm.h diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index 16a79704..dba49c15 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -30,13 +30,14 @@ using namespace reactphysics3d; // Initialize the structure -void HalfEdgeStructure::init(std::vector vertices, std::vector> faces) { +void HalfEdgeStructure::init(std::vector vertices, std::vector> faces) { using edgeKey = std::pair; std::map edges; std::map nextEdges; std::map mapEdgeToStartVertex; + std::map mapEdgeToIndex; // For each vertices for (uint v=0; v vertices, std::vector vertices, std::vector= 1) { - nextEdges.insert(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2); + else if (v >= 1) { + nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2)); } - if (e == (faceVertices.size() - 1)) { - nextEdges.insert(pairV1V2, firstEdgeKey); + if (v == (faceVertices.size() - 1)) { + nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey)); } - edges.insert(pairV1V2, edge); + edges.insert(std::make_pair(pairV1V2, edge)); const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index); - mapEdgeToStartVertex.insert(pairV1V2, v1Index); - mapEdgeToStartVertex.insert(pairV2V1, v2Index); + mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index)); + mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index)); auto itEdge = edges.find(pairV2V1); if (itEdge != edges.end()) { @@ -93,24 +94,25 @@ void HalfEdgeStructure::init(std::vector vertices, std::vectorsecond.twinEdgeIndex = edgeIndex + 1; - itEdge->second.nextEdgeIndex = nextEdges[pairV2V1]; edge.twinEdgeIndex = edgeIndex; - edge.nextEdgeIndex = edges[nextEdges[pairV1V2]].; mVertices[v1Index].edgeIndex = edgeIndex; mVertices[v2Index].edgeIndex = edgeIndex + 1; + mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); + mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); + face.edgeIndex = edgeIndex + 1; } currentFaceEdges.push_back(pairV1V2); } + } - // For each edge of the face - for (uint e=0; e < currentFaceEdges.size(); e++) { - Edge& edge = currentFaceEdges[e]; - edge.nextEdgeIndex = - } + // Set next edges + std::map::iterator it; + for (it = edges.begin(); it != edges.end(); ++it) { + it->second.nextEdgeIndex = mapEdgeToIndex[nextEdges[it->first]]; } } diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 71b549d0..4a3f67ed 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -54,11 +54,11 @@ class HalfEdgeStructure { }; struct Vertex { - const Vector3 point; // Coordinates of the vertex + Vector3 point; // Coordinates of the vertex uint edgeIndex; // Index of one edge emanting from this vertex /// Constructor - Vertex(const Vector3& p) { point = p;} + Vertex(Vector3& p) { point = p;} }; private: @@ -81,7 +81,7 @@ class HalfEdgeStructure { ~HalfEdgeStructure() = default; /// Initialize the structure - void init(std::vector vertices, std::vector> faces); + void init(std::vector vertices, std::vector> faces); /// Return the number of faces uint getNbFaces() const; diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index 8f0e8abf..c7182bd8 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -49,7 +49,7 @@ class PolyhedronMesh { bool mIsFinalized; /// All the vertices - std::vector mVertices; + std::vector mVertices; /// All the indexes of the face vertices std::vector> mFaces; diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 4864dbbe..88d4b9b2 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -33,7 +33,6 @@ using namespace reactphysics3d; // use between two types of collision shapes. NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) { - CollisionShapeType shape1Type = static_cast(type1); CollisionShapeType shape2Type = static_cast(type2); @@ -45,9 +44,13 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { return &mSphereVsSphereAlgorithm; } - // Sphere vs Convex shapes (convex Mesh, BoxShape) algorithm - if (shape1Type == CollisionShapeType::SPHERE && CollisionShape::isConvex(shape2Type)) { - return &mSphereVsConvexMeshAlgorithm; + // Sphere vs Capsule algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::SPHERE) { + return &mSphereVsCapsuleAlgorithm; + } + // Capsule vs Capsule algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) { + return &mCapsuleVsCapsuleAlgorithm; } return nullptr; diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 07a819c0..65e05497 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -31,6 +31,8 @@ #include "ConcaveVsConvexAlgorithm.h" #include "SphereVsSphereAlgorithm.h" #include "SphereVsConvexMeshAlgorithm.h" +#include "SphereVsCapsuleAlgorithm.h" +#include "CapsuleVsCapsuleAlgorithm.h" #include "GJK/GJKAlgorithm.h" namespace reactphysics3d { @@ -51,8 +53,11 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Convex Mesh collision algorithm SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm; - /// GJK Algorithm - GJKAlgorithm mGJKAlgorithm; + /// Sphere vs Capsule collision algorithm + SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; + + /// Capsule vs Capsule collision algorithm + CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; public: diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index cd84f5a0..4ea55ba4 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,6 +26,7 @@ // Libraries #include "GJKAlgorithm.h" #include "constraint/ContactPoint.h" +#include "engine/OverlappingPair.h" #include "configuration.h" #include "engine/Profiler.h" #include diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp new file mode 100644 index 00000000..24cb4e96 --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -0,0 +1,80 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SphereVsCapsuleAlgorithm.h" +#include "collision/shapes/SphereShape.h" +#include "collision/shapes/CapsuleShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { + + // Get the collision shapes + const SphereShape* sphereShape = static_cast(narrowPhaseInfo->collisionShape1); + const CapsuleShape* capsuleShape = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the transform from sphere local-space to capsule local-space + const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse(); + + // Transform the center of the sphere into the local-space of the capsule shape + const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + + // Compute the point on the inner capsule segment that is the closes to center of sphere + const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter); + + // Compute the distance between the sphere center and the closest point on the segment + Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter); + const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare(); + + // Compute the sum of the radius of the sphere and the capsule (virtual sphere) + decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); + + // If the collision shapes overlap + if (sphereSegmentDistanceSquare <= sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) { + + decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); + sphereCenterToSegment /= sphereSegmentDistance; + + const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * sphereCenterToSegment; + + decimal penetrationDepth = sumRadius - sphereSegmentDistance; + + // Create the contact info object + contactPointInfo.init(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal); + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h new file mode 100644 index 00000000..98eaf894 --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a sphere collision shape and a capsule collision shape. + */ +class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~SphereVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsCapsuleAlgorithm(const SphereVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactPointInfo& contactPointInfo) override; +}; + +} + +#endif + diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 8a591614..464c1bd5 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -40,7 +40,7 @@ namespace reactphysics3d { /// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; +enum class CollisionShapeType {TRIANGLE, BOX, CAPSULE, SPHERE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; const int NB_COLLISION_SHAPE_TYPES = 9; // Declarations diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 578cecdd..1cd750ed 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -87,7 +87,7 @@ Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, c /// Compute the closest points between two segments /// This method uses the technique described in the book Real-Time /// collision detection by Christer Ericson. -void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, +void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, const Vector3& seg2PointA, const Vector3& seg2PointB, Vector3& closestPointSeg1, Vector3& closestPointSeg2) { @@ -169,7 +169,7 @@ void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vect // computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such // that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, // there is no intersection between the plane and the segment. -decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { +decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { const decimal parallelEpsilon = decimal(0.0001); decimal t = decimal(-1); @@ -188,7 +188,7 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB } /// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" -decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { +decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { decimal distAB = (linePointB - linePointA).length(); diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index a1bef6dc..3dbbd302 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -50,6 +50,7 @@ #include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/ConcaveMeshShape.h" #include "collision/shapes/HeightFieldShape.h" +#include "collision/PolyhedronMesh.h" #include "collision/shapes/AABB.h" #include "collision/ProxyShape.h" #include "collision/RaycastInfo.h" diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 323b9c34..c2aec0cd 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -33,8 +33,6 @@ #include "SceneDemo.h" #include "Sphere.h" #include "Box.h" -#include "Cone.h" -#include "Cylinder.h" #include "Capsule.h" #include "Line.h" #include "ConvexMesh.h" From 6a01abfae8c70896096420e338779ce88c2de5e3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 20 Feb 2017 17:11:13 +0200 Subject: [PATCH 009/105] Fix issues, work on HalfEdgeStructure and add unit tests --- src/collision/CollisionDetection.cpp | 2 +- src/collision/CollisionDetection.h | 12 - src/collision/HalfEdgeStructure.cpp | 27 +- src/collision/HalfEdgeStructure.h | 10 +- src/mathematics/mathematics_functions.cpp | 6 +- test/main.cpp | 2 + test/tests/collision/TestAABB.h | 2 +- test/tests/collision/TestCollisionWorld.h | 22 +- test/tests/collision/TestHalfEdgeStructure.h | 245 ++++++++++++++++++ test/tests/collision/TestPointInside.h | 2 +- test/tests/collision/TestRaycast.h | 18 +- .../mathematics/TestMathematicsFunctions.h | 6 +- 12 files changed, 286 insertions(+), 68 deletions(-) create mode 100644 test/tests/collision/TestHalfEdgeStructure.h diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 161f6328..96f683d7 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -463,7 +463,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Make sure the shape with the smallest collision shape type comes first const uint shape1TypeIndex = static_cast(shape1->getCollisionShape()->getType()); const uint shape2TypeIndex = static_cast(shape2->getCollisionShape()->getType()); - if (shape2TypeIndex > shape1TypeIndex) { + if (shape1TypeIndex > shape2TypeIndex) { // Swap the two shapes ProxyShape* temp = shape1; diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index c06a6357..9444b319 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -179,12 +179,6 @@ class CollisionDetection { /// Compute the collision detection void computeCollisionDetection(); - // TODO : Remove this method - /// Report collision between two sets of shapes - //void reportCollisionBetweenShapes(CollisionCallback* callback, - // const std::set& shapes1, - // const std::set& shapes2) ; - /// Ray casting method void raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const; @@ -210,12 +204,6 @@ class CollisionDetection { /// Allow the broadphase to notify the collision detection about an overlapping pair. void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); - // TODO : Remove this method - /// Compute the narrow-phase collision detection - //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - // const std::set& shapes1, - // const std::set& shapes2); - /// Return a pointer to the world CollisionWorld* getWorld(); diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index dba49c15..6393acb0 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -38,6 +38,8 @@ void HalfEdgeStructure::init(std::vector vertices, std::vector nextEdges; std::map mapEdgeToStartVertex; std::map mapEdgeToIndex; + std::map mapEdgeIndexToKey; + std::map mapFaceIndexToEdgeKey; // For each vertices for (uint v=0; v vertices, std::vectorsecond); - mEdges.push_back(edge); itEdge->second.twinEdgeIndex = edgeIndex + 1; - edge.twinEdgeIndex = edgeIndex; - mVertices[v1Index].edgeIndex = edgeIndex; - mVertices[v2Index].edgeIndex = edgeIndex + 1; + mapEdgeIndexToKey[edgeIndex] = pairV2V1; + mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2; + + mVertices[v1Index].edgeIndex = edgeIndex + 1; + mVertices[v2Index].edgeIndex = edgeIndex; mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); - face.edgeIndex = edgeIndex + 1; + mEdges.push_back(itEdge->second); + mEdges.push_back(edge); } currentFaceEdges.push_back(pairV1V2); @@ -111,8 +116,12 @@ void HalfEdgeStructure::init(std::vector vertices, std::vector::iterator it; - for (it = edges.begin(); it != edges.end(); ++it) { - it->second.nextEdgeIndex = mapEdgeToIndex[nextEdges[it->first]]; + for (uint i=0; i < mEdges.size(); i++) { + mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]]; + } + + // Set face edge + for (uint f=0; f < faces.size(); f++) { + mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]]; } } diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 4a3f67ed..707c7d83 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -43,7 +43,7 @@ class HalfEdgeStructure { public: struct Edge { - uint vertexIndex; // Index of the vertex at the end of the edge + uint vertexIndex; // Index of the vertex at the beginning of the edge uint twinEdgeIndex; // Index of the twin edge uint faceIndex; // Adjacent face index of the edge uint nextEdgeIndex; // Index of the next edge @@ -54,7 +54,7 @@ class HalfEdgeStructure { }; struct Vertex { - Vector3 point; // Coordinates of the vertex + Vector3 point; // Coordinates of the vertex uint edgeIndex; // Index of one edge emanting from this vertex /// Constructor @@ -86,8 +86,8 @@ class HalfEdgeStructure { /// Return the number of faces uint getNbFaces() const; - /// Return the number of edges - uint getNbEdges() const; + /// Return the number of half-edges + uint getNbHalfEdges() const; /// Return the number of vertices uint getNbVertices() const; @@ -109,7 +109,7 @@ inline uint HalfEdgeStructure::getNbFaces() const { } // Return the number of edges -inline uint HalfEdgeStructure::getNbEdges() const { +inline uint HalfEdgeStructure::getNbHalfEdges() const { return mEdges.size(); } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 1cd750ed..7de898ae 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -171,16 +171,16 @@ void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1Po // there is no intersection between the plane and the segment. decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { - const decimal parallelEpsilon = decimal(0.0001); + const decimal parallelEpsilon = decimal(0.0001); decimal t = decimal(-1); // Segment AB const Vector3 ab = segB - segA; - decimal nDotAB = planeNormal.dot(ab); + decimal nDotAB = planeNormal.dot(ab); // If the segment is not parallel to the plane - if (nDotAB > parallelEpsilon) { + if (std::abs(nDotAB) > parallelEpsilon) { t = (planeD - planeNormal.dot(segA)) / nDotAB; } diff --git a/test/main.cpp b/test/main.cpp index bf949030..54bfbb8f 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -37,6 +37,7 @@ #include "tests/collision/TestCollisionWorld.h" #include "tests/collision/TestAABB.h" #include "tests/collision/TestDynamicAABBTree.h" +#include "tests/collision/TestHalfEdgeStructure.h" using namespace reactphysics3d; @@ -61,6 +62,7 @@ int main() { testSuite.addTest(new TestRaycast("Raycasting")); testSuite.addTest(new TestCollisionWorld("CollisionWorld")); testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); + testSuite.addTest(new TestHalfEdgeStructure("HalfEdgeStructure")); // Run the tests testSuite.run(); diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 32d03f45..4328625c 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -72,7 +72,7 @@ class TestAABB : public Test { } /// Destructor - ~TestAABB() { + virtual ~TestAABB() { } diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index 242ebcf9..678b5f21 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -103,7 +103,6 @@ class TestCollisionWorld : public Test { CollisionBody* mBoxBody; CollisionBody* mSphere1Body; CollisionBody* mSphere2Body; - CollisionBody* mCylinderBody; // Collision shapes BoxShape* mBoxShape; @@ -113,7 +112,6 @@ class TestCollisionWorld : public Test { ProxyShape* mBoxProxyShape; ProxyShape* mSphere1ProxyShape; ProxyShape* mSphere2ProxyShape; - ProxyShape* mCylinderProxyShape; // Collision callback class WorldCollisionCallback mCollisionCallback; @@ -147,16 +145,14 @@ class TestCollisionWorld : public Test { mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3); mCollisionCallback.boxBody = mBoxBody; mCollisionCallback.sphere1Body = mSphere1Body; mCollisionCallback.sphere2Body = mSphere2Body; - mCollisionCallback.cylinderBody = mCylinderBody; } /// Destructor - ~TestCollisionWorld() { + virtual ~TestCollisionWorld() { delete mBoxShape; delete mSphereShape; } @@ -175,17 +171,12 @@ class TestCollisionWorld : public Test { test(!mCollisionCallback.sphere1CollideWithSphere2); test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); mCollisionCallback.reset(); - mWorld->testCollision(mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); test(!mCollisionCallback.sphere1CollideWithSphere2); @@ -195,7 +186,6 @@ class TestCollisionWorld : public Test { test(!mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); test(!mCollisionCallback.sphere1CollideWithSphere2); @@ -213,7 +203,6 @@ class TestCollisionWorld : public Test { test(!mCollisionCallback.sphere1CollideWithSphere2); mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); test(!mCollisionCallback.boxCollideWithSphere1); test(!mCollisionCallback.sphere1CollideWithSphere2); @@ -224,7 +213,6 @@ class TestCollisionWorld : public Test { mCollisionCallback.reset(); mBoxBody->setIsActive(false); - mCylinderBody->setIsActive(false); mSphere1Body->setIsActive(false); mSphere2Body->setIsActive(false); mWorld->testCollision(&mCollisionCallback); @@ -232,17 +220,12 @@ class TestCollisionWorld : public Test { test(!mCollisionCallback.sphere1CollideWithSphere2); test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); mBoxBody->setIsActive(true); - mCylinderBody->setIsActive(true); mSphere1Body->setIsActive(true); mSphere2Body->setIsActive(true); @@ -251,7 +234,6 @@ class TestCollisionWorld : public Test { mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); @@ -269,7 +251,6 @@ class TestCollisionWorld : public Test { mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); mCollisionCallback.reset(); mWorld->testCollision(&mCollisionCallback); @@ -282,7 +263,6 @@ class TestCollisionWorld : public Test { mBoxProxyShape->setCollideWithMaskBits(0xFFFF); mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); - mCylinderProxyShape->setCollideWithMaskBits(0xFFFF); } }; diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h new file mode 100644 index 00000000..871a8ed1 --- /dev/null +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -0,0 +1,245 @@ +#ifndef TEST_HALF_EDGE_STRUCTURE_H +#define TEST_HALF_EDGE_STRUCTURE_H + +// Libraries +#include "reactphysics3d.h" +#include "Test.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + + +// Class TestHalfEdgeStructure +/** + * Unit test for the HalfEdgeStructure class. + */ +class TestHalfEdgeStructure : public Test { + + private : + + // ---------- Atributes ---------- // + + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestHalfEdgeStructure(const std::string& name) : Test(name) { + + } + + /// Destructor + virtual ~TestHalfEdgeStructure() { + + } + + /// Run the tests + void run() { + testCube(); + testTetrahedron(); + } + + void testCube() { + + // Create the half-edge structure for a cube + std::vector vertices; + std::vector> faces; + rp3d::HalfEdgeStructure cubeStructure; + + // Vertices + vertices.push_back(rp3d::Vector3(-0.5, -0.5, 0.5)); + vertices.push_back(rp3d::Vector3(0.5, -0.5, 0.5)); + vertices.push_back(rp3d::Vector3(0.5, 0.5, 0.5)); + vertices.push_back(rp3d::Vector3(-0.5, 0.5, 0.5)); + vertices.push_back(rp3d::Vector3(-0.5, -0.5, -0.5)); + vertices.push_back(rp3d::Vector3(0.5, -0.5, -0.5)); + vertices.push_back(rp3d::Vector3(0.5, 0.5, -0.5)); + vertices.push_back(rp3d::Vector3(-0.5, 0.5, -0.5)); + + // Faces + std::vector face0; + face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); + std::vector face1; + face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); + std::vector face2; + face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6); + std::vector face3; + face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); + std::vector face4; + face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1); + std::vector face5; + face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); + + faces.push_back(face0); + faces.push_back(face1); + faces.push_back(face2); + faces.push_back(face3); + faces.push_back(face4); + faces.push_back(face5); + + cubeStructure.init(vertices, faces); + + // --- Test that the half-edge structure of the cube is valid --- // + + test(cubeStructure.getNbFaces() == 6); + test(cubeStructure.getNbVertices() == 8); + test(cubeStructure.getNbHalfEdges() == 24); + + // Test vertices + test(cubeStructure.getVertex(0).point.x == -0.5); + test(cubeStructure.getVertex(0).point.y == -0.5); + test(cubeStructure.getVertex(0).point.z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(0).edgeIndex).vertexIndex == 0); + + test(cubeStructure.getVertex(1).point.x == 0.5); + test(cubeStructure.getVertex(1).point.y == -0.5); + test(cubeStructure.getVertex(1).point.z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(1).edgeIndex).vertexIndex == 1); + + test(cubeStructure.getVertex(2).point.x == 0.5); + test(cubeStructure.getVertex(2).point.y == 0.5); + test(cubeStructure.getVertex(2).point.z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(2).edgeIndex).vertexIndex == 2); + + test(cubeStructure.getVertex(3).point.x == -0.5); + test(cubeStructure.getVertex(3).point.y == 0.5); + test(cubeStructure.getVertex(3).point.z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(3).edgeIndex).vertexIndex == 3); + + test(cubeStructure.getVertex(4).point.x == -0.5); + test(cubeStructure.getVertex(4).point.y == -0.5); + test(cubeStructure.getVertex(4).point.z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(4).edgeIndex).vertexIndex == 4); + + test(cubeStructure.getVertex(5).point.x == 0.5); + test(cubeStructure.getVertex(5).point.y == -0.5); + test(cubeStructure.getVertex(5).point.z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(5).edgeIndex).vertexIndex == 5); + + test(cubeStructure.getVertex(6).point.x == 0.5); + test(cubeStructure.getVertex(6).point.y == 0.5); + test(cubeStructure.getVertex(6).point.z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(6).edgeIndex).vertexIndex == 6); + + test(cubeStructure.getVertex(7).point.x == -0.5); + test(cubeStructure.getVertex(7).point.y == 0.5); + test(cubeStructure.getVertex(7).point.z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(7).edgeIndex).vertexIndex == 7); + + // Test faces + for (uint f=0; f<6; f++) { + test(cubeStructure.getHalfEdge(cubeStructure.getFace(f).edgeIndex).faceIndex == f); + } + + // Test edges + for (uint f=0; f<6; f++) { + + + uint edgeIndex = cubeStructure.getFace(f).edgeIndex; + const uint firstEdgeIndex = edgeIndex; + + // For each half-edge of the face + for (uint e=0; e<4; e++) { + + rp3d::HalfEdgeStructure::Edge edge = cubeStructure.getHalfEdge(edgeIndex); + + test(cubeStructure.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex); + test(edge.faceIndex == f); + + // Go to the next edge + edgeIndex = edge.nextEdgeIndex; + } + + test(firstEdgeIndex == edgeIndex); + } + } + + void testTetrahedron() { + + // Create the half-edge structure for a tetrahedron + std::vector vertices; + std::vector> faces; + rp3d::HalfEdgeStructure tetrahedron; + + // Vertices + vertices.push_back(rp3d::Vector3(1, -1, -1)); + vertices.push_back(rp3d::Vector3(-1, -1, -1)); + vertices.push_back(rp3d::Vector3(0, -1, 1)); + vertices.push_back(rp3d::Vector3(0, 1, 0)); + + // Faces + std::vector face0; + face0.push_back(0); face0.push_back(1); face0.push_back(2); + std::vector face1; + face1.push_back(0); face1.push_back(3); face1.push_back(1); + std::vector face2; + face2.push_back(1); face2.push_back(3); face2.push_back(2); + std::vector face3; + face3.push_back(0); face3.push_back(2); face3.push_back(3); + + faces.push_back(face0); + faces.push_back(face1); + faces.push_back(face2); + faces.push_back(face3); + + tetrahedron.init(vertices, faces); + + // --- Test that the half-edge structure of the tetrahedron is valid --- // + + test(tetrahedron.getNbFaces() == 4); + test(tetrahedron.getNbVertices() == 4); + test(tetrahedron.getNbHalfEdges() == 12); + + // Test vertices + test(tetrahedron.getVertex(0).point.x == 1); + test(tetrahedron.getVertex(0).point.y == -1); + test(tetrahedron.getVertex(0).point.z == -1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(0).edgeIndex).vertexIndex == 0); + + test(tetrahedron.getVertex(1).point.x == -1); + test(tetrahedron.getVertex(1).point.y == -1); + test(tetrahedron.getVertex(1).point.z == -1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(1).edgeIndex).vertexIndex == 1); + + test(tetrahedron.getVertex(2).point.x == 0); + test(tetrahedron.getVertex(2).point.y == -1); + test(tetrahedron.getVertex(2).point.z == 1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(2).edgeIndex).vertexIndex == 2); + + test(tetrahedron.getVertex(3).point.x == 0); + test(tetrahedron.getVertex(3).point.y == 1); + test(tetrahedron.getVertex(3).point.z == 0); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(3).edgeIndex).vertexIndex == 3); + + // Test faces + for (uint f=0; f<4; f++) { + test(tetrahedron.getHalfEdge(tetrahedron.getFace(f).edgeIndex).faceIndex == f); + } + + // Test edges + for (uint f=0; f<4; f++) { + + uint edgeIndex = tetrahedron.getFace(f).edgeIndex; + const uint firstEdgeIndex = edgeIndex; + + // For each half-edge of the face + for (uint e=0; e<3; e++) { + + rp3d::HalfEdgeStructure::Edge edge = tetrahedron.getHalfEdge(edgeIndex); + + test(tetrahedron.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex); + test(edge.faceIndex == f); + + // Go to the next edge + edgeIndex = edge.nextEdgeIndex; + } + + test(firstEdgeIndex == edgeIndex); + } + } + }; + +} + +#endif diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index dd2a793b..9fd20110 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -171,7 +171,7 @@ class TestPointInside : public Test { } /// Destructor - ~TestPointInside() { + virtual ~TestPointInside() { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index b39b751b..852455c2 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -109,7 +109,6 @@ class TestRaycast : public Test { CollisionBody* mBoxBody; CollisionBody* mSphereBody; CollisionBody* mCapsuleBody; - CollisionBody* mConeBody; CollisionBody* mConvexMeshBody; CollisionBody* mConvexMeshBodyEdgesInfo; CollisionBody* mCylinderBody; @@ -138,12 +137,10 @@ class TestRaycast : public Test { ProxyShape* mBoxProxyShape; ProxyShape* mSphereProxyShape; ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; ProxyShape* mConvexMeshProxyShape; ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; ProxyShape* mCompoundSphereProxyShape; - ProxyShape* mCompoundCylinderProxyShape; + ProxyShape* mCompoundCapsuleProxyShape; ProxyShape* mTriangleProxyShape; ProxyShape* mConcaveMeshProxyShape; ProxyShape* mHeightFieldProxyShape; @@ -177,7 +174,6 @@ class TestRaycast : public Test { mBoxBody = mWorld->createCollisionBody(mBodyTransform); mSphereBody = mWorld->createCollisionBody(mBodyTransform); mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); - mConeBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); @@ -253,7 +249,7 @@ class TestRaycast : public Test { Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); + mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); // Concave Mesh shape @@ -284,7 +280,7 @@ class TestRaycast : public Test { new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), 12, &(mConcaveMeshIndices[0]), sizeof(uint), vertexType, - TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); // Add the triangle vertex array of the subpart to the triangle mesh @@ -302,19 +298,17 @@ class TestRaycast : public Test { mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); - mConeProxyShape->setCollisionCategoryBits(CATEGORY2); mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); - mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); + mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2); mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); mConcaveMeshProxyShape->setCollisionCategoryBits(CATEGORY2); mHeightFieldProxyShape->setCollisionCategoryBits(CATEGORY2); } /// Destructor - ~TestRaycast() { + virtual ~TestRaycast() { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; @@ -1647,7 +1641,7 @@ class TestRaycast : public Test { Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - mCallback.shapeToTest = mCompoundCylinderProxyShape; + mCallback.shapeToTest = mCompoundCapsuleProxyShape; test(mCompoundBody->raycast(ray11, raycastInfo)); mCallback.reset(); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 32e19aad..42e1065c 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -150,19 +150,19 @@ class TestMathematicsFunctions : public Test { test(approxEqual(closestSeg2.z, 0.0, 0.000001)); computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2); test(approxEqual(closestSeg1.x, 1.0, 0.000001)); - test(approxEqual(closestSeg1.y, 5.0, 0.000001)); + test(approxEqual(closestSeg1.y, 4.0, 0.000001)); test(approxEqual(closestSeg1.z, -5.0, 0.000001)); test(approxEqual(closestSeg2.x, 1.0, 0.000001)); test(approxEqual(closestSeg2.y, 5.0, 0.000001)); test(approxEqual(closestSeg2.z, -5.0, 0.000001)); // Test computePlaneSegmentIntersection(); - test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001)); test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001)); test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001)); test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001)); decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0)); - test(tIntersect < 0.0 && tIntersect > 1.0); + test(tIntersect < 0.0 || tIntersect > 1.0); // Test computeDistancePointToLineDistance() test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); From b21a6bb59b104e54364ddf6a004484a1c5824f25 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 26 Feb 2017 13:48:50 +0200 Subject: [PATCH 010/105] Refactor contact manifold and contact point creation --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 216 ++------------- src/collision/ContactManifold.cpp | 254 ++++-------------- src/collision/ContactManifold.h | 27 +- src/collision/ContactManifoldInfo.h | 130 +++++++++ src/collision/ContactManifoldSet.cpp | 217 +++++++++------ src/collision/ContactManifoldSet.h | 17 +- src/collision/ContactPointInfo.h | 88 ++++++ .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 25 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../narrowphase/DefaultCollisionDispatch.cpp | 2 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 6 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +- .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 2 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 11 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexMeshAlgorithm.cpp | 6 +- .../narrowphase/SphereVsConvexMeshAlgorithm.h | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 11 +- .../narrowphase/SphereVsSphereAlgorithm.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/constraint/ContactPoint.cpp | 32 ++- src/constraint/ContactPoint.h | 86 ++---- src/engine/CollisionWorld.h | 6 +- src/engine/EventListener.h | 6 +- src/engine/OverlappingPair.h | 16 +- 28 files changed, 543 insertions(+), 641 deletions(-) create mode 100644 src/collision/ContactManifoldInfo.h create mode 100644 src/collision/ContactPointInfo.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b8febe..58ea37d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,8 @@ SET (REACTPHYSICS3D_SOURCES "src/body/CollisionBody.cpp" "src/body/RigidBody.h" "src/body/RigidBody.cpp" + "src/collision/ContactPointInfo.h" + "src/collision/ContactManifoldInfo.h" "src/collision/broadphase/BroadPhaseAlgorithm.h" "src/collision/broadphase/BroadPhaseAlgorithm.cpp" "src/collision/broadphase/DynamicAABBTree.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 96f683d7..4a0ef718 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -72,66 +72,6 @@ void CollisionDetection::computeCollisionDetection() { mNarrowPhaseInfoList = nullptr; } -// TODO : Remove this method -// Report collision between two sets of shapes -/* -void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - - OverlappingPair* pair = it->second; - - const ProxyShape* shape1 = pair->getShape1(); - const ProxyShape* shape2 = pair->getShape2(); - - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - continue; - } - - // For each contact manifold set of the overlapping pair - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int j=0; jgetNbContactPoints(); i++) { - - ContactPoint* contactPoint = manifold->getContactPoint(i); - - // Create the contact info object for the contact - ContactPointInfo contactInfo; - contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(), - contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2()); - - // Notify the collision callback about this new contact - if (callback != nullptr) callback->notifyContact(contactInfo); - } - } - } -} -*/ - // Compute the broad-phase collision detection void CollisionDetection::computeBroadPhase() { @@ -191,9 +131,6 @@ void CollisionDetection::computeMiddlePhase() { CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body2 = shape2->getBody(); - // Update the contact cache of the overlapping pair - pair->update(); - // Check that at least one body is awake and not static bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; @@ -308,25 +245,21 @@ void CollisionDetection::computeNarrowPhase() { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) { + ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator); + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) { + + // Reduce the number of points in the contact manifold + contactManifoldInfo.reduce(); // If it is the first contact since the pairs are overlapping if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { // Trigger a callback event - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo); + if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo); } - // Create a new contact - ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(contactPointInfo); - - contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform, - currentNarrowPhaseInfo->shape2ToWorldTransform); - - // Add the contact to the contact manifold set of the corresponding overlapping pair - currentNarrowPhaseInfo->overlappingPair->addContact(contact); + // Add the contact manifold to the corresponding overlapping pair + currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo); // Add the overlapping pair into the set of pairs in contact during narrow-phase overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), @@ -334,7 +267,7 @@ void CollisionDetection::computeNarrowPhase() { mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; // Trigger a callback event for the new contact - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo); + if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo); } } @@ -345,111 +278,6 @@ void CollisionDetection::computeNarrowPhase() { addAllContactManifoldsToBodies(); } -// TODO : Remove this method -// Compute the narrow-phase collision detection -/* -void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - - mContactOverlappingPairs.clear(); - - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - - OverlappingPair* pair = it->second; - - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - ++it; - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } - - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the - // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - std::map::iterator itToRemove = it; - ++it; - - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); - continue; - } - else { - ++it; - } - - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); - - // Update the contact cache of the overlapping pair - pair->update(); - - // Check if the two bodies are allowed to collide, otherwise, we do not test for collision - if (body1->getType() != BodyType::DYNAMIC && body2->getType() != BodyType::DYNAMIC) continue; - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - - // Check if the two bodies are sleeping, if so, we do no test collision between them - if (body1->isSleeping() && body2->isSleeping()) continue; - - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; - - // If there is no collision algorithm between those two kinds of shapes - if (narrowPhaseAlgorithm == nullptr) continue; - - // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); - - TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); - } - - // Add all the contact manifolds (between colliding bodies) to the bodies - addAllContactManifoldsToBodies(); -} -*/ - // Allow the broadphase to notify the collision detection about an overlapping pair. /// This method is called by the broad-phase collision detection algorithm void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { @@ -698,8 +526,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); } } @@ -786,8 +614,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); } } @@ -859,10 +687,10 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2, + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2, body1ProxyShape, body2ProxyShape); // Report the contact to the user @@ -937,10 +765,10 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body, + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body, proxyShape->getBody(), bodyProxyShape, proxyShape); @@ -1008,10 +836,10 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(), + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(), shape2->getBody(), shape1, shape2); // Report the contact to the user diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index dc40774b..36103642 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -30,13 +30,30 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, +ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, PoolAllocator& memoryAllocator, short normalDirectionId) : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), mMemoryAllocator(memoryAllocator) { + // For each contact point info in the manifold + const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo(); + while(pointInfo != nullptr) { + + // Create the new contact point + ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) + ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + + // Add the new contact point into the manifold + mContactPoints[mNbContactPoints] = contact; + mNbContactPoints++; + + pointInfo = pointInfo->next; + } + + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(mNbContactPoints > 0); } // Destructor @@ -44,209 +61,6 @@ ContactManifold::~ContactManifold() { clear(); } -// Add a contact point in the manifold -void ContactManifold::addContactPoint(ContactPoint* contact) { - - // For contact already in the manifold - for (uint i=0; igetWorldPointOnBody1() - - contact->getWorldPointOnBody1()).lengthSquare(); - if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { - - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); - - assert(mNbContactPoints > 0); - - return; - } - } - - // If the contact manifold is full - if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { - int indexMaxPenetration = getIndexOfDeepestPenetration(contact); - int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContactPoint(indexToRemove); - } - - // Add the new contact point in the manifold - mContactPoints[mNbContactPoints] = contact; - mNbContactPoints++; - - assert(mNbContactPoints > 0); -} - -// Remove a contact point from the manifold -void ContactManifold::removeContactPoint(uint index) { - assert(index < mNbContactPoints); - assert(mNbContactPoints > 0); - - // Call the destructor explicitly and tell the memory allocator that - // the corresponding memory block is now free - mContactPoints[index]->~ContactPoint(); - mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); - - // If we don't remove the last index - if (index < mNbContactPoints - 1) { - mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; - } - - mNbContactPoints--; -} - -// Update the contact manifold -/// First the world space coordinates of the current contacts in the manifold are recomputed from -/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts -/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also -/// the contacts with a too large distance between the contact points in the plane orthogonal to the -/// contact normal. -void ContactManifold::update(const Transform& transform1, const Transform& transform2) { - - if (mNbContactPoints == 0) return; - - // Update the world coordinates and penetration depth of the contact points in the manifold - for (uint i=0; iupdateWorldContactPoints(transform1, transform2); - mContactPoints[i]->updatePenetrationDepth(); - } - - const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * - PERSISTENT_CONTACT_DIST_THRESHOLD; - - // Remove the contact points that don't represent very well the contact manifold - for (int i=static_cast(mNbContactPoints)-1; i>=0; i--) { - assert(i < static_cast(mNbContactPoints)); - - // Compute the distance between contact points in the normal direction - decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); - - // If the contacts points are too far from each other in the normal direction - if (distanceNormal > squarePersistentContactThreshold) { - removeContactPoint(i); - } - else { - // Compute the distance of the two contact points in the plane - // orthogonal to the contact normal - Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + - mContactPoints[i]->getNormal() * distanceNormal; - Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - - // If the orthogonal distance is larger than the valid distance - // threshold, we remove the contact - if (projDifference.lengthSquare() > squarePersistentContactThreshold) { - removeContactPoint(i); - } - } - } -} - -// Return the index of the contact point with the larger penetration depth. -/// This corresponding contact will be kept in the cache. The method returns -1 is -/// the new contact is the deepest. -int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - int indexMaxPenetrationDepth = -1; - decimal maxPenetrationDepth = newContact->getPenetrationDepth(); - - // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); - indexMaxPenetrationDepth = i; - } - } - - // Return the index of largest penetration depth - return indexMaxPenetrationDepth; -} - -// Return the index that will be removed. -/// The index of the contact point with the larger penetration -/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute -/// the different area and we want to keep the contacts with the largest area. The new point is also -/// kept. In order to compute the area of a quadrilateral, we use the formula : -/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that -/// when we compute this area, we do not calculate it exactly but we -/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, -/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library -/// by Erwin Coumans (http://wwww.bulletphysics.org). -int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { - - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - - decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint - decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint - decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint - decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint - - if (indexMaxPenetration != 0) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area0 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 1) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area1 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 2) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area2 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 3) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area3 = crossProduct.lengthSquare(); - } - - // Return the index of the contact to remove - return getMaxArea(area0, area1, area2, area3); -} - -// Return the index of maximum area -int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { - if (area0 < area1) { - if (area1 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area1 < area3) return 3; - else return 1; - } - } - else { - if (area0 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area0 < area3) return 3; - else return 0; - } - } -} - // Clear the contact manifold void ContactManifold::clear() { for (uint i=0; igetLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + + // Add the new contact point into the manifold + mContactPoints[mNbContactPoints] = contactPoint; + mNbContactPoints++; + +} + +// Remove a contact point +void ContactManifold::removeContactPoint(int index) { + + assert(mNbContactPoints > 0); + assert(index >= 0 && index < mNbContactPoints); + + // Delete the contact + mContactPoints[index]->~ContactPoint(); + mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); + + for (int i=index; (i+1) < mNbContactPoints; i++) { + mContactPoints[i] = mContactPoints[i+1]; + } + + mNbContactPoints--; +} diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index c36c1476..da323c0f 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -31,6 +31,7 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" #include "memory/PoolAllocator.h" /// ReactPhysics3D namespace @@ -130,18 +131,6 @@ class ContactManifold { // -------------------- Methods -------------------- // - /// Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; - - /// Return the index of the contact with the larger penetration depth. - int getIndexOfDeepestPenetration(ContactPoint* newContact) const; - - /// Return the index that will be removed. - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; - - /// Remove a contact point from the manifold - void removeContactPoint(uint index); - /// Return true if the contact manifold has already been added into an island bool isAlreadyInIsland() const; @@ -150,7 +139,7 @@ class ContactManifold { // -------------------- Methods -------------------- // /// Constructor - ContactManifold(ProxyShape* shape1, ProxyShape* shape2, + ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, PoolAllocator& memoryAllocator, short int normalDirectionId); /// Destructor @@ -177,12 +166,6 @@ class ContactManifold { /// Return the normal direction Id short int getNormalDirectionId() const; - /// Add a contact point to the manifold - void addContactPoint(ContactPoint* contact); - - /// Update the contact manifold. - void update(const Transform& transform1, const Transform& transform2); - /// Clear the contact manifold void clear(); @@ -231,6 +214,12 @@ class ContactManifold { /// Return the largest depth of all the contact points decimal getLargestContactDepth() const; + /// Add a contact point + void addContactPoint(const ContactPointInfo* contactPointInfo); + + /// Remove a contact point + void removeContactPoint(int index); + // -------------------- Friendship -------------------- // friend class DynamicsWorld; diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h new file mode 100644 index 00000000..d4afc590 --- /dev/null +++ b/src/collision/ContactManifoldInfo.h @@ -0,0 +1,130 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H +#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H + +// Libraries +#include "collision/ContactPointInfo.h" +#include "memory/Allocator.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + + +// Class ContactManifoldInfo +/** + * This class is used to collect the list of ContactPointInfo that come + * from a collision test between two shapes. + */ +class ContactManifoldInfo { + + private: + + // -------------------- Attributes -------------------- // + + /// Linked list with all the contact points + ContactPointInfo* mContactPointsList; + + /// Memory allocator used to allocate contact points + Allocator& mAllocator; + + // -------------------- Methods -------------------- // + + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {} + + /// Destructor + ~ContactManifoldInfo() { + + // Delete all the contact points in the linked list + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + ContactPointInfo* elementToDelete = element; + element = element->next; + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + } + + /// Deleted copy-constructor + ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; + + /// Deleted assignment operator + ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; + + /// Add a new contact point into the manifold + void addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + // Create the contact point info + ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo))) + ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + + // Add it into the linked list of contact points + contactPointInfo->next = mContactPointsList; + mContactPointsList = contactPointInfo; + } + + /// Get the first contact point info of the linked list of contact points + ContactPointInfo* getFirstContactPointInfo() const { + return mContactPointsList; + } + + /// Reduce the number of points in the contact manifold + void reduce() { + + // TODO : Implement this (do not forget to deallocate removed points) + } + + /// Return the largest penetration depth among the contact points + decimal getLargestPenetrationDepth() const { + + decimal maxDepth = decimal(0.0); + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + + if (element->penetrationDepth > maxDepth) { + maxDepth = element->penetrationDepth; + } + + element = element->next; + } + + return maxDepth; + } +}; + +} +#endif + diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index a7dc9d2e..c9b0d5bb 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -43,92 +43,147 @@ ContactManifoldSet::~ContactManifoldSet() { clear(); } -// Add a contact point to the manifold set -void ContactManifoldSet::addContactPoint(ContactPoint* contact) { +void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contact->getNormal()); + assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr); // If there is no contact manifold yet if (mNbManifolds == 0) { - createManifold(normalDirectionId); - mManifolds[0]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); + // If the maximum number of manifold is 1 + if (mNbMaxManifolds == 1) { + createManifold(contactManifoldInfo, 0); + } + else { + + // Compute an Id corresponding to the normal direction (using a cubemap) + short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); + + createManifold(contactManifoldInfo, normalDirectionId); + } + } + else { // If there is already at least one contact manifold in the set + + // If the maximum number of manifold is 1 + if (mNbMaxManifolds == 1) { + + // Replace the old manifold with the new one + updateManifoldWithNewOne(0, contactManifoldInfo); + } + else { + + // Compute an Id corresponding to the normal direction (using a cubemap) + short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); + + // Select the manifold with the most similar normal (if exists) + int similarManifoldIndex = 0; + if (mNbMaxManifolds > 1) { + similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); + } + + // If a similar manifold has been found + if (similarManifoldIndex != -1) { + + // Replace the old manifold with the new one + updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo); + } + else { + + // If we have not reach the maximum number of manifolds + if (mNbManifolds < mNbMaxManifolds) { + + // Create the new contact manifold + createManifold(contactManifoldInfo, normalDirectionId); + } + else { + + decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth(); + + // We have reached the maximum number of manifold, we do not + // want to keep the manifold with the smallest penetration detph + int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth); + + // If the manifold with the smallest penetration depth is not the new one, + // we have to keep the new manifold and remove the one with the smallest depth + if (smallestPenDepthManifoldIndex >= 0) { + removeManifold(smallestPenDepthManifoldIndex); + createManifold(contactManifoldInfo, normalDirectionId); + } + } + } + } + } +} + +// Update a previous similar manifold with a new one +void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) { + + // For each contact point of the previous manifold + for (int i=0; igetNbContactPoints(); ) { + + ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i); + + // For each contact point in the new manifold + ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo(); + bool needToRemovePoint = true; + while (newPoint != nullptr) { + + // If the new contact point is similar (very close) to the old contact point + if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) { + + // Replace (update) the old contact point with the new one + contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(), + mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform()); + needToRemovePoint = false; + newPoint->isUsed = true; + break; + } + + newPoint = newPoint->next; + } + + // If no new contact point is similar to the old one + if (needToRemovePoint) { + + // Remove the old contact point + mManifolds[oldManifoldIndex]->removeContactPoint(i); + } + else { + i++; + } + } + + // For each point of the new manifold that have not been used yet (to update + // an existing point in the previous manifold), add it into the previous manifold + const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo(); + while (newPointInfo != nullptr) { + + if (!newPointInfo->isUsed) { + mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo); } - return; - } + newPointInfo = newPointInfo->next; + } +} - // Select the manifold with the most similar normal (if exists) - int similarManifoldIndex = 0; - if (mNbMaxManifolds > 1) { - similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); - } - - // If a similar manifold has been found - if (similarManifoldIndex != -1) { - - // Add the contact point to that similar manifold - mManifolds[similarManifoldIndex]->addContactPoint(contact); - assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0); - - return; - } - - // If the maximum number of manifold has not been reached yet - if (mNbManifolds < mNbMaxManifolds) { - - // Create a new manifold for the contact point - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - for (int i=0; igetNbContactPoints() > 0); - } - - return; - } +// Return the manifold with the smallest contact penetration depth +int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { // The contact point will be in a new contact manifold, we now have too much - // manifolds condidates. We need to remove one. We choose to keep the manifolds - // with the largest contact depth among their points - int smallestDepthIndex = -1; - decimal minDepth = contact->getPenetrationDepth(); + // manifolds condidates. We need to remove one. We choose to remove the manifold + // with the smallest contact depth among their points + int smallestDepthManifoldIndex = -1; + decimal minDepth = initDepth; assert(mNbManifolds == mNbMaxManifolds); for (int i=0; igetLargestContactDepth(); if (depth < minDepth) { minDepth = depth; - smallestDepthIndex = i; + smallestDepthManifoldIndex = i; } } - // If we do not want to keep to new manifold (not created yet) with the - // new contact point - if (smallestDepthIndex == -1) { - - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); - - return; - } - - assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds); - - // Here we need to replace an existing manifold with a new one (that contains - // the new contact point) - removeManifold(smallestDepthIndex); - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); - } - - return; + return smallestDepthManifoldIndex; } // Return the index of the contact manifold with a similar average normal. @@ -154,7 +209,7 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons int faceNo; decimal u, v; - decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); + decimal max = max3(std::fabs(normal.x), std::fabs(normal.y), std::fabs(normal.z)); Vector3 normalScaled = normal / max; if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { @@ -173,8 +228,8 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons v = normalScaled.y; } - int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + int indexU = std::floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + int indexV = std::floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; @@ -182,22 +237,6 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons return faceNo * 200 + indexU * nbSubDivInFace + indexV; } -// Update the contact manifolds -void ContactManifoldSet::update() { - - for (int i=mNbManifolds-1; i>=0; i--) { - - // Update the contact manifold - mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), - mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform()); - - // Remove the contact manifold if has no contact points anymore - if (mManifolds[i]->getNbContactPoints() == 0) { - removeManifold(i); - } - } -} - // Clear the contact manifold set void ContactManifoldSet::clear() { @@ -210,11 +249,11 @@ void ContactManifoldSet::clear() { } // Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(short int normalDirectionId) { +void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) { assert(mNbManifolds < mNbMaxManifolds); mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId); + ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId); mNbManifolds++; } diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 21944e36..1a2bac97 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -69,7 +69,7 @@ class ContactManifoldSet { // -------------------- Methods -------------------- // /// Create a new contact manifold and add it to the set - void createManifold(short normalDirectionId); + void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId); /// Remove a contact manifold from the set void removeManifold(int index); @@ -82,6 +82,12 @@ class ContactManifoldSet { // normal vector into of the of the bucket and returns a unique Id for the bucket short int computeCubemapNormalId(const Vector3& normal) const; + /// Return the manifold with the smallest contact penetration depth + int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; + + /// Update a previous similar manifold with a new one + void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold); + public: // -------------------- Methods -------------------- // @@ -93,18 +99,15 @@ class ContactManifoldSet { /// Destructor ~ContactManifoldSet(); + /// Add a contact manifold in the set + void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); + /// Return the first proxy shape ProxyShape* getShape1() const; /// Return the second proxy shape ProxyShape* getShape2() const; - /// Add a contact point to the manifold set - void addContactPoint(ContactPoint* contact); - - /// Update the contact manifolds - void update(); - /// Clear the contact manifold set void clear(); diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h new file mode 100644 index 00000000..1ff8bfdd --- /dev/null +++ b/src/collision/ContactPointInfo.h @@ -0,0 +1,88 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H +#define REACTPHYSICS3D_CONTACT_POINT_INFO_H + +// Libraries +#include "body/CollisionBody.h" +#include "mathematics/mathematics.h" +#include "configuration.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Structure ContactPointInfo +/** + * This structure contains informations about a collision contact + * computed during the narrow-phase collision detection. Those + * informations are used to compute the contact set for a contact + * between two bodies. + */ +struct ContactPointInfo { + + private: + + // -------------------- Methods -------------------- // + + public: + + // -------------------- Attributes -------------------- // + + /// Normalized normal vector of the collision contact in world space + Vector3 normal; + + /// Penetration depth of the contact + decimal penetrationDepth; + + /// Contact point of body 1 in local space of body 1 + Vector3 localPoint1; + + /// Contact point of body 2 in local space of body 2 + Vector3 localPoint2; + + /// Pointer to the next contact point info + ContactPointInfo* next; + + /// True if the contact point has already been inserted into a manifold + bool isUsed; + + // -------------------- Methods -------------------- // + + /// Constructor + ContactPointInfo(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) + : normal(contactNormal), penetrationDepth(penDepth), + localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) { + + } + + /// Destructor + ~ContactPointInfo() = default; +}; + +} + +#endif diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 2f9f8703..e83d694c 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -30,8 +30,11 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { +bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + const decimal parallelEpsilon = decimal(0.001); // Get the capsule collision shapes @@ -39,7 +42,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); // Get the transform from capsule 1 local-space to capsule 2 local-space - const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse(); + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; // Compute the end-points of the inner segment of the first capsule Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0); @@ -63,7 +66,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); - if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) { + if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) { // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. @@ -102,8 +105,9 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA); Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); - const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); @@ -112,9 +116,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - segmentsDistance; // Create the contact info object - // TODO : Make sure we create two contact points at the same time (same method here) - contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local); - contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + + return true; } } @@ -129,7 +134,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); // If the collision shapes overlap - if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { + if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); closestPointsSeg1ToSeg2 /= closestPointsDistance; @@ -142,7 +147,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - closestPointsDistance; // Create the contact info object - contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); return true; } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 7a46f73a..898d6337 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -62,7 +62,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volume collide virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + ContactManifoldInfo& contactManifoldInfo) override; }; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 88d4b9b2..5fdd485b 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -45,7 +45,7 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t return &mSphereVsSphereAlgorithm; } // Sphere vs Capsule algorithm - if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::SPHERE) { + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) { return &mSphereVsCapsuleAlgorithm; } // Capsule vs Capsule algorithm diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 4ea55ba4..6be90769 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -47,7 +47,7 @@ using namespace reactphysics3d; /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { + ContactManifoldInfo& contactManifoldInfo) { PROFILE("GJKAlgorithm::testCollision()"); @@ -201,8 +201,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro return GJKResult::INTERPENETRATE; } - // Create the contact info object - contactPointInfo.init(normal, penetrationDepth, pA, pB); + // Add a new contact point + contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB); return GJKResult::COLLIDE_IN_MARGIN; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 8ca25e33..f40592c4 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -27,7 +27,8 @@ #define REACTPHYSICS3D_GJK_ALGORITHM_H // Libraries -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" #include "collision/shapes/ConvexShape.h" #include "VoronoiSimplex.h" @@ -87,7 +88,7 @@ class GJKAlgorithm { /// Compute a contact info if the two bounding volumes collide. GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo); + ContactManifoldInfo& contactManifoldInfo); /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index f7f68907..f665181b 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "body/Body.h" -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" #include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" #include "collision/NarrowPhaseInfo.h" @@ -84,7 +84,7 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0; + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0; }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index d74faf09..b46ca386 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { + ContactManifoldInfo& contactManifoldInfo) { } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 30156cc2..80feece1 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -27,7 +27,8 @@ #define REACTPHYSICS3D_SAT_ALGORITHM_H // Libraries -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -59,7 +60,7 @@ class SATAlgorithm { /// Compute a contact info if the two bounding volumes collide. bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo); + ContactManifoldInfo& contactManifoldInfo); }; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 24cb4e96..aace17fd 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -31,14 +31,17 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) { +bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + // Get the collision shapes const SphereShape* sphereShape = static_cast(narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape = static_cast(narrowPhaseInfo->collisionShape2); // Get the transform from sphere local-space to capsule local-space - const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse(); + const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; // Transform the center of the sphere into the local-space of the capsule shape const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); @@ -58,7 +61,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); // If the collision shapes overlap - if (sphereSegmentDistanceSquare <= sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) { + if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) { decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); sphereCenterToSegment /= sphereSegmentDistance; @@ -71,7 +74,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI decimal penetrationDepth = sumRadius - sphereSegmentDistance; // Create the contact info object - contactPointInfo.init(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal); return true; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 98eaf894..784df6f9 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -62,7 +62,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volume collide virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + ContactManifoldInfo& contactManifoldInfo) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp index 65371e4f..06dbc615 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { + ContactManifoldInfo& contactManifoldInfo) { // Get the local-space to world-space transforms const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; @@ -41,7 +41,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -55,7 +55,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - return satAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo); + return satAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); } return false; diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h index 448b8a5d..f0046545 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h @@ -62,7 +62,7 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volume collide virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + ContactManifoldInfo& contactManifoldInfo) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index e80688d5..54e51e52 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -31,8 +31,11 @@ using namespace reactphysics3d; bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { + ContactManifoldInfo& contactManifoldInfo) { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + // Get the sphere collision shapes const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); @@ -49,7 +52,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + if (squaredDistanceBetweenCenters < sumRadius * sumRadius) { Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); Vector3 intersectionOnBody1 = sphereShape1->getRadius() * @@ -59,8 +62,8 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); + contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); return true; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index be10d5a1..b70befd2 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -62,7 +62,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volume collide virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + ContactManifoldInfo& contactManifoldInfo) override; }; } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 464c1bd5..c2db3466 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -40,7 +40,7 @@ namespace reactphysics3d { /// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, BOX, CAPSULE, SPHERE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; +enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; const int NB_COLLISION_SHAPE_TYPES = 9; // Declarations diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 00ed60d7..f420bbff 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -31,13 +31,35 @@ using namespace reactphysics3d; using namespace std; // Constructor -ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) - : mNormal(contactInfo.normal), - mPenetrationDepth(contactInfo.penetrationDepth), - mLocalPointOnBody1(contactInfo.localPoint1), - mLocalPointOnBody2(contactInfo.localPoint2), +ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform) + : mNormal(contactInfo->normal), + mPenetrationDepth(contactInfo->penetrationDepth), + mLocalPointOnBody1(contactInfo->localPoint1), + mLocalPointOnBody2(contactInfo->localPoint2), mIsRestingContact(false) { assert(mPenetrationDepth > decimal(0.0)); + // Compute the world position of the contact points + mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; + mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; +} + +// Update the contact point with a new one that is similar (very close) +/// The idea is to keep the cache impulse (for warm starting the contact solver) +void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform) { + + assert(isSimilarWithContactPoint(contactInfo)); + assert(contactInfo->penetrationDepth > decimal(0.0)); + + mNormal = contactInfo->normal; + mPenetrationDepth = contactInfo->penetrationDepth; + mLocalPointOnBody1 = contactInfo->localPoint1; + mLocalPointOnBody2 = contactInfo->localPoint2; + + // Compute the world position of the contact points + mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; + mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 4028ed8f..ec4c6556 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -29,60 +29,13 @@ // Libraries #include "body/CollisionBody.h" #include "collision/NarrowPhaseInfo.h" +#include "collision/ContactPointInfo.h" #include "configuration.h" #include "mathematics/mathematics.h" -#include "configuration.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Structure ContactPointInfo -/** - * This structure contains informations about a collision contact - * computed during the narrow-phase collision detection. Those - * informations are used to compute the contact set for a contact - * between two bodies. - */ -struct ContactPointInfo { - - private: - - // -------------------- Methods -------------------- // - - public: - - // -------------------- Attributes -------------------- // - - /// Normalized normal vector of the collision contact in world space - Vector3 normal; - - /// Penetration depth of the contact - decimal penetrationDepth; - - /// Contact point of body 1 in local space of body 1 - Vector3 localPoint1; - - /// Contact point of body 2 in local space of body 2 - Vector3 localPoint2; - - // -------------------- Methods -------------------- // - - /// Constructor - ContactPointInfo() = default; - - /// Destructor - ~ContactPointInfo() = default; - - /// Initialize the contact point info - void init(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { - normal = contactNormal; - penetrationDepth = penDepth; - localPoint1 = localPt1; - localPoint2 = localPt2; - } -}; - // Class ContactPoint /** * This class represents a collision contact point between two @@ -95,16 +48,16 @@ class ContactPoint { // -------------------- Attributes -------------------- // /// Normalized normal vector of the contact (from body1 toward body2) in world space - const Vector3 mNormal; + Vector3 mNormal; /// Penetration depth decimal mPenetrationDepth; /// Contact point on body 1 in local space of body 1 - const Vector3 mLocalPointOnBody1; + Vector3 mLocalPointOnBody1; /// Contact point on body 2 in local space of body 2 - const Vector3 mLocalPointOnBody2; + Vector3 mLocalPointOnBody2; /// Contact point on body 1 in world space Vector3 mWorldPointOnBody1; @@ -123,7 +76,8 @@ class ContactPoint { // -------------------- Methods -------------------- // /// Constructor - ContactPoint(const ContactPointInfo& contactInfo); + ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform); /// Destructor ~ContactPoint() = default; @@ -134,11 +88,9 @@ class ContactPoint { /// Deleted assignment operator ContactPoint& operator=(const ContactPoint& contact) = delete; - /// Update the world contact points - void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform); - - /// Update the penetration depth - void updatePenetrationDepth(); + /// Update the contact point with a new one that is similar (very close) + void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform); /// Return the normal vector of the contact Vector3 getNormal() const; @@ -158,6 +110,9 @@ class ContactPoint { /// Return the cached penetration impulse decimal getPenetrationImpulse() const; + /// Return true if the contact point is similar (close enougth) to another given contact point + bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; + /// Set the cached penetration impulse void setPenetrationImpulse(decimal impulse); @@ -174,17 +129,6 @@ class ContactPoint { size_t getSizeInBytes() const; }; -// Update the world contact points -inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) { - mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; - mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; -} - -// Update the penetration depth -inline void ContactPoint::updatePenetrationDepth() { - mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal); -} - // Return the normal vector of the contact inline Vector3 ContactPoint::getNormal() const { return mNormal; @@ -215,6 +159,12 @@ inline decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; } +// Return true if the contact point is similar (close enougth) to another given contact point +inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const { + return (localContactPointBody1->localPoint1 - mLocalPointOnBody1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD); +} + // Set the cached penetration impulse inline void ContactPoint::setPenetrationImpulse(decimal impulse) { mPenetrationImpulse = impulse; diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index dc162716..0e24f0c8 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -247,16 +247,16 @@ class CollisionCallback { struct CollisionCallbackInfo { public: - const ContactPointInfo& contactPoint; + const ContactManifoldInfo& contactManifold; CollisionBody* body1; CollisionBody* body2; const ProxyShape* proxyShape1; const ProxyShape* proxyShape2; // Constructor - CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2, + CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2, const ProxyShape* proxShape1, const ProxyShape* proxShape2) : - contactPoint(point), body1(b1), body2(b2), + contactManifold(manifold), body1(b1), body2(b2), proxyShape1(proxShape1), proxyShape2(proxShape2) { } diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h index 562e8ac7..e8594255 100644 --- a/src/engine/EventListener.h +++ b/src/engine/EventListener.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_EVENT_LISTENER_H // Libraries -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" namespace reactphysics3d { @@ -53,13 +53,13 @@ class EventListener { /** * @param contact Information about the contact */ - virtual void beginContact(const ContactPointInfo& contact) {} + virtual void beginContact(const ContactManifoldInfo& contactManifold) {} /// Called when a new contact point is found between two bodies /** * @param contact Information about the contact */ - virtual void newContact(const ContactPointInfo& contact) {} + virtual void newContact(const ContactManifoldInfo& contactManifold) {} /// Called at the beginning of an internal tick of the simulation step. /// Each time the DynamicsWorld::update() method is called, the physics diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 70ee1c3f..c34b91b7 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -80,11 +80,8 @@ class OverlappingPair { /// Return the pointer to second body ProxyShape* getShape2() const; - /// Add a contact to the contact cache - void addContact(ContactPoint* contact); - - /// Update the contact cache - void update(); + /// Add a contact manifold + void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); /// Return the cached separating axis Vector3 getCachedSeparatingAxis() const; @@ -123,13 +120,8 @@ inline ProxyShape* OverlappingPair::getShape2() const { } // Add a contact to the contact manifold -inline void OverlappingPair::addContact(ContactPoint* contact) { - mContactManifoldSet.addContactPoint(contact); -} - -// Update the contact manifold -inline void OverlappingPair::update() { - mContactManifoldSet.update(); +inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { + mContactManifoldSet.addContactManifold(contactManifoldInfo); } // Return the cached separating axis From 0ecd554f500c4e292560091c04d085004a8c8050 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 27 Feb 2017 19:06:28 +0100 Subject: [PATCH 011/105] Update collision detection scene in testbed application --- .../CollisionDetectionScene.cpp | 18 +++++++++ .../CollisionDetectionScene.h | 38 +++++++++++-------- 2 files changed, 41 insertions(+), 15 deletions(-) diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 3d692501..8c0196b3 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -82,6 +82,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule1->setColor(mGreyColorDemo); mCapsule1->setSleepingColor(mRedColorDemo); + // ---------- Capsule 2 ---------- // + openglframework::Vector3 position4(-4, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule2); + + // Set the color + mCapsule2->setColor(mGreyColorDemo); + mCapsule2->setSleepingColor(mRedColorDemo); + // ---------- Cone ---------- // //openglframework::Vector3 position4(0, 0, 0); @@ -163,6 +174,12 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); delete mSphere2; + mCollisionWorld->destroyCollisionBody(mCapsule1->getCollisionBody()); + delete mCapsule1; + + mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); + delete mCapsule2; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -285,6 +302,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index c2aec0cd..2c33b3ac 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -51,8 +51,8 @@ const float CONE_RADIUS = 3.0f; const float CONE_HEIGHT = 5.0f; const float CYLINDER_RADIUS = 3.0f; const float CYLINDER_HEIGHT = 5.0f; -const float CAPSULE_RADIUS = 3.0f; -const float CAPSULE_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; const float DUMBBELL_HEIGHT = 5.0f; const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; @@ -83,21 +83,29 @@ class ContactManager : public rp3d::CollisionCallback { /// This method will be called for each reported contact point virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1)); + // For each contact point + rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo(); + while (contactPointInfo != nullptr) { - rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2)); + rp3d::Vector3 point1 = contactPointInfo->localPoint1; + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1)); - // Create a line to display the normal at hit point - rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position1, position1 + normal); - mNormals.push_back(normalLine); + + rp3d::Vector3 point2 = contactPointInfo->localPoint2; + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2)); + + // Create a line to display the normal at hit point + rp3d::Vector3 n = contactPointInfo->normal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position1, position1 + normal); + mNormals.push_back(normalLine); + + contactPointInfo = contactPointInfo->next; + } } void resetPoints() { From 050e8b36dcfc921a19007548a932470a29ab59d3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 Mar 2017 19:07:31 +0100 Subject: [PATCH 012/105] Refactor convex mesh shape (create PolyhedronMesh, ConvexPolyhedron classes) --- CMakeLists.txt | 4 + src/collision/HalfEdgeStructure.cpp | 29 +-- src/collision/HalfEdgeStructure.h | 37 +++- src/collision/PolygonVertexArray.cpp | 75 +++++++ src/collision/PolygonVertexArray.h | 188 +++++++++++++++++ src/collision/PolyhedronMesh.cpp | 79 +++++-- src/collision/PolyhedronMesh.h | 39 ++-- .../narrowphase/SAT/SATAlgorithm.cpp | 57 ++++- src/collision/narrowphase/SAT/SATAlgorithm.h | 16 ++ src/collision/shapes/BoxShape.cpp | 2 +- src/collision/shapes/BoxShape.h | 4 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConvexMeshShape.cpp | 198 +++--------------- src/collision/shapes/ConvexMeshShape.h | 110 +--------- src/collision/shapes/ConvexPolyhedron.cpp | 37 ++++ src/collision/shapes/ConvexPolyhedron.h | 68 ++++++ src/reactphysics3d.h | 2 + test/tests/collision/TestHalfEdgeStructure.h | 141 +++++++------ test/tests/collision/TestPointInside.h | 10 +- test/tests/collision/TestRaycast.h | 10 +- testbed/common/ConvexMesh.cpp | 58 +++-- testbed/common/ConvexMesh.h | 6 +- 22 files changed, 751 insertions(+), 421 deletions(-) create mode 100644 src/collision/PolygonVertexArray.cpp create mode 100644 src/collision/PolygonVertexArray.h create mode 100644 src/collision/shapes/ConvexPolyhedron.cpp create mode 100644 src/collision/shapes/ConvexPolyhedron.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 58ea37d7..4f6ab0bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,6 +90,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexShape.cpp" + "src/collision/shapes/ConvexPolyhedron.h" + "src/collision/shapes/ConvexPolyhedron.cpp" "src/collision/shapes/ConcaveShape.h" "src/collision/shapes/ConcaveShape.cpp" "src/collision/shapes/BoxShape.h" @@ -114,6 +116,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/ProxyShape.cpp" "src/collision/TriangleVertexArray.h" "src/collision/TriangleVertexArray.cpp" + "src/collision/PolygonVertexArray.h" + "src/collision/PolygonVertexArray.cpp" "src/collision/TriangleMesh.h" "src/collision/TriangleMesh.cpp" "src/collision/PolyhedronMesh.h" diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index 6393acb0..0f7b8f6e 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -29,8 +29,8 @@ using namespace reactphysics3d; -// Initialize the structure -void HalfEdgeStructure::init(std::vector vertices, std::vector> faces) { +// Initialize the structure (when all vertices and faces have been added) +void HalfEdgeStructure::init() { using edgeKey = std::pair; @@ -41,30 +41,19 @@ void HalfEdgeStructure::init(std::vector vertices, std::vector mapEdgeIndexToKey; std::map mapFaceIndexToEdgeKey; - // For each vertices - for (uint v=0; v& faceVertices = faces[f]; + Face face = mFaces[f]; std::vector currentFaceEdges; edgeKey firstEdgeKey; // For each vertex of the face - for (uint v=0; v < faceVertices.size(); v++) { - uint v1Index = faceVertices[v]; - uint v2Index = faceVertices[v == (faceVertices.size() - 1) ? 0 : v + 1]; + for (uint v=0; v < face.faceVertices.size(); v++) { + uint v1Index = face.faceVertices[v]; + uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1]; const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index); @@ -78,7 +67,7 @@ void HalfEdgeStructure::init(std::vector vertices, std::vector= 1) { nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2)); } - if (v == (faceVertices.size() - 1)) { + if (v == (face.faceVertices.size() - 1)) { nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey)); } edges.insert(std::make_pair(pairV1V2, edge)); @@ -121,7 +110,7 @@ void HalfEdgeStructure::init(std::vector vertices, std::vector faceVertices; // Index of the vertices of the face + + /// Constructor + Face(std::vector vertices) : faceVertices(vertices) {} }; struct Vertex { - Vector3 point; // Coordinates of the vertex + uint vertexPointIndex; // Index of the vertex point in the origin vertex array uint edgeIndex; // Index of one edge emanting from this vertex /// Constructor - Vertex(Vector3& p) { point = p;} + Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { } }; private: @@ -80,8 +84,14 @@ class HalfEdgeStructure { /// Destructor ~HalfEdgeStructure() = default; - /// Initialize the structure - void init(std::vector vertices, std::vector> faces); + /// Initialize the structure (when all vertices and faces have been added) + void init(); + + /// Add a vertex + uint addVertex(uint vertexPointIndex); + + /// Add a face + void addFace(std::vector faceVertices); /// Return the number of faces uint getNbFaces() const; @@ -98,11 +108,26 @@ class HalfEdgeStructure { /// Return a given edge Edge getHalfEdge(uint index) const; - /// Retunr a given vertex + /// Return a given vertex Vertex getVertex(uint index) const; }; +// Add a vertex +inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { + Vertex vertex(vertexPointIndex); + mVertices.push_back(vertex); + return mVertices.size() - 1; +} + +// Add a face +inline void HalfEdgeStructure::addFace(std::vector faceVertices) { + + // Create a new face + Face face(faceVertices); + mFaces.push_back(face); +} + // Return the number of faces inline uint HalfEdgeStructure::getNbFaces() const { return mFaces.size(); diff --git a/src/collision/PolygonVertexArray.cpp b/src/collision/PolygonVertexArray.cpp new file mode 100644 index 00000000..309a0d62 --- /dev/null +++ b/src/collision/PolygonVertexArray.cpp @@ -0,0 +1,75 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "PolygonVertexArray.h" + +using namespace reactphysics3d; + +// Constructor +/// Note that your data will not be copied into the PolygonVertexArray and +/// therefore, you need to make sure that those data are always valid during +/// the lifetime of the PolygonVertexArray. +/** + */ +PolygonVertexArray::PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* indexesStart, int indexesStride, + uint nbFaces, PolygonFace* facesStart, + VertexDataType vertexDataType, IndexDataType indexDataType) { + mNbVertices = nbVertices; + mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStride = verticesStride; + mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStride = indexesStride; + mNbFaces = nbFaces; + mPolygonFacesStart = facesStart; + mVertexDataType = vertexDataType; + mIndexDataType = indexDataType; +} + +// Return the vertex index of a given vertex in a face +uint PolygonVertexArray::getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const { + + assert(faceIndex < mNbFaces); + + // Get the face + PolygonFace* face = getPolygonFace(faceIndex); + + assert(noVertexInFace < face->nbVertices); + + void* vertexIndexPointer = mIndicesStart + (face->indexBase + noVertexInFace) * mIndicesStride; + + if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + return *((uint*)vertexIndexPointer); + } + else if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + return *((unsigned short*)vertexIndexPointer); + } + else { + assert(false); + } + + return 0; +} diff --git a/src/collision/PolygonVertexArray.h b/src/collision/PolygonVertexArray.h new file mode 100644 index 00000000..5ae02e0e --- /dev/null +++ b/src/collision/PolygonVertexArray.h @@ -0,0 +1,188 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H +#define REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H + +// Libraries +#include "configuration.h" +#include + +namespace reactphysics3d { + +// Class PolygonVertexArray +/** + * This class is used to describe the vertices and faces of a polyhedron mesh. + * A PolygonVertexArray represents an array of vertices and polygon faces + * of a polyhedron mesh. When you create a PolygonVertexArray, no data is copied + * into the array. It only stores pointer to the data. The purpose is to allow + * the user to share vertices data between the physics engine and the rendering + * part. Therefore, make sure that the data pointed by a PolygonVertexArray + * remains valid during the PolygonVertexArray life. + */ +class PolygonVertexArray { + + public: + + /// Data type for the vertices in the array + enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + + /// Data type for the indices in the array + enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; + + /// Represent a polygon face of the polyhedron + struct PolygonFace { + + /// Number of vertices in the polygon face + uint nbVertices; + + /// Index of the first vertex of the polygon face + /// inside the array with all vertex indices + uint indexBase; + }; + + protected: + + /// Number of vertices in the array + uint mNbVertices; + + /// Pointer to the first vertex value in the array + unsigned char* mVerticesStart; + + /// Stride (number of bytes) between the beginning of two vertices + /// values in the array + int mVerticesStride; + + /// Pointer to the first vertex index of the array + unsigned char* mIndicesStart; + + /// Stride (number of bytes) between the beginning of two indices in + /// the array + int mIndicesStride; + + /// Number of polygon faces in the array + uint mNbFaces; + + /// Pointer to the first polygon face in the polyhedron + PolygonFace* mPolygonFacesStart; + + /// Data type of the vertices in the array + VertexDataType mVertexDataType; + + /// Data type of the indices in the array + IndexDataType mIndexDataType; + + public: + + /// Constructor + PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* indexesStart, int indexesStride, + uint nbFaces, PolygonFace* facesStart, + VertexDataType vertexDataType, IndexDataType indexDataType); + + /// Destructor + ~PolygonVertexArray() = default; + + /// Return the vertex data type + VertexDataType getVertexDataType() const; + + /// Return the index data type + IndexDataType getIndexDataType() const; + + /// Return the number of vertices + uint getNbVertices() const; + + /// Return the number of faces + uint getNbFaces() const; + + /// Return the vertices stride (number of bytes) + int getVerticesStride() const; + + /// Return the indices stride (number of bytes) + int getIndicesStride() const; + + /// Return the vertex index of a given vertex in a face + uint getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const; + + /// Return a polygon face of the polyhedron + PolygonFace* getPolygonFace(uint faceIndex) const; + + /// Return the pointer to the start of the vertices array + unsigned char* getVerticesStart() const; + + /// Return the pointer to the start of the indices array + unsigned char* getIndicesStart() const; +}; + +// Return the vertex data type +inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const { + return mVertexDataType; +} + +// Return the index data type +inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const { + return mIndexDataType; +} + +// Return the number of vertices +inline uint PolygonVertexArray::getNbVertices() const { + return mNbVertices; +} + +// Return the number of faces +inline uint PolygonVertexArray::getNbFaces() const { + return mNbFaces; +} + +// Return the vertices stride (number of bytes) +inline int PolygonVertexArray::getVerticesStride() const { + return mVerticesStride; +} + +// Return the indices stride (number of bytes) +inline int PolygonVertexArray::getIndicesStride() const { + return mIndicesStride; +} + +// Return a polygon face of the polyhedron +inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const { + assert(faceIndex < mNbFaces); + return &mPolygonFacesStart[faceIndex]; +} + +// Return the pointer to the start of the vertices array +inline unsigned char* PolygonVertexArray::getVerticesStart() const { + return mVerticesStart; +} + +// Return the pointer to the start of the indices array +inline unsigned char* PolygonVertexArray::getIndicesStart() const { + return mIndicesStart; +} + +} + +#endif + diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 891a2f47..df43a47a 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -30,33 +30,70 @@ using namespace reactphysics3d; // Constructor -PolyhedronMesh::PolyhedronMesh() : mIsFinalized(false) { +PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { + mPolygonVertexArray = polygonVertexArray; + + // Create the half-edge structure of the mesh + createHalfEdgeStructure(); } -// Add a vertex into the polyhedron. -// This method returns the index of the vertex that you need to use -// to add faces. -uint PolyhedronMesh::addVertex(const Vector3& vertex) { - mVertices.push_back(vertex); - return mVertices.size() - 1; -} +// Create the half-edge structure of the mesh +void PolyhedronMesh::createHalfEdgeStructure() { -// Add a face into the polyhedron. -// A face is a list of vertices indices (returned by addVertex() method). -// The order of the indices are important. You need to specify the vertices of -// of the face sorted counter-clockwise as seen from the outside of the polyhedron. -void PolyhedronMesh::addFace(std::vector faceVertices) { - mFaces.push_back(faceVertices); -} + // For each vertex of the mesh + for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) { + mHalfEdgeStructure.addVertex(v); + } -// Call this method when you are done adding vertices and faces -void PolyhedronMesh::finalize() { + // For each polygon face of the mesh + for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) { - if (mIsFinalized) return; + // Get the polygon face + PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); + + std::vector faceVertices; + + // For each vertex of the face + for (uint v=0; v < face->nbVertices; v++) { + faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v)); + } + + // Addd the face into the half-edge structure + mHalfEdgeStructure.addFace(faceVertices); + } // Initialize the half-edge structure - mHalfEdgeStructure.init(mVertices, mFaces); - - mIsFinalized = true; + mHalfEdgeStructure.init(); +} + +/// Return a vertex +Vector3 PolyhedronMesh::getVertex(uint index) const { + assert(index < getNbVertices()); + + // Get the vertex index in the array with all vertices + uint vertexIndex = mHalfEdgeStructure.getVertex(index).vertexPointIndex; + + PolygonVertexArray::VertexDataType vertexType = mPolygonVertexArray->getVertexDataType(); + unsigned char* verticesStart = mPolygonVertexArray->getVerticesStart(); + int vertexStride = mPolygonVertexArray->getVerticesStride(); + + Vector3 vertex; + if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { + const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); + vertex.x = decimal(vertices[0]); + vertex.y = decimal(vertices[1]); + vertex.z = decimal(vertices[2]); + } + else if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { + const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); + vertex.x = decimal(vertices[0]); + vertex.y = decimal(vertices[1]); + vertex.z = decimal(vertices[2]); + } + else { + assert(false); + } + + return vertex; } diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index c7182bd8..f842cae1 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -29,6 +29,7 @@ // Libraries #include "mathematics/mathematics.h" #include "HalfEdgeStructure.h" +#include "collision/PolygonVertexArray.h" #include namespace reactphysics3d { @@ -36,45 +37,51 @@ namespace reactphysics3d { // Class PolyhedronMesh /** * This class describes a polyhedron mesh made of faces and vertices. - * The faces do not have to be triangle + * The faces do not have to be triangles. */ class PolyhedronMesh { private: + // -------------------- Attributes -------------------- // + + /// Pointer the the polygon vertex array with vertices and faces + /// of the mesh + PolygonVertexArray* mPolygonVertexArray; + /// Half-edge structure of the mesh HalfEdgeStructure mHalfEdgeStructure; - /// True if the half-edge structure has been generated - bool mIsFinalized; + // -------------------- Methods -------------------- // - /// All the vertices - std::vector mVertices; - - /// All the indexes of the face vertices - std::vector> mFaces; + /// Create the half-edge structure of the mesh + void createHalfEdgeStructure(); public: + // -------------------- Methods -------------------- // + /// Constructor - PolyhedronMesh(); + PolyhedronMesh(PolygonVertexArray* polygonVertexArray); /// Destructor ~PolyhedronMesh() = default; - /// Add a vertex into the polyhedron - uint addVertex(const Vector3& vertex); + /// Return the number of vertices + uint getNbVertices() const; - /// Add a face into the polyhedron - void addFace(std::vector faceVertices); - - /// Call this method when you are done adding vertices and faces - void finalize(); + /// Return a vertex + Vector3 getVertex(uint index) const; /// Return the half-edge structure of the mesh const HalfEdgeStructure& getHalfEdgeStructure() const; }; +// Return the number of vertices +inline uint PolyhedronMesh::getNbVertices() const { + return mHalfEdgeStructure.getNbVertices(); +} + // Return the half-edge structure of the mesh inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index b46ca386..61910acc 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -26,6 +26,7 @@ // Libraries #include "SATAlgorithm.h" #include "constraint/ContactPoint.h" +#include "collision/PolyhedronMesh.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -36,8 +37,60 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + + switch (narrowPhaseInfo->collisionShape1->getType()) { + case CollisionShapeType::CONVEX_POLYHEDRON: + return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); + case CollisionShapeType::SPHERE: + return testCollisionSphereVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); + case CollisionShapeType::CAPSULE: + return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); + case CollisionShapeType::TRIANGLE: + return testCollisionTriangleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); + default: assert(false); + } + + return false; +} + +// Test collision between a sphere and a convex mesh +bool SATAlgorithm::testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { } + +// Test collision between a capsule and a convex mesh +bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + +} + +// Test collision between a triangle and a convex mesh +bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + +} + +// Test collision between two convex meshes +bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + +} + + +// Return true if the arcs AB and CD on the Gauss Map (unit sphere) intersect +/// This is used to know if the edge between faces with normal A and B on first polyhedron +/// and edge between faces with normal C and D on second polygon create a face on the Minkowski +/// sum of both polygons. If this is the case, it means that the cross product of both edges +/// might be a separating axis. +bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, + const Vector3& c, const Vector3& d) const { + const Vector3 bCrossA = b.cross(a); + const Vector3 dCrossC = d.cross(c); + + const decimal cba = c.dot(bCrossA); + const decimal dba = d.dot(bCrossA); + const decimal adc = a.dot(dCrossC); + const decimal bdc = b.dot(dCrossC); + + return cba * dba < decimal(0.0) && adc * bdc < decimal(0.0) && cba * bdc > decimal(0.0); +} diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 80feece1..dc5bc5f2 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -42,6 +42,22 @@ class SATAlgorithm { // -------------------- Methods -------------------- // + /// Return true if the arcs AB and CD on the Gauss Map intersect + bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, + const Vector3& c, const Vector3& d) const; + + /// Test collision between a sphere and a convex mesh + bool testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between a capsule and a convex mesh + bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between a triangle and a convex mesh + bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between two convex meshes + bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + public : // -------------------- Methods -------------------- // diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index ce3197e3..ba5dfa1a 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -38,7 +38,7 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexShape(CollisionShapeType::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { + : ConvexPolyhedron(margin), mExtent(extent - Vector3(margin, margin, margin)) { assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.z > decimal(0.0) && extent.z > margin); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 776356ba..c3d5caf2 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -28,7 +28,7 @@ // Libraries #include -#include "ConvexShape.h" +#include "ConvexPolyhedron.h" #include "body/CollisionBody.h" #include "mathematics/mathematics.h" @@ -50,7 +50,7 @@ namespace reactphysics3d { * constructor of the box shape. Otherwise, it is recommended to use the * default margin distance by not using the "margin" parameter in the constructor. */ -class BoxShape : public ConvexShape { +class BoxShape : public ConvexPolyhedron { protected : diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index c2db3466..28e724fd 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -40,7 +40,7 @@ namespace reactphysics3d { /// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; +enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD}; const int NB_COLLISION_SHAPE_TYPES = 9; // Declarations diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index cc880a25..40382aeb 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -30,6 +30,9 @@ using namespace reactphysics3d; +// TODO : Check in every collision shape that localScalling is used correctly and even with SAT +// algorithm (not only in getLocalSupportPoint***() methods) + // Constructor to initialize with an array of 3D vertices. /// This method creates an internal copy of the input vertices. /** @@ -38,108 +41,13 @@ using namespace reactphysics3d; * @param stride Stride between the beginning of two elements in the vertices array * @param margin Collision margin (in meters) around the collision shape */ -ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) - : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { - assert(nbVertices > 0); - assert(stride > 0); - - const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; - - // Copy all the vertices into the internal array - for (uint i=0; igetVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); - - // For each vertex of the mesh - for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) { - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + v * vertexStride); - - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - mVertices.push_back(vertex); - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + v * vertexStride); - - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - mVertices.push_back(vertex); - } - } - - // If we need to use the edges information of the mesh - if (mIsEdgesInformationUsed) { - - // For each triangle of the mesh - for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { - - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - - uint vertexIndex[3] = {0, 0, 0}; - - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex[k] = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - } - - // Add information about the edges - addEdge(vertexIndex[0], vertexIndex[1]); - addEdge(vertexIndex[0], vertexIndex[2]); - addEdge(vertexIndex[1], vertexIndex[2]); - } - } - - mNbVertices = mVertices.size(); - recalculateBounds(); -} - -// Constructor. -/// If you use this constructor, you will need to set the vertices manually one by one using -/// the addVertex() method. -ConvexMeshShape::ConvexMeshShape(decimal margin) - : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { - -} - // Return a local support point in a given direction without the object margin. /// If the edges information is not used for collision detection, this method will go through /// the whole vertices list and pick up the vertex with the largest dot product in the support @@ -151,78 +59,28 @@ ConvexMeshShape::ConvexMeshShape(decimal margin) Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { - assert(mNbVertices == mVertices.size()); - assert(cachedCollisionData != nullptr); + // TODO : Do we still need to have cachedCollisionData or we can remove it from everywhere ? - // Allocate memory for the cached collision data if not allocated yet - if ((*cachedCollisionData) == nullptr) { - *cachedCollisionData = (int*) malloc(sizeof(int)); - *((int*)(*cachedCollisionData)) = 0; - } + double maxDotProduct = DECIMAL_SMALLEST; + uint indexMaxDotProduct = 0; - // If the edges information is used to speed up the collision detection - if (mIsEdgesInformationUsed) { + // For each vertex of the mesh + for (uint i=0; igetNbVertices(); i++) { - assert(mEdgesAdjacencyList.size() == mNbVertices); + // Compute the dot product of the current vertex + double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); - uint maxVertex = *((int*)(*cachedCollisionData)); - decimal maxDotProduct = direction.dot(mVertices[maxVertex]); - bool isOptimal; - - // Perform hill-climbing (local search) - do { - isOptimal = true; - - assert(mEdgesAdjacencyList.at(maxVertex).size() > 0); - - // For all neighbors of the current vertex - std::set::const_iterator it; - std::set::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin(); - std::set::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end(); - for (it = itBegin; it != itEnd; ++it) { - - // Compute the dot product - decimal dotProduct = direction.dot(mVertices[*it]); - - // If the current vertex is a better vertex (larger dot product) - if (dotProduct > maxDotProduct) { - maxVertex = *it; - maxDotProduct = dotProduct; - isOptimal = false; - } - } - - } while(!isOptimal); - - // Cache the support vertex - *((int*)(*cachedCollisionData)) = maxVertex; - - // Return the support vertex - return mVertices[maxVertex] * mScaling; - } - else { // If the edges information is not used - - double maxDotProduct = DECIMAL_SMALLEST; - uint indexMaxDotProduct = 0; - - // For each vertex of the mesh - for (uint i=0; i maxDotProduct) { - indexMaxDotProduct = i; - maxDotProduct = dotProduct; - } + // If the current dot product is larger than the maximum one + if (dotProduct > maxDotProduct) { + indexMaxDotProduct = i; + maxDotProduct = dotProduct; } - - assert(maxDotProduct >= decimal(0.0)); - - // Return the vertex with the largest dot product in the support direction - return mVertices[indexMaxDotProduct] * mScaling; } + + assert(maxDotProduct >= decimal(0.0)); + + // Return the vertex with the largest dot product in the support direction + return mPolyhedronMesh->getVertex(indexMaxDotProduct) * mScaling; } // Recompute the bounds of the mesh @@ -235,16 +93,16 @@ void ConvexMeshShape::recalculateBounds() { mMaxBounds.setToZero(); // For each vertex of the mesh - for (uint i=0; igetNbVertices(); i++) { - if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x; - if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x; + if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x; + if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x; - if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y; - if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y; + if (mPolyhedronMesh->getVertex(i).y > mMaxBounds.y) mMaxBounds.y = mPolyhedronMesh->getVertex(i).y; + if (mPolyhedronMesh->getVertex(i).y < mMinBounds.y) mMinBounds.y = mPolyhedronMesh->getVertex(i).y; - if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z; - if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z; + if (mPolyhedronMesh->getVertex(i).z > mMaxBounds.z) mMaxBounds.z = mPolyhedronMesh->getVertex(i).z; + if (mPolyhedronMesh->getVertex(i).z < mMinBounds.z) mMinBounds.z = mPolyhedronMesh->getVertex(i).z; } // Apply the local scaling factor diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 66b624aa..c33fd5bd 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -1,4 +1,4 @@ -/******************************************************************************** + /******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2016 Daniel Chappuis * ********************************************************************************* @@ -27,10 +27,11 @@ #define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H // Libraries -#include "ConvexShape.h" +#include "ConvexPolyhedron.h" #include "engine/CollisionWorld.h" #include "mathematics/mathematics.h" #include "collision/TriangleMesh.h" +#include "collision/PolyhedronMesh.h" #include "collision/narrowphase/GJK/GJKAlgorithm.h" #include #include @@ -58,17 +59,14 @@ class CollisionWorld; * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method * in order to use the edges information for collision detection. */ -class ConvexMeshShape : public ConvexShape { +class ConvexMeshShape : public ConvexPolyhedron { protected : // -------------------- Attributes -------------------- // - /// Array with the vertices of the mesh - std::vector mVertices; - - /// Number of vertices in the mesh - uint mNbVertices; + /// Polyhedron structure of the mesh + PolyhedronMesh* mPolyhedronMesh; /// Mesh minimum bounds in the three local x, y and z directions Vector3 mMinBounds; @@ -76,13 +74,6 @@ class ConvexMeshShape : public ConvexShape { /// Mesh maximum bounds in the three local x, y and z directions Vector3 mMaxBounds; - /// True if the shape contains the edges of the convex mesh in order to - /// make the collision detection faster - bool mIsEdgesInformationUsed; - - /// Adjacency list representing the edges of the mesh - std::map > mEdgesAdjacencyList; - // -------------------- Methods -------------------- // /// Recompute the bounds of the mesh @@ -108,16 +99,10 @@ class ConvexMeshShape : public ConvexShape { // -------------------- Methods -------------------- // - /// Constructor to initialize with an array of 3D vertices. - ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, - decimal margin = OBJECT_MARGIN); - - /// Constructor to initialize with a triangle vertex array - ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, - decimal margin = OBJECT_MARGIN); - - /// Constructor. - ConvexMeshShape(decimal margin = OBJECT_MARGIN); + /// Constructor + // TODO : Do we really need to use the margin anymore ? Maybe for raycasting ? If not, remove all the + // comments documentation about margin + ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin = OBJECT_MARGIN); /// Destructor virtual ~ConvexMeshShape() override = default; @@ -134,21 +119,8 @@ class ConvexMeshShape : public ConvexShape { /// Return the local inertia tensor of the collision shape. virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; - /// Add a vertex into the convex mesh - void addVertex(const Vector3& vertex); - - /// Add an edge into the convex mesh by specifying the two vertex indices of the edge. - void addEdge(uint v1, uint v2); - /// Return true if the collision shape is a polyhedron virtual bool isPolyhedron() const override; - - /// Return true if the edges information is used to speed up the collision detection - bool isEdgesInformationUsed() const; - - /// Set the variable to know if the edges information is used to speed up the - /// collision detection - void setIsEdgesInformationUsed(bool isEdgesUsed); }; /// Set the scaling vector of the collision shape @@ -197,68 +169,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima 0.0, 0.0, factor * (xSquare + ySquare)); } -// Add a vertex into the convex mesh -/** - * @param vertex Vertex to be added - */ -inline void ConvexMeshShape::addVertex(const Vector3& vertex) { - - // Add the vertex in to vertices array - mVertices.push_back(vertex); - mNbVertices++; - - // Update the bounds of the mesh - if (vertex.x * mScaling.x > mMaxBounds.x) mMaxBounds.x = vertex.x * mScaling.x; - if (vertex.x * mScaling.x < mMinBounds.x) mMinBounds.x = vertex.x * mScaling.x; - if (vertex.y * mScaling.y > mMaxBounds.y) mMaxBounds.y = vertex.y * mScaling.y; - if (vertex.y * mScaling.y < mMinBounds.y) mMinBounds.y = vertex.y * mScaling.y; - if (vertex.z * mScaling.z > mMaxBounds.z) mMaxBounds.z = vertex.z * mScaling.z; - if (vertex.z * mScaling.z < mMinBounds.z) mMinBounds.z = vertex.z * mScaling.z; -} - -// Add an edge into the convex mesh by specifying the two vertex indices of the edge. -/// Note that the vertex indices start at zero and need to correspond to the order of -/// the vertices in the vertices array in the constructor or the order of the calls -/// of the addVertex() methods that you use to add vertices into the convex mesh. -/** -* @param v1 Index of the first vertex of the edge to add -* @param v2 Index of the second vertex of the edge to add -*/ -inline void ConvexMeshShape::addEdge(uint v1, uint v2) { - - // If the entry for vertex v1 does not exist in the adjacency list - if (mEdgesAdjacencyList.count(v1) == 0) { - mEdgesAdjacencyList.insert(std::make_pair(v1, std::set())); - } - - // If the entry for vertex v2 does not exist in the adjacency list - if (mEdgesAdjacencyList.count(v2) == 0) { - mEdgesAdjacencyList.insert(std::make_pair(v2, std::set())); - } - - // Add the edge in the adjacency list - mEdgesAdjacencyList[v1].insert(v2); - mEdgesAdjacencyList[v2].insert(v1); -} - -// Return true if the edges information is used to speed up the collision detection -/** - * @return True if the edges information is used and false otherwise - */ -inline bool ConvexMeshShape::isEdgesInformationUsed() const { - return mIsEdgesInformationUsed; -} - -// Set the variable to know if the edges information is used to speed up the -// collision detection -/** - * @param isEdgesUsed True if you want to use the edges information to speed up - * the collision detection with the convex mesh shape - */ -inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { - mIsEdgesInformationUsed = isEdgesUsed; -} - // Return true if a point is inside the collision shape inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { diff --git a/src/collision/shapes/ConvexPolyhedron.cpp b/src/collision/shapes/ConvexPolyhedron.cpp new file mode 100644 index 00000000..26964554 --- /dev/null +++ b/src/collision/shapes/ConvexPolyhedron.cpp @@ -0,0 +1,37 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ConvexPolyhedron.h" + + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +ConvexPolyhedron::ConvexPolyhedron(decimal margin) + : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) { + +} diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedron.h new file mode 100644 index 00000000..27bbc0b5 --- /dev/null +++ b/src/collision/shapes/ConvexPolyhedron.h @@ -0,0 +1,68 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_H +#define REACTPHYSICS3D_CONVEX_POLYHEDRON_H + +// Libraries +#include "ConvexShape.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class ConvexPolyhedron +/** + * This abstract class represents a convex polyhedron collision shape associated with a + * body that is used during the narrow-phase collision detection. + */ +class ConvexPolyhedron : public ConvexShape { + + protected : + + // -------------------- Attributes -------------------- // + + // -------------------- Methods -------------------- // + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConvexPolyhedron(decimal margin); + + /// Destructor + virtual ~ConvexPolyhedron() override = default; + + /// Deleted copy-constructor + ConvexPolyhedron(const ConvexPolyhedron& shape) = delete; + + /// Deleted assignment operator + ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete; +}; + +} + +#endif + diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 3dbbd302..f6b8b8bf 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -55,7 +55,9 @@ #include "collision/ProxyShape.h" #include "collision/RaycastInfo.h" #include "collision/TriangleMesh.h" +#include "collision/PolyhedronMesh.h" #include "collision/TriangleVertexArray.h" +#include "collision/PolygonVertexArray.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h index 871a8ed1..4c4a2b39 100644 --- a/test/tests/collision/TestHalfEdgeStructure.h +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -43,19 +43,28 @@ class TestHalfEdgeStructure : public Test { void testCube() { // Create the half-edge structure for a cube - std::vector vertices; - std::vector> faces; rp3d::HalfEdgeStructure cubeStructure; + rp3d::Vector3 vertices[8] = { + rp3d::Vector3(-0.5, -0.5, 0.5), + rp3d::Vector3(0.5, -0.5, 0.5), + rp3d::Vector3(0.5, 0.5, 0.5), + rp3d::Vector3(-0.5, 0.5, 0.5), + rp3d::Vector3(-0.5, -0.5, -0.5), + rp3d::Vector3(0.5, -0.5, -0.5), + rp3d::Vector3(0.5, 0.5, -0.5), + rp3d::Vector3(-0.5, 0.5, -0.5) + }; + // Vertices - vertices.push_back(rp3d::Vector3(-0.5, -0.5, 0.5)); - vertices.push_back(rp3d::Vector3(0.5, -0.5, 0.5)); - vertices.push_back(rp3d::Vector3(0.5, 0.5, 0.5)); - vertices.push_back(rp3d::Vector3(-0.5, 0.5, 0.5)); - vertices.push_back(rp3d::Vector3(-0.5, -0.5, -0.5)); - vertices.push_back(rp3d::Vector3(0.5, -0.5, -0.5)); - vertices.push_back(rp3d::Vector3(0.5, 0.5, -0.5)); - vertices.push_back(rp3d::Vector3(-0.5, 0.5, -0.5)); + cubeStructure.addVertex(0); + cubeStructure.addVertex(1); + cubeStructure.addVertex(2); + cubeStructure.addVertex(3); + cubeStructure.addVertex(4); + cubeStructure.addVertex(5); + cubeStructure.addVertex(6); + cubeStructure.addVertex(7); // Faces std::vector face0; @@ -71,14 +80,14 @@ class TestHalfEdgeStructure : public Test { std::vector face5; face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); - faces.push_back(face0); - faces.push_back(face1); - faces.push_back(face2); - faces.push_back(face3); - faces.push_back(face4); - faces.push_back(face5); + cubeStructure.addFace(face0); + cubeStructure.addFace(face1); + cubeStructure.addFace(face2); + cubeStructure.addFace(face3); + cubeStructure.addFace(face4); + cubeStructure.addFace(face5); - cubeStructure.init(vertices, faces); + cubeStructure.init(); // --- Test that the half-edge structure of the cube is valid --- // @@ -87,44 +96,44 @@ class TestHalfEdgeStructure : public Test { test(cubeStructure.getNbHalfEdges() == 24); // Test vertices - test(cubeStructure.getVertex(0).point.x == -0.5); - test(cubeStructure.getVertex(0).point.y == -0.5); - test(cubeStructure.getVertex(0).point.z == 0.5); + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].z == 0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(0).edgeIndex).vertexIndex == 0); - test(cubeStructure.getVertex(1).point.x == 0.5); - test(cubeStructure.getVertex(1).point.y == -0.5); - test(cubeStructure.getVertex(1).point.z == 0.5); + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].z == 0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(1).edgeIndex).vertexIndex == 1); - test(cubeStructure.getVertex(2).point.x == 0.5); - test(cubeStructure.getVertex(2).point.y == 0.5); - test(cubeStructure.getVertex(2).point.z == 0.5); + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].z == 0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(2).edgeIndex).vertexIndex == 2); - test(cubeStructure.getVertex(3).point.x == -0.5); - test(cubeStructure.getVertex(3).point.y == 0.5); - test(cubeStructure.getVertex(3).point.z == 0.5); + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].z == 0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(3).edgeIndex).vertexIndex == 3); - test(cubeStructure.getVertex(4).point.x == -0.5); - test(cubeStructure.getVertex(4).point.y == -0.5); - test(cubeStructure.getVertex(4).point.z == -0.5); + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].z == -0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(4).edgeIndex).vertexIndex == 4); - test(cubeStructure.getVertex(5).point.x == 0.5); - test(cubeStructure.getVertex(5).point.y == -0.5); - test(cubeStructure.getVertex(5).point.z == -0.5); + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].z == -0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(5).edgeIndex).vertexIndex == 5); - test(cubeStructure.getVertex(6).point.x == 0.5); - test(cubeStructure.getVertex(6).point.y == 0.5); - test(cubeStructure.getVertex(6).point.z == -0.5); + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].z == -0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(6).edgeIndex).vertexIndex == 6); - test(cubeStructure.getVertex(7).point.x == -0.5); - test(cubeStructure.getVertex(7).point.y == 0.5); - test(cubeStructure.getVertex(7).point.z == -0.5); + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].z == -0.5); test(cubeStructure.getHalfEdge(cubeStructure.getVertex(7).edgeIndex).vertexIndex == 7); // Test faces @@ -158,15 +167,21 @@ class TestHalfEdgeStructure : public Test { void testTetrahedron() { // Create the half-edge structure for a tetrahedron - std::vector vertices; std::vector> faces; rp3d::HalfEdgeStructure tetrahedron; // Vertices - vertices.push_back(rp3d::Vector3(1, -1, -1)); - vertices.push_back(rp3d::Vector3(-1, -1, -1)); - vertices.push_back(rp3d::Vector3(0, -1, 1)); - vertices.push_back(rp3d::Vector3(0, 1, 0)); + rp3d::Vector3 vertices[4] = { + rp3d::Vector3(1, -1, -1), + rp3d::Vector3(-1, -1, -1), + rp3d::Vector3(0, -1, 1), + rp3d::Vector3(0, 1, 0) + }; + + tetrahedron.addVertex(0); + tetrahedron.addVertex(1); + tetrahedron.addVertex(2); + tetrahedron.addVertex(3); // Faces std::vector face0; @@ -178,12 +193,12 @@ class TestHalfEdgeStructure : public Test { std::vector face3; face3.push_back(0); face3.push_back(2); face3.push_back(3); - faces.push_back(face0); - faces.push_back(face1); - faces.push_back(face2); - faces.push_back(face3); + tetrahedron.addFace(face0); + tetrahedron.addFace(face1); + tetrahedron.addFace(face2); + tetrahedron.addFace(face3); - tetrahedron.init(vertices, faces); + tetrahedron.init(); // --- Test that the half-edge structure of the tetrahedron is valid --- // @@ -192,24 +207,24 @@ class TestHalfEdgeStructure : public Test { test(tetrahedron.getNbHalfEdges() == 12); // Test vertices - test(tetrahedron.getVertex(0).point.x == 1); - test(tetrahedron.getVertex(0).point.y == -1); - test(tetrahedron.getVertex(0).point.z == -1); + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].x == 1); + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].z == -1); test(tetrahedron.getHalfEdge(tetrahedron.getVertex(0).edgeIndex).vertexIndex == 0); - test(tetrahedron.getVertex(1).point.x == -1); - test(tetrahedron.getVertex(1).point.y == -1); - test(tetrahedron.getVertex(1).point.z == -1); + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].x == -1); + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].z == -1); test(tetrahedron.getHalfEdge(tetrahedron.getVertex(1).edgeIndex).vertexIndex == 1); - test(tetrahedron.getVertex(2).point.x == 0); - test(tetrahedron.getVertex(2).point.y == -1); - test(tetrahedron.getVertex(2).point.z == 1); + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].x == 0); + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].z == 1); test(tetrahedron.getHalfEdge(tetrahedron.getVertex(2).edgeIndex).vertexIndex == 2); - test(tetrahedron.getVertex(3).point.x == 0); - test(tetrahedron.getVertex(3).point.y == 1); - test(tetrahedron.getVertex(3).point.z == 0); + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].x == 0); + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].y == 1); + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].z == 0); test(tetrahedron.getHalfEdge(tetrahedron.getVertex(3).edgeIndex).vertexIndex == 3); // Test faces diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 9fd20110..abb54791 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -124,7 +124,8 @@ class TestPointInside : public Test { mCapsuleShape = new CapsuleShape(2, 10); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) + // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again + /*mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, 4)); @@ -160,6 +161,7 @@ class TestPointInside : public Test { mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( mConvexMeshShapeBodyEdgesInfo, mShapeTransform); + */ // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); @@ -175,8 +177,8 @@ class TestPointInside : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConvexMeshShape; - delete mConvexMeshShapeBodyEdgesInfo; + //delete mConvexMeshShape; + //delete mConvexMeshShapeBodyEdgesInfo; } /// Run the tests @@ -399,6 +401,7 @@ class TestPointInside : public Test { // ----- Tests without using edges information ----- // + /* // Tests with CollisionBody test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); @@ -504,6 +507,7 @@ class TestPointInside : public Test { test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); + */ } /// Test the CollisionBody::testPointInside() method diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 852455c2..fe8926d5 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -206,8 +206,9 @@ class TestRaycast : public Test { mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); + // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again // Box of dimension (2, 3, 4) - mConvexMeshShape = new ConvexMeshShape(0.0); + /*mConvexMeshShape = new ConvexMeshShape(0.0); mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, 4)); @@ -243,6 +244,7 @@ class TestRaycast : public Test { mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( mConvexMeshShapeEdgesInfo, mShapeTransform); + */ // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); @@ -312,8 +314,8 @@ class TestRaycast : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConvexMeshShape; - delete mConvexMeshShapeEdgesInfo; + //delete mConvexMeshShape; + //delete mConvexMeshShapeEdgesInfo; delete mTriangleShape; delete mConcaveMeshShape; delete mHeightFieldShape; @@ -1297,6 +1299,7 @@ class TestRaycast : public Test { /// CollisionWorld::raycast() methods. void testConvexMesh() { + /* // ----- Test feedback data ----- // Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); @@ -1555,6 +1558,7 @@ class TestRaycast : public Test { mCallback.reset(); mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); test(mCallback.isHit); + */ } /// Test the CollisionBody::raycast() and diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index 7842c529..11be5732 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -40,17 +40,36 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = + /*mPolygonVertexArray = new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), getNbFaces(0), &(mIndices[0][0]), sizeof(int), rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);*/ + + // Polygon faces descriptions for the polyhedron + mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)]; + rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f=0; f < getNbFaces(0); f++) { + face->indexBase = f * 3; + face->nbVertices = 3; + face++; + } + + // Create the polygon vertex array + mPolygonVertexArray = + new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), + &(mIndices[0][0]), sizeof(int), + getNbFaces(0), mPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create the polyhedron mesh + mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray); // Create the collision shape for the rigid body (convex mesh shape) and // do not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); @@ -85,16 +104,29 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + // Polygon faces descriptions for the polyhedron + mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)]; + rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f=0; f < getNbFaces(0); f++) { + face->indexBase = f * 3; + face->nbVertices = 3; + face++; + } + + // Create the polygon vertex array + mPolygonVertexArray = + new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), + &(mIndices[0][0]), sizeof(int), + getNbFaces(0), mPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create the polyhedron mesh + mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray); // Create the collision shape for the rigid body (convex mesh shape) and do // not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); @@ -128,7 +160,9 @@ ConvexMesh::~ConvexMesh() { mVBOTextureCoords.destroy(); mVAO.destroy(); - delete mPhysicsTriangleVertexArray; + delete mPolyhedronMesh; + delete mPolygonVertexArray; + delete[] mPolygonFaces; delete mConvexShape; } diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index b6cb90c1..e2ba5836 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -41,7 +41,11 @@ class ConvexMesh : public PhysicsObject { /// Previous transform (for interpolation) rp3d::Transform mPreviousTransform; - rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray; + rp3d::PolygonVertexArray::PolygonFace* mPolygonFaces; + + rp3d::PolygonVertexArray* mPolygonVertexArray; + + rp3d::PolyhedronMesh* mPolyhedronMesh; /// Collision shape rp3d::ConvexMeshShape* mConvexShape; From a9b3afae59f092a75755e6edb767d079303c31bf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Mar 2017 23:07:10 +0200 Subject: [PATCH 013/105] Finish implementing capsule vs capsule narrow-phase algorithm --- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 74 +++++++++---------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index e83d694c..11160b8c 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -66,60 +66,60 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); - if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) { + if (segmentsDistance >= sumRadius) { // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. return false; } - // Compute the planes that goes through the extrem points of the inner segment of capsule 1 - decimal d1 = seg1.dot(capsule1SegA); - decimal d2 = -seg1.dot(capsule1SegB); + // If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping) + // If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case, + // we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules) + if (segmentsDistance > MACHINE_EPSILON) { - // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner - // segment of capsule 1 - decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); - decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + // Compute the planes that goes through the extreme points of the inner segment of capsule 1 + decimal d1 = seg1.dot(capsule1SegA); + decimal d2 = -seg1.dot(capsule1SegB); - bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid) + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); - // Clip the inner segment of capsule 2 - Vector3 clipPointA, clipPointB; - if (t1 >= decimal(0.0)) { + // If the segments were overlapping (the clip segment is valid) + if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - if (t1 > decimal(1.0)) t1 = decimal(1.0); - clipPointA = capsule2SegB - t1 * seg2; - isClipValid = true; - } - if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) { + // Clip the inner segment of capsule 2 + if (t1 > decimal(1.0)) t1 = decimal(1.0); + const Vector3 clipPointA = capsule2SegB - t1 * seg2; + if (t2 > decimal(1.0)) t2 = decimal(1.0); + const Vector3 clipPointB = capsule2SegA + t2 * seg2; - if (t2 > decimal(1.0)) t2 = decimal(1.0); - clipPointB = capsule2SegA + t2 * seg2; - isClipValid = true; - } + // Project point capsule2SegA onto line of innner segment of capsule 1 + const Vector3 seg1Normalized = seg1.getUnit(); + Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; - // If we have a valid clip segment - if (isClipValid) { + // Compute a perpendicular vector from segment 1 to segment 2 + Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); + Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); - Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA); - Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); - Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); - const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); - const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + decimal penetrationDepth = sumRadius - segmentsDistance; - decimal penetrationDepth = sumRadius - segmentsDistance; + // Create the contact info object + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); - // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); - - return true; + return true; + } } } From 951ba3e42c3680f42e8204ca4b3f9a01d1d28c07 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 30 Mar 2017 22:39:06 +0200 Subject: [PATCH 014/105] Working on ConvexPolyhedron class --- src/collision/PolyhedronMesh.cpp | 27 +++++++ src/collision/PolyhedronMesh.h | 17 +++- src/collision/shapes/BoxShape.cpp | 31 ++++++++ src/collision/shapes/BoxShape.h | 100 ++++++++++++++++++++++-- src/collision/shapes/ConvexMeshShape.h | 75 ++++++++++++++++-- src/collision/shapes/ConvexPolyhedron.h | 33 ++++++++ src/collision/shapes/ConvexShape.h | 2 +- 7 files changed, 268 insertions(+), 17 deletions(-) diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index df43a47a..50ef5bab 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -36,6 +36,17 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { // Create the half-edge structure of the mesh createHalfEdgeStructure(); + + // Create the face normals array + mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()]; + + // Compute the faces normals + computeFacesNormals(); +} + +// Destructor +PolyhedronMesh::~PolyhedronMesh() { + delete[] mFacesNormals; } // Create the half-edge structure of the mesh @@ -59,6 +70,8 @@ void PolyhedronMesh::createHalfEdgeStructure() { faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v)); } + assert(faceVertices.size() >= 3); + // Addd the face into the half-edge structure mHalfEdgeStructure.addFace(faceVertices); } @@ -97,3 +110,17 @@ Vector3 PolyhedronMesh::getVertex(uint index) const { return vertex; } + +void PolyhedronMesh::computeFacesNormals() { + + // For each face + for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { + HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f); + + assert(face.faceVertices.size() >= 3); + + const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]); + const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]); + mFacesNormals[f] = vec1.cross(vec2); + } +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index f842cae1..940b2db0 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -52,11 +52,17 @@ class PolyhedronMesh { /// Half-edge structure of the mesh HalfEdgeStructure mHalfEdgeStructure; + /// Array with the face normals + Vector3* mFacesNormals; + // -------------------- Methods -------------------- // /// Create the half-edge structure of the mesh void createHalfEdgeStructure(); + /// Compute the faces normals + void computeFacesNormals(); + public: // -------------------- Methods -------------------- // @@ -65,7 +71,7 @@ class PolyhedronMesh { PolyhedronMesh(PolygonVertexArray* polygonVertexArray); /// Destructor - ~PolyhedronMesh() = default; + ~PolyhedronMesh(); /// Return the number of vertices uint getNbVertices() const; @@ -73,6 +79,9 @@ class PolyhedronMesh { /// Return a vertex Vector3 getVertex(uint index) const; + /// Return a face normal + Vector3 getFaceNormal(uint faceIndex) const; + /// Return the half-edge structure of the mesh const HalfEdgeStructure& getHalfEdgeStructure() const; }; @@ -82,6 +91,12 @@ inline uint PolyhedronMesh::getNbVertices() const { return mHalfEdgeStructure.getNbVertices(); } +// Return a face normal +inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mFacesNormals[faceIndex]; +} + // Return the half-edge structure of the mesh inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index ba5dfa1a..f00210c2 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -42,6 +42,37 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.z > decimal(0.0) && extent.z > margin); + + // Vertices + mHalfEdgeStructure.addVertex(0); + mHalfEdgeStructure.addVertex(1); + mHalfEdgeStructure.addVertex(2); + mHalfEdgeStructure.addVertex(3); + mHalfEdgeStructure.addVertex(4); + mHalfEdgeStructure.addVertex(5); + mHalfEdgeStructure.addVertex(6); + mHalfEdgeStructure.addVertex(7); + + // Faces + std::vector face0; + face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); + std::vector face1; + face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); + std::vector face2; + face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); + std::vector face3; + face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); + std::vector face4; + face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); + std::vector face5; + face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); + + mHalfEdgeStructure.addFace(face0); + mHalfEdgeStructure.addFace(face1); + mHalfEdgeStructure.addFace(face2); + mHalfEdgeStructure.addFace(face3); + mHalfEdgeStructure.addFace(face4); + mHalfEdgeStructure.addFace(face5); } // Return the local inertia tensor of the collision shape diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index c3d5caf2..d78bda25 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -59,6 +59,9 @@ class BoxShape : public ConvexPolyhedron { /// Extent sizes of the box in the x, y and z direction Vector3 mExtent; + /// Half-edge structure of the polyhedron + HalfEdgeStructure mHalfEdgeStructure; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -99,11 +102,32 @@ class BoxShape : public ConvexPolyhedron { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; }; // Return the extents of the box @@ -137,11 +161,6 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { min = -max; } -// Return true if the collision shape is a polyhedron -inline bool BoxShape::isPolyhedron() const { - return true; -} - // Return the number of bytes used by the collision shape inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); @@ -163,6 +182,71 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); } +// Return the number of faces of the polyhedron +inline uint BoxShape::getNbFaces() const { + return 6; +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mHalfEdgeStructure.getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint BoxShape::getNbVertices() const { + return 8; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mHalfEdgeStructure.getVertex(vertexIndex); +} + +// Return the position of a given vertex +inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + + Vector3 extent = getExtent(); + + switch(vertexIndex) { + case 0: return Vector3(-extent.x, -extent.y, extent.z); + case 1: return Vector3(extent.x, -extent.y, extent.z); + case 2: return Vector3(extent.x, extent.y, extent.z); + case 3: return Vector3(-extent.x, extent.y, extent.z); + case 4: return Vector3(-extent.x, -extent.y, -extent.z); + case 5: return Vector3(extent.x, -extent.y, -extent.z); + case 6: return Vector3(extent.x, extent.y, -extent.z); + case 7: return Vector3(-extent.x, extent.y, -extent.z); + } +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + + switch(faceIndex) { + case 0: return Vector3(0, 0, 1); + case 1: return Vector3(1, 0, 0); + case 2: return Vector3(0, 0, -1); + case 3: return Vector3(-1, 0, 0); + case 4: return Vector3(0, -1, 0); + case 5: return Vector3(0, 1, 0); + } +} + +// Return the number of half-edges of the polyhedron +inline uint BoxShape::getNbHalfEdges() const { + return 24; +} + +// Return a given half-edge of the polyhedron +inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mHalfEdgeStructure.getHalfEdge(edgeIndex); +} + } #endif diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index c33fd5bd..cc88d38d 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -119,8 +119,29 @@ class ConvexMeshShape : public ConvexPolyhedron { /// Return the local inertia tensor of the collision shape. virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; }; /// Set the scaling vector of the collision shape @@ -134,11 +155,6 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } -// Return true if the collision shape is a polyhedron -inline bool ConvexMeshShape::isPolyhedron() const { - return true; -} - // Return the local bounds of the shape in x, y and z directions /** * @param min The minimum bounds of the shape in local-space coordinates @@ -178,6 +194,51 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } +// Return the number of faces of the polyhedron +inline uint ConvexMeshShape::getNbFaces() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint ConvexMeshShape::getNbVertices() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices(); +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex); +} + +// Return the number of half-edges of the polyhedron +inline uint ConvexMeshShape::getNbHalfEdges() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges(); +} + +// Return a given half-edge of the polyhedron +inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); +} + +// Return the position of a given vertex +inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getVertex(vertexIndex); +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getFaceNormal(faceIndex); +} + } #endif diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedron.h index 27bbc0b5..2a544077 100644 --- a/src/collision/shapes/ConvexPolyhedron.h +++ b/src/collision/shapes/ConvexPolyhedron.h @@ -28,6 +28,7 @@ // Libraries #include "ConvexShape.h" +#include "collision/HalfEdgeStructure.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -60,8 +61,40 @@ class ConvexPolyhedron : public ConvexShape { /// Deleted assignment operator ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const=0; + + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const=0; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const=0; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const=0; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const=0; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const=0; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0; + + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const override; }; +// Return true if the collision shape is a polyhedron +inline bool ConvexPolyhedron::isPolyhedron() const { + return true; +} + } #endif diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 4b12cae4..9180fefe 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -84,7 +84,7 @@ class ConvexShape : public CollisionShape { friend class EPAAlgorithm; }; -/// Return true if the collision shape is convex, false if it is concave +// Return true if the collision shape is convex, false if it is concave inline bool ConvexShape::isConvex() const { return true; } From 57da79492f3cfc99e5758fb650988a4c778dcf4e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 2 Apr 2017 00:33:29 +0200 Subject: [PATCH 015/105] Add sphere vs convex polyhedron test in SAT algorithm --- CMakeLists.txt | 8 +-- src/collision/PolyhedronMesh.cpp | 2 + .../narrowphase/DefaultCollisionDispatch.cpp | 4 ++ .../narrowphase/DefaultCollisionDispatch.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 64 ++++++++++++++++++- src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +- ... => SphereVsConvexPolyhedronAlgorithm.cpp} | 4 +- ....h => SphereVsConvexPolyhedronAlgorithm.h} | 14 ++-- src/collision/shapes/BoxShape.cpp | 12 ++-- src/collision/shapes/BoxShape.h | 4 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConvexMeshShape.cpp | 2 +- src/collision/shapes/ConvexMeshShape.h | 4 +- ...lyhedron.cpp => ConvexPolyhedronShape.cpp} | 4 +- ...exPolyhedron.h => ConvexPolyhedronShape.h} | 14 ++-- 15 files changed, 105 insertions(+), 39 deletions(-) rename src/collision/narrowphase/{SphereVsConvexMeshAlgorithm.cpp => SphereVsConvexPolyhedronAlgorithm.cpp} (95%) rename src/collision/narrowphase/{SphereVsConvexMeshAlgorithm.h => SphereVsConvexPolyhedronAlgorithm.h} (84%) rename src/collision/shapes/{ConvexPolyhedron.cpp => ConvexPolyhedronShape.cpp} (95%) rename src/collision/shapes/{ConvexPolyhedron.h => ConvexPolyhedronShape.h} (90%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f6ab0bc..0a30f8c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,14 +84,14 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" - "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h" - "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp" + "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexShape.cpp" - "src/collision/shapes/ConvexPolyhedron.h" - "src/collision/shapes/ConvexPolyhedron.cpp" + "src/collision/shapes/ConvexPolyhedronShape.h" + "src/collision/shapes/ConvexPolyhedronShape.cpp" "src/collision/shapes/ConcaveShape.h" "src/collision/shapes/ConcaveShape.cpp" "src/collision/shapes/BoxShape.h" diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 50ef5bab..60a3be9f 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -111,6 +111,7 @@ Vector3 PolyhedronMesh::getVertex(uint index) const { return vertex; } +// Compute the faces normals void PolyhedronMesh::computeFacesNormals() { // For each face @@ -122,5 +123,6 @@ void PolyhedronMesh::computeFacesNormals() { const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]); const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]); mFacesNormals[f] = vec1.cross(vec2); + mFacesNormals[f].normalize(); } } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 5fdd485b..0a1e498c 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -52,6 +52,10 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) { return &mCapsuleVsCapsuleAlgorithm; } + // Sphere vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mSphereVsConvexPolyhedronAlgorithm; + } return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 65e05497..7c5d77ea 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -30,7 +30,7 @@ #include "CollisionDispatch.h" #include "ConcaveVsConvexAlgorithm.h" #include "SphereVsSphereAlgorithm.h" -#include "SphereVsConvexMeshAlgorithm.h" +#include "SphereVsConvexPolyhedronAlgorithm.h" #include "SphereVsCapsuleAlgorithm.h" #include "CapsuleVsCapsuleAlgorithm.h" #include "GJK/GJKAlgorithm.h" @@ -51,7 +51,7 @@ class DefaultCollisionDispatch : public CollisionDispatch { SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; /// Sphere vs Convex Mesh collision algorithm - SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm; + SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; /// Sphere vs Capsule collision algorithm SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 61910acc..ee241c77 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -27,6 +27,8 @@ #include "SATAlgorithm.h" #include "constraint/ContactPoint.h" #include "collision/PolyhedronMesh.h" +#include "collision/shapes/ConvexPolyhedronShape.h" +#include "collision/shapes/SphereShape.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -45,7 +47,7 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact case CollisionShapeType::CONVEX_POLYHEDRON: return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); case CollisionShapeType::SPHERE: - return testCollisionSphereVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); + return testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); case CollisionShapeType::CAPSULE: return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); case CollisionShapeType::TRIANGLE: @@ -57,23 +59,81 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact } // Test collision between a sphere and a convex mesh -bool SATAlgorithm::testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + + // Get the capsule collision shapes + const SphereShape* sphere = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexPolyhedronShape* polyhedron = static_cast(narrowPhaseInfo->collisionShape2); + + + // Get the transform from sphere local-space to polyhedron local-space + const Transform sphereToPolyhedronSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * + narrowPhaseInfo->shape1ToWorldTransform; + + // Transform the center of the sphere into the local-space of the convex polyhedron + const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition(); + + // Minimum penetration depth + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + + Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter; + decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius(); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } + } + + const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); + const Vector3 normalWorld = -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minFaceNormal); + const Vector3 contactPointSphereLocal = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * normalWorld * sphere->getRadius(); + const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + + // Create the contact info object + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, contactPointSphereLocal, contactPointPolyhedronLocal); + + return true; } // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); } // Test collision between a triangle and a convex mesh bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); } // Test collision between two convex meshes bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index dc5bc5f2..f3ae2ab2 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -47,7 +47,7 @@ class SATAlgorithm { const Vector3& c, const Vector3& d) const; /// Test collision between a sphere and a convex mesh - bool testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; /// Test collision between a capsule and a convex mesh bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp similarity index 95% rename from src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp rename to src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 06dbc615..107348db 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -24,7 +24,7 @@ ********************************************************************************/ // Libraries -#include "SphereVsConvexMeshAlgorithm.h" +#include "SphereVsConvexPolyhedronAlgorithm.h" #include "SAT/SATAlgorithm.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/ConvexMeshShape.h" @@ -32,7 +32,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, +bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { // Get the local-space to world-space transforms diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h similarity index 84% rename from src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h rename to src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index f0046545..0bdac19a 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -35,12 +35,12 @@ /// Namespace ReactPhysics3D namespace reactphysics3d { -// Class SphereVsConvexMeshAlgorithm +// Class SphereVsConvexPolyhedronAlgorithm /** * This class is used to compute the narrow-phase collision detection - * between a sphere and a convex mesh. + * between a sphere and a convex polyhedron. */ -class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm { +class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { protected : @@ -49,16 +49,16 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm { // -------------------- Methods -------------------- // /// Constructor - SphereVsConvexMeshAlgorithm() = default; + SphereVsConvexPolyhedronAlgorithm() = default; /// Destructor - virtual ~SphereVsConvexMeshAlgorithm() override = default; + virtual ~SphereVsConvexPolyhedronAlgorithm() override = default; /// Deleted copy-constructor - SphereVsConvexMeshAlgorithm(const SphereVsConvexMeshAlgorithm& algorithm) = delete; + SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Deleted assignment operator - SphereVsConvexMeshAlgorithm& operator=(const SphereVsConvexMeshAlgorithm& algorithm) = delete; + SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index f00210c2..e1c97844 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -38,7 +38,7 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexPolyhedron(margin), mExtent(extent - Vector3(margin, margin, margin)) { + : ConvexPolyhedronShape(margin), mExtent(extent - Vector3(margin, margin, margin)) { assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.z > decimal(0.0) && extent.z > margin); @@ -57,15 +57,15 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) std::vector face0; face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); std::vector face1; - face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); + face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); std::vector face2; - face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); + face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); std::vector face3; - face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); + face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); std::vector face4; - face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); + face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); std::vector face5; - face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); + face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); mHalfEdgeStructure.addFace(face0); mHalfEdgeStructure.addFace(face1); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d78bda25..be79e3dd 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -28,7 +28,7 @@ // Libraries #include -#include "ConvexPolyhedron.h" +#include "ConvexPolyhedronShape.h" #include "body/CollisionBody.h" #include "mathematics/mathematics.h" @@ -50,7 +50,7 @@ namespace reactphysics3d { * constructor of the box shape. Otherwise, it is recommended to use the * default margin distance by not using the "margin" parameter in the constructor. */ -class BoxShape : public ConvexPolyhedron { +class BoxShape : public ConvexPolyhedronShape { protected : diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 28e724fd..2b1fa576 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -41,7 +41,7 @@ namespace reactphysics3d { /// Type of the collision shape enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD}; -const int NB_COLLISION_SHAPE_TYPES = 9; +const int NB_COLLISION_SHAPE_TYPES = 6; // Declarations class ProxyShape; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 40382aeb..263bb736 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -42,7 +42,7 @@ using namespace reactphysics3d; * @param margin Collision margin (in meters) around the collision shape */ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin) - : ConvexPolyhedron(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { + : ConvexPolyhedronShape(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { // Recalculate the bounds of the mesh recalculateBounds(); diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index cc88d38d..ff11c232 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H // Libraries -#include "ConvexPolyhedron.h" +#include "ConvexPolyhedronShape.h" #include "engine/CollisionWorld.h" #include "mathematics/mathematics.h" #include "collision/TriangleMesh.h" @@ -59,7 +59,7 @@ class CollisionWorld; * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method * in order to use the edges information for collision detection. */ -class ConvexMeshShape : public ConvexPolyhedron { +class ConvexMeshShape : public ConvexPolyhedronShape { protected : diff --git a/src/collision/shapes/ConvexPolyhedron.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp similarity index 95% rename from src/collision/shapes/ConvexPolyhedron.cpp rename to src/collision/shapes/ConvexPolyhedronShape.cpp index 26964554..e73a7cd7 100644 --- a/src/collision/shapes/ConvexPolyhedron.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -24,14 +24,14 @@ ********************************************************************************/ // Libraries -#include "ConvexPolyhedron.h" +#include "ConvexPolyhedronShape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -ConvexPolyhedron::ConvexPolyhedron(decimal margin) +ConvexPolyhedronShape::ConvexPolyhedronShape(decimal margin) : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) { } diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedronShape.h similarity index 90% rename from src/collision/shapes/ConvexPolyhedron.h rename to src/collision/shapes/ConvexPolyhedronShape.h index 2a544077..059e5a3b 100644 --- a/src/collision/shapes/ConvexPolyhedron.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -33,12 +33,12 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Class ConvexPolyhedron +// Class ConvexPolyhedronShape /** * This abstract class represents a convex polyhedron collision shape associated with a * body that is used during the narrow-phase collision detection. */ -class ConvexPolyhedron : public ConvexShape { +class ConvexPolyhedronShape : public ConvexShape { protected : @@ -51,16 +51,16 @@ class ConvexPolyhedron : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - ConvexPolyhedron(decimal margin); + ConvexPolyhedronShape(decimal margin); /// Destructor - virtual ~ConvexPolyhedron() override = default; + virtual ~ConvexPolyhedronShape() override = default; /// Deleted copy-constructor - ConvexPolyhedron(const ConvexPolyhedron& shape) = delete; + ConvexPolyhedronShape(const ConvexPolyhedronShape& shape) = delete; /// Deleted assignment operator - ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete; + ConvexPolyhedronShape& operator=(const ConvexPolyhedronShape& shape) = delete; /// Return the number of faces of the polyhedron virtual uint getNbFaces() const=0; @@ -91,7 +91,7 @@ class ConvexPolyhedron : public ConvexShape { }; // Return true if the collision shape is a polyhedron -inline bool ConvexPolyhedron::isPolyhedron() const { +inline bool ConvexPolyhedronShape::isPolyhedron() const { return true; } From f61fea8b8ab3d592c42a3074200307ebd7014d36 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Apr 2017 22:09:59 +0200 Subject: [PATCH 016/105] Add clippling segment/polygons methods, fix issues and add convex vs capsule algorithm --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 11 -- src/collision/CollisionDetection.h | 11 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 61 ++++++++ .../CapsuleVsConvexPolyhedronAlgorithm.h | 71 +++++++++ .../narrowphase/SAT/SATAlgorithm.cpp | 50 +++--- src/collision/narrowphase/SAT/SATAlgorithm.h | 27 ++-- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 33 ++-- .../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +- .../SphereVsConvexPolyhedronAlgorithm.h | 4 +- src/engine/OverlappingPair.cpp | 2 - src/mathematics/mathematics_functions.cpp | 147 +++++++++++++++++- src/mathematics/mathematics_functions.h | 11 +- test/Test.h | 2 +- test/tests/collision/TestRaycast.h | 6 +- .../mathematics/TestMathematicsFunctions.h | 90 ++++++++++- 16 files changed, 452 insertions(+), 83 deletions(-) create mode 100644 src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp create mode 100644 src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a30f8c2..5cacac60 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,6 +86,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 4a0ef718..908c5bf5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -288,17 +288,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; - // Make sure the shape with the smallest collision shape type comes first - const uint shape1TypeIndex = static_cast(shape1->getCollisionShape()->getType()); - const uint shape2TypeIndex = static_cast(shape2->getCollisionShape()->getType()); - if (shape1TypeIndex > shape2TypeIndex) { - - // Swap the two shapes - ProxyShape* temp = shape1; - shape1 = shape2; - shape2 = temp; - } - // Compute the overlapping pair ID overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 9444b319..29200ec7 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -266,8 +266,15 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, const CollisionShapeType& shape2Type) const { - const unsigned int shape1Index = static_cast(shape1Type); - const unsigned int shape2Index = static_cast(shape2Type); + uint shape1Index = static_cast(shape1Type); + uint shape2Index = static_cast(shape2Type); + + // Swap the shape types if necessary + if (shape1Index > shape2Index) { + const uint tempIndex = shape1Index; + shape1Index = shape2Index; + shape2Index = tempIndex; + } assert(shape1Index <= shape2Index); diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..8deb17e5 --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,61 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "SAT/SATAlgorithm.h" +#include "GJK/GJKAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) { + + // Get the local-space to world-space transforms + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; + + // First, we run the GJK algorithm + GJKAlgorithm gjkAlgorithm; + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + + // If we have found a contact point inside the margins (shallow penetration) + if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + + // Return true + return true; + } + + // If we have overlap even without the margins (deep penetration) + if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm; + return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + } + + return false; +} diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..e9905596 --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a capsule and a convex polyhedron. + */ +class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsConvexPolyhedronAlgorithm(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index ee241c77..1b57e36f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -39,39 +39,25 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { - - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - - switch (narrowPhaseInfo->collisionShape1->getType()) { - case CollisionShapeType::CONVEX_POLYHEDRON: - return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); - case CollisionShapeType::SPHERE: - return testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); - case CollisionShapeType::CAPSULE: - return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); - case CollisionShapeType::TRIANGLE: - return testCollisionTriangleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo); - default: assert(false); - } - - return false; -} - // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; + + assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the capsule collision shapes - const SphereShape* sphere = static_cast(narrowPhaseInfo->collisionShape1); - const ConvexPolyhedronShape* polyhedron = static_cast(narrowPhaseInfo->collisionShape2); + const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; // Get the transform from sphere local-space to polyhedron local-space - const Transform sphereToPolyhedronSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * - narrowPhaseInfo->shape1ToWorldTransform; + const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse(); + const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform; // Transform the center of the sphere into the local-space of the convex polyhedron const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition(); @@ -105,18 +91,24 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* } const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); - const Vector3 normalWorld = -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minFaceNormal); - const Vector3 contactPointSphereLocal = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * normalWorld * sphere->getRadius(); + Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); + const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse() * normalWorld * sphere->getRadius(); const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + if (!isSphereShape1) { + normalWorld = -normalWorld; + } + // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, contactPointSphereLocal, contactPointPolyhedronLocal); + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); return true; } // Test collision between a capsule and a convex mesh -bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index f3ae2ab2..b779d711 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -46,18 +46,6 @@ class SATAlgorithm { bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const; - /// Test collision between a sphere and a convex mesh - bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - - /// Test collision between a capsule and a convex mesh - bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - - /// Test collision between a triangle and a convex mesh - bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - - /// Test collision between two convex meshes - bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - public : // -------------------- Methods -------------------- // @@ -74,9 +62,18 @@ class SATAlgorithm { /// Deleted assignment operator SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volumes collide. - bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo); + /// Test collision between a sphere and a convex mesh + bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between a capsule and a convex mesh + bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between a triangle and a convex mesh + bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + + /// Test collision between two convex meshes + bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + }; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index aace17fd..a311b2bf 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -32,23 +32,30 @@ using namespace reactphysics3d; bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { - - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; + + assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the collision shapes - const SphereShape* sphereShape = static_cast(narrowPhaseInfo->collisionShape1); - const CapsuleShape* capsuleShape = static_cast(narrowPhaseInfo->collisionShape2); + const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); // Get the transform from sphere local-space to capsule local-space - const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse(); + const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform; // Transform the center of the sphere into the local-space of the capsule shape const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); // Compute the end-points of the inner segment of the capsule - const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5); + const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0); + const Vector3 capsuleSegB(0, capsuleHalfHeight, 0); // Compute the point on the inner capsule segment that is the closes to center of sphere const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter); @@ -69,12 +76,18 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * sphereCenterToSegment; + Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; decimal penetrationDepth = sumRadius - sphereSegmentDistance; + + if (!isSphereShape1) { + normalWorld = -normalWorld; + } // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, + isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); return true; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 107348db..44959971 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -25,15 +25,14 @@ // Libraries #include "SphereVsConvexPolyhedronAlgorithm.h" +#include "GJK/GJKAlgorithm.h" #include "SAT/SATAlgorithm.h" -#include "collision/shapes/SphereShape.h" -#include "collision/shapes/ConvexMeshShape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { + ContactManifoldInfo& contactManifoldInfo) { // Get the local-space to world-space transforms const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; @@ -55,7 +54,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - return satAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); } return false; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 0bdac19a..591ebd44 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H -#define REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H +#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H // Libraries #include "body/Body.h" diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 6df4bf4e..1f5d894e 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -35,6 +35,4 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), mCachedSeparatingAxis(0.0, 1.0, 0.0) { - assert(static_cast(shape1->getCollisionShape()->getType()) <= - static_cast(shape2->getCollisionShape()->getType())); } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 7de898ae..07e3530f 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -27,6 +27,7 @@ #include "mathematics_functions.h" #include "Vector3.h" #include +#include using namespace reactphysics3d; @@ -187,7 +188,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con return t; } -/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { decimal distAB = (linePointB - linePointA).length(); @@ -199,4 +200,148 @@ decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePo return ((point - linePointA).cross(point - linePointB)).length() / distAB; } +// Clip a segment against multiple planes and return the clipped segment vertices +// This method implements the Sutherland–Hodgman clipping algorithm +std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const std::vector& planesPoints, + const std::vector& planesNormals) { + + assert(planesPoints.size() == planesNormals.size()); + + std::vector inputVertices = {segA, segB}; + std::vector outputVertices; + + // For each clipping plane + for (uint p=0; p= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices.push_back(v1 + t * (v2 - v1)); + } + else { + outputVertices.push_back(v2); + } + } + else { + outputVertices.push_back(v1); + } + + // Add the second vertex + outputVertices.push_back(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + outputVertices.push_back(v1); + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices.push_back(v1 + t * (v2 - v1)); + } + } + } + + inputVertices = outputVertices; + } + + return outputVertices; +} + +/// Clip a polygon against multiple planes and return the clipped polygon vertices +/// This method implements the Sutherland–Hodgman clipping algorithm +std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, + const std::vector& planesNormals) { + + assert(planesPoints.size() == planesNormals.size()); + + std::vector inputVertices(polygonVertices); + std::vector outputVertices; + + // For each clipping plane + for (uint p=0; p= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices.push_back(v1 + t * (v2 - v1)); + } + else { + outputVertices.push_back(v2); + } + } + + // Add the second vertex + outputVertices.push_back(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices.push_back(v1 + t * (v2 - v1)); + } + else { + outputVertices.push_back(v1); + } + } + } + + vStart = vEnd; + } + + inputVertices = outputVertices; + } + + return outputVertices; +} + diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 315de65c..639e8d3b 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -32,6 +32,7 @@ #include #include #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -96,8 +97,16 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB /// Compute the distance between a point and a line decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); +/// Clip a segment against multiple planes and return the clipped segment vertices +std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const std::vector& planesPoints, + const std::vector& planesNormals); + +/// Clip a polygon against multiple planes and return the clipped polygon vertices +std::vector clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, + const std::vector& planesNormals); + } - #endif diff --git a/test/Test.h b/test/Test.h index 37fa9dee..1288813d 100644 --- a/test/Test.h +++ b/test/Test.h @@ -35,7 +35,7 @@ namespace reactphysics3d { // Macros -#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__) +#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__); #define fail(text) applyFail(text, __FILE__, __LINE__); // Class Test diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index fe8926d5..0f25ab94 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -300,8 +300,8 @@ class TestRaycast : public Test { mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); - mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); - mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); + //mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); + //mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2); mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); @@ -1787,7 +1787,7 @@ class TestRaycast : public Test { // ----- Test raycast miss ----- // test(!mConcaveMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); + //test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray1, &mCallback); test(!mCallback.isHit); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 42e1065c..04a81a69 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -27,8 +27,6 @@ #define TEST_MATHEMATICS_FUNCTIONS_H // Libraries -#include "Test.h" -#include "mathematics/mathematics_functions.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -170,6 +168,94 @@ class TestMathematicsFunctions : public Test { test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); + + + // Test clipSegmentWithPlanes() + std::vector segmentVertices; + segmentVertices.push_back(Vector3(-6, 3, 0)); + segmentVertices.push_back(Vector3(8, 3, 0)); + + std::vector planesNormals; + std::vector planesPoints; + planesNormals.push_back(Vector3(-1, 0, 0)); + planesPoints.push_back(Vector3(4, 0, 0)); + + std::vector clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], + planesPoints, planesNormals); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(8, 3, 0)); + segmentVertices.push_back(Vector3(-6, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(-6, 3, 0)); + segmentVertices.push_back(Vector3(3, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(5, 3, 0)); + segmentVertices.push_back(Vector3(8, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + test(clipSegmentVertices.size() == 0); + + // Test clipPolygonWithPlanes() + std::vector polygonVertices; + polygonVertices.push_back(Vector3(-4, 2, 0)); + polygonVertices.push_back(Vector3(7, 2, 0)); + polygonVertices.push_back(Vector3(7, 4, 0)); + polygonVertices.push_back(Vector3(-4, 4, 0)); + + planesNormals.clear(); + planesPoints.clear(); + planesNormals.push_back(Vector3(1, 0, 0)); + planesPoints.push_back(Vector3(0, 0, 0)); + planesNormals.push_back(Vector3(0, 1, 0)); + planesPoints.push_back(Vector3(0, 0, 0)); + planesNormals.push_back(Vector3(-1, 0, 0)); + planesPoints.push_back(Vector3(10, 0, 0)); + planesNormals.push_back(Vector3(0, -1, 0)); + planesPoints.push_back(Vector3(10, 5, 0)); + + clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + test(clipSegmentVertices.size() == 4); + test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001)); + test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001)); + } }; From 7fb6f49149b04f5303735394ab568af726c03cbf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 28 Apr 2017 21:40:16 +0200 Subject: [PATCH 017/105] Working on capsule vs polyhedron narrow-phase algorithm --- src/collision/ContactManifoldInfo.h | 27 ++- src/collision/PolyhedronMesh.cpp | 15 ++ src/collision/PolyhedronMesh.h | 14 ++ src/collision/ProxyShape.h | 1 - .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 7 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 71 +++++- .../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/SAT/SATAlgorithm.cpp | 218 +++++++++++++++++- src/collision/narrowphase/SAT/SATAlgorithm.h | 12 + .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 + .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- src/collision/shapes/BoxShape.h | 8 + src/collision/shapes/ConvexMeshShape.h | 8 + src/collision/shapes/ConvexPolyhedronShape.h | 3 + src/collision/shapes/ConvexShape.h | 2 +- src/mathematics/mathematics_functions.cpp | 5 + src/mathematics/mathematics_functions.h | 3 + 20 files changed, 382 insertions(+), 30 deletions(-) diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index d4afc590..f631b9fc 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -64,15 +64,8 @@ class ContactManifoldInfo { /// Destructor ~ContactManifoldInfo() { - // Delete all the contact points in the linked list - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - ContactPointInfo* elementToDelete = element; - element = element->next; - - // Delete the current element - mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); - } + // Remove all the contact points + reset(); } /// Deleted copy-constructor @@ -96,6 +89,22 @@ class ContactManifoldInfo { mContactPointsList = contactPointInfo; } + /// Remove all the contact points + void reset() { + + // Delete all the contact points in the linked list + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + ContactPointInfo* elementToDelete = element; + element = element->next; + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mContactPointsList = nullptr; + } + /// Get the first contact point info of the linked list of contact points ContactPointInfo* getFirstContactPointInfo() const { return mContactPointsList; diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 60a3be9f..61b1e3c8 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -42,6 +42,9 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { // Compute the faces normals computeFacesNormals(); + + // Compute the centroid + computeCentroid(); } // Destructor @@ -126,3 +129,15 @@ void PolyhedronMesh::computeFacesNormals() { mFacesNormals[f].normalize(); } } + +// Compute the centroid of the polyhedron +void PolyhedronMesh::computeCentroid() { + + mCentroid.setToZero(); + + for (uint v=0; v < getNbVertices(); v++) { + mCentroid += getVertex(v); + } + + mCentroid /= getNbVertices(); +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index 940b2db0..a64db670 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -55,6 +55,9 @@ class PolyhedronMesh { /// Array with the face normals Vector3* mFacesNormals; + /// Centroid of the polyhedron + Vector3 mCentroid; + // -------------------- Methods -------------------- // /// Create the half-edge structure of the mesh @@ -63,6 +66,9 @@ class PolyhedronMesh { /// Compute the faces normals void computeFacesNormals(); + /// Compute the centroid of the polyhedron + void computeCentroid() ; + public: // -------------------- Methods -------------------- // @@ -84,6 +90,9 @@ class PolyhedronMesh { /// Return the half-edge structure of the mesh const HalfEdgeStructure& getHalfEdgeStructure() const; + + /// Return the centroid of the polyhedron + Vector3 getCentroid() const; }; // Return the number of vertices @@ -102,6 +111,11 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; } +// Return the centroid of the polyhedron +inline Vector3 PolyhedronMesh::getCentroid() const { + return mCentroid; +} + } #endif diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 45c0add2..d9575b3d 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -175,7 +175,6 @@ class ProxyShape { friend class CollisionDetection; friend class CollisionWorld; friend class DynamicsWorld; - friend class EPAAlgorithm; friend class GJKAlgorithm; friend class ConvexMeshShape; diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 11160b8c..a6c7a020 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -30,13 +30,14 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between two capsules +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); - const decimal parallelEpsilon = decimal(0.001); - // Get the capsule collision shapes const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); @@ -62,7 +63,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); // If the two capsules are parallel (we create two contact points) - if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) { + if (areParallelVectors(seg1, seg2)) { // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 898d6337..9d8393c9 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -60,7 +60,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between two capsules virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 8deb17e5..5e8dd5f9 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -27,24 +27,86 @@ #include "CapsuleVsConvexPolyhedronAlgorithm.h" #include "SAT/SATAlgorithm.h" #include "GJK/GJKAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/ConvexPolyhedronShape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between a capsule and a polyhedron +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; + SATAlgorithm satAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + // GJK has found a shallow contact. If the contact normal is parallel to a face of the + // polyhedron mesh, we would like to create two contact points instead of a single one + // (as in the deep contact case with SAT algorithm) + + // Get the contact point created by GJK + ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); + + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + // For each face of the polyhedron + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; + + // If the polyhedron face normal is parallel to the computed GJK contact normal + if (areParallelVectors(faceNormalWorld, contactPoint->normal)) { + + // Remove the previous contact point computed by GJK + contactManifoldInfo.reset(); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + + // Compute and create two contact points + satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + contactManifoldInfo, isCapsuleShape1); + + break; + } + + } + // Return true return true; } @@ -53,7 +115,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index e9905596..6b3f504f 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -60,7 +60,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between a capsule and a polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 1b57e36f..3637b3dc 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -27,7 +27,7 @@ #include "SATAlgorithm.h" #include "constraint/ContactPoint.h" #include "collision/PolyhedronMesh.h" -#include "collision/shapes/ConvexPolyhedronShape.h" +#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" #include "configuration.h" #include "engine/Profiler.h" @@ -110,8 +110,220 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + + assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Minimum penetration depth + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + uint minEdgeIndex = 0; + bool isMinPenetrationFaceNormal = false; + Vector3 separatingAxisCapsuleSpace; + Vector3 separatingPolyhedronEdgeVertex1; + Vector3 separatingPolyhedronEdgeVertex2; + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + + // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) + const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr); + const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); + const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = faceNormalCapsuleSpace; + } + } + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; + + // For each direction that is the cross product of the capsule inner segment and + // an edge of the polyhedron + for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { + + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); + const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + + // Check using the Gauss Map if this edge cross product can be as separating axis + if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + + // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) + Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace); + + // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment + if (axis.lengthSquare() >= decimal(0.00001)) { + + const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid(); + const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1; + + // Swap axis direction if necessary such that it points out of the polyhedron + if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) { + axis = -axis; + } + + axis.normalize(); + + // Compute the penetration depth + const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr); + const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minEdgeIndex = e; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = axis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } + } + } + } + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + const decimal capsuleRadius = capsuleShape->getRadius(); + + // If the separating axis is a face normal + // We need to clip the inner capsule segment with the adjacent faces of the separating face + if (isMinPenetrationFaceNormal) { + + computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, + polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + contactManifoldInfo, isCapsuleShape1); + } + else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment + + // Compute the closest points between the inner capsule segment and the + // edge of the polyhedron in polyhedron local-space + Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment; + computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2, + closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); + + + // Project closest capsule inner segment point into the capsule bounds + const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + + // Create the contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + } + + return true; +} + +// Compute the two contact points between a polyhedron and a capsule when the separating +// axis is a face normal of the polyhedron +void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const { + + HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); + uint firstEdgeIndex = face.edgeIndex; + uint edgeIndex = firstEdgeIndex; + + std::vector planesPoints; + std::vector planesNormals; + + // For each adjacent edge of the separating face of the polyhedron + do { + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron + planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); + planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex)); + + edgeIndex = edge.nextEdgeIndex; + + } while(edgeIndex != firstEdgeIndex); + + // First we clip the inner segment of the capsule with the four planes of the adjacent faces + std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + planesPoints, planesNormals); + + // Project the two clipped points into the polyhedron face + const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); + const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius); + const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius); + + // Project the two clipped points into the capsule bounds + const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius; + const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius; + + // Create the contact points + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron, + isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron, + isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule); +} + +// This method returns true if an edge of a polyhedron and a capsule forms a +// face of the Minkowski Difference. This test is used to know if two edges +// (one edge of the polyhedron vs the inner segment of the capsule in this case) +// have to be test as a possible separating axis +bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const { + + // Return true if the arc on the Gauss Map corresponding to the polyhedron edge + // intersect the unit circle plane corresponding to capsule Gauss Map + return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0); } // Test collision between a triangle and a convex mesh diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index b779d711..42e380f2 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -29,6 +29,7 @@ // Libraries #include "collision/ContactManifoldInfo.h" #include "collision/NarrowPhaseInfo.h" +#include "collision/shapes/ConvexPolyhedronShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -68,6 +69,17 @@ class SATAlgorithm { /// Test collision between a capsule and a convex mesh bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron + void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const; + + // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference + bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const; + /// Test collision between a triangle and a convex mesh bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a311b2bf..cd437d6e 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -31,6 +31,9 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between a sphere and a capsule +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 784df6f9..154f6e4c 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -60,7 +60,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between a sphere and a capsule virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 44959971..708d9f8d 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -31,13 +31,12 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection a sphere and a convex polyhedron +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 591ebd44..c1fd3e55 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection a sphere and a convex polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index be79e3dd..d2b92dcc 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -128,6 +128,9 @@ class BoxShape : public ConvexPolyhedronShape { /// Return the normal vector of a given face of the polyhedron virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; // Return the extents of the box @@ -236,6 +239,11 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { } } +// Return the centroid of the box +inline Vector3 BoxShape::getCentroid() const { + return Vector3::zero(); +} + // Return the number of half-edges of the polyhedron inline uint BoxShape::getNbHalfEdges() const { return 24; diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index ff11c232..28ba9c86 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -142,6 +142,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape { /// Return the normal vector of a given face of the polyhedron virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; /// Set the scaling vector of the collision shape @@ -239,6 +242,11 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { return mPolyhedronMesh->getFaceNormal(faceIndex); } +// Return the centroid of the polyhedron +inline Vector3 ConvexMeshShape::getCentroid() const { + return mPolyhedronMesh->getCentroid(); +} + } #endif diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 059e5a3b..f6d2a34b 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -88,6 +88,9 @@ class ConvexPolyhedronShape : public ConvexShape { /// Return true if the collision shape is a polyhedron virtual bool isPolyhedron() const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const=0; }; // Return true if the collision shape is a polyhedron diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 9180fefe..4b98c9fa 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -81,7 +81,7 @@ class ConvexShape : public CollisionShape { // -------------------- Friendship -------------------- // friend class GJKAlgorithm; - friend class EPAAlgorithm; + friend class SATAlgorithm; }; // Return true if the collision shape is convex, false if it is concave diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 07e3530f..36ba113a 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -60,6 +60,11 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { return vector; } +/// Return true if two vectors are parallel +bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { + return vector1.cross(vector2).lengthSquare() < decimal(0.00001); +} + /// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 639e8d3b..44ce9cf4 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -76,6 +76,9 @@ inline bool sameSign(decimal a, decimal b) { return a * b >= decimal(0.0); } +/// Return true if two vectors are parallel +bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); + /// Clamp a vector such that it is no longer than a given maximum length Vector3 clamp(const Vector3& vector, decimal maxLength); From 0ec21e36b9e823aefed5c03949758af41bf240ea Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 16 May 2017 07:10:44 +0200 Subject: [PATCH 018/105] Working on SAT algorithm between two polyhedra --- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 2 +- .../narrowphase/SAT/SATAlgorithm.cpp | 336 +++++++++++++++++- src/collision/narrowphase/SAT/SATAlgorithm.h | 24 +- src/mathematics/mathematics_functions.cpp | 2 +- src/mathematics/mathematics_functions.h | 2 +- .../mathematics/TestMathematicsFunctions.h | 13 +- 6 files changed, 349 insertions(+), 30 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index a6c7a020..17008622 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -66,7 +66,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase if (areParallelVectors(seg1, seg2)) { // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) - const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); + const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); if (segmentsDistance >= sumRadius) { // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 3637b3dc..fd09db27 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -169,8 +169,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; - // For each direction that is the cross product of the capsule inner segment and - // an edge of the polyhedron + // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { // Get an edge from the polyhedron (convert it into the capsule local-space) @@ -326,18 +325,327 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0); } -// Test collision between a triangle and a convex mesh -bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { - - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); -} - -// Test collision between two convex meshes -bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +// Test collision between two convex polyhedrons +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + + const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfo->collisionShape2); + + const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; + const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); + + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + bool isMinPenetrationFaceNormal = false; + bool isMinPenetrationFaceNormalPolyhedron1 = false; + Vector3 separatingEdge1A, separatingEdge1B; + Vector3 separatingEdge2A, separatingEdge2B; + Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Test all the face normals of the polyhedron 1 for separating axis + uint faceIndex; + decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + + // Test all the face normals of the polyhedron 2 for separating axis + penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + + // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis + for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + + // Get an edge of polyhedron 1 + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + + for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { + + // Get an edge of polyhedron 2 + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + if (penetrationDepth <= decimal(0.0)) { + + // We have found a separating axis + return false; + } + + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormalPolyhedron1 = false; + isMinPenetrationFaceNormal = false; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + + } + } + } + + assert(minPenetrationDepth > decimal(0.0)); + assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal); + + // If the separation axis is a face normal + if (isMinPenetrationFaceNormal) { + + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; + const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; + const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); + const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; + + // Compute the world normal + const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); + + // Get the reference face + HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + + // Find the incident face on the other polyhedron (most anti-parallel face) + uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + + // Get the incident face + HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + + std::vector polygonVertices; // Vertices to clip of the incident face + std::vector planesNormals; // Normals of the clipping planes + std::vector planesPoints; // Points on the clipping planes + + // Get all the vertices of the incident face (in the reference local-space) + std::vector::const_iterator it; + for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); + polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + } + + // Get the reference face clipping planes + uint currentEdgeIndex = referenceFace.edgeIndex; + uint firstEdgeIndex = currentEdgeIndex; + do { + + // Get the adjacent edge + HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + + // Get the twin edge + HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Get the adjacent face normal (and negate it to have a clipping plane) + Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex); + + // Get a vertex of the clipping plane (vertex of the adjacent edge) + Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex); + + planesNormals.push_back(faceNormal); + planesPoints.push_back(faceVertex); + + // Go to the next adjacent edge of the reference face + currentEdgeIndex = edge.nextEdgeIndex; + + } while (currentEdgeIndex != firstEdgeIndex); + + // Clip the reference faces with the adjacent planes of the reference face + std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + + // We only keep the clipped points that are below the reference face + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex); + std::vector::const_iterator itPoints; + for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + + // If the clip point is bellow the reference face + if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { + + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space + const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + + // Project the contact point onto the reference face + Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth; + + // Create a new contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + } + } + } + else { // If we have an edge vs edge contact + + // Compute the closest points between the two edges (in the local-space of poylhedron 2) + Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; + computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B, + closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); + + // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 + const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; + + // Compute the world normal + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Create the contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + } + + return true; +} + +// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector +// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron +uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + for (uint i=0; i < polyhedron->getNbFaces(); i++) { + + // Get the face normal + decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} + +// Compute and return the distance between the two edges in the direction of the candidate separating axis +decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxisPolyhedron2Space) const { + + // If the two edges are parallel + if (areParallelVectors(edge1Direction, edge2Direction)) { + + // Return a large penetration depth to skip those edges + return DECIMAL_LARGEST; + } + + // Compute the candidate separating axis (cross product between two polyhedrons edges) + Vector3 axis = edge1Direction.cross(edge2Direction).getUnit(); + + // Make sure the axis direction is going from first to second polyhedron + if (axis.dot(edge2A - polyhedron2Centroid) > decimal(0.0)) { + axis = -axis; + } + + outSeparatingAxisPolyhedron2Space = axis; + + // Compute and return the distance between the edges + return -axis.dot(edge2A - edge1A); +} + +// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case +decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint& minFaceIndex) const { + + decimal minPenetrationDepth = DECIMAL_LARGEST; + + // For each face of the first polyhedron + for (uint f = 0; f < polyhedron1->getNbFaces(); f++) { + + HalfEdgeStructure::Face face = polyhedron1->getFace(f); + + // Get the face normal + const Vector3 faceNormal = polyhedron1->getFaceNormal(f); + + // Convert the face normal into the local-space of polyhedron 2 + const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; + + // Get the support point of polyhedron 2 in the inverse direction of face normal + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); + + // Compute the penetration depth + const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); + decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return penetrationDepth; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } + } + + return minPenetrationDepth; +} + + +// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) +bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const { + + const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex); + const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); + + const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex); + const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex); + + // Compute b.cross(a) using the edge direction + const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex); + const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1); + + // Compute d.cross(c) using the edge direction + const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex); + const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1; + + // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face) + // Note that we negate the normals of the second polyhedron because we are looking at the + // Gauss map of the minkowski difference of the polyhedrons + return testGaussMapArcsIntersect(a, b, -c, -d, bCrossA, dCrossC); } @@ -346,10 +654,8 @@ bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* na /// and edge between faces with normal C and D on second polygon create a face on the Minkowski /// sum of both polygons. If this is the case, it means that the cross product of both edges /// might be a separating axis. -bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, - const Vector3& c, const Vector3& d) const { - const Vector3 bCrossA = b.cross(a); - const Vector3 dCrossC = d.cross(c); +bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const { const decimal cba = c.dot(bCrossA); const decimal dba = d.dot(bCrossA); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 42e380f2..ae5da16e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -43,9 +43,23 @@ class SATAlgorithm { // -------------------- Methods -------------------- // + /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) + bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const; + /// Return true if the arcs AB and CD on the Gauss Map intersect bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, - const Vector3& c, const Vector3& d) const; + const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const; + + // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector + uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const; + + /// Compute and return the distance between the two edges in the direction of the candidate separating axis + decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxis) const; public : @@ -80,12 +94,12 @@ class SATAlgorithm { bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, const Vector3& edgeAdjacentFace2Normal) const; - /// Test collision between a triangle and a convex mesh - bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - /// Test collision between two convex meshes - bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case + decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; }; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 36ba113a..e01c749a 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -194,7 +194,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con } // Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" -decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { +decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { decimal distAB = (linePointB - linePointA).length(); diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 44ce9cf4..c695f342 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -98,7 +98,7 @@ void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); /// Compute the distance between a point and a line -decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); +decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); /// Clip a segment against multiple planes and return the clipped segment vertices std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 04a81a69..b45415ee 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -162,13 +162,12 @@ class TestMathematicsFunctions : public Test { decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0)); test(tIntersect < 0.0 || tIntersect > 1.0); - // Test computeDistancePointToLineDistance() - test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); - test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); - test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); - test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); - test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); - + // Test computePointToLineDistance() + test(approxEqual(computePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); // Test clipSegmentWithPlanes() std::vector segmentVertices; From 678c88d3bd5be850a3ec354e742b8002363120e6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 16 May 2017 07:11:18 +0200 Subject: [PATCH 019/105] Add box in collision detection scene for the testbed application --- .../CollisionDetectionScene.cpp | 20 +++++++++++++++++++ .../CollisionDetectionScene.h | 1 + 2 files changed, 21 insertions(+) diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 8c0196b3..b96620b6 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -60,6 +60,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere1->setColor(mGreyColorDemo); mSphere1->setSleepingColor(mRedColorDemo); + /* // ---------- Sphere 2 ---------- // openglframework::Vector3 position2(4, 0, 0); @@ -92,6 +93,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mCapsule2->setColor(mGreyColorDemo); mCapsule2->setSleepingColor(mRedColorDemo); +*/ + // ---------- Box 1 ---------- // + openglframework::Vector3 position5(4, 5, 0); + + // Create a box and a corresponding collision body in the dynamics world + mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mBox1); + + // Set the color + mBox1->setColor(mGreyColorDemo); + mBox1->setSleepingColor(mRedColorDemo); // ---------- Cone ---------- // //openglframework::Vector3 position4(0, 0, 0); @@ -171,6 +183,7 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); delete mSphere1; + /* mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); delete mSphere2; @@ -179,6 +192,10 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); delete mCapsule2; + */ + + mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody()); + delete mBox1; /* // Destroy the corresponding rigid body from the dynamics world @@ -300,9 +317,12 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, // Render the shapes if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + /* if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + */ + if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 2c33b3ac..01df3df8 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -146,6 +146,7 @@ class CollisionDetectionScene : public SceneDemo { Sphere* mSphere2; Capsule* mCapsule1; Capsule* mCapsule2; + Box* mBox1; //Cone* mCone; //Cylinder* mCylinder; //Capsule* mCapsule; From 2af87d48045d97b266d2cddaff5085f58e20dcc6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 16 May 2017 07:42:04 +0200 Subject: [PATCH 020/105] Add bias to prefer some axis when penetration depths are the same in SAT algorithm --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 10 +++++++--- src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +++++ 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index fd09db27..c82256f3 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -39,6 +39,9 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Static variables initialization +const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); + // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { @@ -353,7 +356,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // We have found a separating axis return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { isMinPenetrationFaceNormal = true; minPenetrationDepth = penetrationDepth; minFaceIndex = faceIndex; @@ -367,7 +370,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // We have found a separating axis return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { isMinPenetrationFaceNormal = true; minPenetrationDepth = penetrationDepth; minFaceIndex = faceIndex; @@ -409,7 +412,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP return false; } - if (penetrationDepth < minPenetrationDepth) { + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + minPenetrationDepth = penetrationDepth; isMinPenetrationFaceNormalPolyhedron1 = false; isMinPenetrationFaceNormal = false; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index ae5da16e..a95abe31 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -41,6 +41,11 @@ class SATAlgorithm { // -------------------- Attributes -------------------- // + /// Bias used to make sure the SAT algorithm returns the same penetration axis between frames + /// when there are multiple separating axis with the same penetration depth. The goal is to + /// make sure the contact manifold does not change too much between frames. + static const decimal SAME_SEPARATING_AXIS_BIAS; + // -------------------- Methods -------------------- // /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) From 9d55255c56aa171654395c4a77b81efdaa88c086 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 17 May 2017 23:40:17 +0200 Subject: [PATCH 021/105] Add contact point normals and constant color shader in testbed application --- testbed/common/VisualContactPoint.cpp | 96 ++++++++++++++-- testbed/common/VisualContactPoint.h | 24 +++- .../CollisionDetectionScene.cpp | 103 +----------------- .../CollisionDetectionScene.h | 44 ++------ testbed/scenes/raycast/RaycastScene.cpp | 40 ++----- testbed/scenes/raycast/RaycastScene.h | 13 ++- testbed/shaders/color.frag | 38 +++++++ testbed/shaders/color.vert | 43 ++++++++ testbed/src/Scene.h | 5 +- testbed/src/SceneDemo.cpp | 25 ++++- testbed/src/SceneDemo.h | 3 + 11 files changed, 240 insertions(+), 194 deletions(-) mode change 100644 => 100755 testbed/scenes/collisiondetection/CollisionDetectionScene.cpp mode change 100644 => 100755 testbed/scenes/collisiondetection/CollisionDetectionScene.h create mode 100644 testbed/shaders/color.frag create mode 100644 testbed/shaders/color.vert diff --git a/testbed/common/VisualContactPoint.cpp b/testbed/common/VisualContactPoint.cpp index 7661587e..03ed864b 100644 --- a/testbed/common/VisualContactPoint.cpp +++ b/testbed/common/VisualContactPoint.cpp @@ -36,17 +36,24 @@ openglframework::Mesh VisualContactPoint::mMesh; bool VisualContactPoint::mStaticDataCreated = false; // Constructor -VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, - const std::string& meshFolderPath) - : mColor(1.0f, 0.0f, 0.0f, 1.0f) { +VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, const std::string& meshFolderPath, + const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color) + : mColor(color), mVBOVerticesNormalLine(GL_ARRAY_BUFFER) { + + mContactNormalLinePoints[0] = openglframework::Vector3(0, 0, 0); + mContactNormalLinePoints[1] = (normalLineEndPointLocal - position) * 0.5f; // Initialize the position where the mesh will be rendered translateWorld(position); + + // Create the VBO and VAO to render the contact normal line + createContactNormalLineVBOAndVAO(); } // Destructor VisualContactPoint::~VisualContactPoint() { - + mVAONormalLine.destroy(); + mVBOVerticesNormalLine.destroy(); } // Load and initialize the mesh for all the contact points @@ -84,8 +91,7 @@ void VisualContactPoint::destroyStaticData() { } // Render the sphere at the correct position and with the correct orientation -void VisualContactPoint::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void VisualContactPoint::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the VAO mVAO.bind(); @@ -93,6 +99,10 @@ void VisualContactPoint::render(openglframework::Shader& shader, // Bind the shader shader.bind(); + // Set the vertex color + openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); + shader.setVector4Uniform("vertexColor", color, false); + mVBOVertices.bind(); // Set the model to camera matrix @@ -106,10 +116,6 @@ void VisualContactPoint::render(openglframework::Shader& shader, localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); - shader.setVector4Uniform("vertexColor", color, false); - // Get the location of shader attribute variables GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); @@ -138,9 +144,56 @@ void VisualContactPoint::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); + + // Render the contact normal line + renderContactNormalLine(shader, worldToCameraMatrix); } -// Create the Vertex Buffer Objects used to render with OpenGL. +void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { + + // Bind the VAO + mVAONormalLine.bind(); + + // Bind the shader + shader.bind(); + + mVBOVerticesNormalLine.bind(); + + // Set the model to camera matrix + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + // Set the vertex color + openglframework::Vector4 color(0, 1, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + + // Draw the lines + glDrawArrays(GL_LINES, 0, 2); + + glDisableVertexAttribArray(vertexPositionLoc); + + mVBOVerticesNormalLine.unbind(); + + // Unbind the VAO + mVAONormalLine.unbind(); + + shader.unbind(); +} + +// Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL. /// We create two VBOs (one for vertices and one for indices) void VisualContactPoint::createVBOAndVAO() { @@ -181,3 +234,24 @@ void VisualContactPoint::createVBOAndVAO() { // Unbind the VAO mVAO.unbind(); } + +// Create the Vertex Buffer Objects used to render the contact normal line +void VisualContactPoint::createContactNormalLineVBOAndVAO() { + + // Create the VBO for the vertices data + mVBOVerticesNormalLine.create(); + mVBOVerticesNormalLine.bind(); + size_t sizeNormalLineVertices = 2 * sizeof(openglframework::Vector3); + mVBOVerticesNormalLine.copyDataIntoVBO(sizeNormalLineVertices, &mContactNormalLinePoints[0], GL_STATIC_DRAW); + mVBOVerticesNormalLine.unbind(); + + // Create the VAO for both VBOs + mVAONormalLine.create(); + mVAONormalLine.bind(); + + // Bind the VBO of vertices + mVBOVerticesNormalLine.bind(); + + // Unbind the VAO + mVAONormalLine.unbind(); +} diff --git a/testbed/common/VisualContactPoint.h b/testbed/common/VisualContactPoint.h index 944caf21..b540e751 100644 --- a/testbed/common/VisualContactPoint.h +++ b/testbed/common/VisualContactPoint.h @@ -28,6 +28,7 @@ // Libraries #include "openglframework.h" +#include "Line.h" const float VISUAL_CONTACT_POINT_RADIUS = 0.2f; @@ -56,15 +57,30 @@ class VisualContactPoint : public openglframework::Object3D { /// Vertex Array Object for the vertex data static openglframework::VertexArrayObject mVAO; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVerticesNormalLine; + + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAONormalLine; + /// True if static data (VBO, VAO) has been created already static bool mStaticDataCreated; + // Two end-points of the contact normal line + openglframework::Vector3 mContactNormalLinePoints[2]; + /// Color openglframework::Color mColor; - // Create the Vertex Buffer Objects used to render with OpenGL. + // Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL. static void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render the contact normal line with OpenGL. + void createContactNormalLineVBOAndVAO(); + + /// Render the contact normal line + void renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); + // -------------------- Methods -------------------- // public : @@ -72,8 +88,8 @@ class VisualContactPoint : public openglframework::Object3D { // -------------------- Methods -------------------- // /// Constructor - VisualContactPoint(const openglframework::Vector3& position, - const std::string &meshFolderPath); + VisualContactPoint(const openglframework::Vector3& position, const std::string &meshFolderPath, + const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color); /// Destructor ~VisualContactPoint(); @@ -84,7 +100,7 @@ class VisualContactPoint : public openglframework::Object3D { /// Destroy the mesh for the contact points static void destroyStaticData(); - /// Render the sphere at the correct position and with the correct orientation + /// Render the sphere contact point at the correct position and with the correct orientation void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); }; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp old mode 100644 new mode 100755 index b96620b6..21a0b51a --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -34,7 +34,7 @@ using namespace collisiondetectionscene; CollisionDetectionScene::CollisionDetectionScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), mContactManager(mPhongShader, mMeshFolderPath), - mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { + mAreNormalsDisplayed(false) { mSelectedShapeIndex = 0; mIsContactPointsDisplayed = true; @@ -60,7 +60,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere1->setColor(mGreyColorDemo); mSphere1->setSleepingColor(mRedColorDemo); - /* // ---------- Sphere 2 ---------- // openglframework::Vector3 position2(4, 0, 0); @@ -93,17 +92,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mCapsule2->setColor(mGreyColorDemo); mCapsule2->setSleepingColor(mRedColorDemo); -*/ - // ---------- Box 1 ---------- // - openglframework::Vector3 position5(4, 5, 0); - - // Create a box and a corresponding collision body in the dynamics world - mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mBox1); - - // Set the color - mBox1->setColor(mGreyColorDemo); - mBox1->setSleepingColor(mRedColorDemo); // ---------- Cone ---------- // //openglframework::Vector3 position4(0, 0, 0); @@ -158,9 +146,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) //mHeightField->setColor(mGreyColorDemo); //mHeightField->setSleepingColor(mRedColorDemo); - // Create the VBO and VAO to render the lines - //createVBOAndVAO(mPhongShader); - mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); } @@ -172,9 +157,6 @@ void CollisionDetectionScene::reset() { // Destructor CollisionDetectionScene::~CollisionDetectionScene() { - // Destroy the shader - mPhongShader.destroy(); - // Destroy the box rigid body from the dynamics world //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); //delete mBox; @@ -183,7 +165,6 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); delete mSphere1; - /* mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); delete mSphere2; @@ -192,10 +173,6 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); delete mCapsule2; - */ - - mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody()); - delete mBox1; /* // Destroy the corresponding rigid body from the dynamics world @@ -246,10 +223,6 @@ CollisionDetectionScene::~CollisionDetectionScene() { // Destroy the collision world delete mCollisionWorld; - - // Destroy the VBOs and VAO - mVBOVertices.destroy(); - mVAO.destroy(); } // Update the physics world (take a simulation step) @@ -272,57 +245,11 @@ void CollisionDetectionScene::update() { void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - /* - // Bind the VAO - mVAO.bind(); - - // Bind the shader - shader.bind(); - - mVBOVertices.bind(); - - // Set the model to camera matrix - const Matrix4 localToCameraMatrix = Matrix4::identity(); - shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); - - // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); - - glDisableVertexAttribArray(vertexPositionLoc); - - mVBOVertices.unbind(); - - // Unbind the VAO - mVAO.unbind(); - - shader.unbind(); - */ - // Render the shapes if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - /* if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - */ - if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); @@ -338,34 +265,6 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, shader.unbind(); } -// Create the Vertex Buffer Objects used to render with OpenGL. -/// We create two VBOs (one for vertices and one for indices) -void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) { - - // Bind the shader - shader.bind(); - - // Create the VBO for the vertices data - mVBOVertices.create(); - mVBOVertices.bind(); - size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3); - mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW); - mVBOVertices.unbind(); - - // Create the VAO for both VBOs - mVAO.create(); - mVAO.bind(); - - // Bind the VBO of vertices - mVBOVertices.bind(); - - // Unbind the VAO - mVAO.unbind(); - - // Unbind the shader - shader.unbind(); -} - void CollisionDetectionScene::selectNextShape() { int previousIndex = mSelectedShapeIndex; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h old mode 100644 new mode 100755 index 01df3df8..6eeeb25b --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -58,7 +58,7 @@ const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; const int NB_BODIES = 9; -// Raycast manager +// Contact manager class ContactManager : public rp3d::CollisionCallback { private: @@ -66,16 +66,12 @@ class ContactManager : public rp3d::CollisionCallback { /// All the visual contact points std::vector mContactPoints; - /// All the normals at contact points - std::vector mNormals; - /// Contact point mesh folder path std::string mMeshFolderPath; public: - ContactManager(openglframework::Shader& shader, - const std::string& meshFolderPath) + ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath) : mMeshFolderPath(meshFolderPath) { } @@ -87,22 +83,20 @@ class ContactManager : public rp3d::CollisionCallback { rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo(); while (contactPointInfo != nullptr) { + // Contact normal + rp3d::Vector3 normal = contactPointInfo->normal; + openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); + rp3d::Vector3 point1 = contactPointInfo->localPoint1; point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1)); - + mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); rp3d::Vector3 point2 = contactPointInfo->localPoint2; point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2)); - - // Create a line to display the normal at hit point - rp3d::Vector3 n = contactPointInfo->normal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position1, position1 + normal); - mNormals.push_back(normalLine); + mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); contactPointInfo = contactPointInfo->next; } @@ -111,13 +105,6 @@ class ContactManager : public rp3d::CollisionCallback { void resetPoints() { mContactPoints.clear(); - - // Destroy all the normals - for (std::vector::iterator it = mNormals.begin(); - it != mNormals.end(); ++it) { - delete (*it); - } - mNormals.clear(); } std::vector getContactPoints() const { @@ -146,7 +133,6 @@ class CollisionDetectionScene : public SceneDemo { Sphere* mSphere2; Capsule* mCapsule1; Capsule* mCapsule2; - Box* mBox1; //Cone* mCone; //Cylinder* mCylinder; //Capsule* mCapsule; @@ -162,18 +148,6 @@ class CollisionDetectionScene : public SceneDemo { /// Collision world used for the physics simulation rp3d::CollisionWorld* mCollisionWorld; - /// All the points to render the lines - std::vector mLinePoints; - - /// Vertex Buffer Object for the vertices data - openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Array Object for the vertex data - openglframework::VertexArrayObject mVAO; - - /// Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); - /// Select the next shape void selectNextShape(); diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index c403ebb2..d420073f 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -124,7 +124,7 @@ RaycastScene::RaycastScene(const std::string& name) createLines(); // Create the VBO and VAO to render the lines - createVBOAndVAO(mPhongShader); + createVBOAndVAO(); changeBody(); } @@ -199,9 +199,6 @@ void RaycastScene::reset() { // Destructor RaycastScene::~RaycastScene() { - // Destroy the shader - mPhongShader.destroy(); - // Destroy the box rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); delete mBox; @@ -293,40 +290,33 @@ void RaycastScene::update() { } // Render the scene -void RaycastScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the VAO mVAO.bind(); // Bind the shader - shader.bind(); + mColorShader.bind(); mVBOVertices.bind(); // Set the model to camera matrix const Matrix4 localToCameraMatrix = Matrix4::identity(); - shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + mColorShader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); + mColorShader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); + openglframework::Vector4 color(1, 0.55, 0, 1); + mColorShader.setVector4Uniform("vertexColor", color, false); // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint vertexPositionLoc = mColorShader.getAttribLocation("vertexPosition"); glEnableVertexAttribArray(vertexPositionLoc); glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); + glDrawArrays(GL_LINES, 0, mLinePoints.size() * 2); glDisableVertexAttribArray(vertexPositionLoc); @@ -335,7 +325,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, // Unbind the VAO mVAO.unbind(); - shader.unbind(); + mColorShader.unbind(); // Render the shapes if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); @@ -345,16 +335,11 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - shader.unbind(); } // Create the Vertex Buffer Objects used to render with OpenGL. /// We create two VBOs (one for vertices and one for indices) -void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { - - // Bind the shader - shader.bind(); +void RaycastScene::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); @@ -372,9 +357,6 @@ void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { // Unbind the VAO mVAO.unbind(); - - // Unbind the shader - shader.unbind(); } // Called when a keyboard event occurs diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index be698e53..39b73799 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -82,13 +82,16 @@ class RaycastManager : public rp3d::RaycastCallback { } virtual rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) override { + + rp3d::Vector3 n = raycastInfo.worldNormal; + openglframework::Vector3 normal(n.x, n.y, n.z); + rp3d::Vector3 hitPos = raycastInfo.worldPoint; openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z); - mHitPoints.push_back(ContactPoint(position)); + mHitPoints.push_back(ContactPoint(position, normal, openglframework::Color::red())); // Create a line to display the normal at hit point - rp3d::Vector3 n = raycastInfo.worldNormal; - openglframework::Vector3 normal(n.x, n.y, n.z); + // TODO : Remove the mNormals because the VisualContactPoint is now able to display the contact normal on its own Line* normalLine = new Line(position, position + normal); mNormals.push_back(normalLine); @@ -134,8 +137,6 @@ class RaycastScene : public SceneDemo { /// True if the hit points normals are displayed bool mAreNormalsDisplayed; - /// Raycast manager - /// All objects on the scene Box* mBox; Sphere* mSphere; @@ -161,7 +162,7 @@ class RaycastScene : public SceneDemo { void createLines(); // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); + void createVBOAndVAO(); public: diff --git a/testbed/shaders/color.frag b/testbed/shaders/color.frag new file mode 100644 index 00000000..7ba710f0 --- /dev/null +++ b/testbed/shaders/color.frag @@ -0,0 +1,38 @@ +#version 330 + +/******************************************************************************** +* OpenGL-Framework * +* Copyright (c) 2015 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Uniform variables +uniform vec4 vertexColor; // Vertex color + +// Out variable +out vec4 color; // Output color + +void main() { + + // Compute the final color + color = vertexColor; +} diff --git a/testbed/shaders/color.vert b/testbed/shaders/color.vert new file mode 100644 index 00000000..72083abc --- /dev/null +++ b/testbed/shaders/color.vert @@ -0,0 +1,43 @@ +#version 330 + +/******************************************************************************** +* OpenGL-Framework * +* Copyright (c) 2015 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Uniform variables +uniform mat4 localToWorldMatrix; // Local-space to world-space matrix +uniform mat4 worldToCameraMatrix; // World-space to camera-space matrix +uniform mat4 projectionMatrix; // Projection matrix + +// In variables +in vec4 vertexPosition; + +void main() { + + // Compute the vertex position + vec4 positionCameraSpace = worldToCameraMatrix * localToWorldMatrix * vertexPosition; + + // Compute the clip-space vertex coordinates + gl_Position = projectionMatrix * positionCameraSpace; +} diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index d51ffec1..aa29b9e5 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -34,9 +34,12 @@ struct ContactPoint { public: openglframework::Vector3 point; + openglframework::Vector3 normal; + openglframework::Color color; /// Constructor - ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) { + ContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint) + : point(pointWorld), normal(normalWorld), color(colorPoint) { } }; diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 1906d9f4..2cb3e440 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -46,6 +46,7 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa mDepthShader("shaders/depth.vert", "shaders/depth.frag"), mPhongShader("shaders/phong.vert", "shaders/phong.frag"), mQuadShader("shaders/quad.vert", "shaders/quad.frag"), + mColorShader("shaders/color.vert", "shaders/color.frag"), mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") { shadowMapTextureLevel++; @@ -84,6 +85,11 @@ SceneDemo::~SceneDemo() { mFBOShadowMap.destroy(); mVBOQuad.destroy(); + mDepthShader.destroy(); + mPhongShader.destroy(); + mQuadShader.destroy(); + mColorShader.destroy(); + // Destroy the contact points removeAllContactPoints(); @@ -156,7 +162,7 @@ void SceneDemo::render() { if (mIsShadowMappingEnabled) mShadowMapTexture.bind(); const GLuint textureUnit = 0; - // Set the variables of the shader + // Set the variables of the phong shader mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix); mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix); @@ -166,6 +172,12 @@ void SceneDemo::render() { mPhongShader.setIntUniform("shadowMapSampler", textureUnit); mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled); mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); + mPhongShader.unbind(); + + // Set the variables of the color shader + mColorShader.bind(); + mColorShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); + mColorShader.unbind(); // Set the viewport to render the scene glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); @@ -289,20 +301,19 @@ void SceneDemo::updateContactPoints() { for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { // Create a visual contact point for rendering - VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath); + VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath, it->point + it->normal, it->color); mContactPoints.push_back(point); } } } // Render the contact points -void SceneDemo::renderContactPoints(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Render all the raycast hit points for (std::vector::iterator it = mContactPoints.begin(); it != mContactPoints.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(mColorShader, worldToCameraMatrix); } } @@ -335,7 +346,9 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); - ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z)); + rp3d::Vector3 normalWorld = contactPoint->getNormal(); + openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); + ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); contactPoints.push_back(contact); } diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h index c64c8a9b..f7b5f7ce 100644 --- a/testbed/src/SceneDemo.h +++ b/testbed/src/SceneDemo.h @@ -73,6 +73,9 @@ class SceneDemo : public Scene { /// Phong shader openglframework::Shader mPhongShader; + /// Constant color shader + openglframework::Shader mColorShader; + // TODO : Delete this openglframework::Shader mQuadShader; From 730b68787735cf246a23b18c1ed78332520b026c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 29 May 2017 08:32:10 +0200 Subject: [PATCH 022/105] Working on temporal coherence in SAT (polyhedron vs polyhedron) --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 13 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 14 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 48 +++ ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 71 ++++ .../narrowphase/DefaultCollisionDispatch.cpp | 9 + .../narrowphase/DefaultCollisionDispatch.h | 16 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 11 +- .../narrowphase/SAT/SATAlgorithm.cpp | 333 +++++++++++++----- src/collision/narrowphase/SAT/SATAlgorithm.h | 15 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 12 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- src/engine/OverlappingPair.cpp | 3 +- src/engine/OverlappingPair.h | 68 +++- 14 files changed, 502 insertions(+), 115 deletions(-) create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cacac60..2e3be144 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 908c5bf5..9ce02f9d 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -241,7 +241,7 @@ void CollisionDetection::computeNarrowPhase() { // If there is no collision algorithm between those two kinds of shapes, skip it if (narrowPhaseAlgorithm != nullptr) { - + // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. @@ -268,7 +268,15 @@ void CollisionDetection::computeNarrowPhase() { // Trigger a callback event for the new contact if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo); + + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; } + else { + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false; + } + + // The previous frame collision info is now valid + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; @@ -834,6 +842,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Report the contact to the user callback->notifyContact(collisionInfo); } + + // The previous frame collision info is now valid + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 5e8dd5f9..142a7fc9 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -44,6 +44,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na SATAlgorithm satAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -104,9 +107,11 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na break; } - } + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + // Return true return true; } @@ -115,7 +120,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + + return isColliding; } return false; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..16b9edf5 --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,48 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" +#include "GJK/GJKAlgorithm.h" +#include "SAT/SATAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between two convex polyhedra +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm; + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + + return isColliding; +} diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..ed76d6fc --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two convex polyhedra. + */ +class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between two convex polyhedra + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 0a1e498c..df1828ad 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -56,6 +56,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { return &mSphereVsConvexPolyhedronAlgorithm; } + // Capsule vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mCapsuleVsConvexPolyhedronAlgorithm; + } + // Convex Polyhedron vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON && + shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mConvexPolyhedronVsConvexPolyhedronAlgorithm; + } return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 7c5d77ea..7b3945d3 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -33,6 +33,8 @@ #include "SphereVsConvexPolyhedronAlgorithm.h" #include "SphereVsCapsuleAlgorithm.h" #include "CapsuleVsCapsuleAlgorithm.h" +#include "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" #include "GJK/GJKAlgorithm.h" namespace reactphysics3d { @@ -50,14 +52,20 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Sphere collision algorithm SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - /// Sphere vs Convex Mesh collision algorithm - SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; + /// Capsule vs Capsule collision algorithm + CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; /// Sphere vs Capsule collision algorithm SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; - /// Capsule vs Capsule collision algorithm - CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; + /// Sphere vs Convex Polyhedron collision algorithm + SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; + + /// Capsule vs Convex Polyhedron collision algorithm + CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm; + + /// Convex Polyhedron vs Convex Polyhedron collision algorithm + ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm; public: diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 6be90769..4f7e1177 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -93,7 +93,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro VoronoiSimplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis(); + Vector3 v; + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) { + v = lastFrameInfo.gjkSeparatingAxis; + } + else { + v.setAllValues(0, 1, 0); + } // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -113,7 +120,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v; // No intersection, we return return GJKResult::SEPARATED; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index c82256f3..6e85e303 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -29,6 +29,7 @@ #include "collision/PolyhedronMesh.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" +#include "engine/OverlappingPair.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -329,7 +330,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) const { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -344,94 +346,235 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP uint minFaceIndex = 0; bool isMinPenetrationFaceNormal = false; bool isMinPenetrationFaceNormalPolyhedron1 = false; + uint minSeparatingEdge1Index, minSeparatingEdge2Index; Vector3 separatingEdge1A, separatingEdge1B; Vector3 separatingEdge2A, separatingEdge2B; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - // Test all the face normals of the polyhedron 1 for separating axis - uint faceIndex; - decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); - if (penetrationDepth <= decimal(0.0)) { + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // We have found a separating axis - return false; - } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { - isMinPenetrationFaceNormal = true; - minPenetrationDepth = penetrationDepth; - minFaceIndex = faceIndex; - isMinPenetrationFaceNormalPolyhedron1 = true; - } + // True if the shapes were overlapping in the previous frame and are + // still overlapping on the same axis in this frame + bool isTemporalCoherenceValid = false; - // Test all the face normals of the polyhedron 2 for separating axis - penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); - if (penetrationDepth <= decimal(0.0)) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) { - // We have found a separating axis - return false; - } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { - isMinPenetrationFaceNormal = true; - minPenetrationDepth = penetrationDepth; - minFaceIndex = faceIndex; - isMinPenetrationFaceNormalPolyhedron1 = false; - } + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis - for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // Get an edge of polyhedron 1 - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); - - const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); - const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); - const Vector3 edge1Direction = edge1B - edge1A; - - for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { - - // Get an edge of polyhedron 2 - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); - - const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); - const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); - const Vector3 edge2Direction = edge2B - edge2A; - - // If the two edges build a minkowski face (and the cross product is - // therefore a candidate for separating axis - if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { - - Vector3 separatingAxisPolyhedron2Space; - - // Compute the penetration depth - decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), - edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 1 + if (lastFrameInfo.satIsAxisFacePolyhedron1) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + lastFrameInfo.satMinAxisFaceIndex); + // If the previous axis is a separating axis if (penetrationDepth <= decimal(0.0)) { - // We have found a separating axis + // Return no collision return false; } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { minPenetrationDepth = penetrationDepth; - isMinPenetrationFaceNormalPolyhedron1 = false; - isMinPenetrationFaceNormal = false; - separatingEdge1A = edge1A; - separatingEdge1B = edge1B; - separatingEdge2A = edge2A; - separatingEdge2B = edge2B; - minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + } + else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 2 + + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, + lastFrameInfo.satMinAxisFaceIndex); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; } + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + } + else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges + + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index); + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index); + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; + minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + } } } } + // We the shapes are still overlapping in the same axis as in + // the previous frame, we skip the whole SAT algorithm + if (isTemporalCoherenceValid) { + + // Test all the face normals of the polyhedron 1 for separating axis + uint faceIndex; + decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = true; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + + // Test all the face normals of the polyhedron 2 for separating axis + penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = true; + lastFrameInfo.satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + + // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis + for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + + // Get an edge of polyhedron 1 + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + + for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { + + // Get an edge of polyhedron 2 + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinEdge1Index = i; + lastFrameInfo.satMinEdge2Index = j; + + // We have found a separating axis + return false; + } + + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormalPolyhedron1 = false; + isMinPenetrationFaceNormal = false; + minSeparatingEdge1Index = i; + minSeparatingEdge2Index = j; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + + } + } + } + } + + // Here we know the shapes are overlapping on a given minimum separating axis. + // Now, we will clip the shapes along this axis to find the contact points + assert(minPenetrationDepth > decimal(0.0)); assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal); - // If the separation axis is a face normal + // If the minimum separating axis is a face normal if (isMinPenetrationFaceNormal) { const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; @@ -439,6 +582,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + assert(minPenetrationDepth > decimal(0.0)); + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; @@ -491,8 +636,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP } while (currentEdgeIndex != firstEdgeIndex); + assert(planesNormals.size() > 0); + assert(planesNormals.size() == planesPoints.size()); + // Clip the reference faces with the adjacent planes of the reference face std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + assert(clipPolygonVertices.size() > 0); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex); @@ -514,6 +663,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } } + + lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; } else { // If we have an edge vs edge contact @@ -531,6 +684,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // Create the contact point contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index; + lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index; } return true; @@ -583,34 +741,47 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V return -axis.dot(edge2A - edge1A); } +// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron +decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const { + + HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex); + + // Convert the face normal into the local-space of polyhedron 2 + const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; + + // Get the support point of polyhedron 2 in the inverse direction of face normal + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); + + // Compute the penetration depth + const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); + decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + + return penetrationDepth; +} + // Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case -decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, - const ConvexPolyhedronShape* polyhedron2, - const Transform& polyhedron1ToPolyhedron2, - uint& minFaceIndex) const { +decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint& minFaceIndex) const { decimal minPenetrationDepth = DECIMAL_LARGEST; // For each face of the first polyhedron for (uint f = 0; f < polyhedron1->getNbFaces(); f++) { - HalfEdgeStructure::Face face = polyhedron1->getFace(f); - - // Get the face normal - const Vector3 faceNormal = polyhedron1->getFaceNormal(f); - - // Convert the face normal into the local-space of polyhedron 2 - const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; - - // Get the support point of polyhedron 2 in the inverse direction of face normal - const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); - - // Compute the penetration depth - const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); - decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, f); // If the penetration depth is negative, we have found a separating axis if (penetrationDepth <= decimal(0.0)) { + minFaceIndex = f; return penetrationDepth; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index a95abe31..3512ef4d 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -66,6 +66,17 @@ class SATAlgorithm { const Vector3& edge1Direction, const Vector3& edge2Direction, Vector3& outSeparatingAxis) const; + /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron + decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const; + + + /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case + decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; + public : // -------------------- Methods -------------------- // @@ -101,10 +112,6 @@ class SATAlgorithm { /// Test collision between two convex meshes bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - - /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case - decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, - const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 708d9f8d..bb43bd33 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -31,7 +31,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Compute the narrow-phase collision detection a sphere and a convex polyhedron +// Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, @@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -53,7 +56,12 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + + return isColliding; } return false; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index c1fd3e55..f2f8d6e5 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; - /// Compute the narrow-phase collision detection a sphere and a convex polyhedron + /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 1f5d894e..ce628b7d 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -32,7 +32,6 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int nbMaxContactManifolds, PoolAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), - mCachedSeparatingAxis(0.0, 1.0, 0.0) { + : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) { } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index c34b91b7..ce028f9c 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -37,6 +37,51 @@ namespace reactphysics3d { // Type for the overlapping pair ID using overlappingpairid = std::pair; +// Structure LastFrameCollisionInfo +/** + * This structure contains collision info about the last frame. + * This is used for temporal coherence between frames. + */ +struct LastFrameCollisionInfo { + + /// True if we have information about the previous frame + bool isValid; + + /// True if the two shapes were colliding in the previous frame + bool wasColliding; + + /// True if we were using GJK algorithm to check for collision in the previous frame + bool wasUsingGJK; + + /// True if we were using SAT algorithm to check for collision in the previous frame + bool wasUsingSAT; + + /// True if there was a narrow-phase collision + /// in the previous frame + bool wasCollidingLastFrame; + + // ----- GJK Algorithm ----- + + /// Previous separating axis + Vector3 gjkSeparatingAxis; + + // SAT Algorithm + bool satIsAxisFacePolyhedron1; + bool satIsAxisFacePolyhedron2; + uint satMinAxisFaceIndex; + uint satMinEdge1Index; + uint satMinEdge2Index; + + /// Constructor + LastFrameCollisionInfo() { + + isValid = false; + wasColliding = false; + wasUsingSAT = false; + wasUsingGJK = false; + } +}; + // Class OverlappingPair /** * This class represents a pair of two proxy collision shapes that are overlapping @@ -54,8 +99,8 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Cached previous separating axis - Vector3 mCachedSeparatingAxis; + /// Collision information about the last frame (for temporal coherence) + LastFrameCollisionInfo mLastFrameCollisionInfo; public: @@ -83,11 +128,8 @@ class OverlappingPair { /// Add a contact manifold void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); - /// Return the cached separating axis - Vector3 getCachedSeparatingAxis() const; - - /// Set the cached separating axis - void setCachedSeparatingAxis(const Vector3& axis); + /// Return the last frame collision info + LastFrameCollisionInfo& getLastFrameCollisionInfo(); /// Return the number of contacts in the cache uint getNbContactPoints() const; @@ -124,17 +166,11 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& conta mContactManifoldSet.addContactManifold(contactManifoldInfo); } -// Return the cached separating axis -inline Vector3 OverlappingPair::getCachedSeparatingAxis() const { - return mCachedSeparatingAxis; +// Return the last frame collision info +inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() { + return mLastFrameCollisionInfo; } -// Set the cached separating axis -inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { - mCachedSeparatingAxis = axis; -} - - // Return the number of contact points in the contact manifold inline uint OverlappingPair::getNbContactPoints() const { return mContactManifoldSet.getTotalNbContactPoints(); From 6b0ba1cfbb217fd3c90394e48ba25d5cf3e2dc59 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 29 May 2017 22:30:30 +0200 Subject: [PATCH 023/105] Fix issues in collision detection --- src/collision/CollisionDetection.cpp | 32 +++++++++++++------ src/collision/CollisionDetection.h | 2 +- .../narrowphase/SAT/SATAlgorithm.cpp | 14 ++++---- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 2 -- 4 files changed, 31 insertions(+), 19 deletions(-) mode change 100644 => 100755 src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 9ce02f9d..dc78d59e 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -407,10 +407,10 @@ void CollisionDetection::clearContactPoints() { } // Compute the middle-phase collision detection between two proxy shapes -NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2) { +NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { - // Create a temporary overlapping pair - OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator); + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); // ------------------------------------------------------- @@ -424,7 +424,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(), + narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), shape2->getCachedCollisionData()); @@ -436,7 +436,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo); } return narrowPhaseInfo; @@ -503,8 +503,11 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); + // Create a temporary overlapping pair + OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); bool isColliding = false; @@ -591,8 +594,11 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); bool isColliding = false; @@ -666,8 +672,11 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Test if the AABBs of the two proxy shapes overlap if (aabb1.testCollision(aabb2)) { + // Create a temporary overlapping pair + OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); @@ -747,8 +756,11 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { @@ -816,7 +828,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 29200ec7..3fe88812 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -136,7 +136,7 @@ class CollisionDetection { NarrowPhaseInfo** firstNarrowPhaseInfo); /// Compute the middle-phase collision detection between two proxy shapes - NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2); + NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair); public : diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 6e85e303..91db1383 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -48,9 +48,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; - assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the capsule collision shapes const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); @@ -116,9 +117,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; - assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); // Get the collision shapes const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp old mode 100644 new mode 100755 index cd437d6e..86255c2b --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -39,8 +39,6 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); - assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); - assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); // Get the collision shapes const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); From 3ec8dddd9198c976899041a822125c6f5faafa29 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 29 May 2017 22:31:33 +0200 Subject: [PATCH 024/105] Add box shapes in collision detection scene of testbed application --- .../CollisionDetectionScene.cpp | 46 +++++++++++++++---- .../CollisionDetectionScene.h | 2 + 2 files changed, 40 insertions(+), 8 deletions(-) diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 21a0b51a..6351028a 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -50,10 +50,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCollisionWorld = new rp3d::CollisionWorld(); // ---------- Sphere 1 ---------- // - openglframework::Vector3 position1(0, 0, 0); + openglframework::Vector3 position1(12, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath); + mSphere1 = new Sphere(4, position1, mCollisionWorld, mMeshFolderPath); mAllShapes.push_back(mSphere1); // Set the color @@ -61,10 +61,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere1->setSleepingColor(mRedColorDemo); // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(4, 0, 0); + openglframework::Vector3 position2(0, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath); + mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath); mAllShapes.push_back(mSphere2); // Set the color @@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere2->setSleepingColor(mRedColorDemo); // ---------- Capsule 1 ---------- // - openglframework::Vector3 position3(4, 0, 0); + openglframework::Vector3 position3(8, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); @@ -83,7 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule1->setSleepingColor(mRedColorDemo); // ---------- Capsule 2 ---------- // - openglframework::Vector3 position4(-4, 0, 0); + openglframework::Vector3 position4(-8, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath); @@ -93,6 +93,28 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule2->setColor(mGreyColorDemo); mCapsule2->setSleepingColor(mRedColorDemo); + // ---------- Box 1 ---------- // + openglframework::Vector3 position5(0, -12, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mBox1); + + // Set the color + mBox1->setColor(mGreyColorDemo); + mBox1->setSleepingColor(mRedColorDemo); + + // ---------- Box 2 ---------- // + openglframework::Vector3 position6(0, 12, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mBox2); + + // Set the color + mBox2->setColor(mGreyColorDemo); + mBox2->setSleepingColor(mRedColorDemo); + // ---------- Cone ---------- // //openglframework::Vector3 position4(0, 0, 0); @@ -174,6 +196,12 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); delete mCapsule2; + mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody()); + delete mBox1; + + mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody()); + delete mBox2; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -250,6 +278,8 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); @@ -286,8 +316,8 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i return true; } - float stepDist = 0.5f; - float stepAngle = 20 * (3.14f / 180.0f); + float stepDist = 0.2f; + float stepAngle = 15 * (3.14f / 180.0f); if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 6eeeb25b..51a05412 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -133,6 +133,8 @@ class CollisionDetectionScene : public SceneDemo { Sphere* mSphere2; Capsule* mCapsule1; Capsule* mCapsule2; + Box* mBox1; + Box* mBox2; //Cone* mCone; //Cylinder* mCylinder; //Capsule* mCapsule; From b1597c508fcfae1cd733ece9bfabfb65a1c59694 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 31 May 2017 07:36:39 +0200 Subject: [PATCH 025/105] Working on temporal coherence in SAT algorithm --- .../narrowphase/SAT/SATAlgorithm.cpp | 385 +++++++++++++----- src/collision/narrowphase/SAT/SATAlgorithm.h | 19 + 2 files changed, 305 insertions(+), 99 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 91db1383..767aff8a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -71,27 +71,64 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* decimal minPenetrationDepth = DECIMAL_LARGEST; uint minFaceIndex = 0; - // For each face of the convex mesh - for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + // True if the shapes were overlapping in the previous frame and are + // still overlapping on the same axis in this frame + bool isTemporalCoherenceValid = false; - // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(f); + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // Get the face normal - const Vector3 faceNormal = polyhedron->getFaceNormal(f); + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter; - decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius(); + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // If the penetration depth is negative, we have found a separating axis + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + sphere, sphereCenter); + + // If the previous axis is a separating axis if (penetrationDepth <= decimal(0.0)) { + + // Return no collision return false; } - // Check if we have found a new minimum penetration axis - if (penetrationDepth < minPenetrationDepth) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + minPenetrationDepth = penetrationDepth; - minFaceIndex = f; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + } + } + + // We the shapes are still overlapping in the same axis as in + // the previous frame, we skip the whole SAT algorithm + if (!isTemporalCoherenceValid) { + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satMinAxisFaceIndex = f; + + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } } } @@ -109,9 +146,27 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + return true; } +// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction +decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron, + const SphereShape* sphere, const Vector3& sphereCenter) const { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex); + + Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter; + decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius(); + + return penetrationDepth; +} + // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { @@ -131,6 +186,11 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; + // Minimum penetration depth decimal minPenetrationDepth = DECIMAL_LARGEST; uint minFaceIndex = 0; @@ -140,80 +200,151 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* Vector3 separatingPolyhedronEdgeVertex1; Vector3 separatingPolyhedronEdgeVertex2; - // For each face of the convex mesh - for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + // True if the shapes were overlapping in the previous frame and are + // still overlapping on the same axis in this frame + bool isTemporalCoherenceValid = false; - // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(f); + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // Get the face normal - const Vector3 faceNormal = polyhedron->getFaceNormal(f); + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) - const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; - const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr); - const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); - const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; - const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // If the penetration depth is negative, we have found a separating axis - if (penetrationDepth <= decimal(0.0)) { - return false; + // If the previous minimum separation axis was a face normal of the polyhedron + if (lastFrameInfo.satIsAxisFacePolyhedron1) { + + Vector3 outFaceNormalCapsuleSpace; + + // Compute the penetration depth along the polyhedron face normal direction + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + capsuleShape, polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + } } + else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron - // Check if we have found a new minimum penetration axis - if (penetrationDepth < minPenetrationDepth) { - minPenetrationDepth = penetrationDepth; - minFaceIndex = f; - isMinPenetrationFaceNormal = true; - separatingAxisCapsuleSpace = faceNormalCapsuleSpace; + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + + Vector3 outAxis; + + // Compute the penetration depth along this axis + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minEdgeIndex = lastFrameInfo.satMinEdge1Index; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = outAxis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } } } - // Compute the end-points of the inner segment of the capsule - const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; + // We the shapes are still overlapping in the same axis as in + // the previous frame, we skip the whole SAT algorithm + if (!isTemporalCoherenceValid) { - // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron - for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); - const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); - const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); - const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + Vector3 outFaceNormalCapsuleSpace; - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); - const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); - const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + // Compute the penetration depth + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape, + polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); - // Check using the Gauss Map if this edge cross product can be as separating axis - if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { - // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) - Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace); + lastFrameInfo.satIsAxisFacePolyhedron1 = true; + lastFrameInfo.satMinAxisFaceIndex = f; - // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment - if (axis.lengthSquare() >= decimal(0.00001)) { + return false; + } - const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid(); - const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1; + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + } + } - // Swap axis direction if necessary such that it points out of the polyhedron - if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) { - axis = -axis; - } + // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron + for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { - axis.normalize(); + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); + const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + + // Check using the Gauss Map if this edge cross product can be as separating axis + if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + + Vector3 outAxis; // Compute the penetration depth - const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr); - const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; - const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis); + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); // If the penetration depth is negative, we have found a separating axis if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satMinEdge1Index = e; + return false; } @@ -222,12 +353,13 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* minPenetrationDepth = penetrationDepth; minEdgeIndex = e; isMinPenetrationFaceNormal = false; - separatingAxisCapsuleSpace = axis; + separatingAxisCapsuleSpace = outAxis; separatingPolyhedronEdgeVertex1 = edgeVertex1; separatingPolyhedronEdgeVertex2 = edgeVertex2; } } } + } // Convert the inner capsule segment points into the polyhedron local-space @@ -246,6 +378,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, contactManifoldInfo, isCapsuleShape1); + + lastFrameInfo.satIsAxisFacePolyhedron1 = true; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; } else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment @@ -264,11 +399,68 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satMinEdge1Index = minEdgeIndex; } return true; } +// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment +decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule, + const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1, + const Vector3& edgeDirectionCapsuleSpace, + const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const { + + decimal penetrationDepth = DECIMAL_LARGEST; + + // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) + outAxis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace); + + // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment + if (outAxis.lengthSquare() >= decimal(0.00001)) { + + const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid(); + const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1; + + // Swap axis direction if necessary such that it points out of the polyhedron + if (outAxis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) { + outAxis = -outAxis; + } + + outAxis.normalize(); + + // Compute the penetration depth + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis, nullptr); + const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; + penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis); + } + + return penetrationDepth; +} + +// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction +decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron, + const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform, + Vector3& outFaceNormalCapsuleSpace) const { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex); + + // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) + outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr); + const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); + const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + + return penetrationDepth; +} + // Compute the two contact points between a polyhedron and a capsule when the separating // axis is a face normal of the polyhedron void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, @@ -424,47 +616,42 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index); HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index); - // If the two edges build a minkowski face (and the cross product is - // therefore a candidate for separating axis - if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + Vector3 separatingAxisPolyhedron2Space; - Vector3 separatingAxisPolyhedron2Space; + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; - const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); - const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); - const Vector3 edge1Direction = edge1B - edge1A; - const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); - const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); - const Vector3 edge2Direction = edge2B - edge2A; + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); - // Compute the penetration depth - decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), - edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // Return no collision + return false; + } - // Return no collision - return false; - } + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; + if (isTemporalCoherenceValid) { - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - isMinPenetrationFaceNormal = false; - isMinPenetrationFaceNormalPolyhedron1 = false; - minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; - minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; - separatingEdge1A = edge1A; - separatingEdge1B = edge1B; - separatingEdge2A = edge2A; - separatingEdge2B = edge2B; - minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; - } + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; + minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; } } } @@ -472,7 +659,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // We the shapes are still overlapping in the same axis as in // the previous frame, we skip the whole SAT algorithm - if (isTemporalCoherenceValid) { + if (!isTemporalCoherenceValid) { // Test all the face normals of the polyhedron 1 for separating axis uint faceIndex; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 3512ef4d..7a002fce 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -31,9 +31,13 @@ #include "collision/NarrowPhaseInfo.h" #include "collision/shapes/ConvexPolyhedronShape.h" + /// ReactPhysics3D namespace namespace reactphysics3d { +class CapsuleShape; +class SphereShape; + // Class SATAlgorithm class SATAlgorithm { @@ -77,6 +81,21 @@ class SATAlgorithm { decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; + /// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction + decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron, + const SphereShape* sphere, const Vector3& sphereCenter) const; + + /// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction + decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron, + const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform, + Vector3& outFaceNormalCapsuleSpace) const; + + /// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment + decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule, + const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1, + const Vector3& edgeDirectionCapsuleSpace, + const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const; + public : // -------------------- Methods -------------------- // From 95db87fd624fd2cde6b6f4f750eda8ba7a4bd205 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 5 Jun 2017 00:05:49 +0200 Subject: [PATCH 026/105] Working on contacts reduction --- CMakeLists.txt | 1 + src/collision/CollisionDetection.cpp | 13 +- src/collision/ContactManifold.h | 3 - src/collision/ContactManifoldInfo.cpp | 278 ++++++++++++++++++++++++++ src/collision/ContactManifoldInfo.h | 72 ++----- 5 files changed, 306 insertions(+), 61 deletions(-) create mode 100644 src/collision/ContactManifoldInfo.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e3be144..cba141df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ SET (REACTPHYSICS3D_SOURCES "src/body/RigidBody.cpp" "src/collision/ContactPointInfo.h" "src/collision/ContactManifoldInfo.h" + "src/collision/ContactManifoldInfo.cpp" "src/collision/broadphase/BroadPhaseAlgorithm.h" "src/collision/broadphase/BroadPhaseAlgorithm.cpp" "src/collision/broadphase/DynamicAABBTree.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index dc78d59e..0e7c8078 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -248,8 +248,8 @@ void CollisionDetection::computeNarrowPhase() { ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator); if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) { - // Reduce the number of points in the contact manifold - contactManifoldInfo.reduce(); + // Reduce the number of points in the contact manifold (if necessary) + contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform); // If it is the first contact since the pairs are overlapping if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { @@ -696,6 +696,9 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + // Reduce the number of points in the contact manifold (if necessary) + contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2, body1ProxyShape, body2ProxyShape); @@ -777,6 +780,9 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + // Reduce the number of points in the contact manifold (if necessary) + contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body, proxyShape->getBody(), bodyProxyShape, proxyShape); @@ -848,6 +854,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + // Reduce the number of points in the contact manifold (if necessary) + contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(), shape2->getBody(), shape1, shape2); diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index da323c0f..893a84a7 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -37,9 +37,6 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Constants -const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold - // Class declarations class ContactManifold; diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp new file mode 100644 index 00000000..c06785ea --- /dev/null +++ b/src/collision/ContactManifoldInfo.cpp @@ -0,0 +1,278 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactManifoldInfo.h" + +using namespace reactphysics3d; + +// Constructor +ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator) + : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {} + +// Destructor +ContactManifoldInfo::~ContactManifoldInfo() { + + // Remove all the contact points + reset(); +} + +// Add a new contact point into the manifold +void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + // Create the contact point info + ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo))) + ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + + // Add it into the linked list of contact points + contactPointInfo->next = mContactPointsList; + mContactPointsList = contactPointInfo; + + mNbContactPoints++; +} + +// Remove all the contact points +void ContactManifoldInfo::reset() { + + // Delete all the contact points in the linked list + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + ContactPointInfo* elementToDelete = element; + element = element->next; + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mContactPointsList = nullptr; + mNbContactPoints = 0; +} + +// Reduce the number of points in the contact manifold +// This is based on the technique described by Dirk Gregorius in his +// "Contacts Creation" GDC presentation +void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { + + assert(mContactPointsList != nullptr); + + // TODO : Implement this (do not forget to deallocate removed points) + + // The following algorithm only works to reduce to 4 contact points + assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); + + // If there are too many contact points in the manifold + if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { + + ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD]; + + // Compute the initial contact point we need to keep. + // The first point we keep is always the point in a given + // constant direction (in order to always have same contact points + // between frames for better stability) + + const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse(); + + // Compute the contact normal of the manifold (we use the first contact point) + // in the local-space of the first collision shape + const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal; + + // Compute a search direction + const Vector3 searchDirection(1, 1, 1); + ContactPointInfo* element = mContactPointsList; + pointsToKeep[0] = element; + decimal maxDotProduct = searchDirection.dot(element->localPoint1); + element = element->next; + while(element != nullptr) { + + decimal dotProduct = searchDirection.dot(element->localPoint1); + if (dotProduct > maxDotProduct) { + maxDotProduct = dotProduct; + pointsToKeep[0] = element; + } + element = element->next; + } + + // Compute the second contact point we need to keep. + // The second point we keep is the one farthest away from the first point. + + decimal maxDistance = decimal(0.0); + element = mContactPointsList; + while(element != nullptr) { + + if (element == pointsToKeep[0]) { + element = element->next; + continue; + } + + decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare(); + if (distance >= maxDistance) { + maxDistance = distance; + pointsToKeep[1] = element; + } + element = element->next; + } + assert(pointsToKeep[1] != nullptr); + + // Compute the third contact point we need to keep. + // The second point is the one producing the triangle with the larger area + // with first and second point. + + // We compute the most positive or most negative triangle area (depending on winding) + ContactPointInfo* thirdPointMaxArea = nullptr; + ContactPointInfo* thirdPointMinArea = nullptr; + decimal minArea = decimal(0.0); + decimal maxArea = decimal(0.0); + bool isPreviousAreaPositive = true; + element = mContactPointsList; + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1]) { + element = element->next; + continue; + } + + const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1; + const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1; + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + if (area >= maxArea) { + maxArea = area; + thirdPointMaxArea = element; + } + if (area <= minArea) { + minArea = area; + thirdPointMinArea = element; + } + element = element->next; + } + assert(minArea <= decimal(0.0)); + assert(maxArea >= decimal(0.0)); + if (maxArea > (-minArea)) { + isPreviousAreaPositive = true; + pointsToKeep[2] = thirdPointMaxArea; + } + else { + isPreviousAreaPositive = false; + pointsToKeep[2] = thirdPointMinArea; + } + assert(pointsToKeep[2] != nullptr); + + // Compute the 4th point by choosing the triangle that add the most + // triangle area to the previous triangle and has opposite sign area (opposite winding) + + decimal largestArea = decimal(0.0); // Largest area (positive or negative) + element = mContactPointsList; + + // For each remaining point + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) { + element = element->next; + continue; + } + + // For each edge of the triangle made by the first three points + for (uint i=0; i<3; i++) { + + uint edgeVertex1Index = i; + uint edgeVertex2Index = i < 2 ? i + 1 : 0; + + const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1; + const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1; + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + // We are looking at the triangle with maximal area (positive or negative). + // If the previous area is positive, we are looking at negative area now. + // If the previous area is negative, we are looking at the positive area now. + if (isPreviousAreaPositive && area < largestArea) { + largestArea = area; + pointsToKeep[3] = element; + } + else if (!isPreviousAreaPositive && area > largestArea) { + largestArea = area; + pointsToKeep[3] = element; + } + + element = element->next; + } + } + assert(pointsToKeep[3] != nullptr); + + // Delete the contact points we do not want to keep from the linked list + element = mContactPointsList; + ContactPointInfo* previousElement = nullptr; + while(element != nullptr) { + + // Skip the points we want to keep + if (element == pointsToKeep[0] || element == pointsToKeep[1] || + element == pointsToKeep[2] || element == pointsToKeep[3]) { + + previousElement = element; + element = element->next; + continue; + } + + ContactPointInfo* elementToDelete = element; + if (previousElement != nullptr) { + previousElement->next = elementToDelete->next; + } + else { + mContactPointsList = elementToDelete->next; + } + element = element->next; + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mNbContactPoints = 4; + } +} + +/// Return the largest penetration depth among the contact points +decimal ContactManifoldInfo::getLargestPenetrationDepth() const { + + decimal maxDepth = decimal(0.0); + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + + if (element->penetrationDepth > maxDepth) { + maxDepth = element->penetrationDepth; + } + + element = element->next; + } + + return maxDepth; +} + + diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index f631b9fc..a7e84a98 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -33,6 +33,8 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +// Constants +const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold // Class ContactManifoldInfo /** @@ -51,22 +53,18 @@ class ContactManifoldInfo { /// Memory allocator used to allocate contact points Allocator& mAllocator; - // -------------------- Methods -------------------- // - + /// Number of contact points in the manifold + uint mNbContactPoints; public: // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {} + ContactManifoldInfo(Allocator& allocator); /// Destructor - ~ContactManifoldInfo() { - - // Remove all the contact points - reset(); - } + ~ContactManifoldInfo(); /// Deleted copy-constructor ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; @@ -76,64 +74,26 @@ class ContactManifoldInfo { /// Add a new contact point into the manifold void addContactPoint(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { - - assert(penDepth > decimal(0.0)); - - // Create the contact point info - ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo))) - ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); - - // Add it into the linked list of contact points - contactPointInfo->next = mContactPointsList; - mContactPointsList = contactPointInfo; - } + const Vector3& localPt1, const Vector3& localPt2); /// Remove all the contact points - void reset() { - - // Delete all the contact points in the linked list - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - ContactPointInfo* elementToDelete = element; - element = element->next; - - // Delete the current element - mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); - } - - mContactPointsList = nullptr; - } + void reset(); /// Get the first contact point info of the linked list of contact points - ContactPointInfo* getFirstContactPointInfo() const { - return mContactPointsList; - } + ContactPointInfo* getFirstContactPointInfo() const; /// Reduce the number of points in the contact manifold - void reduce() { - - // TODO : Implement this (do not forget to deallocate removed points) - } + void reduce(const Transform& shape1ToWorldTransform); /// Return the largest penetration depth among the contact points - decimal getLargestPenetrationDepth() const { - - decimal maxDepth = decimal(0.0); - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - - if (element->penetrationDepth > maxDepth) { - maxDepth = element->penetrationDepth; - } - - element = element->next; - } - - return maxDepth; - } + decimal getLargestPenetrationDepth() const; }; +// Get the first contact point info of the linked list of contact points +inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const { + return mContactPointsList; +} + } #endif From 2f43e554b54f674f26a22c5e0c5cf4af0453dab8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 6 Jun 2017 21:12:26 +0200 Subject: [PATCH 027/105] Make TriangleShape inherits from ConvexPolyhedronShape --- src/collision/HalfEdgeStructure.h | 5 +- .../narrowphase/SAT/SATAlgorithm.cpp | 168 ++++++++++-------- src/collision/shapes/ConvexPolyhedronShape.h | 4 - src/collision/shapes/TriangleShape.cpp | 56 +++++- src/collision/shapes/TriangleShape.h | 118 +++++++++--- 5 files changed, 249 insertions(+), 102 deletions(-) diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 01e305c5..6b4e3aa1 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -53,6 +53,9 @@ class HalfEdgeStructure { uint edgeIndex; // Index of an half-edge of the face std::vector faceVertices; // Index of the vertices of the face + /// Constructor + Face() {} + /// Constructor Face(std::vector vertices) : faceVertices(vertices) {} }; @@ -155,7 +158,7 @@ inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const return mEdges[index]; } -// Retunr a given vertex +// Return a given vertex inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const { assert(index < mVertices.size()); return mVertices[index]; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 767aff8a..3bd3e796 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -46,6 +46,8 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()"); + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || @@ -77,32 +79,37 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // Compute the penetration depth of the shapes along the face normal direction - decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - sphere, sphereCenter); + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + sphere, sphereCenter); - // Return no collision - return false; - } + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; + // Return no collision + return false; + } - if (isTemporalCoherenceValid) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + } } } @@ -170,6 +177,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()"); + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || @@ -206,78 +215,83 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // If the previous minimum separation axis was a face normal of the polyhedron - if (lastFrameInfo.satIsAxisFacePolyhedron1) { + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - Vector3 outFaceNormalCapsuleSpace; + // If the previous minimum separation axis was a face normal of the polyhedron + if (lastFrameInfo.satIsAxisFacePolyhedron1) { - // Compute the penetration depth along the polyhedron face normal direction - const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - capsuleShape, polyhedronToCapsuleTransform, - outFaceNormalCapsuleSpace); + Vector3 outFaceNormalCapsuleSpace; - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // Compute the penetration depth along the polyhedron face normal direction + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, + capsuleShape, polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); - // Return no collision - return false; + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + } } + else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - if (isTemporalCoherenceValid) { + Vector3 outAxis; - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - isMinPenetrationFaceNormal = true; - separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; - } - } - else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron + // Compute the penetration depth along this axis + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); - // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); - const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); - const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); - const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - Vector3 outAxis; + // Return no collision + return false; + } - // Compute the penetration depth along this axis - const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, - capsuleSegmentAxis, edgeVertex1, - edgeDirectionCapsuleSpace, - polyhedronToCapsuleTransform, - outAxis); + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + if (isTemporalCoherenceValid) { - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minEdgeIndex = lastFrameInfo.satMinEdge1Index; - isMinPenetrationFaceNormal = false; - separatingAxisCapsuleSpace = outAxis; - separatingPolyhedronEdgeVertex1 = edgeVertex1; - separatingPolyhedronEdgeVertex2 = edgeVertex2; + minPenetrationDepth = penetrationDepth; + minEdgeIndex = lastFrameInfo.satMinEdge1Index; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = outAxis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } } } } @@ -527,6 +541,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { + PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index f6d2a34b..6c1de4d5 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -42,10 +42,6 @@ class ConvexPolyhedronShape : public ConvexShape { protected : - // -------------------- Attributes -------------------- // - - // -------------------- Methods -------------------- // - public : // -------------------- Methods -------------------- // diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 4767435b..9c0deb38 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -40,10 +40,16 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) - : ConvexShape(CollisionShapeType::TRIANGLE, margin) { + : ConvexPolyhedronShape(margin) { + mPoints[0] = point1; mPoints[1] = point2; mPoints[2] = point3; + + // Compute the triangle normal + mNormal = (point3 - point1).cross(point2 - point1); + mNormal.normalize(); + mRaycastTestType = TriangleRaycastSide::FRONT; } @@ -120,3 +126,51 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape return true; } +// Return a given half-edge of the polyhedron +HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + + HalfEdgeStructure::Edge edge; + + switch(edgeIndex) { + case 0: + edge.vertexIndex = 0; + edge.twinEdgeIndex = 1; + edge.faceIndex = 0; + edge.nextEdgeIndex = 2; + break; + case 1: + edge.vertexIndex = 1; + edge.twinEdgeIndex = 0; + edge.faceIndex = 1; + edge.nextEdgeIndex = 5; + break; + case 2: + edge.vertexIndex = 1; + edge.twinEdgeIndex = 3; + edge.faceIndex = 0; + edge.nextEdgeIndex = 4; + break; + case 3: + edge.vertexIndex = 2; + edge.twinEdgeIndex = 2; + edge.faceIndex = 1; + edge.nextEdgeIndex = 1; + break; + case 4: + edge.vertexIndex = 2; + edge.twinEdgeIndex = 5; + edge.faceIndex = 0; + edge.nextEdgeIndex = 0; + break; + case 5: + edge.vertexIndex = 0; + edge.twinEdgeIndex = 4; + edge.faceIndex = 1; + edge.nextEdgeIndex = 3; + break; + } + + return edge; + +} diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 29732f8f..e25086b8 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -28,7 +28,7 @@ // Libraries #include "mathematics/mathematics.h" -#include "ConvexShape.h" +#include "ConvexPolyhedronShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -51,7 +51,7 @@ enum class TriangleRaycastSide { * This class represents a triangle collision shape that is centered * at the origin and defined three points. */ -class TriangleShape : public ConvexShape { +class TriangleShape : public ConvexPolyhedronShape { protected: @@ -60,6 +60,9 @@ class TriangleShape : public ConvexShape { /// Three points of the triangle Vector3 mPoints[3]; + /// Normal of the triangle + Vector3 mNormal; + /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; @@ -113,11 +116,32 @@ class TriangleShape : public ConvexShape { // Set the raycast test type (front, back, front-back) void setRaycastTestType(TriangleRaycastSide testType); - /// Return the coordinates of a given vertex of the triangle - Vector3 getVertex(int index) const; + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; + /// Return a given face of the polyhedron + virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; // ---------- Friendship ---------- // @@ -199,6 +223,74 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape return false; } +// Return the number of faces of the polyhedron +inline uint TriangleShape::getNbFaces() const { + return 2; +} + +// Return a given face of the polyhedron +inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const { + assert(faceIndex < 2); + + HalfEdgeStructure::Face face; + + if (faceIndex == 0) { + face.faceVertices.push_back(0); + face.faceVertices.push_back(1); + face.faceVertices.push_back(2); + face.edgeIndex = 0; + + } + else { + face.faceVertices.push_back(0); + face.faceVertices.push_back(2); + face.faceVertices.push_back(1); + face.edgeIndex = 1; + } + + return face; +} + +// Return the number of vertices of the polyhedron +inline uint TriangleShape::getNbVertices() const { + return 3; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < 3); + + HalfEdgeStructure::Vertex vertex(vertexIndex); + switch (vertexIndex) { + case 0: vertex.edgeIndex = 0; break; + case 1: vertex.edgeIndex = 2; break; + case 2: vertex.edgeIndex = 4; break; + } + return vertex; +} + +// Return the position of a given vertex +inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < 3); + return mPoints[vertexIndex]; +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < 2); + return faceIndex == 0 ? mNormal : -mNormal; +} + +// Return the centroid of the box +inline Vector3 TriangleShape::getCentroid() const { + return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); +} + +// Return the number of half-edges of the polyhedron +inline uint TriangleShape::getNbHalfEdges() const { + return 6; +} + // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { return mRaycastTestType; @@ -212,20 +304,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { mRaycastTestType = testType; } -// Return the coordinates of a given vertex of the triangle -/** - * @param index Index (0 to 2) of a vertex of the triangle - */ -inline Vector3 TriangleShape::getVertex(int index) const { - assert(index >= 0 && index < 3); - return mPoints[index]; -} - -// Return true if the collision shape is a polyhedron -inline bool TriangleShape::isPolyhedron() const { - return true; -} - } #endif From 6e9a84823a77c5f1aef7e94e44f654ffb3b17021 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 17 Jul 2017 08:05:40 +0200 Subject: [PATCH 028/105] Fix issues in collision detection --- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 31 +++++++++++++------ .../narrowphase/SAT/SATAlgorithm.cpp | 4 +-- .../SphereVsConvexPolyhedronAlgorithm.cpp | 5 +++ src/collision/shapes/BoxShape.cpp | 12 ++++--- src/mathematics/mathematics_functions.cpp | 5 +++ src/mathematics/mathematics_functions.h | 3 ++ .../CollisionDetectionScene.cpp | 8 ++--- 7 files changed, 48 insertions(+), 20 deletions(-) mode change 100644 => 100755 src/mathematics/mathematics_functions.cpp mode change 100644 => 100755 src/mathematics/mathematics_functions.h diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 142a7fc9..b538dfa7 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -29,6 +29,7 @@ #include "GJK/GJKAlgorithm.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexPolyhedronShape.h" +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -47,40 +48,52 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { - // GJK has found a shallow contact. If the contact normal is parallel to a face of the - // polyhedron mesh, we would like to create two contact points instead of a single one - // (as in the deep contact case with SAT algorithm) + // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the + // capsule inner segment and parallel to the contact point normal, we would like to create + // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); + assert(contactPoint != nullptr); bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; - assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); // Get the collision shapes const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); // For each face of the polyhedron - // For each face of the convex mesh for (uint f = 0; f < polyhedron->getNbFaces(); f++) { // Get the face HalfEdgeStructure::Face face = polyhedron->getFace(f); const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(f); const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; - // If the polyhedron face normal is parallel to the computed GJK contact normal - if (areParallelVectors(faceNormalWorld, contactPoint->normal)) { + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); + + // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal + // is in direction of the contact normal (from the polyhedron point of view). + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) + && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK contactManifoldInfo.reset(); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 3bd3e796..d204a88e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -296,7 +296,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* } } - // We the shapes are still overlapping in the same axis as in + // If the shapes are still overlapping in the same axis as in the previous frame // the previous frame, we skip the whole SAT algorithm if (!isTemporalCoherenceValid) { @@ -470,7 +470,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr); const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; - const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace); return penetrationDepth; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index bb43bd33..068a79ff 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -41,6 +41,11 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index e1c97844..af6a30b8 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -57,15 +57,15 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) std::vector face0; face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); std::vector face1; - face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); + face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); std::vector face2; - face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); + face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5); std::vector face3; - face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); + face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); std::vector face4; - face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); + face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0); std::vector face5; - face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); + face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); mHalfEdgeStructure.addFace(face0); mHalfEdgeStructure.addFace(face1); @@ -73,6 +73,8 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) mHalfEdgeStructure.addFace(face3); mHalfEdgeStructure.addFace(face4); mHalfEdgeStructure.addFace(face5); + + mHalfEdgeStructure.init(); } // Return the local inertia tensor of the collision shape diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp old mode 100644 new mode 100755 index e01c749a..24329732 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -65,6 +65,11 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v return vector1.cross(vector2).lengthSquare() < decimal(0.00001); } +/// Return true if two vectors are orthogonal +bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { + return std::abs(vector1.dot(vector2)) < decimal(0.00001); +} + /// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h old mode 100644 new mode 100755 index c695f342..5ee03a19 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -79,6 +79,9 @@ inline bool sameSign(decimal a, decimal b) { /// Return true if two vectors are parallel bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); +/// Return true if two vectors are orthogonal +bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2); + /// Clamp a vector such that it is no longer than a given maximum length Vector3 clamp(const Vector3& vector, decimal maxLength); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 6351028a..e8f09670 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -61,7 +61,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere1->setSleepingColor(mRedColorDemo); // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(0, 0, 0); + openglframework::Vector3 position2(12, 8, 0); // Create a sphere and a corresponding collision body in the dynamics world mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath); @@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere2->setSleepingColor(mRedColorDemo); // ---------- Capsule 1 ---------- // - openglframework::Vector3 position3(8, 0, 0); + openglframework::Vector3 position3(0, -12, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); @@ -94,7 +94,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule2->setSleepingColor(mRedColorDemo); // ---------- Box 1 ---------- // - openglframework::Vector3 position5(0, -12, 0); + openglframework::Vector3 position5(0, -0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); @@ -105,7 +105,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mBox1->setSleepingColor(mRedColorDemo); // ---------- Box 2 ---------- // - openglframework::Vector3 position6(0, 12, 0); + openglframework::Vector3 position6(0, 8, 0); // Create a cylinder and a corresponding collision body in the dynamics world mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath); From ddd7f500a659b983c59c31984af074845864ab9f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 17 Jul 2017 18:35:51 +0200 Subject: [PATCH 029/105] Fix issues in SAT algorithm --- .../narrowphase/SAT/SATAlgorithm.cpp | 51 ++++++++++--------- .../SphereVsConvexPolyhedronAlgorithm.cpp | 4 +- 2 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index d204a88e..4e15eb15 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -141,15 +141,11 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); - const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse() * normalWorld * sphere->getRadius(); + const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); - if (!isSphereShape1) { - normalWorld = -normalWorld; - } - // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); @@ -497,32 +493,41 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); - planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex)); + planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex)); edgeIndex = edge.nextEdgeIndex; } while(edgeIndex != firstEdgeIndex); // First we clip the inner segment of the capsule with the four planes of the adjacent faces - std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - planesPoints, planesNormals); + std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); + assert(clipSegment.size() == 2); - // Project the two clipped points into the polyhedron face - const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); - const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius); - const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius); + const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); - // Project the two clipped points into the capsule bounds - const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius; - const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius; + // Project the two clipped points into the polyhedron face + const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); - // Create the contact points - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, - isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron, - isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule); - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, - isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron, - isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule); + // For each of the two clipped points + for (int i = 0; i<2; i++) { + + // Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth) + const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal); + + // If the clipped point is one that produce this penetration depth, we keep it + if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) { + + const Vector3 contactPointPolyhedron = clipSegment[i] + delta; + + // Project the clipped point into the capsule bounds + const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; + + // Create the contact point + contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, + isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); + } + } } // This method returns true if an edge of a polyhedron and a capsule forms a diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 068a79ff..d90a29b3 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -43,8 +43,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || - narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; From 6eec956eb0afcdf6cf0194da3c520158fd3aa0d4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 21 Jul 2017 08:09:43 +0200 Subject: [PATCH 030/105] Fix issues in SAT algorithm between two convex polyhedra --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++------- src/mathematics/mathematics_functions.cpp | 5 +++++ src/mathematics/mathematics_functions.h | 3 +++ 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4e15eb15..c838d8ab 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -854,18 +854,19 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP assert(clipPolygonVertices.size() > 0); // We only keep the clipped points that are below the reference face - const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex); + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); std::vector::const_iterator itPoints; for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { // If the clip point is bellow the reference face - if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { + if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) + { // Convert the clip incident polyhedron vertex into the incident polyhedron local-space const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth; + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); // Create a new contact point contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, @@ -1011,8 +1012,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, const Transform& polyhedron1ToPolyhedron2) const { - const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex); - const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); + const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex); + const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex); const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex); @@ -1020,12 +1021,12 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly // Compute b.cross(a) using the edge direction const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex); const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex); - const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1); + const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex1 - edge1Vertex2); // Compute d.cross(c) using the edge direction const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex); const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex); - const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1; + const Vector3 dCrossC = edge2Vertex1 - edge2Vertex2; // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face) // Note that we negate the normals of the second polyhedron because we are looking at the diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 24329732..d627ed7b 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -354,4 +354,9 @@ std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& s std::vector clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, const std::vector& planesNormals); +/// Project a point onto a plane that is given by a point and its unit length normal +Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + } From 8b82c4ac819998ea2af634a8a5858fd0cada31df Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 30 Jul 2017 22:14:46 +0200 Subject: [PATCH 031/105] Refactor the way to create the contact manifolds and contact points --- CMakeLists.txt | 4 + src/body/CollisionBody.cpp | 6 +- src/collision/CollisionCallback.cpp | 79 ++++ src/collision/CollisionCallback.h | 89 ++++ src/collision/CollisionDetection.cpp | 425 ++++++++++++------ src/collision/CollisionDetection.h | 17 +- src/collision/ContactManifold.cpp | 82 ++-- src/collision/ContactManifold.h | 248 ++++++---- src/collision/ContactManifoldInfo.cpp | 60 +-- src/collision/ContactManifoldInfo.h | 45 +- src/collision/ContactManifoldSet.cpp | 278 +++++++----- src/collision/ContactManifoldSet.h | 84 ++-- src/collision/NarrowPhaseInfo.cpp | 97 ++++ src/collision/NarrowPhaseInfo.h | 22 +- src/collision/OverlapCallback.h | 57 +++ .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 8 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 3 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +- .../CapsuleVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 178 ++++---- .../narrowphase/ConcaveVsConvexAlgorithm.h | 26 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +- ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 5 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +- .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 22 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 8 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 4 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 3 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +- .../SphereVsConvexPolyhedronAlgorithm.h | 3 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 5 +- .../narrowphase/SphereVsSphereAlgorithm.h | 3 +- src/collision/shapes/BoxShape.h | 2 + src/collision/shapes/CollisionShape.h | 17 - src/constraint/ContactPoint.cpp | 9 +- src/constraint/ContactPoint.h | 70 ++- src/engine/CollisionWorld.h | 58 --- src/engine/ContactSolver.cpp | 8 +- src/engine/DynamicsWorld.cpp | 11 +- src/engine/EventListener.h | 10 +- src/engine/OverlappingPair.cpp | 100 ++++- src/engine/OverlappingPair.h | 69 ++- src/memory/Allocator.h | 3 + src/memory/PoolAllocator.h | 8 + src/memory/SingleFrameAllocator.h | 9 +- src/reactphysics3d.h | 2 + .../CollisionDetectionScene.h | 40 +- testbed/src/SceneDemo.cpp | 6 +- 50 files changed, 1558 insertions(+), 767 deletions(-) create mode 100644 src/collision/CollisionCallback.cpp create mode 100644 src/collision/CollisionCallback.h create mode 100644 src/collision/NarrowPhaseInfo.cpp create mode 100644 src/collision/OverlapCallback.h diff --git a/CMakeLists.txt b/CMakeLists.txt index cba141df..1e1d2fcf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" "src/collision/NarrowPhaseInfo.h" + "src/collision/NarrowPhaseInfo.cpp" "src/collision/ContactManifold.h" "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.h" @@ -167,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/Profiler.cpp" "src/engine/Timer.h" "src/engine/Timer.cpp" + "src/collision/CollisionCallback.h" + "src/collision/CollisionCallback.cpp" + "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" "src/mathematics/mathematics_functions.cpp" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index db22f606..39d47de4 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -177,7 +177,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the linked list of contact manifolds of that body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - ContactManifoldListElement* nextElement = currentElement->next; + ContactManifoldListElement* nextElement = currentElement->getNext(); // Delete the current element currentElement->~ContactManifoldListElement(); @@ -272,8 +272,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { // this body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - currentElement->contactManifold->mIsAlreadyInIsland = false; - currentElement = currentElement->next; + currentElement->getContactManifold()->mIsAlreadyInIsland = false; + currentElement = currentElement->getNext(); nbManifolds++; } diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp new file mode 100644 index 00000000..f470763d --- /dev/null +++ b/src/collision/CollisionCallback.cpp @@ -0,0 +1,79 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "collision/CollisionCallback.h" +#include "engine/OverlappingPair.h" +#include "memory/Allocator.h" +#include "collision/ContactManifold.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) : + contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()), + body2(pair->getShape2()->getBody()), + proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()), + mMemoryAllocator(allocator) { + + assert(pair != nullptr); + + const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); + + // For each contact manifold in the set of manifolds in the pair + ContactManifold* contactManifold = manifoldSet.getContactManifolds(); + while (contactManifold != nullptr) { + + assert(contactManifold->getNbContactPoints() > 0); + + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement))) + ContactManifoldListElement(contactManifold, + contactManifoldElements); + contactManifoldElements = element; + + contactManifold = contactManifold->getNext(); + } +} + +// Destructor +CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() { + + // Release memory allocator for the contact manifold list elements + ContactManifoldListElement* element = contactManifoldElements; + while (element != nullptr) { + + ContactManifoldListElement* nextElement = element->getNext(); + + // Delete and release memory + element->~ContactManifoldListElement(); + mMemoryAllocator.release(element, sizeof(ContactManifoldListElement)); + + element = nextElement; + } +} + diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h new file mode 100644 index 00000000..f860b42d --- /dev/null +++ b/src/collision/CollisionCallback.h @@ -0,0 +1,89 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H +#define REACTPHYSICS3D_COLLISION_CALLBACK_H + +// Libraries +#include "collision/ContactManifold.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class CollisionCallback +/** + * This class can be used to register a callback for collision test queries. + * You should implement your own class inherited from this one and implement + * the notifyContact() method. This method will called each time a contact + * point is reported. + */ +class CollisionCallback { + + public: + + // Structure CollisionCallbackInfo + /** + * This structure represents the contact information between two collision + * shapes of two bodies + */ + struct CollisionCallbackInfo { + + public: + + /// Pointer to the first element of the linked-list that contains contact manifolds + ContactManifoldListElement* contactManifoldElements; + + /// Pointer to the first body of the contact + CollisionBody* body1; + + /// Pointer to the second body of the contact + CollisionBody* body2; + + /// Pointer to the proxy shape of first body + const ProxyShape* proxyShape1; + + /// Pointer to the proxy shape of second body + const ProxyShape* proxyShape2; + + /// Memory allocator + Allocator& mMemoryAllocator; + + // Constructor + CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator); + + // Destructor + ~CollisionCallbackInfo(); + }; + + /// Destructor + virtual ~CollisionCallback() = default; + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; +}; + +} + +#endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 0e7c8078..57360706 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -26,10 +26,13 @@ // Libraries #include "CollisionDetection.h" #include "engine/CollisionWorld.h" +#include "collision/OverlapCallback.h" #include "body/Body.h" #include "collision/shapes/BoxShape.h" #include "body/RigidBody.h" #include "configuration.h" +#include "collision/CollisionCallback.h" +#include "collision/OverlapCallback.h" #include #include #include @@ -43,7 +46,7 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator) - : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), + : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { @@ -83,7 +86,7 @@ void CollisionDetection::computeBroadPhase() { // Ask the broad-phase to recompute the overlapping pairs of collision // shapes. This call can only add new overlapping pairs in the collision // detection. - mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator); + mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator); } } @@ -101,25 +104,24 @@ void CollisionDetection::computeMiddlePhase() { OverlappingPair* pair = it->second; + // Make all the contact manifolds and contact points of the pair obselete + pair->makeContactsObselete(); + ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the + // Check if the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { std::map::iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; @@ -128,56 +130,61 @@ void CollisionDetection::computeMiddlePhase() { ++it; } - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); + // Check if the collision filtering allows collision between the two shapes + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) { - // Check that at least one body is awake and not static - bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; - bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; - if (!isBody1Active && !isBody2Active) continue; + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); - // Check if the bodies are in the set of bodies that cannot collide between each other - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; + // Check that at least one body is awake and not static + bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; + bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; + if (!isBody1Active && !isBody2Active) continue; - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + // Check if the bodies are in the set of bodies that cannot collide between each other + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(pair, shape1->getCollisionShape(), - shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); - mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; + // If both shapes are convex + if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { - } - // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), + shape2->getCachedCollisionData()); + mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; - NarrowPhaseInfo* narrowPhaseInfo = nullptr; - computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); - - // Add all the narrow-phase info object reported by the callback into the - // list of all the narrow-phase info object - while (narrowPhaseInfo != nullptr) { - NarrowPhaseInfo* next = narrowPhaseInfo->next; - narrowPhaseInfo->next = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = narrowPhaseInfo; - - narrowPhaseInfo = next; } - } - // Concave vs Concave shape - else { - // Not handled - continue; + // Concave vs Convex algorithm + else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || + (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + while (narrowPhaseInfo != nullptr) { + NarrowPhaseInfo* next = narrowPhaseInfo->next; + narrowPhaseInfo->next = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = narrowPhaseInfo; + + narrowPhaseInfo = next; + } + } + // Concave vs Concave shape + else { + // Not handled + continue; + } } } } @@ -231,7 +238,7 @@ void CollisionDetection::computeNarrowPhase() { PROFILE("CollisionDetection::computeNarrowPhase()"); - const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; + NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; while (currentNarrowPhaseInfo != nullptr) { // Select the narrow phase algorithm to use according to the two collision shapes @@ -245,30 +252,16 @@ void CollisionDetection::computeNarrowPhase() { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator); - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform); - - // If it is the first contact since the pairs are overlapping - if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { - - // Trigger a callback event - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo); - } - - // Add the contact manifold to the corresponding overlapping pair - currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo); + // Add the contact points as a potential contact manifold into the pair + currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); // Add the overlapping pair into the set of pairs in contact during narrow-phase overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), currentNarrowPhaseInfo->overlappingPair->getShape2()); mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; - // Trigger a callback event for the new contact - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo); - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; } else { @@ -282,8 +275,14 @@ void CollisionDetection::computeNarrowPhase() { currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; } + // Convert the potential contact into actual contacts + processAllPotentialContacts(); + // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); + + // Report contacts to the user + reportAllContacts(); } // Allow the broadphase to notify the collision detection about an overlapping pair. @@ -302,13 +301,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Check if the overlapping pair already exists if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; - // Compute the maximum number of contact manifolds for this pair - int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), - shape2->getCollisionShape()->getType()); - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); + OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator); assert(newPair != nullptr); #ifndef NDEBUG @@ -372,40 +367,173 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); // For each contact manifold in the set of manifolds in the pair - for (int i=0; igetNbContactPoints() > 0); // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement1 = new (allocatedMemory1) + ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); body1->mContactManifoldsList = listElement1; // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement2 = new (allocatedMemory2) + ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); body2->mContactManifoldsList = listElement2; + + contactManifold = contactManifold->getNext(); } } -// Delete all the contact points in the currently overlapping pairs -void CollisionDetection::clearContactPoints() { +/// Convert the potential contact into actual contacts +void CollisionDetection::processAllPotentialContacts() { - // For each overlapping pair + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - it->second->clearContactPoints(); + for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + + // Process the potential contacts of the overlapping pair + processPotentialContacts(it->second); } } +// Process the potential contact manifold of a pair to create actual contact manifold +void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { + + // Reduce the number of contact points of the manifold + pair->reducePotentialContactManifolds(); + + // If there is a concave mesh shape in the pair + if (pair->hasConcaveShape()) { + + processSmoothMeshContacts(pair); + } + else { // If both collision shapes are convex + + // Add all the potential contact manifolds as actual contact manifolds to the pair + ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); + while (potentialManifold != nullptr) { + + pair->addContactManifold(potentialManifold); + + potentialManifold = potentialManifold->mNext; + } + } + + // Clear the obselete contact manifolds and contact points + pair->clearObseleteManifoldsAndContactPoints(); + + // Reset the potential contacts of the pair + pair->clearPotentialContactManifolds(); +} + +// Report contacts for all the colliding overlapping pairs +void CollisionDetection::reportAllContacts() { + + // For each overlapping pairs in contact during the narrow-phase + std::map::iterator it; + for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + + // If there is a user callback + if (mWorld->mEventListener != nullptr) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator); + + // Trigger a callback event to report the new contact to the user + mWorld->mEventListener->newContact(collisionInfo); + } + } +} + +// Process the potential contacts where one collion is a concave shape. +// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described +// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision +// issue with some internal edges. +void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) { + +// // Set with the triangle vertices already processed to void further contacts with same triangle +// std::unordered_multimap processTriangleVertices; + +// std::vector smoothContactPoints; + +// // If the collision shape 1 is the triangle +// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE; +// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE); +// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE); + +// const TriangleShape* triangleShape = nullptr; +// if (isFirstShapeTriangle) { +// triangleShape = static_cast(pair->getShape1()->getCollisionShape()); +// } +// else { +// triangleShape = static_cast(pair->getShape2()->getCollisionShape()); +// } +// assert(triangleShape != nullptr); + +// // Get the temporary memory allocator +// Allocator& allocator = pair->getTemporaryAllocator(); + +// // For each potential contact manifold of the pair +// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); +// while (potentialManifold != nullptr) { + +// // For each contact point of the potential manifold +// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo(); +// while (contactPointInfo != nullptr) { + +// // Compute the barycentric coordinates of the point in the triangle +// decimal u, v, w; +// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0), +// triangleShape->getVertexPosition(1), +// triangleShape->getVertexPosition(2), +// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2, +// u, v, w); +// int nbZeros = 0; +// bool isUZero = approxEqual(u, 0, 0.0001); +// bool isVZero = approxEqual(v, 0, 0.0001); +// bool isWZero = approxEqual(w, 0, 0.0001); +// if (isUZero) nbZeros++; +// if (isVZero) nbZeros++; +// if (isWZero) nbZeros++; + +// // If the triangle contact point is on a triangle vertex of a triangle edge +// if (nbZeros == 1 || nbZeros == 2) { + + +// // Create a smooth mesh contact info +// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo))) +// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle, +// triangleShape->getVertexPosition(0), +// triangleShape->getVertexPosition(1), +// triangleShape->getVertexPosition(2)); + +// smoothContactPoints.push_back(smoothContactInfo); + +// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we +// // will put the contact point back in the manifold. +// ... +// } + +// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle +// // from the contact manifold because we know we will keep to contact points. We only remove the vertices +// // and edges contact points for the moment. If those points will be kept in the future, we will have to +// // put them back again in the contact manifold +// } + +// potentialManifold = potentialManifold->mNext; +// } + +// // Sort the list of narrow-phase contacts according to their penetration depth +// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare()); + +// ... +} + // Compute the middle-phase collision detection between two proxy shapes NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { @@ -424,7 +552,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), + narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), shape2->getCachedCollisionData()); @@ -436,7 +564,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo); } return narrowPhaseInfo; @@ -450,7 +578,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over std::unordered_set reportedBodies; // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); // For each overlaping proxy shape @@ -504,7 +632,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -526,16 +654,18 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -570,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -595,7 +725,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -617,16 +747,18 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -673,7 +805,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body if (aabb1.testCollision(aabb2)) { // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -693,26 +825,29 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); - - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2, - body1ProxyShape, body2ProxyShape); - - // Report the contact to the user - collisionCallback->notifyContact(collisionInfo); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + collisionCallback->notifyContact(collisionInfo); } // Go to the next proxy shape @@ -737,7 +872,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mPoolAllocator); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -760,7 +895,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -777,17 +912,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body, - proxyShape->getBody(), bodyProxyShape, - proxyShape); - - // Report the contact to the user + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); callback->notifyContact(collisionInfo); } } @@ -795,9 +926,15 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); } } @@ -822,10 +959,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; + OverlappingPair* originalPair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); + // Create a new overlapping pair so that we do not work on the original one + OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator, + mPoolAllocator); + + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. @@ -834,14 +975,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); - - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); + const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -851,29 +992,29 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - ContactManifoldInfo contactManifoldInfo(mMemoryAllocator); - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { - // Reduce the number of points in the contact manifold (if necessary) - contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(), - shape2->getBody(), shape1, shape2); - - // Report the contact to the user + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); callback->notifyContact(collisionInfo); } - - // The previous frame collision info is now valid - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); } } } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 3fe88812..49f0d56b 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -72,7 +72,7 @@ class CollisionDetection { NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + PoolAllocator& mPoolAllocator; /// Reference to the single frame memory allocator SingleFrameAllocator& mSingleFrameAllocator; @@ -118,9 +118,6 @@ class CollisionDetection { /// involed in the corresponding contact. void addContactManifoldToBody(OverlappingPair* pair); - /// Delete all the contact points in the currently overlapping pairs - void clearContactPoints(); - /// Fill-in the collision detection matrix void fillInCollisionMatrix(); @@ -137,6 +134,18 @@ class CollisionDetection { /// Compute the middle-phase collision detection between two proxy shapes NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair); + + /// Convert the potential contact into actual contacts + void processAllPotentialContacts(); + + /// Process the potential contact manifold of a pair to create actual contact manifold + void processPotentialContacts(OverlappingPair* pair); + + /// Report contacts for all the colliding overlapping pairs + void reportAllContacts(); + + /// Process the potential contacts where one collion is a concave shape + void processSmoothMeshContacts(OverlappingPair* pair); public : diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 36103642..846a418a 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -30,15 +30,14 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short normalDirectionId) - : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), +ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator) + : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) { // For each contact point info in the manifold - const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo(); + const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); while(pointInfo != nullptr) { // Create the new contact point @@ -46,7 +45,8 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); // Add the new contact point into the manifold - mContactPoints[mNbContactPoints] = contact; + contact->setNext(mContactPoints); + mContactPoints = contact; mNbContactPoints++; pointInfo = pointInfo->next; @@ -58,49 +58,69 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS // Destructor ContactManifold::~ContactManifold() { - clear(); -} -// Clear the contact manifold -void ContactManifold::clear() { - for (uint i=0; i~ContactPoint(); - mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); + // Delete all the contact points + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + // TODO : Delete this + bool test = mMemoryAllocator.isReleaseNeeded(); + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + contactPoint = nextContactPoint; } - mNbContactPoints = 0; } // Add a contact point void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) { - assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD); - // Create the new contact point ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); // Add the new contact point into the manifold - mContactPoints[mNbContactPoints] = contactPoint; - mNbContactPoints++; + contactPoint->setNext(mContactPoints); + mContactPoints = contactPoint; + mNbContactPoints++; } -// Remove a contact point -void ContactManifold::removeContactPoint(int index) { +// Clear the obselete contact points +void ContactManifold::clearObseleteContactPoints() { - assert(mNbContactPoints > 0); - assert(index >= 0 && index < mNbContactPoints); + ContactPoint* contactPoint = mContactPoints; + ContactPoint* previousContactPoint = nullptr; + while (contactPoint != nullptr) { - // Delete the contact - mContactPoints[index]->~ContactPoint(); - mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); + ContactPoint* nextContactPoint = contactPoint->getNext(); - for (int i=index; (i+1) < mNbContactPoints; i++) { - mContactPoints[i] = mContactPoints[i+1]; + if (contactPoint->getIsObselete()) { + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + if (previousContactPoint != nullptr) { + previousContactPoint->setNext(nextContactPoint); + } + else { + mContactPoints = nextContactPoint; + } + + mNbContactPoints--; + } + else { + previousContactPoint = contactPoint; + } + + contactPoint = nextContactPoint; } - mNbContactPoints--; + assert(mNbContactPoints >= 0); + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 893a84a7..0704beed 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -46,40 +46,43 @@ class ContactManifold; */ struct ContactManifoldListElement { - public: + private: // -------------------- Attributes -------------------- // - /// Pointer to the actual contact manifold - ContactManifold* contactManifold; + /// Pointer to a contact manifold with contact points + ContactManifold* mContactManifold; /// Next element of the list - ContactManifoldListElement* next; + ContactManifoldListElement* mNext; + + public: // -------------------- Methods -------------------- // /// Constructor - ContactManifoldListElement(ContactManifold* initContactManifold, - ContactManifoldListElement* initNext) - :contactManifold(initContactManifold), next(initNext) { + ContactManifoldListElement(ContactManifold* contactManifold, + ContactManifoldListElement* next) + :mContactManifold(contactManifold), mNext(next) { } + + /// Return the contact manifold + ContactManifold* getContactManifold() { + return mContactManifold; + } + + /// Return the next element in the linked-list + ContactManifoldListElement* getNext() { + return mNext; + } }; // Class ContactManifold /** * This class represents the set of contact points between two bodies. * The contact manifold is implemented in a way to cache the contact - * points among the frames for better stability following the - * "Contact Generation" presentation of Erwin Coumans at GDC 2010 - * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). - * Some code of this class is based on the implementation of the - * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). - * The contacts between two bodies are added one after the other in the cache. - * When the cache is full, we have to remove one point. The idea is to keep - * the point with the deepest penetration depth and also to keep the - * points producing the larger area (for a more stable contact manifold). - * The new added point is always kept. + * points among the frames for better stability. */ class ContactManifold { @@ -94,10 +97,10 @@ class ContactManifold { ProxyShape* mShape2; /// Contact points in the manifold - ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + ContactPoint* mContactPoints; /// Normal direction Id (Unique Id representing the normal direction) - short int mNormalDirectionId; + short int mContactNormalId; /// Number of contacts in the cache int8 mNbContactPoints; @@ -124,20 +127,86 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + Allocator& mMemoryAllocator; + + /// Pointer to the next contact manifold in the linked-list + ContactManifold* mNext; + + /// Pointer to the previous contact manifold in linked-list + ContactManifold* mPrevious; + + /// True if the contact manifold is obselete + bool mIsObselete; // -------------------- Methods -------------------- // /// Return true if the contact manifold has already been added into an island bool isAlreadyInIsland() const; + + /// Set the pointer to the next element in the linked-list + void setNext(ContactManifold* nextManifold); + + /// Return true if the manifold is obselete + bool getIsObselete() const; + + /// Set to true to make the manifold obselete + void setIsObselete(bool isObselete, bool setContactPoints); + + /// Clear the obselete contact points + void clearObseleteContactPoints(); + + /// Return the contact normal direction Id of the manifold + short getContactNormalId() const; + + /// Return the largest depth of all the contact points + decimal getLargestContactDepth() const; + + /// set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + /// set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + /// Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + /// Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + /// Add a contact point + void addContactPoint(const ContactPointInfo* contactPointInfo); + + /// Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + /// Set the accumulated rolling resistance impulse + void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); + + /// Set the pointer to the previous element in the linked-list + void setPrevious(ContactManifold* previousManifold); + + /// Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + /// Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + /// Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + /// Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + /// Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; public: // -------------------- Methods -------------------- // /// Constructor - ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short int normalDirectionId); + ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, + Allocator& memoryAllocator); /// Destructor ~ContactManifold(); @@ -160,68 +229,25 @@ class ContactManifold { /// Return a pointer to the second body of the contact manifold CollisionBody* getBody2() const; - /// Return the normal direction Id - short int getNormalDirectionId() const; - - /// Clear the contact manifold - void clear(); - /// Return the number of contact points in the manifold int8 getNbContactPoints() const; - /// Return the first friction vector at the center of the contact manifold - const Vector3& getFrictionVector1() const; + /// Return a pointer to the first contact point of the manifold + ContactPoint* getContactPoints() const; - /// set the first friction vector at the center of the contact manifold - void setFrictionVector1(const Vector3& mFrictionVector1); + /// Return a pointer to the previous element in the linked-list + ContactManifold* getPrevious() const; - /// Return the second friction vector at the center of the contact manifold - const Vector3& getFrictionVector2() const; - - /// set the second friction vector at the center of the contact manifold - void setFrictionVector2(const Vector3& mFrictionVector2); - - /// Return the first friction accumulated impulse - decimal getFrictionImpulse1() const; - - /// Set the first friction accumulated impulse - void setFrictionImpulse1(decimal frictionImpulse1); - - /// Return the second friction accumulated impulse - decimal getFrictionImpulse2() const; - - /// Set the second friction accumulated impulse - void setFrictionImpulse2(decimal frictionImpulse2); - - /// Return the friction twist accumulated impulse - decimal getFrictionTwistImpulse() const; - - /// Set the friction twist accumulated impulse - void setFrictionTwistImpulse(decimal frictionTwistImpulse); - - /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); - - /// Return a contact point of the manifold - ContactPoint* getContactPoint(uint index) const; - - /// Return the normalized averaged normal vector - Vector3 getAverageContactNormal() const; - - /// Return the largest depth of all the contact points - decimal getLargestContactDepth() const; - - /// Add a contact point - void addContactPoint(const ContactPointInfo* contactPointInfo); - - /// Remove a contact point - void removeContactPoint(int index); + /// Return a pointer to the next element in the linked-list + ContactManifold* getNext() const; // -------------------- Friendship -------------------- // friend class DynamicsWorld; friend class Island; friend class CollisionBody; + friend class ContactManifoldSet; + friend class ContactSolver; }; // Return a pointer to the first proxy shape of the contact @@ -244,11 +270,6 @@ inline CollisionBody* ContactManifold::getBody2() const { return mShape2->getBody(); } -// Return the normal direction Id -inline short int ContactManifold::getNormalDirectionId() const { - return mNormalDirectionId; -} - // Return the number of contact points in the manifold inline int8 ContactManifold::getNbContactPoints() const { return mNbContactPoints; @@ -309,10 +330,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR mRollingResistanceImpulse = rollingResistanceImpulse; } -// Return a contact point of the manifold -inline ContactPoint* ContactManifold::getContactPoint(uint index) const { - assert(index < mNbContactPoints); - return mContactPoints[index]; +// Return a pointer to the first contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoints() const { + return mContactPoints; } // Return true if the contact manifold has already been added into an island @@ -320,31 +340,69 @@ inline bool ContactManifold::isAlreadyInIsland() const { return mIsAlreadyInIsland; } -// Return the normalized averaged normal vector -inline Vector3 ContactManifold::getAverageContactNormal() const { - Vector3 averageNormal; - - for (uint i=0; igetNormal(); - } - - return averageNormal.getUnit(); -} - // Return the largest depth of all the contact points inline decimal ContactManifold::getLargestContactDepth() const { decimal largestDepth = 0.0f; - for (uint i=0; igetPenetrationDepth(); + assert(mNbContactPoints > 0); + + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr){ + decimal depth = contactPoint->getPenetrationDepth(); if (depth > largestDepth) { largestDepth = depth; } + + contactPoint = contactPoint->getNext(); } return largestDepth; } +// Return a pointer to the previous element in the linked-list +inline ContactManifold* ContactManifold::getPrevious() const { + return mPrevious; +} + +// Set the pointer to the previous element in the linked-list +inline void ContactManifold::setPrevious(ContactManifold* previousManifold) { + mPrevious = previousManifold; +} + +// Return a pointer to the next element in the linked-list +inline ContactManifold* ContactManifold::getNext() const { + return mNext; +} + +// Set the pointer to the next element in the linked-list +inline void ContactManifold::setNext(ContactManifold* nextManifold) { + mNext = nextManifold; +} + +// Return true if the manifold is obselete +inline bool ContactManifold::getIsObselete() const { + return mIsObselete; +} + +// Set to true to make the manifold obselete +inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) { + mIsObselete = isObselete; + + if (setContactPoints) { + ContactPoint* contactPoint = mContactPoints; + while (contactPoint != nullptr) { + contactPoint->setIsObselete(isObselete); + + contactPoint = contactPoint->getNext(); + } + } +} + +// Return the contact normal direction Id of the manifold +inline short ContactManifold::getContactNormalId() const { + return mContactNormalId; +} + } #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index c06785ea..a6b8c6df 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -30,7 +30,8 @@ using namespace reactphysics3d; // Constructor ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator) - : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {} + : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator), + mContactNormalId(-1) {} // Destructor ContactManifoldInfo::~ContactManifoldInfo() { @@ -40,14 +41,13 @@ ContactManifoldInfo::~ContactManifoldInfo() { } // Add a new contact point into the manifold -void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { +void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) { - assert(penDepth > decimal(0.0)); + assert(contactPointInfo->penetrationDepth > decimal(0.0)); + assert(contactNormalId >= 0); + assert(mContactNormalId == -1 || contactNormalId == mContactNormalId); - // Create the contact point info - ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo))) - ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + mContactNormalId = contactNormalId; // Add it into the linked list of contact points contactPointInfo->next = mContactPointsList; @@ -73,15 +73,31 @@ void ContactManifoldInfo::reset() { mNbContactPoints = 0; } -// Reduce the number of points in the contact manifold +// Return the largest penetration depth among its contact points +decimal ContactManifoldInfo::getLargestPenetrationDepth() const { + + ContactPointInfo* contactPoint = mContactPointsList; + assert(contactPoint != nullptr); + decimal maxDepth = decimal(0.0); + while (contactPoint != nullptr) { + + if (contactPoint->penetrationDepth > maxDepth) { + maxDepth = contactPoint->penetrationDepth; + } + + contactPoint = contactPoint->next; + } + + return maxDepth; +} + +// Reduce the number of contact points of the currently computed manifold // This is based on the technique described by Dirk Gregorius in his // "Contacts Creation" GDC presentation void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { assert(mContactPointsList != nullptr); - // TODO : Implement this (do not forget to deallocate removed points) - // The following algorithm only works to reduce to 4 contact points assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); @@ -221,9 +237,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { largestArea = area; pointsToKeep[3] = element; } - - element = element->next; } + + element = element->next; } assert(pointsToKeep[3] != nullptr); @@ -250,6 +266,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { } element = element->next; + // Call the destructor + elementToDelete->~ContactPointInfo(); + // Delete the current element mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); } @@ -258,21 +277,4 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { } } -/// Return the largest penetration depth among the contact points -decimal ContactManifoldInfo::getLargestPenetrationDepth() const { - - decimal maxDepth = decimal(0.0); - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - - if (element->penetrationDepth > maxDepth) { - maxDepth = element->penetrationDepth; - } - - element = element->next; - } - - return maxDepth; -} - diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index a7e84a98..5851c375 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Constants -const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold +const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold // Class ContactManifoldInfo /** @@ -50,12 +50,18 @@ class ContactManifoldInfo { /// Linked list with all the contact points ContactPointInfo* mContactPointsList; - /// Memory allocator used to allocate contact points - Allocator& mAllocator; - /// Number of contact points in the manifold uint mNbContactPoints; + /// Next element in the linked-list of contact manifold info + ContactManifoldInfo* mNext; + + /// Reference the the memory allocator where the contact point infos have been allocated + Allocator& mAllocator; + + /// Contact normal direction Id (Identify the contact normal direction of points in manifold) + short mContactNormalId; + public: // -------------------- Methods -------------------- // @@ -66,15 +72,14 @@ class ContactManifoldInfo { /// Destructor ~ContactManifoldInfo(); - /// Deleted copy-constructor + /// Deleted Copy-constructor ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; /// Deleted assignment operator ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; /// Add a new contact point into the manifold - void addContactPoint(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2); + void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId); /// Remove all the contact points void reset(); @@ -82,11 +87,21 @@ class ContactManifoldInfo { /// Get the first contact point info of the linked list of contact points ContactPointInfo* getFirstContactPointInfo() const; - /// Reduce the number of points in the contact manifold + /// Return the largest penetration depth among its contact points + decimal getLargestPenetrationDepth() const; + + /// Return the pointer to the next manifold info in the linked-list + ContactManifoldInfo* getNext(); + + /// Return the contact normal Id + short getContactNormalId() const; + + /// Reduce the number of contact points of the currently computed manifold void reduce(const Transform& shape1ToWorldTransform); - /// Return the largest penetration depth among the contact points - decimal getLargestPenetrationDepth() const; + // Friendship + friend class OverlappingPair; + friend class CollisionDetection; }; // Get the first contact point info of the linked list of contact points @@ -94,6 +109,16 @@ inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const { return mContactPointsList; } +// Return the pointer to the next manifold info in the linked-list +inline ContactManifoldInfo* ContactManifoldInfo::getNext() { + return mNext; +} + +// Return the contact normal Id +inline short ContactManifoldInfo::getContactNormalId() const { + return mContactNormalId; +} + } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index c9b0d5bb..b9b7f3e8 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,10 +30,12 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds) - : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), - mShape2(shape2), mMemoryAllocator(memoryAllocator) { + Allocator& memoryAllocator) + : mNbManifolds(0), mShape1(shape1), + mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) { + // Compute the maximum number of manifolds allowed between the two shapes + mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape()); } // Destructor @@ -43,167 +45,131 @@ ContactManifoldSet::~ContactManifoldSet() { clear(); } -void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { +void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { - assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr); + assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr); - // If there is no contact manifold yet - if (mNbManifolds == 0) { + // Try to find an existing contact manifold with similar contact normal + ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId()); - // If the maximum number of manifold is 1 - if (mNbMaxManifolds == 1) { - createManifold(contactManifoldInfo, 0); - } - else { + // If a similar manifold has been found + if (similarManifold != nullptr) { - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); - - createManifold(contactManifoldInfo, normalDirectionId); - } + // Update the old manifold with the new one + updateManifoldWithNewOne(similarManifold, contactManifoldInfo); } - else { // If there is already at least one contact manifold in the set + else { - // If the maximum number of manifold is 1 - if (mNbMaxManifolds == 1) { + // If there are too much contact manifolds in the set + if (mNbManifolds >= mNbMaxManifolds) { - // Replace the old manifold with the new one - updateManifoldWithNewOne(0, contactManifoldInfo); - } - else { + // We need to remove a manifold from the set. + // We seach for the one with the smallest maximum penetration depth among its contact points + ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth()); - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal); + // If the manifold with the minimum penetration depth is an existing one + if (minDepthManifold != nullptr) { - // Select the manifold with the most similar normal (if exists) - int similarManifoldIndex = 0; - if (mNbMaxManifolds > 1) { - similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); - } + // Remove the manifold + removeManifold(minDepthManifold); - // If a similar manifold has been found - if (similarManifoldIndex != -1) { - - // Replace the old manifold with the new one - updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo); - } - else { - - // If we have not reach the maximum number of manifolds - if (mNbManifolds < mNbMaxManifolds) { - - // Create the new contact manifold - createManifold(contactManifoldInfo, normalDirectionId); - } - else { - - decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth(); - - // We have reached the maximum number of manifold, we do not - // want to keep the manifold with the smallest penetration detph - int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth); - - // If the manifold with the smallest penetration depth is not the new one, - // we have to keep the new manifold and remove the one with the smallest depth - if (smallestPenDepthManifoldIndex >= 0) { - removeManifold(smallestPenDepthManifoldIndex); - createManifold(contactManifoldInfo, normalDirectionId); - } - } + // Create a new contact manifold + createManifold(contactManifoldInfo); } } + + // Create a new contact manifold + createManifold(contactManifoldInfo); } } // Update a previous similar manifold with a new one -void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) { +void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) { - // For each contact point of the previous manifold - for (int i=0; igetNbContactPoints(); ) { + assert(oldManifold != nullptr); + assert(newManifold != nullptr); - ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i); + // For each contact point of the new manifold + ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo(); + assert(contactPointInfo != nullptr); + while (contactPointInfo != nullptr) { - // For each contact point in the new manifold - ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo(); - bool needToRemovePoint = true; - while (newPoint != nullptr) { + // For each contact point in the old manifold + bool isSimilarPointFound = false; + ContactPoint* oldContactPoint = oldManifold->getContactPoints(); + while (oldContactPoint != nullptr) { + + assert(oldContactPoint != nullptr); // If the new contact point is similar (very close) to the old contact point - if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) { + if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { // Replace (update) the old contact point with the new one - contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(), - mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform()); - needToRemovePoint = false; - newPoint->isUsed = true; + oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + isSimilarPointFound = true; break; } - newPoint = newPoint->next; + oldContactPoint = oldContactPoint->getNext(); } - // If no new contact point is similar to the old one - if (needToRemovePoint) { + // If we have not found a similar contact point + if (!isSimilarPointFound) { - // Remove the old contact point - mManifolds[oldManifoldIndex]->removeContactPoint(i); - } - else { - i++; + // Add the contact point to the manifold + oldManifold->addContactPoint(contactPointInfo); } + + contactPointInfo = contactPointInfo->next; } - // For each point of the new manifold that have not been used yet (to update - // an existing point in the previous manifold), add it into the previous manifold - const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo(); - while (newPointInfo != nullptr) { - - if (!newPointInfo->isUsed) { - mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo); - } - - newPointInfo = newPointInfo->next; - } + // The old manifold is no longer obselete + oldManifold->setIsObselete(false, false); } -// Return the manifold with the smallest contact penetration depth -int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { +// Return the manifold with the smallest contact penetration depth among its points +ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { - // The contact point will be in a new contact manifold, we now have too much - // manifolds condidates. We need to remove one. We choose to remove the manifold - // with the smallest contact depth among their points - int smallestDepthManifoldIndex = -1; - decimal minDepth = initDepth; assert(mNbManifolds == mNbMaxManifolds); - for (int i=0; igetLargestContactDepth(); + + ContactManifold* minDepthManifold = nullptr; + decimal minDepth = initDepth; + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + decimal depth = manifold->getLargestContactDepth(); if (depth < minDepth) { minDepth = depth; - smallestDepthManifoldIndex = i; + minDepthManifold = manifold; } + + manifold = manifold->getNext(); } - return smallestDepthManifoldIndex; + return minDepthManifold; } -// Return the index of the contact manifold with a similar average normal. -// If no manifold has close enough average normal, it returns -1 -int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { +// Return the contact manifold with a similar average normal. +// If no manifold has close enough average normal, it returns nullptr +ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { + + ContactManifold* manifold = mManifolds; // Return the Id of the manifold with the same normal direction id (if exists) - for (int i=0; igetNormalDirectionId()) { - return i; + while (manifold != nullptr) { + if (normalDirectionId == manifold->getContactNormalId()) { + return manifold; } + + manifold = manifold->getNext(); } - return -1; + return nullptr; } // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the // normal vector into of the of the bucket and returns a unique Id for the bucket -short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { +short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) { assert(normal.lengthSquare() > MACHINE_EPSILON); @@ -240,36 +206,100 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons // Clear the contact manifold set void ContactManifoldSet::clear() { - // Destroy all the contact manifolds - for (int i=mNbManifolds-1; i>=0; i--) { - removeManifold(i); + ContactManifold* manifold = mManifolds; + while(manifold != nullptr) { + + ContactManifold* nextManifold = manifold->getNext(); + + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); + + manifold = nextManifold; + + mNbManifolds--; } assert(mNbManifolds == 0); } // Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) { +void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) { assert(mNbManifolds < mNbMaxManifolds); - mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId); + ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator); + manifold->setPrevious(nullptr); + manifold->setNext(mManifolds); + mManifolds = manifold; + mNbManifolds++; } // Remove a contact manifold from the set -void ContactManifoldSet::removeManifold(int index) { +void ContactManifoldSet::removeManifold(ContactManifold* manifold) { assert(mNbManifolds > 0); - assert(index >= 0 && index < mNbManifolds); - // Delete the new contact - mManifolds[index]->~ContactManifold(); - mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); + ContactManifold* previous = manifold->getPrevious(); + ContactManifold* next = manifold->getNext(); - for (int i=index; (i+1) < mNbManifolds; i++) { - mManifolds[i] = mManifolds[i+1]; + if (previous != nullptr) { + previous->setNext(manifold->getNext()); } + if (next != nullptr) { + next->setPrevious(manifold->getPrevious()); + } + + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); mNbManifolds--; } + +// Make all the contact manifolds and contact points obselete +void ContactManifoldSet::makeContactsObselete() { + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + + manifold->setIsObselete(true, true); + + manifold = manifold->getNext(); + } +} + +// Clear the obselete contact manifolds and contact points +void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() { + + ContactManifold* manifold = mManifolds; + ContactManifold* previousManifold = nullptr; + while (manifold != nullptr) { + ContactManifold* nextManifold = manifold->getNext(); + + if (manifold->getIsObselete()) { + + if (previousManifold != nullptr) { + previousManifold->setNext(nextManifold); + + if (nextManifold != nullptr) { + nextManifold->setPrevious(previousManifold); + } + } + else { + mManifolds = nextManifold; + } + + // Delete the contact manifold + removeManifold(manifold); + + } + else { + manifold->clearObseleteContactPoints(); + previousManifold = manifold; + } + + manifold = nextManifold; + } +} diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 1a2bac97..0990277c 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -60,33 +60,34 @@ class ContactManifoldSet { /// Pointer to the second proxy shape of the contact ProxyShape* mShape2; - /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + /// Reference to the memory allocator for the contact manifolds + Allocator& mMemoryAllocator; /// Contact manifolds of the set - ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; + ContactManifold* mManifolds; // -------------------- Methods -------------------- // /// Create a new contact manifold and add it to the set - void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId); + void createManifold(const ContactManifoldInfo* manifoldInfo); - /// Remove a contact manifold from the set - void removeManifold(int index); - - // Return the index of the contact manifold with a similar average normal. - int selectManifoldWithSimilarNormal(short int normalDirectionId) const; - - // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) - // Each face of the cube is divided into 4x4 buckets. This method maps the - // normal vector into of the of the bucket and returns a unique Id for the bucket - short int computeCubemapNormalId(const Vector3& normal) const; + // Return the contact manifold with a similar average normal. + ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const; /// Return the manifold with the smallest contact penetration depth - int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; + ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; /// Update a previous similar manifold with a new one - void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold); + void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); + + /// Return the maximum number of contact manifolds allowed between to collision shapes + int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2); + + /// Clear the contact manifold set + void clear(); + + /// Delete a contact manifold + void removeManifold(ContactManifold* manifold); public: @@ -94,13 +95,13 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds); + Allocator& memoryAllocator); /// Destructor ~ContactManifoldSet(); /// Add a contact manifold in the set - void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); /// Return the first proxy shape ProxyShape* getShape1() const; @@ -108,17 +109,25 @@ class ContactManifoldSet { /// Return the second proxy shape ProxyShape* getShape2() const; - /// Clear the contact manifold set - void clear(); - /// Return the number of manifolds in the set int getNbContactManifolds() const; - /// Return a given contact manifold - ContactManifold* getContactManifold(int index) const; + /// Return a pointer to the first element of the linked-list of contact manifolds + ContactManifold* getContactManifolds() const; + + /// Make all the contact manifolds and contact points obselete + void makeContactsObselete(); /// Return the total number of contact points in the set of manifolds int getTotalNbContactPoints() const; + + /// Clear the obselete contact manifolds and contact points + void clearObseleteManifoldsAndContactPoints(); + + // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) + // Each face of the cube is divided into 4x4 buckets. This method maps the + // normal vector into of the of the bucket and returns a unique Id for the bucket + static short int computeCubemapNormalId(const Vector3& normal); }; // Return the first proxy shape @@ -136,21 +145,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const { return mNbManifolds; } -// Return a given contact manifold -inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { - assert(index >= 0 && index < mNbManifolds); - return mManifolds[index]; +// Return a pointer to the first element of the linked-list of contact manifolds +inline ContactManifold* ContactManifoldSet::getContactManifolds() const { + return mManifolds; } // Return the total number of contact points in the set of manifolds inline int ContactManifoldSet::getTotalNbContactPoints() const { int nbPoints = 0; - for (int i=0; igetNbContactPoints(); + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + nbPoints += manifold->getNbContactPoints(); + + manifold = manifold->getNext(); } + return nbPoints; } +// Return the maximum number of contact manifolds allowed between to collision shapes +inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) { + + // If both shapes are convex + if (shape1->isConvex() && shape2->isConvex()) { + return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + + } // If there is at least one concave shape + else { + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } +} + } #endif diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp new file mode 100644 index 00000000..22ee6666 --- /dev/null +++ b/src/collision/NarrowPhaseInfo.cpp @@ -0,0 +1,97 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include +#include "NarrowPhaseInfo.h" +#include "ContactPointInfo.h" +#include "engine/OverlappingPair.h" + +using namespace reactphysics3d; + +// Constructor +NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, + const CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, void** cachedData1, void** cachedData2) + : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), + shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), + contactPoints(nullptr), cachedCollisionData1(cachedData1), + cachedCollisionData2(cachedData2), next(nullptr) { + +} + +// Destructor +NarrowPhaseInfo::~NarrowPhaseInfo() { + + assert(contactPoints == nullptr); +} + +// Add a new contact point +void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + // Get the memory allocator + Allocator& allocator = overlappingPair->getTemporaryAllocator(); + + // Create the contact point info + ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) + ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + + // Add it into the linked list of contact points + contactPointInfo->next = contactPoints; + contactPoints = contactPointInfo; +} + +/// Take all the generated contact points and create a new potential +/// contact manifold into the overlapping pair +void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() { + overlappingPair->addPotentialContactPoints(this); +} + +// Reset the remaining contact points +void NarrowPhaseInfo::resetContactPoints() { + + // Get the memory allocator + Allocator& allocator = overlappingPair->getTemporaryAllocator(); + + // For each remaining contact point info + ContactPointInfo* element = contactPoints; + while(element != nullptr) { + + ContactPointInfo* elementToDelete = element; + + element = element->next; + + // Call the destructor + elementToDelete->~ContactPointInfo(); + + // Delete the current element + allocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + contactPoints = nullptr; +} diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index b97a71be..29bc6c9d 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -28,6 +28,7 @@ // Libraries #include "shapes/CollisionShape.h" +#include "collision/ContactManifoldInfo.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -58,6 +59,9 @@ struct NarrowPhaseInfo { /// Transform that maps from collision shape 2 local-space to world-space Transform shape2ToWorldTransform; + /// Linked-list of contact points created during the narrow-phase + ContactPointInfo* contactPoints; + /// Cached collision data of the proxy shape // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 void** cachedCollisionData1; @@ -72,12 +76,20 @@ struct NarrowPhaseInfo { /// Constructor NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, const CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2) - : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), - shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), - cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) { + const Transform& shape2Transform, void** cachedData1, void** cachedData2); - } + /// Destructor + ~NarrowPhaseInfo(); + + /// Add a new contact point + void addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2); + + /// Create a new potential contact manifold into the overlapping pair using current contact points + void addContactPointsAsPotentialContactManifold(); + + /// Reset the remaining contact points + void resetContactPoints(); }; } diff --git a/src/collision/OverlapCallback.h b/src/collision/OverlapCallback.h new file mode 100644 index 00000000..bbba73d1 --- /dev/null +++ b/src/collision/OverlapCallback.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H +#define REACTPHYSICS3D_OVERLAP_CALLBACK_H + +// Libraries +#include "body/CollisionBody.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class OverlapCallback +/** + * This class can be used to register a callback for collision overlap queries. + * You should implement your own class inherited from this one and implement + * the notifyOverlap() method. This method will called each time a contact + * point is reported. + */ +class OverlapCallback { + + public: + + /// Destructor + virtual ~OverlapCallback() { + + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody)=0; +}; + +} + +#endif diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 17008622..f98e4d20 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); @@ -116,8 +116,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - segmentsDistance; // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); return true; } @@ -148,7 +148,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal penetrationDepth = sumRadius - closestPointsDistance; // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); return true; } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 9d8393c9..0b19338f 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -61,8 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index b538dfa7..88bd9ace 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -37,13 +37,12 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a capsule and a polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; SATAlgorithm satAlgorithm; - GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; @@ -61,8 +60,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK - ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); - assert(contactPoint != nullptr); + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + assert(contactPoint != nullptr); bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; @@ -96,7 +95,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK - contactManifoldInfo.reset(); + narrowPhaseInfo->resetContactPoints(); const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; @@ -116,7 +115,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - contactManifoldInfo, isCapsuleShape1); + narrowPhaseInfo, isCapsuleShape1); break; } @@ -133,7 +132,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index 6b3f504f..c0dd46d5 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index c0da7611..7f5f439b 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -141,114 +141,114 @@ void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI // Process the concave triangle mesh collision using the smooth mesh collision algorithm described // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision // issue with some internal edges. -void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { +//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, +// std::vector contactPoints, +// NarrowPhaseCallback* narrowPhaseCallback) { - // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; +// // Set with the triangle vertices already processed to void further contacts with same triangle +// std::unordered_multimap processTriangleVertices; - // Sort the list of narrow-phase contacts according to their penetration depth - std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); +// // Sort the list of narrow-phase contacts according to their penetration depth +// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); - // For each contact point (from smaller penetration depth to larger) - std::vector::const_iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { +// // For each contact point (from smaller penetration depth to larger) +// std::vector::const_iterator it; +// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - const SmoothMeshContactInfo info = *it; - const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; +// const SmoothMeshContactInfo info = *it; +// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], - info.triangleVertices[1], - info.triangleVertices[2], - contactPoint, u, v, w); - int nbZeros = 0; - bool isUZero = approxEqual(u, 0, 0.0001); - bool isVZero = approxEqual(v, 0, 0.0001); - bool isWZero = approxEqual(w, 0, 0.0001); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; +// // Compute the barycentric coordinates of the point in the triangle +// decimal u, v, w; +// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], +// info.triangleVertices[1], +// info.triangleVertices[2], +// contactPoint, u, v, w); +// int nbZeros = 0; +// bool isUZero = approxEqual(u, 0, 0.0001); +// bool isVZero = approxEqual(v, 0, 0.0001); +// bool isWZero = approxEqual(w, 0, 0.0001); +// if (isUZero) nbZeros++; +// if (isVZero) nbZeros++; +// if (isWZero) nbZeros++; - // If it is a vertex contact - if (nbZeros == 2) { +// // If it is a vertex contact +// if (nbZeros == 2) { - Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); +// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - // Check that this triangle vertex has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { +// // Check that this triangle vertex has not been processed yet +// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } - } - else if (nbZeros == 1) { // If it is an edge contact +// // Keep the contact as it is and report it +// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); +// } +// } +// else if (nbZeros == 1) { // If it is an edge contact - Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); +// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); +// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); - // Check that this triangle edge has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && - !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { +// // Check that this triangle edge has not been processed yet +// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && +// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } +// // Keep the contact as it is and report it +// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); +// } - } - else { // If it is a face contact +// } +// else { // If it is a face contact - ContactPointInfo newContactInfo(info.contactInfo); +// ContactPointInfo newContactInfo(info.contactInfo); - ProxyShape* firstShape; - ProxyShape* secondShape; - if (info.isFirstShapeTriangle) { - firstShape = overlappingPair->getShape1(); - secondShape = overlappingPair->getShape2(); - } - else { - firstShape = overlappingPair->getShape2(); - secondShape = overlappingPair->getShape1(); - } +// ProxyShape* firstShape; +// ProxyShape* secondShape; +// if (info.isFirstShapeTriangle) { +// firstShape = overlappingPair->getShape1(); +// secondShape = overlappingPair->getShape2(); +// } +// else { +// firstShape = overlappingPair->getShape2(); +// secondShape = overlappingPair->getShape1(); +// } - // We use the triangle normal as the contact normal - Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; - Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; - Vector3 localNormal = a.cross(b); - newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; - newContactInfo.normal.normalize(); - if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { - newContactInfo.normal = -newContactInfo.normal; - } +// // We use the triangle normal as the contact normal +// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; +// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; +// Vector3 localNormal = a.cross(b); +// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; +// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; +// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; +// newContactInfo.normal.normalize(); +// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { +// newContactInfo.normal = -newContactInfo.normal; +// } - // We recompute the contact point on the second body with the new normal as described in - // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and - // Dirk Gregorius) to avoid adding torque - Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); - if (info.isFirstShapeTriangle) { - Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; - newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; - } - else { - Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; - newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; - } +// // We recompute the contact point on the second body with the new normal as described in +// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and +// // Dirk Gregorius) to avoid adding torque +// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); +// if (info.isFirstShapeTriangle) { +// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; +// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; +// } +// else { +// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; +// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; +// } - // Report the contact - narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); - } +// // Report the contact +// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); +// } - // Add the three vertices of the triangle to the set of processed - // triangle vertices - addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); - } -} +// // Add the three vertices of the triangle to the set of processed +// // triangle vertices +// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); +// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); +// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); +// } +//} // Return true if the vertex is in the set of already processed vertices bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h index 531615da..4619e480 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -83,25 +83,35 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { // Class SmoothMeshContactInfo /** - * This class is used to store data about a contact with a triangle for the smooth - * mesh algorithm. + * Contains data for of potential smooth contact during the smooth mesh + * contacts computation. */ -class SmoothMeshContactInfo { +struct SmoothMeshContactInfo { public: - ContactPointInfo contactInfo; + ContactManifoldInfo* contactManifoldInfo; + ContactPointInfo* contactInfo; bool isFirstShapeTriangle; Vector3 triangleVertices[3]; + bool isUVWZero[3]; /// Constructor - SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, - const Vector3& trianglePoint2, const Vector3& trianglePoint3) - : contactInfo(contact) { + SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo, + bool firstShapeTriangle, + const Vector3& trianglePoint1, const Vector3& trianglePoint2, + const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero) + : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) { + isFirstShapeTriangle = firstShapeTriangle; + triangleVertices[0] = trianglePoint1; triangleVertices[1] = trianglePoint2; triangleVertices[2] = trianglePoint3; + + isUVWZero[0] = isUZero; + isUVWZero[1] = isVZero; + isUVWZero[2] = isWZero; } }; @@ -109,7 +119,7 @@ class SmoothMeshContactInfo { struct ContactsDepthCompare { bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) { - return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; + return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth; } }; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 16b9edf5..c6297fb2 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -34,12 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index ed76d6fc..1e0c9748 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 4f7e1177..ff81782f 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -46,8 +46,7 @@ using namespace reactphysics3d; /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. -GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { PROFILE("GJKAlgorithm::testCollision()"); @@ -209,7 +208,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro } // Add a new contact point - contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB); + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); return GJKResult::COLLIDE_IN_MARGIN; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index f40592c4..b8c7319c 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -87,16 +87,13 @@ class GJKAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volumes collide. - GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo); + GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); - - }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index f665181b..035bd8e5 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -83,8 +83,10 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; + // TODO : Use the following reportContacts variable in all narrow-phase algorithms + /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index c838d8ab..4974d367 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -44,7 +44,7 @@ using namespace reactphysics3d; const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Test collision between a sphere and a convex mesh -bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()"); @@ -145,7 +145,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); // Create the contact info object - contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); @@ -171,7 +171,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd } // Test collision between a capsule and a convex mesh -bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()"); @@ -387,7 +387,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - contactManifoldInfo, isCapsuleShape1); + narrowPhaseInfo, isCapsuleShape1); lastFrameInfo.satIsAxisFacePolyhedron1 = true; lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; @@ -406,7 +406,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; // Create the contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); @@ -477,7 +477,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, - ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const { + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); uint firstEdgeIndex = face.edgeIndex; @@ -523,7 +523,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; // Create the contact point - contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, + narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); } @@ -543,8 +543,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, + bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); @@ -869,7 +869,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); // Create a new contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } @@ -893,7 +893,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // Create the contact point - contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); lastFrameInfo.satIsAxisFacePolyhedron1 = false; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 7a002fce..49cd2626 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -113,24 +113,24 @@ class SATAlgorithm { SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; /// Test collision between a sphere and a convex mesh - bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; /// Test collision between a capsule and a convex mesh - bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, - ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const; + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const; // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, const Vector3& edgeAdjacentFace2Normal) const; /// Test collision between two convex meshes - bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; }; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 86255c2b..e291fd83 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -34,7 +34,7 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; @@ -86,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI } // Create the contact info object - contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 154f6e4c..bb9e9866 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index d90a29b3..049ce449 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -34,12 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -61,7 +60,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index f2f8d6e5..2d9fa801 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 54e51e52..94211ac0 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -30,8 +30,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); @@ -62,7 +61,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, + narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1, intersectionOnBody2); return true; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index b70befd2..1fd885a5 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,8 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactManifoldInfo& contactManifoldInfo) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; }; } diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d2b92dcc..5a07b0cb 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -237,6 +237,8 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { case 4: return Vector3(0, -1, 0); case 5: return Vector3(0, 1, 0); } + + assert(false); } // Return the centroid of the box diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 2b1fa576..78b82ae0 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -118,10 +118,6 @@ class CollisionShape { /// Return true if the collision shape type is a convex shape static bool isConvex(CollisionShapeType shapeType); - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types - static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2); - // -------------------- Friendship -------------------- // friend class ProxyShape; @@ -151,19 +147,6 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } -// Return the maximum number of contact manifolds allowed in an overlapping -// pair wit the given two collision shape types -inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2) { - // If both shapes are convex - if (isConvex(shapeType1) && isConvex(shapeType2)) { - return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; - } // If there is at least one concave shape - else { - return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; - } -} - } #endif diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index f420bbff..fa6838ec 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -37,19 +37,20 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), - mIsRestingContact(false) { + mIsRestingContact(false), mIsObselete(false), mNext(nullptr) { assert(mPenetrationDepth > decimal(0.0)); // Compute the world position of the contact points mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; + + mIsObselete = false; } // Update the contact point with a new one that is similar (very close) /// The idea is to keep the cache impulse (for warm starting the contact solver) -void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform) { +void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) { assert(isSimilarWithContactPoint(contactInfo)); assert(contactInfo->penetrationDepth > decimal(0.0)); @@ -62,4 +63,6 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& // Compute the world position of the contact points mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; + + mIsObselete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index ec4c6556..41cc35e4 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -71,6 +71,37 @@ class ContactPoint { /// Cached penetration impulse decimal mPenetrationImpulse; + /// True if the contact point is obselete + bool mIsObselete; + + /// Pointer to the next contact point in the linked-list + ContactPoint* mNext; + + // -------------------- Methods -------------------- // + + /// Update the contact point with a new one that is similar (very close) + void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, + const Transform& body2Transform); + + /// Return true if the contact point is similar (close enougth) to another given contact point + bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; + + /// Set the cached penetration impulse + void setPenetrationImpulse(decimal impulse); + + + /// Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); + + /// Set to true to make the contact point obselete + void setIsObselete(bool isObselete); + + /// Set the next contact point in the linked list + void setNext(ContactPoint* next); + + /// Return true if the contact point is obselete + bool getIsObselete() const; + public : // -------------------- Methods -------------------- // @@ -88,10 +119,6 @@ class ContactPoint { /// Deleted assignment operator ContactPoint& operator=(const ContactPoint& contact) = delete; - /// Update the contact point with a new one that is similar (very close) - void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform); - /// Return the normal vector of the contact Vector3 getNormal() const; @@ -110,23 +137,22 @@ class ContactPoint { /// Return the cached penetration impulse decimal getPenetrationImpulse() const; - /// Return true if the contact point is similar (close enougth) to another given contact point - bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; - - /// Set the cached penetration impulse - void setPenetrationImpulse(decimal impulse); - /// Return true if the contact is a resting contact bool getIsRestingContact() const; - /// Set the mIsRestingContact variable - void setIsRestingContact(bool isRestingContact); + /// Return the next contact point in the linked list + ContactPoint* getNext() const; /// Return the penetration depth decimal getPenetrationDepth() const; /// Return the number of bytes used by the contact point size_t getSizeInBytes() const; + + // Friendship + friend class ContactManifold; + friend class ContactManifoldSet; + friend class ContactSolver; }; // Return the normal vector of the contact @@ -180,6 +206,26 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } +// Return true if the contact point is obselete +inline bool ContactPoint::getIsObselete() const { + return mIsObselete; +} + +// Set to true to make the contact point obselete +inline void ContactPoint::setIsObselete(bool isObselete) { + mIsObselete = isObselete; +} + +// Return the next contact point in the linked list +inline ContactPoint* ContactPoint::getNext() const { + return mNext; +} + +// Set the next contact point in the linked list +inline void ContactPoint::setNext(ContactPoint* next) { + mNext = next; +} + // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 0e24f0c8..82d2601f 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -233,64 +233,6 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits); } -// Class CollisionCallback -/** - * This class can be used to register a callback for collision test queries. - * You should implement your own class inherited from this one and implement - * the notifyContact() method. This method will called each time a contact - * point is reported. - */ -class CollisionCallback { - - public: - - struct CollisionCallbackInfo { - - public: - const ContactManifoldInfo& contactManifold; - CollisionBody* body1; - CollisionBody* body2; - const ProxyShape* proxyShape1; - const ProxyShape* proxyShape2; - - // Constructor - CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2, - const ProxyShape* proxShape1, const ProxyShape* proxShape2) : - contactManifold(manifold), body1(b1), body2(b2), - proxyShape1(proxShape1), proxyShape2(proxShape2) { - - } - }; - - /// Destructor - virtual ~CollisionCallback() { - - } - - /// This method will be called for each reported contact point - virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; -}; - -// Class OverlapCallback -/** - * This class can be used to register a callback for collision overlap queries. - * You should implement your own class inherited from this one and implement - * the notifyOverlap() method. This method will called each time a contact - * point is reported. - */ -class OverlapCallback { - - public: - - /// Destructor - virtual ~OverlapCallback() { - - } - - /// This method will be called for each reported overlapping bodies - virtual void notifyOverlap(CollisionBody* collisionBody)=0; -}; - } #endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d5825d5c..94a82b14 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -148,10 +148,8 @@ void ContactSolver::initializeForIsland(Island* island) { const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { - - // Get a contact point - ContactPoint* externalContact = externalManifold->getContactPoint(c); + ContactPoint* externalContact = externalManifold->getContactPoints(); + while (externalContact != nullptr) { // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); @@ -200,6 +198,8 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; mNbContactPoints++; + + externalContact = externalContact->getNext(); } mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 4f7bd15a..136d34f6 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -683,9 +683,9 @@ void DynamicsWorld::computeIslands() { // For each contact manifold in which the current body is involded ContactManifoldListElement* contactElement; for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr; - contactElement = contactElement->next) { + contactElement = contactElement->getNext()) { - ContactManifold* contactManifold = contactElement->contactManifold; + ContactManifold* contactManifold = contactElement->getContactManifold(); assert(contactManifold->getNbContactPoints() > 0); @@ -842,12 +842,13 @@ std::vector DynamicsWorld::getContactsList() const { // For each contact manifold of the pair const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int i=0; igetNext(); } } diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h index e8594255..061724a0 100644 --- a/src/engine/EventListener.h +++ b/src/engine/EventListener.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_EVENT_LISTENER_H // Libraries -#include "collision/ContactManifoldInfo.h" +#include "collision/CollisionCallback.h" namespace reactphysics3d { @@ -49,17 +49,11 @@ class EventListener { /// Destructor virtual ~EventListener() = default; - /// Called when a new contact point is found between two bodies that were separated before - /** - * @param contact Information about the contact - */ - virtual void beginContact(const ContactManifoldInfo& contactManifold) {} - /// Called when a new contact point is found between two bodies /** * @param contact Information about the contact */ - virtual void newContact(const ContactManifoldInfo& contactManifold) {} + virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {} /// Called at the beginning of an internal tick of the simulation step. /// Each time the DynamicsWorld::update() method is called, the physics diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index ce628b7d..a23857e3 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -26,12 +26,108 @@ // Libraries #include #include "OverlappingPair.h" +#include "collision/ContactManifoldInfo.h" using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) { + Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator) + : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr), + mTempMemoryAllocator(temporaryMemoryAllocator) { } + +// Create a new potential contact manifold using contact-points from narrow-phase +void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { + + assert(narrowPhaseInfo->contactPoints != nullptr); + + // For each potential contact point to add + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + while (contactPoint != nullptr) { + + ContactPointInfo* nextContactPoint = contactPoint->next; + + // Compute the contact normal id + short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal); + + // Look if the contact point correspond to an existing potential manifold + // (if the contact point normal is similar to the normal of an existing manifold) + ContactManifoldInfo* manifold = mPotentialContactManifolds; + bool similarManifoldFound = false; + while(manifold != nullptr) { + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (manifold->getContactNormalId() == contactNormalId) { + + // Add the contact point to the manifold + manifold->addContactPoint(contactPoint, contactNormalId); + + similarManifoldFound = true; + + break; + } + + manifold = manifold->getNext(); + } + + // If we have not found an existing manifold with a similar contact normal + if (!similarManifoldFound) { + + // Create a new potential contact manifold + ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo))) + ContactManifoldInfo(mTempMemoryAllocator); + + // Add the manifold into the linked-list of potential contact manifolds + manifoldInfo->mNext = mPotentialContactManifolds; + mPotentialContactManifolds = manifoldInfo; + + // Add the contact point to the manifold + manifoldInfo->addContactPoint(contactPoint, contactNormalId); + } + + contactPoint = nextContactPoint; + } + + // All the contact point info of the narrow-phase info have been moved + // into the potential contacts of the overlapping pair + narrowPhaseInfo->contactPoints = nullptr; +} + +// Clear all the potential contact manifolds +void OverlappingPair::clearPotentialContactManifolds() { + + // Do we need to release memory + if (mTempMemoryAllocator.isReleaseNeeded()) { + + ContactManifoldInfo* element = mPotentialContactManifolds; + while(element != nullptr) { + + // Remove the proxy collision shape + ContactManifoldInfo* elementToRemove = element; + element = element->getNext(); + + // Delete the element + elementToRemove->~ContactManifoldInfo(); + mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); + } + } + + mPotentialContactManifolds = nullptr; +} + +// Reduce the number of contact points of all the potential contact manifolds +void OverlappingPair::reducePotentialContactManifolds() { + + // For each potential contact manifold + ContactManifoldInfo* manifold = mPotentialContactManifolds; + while (manifold != nullptr) { + + // Reduce the number of contact points of the manifold + manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform()); + + manifold = manifold->getNext(); + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index ce028f9c..a55df081 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -102,13 +102,19 @@ class OverlappingPair { /// Collision information about the last frame (for temporal coherence) LastFrameCollisionInfo mLastFrameCollisionInfo; + /// Linked-list of potential contact manifold + ContactManifoldInfo* mPotentialContactManifolds; + + /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo + Allocator& mTempMemoryAllocator; + public: // -------------------- Methods -------------------- // /// Constructor OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator); + Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator); /// Destructor ~OverlappingPair() = default; @@ -125,9 +131,6 @@ class OverlappingPair { /// Return the pointer to second body ProxyShape* getShape2() const; - /// Add a contact manifold - void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); - /// Return the last frame collision info LastFrameCollisionInfo& getLastFrameCollisionInfo(); @@ -137,8 +140,32 @@ class OverlappingPair { /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); - /// Clear the contact points of the contact manifold - void clearContactPoints(); + /// Clear all the potential contact manifolds + void clearPotentialContactManifolds(); + + /// Add potential contact-points from narrow-phase into potential contact manifolds + void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo); + + /// Add a contact to the contact manifold + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); + + /// Return a reference to the temporary memory allocator + Allocator& getTemporaryAllocator(); + + /// Return true if one of the shapes of the pair is a concave shape + bool hasConcaveShape() const; + + /// Return a pointer to the first potential contact manifold in the linked-list + ContactManifoldInfo* getPotentialContactManifolds(); + + /// Reduce the number of contact points of all the potential contact manifolds + void reducePotentialContactManifolds(); + + /// Make the contact manifolds and contact points obselete + void makeContactsObselete(); + + /// Clear the obselete contact manifold and contact points + void clearObseleteManifoldsAndContactPoints(); /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -162,7 +189,7 @@ inline ProxyShape* OverlappingPair::getShape2() const { } // Add a contact to the contact manifold -inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) { +inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { mContactManifoldSet.addContactManifold(contactManifoldInfo); } @@ -181,6 +208,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { return mContactManifoldSet; } +// Make the contact manifolds and contact points obselete +inline void OverlappingPair::makeContactsObselete() { + + mContactManifoldSet.makeContactsObselete(); +} + // Return the pair of bodies index inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); @@ -205,9 +238,25 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body return indexPair; } -// Clear the contact points of the contact manifold -inline void OverlappingPair::clearContactPoints() { - mContactManifoldSet.clear(); +// Return a reference to the temporary memory allocator +inline Allocator& OverlappingPair::getTemporaryAllocator() { + return mTempMemoryAllocator; +} + +// Return true if one of the shapes of the pair is a concave shape +inline bool OverlappingPair::hasConcaveShape() const { + return !getShape1()->getCollisionShape()->isConvex() || + !getShape2()->getCollisionShape()->isConvex(); +} + +// Return a pointer to the first potential contact manifold in the linked-list +inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { + return mPotentialContactManifolds; +} + +// Clear the obselete contact manifold and contact points +inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() { + mContactManifoldSet.clearObseleteManifoldsAndContactPoints(); } } diff --git a/src/memory/Allocator.h b/src/memory/Allocator.h index f7b9ac73..0440a5c9 100644 --- a/src/memory/Allocator.h +++ b/src/memory/Allocator.h @@ -49,6 +49,9 @@ class Allocator { /// Release previously allocated memory. virtual void release(void* pointer, size_t size)=0; + + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const=0; }; } diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 71e11714..0664e559 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -138,8 +138,16 @@ class PoolAllocator : public Allocator { /// Release previously allocated memory. virtual void release(void* pointer, size_t size) override; + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override; + }; +// Return true if memory needs to be release with this allocator +inline bool PoolAllocator::isReleaseNeeded() const { + return true; +} + } #endif diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h index cc030daa..426c2a2d 100644 --- a/src/memory/SingleFrameAllocator.h +++ b/src/memory/SingleFrameAllocator.h @@ -72,10 +72,15 @@ class SingleFrameAllocator : public Allocator { /// Reset the marker of the current allocated memory virtual void reset(); - - + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override; }; +// Return true if memory needs to be release with this allocator +inline bool SingleFrameAllocator::isReleaseNeeded() const { + return false; +} + } #endif diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index f6b8b8bf..dd2a6265 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -58,6 +58,8 @@ #include "collision/PolyhedronMesh.h" #include "collision/TriangleVertexArray.h" #include "collision/PolygonVertexArray.h" +#include "collision/CollisionCallback.h" +#include "collision/OverlapCallback.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 51a05412..14ceab1f 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -79,26 +79,34 @@ class ContactManager : public rp3d::CollisionCallback { /// This method will be called for each reported contact point virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - // For each contact point - rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo(); - while (contactPointInfo != nullptr) { + // For each contact manifold + rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements; + while (manifoldElement != nullptr) { - // Contact normal - rp3d::Vector3 normal = contactPointInfo->normal; - openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); + // Get the contact manifold + rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold(); - rp3d::Vector3 point1 = contactPointInfo->localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); + // For each contact point + rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { - rp3d::Vector3 point2 = contactPointInfo->localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); + // Contact normal + rp3d::Vector3 normal = contactPoint->getNormal(); + openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); - contactPointInfo = contactPointInfo->next; + rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1(); + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); + + rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2(); + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); + + contactPoint = contactPoint->getNext(); + } } } diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 2cb3e440..85e8a66f 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -342,14 +342,16 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn const rp3d::ContactManifold* manifold = *it; // For each contact point of the manifold - for (uint i=0; igetNbContactPoints(); i++) { + rp3d::ContactPoint* contactPoint = manifold->getContactPoints(); + while (contactPoint != nullptr) { - rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); rp3d::Vector3 normalWorld = contactPoint->getNormal(); openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); contactPoints.push_back(contact); + + contactPoint = contactPoint->getNext(); } } From b6ad69b2786111c965a8560516fc5d12c1abb815 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 30 Jul 2017 23:56:20 +0200 Subject: [PATCH 032/105] Do not compute contacts if not necessary --- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 66 +++--- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 100 ++++---- .../narrowphase/NarrowPhaseAlgorithm.h | 2 - .../narrowphase/SAT/SATAlgorithm.cpp | 214 +++++++++--------- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 33 +-- .../narrowphase/SphereVsSphereAlgorithm.cpp | 26 ++- 6 files changed, 235 insertions(+), 206 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index f98e4d20..c98329bf 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -91,33 +91,37 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, // If the segments were overlapping (the clip segment is valid) if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - // Clip the inner segment of capsule 2 - if (t1 > decimal(1.0)) t1 = decimal(1.0); - const Vector3 clipPointA = capsule2SegB - t1 * seg2; - if (t2 > decimal(1.0)) t2 = decimal(1.0); - const Vector3 clipPointB = capsule2SegA + t2 * seg2; + if (reportContacts) { - // Project point capsule2SegA onto line of innner segment of capsule 1 - const Vector3 seg1Normalized = seg1.getUnit(); - Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; + // Clip the inner segment of capsule 2 + if (t1 > decimal(1.0)) t1 = decimal(1.0); + const Vector3 clipPointA = capsule2SegB - t1 * seg2; + if (t2 > decimal(1.0)) t2 = decimal(1.0); + const Vector3 clipPointB = capsule2SegA + t2 * seg2; - // Compute a perpendicular vector from segment 1 to segment 2 - Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); - Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + // Project point capsule2SegA onto line of innner segment of capsule 1 + const Vector3 seg1Normalized = seg1.getUnit(); + Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; - Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); - const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); - const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + // Compute a perpendicular vector from segment 1 to segment 2 + Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); + Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); - decimal penetrationDepth = sumRadius - segmentsDistance; + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + decimal penetrationDepth = sumRadius - segmentsDistance; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + + } return true; } @@ -137,18 +141,22 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, // If the collision shapes overlap if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { - decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); - closestPointsSeg1ToSeg2 /= closestPointsDistance; + if (reportContacts) { - const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); - const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); - decimal penetrationDepth = sumRadius - closestPointsDistance; + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + decimal penetrationDepth = sumRadius - closestPointsDistance; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + + } return true; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 88bd9ace..be9cc05f 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -55,70 +55,74 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { - // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the - // capsule inner segment and parallel to the contact point normal, we would like to create - // two contact points instead of a single one (as in the deep contact case with SAT algorithm) + if (reportContacts) { - // Get the contact point created by GJK - ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; - assert(contactPoint != nullptr); + // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the + // capsule inner segment and parallel to the contact point normal, we would like to create + // two contact points instead of a single one (as in the deep contact case with SAT algorithm) - bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + // Get the contact point created by GJK + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + assert(contactPoint != nullptr); - // Get the collision shapes - const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); - const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; - // For each face of the polyhedron - for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); - // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(f); + // For each face of the polyhedron + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; - const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; - - // Get the face normal - const Vector3 faceNormal = polyhedron->getFaceNormal(f); - const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; - - const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); - - bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); - bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); - - // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal - // is in direction of the contact normal (from the polyhedron point of view). - if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) - && areParallelVectors(faceNormalWorld, contactPoint->normal)) { - - // Remove the previous contact point computed by GJK - narrowPhaseInfo->resetContactPoints(); + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; - const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; - // Compute the end-points of the inner segment of the capsule + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); - // Convert the inner capsule segment points into the polyhedron local-space - const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); - const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; - const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); - const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal + // is in direction of the contact normal (from the polyhedron point of view). + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) + && areParallelVectors(faceNormalWorld, contactPoint->normal)) { - // Compute and create two contact points - satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, - polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, - capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - narrowPhaseInfo, isCapsuleShape1); + // Remove the previous contact point computed by GJK + narrowPhaseInfo->resetContactPoints(); - break; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + + // Compute and create two contact points + satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + narrowPhaseInfo, isCapsuleShape1); + + break; + } } + } narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 035bd8e5..6a8746c6 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -83,8 +83,6 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; - // TODO : Use the following reportContacts variable in all narrow-phase algorithms - /// Compute a contact info if the two bounding volume collide virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; }; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4974d367..98686311 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -139,15 +139,18 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow } } - const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); - Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); - const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); - const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + if (reportContacts) { - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, - isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); + const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); + Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); + const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); + const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); + } lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; @@ -384,34 +387,39 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro // We need to clip the inner capsule segment with the adjacent faces of the separating face if (isMinPenetrationFaceNormal) { - computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, - polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, - capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - narrowPhaseInfo, isCapsuleShape1); + if (reportContacts) { + + computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, + polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + narrowPhaseInfo, isCapsuleShape1); + } lastFrameInfo.satIsAxisFacePolyhedron1 = true; lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; } else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment - // Compute the closest points between the inner capsule segment and the - // edge of the polyhedron in polyhedron local-space - Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment; - computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, - separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2, - closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); + if (reportContacts) { + // Compute the closest points between the inner capsule segment and the + // edge of the polyhedron in polyhedron local-space + Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment; + computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2, + closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); - // Project closest capsule inner segment point into the capsule bounds - const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + // Project closest capsule inner segment point into the capsule bounds + const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; - // Create the contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, - isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + // Create the contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + } - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satMinEdge1Index = minEdgeIndex; + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satMinEdge1Index = minEdgeIndex; } return true; @@ -543,8 +551,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, - bool reportContacts) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); @@ -787,91 +794,93 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // If the minimum separating axis is a face normal if (isMinPenetrationFaceNormal) { - const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; - const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; - const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; - const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + if (reportContacts) { - assert(minPenetrationDepth > decimal(0.0)); + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; + const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; + const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; - const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); - const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; + assert(minPenetrationDepth > decimal(0.0)); - // Compute the world normal - const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : - -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); + const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; - // Get the reference face - HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + // Compute the world normal + const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); - // Find the incident face on the other polyhedron (most anti-parallel face) - uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + // Get the reference face + HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); - // Get the incident face - HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + // Find the incident face on the other polyhedron (most anti-parallel face) + uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); - std::vector polygonVertices; // Vertices to clip of the incident face - std::vector planesNormals; // Normals of the clipping planes - std::vector planesPoints; // Points on the clipping planes + // Get the incident face + HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); - // Get all the vertices of the incident face (in the reference local-space) - std::vector::const_iterator it; - for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { - const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); - polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); - } + std::vector polygonVertices; // Vertices to clip of the incident face + std::vector planesNormals; // Normals of the clipping planes + std::vector planesPoints; // Points on the clipping planes - // Get the reference face clipping planes - uint currentEdgeIndex = referenceFace.edgeIndex; - uint firstEdgeIndex = currentEdgeIndex; - do { + // Get all the vertices of the incident face (in the reference local-space) + std::vector::const_iterator it; + for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); + polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + } - // Get the adjacent edge - HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + // Get the reference face clipping planes + uint currentEdgeIndex = referenceFace.edgeIndex; + uint firstEdgeIndex = currentEdgeIndex; + do { - // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + // Get the adjacent edge + HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); - // Get the adjacent face normal (and negate it to have a clipping plane) - Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex); + // Get the twin edge + HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); - // Get a vertex of the clipping plane (vertex of the adjacent edge) - Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex); + // Get the adjacent face normal (and negate it to have a clipping plane) + Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex); - planesNormals.push_back(faceNormal); - planesPoints.push_back(faceVertex); + // Get a vertex of the clipping plane (vertex of the adjacent edge) + Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex); - // Go to the next adjacent edge of the reference face - currentEdgeIndex = edge.nextEdgeIndex; + planesNormals.push_back(faceNormal); + planesPoints.push_back(faceVertex); - } while (currentEdgeIndex != firstEdgeIndex); + // Go to the next adjacent edge of the reference face + currentEdgeIndex = edge.nextEdgeIndex; - assert(planesNormals.size() > 0); - assert(planesNormals.size() == planesPoints.size()); + } while (currentEdgeIndex != firstEdgeIndex); - // Clip the reference faces with the adjacent planes of the reference face - std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); - assert(clipPolygonVertices.size() > 0); + assert(planesNormals.size() > 0); + assert(planesNormals.size() == planesPoints.size()); - // We only keep the clipped points that are below the reference face - const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); - std::vector::const_iterator itPoints; - for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + // Clip the reference faces with the adjacent planes of the reference face + std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + assert(clipPolygonVertices.size() > 0); - // If the clip point is bellow the reference face - if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) - { + // We only keep the clipped points that are below the reference face + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); + std::vector::const_iterator itPoints; + for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { - // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + // If the clip point is bellow the reference face + if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { - // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space + const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); - // Create a new contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + // Project the contact point onto the reference face + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + + // Create a new contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + } } } @@ -881,20 +890,23 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn } else { // If we have an edge vs edge contact - // Compute the closest points between the two edges (in the local-space of poylhedron 2) - Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; - computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B, - closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); + if (reportContacts) { - // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 - const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; + // Compute the closest points between the two edges (in the local-space of poylhedron 2) + Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; + computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B, + closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); - // Compute the world normal - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 + const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; - // Create the contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + // Compute the world normal + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Create the contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + } lastFrameInfo.satIsAxisFacePolyhedron1 = false; lastFrameInfo.satIsAxisFacePolyhedron2 = false; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index e291fd83..52016f2f 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -71,24 +71,27 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b // If the collision shapes overlap if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) { - decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); - sphereCenterToSegment /= sphereSegmentDistance; + if (reportContacts) { - const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); - const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); - - Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; - - decimal penetrationDepth = sumRadius - sphereSegmentDistance; + decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); + sphereCenterToSegment /= sphereSegmentDistance; - if (!isSphereShape1) { - normalWorld = -normalWorld; + const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + + Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; + + decimal penetrationDepth = sumRadius - sphereSegmentDistance; + + if (!isSphereShape1) { + normalWorld = -normalWorld; + } + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, + isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); } - - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, - isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, - isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); return true; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 94211ac0..1335c911 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -52,17 +52,21 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo // If the sphere collision shapes intersect if (squaredDistanceBetweenCenters < sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); - - // Create the contact info object - narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); + + if (reportContacts) { + + Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + Vector3 intersectionOnBody1 = sphereShape1->getRadius() * + centerSphere2InBody1LocalSpace.getUnit(); + Vector3 intersectionOnBody2 = sphereShape2->getRadius() * + centerSphere1InBody2LocalSpace.getUnit(); + decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + + // Create the contact info object + narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, + intersectionOnBody1, intersectionOnBody2); + } return true; } From 2f601909423be819bf1d67e4535fe31cf8edb009 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 1 Aug 2017 15:57:46 +0200 Subject: [PATCH 033/105] Do not generate contact in GJK algorithm if not needed --- .../narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp | 1 - src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 7 +++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index be9cc05f..73a1dd13 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -122,7 +122,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh break; } } - } narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index ff81782f..10d344f1 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -207,8 +207,11 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase return GJKResult::INTERPENETRATE; } - // Add a new contact point - narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); + if (reportContacts) { + + // Add a new contact point + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); + } return GJKResult::COLLIDE_IN_MARGIN; } From 319cc72cde3052239b62cbb0f51a1f8bf8db252c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 18 Aug 2017 17:50:27 +0200 Subject: [PATCH 034/105] Fix issues in collision detection --- src/collision/CollisionCallback.cpp | 1 + src/collision/CollisionDetection.cpp | 33 ++-- src/collision/ContactManifoldSet.cpp | 4 + .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 167 ++++++++++++------ .../narrowphase/SAT/SATAlgorithm.cpp | 19 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 57 ++++-- .../narrowphase/SphereVsSphereAlgorithm.cpp | 28 ++- src/collision/shapes/ConvexMeshShape.h | 4 +- src/engine/OverlappingPair.h | 8 + 9 files changed, 227 insertions(+), 94 deletions(-) mode change 100644 => 100755 src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp mode change 100644 => 100755 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp index f470763d..db820db4 100644 --- a/src/collision/CollisionCallback.cpp +++ b/src/collision/CollisionCallback.cpp @@ -45,6 +45,7 @@ CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* // For each contact manifold in the set of manifolds in the pair ContactManifold* contactManifold = manifoldSet.getContactManifolds(); + assert(contactManifold != nullptr); while (contactManifold != nullptr) { assert(contactManifold->getNbContactPoints() > 0); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 57360706..0abd82a3 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -440,7 +440,7 @@ void CollisionDetection::reportAllContacts() { for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { // If there is a user callback - if (mWorld->mEventListener != nullptr) { + if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator); @@ -845,9 +845,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Process the potential contacts processPotentialContacts(&pair); - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - collisionCallback->notifyContact(collisionInfo); + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + collisionCallback->notifyContact(collisionInfo); + } } // Go to the next proxy shape @@ -916,10 +919,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - callback->notifyContact(collisionInfo); } } @@ -935,6 +934,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Process the potential contacts processPotentialContacts(&pair); + + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + callback->notifyContact(collisionInfo); + } } } @@ -996,10 +1002,6 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - callback->notifyContact(collisionInfo); } } @@ -1015,6 +1017,13 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Process the potential contacts processPotentialContacts(&pair); + + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + callback->notifyContact(collisionInfo); + } } } } diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index b9b7f3e8..23602dda 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -247,6 +247,10 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) { if (previous != nullptr) { previous->setNext(manifold->getNext()); } + else { + mManifolds = next; + } + if (next != nullptr) { next->setPrevious(manifold->getPrevious()); } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp old mode 100644 new mode 100755 index c98329bf..b15cb186 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -63,68 +63,88 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); // If the two capsules are parallel (we create two contact points) - if (areParallelVectors(seg1, seg2)) { + bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2); + if (areCapsuleInnerSegmentsParralel) { // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) - const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); - if (segmentsDistance >= sumRadius) { + const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); + if (segmentsPerpendicularDistance >= sumRadius) { // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. return false; } - // If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping) - // If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case, - // we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules) - if (segmentsDistance > MACHINE_EPSILON) { + // Compute the planes that goes through the extreme points of the inner segment of capsule 1 + decimal d1 = seg1.dot(capsule1SegA); + decimal d2 = -seg1.dot(capsule1SegB); - // Compute the planes that goes through the extreme points of the inner segment of capsule 1 - decimal d1 = seg1.dot(capsule1SegA); - decimal d2 = -seg1.dot(capsule1SegB); + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); - // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner - // segment of capsule 1 - decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); - decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + // If the segments were overlapping (the clip segment is valid) + if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - // If the segments were overlapping (the clip segment is valid) - if (t1 > decimal(0.0) && t2 > decimal(0.0)) { + if (reportContacts) { - if (reportContacts) { + // Clip the inner segment of capsule 2 + if (t1 > decimal(1.0)) t1 = decimal(1.0); + const Vector3 clipPointA = capsule2SegB - t1 * seg2; + if (t2 > decimal(1.0)) t2 = decimal(1.0); + const Vector3 clipPointB = capsule2SegA + t2 * seg2; - // Clip the inner segment of capsule 2 - if (t1 > decimal(1.0)) t1 = decimal(1.0); - const Vector3 clipPointA = capsule2SegB - t1 * seg2; - if (t2 > decimal(1.0)) t2 = decimal(1.0); - const Vector3 clipPointB = capsule2SegA + t2 * seg2; + // Project point capsule2SegA onto line of innner segment of capsule 1 + const Vector3 seg1Normalized = seg1.getUnit(); + Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; - // Project point capsule2SegA onto line of innner segment of capsule 1 - const Vector3 seg1Normalized = seg1.getUnit(); - Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; + Vector3 normalCapsule2SpaceNormalized; + Vector3 segment1ToSegment2; - // Compute a perpendicular vector from segment 1 to segment 2 - Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); - Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + // If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping) + if (segmentsPerpendicularDistance > MACHINE_EPSILON) { - Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); - const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); - const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); - const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); + // Compute a perpendicular vector from segment 1 to segment 2 + segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); + normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit(); + } + else { // If the capsule inner segments are overlapping (degenerate case) - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + // We cannot use the vector between segments as a contact normal. To generate a contact normal, we take + // any vector that is orthogonal to the inner capsule segments. - decimal penetrationDepth = sumRadius - segmentsDistance; + Vector3 vec1(1, 0, 0); + Vector3 vec2(0, 1, 0); - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + Vector3 seg2Normalized = seg2.getUnit(); - } + // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product) + decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2)) + decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2)) - return true; - } + segment1ToSegment2.setToZero(); + + // We choose as a contact normal, any direction that is perpendicular to the inner capsules segments + normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2); + } + + Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); + + decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + } + + return true; } } @@ -132,31 +152,72 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, Vector3 closestPointCapsule1Seg; Vector3 closestPointCapsule2Seg; computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, - closestPointCapsule1Seg, closestPointCapsule2Seg); + closestPointCapsule1Seg, closestPointCapsule2Seg); // Compute the distance between the sphere center and the closest point on the segment Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); // If the collision shapes overlap - if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { + if (closestPointsDistanceSquare < sumRadius * sumRadius) { + + if (reportContacts) { - if (reportContacts) { + // If the distance between the inner segments is not zero + if (closestPointsDistanceSquare > MACHINE_EPSILON) { - decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); - closestPointsSeg1ToSeg2 /= closestPointsDistance; + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; - const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); - const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; - decimal penetrationDepth = sumRadius - closestPointsDistance; + decimal penetrationDepth = sumRadius - closestPointsDistance; - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + } + else { // The segment are overlapping (degenerate case) - } + // If the capsule segments are parralel + if (areCapsuleInnerSegmentsParralel) { + + // The segment are parallel, not overlapping and their distance is zero. + // Therefore, the capsules are just touching at the top of their inner segments + decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare(); + + Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB; + Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint); + normalCapsuleSpace2.normalize(); + + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); + } + else { // If the capsules inner segments are not parallel + + // We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross + // product between the two segments. + Vector3 normalCapsuleSpace2 = seg1.cross(seg2); + normalCapsuleSpace2.normalize(); + + // Compute the contact points on both shapes + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); + } + } + } return true; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 98686311..cebec9a6 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -838,17 +838,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // Get the adjacent edge HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); - // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + // Get the twin edge + HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); - // Get the adjacent face normal (and negate it to have a clipping plane) - Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex); + // Compute the edge vertices and edge direction + Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); + Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); + Vector3 edgeDirection = edgeV2 - edgeV1; - // Get a vertex of the clipping plane (vertex of the adjacent edge) - Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex); + // Compute the normal of the clipping plane for this edge + // The clipping plane is perpendicular to the edge direction and the reference face normal + Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - planesNormals.push_back(faceNormal); - planesPoints.push_back(faceVertex); + planesNormals.push_back(clipPlaneNormal); + planesPoints.push_back(edgeV1); // Go to the next adjacent edge of the reference face currentEdgeIndex = edge.nextEdgeIndex; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 52016f2f..a5d7a6ac 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -69,28 +69,61 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); // If the collision shapes overlap - if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) { + if (sphereSegmentDistanceSquare < sumRadius * sumRadius) { + + decimal penetrationDepth; + Vector3 normalWorld; + Vector3 contactPointSphereLocal; + Vector3 contactPointCapsuleLocal; if (reportContacts) { - decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); - sphereCenterToSegment /= sphereSegmentDistance; + // If the sphere center is not on the capsule inner segment + if (sphereSegmentDistanceSquare > MACHINE_EPSILON) { - const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); - const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); + sphereCenterToSegment /= sphereSegmentDistance; - Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); - decimal penetrationDepth = sumRadius - sphereSegmentDistance; + normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; - if (!isSphereShape1) { - normalWorld = -normalWorld; - } + penetrationDepth = sumRadius - sphereSegmentDistance; + + if (!isSphereShape1) { + normalWorld = -normalWorld; + } + } + else { // If the sphere center is on the capsule inner segment (degenerate case) + + // We take any direction that is orthogonal to the inner capsule segment as a contact normal + + // Capsule inner segment + Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit(); + + Vector3 vec1(1, 0, 0); + Vector3 vec2(0, 1, 0); + + // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product) + decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2)) + decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2)) + + penetrationDepth = sumRadius; + + // We choose as a contact normal, any direction that is perpendicular to the inner capsule segment + Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2); + normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace; + + // Compute the two local contact points + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius()); + contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius(); + } // Create the contact info object narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, - isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, - isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); + isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, + isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); } return true; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp old mode 100644 new mode 100755 index 1335c911..0b520213 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -57,15 +57,29 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + Vector3 intersectionOnBody1; + Vector3 intersectionOnBody2; + Vector3 normal; - // Create the contact info object - narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); + // If the two sphere centers are not at the same position + if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { + + intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit(); + intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit(); + normal = vectorBetweenCenters.getUnit(); + } + else { // If the sphere centers are at the same position (degenerate case) + + // Take any contact normal direction + normal.setAllValues(0, 1, 0); + + intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal); + intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal); + } + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); } return true; diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 28ba9c86..dd2b7021 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -233,7 +233,7 @@ inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) cons // Return the position of a given vertex inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < getNbVertices()); - return mPolyhedronMesh->getVertex(vertexIndex); + return mPolyhedronMesh->getVertex(vertexIndex) * mScaling; } // Return the normal vector of a given face of the polyhedron @@ -244,7 +244,7 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { // Return the centroid of the polyhedron inline Vector3 ConvexMeshShape::getCentroid() const { - return mPolyhedronMesh->getCentroid(); + return mPolyhedronMesh->getCentroid() * mScaling; } } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index a55df081..906cddb7 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -155,6 +155,9 @@ class OverlappingPair { /// Return true if one of the shapes of the pair is a concave shape bool hasConcaveShape() const; + /// Return true if the overlapping pair has contact manifolds with contacts + bool hasContacts() const; + /// Return a pointer to the first potential contact manifold in the linked-list ContactManifoldInfo* getPotentialContactManifolds(); @@ -249,6 +252,11 @@ inline bool OverlappingPair::hasConcaveShape() const { !getShape2()->getCollisionShape()->isConvex(); } +// Return true if the overlapping pair has contact manifolds with contacts +inline bool OverlappingPair::hasContacts() const { + return mContactManifoldSet.getContactManifolds() != nullptr; +} + // Return a pointer to the first potential contact manifold in the linked-list inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { return mPotentialContactManifolds; From 11589dbb2cc5e8048260f90c548ed464e833fd5e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 18 Aug 2017 17:51:10 +0200 Subject: [PATCH 035/105] Edit collision detection scene --- .../CollisionDetectionScene.cpp | 49 +++++-------------- .../CollisionDetectionScene.h | 7 ++- 2 files changed, 15 insertions(+), 41 deletions(-) diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index e8f09670..a99d494d 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere2->setSleepingColor(mRedColorDemo); // ---------- Capsule 1 ---------- // - openglframework::Vector3 position3(0, -12, 0); + openglframework::Vector3 position3(-6, 7, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); @@ -83,7 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule1->setSleepingColor(mRedColorDemo); // ---------- Capsule 2 ---------- // - openglframework::Vector3 position4(-8, 0, 0); + openglframework::Vector3 position4(11, -8, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath); @@ -115,38 +115,16 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mBox2->setColor(mGreyColorDemo); mBox2->setSleepingColor(mRedColorDemo); - // ---------- Cone ---------- // - //openglframework::Vector3 position4(0, 0, 0); - - // Create a cone and a corresponding collision body in the dynamics world - //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, - // mMeshFolderPath); - - // Set the color - //mCone->setColor(mGreyColorDemo); - //mCone->setSleepingColor(mRedColorDemo); - - // ---------- Cylinder ---------- // - //openglframework::Vector3 position5(0, 0, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, - // mCollisionWorld, mMeshFolderPath); - - // Set the color - //mCylinder->setColor(mGreyColorDemo); - //mCylinder->setSleepingColor(mRedColorDemo); - - // ---------- Convex Mesh ---------- // - //openglframework::Vector3 position7(0, 0, 0); + openglframework::Vector3 position7(-5, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + mAllShapes.push_back(mConvexMesh); // Set the color - //mConvexMesh->setColor(mGreyColorDemo); - //mConvexMesh->setSleepingColor(mRedColorDemo); + mConvexMesh->setColor(mGreyColorDemo); + mConvexMesh->setSleepingColor(mRedColorDemo); // ---------- Concave Mesh ---------- // //openglframework::Vector3 position8(0, 0, 0); @@ -202,6 +180,9 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody()); delete mBox2; + mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + delete mConvexMesh; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -217,13 +198,7 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); // Destroy the sphere - delete mCapsule; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConvexMesh; + delete mCapsule; // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); @@ -280,13 +255,13 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 14ceab1f..d0f1b569 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -107,6 +107,8 @@ class ContactManager : public rp3d::CollisionCallback { contactPoint = contactPoint->getNext(); } + + manifoldElement = manifoldElement->getNext(); } } @@ -143,10 +145,7 @@ class CollisionDetectionScene : public SceneDemo { Capsule* mCapsule2; Box* mBox1; Box* mBox2; - //Cone* mCone; - //Cylinder* mCylinder; - //Capsule* mCapsule; - //ConvexMesh* mConvexMesh; + ConvexMesh* mConvexMesh; //Dumbbell* mDumbbell; //ConcaveMesh* mConcaveMesh; //HeightField* mHeightField; From 624e01b595e8723295caa13f6c6a13c84033c2e8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 21 Aug 2017 07:35:08 +0200 Subject: [PATCH 036/105] Working on ConcaveMeshShape and HeightFieldShape collision detection --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 2 + src/collision/ContactManifold.h | 9 +- src/collision/MiddlePhaseTriangleCallback.cpp | 49 ++++ src/collision/MiddlePhaseTriangleCallback.h | 84 +++++++ src/collision/PolyhedronMesh.cpp | 4 + src/collision/TriangleVertexArray.cpp | 238 +++++++++++++++++- src/collision/TriangleVertexArray.h | 71 +++++- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 2 +- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 9 +- .../narrowphase/ConcaveVsConvexAlgorithm.h | 26 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 9 +- .../narrowphase/SAT/SATAlgorithm.cpp | 75 ++++-- src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 10 +- src/collision/shapes/ConcaveMeshShape.cpp | 112 ++------- src/collision/shapes/ConcaveMeshShape.h | 22 +- src/collision/shapes/ConcaveShape.h | 3 +- src/collision/shapes/ConvexShape.h | 2 + src/collision/shapes/HeightFieldShape.cpp | 42 +++- src/collision/shapes/HeightFieldShape.h | 3 +- src/collision/shapes/TriangleShape.cpp | 104 +++++++- src/collision/shapes/TriangleShape.h | 52 +++- 23 files changed, 770 insertions(+), 162 deletions(-) create mode 100644 src/collision/MiddlePhaseTriangleCallback.cpp create mode 100644 src/collision/MiddlePhaseTriangleCallback.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e1d2fcf..dcc2e4bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.h" "src/collision/ContactManifoldSet.cpp" + "src/collision/MiddlePhaseTriangleCallback.h" + "src/collision/MiddlePhaseTriangleCallback.cpp" "src/constraint/BallAndSocketJoint.h" "src/constraint/BallAndSocketJoint.cpp" "src/constraint/ContactPoint.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 0abd82a3..0c21240a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -29,9 +29,11 @@ #include "collision/OverlapCallback.h" #include "body/Body.h" #include "collision/shapes/BoxShape.h" +#include "collision/shapes/ConcaveShape.h" #include "body/RigidBody.h" #include "configuration.h" #include "collision/CollisionCallback.h" +#include "collision/MiddlePhaseTriangleCallback.h" #include "collision/OverlapCallback.h" #include #include diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 0704beed..ada20003 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -80,9 +80,14 @@ struct ContactManifoldListElement { // Class ContactManifold /** - * This class represents the set of contact points between two bodies. + * This class represents a set of contact points between two bodies that + * all have a similar contact normal direction. Usually, there is a single + * contact manifold when two convex shapes are in contact. However, when + * a convex shape collides with a concave shape, there can be several + * contact manifolds with different normal directions. * The contact manifold is implemented in a way to cache the contact - * points among the frames for better stability. + * points among the frames for better stability (warm starting of the + * contact solver) */ class ContactManifold { diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp new file mode 100644 index 00000000..e99da461 --- /dev/null +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -0,0 +1,49 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "collision/MiddlePhaseTriangleCallback.h" + +using namespace reactphysics3d; + +// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) +void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, + const Vector3* verticesNormals) { + + // Create a triangle collision shape + decimal margin = mConcaveShape->getTriangleMargin(); + TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) + TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], + verticesNormals, meshSubPart, triangleIndex, margin); + + // Create a narrow phase info for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; + narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), + triangleShape, mConvexProxyShape->getLocalToWorldTransform(), + mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), + mConcaveProxyShape->getCachedCollisionData()); + narrowPhaseInfoList->next = firstNarrowPhaseInfo; +} diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h new file mode 100644 index 00000000..36bfac24 --- /dev/null +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -0,0 +1,84 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H +#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H + +// Libraries +#include "collision/shapes/ConcaveShape.h" +#include "collision/narrowphase/NarrowPhaseAlgorithm.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class ConvexVsTriangleCallback +/** + * This class is used to report a collision between the triangle + * of a concave mesh shape and a convex shape during the + * middle-phase algorithm. + */ +class MiddlePhaseTriangleCallback : public TriangleCallback { + + protected: + + /// Broadphase overlapping pair + OverlappingPair* mOverlappingPair; + + /// Pointer to the concave proxy shape + ProxyShape* mConcaveProxyShape; + + /// Pointer to the convex proxy shape + ProxyShape* mConvexProxyShape; + + /// Pointer to the concave collision shape + const ConcaveShape* mConcaveShape; + + /// Reference to the single-frame memory allocator + Allocator& mAllocator; + + public: + + /// Pointer to the first element of the linked-list of narrow-phase info + NarrowPhaseInfo* narrowPhaseInfoList; + + /// Constructor + MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, + ProxyShape* concaveProxyShape, + ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, + Allocator& allocator) + :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), + mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), + mAllocator(allocator), narrowPhaseInfoList(nullptr) { + + } + + /// Test collision between a triangle and the convex mesh shape + virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, + const Vector3* verticesNormals) override; +}; + +} + +#endif diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 61b1e3c8..b4f82fad 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -30,6 +30,10 @@ using namespace reactphysics3d; // Constructor +/* + * Create a polyhedron mesh given an array of polygons. + * @param polygonVertexArray Pointer to the array of polygons and their vertices + */ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { mPolygonVertexArray = polygonVertexArray; diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index 883b0ab7..ad59d37a 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -25,13 +25,19 @@ // Libraries #include "TriangleVertexArray.h" +#include "mathematics/Vector3.h" +#include using namespace reactphysics3d; -// Constructor +// Constructor without vertices normals /// Note that your data will not be copied into the TriangleVertexArray and /// therefore, you need to make sure that those data are always valid during -/// the lifetime of the TriangleVertexArray. +/// the lifetime of the TriangleVertexArray. With this constructor, you do not +/// need to provide vertices normals for smooth mesh collision. Therefore, the +/// vertices normals will be computed automatically. The vertices normals are +/// computed with weighted average of the associated triangle face normal. The +/// weights are the angle between the associated edges of neighbor triangle face. /** * @param nbVertices Number of vertices in the array * @param verticesStart Pointer to the first vertices of the array @@ -48,9 +54,237 @@ TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, i mNbVertices = nbVertices; mVerticesStart = reinterpret_cast(verticesStart); mVerticesStride = verticesStride; + mVerticesNormalsStart = nullptr; + mVerticesNormalsStride = 0; mNbTriangles = nbTriangles; mIndicesStart = reinterpret_cast(indexesStart); mIndicesStride = indexesStride; mVertexDataType = vertexDataType; + mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE; mIndexDataType = indexDataType; + mAreVerticesNormalsProvidedByUser = false; + + // Compute the vertices normals because they are not provided by the user + computeVerticesNormals(); +} + +// Constructor with vertices normals +/// Note that your data will not be copied into the TriangleVertexArray and +/// therefore, you need to make sure that those data are always valid during +/// the lifetime of the TriangleVertexArray. With this constructor, you need +/// to provide the vertices normals that will be used for smooth mesh collision. +/** + * @param nbVertices Number of vertices in the array + * @param verticesStart Pointer to the first vertices of the array + * @param verticesStride Number of bytes between the beginning of two consecutive vertices + * @param verticesNormalsStart Pointer to the first vertex normal of the array + * @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals + * @param nbTriangles Number of triangles in the array + * @param indexesStart Pointer to the first triangle index + * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices + * @param vertexDataType Type of data for the vertices (float, double) + * @param indexDataType Type of data for the indices (short, int) + */ +TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* verticesNormalsStart, int verticesNormalsStride, + uint nbTriangles, void* indexesStart, int indexesStride, + VertexDataType vertexDataType, NormalDataType normalDataType, + IndexDataType indexDataType) { + + mNbVertices = nbVertices; + mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStride = verticesStride; + mVerticesNormalsStart = reinterpret_cast(verticesNormalsStart); + mVerticesNormalsStride = verticesNormalsStride; + mNbTriangles = nbTriangles; + mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStride = indexesStride; + mVertexDataType = vertexDataType; + mVertexNormaldDataType = normalDataType; + mIndexDataType = indexDataType; + mAreVerticesNormalsProvidedByUser = true; + + assert(mVerticesNormalsStart != nullptr); +} + +// Destructor +TriangleVertexArray::~TriangleVertexArray() { + + // If the vertices normals have not been provided by the user + if (!mAreVerticesNormalsProvidedByUser) { + + // Release the allocated memory + float* verticesNormals = reinterpret_cast(mVerticesNormalsStart); + delete[] verticesNormals; + } +} + +// Compute the vertices normals when they are not provided by the user +/// The vertices normals are computed with weighted average of the associated +/// triangle face normal. The weights are the angle between the associated edges +/// of neighbor triangle face. +void TriangleVertexArray::computeVerticesNormals() { + + // Allocate memory for the vertices normals + float* verticesNormals = new float[mNbVertices * 3]; + + // Init vertices normals to zero + for (uint i=0; i(verticesNormals); +} + +// Return the indices of the three vertices of a given triangle in the array +void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + + // For each vertex of the triangle + for (int i=0; i < 3; i++) { + + // Get the index of the current vertex in the triangle + if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + outVerticesIndices[i] = ((uint*)vertexIndexPointer)[i]; + } + else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + outVerticesIndices[i] = ((unsigned short*)vertexIndexPointer)[i]; + } + else { + assert(false); + } + } +} + +// Return the vertices coordinates of a triangle +void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + + // For each vertex of the triangle + for (int k=0; k < 3; k++) { + + // Get the index of the current vertex in the triangle + int vertexIndex = 0; + if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + vertexIndex = ((uint*)vertexIndexPointer)[k]; + } + else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; + } + else { + assert(false); + } + + // Get the vertices components of the triangle + if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { + const float* vertices = (float*)(mVerticesStart + vertexIndex * mVerticesStride); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { + const double* vertices = (double*)(mVerticesStart + vertexIndex * mVerticesStride); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + else { + assert(false); + } + } +} + +// Return the three vertices normals of a triangle +void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + + // For each vertex of the triangle + for (int k=0; k < 3; k++) { + + // Get the index of the current vertex in the triangle + int vertexIndex = 0; + if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + vertexIndex = ((uint*)vertexIndexPointer)[k]; + } + else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; + } + else { + assert(false); + } + + // Get the normals from the array + if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) { + const float* normal = (float*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride); + outTriangleVerticesNormals[k][0] = decimal(normal[0]); + outTriangleVerticesNormals[k][1] = decimal(normal[1]); + outTriangleVerticesNormals[k][2] = decimal(normal[2]); + } + else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) { + const double* normal = (double*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride); + outTriangleVerticesNormals[k][0] = decimal(normal[0]); + outTriangleVerticesNormals[k][1] = decimal(normal[1]); + outTriangleVerticesNormals[k][2] = decimal(normal[2]); + } + else { + assert(false); + } + } } diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h index 8e434919..20ae9387 100644 --- a/src/collision/TriangleVertexArray.h +++ b/src/collision/TriangleVertexArray.h @@ -31,6 +31,8 @@ namespace reactphysics3d { +struct Vector3; + // Class TriangleVertexArray /** * This class is used to describe the vertices and faces of a triangular mesh. @@ -48,11 +50,16 @@ class TriangleVertexArray { /// Data type for the vertices in the array enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + /// Data type for the vertex normals in the array + enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE}; + /// Data type for the indices in the array enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; protected: + // -------------------- Attributes -------------------- // + /// Number of vertices in the array uint mNbVertices; @@ -63,6 +70,13 @@ class TriangleVertexArray { /// values in the array int mVerticesStride; + /// Pointer to the first vertex normal value in the array + unsigned char* mVerticesNormalsStart; + + /// Stride (number of bytes) between the beginning of two vertex normals + /// values in the array + int mVerticesNormalsStride; + /// Number of triangles in the array uint mNbTriangles; @@ -76,22 +90,45 @@ class TriangleVertexArray { /// Data type of the vertices in the array VertexDataType mVertexDataType; + /// Data type of the vertex normals in the array + NormalDataType mVertexNormaldDataType; + /// Data type of the indices in the array IndexDataType mIndexDataType; + /// True if the vertices normals are provided by the user + bool mAreVerticesNormalsProvidedByUser; + + // -------------------- Methods -------------------- // + + /// Compute the vertices normals when they are not provided by the user + void computeVerticesNormals(); + public: - /// Constructor + // -------------------- Methods -------------------- // + + /// Constructor without vertices normals TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, uint nbTriangles, void* indexesStart, int indexesStride, VertexDataType vertexDataType, IndexDataType indexDataType); + /// Constructor with vertices normals + TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* verticesNormalsStart, int verticesNormalsStride, + uint nbTriangles, void* indexesStart, int indexesStride, + VertexDataType vertexDataType, NormalDataType normalDataType, + IndexDataType indexDataType); + /// Destructor - ~TriangleVertexArray() = default; + ~TriangleVertexArray(); /// Return the vertex data type VertexDataType getVertexDataType() const; + /// Return the vertex normal data type + NormalDataType getVertexNormalDataType() const; + /// Return the index data type IndexDataType getIndexDataType() const; @@ -104,14 +141,29 @@ class TriangleVertexArray { /// Return the vertices stride (number of bytes) int getVerticesStride() const; + /// Return the vertex normals stride (number of bytes) + int getVerticesNormlasStride() const; + /// Return the indices stride (number of bytes) int getIndicesStride() const; /// Return the pointer to the start of the vertices array unsigned char* getVerticesStart() const; + /// Return the pointer to the start of the vertex normals array + unsigned char* getVerticesNormalsStart() const; + /// Return the pointer to the start of the indices array unsigned char* getIndicesStart() const; + + /// Return the vertices coordinates of a triangle + void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const; + + /// Return the three vertices normals of a triangle + void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const; + + /// Return the indices of the three vertices of a given triangle in the array + void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const; }; // Return the vertex data type @@ -119,6 +171,11 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp return mVertexDataType; } +// Return the vertex normal data type +inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const { + return mVertexNormaldDataType; +} + // Return the index data type inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { return mIndexDataType; @@ -139,6 +196,11 @@ inline int TriangleVertexArray::getVerticesStride() const { return mVerticesStride; } +// Return the vertex normals stride (number of bytes) +inline int TriangleVertexArray::getVerticesNormlasStride() const { + return mVerticesNormalsStride; +} + // Return the indices stride (number of bytes) inline int TriangleVertexArray::getIndicesStride() const { return mIndicesStride; @@ -149,6 +211,11 @@ inline unsigned char* TriangleVertexArray::getVerticesStart() const { return mVerticesStart; } +// Return the pointer to the start of the vertex normals array +inline unsigned char* TriangleVertexArray::getVerticesNormalsStart() const { + return mVerticesNormalsStart; +} + // Return the pointer to the start of the indices array inline unsigned char* TriangleVertexArray::getIndicesStart() const { return mIndicesStart; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 73a1dd13..72ba2e06 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -82,7 +82,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(f); - const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; + Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 7f5f439b..8a2475a2 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -22,7 +22,7 @@ * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ - +/* // Libraries #include "collision/shapes/ConcaveShape.h" #include "collision/shapes/TriangleShape.h" @@ -34,12 +34,14 @@ using namespace reactphysics3d; // Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) -void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) { +void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, + const Vector3* verticesNormals) { // Create a triangle collision shape decimal margin = mConcaveShape->getTriangleMargin(); TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], + verticesNormals, meshSubPart, triangleIndex, margin); // Create a narrow phase info for the narrow-phase collision detection NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; @@ -297,3 +299,4 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi // // smooth mesh collision // mContactPoints.push_back(smoothContactInfo); //} +*/ diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h index 4619e480..70cd3dbc 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -22,7 +22,7 @@ * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ - +/* #ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H #define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H @@ -37,11 +37,6 @@ namespace reactphysics3d { // Class ConvexVsTriangleCallback -/** - * This class is used to report a collision between the triangle - * of a concave mesh shape and a convex shape during the - * middle-phase algorithm - */ class MiddlePhaseTriangleCallback : public TriangleCallback { protected: @@ -78,14 +73,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { } /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(const Vector3* trianglePoints) override; + virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, + const Vector3* verticesNormals) override; }; // Class SmoothMeshContactInfo -/** - * Contains data for of potential smooth contact during the smooth mesh - * contacts computation. - */ struct SmoothMeshContactInfo { public: @@ -131,11 +123,6 @@ struct ContactsDepthCompare { // TODO : Delete this // Class SmoothCollisionNarrowPhaseCallback -/** - * This class is used as a narrow-phase callback to get narrow-phase contacts - * of the concave triangle mesh to temporary store them in order to be used in - * the smooth mesh collision algorithm if this one is enabled. - */ class SmoothCollisionNarrowPhaseCallback { private: @@ -155,12 +142,6 @@ class SmoothCollisionNarrowPhaseCallback { // TODO : Delete this // Class ConcaveVsConvexAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between a concave collision shape and a convex collision shape. The idea is - * to use the GJK collision detection algorithm to compute the collision between - * the convex shape and each of the triangles in the concave shape. - */ class ConcaveVsConvexAlgorithm { protected : @@ -212,3 +193,4 @@ inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap #endif +*/ diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 10d344f1..609c415a 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -27,6 +27,7 @@ #include "GJKAlgorithm.h" #include "constraint/ContactPoint.h" #include "engine/OverlappingPair.h" +#include "collision/shapes/TriangleShape.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -76,7 +77,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = transform1.getInverse() * transform2; + Transform transform1Inverse = transform1.getInverse(); + Transform body2Tobody1 = transform1Inverse * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 @@ -209,6 +211,10 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase if (reportContacts) { + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2, + penetrationDepth, normal); + // Add a new contact point narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); } @@ -219,6 +225,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase return GJKResult::SEPARATED; } + // Use the GJK Algorithm to find if a point is inside a convex collision shape bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index cebec9a6..23ce86e0 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -30,6 +30,7 @@ #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" #include "engine/OverlappingPair.h" +#include "collision/shapes/TriangleShape.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -143,13 +144,21 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); - const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); - const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); + Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + // Create the contact info object narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, - isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); } lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; @@ -380,7 +389,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; - const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; const decimal capsuleRadius = capsuleShape->getRadius(); // If the separating axis is a face normal @@ -410,7 +419,14 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); // Project closest capsule inner segment point into the capsule bounds - const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); // Create the contact point narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, @@ -483,7 +499,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe // axis is a face normal of the polyhedron void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, - const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { @@ -525,15 +541,27 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // If the clipped point is one that produce this penetration depth, we keep it if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) { - const Vector3 contactPointPolyhedron = clipSegment[i] + delta; + Vector3 contactPointPolyhedron = clipSegment[i] + delta; // Project the clipped point into the capsule bounds - const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; + Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; + + if (isCapsuleShape1) { + normalWorld = -normalWorld; + } + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, + isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + penetrationDepth, normalWorld); + // Create the contact point - narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth, - isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, - isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, + isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); } } } @@ -807,7 +835,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; // Compute the world normal - const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face @@ -874,11 +902,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); // Project the contact point onto the reference face Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + // Create a new contact point narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, @@ -901,14 +936,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 - const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; + Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); // Create the contact point narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); } lastFrameInfo.satIsAxisFacePolyhedron1 = false; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 49cd2626..73838d95 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -121,7 +121,7 @@ class SATAlgorithm { /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, - const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 049ce449..7ecda325 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -36,15 +36,15 @@ using namespace reactphysics3d; // by Dirk Gregorius. bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || - narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || - narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index a0bd1094..8cb5235a 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -49,51 +49,13 @@ void ConcaveMeshShape::initBVHTree() { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); - TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); - // For each triangle of the concave mesh for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); Vector3 trianglePoints[3]; - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; - } - else { - assert(false); - } - } + // Get the triangle vertices + triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints); // Create the AABB for the triangle AABB aabb = AABB::createAABBForTriangle(trianglePoints); @@ -106,56 +68,32 @@ void ConcaveMeshShape::initBVHTree() { } // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle -// given the start vertex index pointer of the triangle -void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const { +void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex, + Vector3* outTriangleVertices) const { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); - TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); + // Get the vertices coordinates of the triangle + triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices); - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; - } - else { - assert(false); - } - } + // Apply the scaling factor to the vertices + outTriangleVertices[0] *= mScaling.x; + outTriangleVertices[1] *= mScaling.x; + outTriangleVertices[2] *= mScaling.x; } +// Return the three vertex normals (in the array outVerticesNormals) of a triangle +void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const { + + // Get the triangle vertex array of the current sub-part + TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); + + // Get the vertices normals of the triangle + triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals); +} + + // Use a callback method on all triangles of the concave shape inside a given AABB void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { @@ -208,11 +146,15 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Get the triangle vertices for this node from the concave mesh shape Vector3 trianglePoints[3]; - mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints); + // Get the vertices normals of the triangle + Vector3 verticesNormals[3]; + mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape decimal margin = mConcaveMeshShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], + verticesNormals, data[0], data[1], margin); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); // Ray casting test against the collision shape diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 83e5103a..309ec27c 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -118,6 +118,10 @@ class ConcaveMeshShape : public ConcaveShape { /// Dynamic AABB tree to accelerate collision with the triangles DynamicAABBTree mDynamicAABBTree; + /// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only + /// if the user did not provide its own vertices normals) + Vector3** mComputedVerticesNormals; + // -------------------- Methods -------------------- // /// Raycast method with feedback information @@ -130,9 +134,13 @@ class ConcaveMeshShape : public ConcaveShape { void initBVHTree(); /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const; + void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const; + + /// Return the three vertex normals (in the array outVerticesNormals) of a triangle + void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const; + + /// Get a smooth contact normal for collision for a triangle of the mesh + Vector3 computeSmoothLocalContactNormalForTriangle(TriangleShape* triangleShape, const Vector3& localContactPoint) const; public: @@ -224,10 +232,14 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) // Get the triangle vertices for this node from the concave mesh shape Vector3 trianglePoints[3]; - mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints); + + // Get the vertices normals of the triangle + Vector3 verticesNormals[3]; + mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Call the callback to test narrow-phase collision with this triangle - mTriangleTestCallback.testTriangle(trianglePoints); + mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals); } } diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index c5657b2b..6e07187f 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -46,7 +46,8 @@ class TriangleCallback { virtual ~TriangleCallback() = default; /// Report a triangle - virtual void testTriangle(const Vector3* trianglePoints)=0; + virtual void testTriangle(uint meshSubPart, uint triangleIndex, + const Vector3* trianglePoints, const Vector3* verticesNormals)=0; }; diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 4b98c9fa..94acc7f9 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -49,10 +49,12 @@ class ConvexShape : public CollisionShape { // -------------------- Methods -------------------- // // Return a local support point in a given direction with the object margin + // TODO : Try to remove cachedCollisionData parameter Vector3 getLocalSupportPointWithMargin(const Vector3& direction, void** cachedCollisionData) const; /// Return a local support point in a given direction without the object margin + // TODO : Try to remove cachedCollisionData parameter virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const=0; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 02ef852f..c8fc97f2 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -133,24 +133,48 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& for (int j = jMin; j < jMax; j++) { // Compute the four point of the current quad - Vector3 p1 = getVertexAt(i, j); - Vector3 p2 = getVertexAt(i, j + 1); - Vector3 p3 = getVertexAt(i + 1, j); - Vector3 p4 = getVertexAt(i + 1, j + 1); + const Vector3 p1 = getVertexAt(i, j); + const Vector3 p2 = getVertexAt(i, j + 1); + const Vector3 p3 = getVertexAt(i + 1, j); + const Vector3 p4 = getVertexAt(i + 1, j + 1); // Generate the first triangle for the current grid rectangle Vector3 trianglePoints[3] = {p1, p2, p3}; + // Compute the triangle normal + Vector3 triangle1Normal = (p2 - p1).cross(p3 - p1).getUnit(); + + // Use the triangle face normal as vertices normals (this is an aproximation. The correct + // solution would be to compute all the normals of the neighbor triangles and use their + // weighted average (with incident angle as weight) at the vertices. However, this solution + // seems too expensive (it requires to compute the normal of all neighbor triangles instead + // and compute the angle of incident edges with asin(). Maybe we could also precompute the + // vertices normal at the HeightFieldShape constructor but it will require extra memory to + // store them. + Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal}; + // Test collision against the first triangle - callback.testTriangle(trianglePoints); + callback.testTriangle(0, 0, trianglePoints, verticesNormals1); // Generate the second triangle for the current grid rectangle trianglePoints[0] = p3; trianglePoints[1] = p2; trianglePoints[2] = p4; + // Compute the triangle normal + Vector3 triangle2Normal = (p2 - p3).cross(p4 - p3).getUnit(); + + // Use the triangle face normal as vertices normals (this is an aproximation. The correct + // solution would be to compute all the normals of the neighbor triangles and use their + // weighted average (with incident angle as weight) at the vertices. However, this solution + // seems too expensive (it requires to compute the normal of all neighbor triangles instead + // and compute the angle of incident edges with asin(). Maybe we could also precompute the + // vertices normal at the HeightFieldShape constructor but it will require extra memory to + // store them. + Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal}; + // Test collision against the second triangle - callback.testTriangle(trianglePoints); + callback.testTriangle(0, 0, trianglePoints, verticesNormals2); } } } @@ -232,11 +256,13 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { } // Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { +void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, + const Vector3* verticesNormals) { // Create a triangle collision shape decimal margin = mHeightFieldShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], + verticesNormals, meshSubPart, triangleIndex, margin); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); // Ray casting test against the collision shape diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 6f2b8459..aa0cd787 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -64,7 +64,8 @@ class TriangleOverlapCallback : public TriangleCallback { bool getIsHit() const {return mIsHit;} /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(const Vector3* trianglePoints) override; + virtual void testTriangle(uint meshSubPart, uint triangleIndex, + const Vector3* trianglePoints, const Vector3* verticesNormals) override; }; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 9c0deb38..72eb93d3 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -26,6 +26,7 @@ // Libraries #include "TriangleShape.h" #include "collision/ProxyShape.h" +#include "mathematics/mathematics_functions.h" #include "engine/Profiler.h" #include "configuration.h" #include @@ -34,13 +35,17 @@ using namespace reactphysics3d; // Constructor /** + * Do not use this constructor. It is supposed to be used internally only. + * Use a ConcaveMeshShape instead. * @param point1 First point of the triangle * @param point2 Second point of the triangle * @param point3 Third point of the triangle + * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) - : ConvexPolyhedronShape(margin) { +TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, + const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin) + : ConvexPolyhedronShape(margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { mPoints[0] = point1; mPoints[1] = point2; @@ -50,9 +55,104 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const mNormal = (point3 - point1).cross(point2 - point1); mNormal.normalize(); + mVerticesNormals[0] = verticesNormals[0]; + mVerticesNormals[1] = verticesNormals[1]; + mVerticesNormals[2] = verticesNormals[2]; + mRaycastTestType = TriangleRaycastSide::FRONT; } +// This method compute the smooth mesh contact with a triangle in case one of the two collision +// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh +// at the contact point instead of the triangle normal to avoid the internal edge collision issue. +// This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3 localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal) { + + assert(shape1->getType() != CollisionShapeType::TRIANGLE || shape2->getType() != CollisionShapeType::TRIANGLE); + + // If one the shape is a triangle + bool isShape1Triangle = shape1->getType() == CollisionShapeType::TRIANGLE; + if (isShape1Triangle || shape2->getType() == CollisionShapeType::TRIANGLE) { + + const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): + static_cast(shape2); + + // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape + triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, + isShape1Triangle ? shape1ToWorld : shape2ToWorld, + isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), + penetrationDepth, + isShape1Triangle ? localContactPointShape2 : localContactPointShape1, + outSmoothVertexNormal); + + // Make sure the direction of the contact normal is from shape1 to shape2 + if (!isShape1Triangle) { + outSmoothVertexNormal = -outSmoothVertexNormal; + } + } +} + + +// This method implements the technique described in Game Physics Pearl book +// by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is +// to replace the contact normal of the triangle shape with the precomputed normal of the triangle +// mesh at this point. Then, we need to recompute the contact point on the other shape in order to +// stay aligned with the new contact normal. This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const { + + // Get the smooth contact normal of the mesh at the contact point on the triangle + Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); + + // Convert the local contact normal into world-space + outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal; + + // Convert the contact normal into the local-space of the other shape + Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal; + + // Convert the local contact point of the triangle into the local-space of the other shape + Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform * + localContactPointTriangle; + + // Re-align the local contact point on the other shape such that it is aligned along + // the new contact normal + Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth; + outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); +} + +// Get a smooth contact normal for collision for a triangle of the mesh +/// This is used to avoid the internal edges issue that occurs when a shape is colliding with +/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, +/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface +/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface +/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 +/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the +/// mesh are either provided by the user or precomputed if the user did not provide them. +Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { + + // Compute the barycentric coordinates of the point in the triangle + decimal u, v, w; + computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); + + int nbZeros = 0; + bool isUZero = approxEqual(u, decimal(0), decimal(0.0001)); + bool isVZero = approxEqual(v, decimal(0), decimal(0.0001)); + bool isWZero = approxEqual(w, decimal(0), decimal(0.0001)); + if (isUZero) nbZeros++; + if (isVZero) nbZeros++; + if (isWZero) nbZeros++; + + // We compute the contact normal as the barycentric interpolation of the three vertices normals + return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit(); +} + + // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index e25086b8..900b71db 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -49,7 +49,10 @@ enum class TriangleRaycastSide { // Class TriangleShape /** * This class represents a triangle collision shape that is centered - * at the origin and defined three points. + * at the origin and defined three points. A user cannot instanciate + * an object of this class. This class is for internal use only. Instances + * of this class are created when the user creates an HeightFieldShape and + * a ConcaveMeshShape */ class TriangleShape : public ConvexPolyhedronShape { @@ -63,15 +66,27 @@ class TriangleShape : public ConvexPolyhedronShape { /// Normal of the triangle Vector3 mNormal; + /// Three vertices normals for smooth collision with triangle mesh + Vector3 mVerticesNormals[3]; + /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; + /// Index of the mesh sub part in the original mesh + uint mMeshSubPart; + + /// Triangle index of the triangle in the sub mesh + uint mTriangleIndex; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const override; + /// Get a smooth contact normal for collision for a triangle of the mesh + Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const; + /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; @@ -81,13 +96,20 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; + // -------------------- Methods -------------------- // + + /// This method implements the technique described in Game Physics Pearl book + void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; + public: // -------------------- Methods -------------------- // /// Constructor TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - decimal margin = OBJECT_MARGIN); + const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin = OBJECT_MARGIN); /// Destructor virtual ~TriangleShape() override = default; @@ -143,10 +165,23 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const override; + /// Return the index of the sub part mesh of the original mesh + uint getMeshSubPart() const; + + /// Return the triangle index in the original mesh + uint getTriangleIndex() const; + + /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh + static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3 localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal); + // ---------- Friendship ---------- // friend class ConcaveMeshRaycastCallback; friend class TriangleOverlapCallback; + friend class MiddlePhaseTriangleCallback; }; // Return the number of bytes used by the collision shape @@ -155,8 +190,7 @@ inline size_t TriangleShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); return mPoints[dotProducts.getMaxAxis()]; } @@ -286,6 +320,16 @@ inline Vector3 TriangleShape::getCentroid() const { return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); } +// Return the index of the sub part mesh of the original mesh +inline uint TriangleShape::getMeshSubPart() const { + return mMeshSubPart; +} + +// Return the triangle index in the original mesh +inline uint TriangleShape::getTriangleIndex() const { + return mTriangleIndex; +} + // Return the number of half-edges of the polyhedron inline uint TriangleShape::getNbHalfEdges() const { return 6; From e725af80b6bc9da177f01ca1892a65007db1ffa4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 22 Aug 2017 07:38:22 +0200 Subject: [PATCH 037/105] Update raycasting test code for convex mesh and remove commented code --- test/tests/collision/TestRaycast.h | 147 +++++++++-------------------- testbed/common/ConvexMesh.cpp | 7 -- 2 files changed, 46 insertions(+), 108 deletions(-) diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 0f25ab94..93860a1c 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -110,7 +110,6 @@ class TestRaycast : public Test { CollisionBody* mSphereBody; CollisionBody* mCapsuleBody; CollisionBody* mConvexMeshBody; - CollisionBody* mConvexMeshBodyEdgesInfo; CollisionBody* mCylinderBody; CollisionBody* mCompoundBody; CollisionBody* mTriangleBody; @@ -128,7 +127,6 @@ class TestRaycast : public Test { SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeEdgesInfo; TriangleShape* mTriangleShape; ConcaveShape* mConcaveMeshShape; HeightFieldShape* mHeightFieldShape; @@ -138,7 +136,6 @@ class TestRaycast : public Test { ProxyShape* mSphereProxyShape; ProxyShape* mCapsuleProxyShape; ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; ProxyShape* mCompoundSphereProxyShape; ProxyShape* mCompoundCapsuleProxyShape; ProxyShape* mTriangleProxyShape; @@ -152,6 +149,11 @@ class TestRaycast : public Test { std::vector mConcaveMeshIndices; TriangleVertexArray* mConcaveMeshVertexArray; float mHeightFieldData[100]; + PolygonVertexArray::PolygonFace mPolygonFaces[6]; + PolygonVertexArray* mPolygonVertexArray; + PolyhedronMesh* mPolyhedronMesh; + Vector3 mPolyhedronVertices[8]; + int mPolyhedronIndices[4 * 6]; public : @@ -175,7 +177,6 @@ class TestRaycast : public Test { mSphereBody = mWorld->createCollisionBody(mBodyTransform); mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); mCompoundBody = mWorld->createCollisionBody(mBodyTransform); mTriangleBody = mWorld->createCollisionBody(mBodyTransform); @@ -200,51 +201,47 @@ class TestRaycast : public Test { const Vector3 triangleVertex1(100, 100, 0); const Vector3 triangleVertex2(105, 100, 0); const Vector3 triangleVertex3(100, 103, 0); - mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); + Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; + mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again - // Box of dimension (2, 3, 4) - /*mConvexMeshShape = new ConvexMeshShape(0.0); - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); - mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); + // Box of extents (2, 3, 4) + mPolyhedronVertices[0] = Vector3(-2, -3, -4); + mPolyhedronVertices[1] = Vector3(2, -3, -4); + mPolyhedronVertices[2] = Vector3(2, -3, 4); + mPolyhedronVertices[3] = Vector3(-2, -3, 4); + mPolyhedronVertices[4] = Vector3(-2, 3, -4); + mPolyhedronVertices[5] = Vector3(2, 3, -4); + mPolyhedronVertices[6] = Vector3(2, 3, 4); + mPolyhedronVertices[7] = Vector3(-2, 3, 4); - mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeEdgesInfo->addEdge(0, 1); - mConvexMeshShapeEdgesInfo->addEdge(1, 2); - mConvexMeshShapeEdgesInfo->addEdge(2, 3); - mConvexMeshShapeEdgesInfo->addEdge(0, 3); - mConvexMeshShapeEdgesInfo->addEdge(4, 5); - mConvexMeshShapeEdgesInfo->addEdge(5, 6); - mConvexMeshShapeEdgesInfo->addEdge(6, 7); - mConvexMeshShapeEdgesInfo->addEdge(4, 7); - mConvexMeshShapeEdgesInfo->addEdge(0, 4); - mConvexMeshShapeEdgesInfo->addEdge(1, 5); - mConvexMeshShapeEdgesInfo->addEdge(2, 6); - mConvexMeshShapeEdgesInfo->addEdge(3, 7); - mConvexMeshShapeEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeEdgesInfo, - mShapeTransform); - */ + mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3; + mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2; + mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1; + mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4; + mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7; + mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7; + + // Polygon faces descriptions for the polyhedron + for (int f=0; f < 8; f++) { + PolygonVertexArray::PolygonFace& face = mPolygonFaces[f]; + face.indexBase = f * 4; + face.nbVertices = 4; + } + + // Create the polygon vertex array + mPolygonVertexArray = new PolygonVertexArray(8, mPolyhedronVertices, sizeof(Vector3), + mPolyhedronIndices, sizeof(int), 6, mPolygonFaces, + PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + mPolyhedronMesh = new PolyhedronMesh(mPolygonVertexArray); + mConvexMeshShape = new ConvexMeshShape(mPolyhedronMesh); + mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); @@ -300,8 +297,7 @@ class TestRaycast : public Test { mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); - //mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); - //mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); + mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2); mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); @@ -314,13 +310,15 @@ class TestRaycast : public Test { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - //delete mConvexMeshShape; - //delete mConvexMeshShapeEdgesInfo; + delete mConvexMeshShape; delete mTriangleShape; delete mConcaveMeshShape; delete mHeightFieldShape; delete mConcaveMeshVertexArray; + + delete mPolygonVertexArray; + delete mPolyhedronMesh; } /// Run the tests @@ -1299,7 +1297,6 @@ class TestRaycast : public Test { /// CollisionWorld::raycast() methods. void testConvexMesh() { - /* // ----- Test feedback data ----- // Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6); Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4); @@ -1339,16 +1336,6 @@ class TestRaycast : public Test { test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() RaycastInfo raycastInfo4; test(mConvexMeshProxyShape->raycast(ray, raycastInfo4)); @@ -1359,16 +1346,6 @@ class TestRaycast : public Test { test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray, raycastInfo5)); - test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); @@ -1387,10 +1364,9 @@ class TestRaycast : public Test { Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); // ----- Test raycast miss ----- // + RaycastInfo raycastInfo3; test(!mConvexMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray1, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray1, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray1, &mCallback); test(!mCallback.isHit); @@ -1402,73 +1378,55 @@ class TestRaycast : public Test { test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray2, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray2, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray2, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray2, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray2, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray3, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray3, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray3, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray3, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray3, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray4, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray4, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray4, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray4, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray4, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray5, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray5, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray5, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray5, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray5, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray6, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray6, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray6, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray6, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray6, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray7, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray7, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray7, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray7, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray7, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray8, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray8, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray8, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray8, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray8, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray9, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray9, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray9, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray9, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray9, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray10, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray10, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray10, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray10, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray10, &mCallback); test(!mCallback.isHit); @@ -1494,10 +1452,8 @@ class TestRaycast : public Test { // ----- Test raycast hits ----- // test(mConvexMeshBody->raycast(ray11, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray11, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray11, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray11, raycastInfo3)); - mCallback.reset(); + mCallback.reset(); mWorld->raycast(ray11, &mCallback); test(mCallback.isHit); mCallback.reset(); @@ -1505,9 +1461,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray12, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray12, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray12, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray12, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray12, &mCallback); test(mCallback.isHit); @@ -1516,9 +1470,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray13, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray13, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray13, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray13, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray13, &mCallback); test(mCallback.isHit); @@ -1527,9 +1479,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray14, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray14, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray14, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray14, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray14, &mCallback); test(mCallback.isHit); @@ -1538,9 +1488,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray15, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray15, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray15, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray15, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray15, &mCallback); test(mCallback.isHit); @@ -1549,16 +1497,13 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray16, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray16, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray16, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray16, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray16, &mCallback); test(mCallback.isHit); mCallback.reset(); mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); test(mCallback.isHit); - */ } /// Test the CollisionBody::raycast() and diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index 11be5732..b3cb3b95 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -40,13 +40,6 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - /*mPolygonVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);*/ - // Polygon faces descriptions for the polyhedron mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)]; rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces; From a655ffb4623a84f386be4427f60215fe3a7758c0 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 31 Aug 2017 22:42:19 +0200 Subject: [PATCH 038/105] Fix issue in ContactManifoldSet.cpp --- src/collision/ContactManifoldSet.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 23602dda..fb3beda2 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -60,6 +60,8 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa } else { + bool addNewManifold = true; + // If there are too much contact manifolds in the set if (mNbManifolds >= mNbMaxManifolds) { @@ -72,14 +74,20 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa // Remove the manifold removeManifold(minDepthManifold); + } + else { - // Create a new contact manifold - createManifold(contactManifoldInfo); + // The manifold we do not want to get is the new one. Therefore, we do not add it to the set + addNewManifold = false; } } - // Create a new contact manifold - createManifold(contactManifoldInfo); + // If we need to add the new contact manifold + if (addNewManifold) { + + // Create a new contact manifold + createManifold(contactManifoldInfo); + } } } From 9b89f66667806fbca2f3390cf909b9ea8d4c5b68 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 31 Aug 2017 23:11:00 +0200 Subject: [PATCH 039/105] Fix issues and refactor collision shape type and collision shape name --- src/collision/CollisionDetection.cpp | 70 ++++++++----------- src/collision/MiddlePhaseTriangleCallback.cpp | 5 +- src/collision/NarrowPhaseInfo.cpp | 18 +++-- src/collision/NarrowPhaseInfo.h | 13 ++-- src/collision/ProxyShape.h | 15 ++++ .../narrowphase/DefaultCollisionDispatch.cpp | 1 - .../narrowphase/SAT/SATAlgorithm.cpp | 6 +- src/collision/shapes/BoxShape.cpp | 2 +- src/collision/shapes/CapsuleShape.cpp | 2 +- src/collision/shapes/CollisionShape.cpp | 2 +- src/collision/shapes/CollisionShape.h | 37 ++++++---- src/collision/shapes/ConcaveMeshShape.cpp | 6 +- src/collision/shapes/ConcaveMeshShape.h | 3 - src/collision/shapes/ConcaveShape.cpp | 5 +- src/collision/shapes/ConcaveShape.h | 24 +------ src/collision/shapes/ConvexMeshShape.cpp | 2 +- .../shapes/ConvexPolyhedronShape.cpp | 4 +- src/collision/shapes/ConvexPolyhedronShape.h | 2 +- src/collision/shapes/ConvexShape.cpp | 4 +- src/collision/shapes/ConvexShape.h | 2 +- src/collision/shapes/HeightFieldShape.cpp | 2 +- src/collision/shapes/SphereShape.cpp | 2 +- src/collision/shapes/TriangleShape.cpp | 18 ++--- src/collision/shapes/TriangleShape.h | 2 +- src/engine/OverlappingPair.cpp | 7 +- src/engine/OverlappingPair.h | 2 +- testbed/common/ConcaveMesh.cpp | 2 - .../CollisionDetectionScene.cpp | 22 +++--- .../CollisionDetectionScene.h | 2 +- 29 files changed, 138 insertions(+), 144 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 0c21240a..3c4b771b 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -148,11 +148,11 @@ void CollisionDetection::computeMiddlePhase() { bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + bool isShape2Convex = shape2->getCollisionShape()->isConvex(); // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + if (isShape1Convex && isShape2Convex) { // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection @@ -161,13 +161,12 @@ void CollisionDetection::computeMiddlePhase() { NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); + shape2->getCachedCollisionData(), mSingleFrameAllocator); mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; } // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { NarrowPhaseInfo* narrowPhaseInfo = nullptr; computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); @@ -410,22 +409,14 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { // Reduce the number of contact points of the manifold pair->reducePotentialContactManifolds(); - // If there is a concave mesh shape in the pair - if (pair->hasConcaveShape()) { + // Add all the potential contact manifolds as actual contact manifolds to the pair + ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); + while (potentialManifold != nullptr) { - processSmoothMeshContacts(pair); - } - else { // If both collision shapes are convex + pair->addContactManifold(potentialManifold); - // Add all the potential contact manifolds as actual contact manifolds to the pair - ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); - while (potentialManifold != nullptr) { - - pair->addContactManifold(potentialManifold); - - potentialManifold = potentialManifold->mNext; - } - } + potentialManifold = potentialManifold->mNext; + } // Clear the obselete contact manifolds and contact points pair->clearObseleteManifoldsAndContactPoints(); @@ -544,25 +535,24 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // ------------------------------------------------------- - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + const bool isShape2Convex = shape2->getCollisionShape()->isConvex(); NarrowPhaseInfo* narrowPhaseInfo = nullptr; // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + if ((isShape1Convex && isShape2Convex)) { // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); + shape2->getCachedCollisionData(), mPoolAllocator); } // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection @@ -630,9 +620,6 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Test if the AABBs of the two proxy shapes overlap if (aabb1.testCollision(aabb2)) { - const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); - // Create a temporary overlapping pair OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); @@ -647,6 +634,9 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // If we have not found a collision yet if (!isColliding) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -723,9 +713,6 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Check if the collision filtering allows collision between the two shapes if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); - // Create a temporary overlapping pair OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); @@ -740,6 +727,9 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // If we have not found a collision yet if (!isColliding) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -812,12 +802,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); - // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -896,9 +886,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Check if the collision filtering allows collision between the two shapes if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); - // Create a temporary overlapping pair OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); @@ -908,6 +895,9 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); @@ -988,8 +978,8 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); // Select the narrow phase algorithm to use according to the two collision shapes NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index e99da461..a1a183ac 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -32,7 +32,8 @@ using namespace reactphysics3d; void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, const Vector3* verticesNormals) { - // Create a triangle collision shape + // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the + // destructor of the corresponding NarrowPhaseInfo. decimal margin = mConcaveShape->getTriangleMargin(); TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], @@ -44,6 +45,6 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), triangleShape, mConvexProxyShape->getLocalToWorldTransform(), mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), - mConcaveProxyShape->getCachedCollisionData()); + mConcaveProxyShape->getCachedCollisionData(), mAllocator); narrowPhaseInfoList->next = firstNarrowPhaseInfo; } diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index 22ee6666..b4a92a38 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -27,18 +27,19 @@ #include #include "NarrowPhaseInfo.h" #include "ContactPointInfo.h" +#include "collision/shapes/TriangleShape.h" #include "engine/OverlappingPair.h" using namespace reactphysics3d; // Constructor -NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, - const CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2) +NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator) : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), contactPoints(nullptr), cachedCollisionData1(cachedData1), - cachedCollisionData2(cachedData2), next(nullptr) { + cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) { } @@ -46,6 +47,15 @@ NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* sh NarrowPhaseInfo::~NarrowPhaseInfo() { assert(contactPoints == nullptr); + + // Release the memory of the TriangleShape (this memory was allocated in the + // MiddlePhaseTriangleCallback::testTriangle() method) + if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape)); + } + if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) { + collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); + } } // Add a new contact point diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index 29bc6c9d..c9b18094 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -48,10 +48,10 @@ struct NarrowPhaseInfo { OverlappingPair* overlappingPair; /// Pointer to the first collision shape to test collision with - const CollisionShape* collisionShape1; + CollisionShape* collisionShape1; /// Pointer to the second collision shape to test collision with - const CollisionShape* collisionShape2; + CollisionShape* collisionShape2; /// Transform that maps from collision shape 1 local-space to world-space Transform shape1ToWorldTransform; @@ -70,13 +70,16 @@ struct NarrowPhaseInfo { // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 void** cachedCollisionData2; + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + Allocator& collisionShapeAllocator; + /// Pointer to the next element in the linked list NarrowPhaseInfo* next; /// Constructor - NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, - const CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2); + NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator); /// Destructor ~NarrowPhaseInfo(); diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index d9575b3d..36bec740 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -85,6 +85,11 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; + // -------------------- Methods -------------------- // + + /// Return the collision shape + CollisionShape* getCollisionShape(); + public: // -------------------- Methods -------------------- // @@ -177,6 +182,8 @@ class ProxyShape { friend class DynamicsWorld; friend class GJKAlgorithm; friend class ConvexMeshShape; + friend class ContactManifoldSet; + friend class MiddlePhaseTriangleCallback; }; @@ -193,6 +200,14 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const { return mCollisionShape; } +// Return the collision shape +/** +* @return Pointer to the internal collision shape +*/ +inline CollisionShape* ProxyShape::getCollisionShape() { + return mCollisionShape; +} + // Return the parent body /** * @return Pointer to the parent body diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index df1828ad..05616b27 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -36,7 +36,6 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t CollisionShapeType shape1Type = static_cast(type1); CollisionShapeType shape2Type = static_cast(type2); - // Convex vs Convex algorithm (GJK algorithm) if (type1 > type2) { return nullptr; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 23ce86e0..c5210c64 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -82,7 +82,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous // frame collision data per triangle) - if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { + if (polyhedron->getName() != CollisionShapeName::TRIANGLE) { // If the last frame collision info is valid and was also using SAT algorithm if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { @@ -225,7 +225,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous // frame collision data per triangle) - if (polyhedron->getType() != CollisionShapeType::TRIANGLE) { + if (polyhedron->getName() != CollisionShapeName::TRIANGLE) { // If the last frame collision info is valid and was also using SAT algorithm if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { @@ -609,7 +609,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous // frame collision data per triangle) - if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) { + if (polyhedron1->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) { // If the last frame collision info is valid and was also using SAT algorithm if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index af6a30b8..54666136 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -38,7 +38,7 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexPolyhedronShape(margin), mExtent(extent - Vector3(margin, margin, margin)) { + : ConvexPolyhedronShape(CollisionShapeName::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { assert(extent.x > decimal(0.0) && extent.x > margin); assert(extent.y > decimal(0.0) && extent.y > margin); assert(extent.z > decimal(0.0) && extent.z > margin); diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index f9f442ce..32ea146c 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; * @param height The height of the capsule (in meters) */ CapsuleShape::CapsuleShape(decimal radius, decimal height) - : ConvexShape(CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { + : ConvexShape(CollisionShapeName::CAPSULE, CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { assert(radius > decimal(0.0)); assert(height > decimal(0.0)); } diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index 301c97d2..d03b4a1c 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -32,7 +32,7 @@ using namespace reactphysics3d; // Constructor -CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) { +CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) : mName(name), mType(type), mScaling(1.0, 1.0, 1.0) { } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 78b82ae0..77f96dfb 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -39,9 +39,12 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -/// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD}; -const int NB_COLLISION_SHAPE_TYPES = 6; +/// Type of collision shapes +enum class CollisionShapeType {SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_SHAPE}; +const int NB_COLLISION_SHAPE_TYPES = 4; + +/// Names of collision shapes +enum class CollisionShapeName { TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, TRIANGLE_MESH, HEIGHTFIELD }; // Declarations class ProxyShape; @@ -61,6 +64,9 @@ class CollisionShape { /// Type of the collision shape CollisionShapeType mType; + /// Name of the colision shape + CollisionShapeName mName; + /// Scaling vector of the collision shape Vector3 mScaling; @@ -80,7 +86,7 @@ class CollisionShape { // -------------------- Methods -------------------- // /// Constructor - CollisionShape(CollisionShapeType type); + CollisionShape(CollisionShapeName name, CollisionShapeType type); /// Destructor virtual ~CollisionShape() = default; @@ -91,7 +97,10 @@ class CollisionShape { /// Deleted assignment operator CollisionShape& operator=(const CollisionShape& shape) = delete; - /// Return the type of the collision shapes + /// Return the name of the collision shape + CollisionShapeName getName() const; + + /// Return the type of the collision shape CollisionShapeType getType() const; /// Return true if the collision shape is convex, false if it is concave @@ -115,28 +124,28 @@ class CollisionShape { /// Compute the world-space AABB of the collision shape given a transform virtual void computeAABB(AABB& aabb, const Transform& transform) const; - /// Return true if the collision shape type is a convex shape - static bool isConvex(CollisionShapeType shapeType); - // -------------------- Friendship -------------------- // friend class ProxyShape; friend class CollisionWorld; }; +// Return the name of the collision shape +/** +* @return The name of the collision shape (box, sphere, triangle, ...) +*/ +inline CollisionShapeName CollisionShape::getName() const { + return mName; +} + // Return the type of the collision shape /** - * @return The type of the collision shape (box, sphere, cylinder, ...) + * @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh) */ inline CollisionShapeType CollisionShape::getType() const { return mType; } -// Return true if the collision shape type is a convex shape -inline bool CollisionShape::isConvex(CollisionShapeType shapeType) { - return shapeType != CollisionShapeType::CONCAVE_MESH && shapeType != CollisionShapeType::HEIGHTFIELD; -} - // Return the scaling vector of the collision shape inline Vector3 CollisionShape::getScaling() const { return mScaling; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 8cb5235a..0a9e1fd6 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) - : ConcaveShape(CollisionShapeType::CONCAVE_MESH) { + : ConcaveShape(CollisionShapeName::TRIANGLE_MESH) { mTriangleMesh = triangleMesh; mRaycastTestType = TriangleRaycastSide::FRONT; @@ -79,8 +79,8 @@ void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex, // Apply the scaling factor to the vertices outTriangleVertices[0] *= mScaling.x; - outTriangleVertices[1] *= mScaling.x; - outTriangleVertices[2] *= mScaling.x; + outTriangleVertices[1] *= mScaling.y; + outTriangleVertices[2] *= mScaling.z; } // Return the three vertex normals (in the array outVerticesNormals) of a triangle diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 309ec27c..b99f2b4a 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -139,9 +139,6 @@ class ConcaveMeshShape : public ConcaveShape { /// Return the three vertex normals (in the array outVerticesNormals) of a triangle void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const; - /// Get a smooth contact normal for collision for a triangle of the mesh - Vector3 computeSmoothLocalContactNormalForTriangle(TriangleShape* triangleShape, const Vector3& localContactPoint) const; - public: /// Constructor diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp index 3f691468..e8c032d2 100644 --- a/src/collision/shapes/ConcaveShape.cpp +++ b/src/collision/shapes/ConcaveShape.cpp @@ -31,8 +31,7 @@ using namespace reactphysics3d; // Constructor -ConcaveShape::ConcaveShape(CollisionShapeType type) - : CollisionShape(type), mIsSmoothMeshCollisionEnabled(false), - mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) { +ConcaveShape::ConcaveShape(CollisionShapeName name) + : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) { } diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index 6e07187f..34e347e0 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -63,9 +63,6 @@ class ConcaveShape : public CollisionShape { // -------------------- Attributes -------------------- // - /// True if the smooth mesh collision algorithm is enabled - bool mIsSmoothMeshCollisionEnabled; - // Margin use for collision detection for each triangle decimal mTriangleMargin; @@ -82,7 +79,7 @@ class ConcaveShape : public CollisionShape { // -------------------- Methods -------------------- // /// Constructor - ConcaveShape(CollisionShapeType type); + ConcaveShape(CollisionShapeName name); /// Destructor virtual ~ConcaveShape() override = default; @@ -110,12 +107,6 @@ class ConcaveShape : public CollisionShape { /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; - - /// Return true if the smooth mesh collision is enabled - bool getIsSmoothMeshCollisionEnabled() const; - - /// Enable/disable the smooth mesh collision algorithm - void setIsSmoothMeshCollisionEnabled(bool isEnabled); }; // Return the triangle margin @@ -138,19 +129,6 @@ inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* return false; } -// Return true if the smooth mesh collision is enabled -inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { - return mIsSmoothMeshCollisionEnabled; -} - -// Enable/disable the smooth mesh collision algorithm -/// Smooth mesh collision is used to avoid collisions against some internal edges -/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother -/// but collisions computation is a bit more expensive. -inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { - mIsSmoothMeshCollisionEnabled = isEnabled; -} - // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { return mRaycastTestType; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 263bb736..c8f03fa3 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -42,7 +42,7 @@ using namespace reactphysics3d; * @param margin Collision margin (in meters) around the collision shape */ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin) - : ConvexPolyhedronShape(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { + : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH, margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { // Recalculate the bounds of the mesh recalculateBounds(); diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index e73a7cd7..0db54a85 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor -ConvexPolyhedronShape::ConvexPolyhedronShape(decimal margin) - : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) { +ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name, decimal margin) + : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON, margin) { } diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 6c1de4d5..fe1f07a9 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -47,7 +47,7 @@ class ConvexPolyhedronShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - ConvexPolyhedronShape(decimal margin); + ConvexPolyhedronShape(CollisionShapeName name, decimal margin); /// Destructor virtual ~ConvexPolyhedronShape() override = default; diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp index f077cdcb..649e50f5 100644 --- a/src/collision/shapes/ConvexShape.cpp +++ b/src/collision/shapes/ConvexShape.cpp @@ -31,8 +31,8 @@ using namespace reactphysics3d; // Constructor -ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) - : CollisionShape(type), mMargin(margin) { +ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin) + : CollisionShape(name, type), mMargin(margin) { } diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 94acc7f9..4bf4b718 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -63,7 +63,7 @@ class ConvexShape : public CollisionShape { // -------------------- Methods -------------------- // /// Constructor - ConvexShape(CollisionShapeType type, decimal margin); + ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin); /// Destructor virtual ~ConvexShape() override = default; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index c8fc97f2..b13cf69b 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -42,7 +42,7 @@ using namespace reactphysics3d; HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, const void* heightFieldData, HeightDataType dataType, int upAxis, decimal integerHeightScale) - : ConcaveShape(CollisionShapeType::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), + : ConcaveShape(CollisionShapeName::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), mHeightDataType(dataType) { diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 73fe49aa..8583e80d 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; /** * @param radius Radius of the sphere (in meters) */ -SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeType::SPHERE, radius) { +SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { assert(radius > decimal(0.0)); } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 72eb93d3..c72dd133 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; */ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin) - : ConvexPolyhedronShape(margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { mPoints[0] = point1; mPoints[1] = point2; @@ -68,15 +68,15 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const // This method will return the new smooth world contact // normal of the triangle and the the local contact point on the other shape. void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, - Vector3& localContactPointShape1, Vector3 localContactPointShape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, const Transform& shape1ToWorld, const Transform& shape2ToWorld, decimal penetrationDepth, Vector3& outSmoothVertexNormal) { - assert(shape1->getType() != CollisionShapeType::TRIANGLE || shape2->getType() != CollisionShapeType::TRIANGLE); + assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); // If one the shape is a triangle - bool isShape1Triangle = shape1->getType() == CollisionShapeType::TRIANGLE; - if (isShape1Triangle || shape2->getType() == CollisionShapeType::TRIANGLE) { + bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; + if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): static_cast(shape2); @@ -140,14 +140,6 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& decimal u, v, w; computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); - int nbZeros = 0; - bool isUZero = approxEqual(u, decimal(0), decimal(0.0001)); - bool isVZero = approxEqual(v, decimal(0), decimal(0.0001)); - bool isWZero = approxEqual(w, decimal(0), decimal(0.0001)); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; - // We compute the contact normal as the barycentric interpolation of the three vertices normals return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit(); } diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 900b71db..609d5892 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -173,7 +173,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, - Vector3& localContactPointShape1, Vector3 localContactPointShape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, const Transform& shape1ToWorld, const Transform& shape2ToWorld, decimal penetrationDepth, Vector3& outSmoothVertexNormal); diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index a23857e3..f3f85fa1 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -36,7 +36,12 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr), mTempMemoryAllocator(temporaryMemoryAllocator) { -} +} + +// Destructor +OverlappingPair::~OverlappingPair() { + assert(mPotentialContactManifolds == nullptr); +} // Create a new potential contact manifold using contact-points from narrow-phase void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 906cddb7..e8c89dda 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -117,7 +117,7 @@ class OverlappingPair { Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator); /// Destructor - ~OverlappingPair() = default; + ~OverlappingPair(); /// Deleted copy-constructor OverlappingPair(const OverlappingPair& pair) = delete; diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index 608e0020..d6a6bbe3 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -109,8 +109,6 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, // do not forget to delete it at the end mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh); - mConcaveShape->setIsSmoothMeshCollisionEnabled(false); - // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index a99d494d..7606a61e 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -94,7 +94,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule2->setSleepingColor(mRedColorDemo); // ---------- Box 1 ---------- // - openglframework::Vector3 position5(0, -0, 0); + openglframework::Vector3 position5(-4, -7, 0); // Create a cylinder and a corresponding collision body in the dynamics world mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); @@ -127,14 +127,15 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mConvexMesh->setSleepingColor(mRedColorDemo); // ---------- Concave Mesh ---------- // - //openglframework::Vector3 position8(0, 0, 0); + openglframework::Vector3 position8(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - //mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + mAllShapes.push_back(mConcaveMesh); // Set the color - //mConcaveMesh->setColor(mGreyColorDemo); - //mConcaveMesh->setSleepingColor(mRedColorDemo); + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mRedColorDemo); // ---------- Heightfield ---------- // //openglframework::Vector3 position9(0, 0, 0); @@ -183,6 +184,9 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); delete mConvexMesh; + mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + delete mConcaveMesh; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -206,12 +210,6 @@ CollisionDetectionScene::~CollisionDetectionScene() { // Destroy the dumbbell delete mDumbbell; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); - - // Destroy the convex mesh - delete mConcaveMesh; - // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); @@ -256,6 +254,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); @@ -263,7 +262,6 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); */ diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index d0f1b569..8e94eaf5 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -147,7 +147,7 @@ class CollisionDetectionScene : public SceneDemo { Box* mBox2; ConvexMesh* mConvexMesh; //Dumbbell* mDumbbell; - //ConcaveMesh* mConcaveMesh; + ConcaveMesh* mConcaveMesh; //HeightField* mHeightField; std::vector mAllShapes; From 673e487f141948fece1a1dbd0a35d6d78b33bb04 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 1 Sep 2017 07:37:45 +0200 Subject: [PATCH 040/105] Remove temporal coherence from SAT for sphere vs polyhedron and capsule vs polyhedron --- .../narrowphase/SAT/SATAlgorithm.cpp | 258 ++++-------------- 1 file changed, 53 insertions(+), 205 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index c5210c64..b551161d 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -74,69 +74,23 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow decimal minPenetrationDepth = DECIMAL_LARGEST; uint minFaceIndex = 0; - // True if the shapes were overlapping in the previous frame and are - // still overlapping on the same axis in this frame - bool isTemporalCoherenceValid = false; - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous - // frame collision data per triangle) - if (polyhedron->getName() != CollisionShapeName::TRIANGLE) { + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. - - // Compute the penetration depth of the shapes along the face normal direction - decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - sphere, sphereCenter); - - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { - - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - } + return false; } - } - // We the shapes are still overlapping in the same axis as in - // the previous frame, we skip the whole SAT algorithm - if (!isTemporalCoherenceValid) { - - // For each face of the convex mesh - for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - - // Compute the penetration depth of the shapes along the face normal direction - decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter); - - // If the penetration depth is negative, we have found a separating axis - if (penetrationDepth <= decimal(0.0)) { - - lastFrameInfo.satMinAxisFaceIndex = f; - - return false; - } - - // Check if we have found a new minimum penetration axis - if (penetrationDepth < minPenetrationDepth) { - minPenetrationDepth = penetrationDepth; - minFaceIndex = f; - } + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; } } @@ -161,8 +115,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); } - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; - return true; } @@ -217,171 +169,72 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro Vector3 separatingPolyhedronEdgeVertex1; Vector3 separatingPolyhedronEdgeVertex2; - // True if the shapes were overlapping in the previous frame and are - // still overlapping on the same axis in this frame - bool isTemporalCoherenceValid = false; + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); + Vector3 outFaceNormalCapsuleSpace; - // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous - // frame collision data per triangle) - if (polyhedron->getName() != CollisionShapeName::TRIANGLE) { + // Compute the penetration depth + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape, + polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + return false; + } - // If the previous minimum separation axis was a face normal of the polyhedron - if (lastFrameInfo.satIsAxisFacePolyhedron1) { - - Vector3 outFaceNormalCapsuleSpace; - - // Compute the penetration depth along the polyhedron face normal direction - const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron, - capsuleShape, polyhedronToCapsuleTransform, - outFaceNormalCapsuleSpace); - - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { - - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - isMinPenetrationFaceNormal = true; - separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; - } - } - else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron - - // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index); - const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); - const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); - const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - - Vector3 outAxis; - - // Compute the penetration depth along this axis - const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, - capsuleSegmentAxis, edgeVertex1, - edgeDirectionCapsuleSpace, - polyhedronToCapsuleTransform, - outAxis); - - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { - - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minEdgeIndex = lastFrameInfo.satMinEdge1Index; - isMinPenetrationFaceNormal = false; - separatingAxisCapsuleSpace = outAxis; - separatingPolyhedronEdgeVertex1 = edgeVertex1; - separatingPolyhedronEdgeVertex2 = edgeVertex2; - } - } + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; } } - // If the shapes are still overlapping in the same axis as in the previous frame - // the previous frame, we skip the whole SAT algorithm - if (!isTemporalCoherenceValid) { + // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron + for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { - // For each face of the convex mesh - for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - Vector3 outFaceNormalCapsuleSpace; + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); + const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + + // Check using the Gauss Map if this edge cross product can be as separating axis + if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + + Vector3 outAxis; // Compute the penetration depth - const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape, - polyhedronToCapsuleTransform, - outFaceNormalCapsuleSpace); + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); // If the penetration depth is negative, we have found a separating axis if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = true; - lastFrameInfo.satMinAxisFaceIndex = f; - return false; } // Check if we have found a new minimum penetration axis if (penetrationDepth < minPenetrationDepth) { minPenetrationDepth = penetrationDepth; - minFaceIndex = f; - isMinPenetrationFaceNormal = true; - separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + minEdgeIndex = e; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = outAxis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; } } - - // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron - for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { - - // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); - const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); - const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); - const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); - const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); - const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); - - // Check using the Gauss Map if this edge cross product can be as separating axis - if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { - - Vector3 outAxis; - - // Compute the penetration depth - const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, - capsuleSegmentAxis, edgeVertex1, - edgeDirectionCapsuleSpace, - polyhedronToCapsuleTransform, - outAxis); - - // If the penetration depth is negative, we have found a separating axis - if (penetrationDepth <= decimal(0.0)) { - - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satMinEdge1Index = e; - - return false; - } - - // Check if we have found a new minimum penetration axis - if (penetrationDepth < minPenetrationDepth) { - minPenetrationDepth = penetrationDepth; - minEdgeIndex = e; - isMinPenetrationFaceNormal = false; - separatingAxisCapsuleSpace = outAxis; - separatingPolyhedronEdgeVertex1 = edgeVertex1; - separatingPolyhedronEdgeVertex2 = edgeVertex2; - } - } - } - } // Convert the inner capsule segment points into the polyhedron local-space @@ -404,8 +257,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro narrowPhaseInfo, isCapsuleShape1); } - lastFrameInfo.satIsAxisFacePolyhedron1 = true; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; } else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment @@ -433,9 +284,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); } - - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satMinEdge1Index = minEdgeIndex; } return true; From 6a22b3a81d5a128f42fa08333cef9c041fef8697 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 3 Sep 2017 10:48:39 +0200 Subject: [PATCH 041/105] Fix temporal coherence in SAT algorithm between two convex polyhedra --- .../narrowphase/SAT/SATAlgorithm.cpp | 253 +++++++++++------- src/collision/narrowphase/SAT/SATAlgorithm.h | 7 + 2 files changed, 164 insertions(+), 96 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index b551161d..87be645b 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -489,6 +489,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; isMinPenetrationFaceNormal = true; isMinPenetrationFaceNormalPolyhedron1 = true; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + + return true; + } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = false; + } } } else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) @@ -513,6 +531,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; isMinPenetrationFaceNormal = true; isMinPenetrationFaceNormalPolyhedron1 = false; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + + return true; + } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = false; + } } } else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges @@ -544,6 +580,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // we will skip the entire SAT algorithm because the minimum separating axis did not change isTemporalCoherenceValid = lastFrameInfo.wasColliding; + // Temporal coherence is valid only if the two edges build a minkowski + // face (and the cross product is therefore a candidate for separating axis + if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + isTemporalCoherenceValid = false; + } + if (isTemporalCoherenceValid) { minPenetrationDepth = penetrationDepth; @@ -672,102 +714,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (reportContacts) { - const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; - const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; - const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; - const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; - - assert(minPenetrationDepth > decimal(0.0)); - - const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); - const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; - - // Compute the world normal - Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : - -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); - - // Get the reference face - HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); - - // Find the incident face on the other polyhedron (most anti-parallel face) - uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); - - // Get the incident face - HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); - - std::vector polygonVertices; // Vertices to clip of the incident face - std::vector planesNormals; // Normals of the clipping planes - std::vector planesPoints; // Points on the clipping planes - - // Get all the vertices of the incident face (in the reference local-space) - std::vector::const_iterator it; - for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { - const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); - polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); - } - - // Get the reference face clipping planes - uint currentEdgeIndex = referenceFace.edgeIndex; - uint firstEdgeIndex = currentEdgeIndex; - do { - - // Get the adjacent edge - HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); - - // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); - - // Compute the edge vertices and edge direction - Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); - Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); - Vector3 edgeDirection = edgeV2 - edgeV1; - - // Compute the normal of the clipping plane for this edge - // The clipping plane is perpendicular to the edge direction and the reference face normal - Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - - planesNormals.push_back(clipPlaneNormal); - planesPoints.push_back(edgeV1); - - // Go to the next adjacent edge of the reference face - currentEdgeIndex = edge.nextEdgeIndex; - - } while (currentEdgeIndex != firstEdgeIndex); - - assert(planesNormals.size() > 0); - assert(planesNormals.size() == planesPoints.size()); - - // Clip the reference faces with the adjacent planes of the reference face - std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); - assert(clipPolygonVertices.size() > 0); - - // We only keep the clipped points that are below the reference face - const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); - std::vector::const_iterator itPoints; - for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { - - // If the clip point is bellow the reference face - if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { - - // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); - - // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); - - // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle - TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, - narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, - minPenetrationDepth, normalWorld); - - // Create a new contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, - isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); - } - } + // Compute the contact points between two faces of two convex polyhedra. + bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, + polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, + minFaceIndex, narrowPhaseInfo, minPenetrationDepth); + assert(contactsFound); } lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; @@ -809,6 +760,116 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn return true; } +// Compute the contact points between two faces of two convex polyhedra. +/// The method returns true if contact points have been found +bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, + const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1, + uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const { + + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; + const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; + const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + + assert(minPenetrationDepth > decimal(0.0)); + + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); + const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; + + // Compute the world normal + Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); + + // Get the reference face + HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + + // Find the incident face on the other polyhedron (most anti-parallel face) + uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + + // Get the incident face + HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + + std::vector polygonVertices; // Vertices to clip of the incident face + std::vector planesNormals; // Normals of the clipping planes + std::vector planesPoints; // Points on the clipping planes + + // Get all the vertices of the incident face (in the reference local-space) + std::vector::const_iterator it; + for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); + polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + } + + // Get the reference face clipping planes + uint currentEdgeIndex = referenceFace.edgeIndex; + uint firstEdgeIndex = currentEdgeIndex; + do { + + // Get the adjacent edge + HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + + // Get the twin edge + HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Compute the edge vertices and edge direction + Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); + Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); + Vector3 edgeDirection = edgeV2 - edgeV1; + + // Compute the normal of the clipping plane for this edge + // The clipping plane is perpendicular to the edge direction and the reference face normal + Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); + + planesNormals.push_back(clipPlaneNormal); + planesPoints.push_back(edgeV1); + + // Go to the next adjacent edge of the reference face + currentEdgeIndex = edge.nextEdgeIndex; + + } while (currentEdgeIndex != firstEdgeIndex); + + assert(planesNormals.size() > 0); + assert(planesNormals.size() == planesPoints.size()); + + // Clip the reference faces with the adjacent planes of the reference face + std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + assert(clipPolygonVertices.size() > 0); + + // We only keep the clipped points that are below the reference face + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); + std::vector::const_iterator itPoints; + bool contactPointsFound = false; + for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + + // If the clip point is bellow the reference face + if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { + + contactPointsFound = true; + + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + + // Project the contact point onto the reference face + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + + // Create a new contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + } + } + + return contactPointsFound; +} + // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector // This is used to find the incident face on a polyhedron of a given reference face of another polyhedron uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 73838d95..37e4f7d2 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -96,6 +96,13 @@ class SATAlgorithm { const Vector3& edgeDirectionCapsuleSpace, const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const; + /// Compute the contact points between two faces of two convex polyhedra. + bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2, + const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex, + NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const; + + public : // -------------------- Methods -------------------- // From e1602f2b272c571e68d234fcf1e340e811071300 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 3 Sep 2017 17:35:09 +0200 Subject: [PATCH 042/105] Fix issues with normals computation in TriangleVertexArray --- src/collision/TriangleVertexArray.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index ad59d37a..1e500001 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -163,9 +163,9 @@ void TriangleVertexArray::computeVerticesNormals() { Vector3 normalComponent = std::asin(sinA) * crossProduct; // Add the normal component of this vertex into the normals array - verticesNormals[verticesIndices[v]] = normalComponent.x; - verticesNormals[verticesIndices[v] + 1] = normalComponent.y; - verticesNormals[verticesIndices[v] + 2] = normalComponent.z; + verticesNormals[verticesIndices[v] * 3] = normalComponent.x; + verticesNormals[verticesIndices[v] * 3 + 1] = normalComponent.y; + verticesNormals[verticesIndices[v] * 3 + 2] = normalComponent.z; } } @@ -173,12 +173,12 @@ void TriangleVertexArray::computeVerticesNormals() { for (uint v=0; v(verticesNormals); From 8bab9c13483210a865bd96bbdf30b1e2db05f4c2 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 3 Sep 2017 18:05:23 +0200 Subject: [PATCH 043/105] Remove unused cachedCollisionData parameter --- src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 17 +++++------------ src/collision/narrowphase/SAT/SATAlgorithm.cpp | 6 +++--- src/collision/shapes/BoxShape.h | 6 ++---- src/collision/shapes/CapsuleShape.h | 6 ++---- src/collision/shapes/ConvexMeshShape.cpp | 5 +---- src/collision/shapes/ConvexMeshShape.h | 3 +-- src/collision/shapes/ConvexShape.cpp | 5 ++--- src/collision/shapes/ConvexShape.h | 8 ++------ src/collision/shapes/SphereShape.h | 6 ++---- src/collision/shapes/TriangleShape.h | 5 ++--- 10 files changed, 22 insertions(+), 45 deletions(-) diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 609c415a..9bc1d00d 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -66,9 +66,6 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); // Get the local-space to world-space transforms @@ -109,8 +106,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase do { // Compute the support points for original objects (without margins) A and B - suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); + suppA = shape1->getLocalSupportPointWithoutMargin(-v); + suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -237,8 +234,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - // Support point of object B (object B is a single point) const Vector3 suppB(localPoint); @@ -254,7 +249,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS do { // Compute the support points for original objects (without margins) A and B - suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); + suppA = shape->getLocalSupportPointWithoutMargin(-v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -300,8 +295,6 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) Vector3 suppB; // Support point on the collision shape const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; @@ -321,7 +314,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); decimal lambda = decimal(0.0); suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); + suppB = shape->getLocalSupportPointWithoutMargin(rayDirection); Vector3 v = suppA - suppB; decimal vDotW, vDotR; decimal distSquare = v.lengthSquare(); @@ -331,7 +324,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); + suppB = shape->getLocalSupportPointWithoutMargin(v); w = suppA - suppB; vDotW = v.dot(w); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 87be645b..d1062704 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -314,7 +314,7 @@ decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const Con outAxis.normalize(); // Compute the penetration depth - const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis, nullptr); + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis); const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis); } @@ -335,7 +335,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; - const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr); + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace); const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace); @@ -932,7 +932,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; // Get the support point of polyhedron 2 in the inverse direction of face normal - const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space); // Compute the penetration depth const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 5a07b0cb..d64222ff 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -65,8 +65,7 @@ class BoxShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; @@ -170,8 +169,7 @@ inline size_t BoxShape::getSizeInBytes() const { } // Return a local support point in a given direction without the objec margin -inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, direction.y < 0.0 ? -mExtent.y : mExtent.y, diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 9c0a4545..8aebff58 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -56,8 +56,7 @@ class CapsuleShape : public ConvexShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; @@ -169,8 +168,7 @@ inline bool CapsuleShape::isPolyhedron() const { /// Therefore, in this method, we compute the support points of both top and bottom spheres of /// the capsule and return the point with the maximum dot product with the direction vector. Note /// that the object margin is implicitly the radius and height of the capsule. -inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Support point top sphere decimal dotProductTop = mHalfHeight * direction.y; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index c8f03fa3..995ef9bb 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -56,10 +56,7 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin) /// it as a start in a hill-climbing (local search) process to find the new support vertex which /// will be in most of the cases very close to the previous one. Using hill-climbing, this method /// runs in almost constant time. -Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - // TODO : Do we still need to have cachedCollisionData or we can remove it from everywhere ? +Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { double maxDotProduct = DECIMAL_SMALLEST; uint indexMaxDotProduct = 0; diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index dd2b7021..b42c02e2 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -83,8 +83,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual void setLocalScaling(const Vector3& scaling) override; /// Return a local support point in a given direction without the object margin. - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp index 649e50f5..90c5356f 100644 --- a/src/collision/shapes/ConvexShape.cpp +++ b/src/collision/shapes/ConvexShape.cpp @@ -37,11 +37,10 @@ ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decim } // Return a local support point in a given direction with the object margin -Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { +Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // Get the support point without margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); if (mMargin != decimal(0.0)) { diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 4bf4b718..9a2b7e00 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -49,14 +49,10 @@ class ConvexShape : public CollisionShape { // -------------------- Methods -------------------- // // Return a local support point in a given direction with the object margin - // TODO : Try to remove cachedCollisionData parameter - Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; + Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - // TODO : Try to remove cachedCollisionData parameter - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const=0; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0; public : diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index fe55a7a8..70f651de 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -49,8 +49,7 @@ class SphereShape : public ConvexShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; @@ -123,8 +122,7 @@ inline size_t SphereShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Return the center of the sphere (the radius is taken into account in the object margin) return Vector3(0.0, 0.0, 0.0); diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 609d5892..19cedfef 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -81,8 +81,7 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Get a smooth contact normal for collision for a triangle of the mesh Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const; @@ -190,7 +189,7 @@ inline size_t TriangleShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const { +inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); return mPoints[dotProducts.getMaxAxis()]; } From 946e62dd4bdf5d934564830a055fcbd92aaa1eaa Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 3 Sep 2017 19:06:02 +0200 Subject: [PATCH 044/105] Remove unnecessary collision margin for some shapes --- src/collision/ContactManifold.cpp | 3 --- src/collision/MiddlePhaseTriangleCallback.cpp | 3 +-- src/collision/shapes/BoxShape.cpp | 10 +++++----- src/collision/shapes/BoxShape.h | 2 +- src/collision/shapes/ConcaveMeshShape.cpp | 4 +--- src/collision/shapes/ConcaveShape.cpp | 2 +- src/collision/shapes/ConcaveShape.h | 11 ----------- src/collision/shapes/ConvexMeshShape.cpp | 4 ++-- src/collision/shapes/ConvexMeshShape.h | 4 +--- src/collision/shapes/ConvexPolyhedronShape.cpp | 4 ++-- src/collision/shapes/ConvexPolyhedronShape.h | 2 +- src/collision/shapes/ConvexShape.h | 2 +- src/collision/shapes/HeightFieldShape.cpp | 3 +-- src/collision/shapes/TriangleShape.cpp | 4 ++-- src/collision/shapes/TriangleShape.h | 2 +- src/configuration.h | 3 --- test/tests/collision/TestPointInside.h | 2 +- test/tests/collision/TestRaycast.h | 2 +- 18 files changed, 22 insertions(+), 45 deletions(-) diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 846a418a..803a9e82 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -65,9 +65,6 @@ ContactManifold::~ContactManifold() { ContactPoint* nextContactPoint = contactPoint->getNext(); - // TODO : Delete this - bool test = mMemoryAllocator.isReleaseNeeded(); - // Delete the contact point contactPoint->~ContactPoint(); mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index a1a183ac..de6c22ea 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -34,10 +34,9 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. - decimal margin = mConcaveShape->getTriangleMargin(); TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex, margin); + verticesNormals, meshSubPart, triangleIndex); // Create a narrow phase info for the narrow-phase collision detection NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 54666136..01c4d965 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -37,11 +37,11 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) * @param margin The collision margin (in meters) around the collision shape */ -BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexPolyhedronShape(CollisionShapeName::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { - assert(extent.x > decimal(0.0) && extent.x > margin); - assert(extent.y > decimal(0.0) && extent.y > margin); - assert(extent.z > decimal(0.0) && extent.z > margin); +BoxShape::BoxShape(const Vector3& extent) + : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) { + assert(extent.x > decimal(0.0)); + assert(extent.y > decimal(0.0)); + assert(extent.z > decimal(0.0)); // Vertices mHalfEdgeStructure.addVertex(0); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d64222ff..56c14797 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -81,7 +81,7 @@ class BoxShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN); + BoxShape(const Vector3& extent); /// Destructor virtual ~BoxShape() override = default; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 0a9e1fd6..84e95e2c 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -59,7 +59,6 @@ void ConcaveMeshShape::initBVHTree() { // Create the AABB for the triangle AABB aabb = AABB::createAABBForTriangle(trianglePoints); - aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin); // Add the AABB with the index of the triangle into the dynamic AABB tree mDynamicAABBTree.addObject(aabb, subPart, triangleIndex); @@ -152,9 +151,8 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { Vector3 verticesNormals[3]; mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape - decimal margin = mConcaveMeshShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, data[0], data[1], margin); + verticesNormals, data[0], data[1]); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); // Ray casting test against the collision shape diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp index e8c032d2..cac82e45 100644 --- a/src/collision/shapes/ConcaveShape.cpp +++ b/src/collision/shapes/ConcaveShape.cpp @@ -32,6 +32,6 @@ using namespace reactphysics3d; // Constructor ConcaveShape::ConcaveShape(CollisionShapeName name) - : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) { + : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mRaycastTestType(TriangleRaycastSide::FRONT) { } diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index 34e347e0..abc3759b 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -63,9 +63,6 @@ class ConcaveShape : public CollisionShape { // -------------------- Attributes -------------------- // - // Margin use for collision detection for each triangle - decimal mTriangleMargin; - /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; @@ -90,9 +87,6 @@ class ConcaveShape : public CollisionShape { /// Deleted assignment operator ConcaveShape& operator=(const ConcaveShape& shape) = delete; - /// Return the triangle margin - decimal getTriangleMargin() const; - /// Return the raycast test type (front, back, front-back) TriangleRaycastSide getRaycastTestType() const; @@ -109,11 +103,6 @@ class ConcaveShape : public CollisionShape { virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; }; -// Return the triangle margin -inline decimal ConcaveShape::getTriangleMargin() const { - return mTriangleMargin; -} - // Return true if the collision shape is convex, false if it is concave inline bool ConcaveShape::isConvex() const { return false; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 995ef9bb..f6011375 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -41,8 +41,8 @@ using namespace reactphysics3d; * @param stride Stride between the beginning of two elements in the vertices array * @param margin Collision margin (in meters) around the collision shape */ -ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin) - : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH, margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { +ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh) + : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) { // Recalculate the bounds of the mesh recalculateBounds(); diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index b42c02e2..4ceabe11 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -99,9 +99,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - // TODO : Do we really need to use the margin anymore ? Maybe for raycasting ? If not, remove all the - // comments documentation about margin - ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin = OBJECT_MARGIN); + ConvexMeshShape(PolyhedronMesh* polyhedronMesh); /// Destructor virtual ~ConvexMeshShape() override = default; diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index 0db54a85..ab93584d 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor -ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name, decimal margin) - : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON, margin) { +ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name) + : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) { } diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index fe1f07a9..96707d69 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -47,7 +47,7 @@ class ConvexPolyhedronShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - ConvexPolyhedronShape(CollisionShapeName name, decimal margin); + ConvexPolyhedronShape(CollisionShapeName name); /// Destructor virtual ~ConvexPolyhedronShape() override = default; diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 9a2b7e00..04f8aad6 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -59,7 +59,7 @@ class ConvexShape : public CollisionShape { // -------------------- Methods -------------------- // /// Constructor - ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin); + ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin = decimal(0.0)); /// Destructor virtual ~ConvexShape() override = default; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index b13cf69b..2afba401 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -260,9 +260,8 @@ void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* verticesNormals) { // Create a triangle collision shape - decimal margin = mHeightFieldShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex, margin); + verticesNormals, meshSubPart, triangleIndex); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); // Ray casting test against the collision shape diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index c72dd133..48baa225 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -44,8 +44,8 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { + const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { mPoints[0] = point1; mPoints[1] = point2; diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 19cedfef..c31a594c 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -108,7 +108,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// Constructor TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin = OBJECT_MARGIN); + const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex); /// Destructor virtual ~TriangleShape() override = default; diff --git a/src/configuration.h b/src/configuration.h index 04cb0956..b2c53021 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -100,9 +100,6 @@ constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); /// True if the spleeping technique is enabled constexpr bool SPLEEPING_ENABLED = true; -/// Object margin for collision detection in meters (for the GJK-EPA Algorithm) -constexpr decimal OBJECT_MARGIN = decimal(0.04); - /// Distance threshold for two contact points for a valid persistent contact (in meters) constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index abb54791..608b439b 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -115,7 +115,7 @@ class TestPointInside : public Test { mLocalShapeToWorld = mBodyTransform * mShapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(Vector3(2, 3, 4)); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); mSphereShape = new SphereShape(3); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 93860a1c..81013570 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -192,7 +192,7 @@ class TestRaycast : public Test { mLocalShapeToWorld = mBodyTransform * mShapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(Vector3(2, 3, 4)); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); mSphereShape = new SphereShape(3); From 501bca5e3d7ee7b6a0b7b918eeb4e179cd15711a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 4 Sep 2017 07:26:01 +0200 Subject: [PATCH 045/105] Fix issue in TriangleVertexArray --- src/collision/TriangleVertexArray.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index 1e500001..fa87e684 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -173,12 +173,12 @@ void TriangleVertexArray::computeVerticesNormals() { for (uint v=0; v(verticesNormals); From 95ade79af5d6e8ceb9427ff5d15fe49678cda7a3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 4 Sep 2017 21:23:07 +0200 Subject: [PATCH 046/105] Fix issue with obsolete contact points that were not removed --- src/collision/CollisionDetection.cpp | 14 +++----------- src/collision/CollisionDetection.h | 3 --- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 3c4b771b..3f9a45f6 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -97,9 +97,6 @@ void CollisionDetection::computeMiddlePhase() { PROFILE("CollisionDetection::computeMiddlePhase()"); - // Clear the set of overlapping pairs in narrow-phase contact - mContactOverlappingPairs.clear(); - // For each possible collision pair of bodies map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { @@ -258,11 +255,6 @@ void CollisionDetection::computeNarrowPhase() { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - // Add the overlapping pair into the set of pairs in contact during narrow-phase - overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), - currentNarrowPhaseInfo->overlappingPair->getShape2()); - mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; } else { @@ -349,7 +341,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() { // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // Add all the contact manifolds of the pair into the list of contact manifolds // of the two bodies involved in the contact @@ -396,7 +388,7 @@ void CollisionDetection::processAllPotentialContacts() { // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // Process the potential contacts of the overlapping pair processPotentialContacts(it->second); @@ -430,7 +422,7 @@ void CollisionDetection::reportAllContacts() { // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // If there is a user callback if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 49f0d56b..848e7d09 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -86,9 +86,6 @@ class CollisionDetection { /// Broad-phase overlapping pairs std::map mOverlappingPairs; - /// Overlapping pairs in contact (during the current Narrow-phase collision detection) - std::map mContactOverlappingPairs; - /// Broad-phase algorithm BroadPhaseAlgorithm mBroadPhaseAlgorithm; From dd91f6dcbf49eaba97d15b603cf3fcf36ba60426 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 4 Sep 2017 22:23:29 +0200 Subject: [PATCH 047/105] Fix typo --- src/collision/CollisionDetection.cpp | 6 +++--- src/collision/ContactManifold.cpp | 6 +++--- src/collision/ContactManifold.h | 28 ++++++++++++++-------------- src/collision/ContactManifoldSet.cpp | 16 ++++++++-------- src/collision/ContactManifoldSet.h | 8 ++++---- src/constraint/ContactPoint.cpp | 6 +++--- src/constraint/ContactPoint.h | 24 ++++++++++++------------ src/engine/OverlappingPair.h | 16 ++++++++-------- 8 files changed, 55 insertions(+), 55 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 3f9a45f6..b284c034 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -103,7 +103,7 @@ void CollisionDetection::computeMiddlePhase() { OverlappingPair* pair = it->second; - // Make all the contact manifolds and contact points of the pair obselete + // Make all the contact manifolds and contact points of the pair obsolete pair->makeContactsObselete(); ProxyShape* shape1 = pair->getShape1(); @@ -410,8 +410,8 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { potentialManifold = potentialManifold->mNext; } - // Clear the obselete contact manifolds and contact points - pair->clearObseleteManifoldsAndContactPoints(); + // Clear the obsolete contact manifolds and contact points + pair->clearObsoleteManifoldsAndContactPoints(); // Reset the potential contacts of the pair pair->clearPotentialContactManifolds(); diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 803a9e82..62ca7a0c 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -34,7 +34,7 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) { // For each contact point info in the manifold const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); @@ -87,7 +87,7 @@ void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) mNbContactPoints++; } -// Clear the obselete contact points +// Clear the obsolete contact points void ContactManifold::clearObseleteContactPoints() { ContactPoint* contactPoint = mContactPoints; @@ -96,7 +96,7 @@ void ContactManifold::clearObseleteContactPoints() { ContactPoint* nextContactPoint = contactPoint->getNext(); - if (contactPoint->getIsObselete()) { + if (contactPoint->getIsObsolete()) { // Delete the contact point contactPoint->~ContactPoint(); diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index ada20003..e34c4543 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -140,8 +140,8 @@ class ContactManifold { /// Pointer to the previous contact manifold in linked-list ContactManifold* mPrevious; - /// True if the contact manifold is obselete - bool mIsObselete; + /// True if the contact manifold is obsolete + bool mIsObsolete; // -------------------- Methods -------------------- // @@ -151,13 +151,13 @@ class ContactManifold { /// Set the pointer to the next element in the linked-list void setNext(ContactManifold* nextManifold); - /// Return true if the manifold is obselete - bool getIsObselete() const; + /// Return true if the manifold is obsolete + bool getIsObsolete() const; - /// Set to true to make the manifold obselete - void setIsObselete(bool isObselete, bool setContactPoints); + /// Set to true to make the manifold obsolete + void setIsObsolete(bool isObselete, bool setContactPoints); - /// Clear the obselete contact points + /// Clear the obsolete contact points void clearObseleteContactPoints(); /// Return the contact normal direction Id of the manifold @@ -384,19 +384,19 @@ inline void ContactManifold::setNext(ContactManifold* nextManifold) { mNext = nextManifold; } -// Return true if the manifold is obselete -inline bool ContactManifold::getIsObselete() const { - return mIsObselete; +// Return true if the manifold is obsolete +inline bool ContactManifold::getIsObsolete() const { + return mIsObsolete; } -// Set to true to make the manifold obselete -inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) { - mIsObselete = isObselete; +// Set to true to make the manifold obsolete +inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) { + mIsObsolete = isObsolete; if (setContactPoints) { ContactPoint* contactPoint = mContactPoints; while (contactPoint != nullptr) { - contactPoint->setIsObselete(isObselete); + contactPoint->setIsObsolete(isObsolete); contactPoint = contactPoint->getNext(); } diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index fb3beda2..5ad8ce71 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -131,8 +131,8 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, contactPointInfo = contactPointInfo->next; } - // The old manifold is no longer obselete - oldManifold->setIsObselete(false, false); + // The old manifold is no longer obsolete + oldManifold->setIsObsolete(false, false); } // Return the manifold with the smallest contact penetration depth among its points @@ -270,27 +270,27 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) { mNbManifolds--; } -// Make all the contact manifolds and contact points obselete -void ContactManifoldSet::makeContactsObselete() { +// Make all the contact manifolds and contact points obsolete +void ContactManifoldSet::makeContactsObsolete() { ContactManifold* manifold = mManifolds; while (manifold != nullptr) { - manifold->setIsObselete(true, true); + manifold->setIsObsolete(true, true); manifold = manifold->getNext(); } } -// Clear the obselete contact manifolds and contact points -void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() { +// Clear the obsolete contact manifolds and contact points +void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() { ContactManifold* manifold = mManifolds; ContactManifold* previousManifold = nullptr; while (manifold != nullptr) { ContactManifold* nextManifold = manifold->getNext(); - if (manifold->getIsObselete()) { + if (manifold->getIsObsolete()) { if (previousManifold != nullptr) { previousManifold->setNext(nextManifold); diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 0990277c..1676f5a2 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -115,14 +115,14 @@ class ContactManifoldSet { /// Return a pointer to the first element of the linked-list of contact manifolds ContactManifold* getContactManifolds() const; - /// Make all the contact manifolds and contact points obselete - void makeContactsObselete(); + /// Make all the contact manifolds and contact points obsolete + void makeContactsObsolete(); /// Return the total number of contact points in the set of manifolds int getTotalNbContactPoints() const; - /// Clear the obselete contact manifolds and contact points - void clearObseleteManifoldsAndContactPoints(); + /// Clear the obsolete contact manifolds and contact points + void clearObsoleteManifoldsAndContactPoints(); // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index fa6838ec..2cd951b4 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -37,7 +37,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), - mIsRestingContact(false), mIsObselete(false), mNext(nullptr) { + mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) { assert(mPenetrationDepth > decimal(0.0)); @@ -45,7 +45,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; - mIsObselete = false; + mIsObsolete = false; } // Update the contact point with a new one that is similar (very close) @@ -64,5 +64,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; - mIsObselete = false; + mIsObsolete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 41cc35e4..288abdcc 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -71,8 +71,8 @@ class ContactPoint { /// Cached penetration impulse decimal mPenetrationImpulse; - /// True if the contact point is obselete - bool mIsObselete; + /// True if the contact point is obsolete + bool mIsObsolete; /// Pointer to the next contact point in the linked-list ContactPoint* mNext; @@ -93,14 +93,14 @@ class ContactPoint { /// Set the mIsRestingContact variable void setIsRestingContact(bool isRestingContact); - /// Set to true to make the contact point obselete - void setIsObselete(bool isObselete); + /// Set to true to make the contact point obsolete + void setIsObsolete(bool isObselete); /// Set the next contact point in the linked list void setNext(ContactPoint* next); - /// Return true if the contact point is obselete - bool getIsObselete() const; + /// Return true if the contact point is obsolete + bool getIsObsolete() const; public : @@ -206,14 +206,14 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } -// Return true if the contact point is obselete -inline bool ContactPoint::getIsObselete() const { - return mIsObselete; +// Return true if the contact point is obsolete +inline bool ContactPoint::getIsObsolete() const { + return mIsObsolete; } -// Set to true to make the contact point obselete -inline void ContactPoint::setIsObselete(bool isObselete) { - mIsObselete = isObselete; +// Set to true to make the contact point obsolete +inline void ContactPoint::setIsObsolete(bool isObsolete) { + mIsObsolete = isObsolete; } // Return the next contact point in the linked list diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index e8c89dda..5141b8cc 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -164,11 +164,11 @@ class OverlappingPair { /// Reduce the number of contact points of all the potential contact manifolds void reducePotentialContactManifolds(); - /// Make the contact manifolds and contact points obselete + /// Make the contact manifolds and contact points obsolete void makeContactsObselete(); - /// Clear the obselete contact manifold and contact points - void clearObseleteManifoldsAndContactPoints(); + /// Clear the obsolete contact manifold and contact points + void clearObsoleteManifoldsAndContactPoints(); /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -211,10 +211,10 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { return mContactManifoldSet; } -// Make the contact manifolds and contact points obselete +// Make the contact manifolds and contact points obsolete inline void OverlappingPair::makeContactsObselete() { - mContactManifoldSet.makeContactsObselete(); + mContactManifoldSet.makeContactsObsolete(); } // Return the pair of bodies index @@ -262,9 +262,9 @@ inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { return mPotentialContactManifolds; } -// Clear the obselete contact manifold and contact points -inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() { - mContactManifoldSet.clearObseleteManifoldsAndContactPoints(); +// Clear the obsolete contact manifold and contact points +inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() { + mContactManifoldSet.clearObsoleteManifoldsAndContactPoints(); } } From 1b82a3e22891ce863e94c70b8ecc842a7ed75b8c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 7 Sep 2017 22:23:00 +0200 Subject: [PATCH 048/105] Fix issue in GJK algorithm when numerical issue occurs --- src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 9bc1d00d..7979f1df 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -198,7 +198,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // If the penetration depth is negative (due too numerical errors), there is no contact if (penetrationDepth <= decimal(0.0)) { - return GJKResult::INTERPENETRATE; + return GJKResult::SEPARATED; } // Do not generate a contact point with zero normal length From b8907730537bd34ea5cbff8007b1c4a7ec3f7006 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 7 Sep 2017 22:24:30 +0200 Subject: [PATCH 049/105] Fix issue when computing clipping planes in SAT algorithm --- .../narrowphase/SAT/SATAlgorithm.cpp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index d1062704..7a11fbf7 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -256,7 +256,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, narrowPhaseInfo, isCapsuleShape1); } - } else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment @@ -352,6 +351,10 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); + + // Get the face normal + Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); + uint firstEdgeIndex = face.edgeIndex; uint edgeIndex = firstEdgeIndex; @@ -360,12 +363,22 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // For each adjacent edge of the separating face of the polyhedron do { + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); - // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron + // Compute the edge vertices and edge direction + Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex); + Vector3 edgeV2 = polyhedron->getVertexPosition(twinEdge.vertexIndex); + Vector3 edgeDirection = edgeV2 - edgeV1; + + // Compute the normal of the clipping plane for this edge + // The clipping plane is perpendicular to the edge direction and the reference face normal + Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection); + + // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); - planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex)); + planesNormals.push_back(clipPlaneNormal); edgeIndex = edge.nextEdgeIndex; @@ -373,9 +386,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // First we clip the inner segment of the capsule with the four planes of the adjacent faces std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); - assert(clipSegment.size() == 2); - - const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); + assert(clipSegment.size() == 2); // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); From 8cb2ec7e177453c3fa990d1ab6461416bbf4ea9b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 8 Sep 2017 07:38:57 +0200 Subject: [PATCH 050/105] Remove wrong assert --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 7a11fbf7..2f345738 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -845,7 +845,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // Clip the reference faces with the adjacent planes of the reference face std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); - assert(clipPolygonVertices.size() > 0); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); From 709bed3cec3cc5f6ec3e8ee77fc5d29bc24f8fa3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 9 Sep 2017 15:37:55 +0200 Subject: [PATCH 051/105] Fix issues with GJK algorithm --- .../narrowphase/GJK/GJKAlgorithm.cpp | 22 +++++-------------- src/engine/OverlappingPair.h | 2 ++ 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 7979f1df..629811eb 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -66,8 +66,6 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); - // Get the local-space to world-space transforms const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; @@ -95,6 +93,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) { v = lastFrameInfo.gjkSeparatingAxis; + assert(v.lengthSquare() > decimal(0.000001)); } else { v.setAllValues(0, 1, 0); @@ -140,8 +139,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Contact point has been found contactFound = true; - - return GJKResult::INTERPENETRATE; + break; } // Compute the point of the simplex closest to the origin @@ -150,14 +148,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Contact point has been found contactFound = true; - - return GJKResult::INTERPENETRATE; - } - - // Closest point is almost the origin, go to EPA algorithm - // Vector v to small to continue computing support points - if (v.lengthSquare() < MACHINE_EPSILON) { - return GJKResult::INTERPENETRATE; + break; } // Store and update the squared distance of the closest point @@ -177,8 +168,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase break; } - } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() && - distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()); if (contactFound && distSquare > MACHINE_EPSILON) { @@ -203,7 +193,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Do not generate a contact point with zero normal length if (normal.lengthSquare() < MACHINE_EPSILON) { - return GJKResult::INTERPENETRATE; + return GJKResult::SEPARATED; } if (reportContacts) { @@ -219,7 +209,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase return GJKResult::COLLIDE_IN_MARGIN; } - return GJKResult::SEPARATED; + return GJKResult::INTERPENETRATE; } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 5141b8cc..bb7c1048 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -79,6 +79,8 @@ struct LastFrameCollisionInfo { wasColliding = false; wasUsingSAT = false; wasUsingGJK = false; + + gjkSeparatingAxis = Vector3(0, 1, 0); } }; From 63833621a046d58e95a99b73ed35e2beb1ed7658 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Sep 2017 23:25:21 +0200 Subject: [PATCH 052/105] Fix issue with triangle shape normal computation --- src/collision/shapes/TriangleShape.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 48baa225..254bc5a1 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -52,7 +52,7 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const mPoints[2] = point3; // Compute the triangle normal - mNormal = (point3 - point1).cross(point2 - point1); + mNormal = (point2 - point1).cross(point3 - point1); mNormal.normalize(); mVerticesNormals[0] = verticesNormals[0]; From 643c781fa09b050bbe85e71f57b14353bc421470 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 21 Sep 2017 22:44:42 +0200 Subject: [PATCH 053/105] Fix issues with smooth triangle contact --- .../narrowphase/SAT/SATAlgorithm.cpp | 6 ++-- src/collision/shapes/TriangleShape.cpp | 34 +++++++++---------- src/collision/shapes/TriangleShape.h | 2 +- 3 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 2f345738..0d2ae20e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -857,6 +857,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene contactPointsFound = true; + Vector3 outWorldNormal = normalWorld; + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); @@ -868,10 +870,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, - minPenetrationDepth, normalWorld); + minPenetrationDepth, outWorldNormal); // Create a new contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 254bc5a1..263595b3 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -85,14 +85,9 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, isShape1Triangle ? shape1ToWorld : shape2ToWorld, isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), - penetrationDepth, + penetrationDepth, isShape1Triangle, isShape1Triangle ? localContactPointShape2 : localContactPointShape1, outSmoothVertexNormal); - - // Make sure the direction of the contact normal is from shape1 to shape2 - if (!isShape1Triangle) { - outSmoothVertexNormal = -outSmoothVertexNormal; - } } } @@ -104,25 +99,30 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape // stay aligned with the new contact normal. This method will return the new smooth world contact // normal of the triangle and the the local contact point on the other shape. void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, - const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const { // Get the smooth contact normal of the mesh at the contact point on the triangle - Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); + Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); // Convert the local contact normal into world-space - outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal; + Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal; - // Convert the contact normal into the local-space of the other shape - Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal; + // Penetration axis with direction from triangle to other shape + Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal; - // Convert the local contact point of the triangle into the local-space of the other shape - Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform * - localContactPointTriangle; + // The triangle normal should be the one in the direction out of the current colliding face of the triangle + if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) { + triangleWorldNormal = -triangleWorldNormal; + triangleLocalNormal = -triangleLocalNormal; + } - // Re-align the local contact point on the other shape such that it is aligned along - // the new contact normal - Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth; + // Compute the final contact normal from shape 1 to shape 2 + outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal; + + // Re-align the local contact point on the other shape such that it is aligned along the new contact normal + Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth; + Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace; outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); } diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index c31a594c..8360c32b 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -99,7 +99,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// This method implements the technique described in Game Physics Pearl book void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, - const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; public: From b33b8e0dc5e59e378839e135e09b4ca3b1e5ab9f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 25 Sep 2017 23:06:17 +0200 Subject: [PATCH 054/105] Fix issue in SAT algorithm between polyhedron and capsule --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 0d2ae20e..a663791c 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -391,6 +391,10 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); + if (isCapsuleShape1) { + normalWorld = -normalWorld; + } + // For each of the two clipped points for (int i = 0; i<2; i++) { @@ -405,9 +409,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Project the clipped point into the capsule bounds Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; - if (isCapsuleShape1) { - normalWorld = -normalWorld; - } + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, From 310fef1c523c67d34201cef43a9177789d6bac66 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 28 Sep 2017 08:34:45 +0200 Subject: [PATCH 055/105] Fix issue with middle phase collision detection (AABB not computed in correct space) --- src/collision/CollisionDetection.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index b284c034..f27b5ddf 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -218,10 +218,10 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair concaveShape, allocator); // Compute the convex shape AABB in the local-space of the convex shape + const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() * + convexProxyShape->getLocalToWorldTransform(); AABB aabb; - convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - - // TODO : Implement smooth concave mesh collision somewhere + convexShape->computeAABB(aabb, convexToConcaveTransform); // Call the convex vs triangle callback for each triangle of the concave shape concaveShape->testAllTriangles(middlePhaseCallback, aabb); From cbfeb608df332cade904b64a744c8291cff42f5d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 4 Oct 2017 22:38:14 +0200 Subject: [PATCH 056/105] Fix bug and clean the pointers casting in TriangleVertexArray --- src/collision/TriangleVertexArray.cpp | 90 ++++++++++++--------------- src/collision/TriangleVertexArray.h | 49 +++++++-------- src/configuration.h | 2 + 3 files changed, 67 insertions(+), 74 deletions(-) diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index fa87e684..56f3b422 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -44,20 +44,20 @@ using namespace reactphysics3d; * @param verticesStride Number of bytes between the beginning of two consecutive vertices * @param nbTriangles Number of triangles in the array * @param indexesStart Pointer to the first triangle index - * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices + * @param indexesStride Number of bytes between the beginning of the three indices of two triangles * @param vertexDataType Type of data for the vertices (float, double) * @param indexDataType Type of data for the indices (short, int) */ -TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, +TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, VertexDataType vertexDataType, IndexDataType indexDataType) { mNbVertices = nbVertices; - mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStart = static_cast(verticesStart); mVerticesStride = verticesStride; mVerticesNormalsStart = nullptr; - mVerticesNormalsStride = 0; + mVerticesNormalsStride = 3 * sizeof(float); mNbTriangles = nbTriangles; - mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStart = static_cast(indexesStart); mIndicesStride = indexesStride; mVertexDataType = vertexDataType; mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE; @@ -85,19 +85,19 @@ TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, i * @param vertexDataType Type of data for the vertices (float, double) * @param indexDataType Type of data for the indices (short, int) */ -TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - void* verticesNormalsStart, int verticesNormalsStride, - uint nbTriangles, void* indexesStart, int indexesStride, +TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + const void* verticesNormalsStart, uint verticesNormalsStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, VertexDataType vertexDataType, NormalDataType normalDataType, IndexDataType indexDataType) { mNbVertices = nbVertices; - mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStart = static_cast(verticesStart); mVerticesStride = verticesStride; - mVerticesNormalsStart = reinterpret_cast(verticesNormalsStart); + mVerticesNormalsStart = static_cast(verticesNormalsStart); mVerticesNormalsStride = verticesNormalsStride; mNbTriangles = nbTriangles; - mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStart = static_cast(indexesStart); mIndicesStride = indexesStride; mVertexDataType = vertexDataType; mVertexNormaldDataType = normalDataType; @@ -114,7 +114,8 @@ TriangleVertexArray::~TriangleVertexArray() { if (!mAreVerticesNormalsProvidedByUser) { // Release the allocated memory - float* verticesNormals = reinterpret_cast(mVerticesNormalsStart); + const void* verticesNormalPointer = static_cast(mVerticesNormalsStart); + const float* verticesNormals = static_cast(verticesNormalPointer); delete[] verticesNormals; } } @@ -160,12 +161,15 @@ void TriangleVertexArray::computeVerticesNormals() { Vector3 crossProduct = a.cross(b); decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]); - Vector3 normalComponent = std::asin(sinA) * crossProduct; + sinA = std::min(std::max(sinA, decimal(0.0)), decimal(1.0)); + decimal arcSinA = std::asin(sinA); + assert(arcSinA >= decimal(0.0)); + Vector3 normalComponent = arcSinA * crossProduct; // Add the normal component of this vertex into the normals array - verticesNormals[verticesIndices[v] * 3] = normalComponent.x; - verticesNormals[verticesIndices[v] * 3 + 1] = normalComponent.y; - verticesNormals[verticesIndices[v] * 3 + 2] = normalComponent.z; + verticesNormals[verticesIndices[v] * 3] += normalComponent.x; + verticesNormals[verticesIndices[v] * 3 + 1] += normalComponent.y; + verticesNormals[verticesIndices[v] * 3 + 2] += normalComponent.z; } } @@ -181,7 +185,8 @@ void TriangleVertexArray::computeVerticesNormals() { verticesNormals[v + 2] = normal.z; } - mVerticesNormalsStart = reinterpret_cast(verticesNormals); + const void* verticesNormalsPointer = static_cast(verticesNormals); + mVerticesNormalsStart = static_cast(verticesNormalsPointer); } // Return the indices of the three vertices of a given triangle in the array @@ -189,17 +194,18 @@ void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* o assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); - void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * 3 * mIndicesStride; + const void* startTriangleIndices = static_cast(triangleIndicesPointer); // For each vertex of the triangle for (int i=0; i < 3; i++) { // Get the index of the current vertex in the triangle if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - outVerticesIndices[i] = ((uint*)vertexIndexPointer)[i]; + outVerticesIndices[i] = static_cast(startTriangleIndices)[i]; } else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - outVerticesIndices[i] = ((unsigned short*)vertexIndexPointer)[i]; + outVerticesIndices[i] = static_cast(startTriangleIndices)[i]; } else { assert(false); @@ -212,32 +218,25 @@ void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTr assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); - void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + // Get the three vertex index of the three vertices of the triangle + uint verticesIndices[3]; + getTriangleVerticesIndices(triangleIndex, verticesIndices); // For each vertex of the triangle for (int k=0; k < 3; k++) { - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } + const uchar* vertexPointerChar = mVerticesStart + verticesIndices[k] * mVerticesStride; + const void* vertexPointer = static_cast(vertexPointerChar); // Get the vertices components of the triangle if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(mVerticesStart + vertexIndex * mVerticesStride); + const float* vertices = static_cast(vertexPointer); outTriangleVertices[k][0] = decimal(vertices[0]); outTriangleVertices[k][1] = decimal(vertices[1]); outTriangleVertices[k][2] = decimal(vertices[2]); } else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(mVerticesStart + vertexIndex * mVerticesStride); + const double* vertices = static_cast(vertexPointer); outTriangleVertices[k][0] = decimal(vertices[0]); outTriangleVertices[k][1] = decimal(vertices[1]); outTriangleVertices[k][2] = decimal(vertices[2]); @@ -253,32 +252,25 @@ void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3 assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); - void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride); + // Get the three vertex index of the three vertices of the triangle + uint verticesIndices[3]; + getTriangleVerticesIndices(triangleIndex, verticesIndices); // For each vertex of the triangle for (int k=0; k < 3; k++) { - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } + const uchar* vertexNormalPointerChar = mVerticesNormalsStart + verticesIndices[k] * mVerticesNormalsStride; + const void* vertexNormalPointer = static_cast(vertexNormalPointerChar); // Get the normals from the array if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) { - const float* normal = (float*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride); + const float* normal = static_cast(vertexNormalPointer); outTriangleVerticesNormals[k][0] = decimal(normal[0]); outTriangleVerticesNormals[k][1] = decimal(normal[1]); outTriangleVerticesNormals[k][2] = decimal(normal[2]); } else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) { - const double* normal = (double*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride); + const double* normal = static_cast(vertexNormalPointer); outTriangleVerticesNormals[k][0] = decimal(normal[0]); outTriangleVerticesNormals[k][1] = decimal(normal[1]); outTriangleVerticesNormals[k][2] = decimal(normal[2]); diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h index 20ae9387..3db79c7e 100644 --- a/src/collision/TriangleVertexArray.h +++ b/src/collision/TriangleVertexArray.h @@ -64,28 +64,27 @@ class TriangleVertexArray { uint mNbVertices; /// Pointer to the first vertex value in the array - unsigned char* mVerticesStart; + const uchar* mVerticesStart; /// Stride (number of bytes) between the beginning of two vertices /// values in the array - int mVerticesStride; + uint mVerticesStride; /// Pointer to the first vertex normal value in the array - unsigned char* mVerticesNormalsStart; + const uchar* mVerticesNormalsStart; /// Stride (number of bytes) between the beginning of two vertex normals /// values in the array - int mVerticesNormalsStride; + uint mVerticesNormalsStride; /// Number of triangles in the array uint mNbTriangles; /// Pointer to the first vertex index of the array - unsigned char* mIndicesStart; + const uchar* mIndicesStart; - /// Stride (number of bytes) between the beginning of two indices in - /// the array - int mIndicesStride; + /// Stride (number of bytes) between the beginning of the three indices of two triangles + uint mIndicesStride; /// Data type of the vertices in the array VertexDataType mVertexDataType; @@ -109,14 +108,14 @@ class TriangleVertexArray { // -------------------- Methods -------------------- // /// Constructor without vertices normals - TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, + TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, VertexDataType vertexDataType, IndexDataType indexDataType); /// Constructor with vertices normals - TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - void* verticesNormalsStart, int verticesNormalsStride, - uint nbTriangles, void* indexesStart, int indexesStride, + TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + const void* verticesNormalsStart, uint uverticesNormalsStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, VertexDataType vertexDataType, NormalDataType normalDataType, IndexDataType indexDataType); @@ -139,22 +138,22 @@ class TriangleVertexArray { uint getNbTriangles() const; /// Return the vertices stride (number of bytes) - int getVerticesStride() const; + uint getVerticesStride() const; /// Return the vertex normals stride (number of bytes) - int getVerticesNormlasStride() const; + uint getVerticesNormlasStride() const; /// Return the indices stride (number of bytes) - int getIndicesStride() const; + uint getIndicesStride() const; /// Return the pointer to the start of the vertices array - unsigned char* getVerticesStart() const; + const void* getVerticesStart() const; /// Return the pointer to the start of the vertex normals array - unsigned char* getVerticesNormalsStart() const; + const void* getVerticesNormalsStart() const; /// Return the pointer to the start of the indices array - unsigned char* getIndicesStart() const; + const void* getIndicesStart() const; /// Return the vertices coordinates of a triangle void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const; @@ -192,32 +191,32 @@ inline uint TriangleVertexArray::getNbTriangles() const { } // Return the vertices stride (number of bytes) -inline int TriangleVertexArray::getVerticesStride() const { +inline uint TriangleVertexArray::getVerticesStride() const { return mVerticesStride; } // Return the vertex normals stride (number of bytes) -inline int TriangleVertexArray::getVerticesNormlasStride() const { +inline uint TriangleVertexArray::getVerticesNormlasStride() const { return mVerticesNormalsStride; } // Return the indices stride (number of bytes) -inline int TriangleVertexArray::getIndicesStride() const { +inline uint TriangleVertexArray::getIndicesStride() const { return mIndicesStride; } // Return the pointer to the start of the vertices array -inline unsigned char* TriangleVertexArray::getVerticesStart() const { +inline const void* TriangleVertexArray::getVerticesStart() const { return mVerticesStart; } // Return the pointer to the start of the vertex normals array -inline unsigned char* TriangleVertexArray::getVerticesNormalsStart() const { +inline const void* TriangleVertexArray::getVerticesNormalsStart() const { return mVerticesNormalsStart; } // Return the pointer to the start of the indices array -inline unsigned char* TriangleVertexArray::getIndicesStart() const { +inline const void* TriangleVertexArray::getIndicesStart() const { return mIndicesStart; } diff --git a/src/configuration.h b/src/configuration.h index b2c53021..444bd1db 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -48,6 +48,8 @@ namespace reactphysics3d { // ------------------- Type definitions ------------------- // using uint = unsigned int; +using uchar = unsigned char; +using ushort = unsigned short; using luint = long unsigned int; using bodyindex = luint; using bodyindexpair = std::pair; From d62aa419743a68083eb61fa78dcf140b72137d3a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 4 Oct 2017 22:38:39 +0200 Subject: [PATCH 057/105] Fix issue in ContactManifoldSet --- src/collision/ContactManifoldSet.cpp | 23 +++++++---------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 5ad8ce71..2fcea3f3 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -253,14 +253,14 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) { ContactManifold* next = manifold->getNext(); if (previous != nullptr) { - previous->setNext(manifold->getNext()); + previous->setNext(next); } else { mManifolds = next; } if (next != nullptr) { - next->setPrevious(manifold->getPrevious()); + next->setPrevious(previous); } // Delete the contact manifold @@ -286,30 +286,21 @@ void ContactManifoldSet::makeContactsObsolete() { void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() { ContactManifold* manifold = mManifolds; - ContactManifold* previousManifold = nullptr; while (manifold != nullptr) { + + // Get the next manifold in the linked-list ContactManifold* nextManifold = manifold->getNext(); + // If the manifold is obsolete if (manifold->getIsObsolete()) { - if (previousManifold != nullptr) { - previousManifold->setNext(nextManifold); - - if (nextManifold != nullptr) { - nextManifold->setPrevious(previousManifold); - } - } - else { - mManifolds = nextManifold; - } - // Delete the contact manifold removeManifold(manifold); - } else { + + // Clear the obsolete contact points of the manifold manifold->clearObseleteContactPoints(); - previousManifold = manifold; } manifold = nextManifold; From 38eff07d0d6b588dcbe529ba0e6d95ec0725c61d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 9 Oct 2017 22:36:39 +0200 Subject: [PATCH 058/105] Fix issue and small changes in TriangleVertexArray --- src/collision/TriangleVertexArray.cpp | 2 +- src/collision/TriangleVertexArray.h | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index 56f3b422..7cfb2076 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -194,7 +194,7 @@ void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* o assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); - const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * 3 * mIndicesStride; + const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * mIndicesStride; const void* startTriangleIndices = static_cast(triangleIndicesPointer); // For each vertex of the triangle diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h index 3db79c7e..3a48c2e3 100644 --- a/src/collision/TriangleVertexArray.h +++ b/src/collision/TriangleVertexArray.h @@ -122,6 +122,12 @@ class TriangleVertexArray { /// Destructor ~TriangleVertexArray(); + /// Deleted assignment operator + TriangleVertexArray& operator=(const TriangleVertexArray& triangleVertexArray) = delete; + + /// Deleted copy-constructor + TriangleVertexArray(const TriangleVertexArray& triangleVertexArray) = delete; + /// Return the vertex data type VertexDataType getVertexDataType() const; @@ -141,7 +147,7 @@ class TriangleVertexArray { uint getVerticesStride() const; /// Return the vertex normals stride (number of bytes) - uint getVerticesNormlasStride() const; + uint getVerticesNormalsStride() const; /// Return the indices stride (number of bytes) uint getIndicesStride() const; @@ -196,7 +202,7 @@ inline uint TriangleVertexArray::getVerticesStride() const { } // Return the vertex normals stride (number of bytes) -inline uint TriangleVertexArray::getVerticesNormlasStride() const { +inline uint TriangleVertexArray::getVerticesNormalsStride() const { return mVerticesNormalsStride; } From 37e2c79cf22b1a32c53e2bad390460b9e3d9dea3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 9 Oct 2017 22:41:45 +0200 Subject: [PATCH 059/105] Fix issue in ContactManifoldSet --- src/collision/ContactManifoldSet.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 2fcea3f3..4e73aa9f 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -239,6 +239,9 @@ void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator); manifold->setPrevious(nullptr); manifold->setNext(mManifolds); + if (mManifolds != nullptr) { + mManifolds->setPrevious(manifold); + } mManifolds = manifold; mNbManifolds++; @@ -248,6 +251,7 @@ void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) void ContactManifoldSet::removeManifold(ContactManifold* manifold) { assert(mNbManifolds > 0); + assert(manifold != nullptr); ContactManifold* previous = manifold->getPrevious(); ContactManifold* next = manifold->getNext(); From 4a37ba39948ae053445f20a058815f609543c30a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 9 Oct 2017 23:34:35 +0200 Subject: [PATCH 060/105] Add unit tests for the TriangleVertexArray --- src/mathematics/mathematics_functions.cpp | 39 ++- src/mathematics/mathematics_functions.h | 7 + test/main.cpp | 2 + test/tests/collision/TestRaycast.h | 5 +- .../tests/collision/TestTriangleVertexArray.h | 261 ++++++++++++++++++ 5 files changed, 298 insertions(+), 16 deletions(-) create mode 100644 test/tests/collision/TestTriangleVertexArray.h diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index d627ed7b..fd813cdc 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -26,14 +26,27 @@ // Libraries #include "mathematics_functions.h" #include "Vector3.h" +#include "Vector2.h" #include #include using namespace reactphysics3d; -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -/// This method uses the technique described in the book Real-Time collision detection by -/// Christer Ericson. + +// Function to test if two vectors are (almost) equal +bool reactphysics3d::approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && + approxEqual(vec1.z, vec2.z, epsilon); +} + +// Function to test if two vectors are (almost) equal +bool reactphysics3d::approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); +} + +// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +// This method uses the technique described in the book Real-Time collision detection by +// Christer Ericson. void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p, decimal& u, decimal& v, decimal& w) { const Vector3 v0 = b - a; @@ -52,7 +65,7 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c u = decimal(1.0) - v - w; } -/// Clamp a vector such that it is no longer than a given maximum length +// Clamp a vector such that it is no longer than a given maximum length Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { if (vector.lengthSquare() > maxLength * maxLength) { return vector.getUnit() * maxLength; @@ -60,17 +73,17 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { return vector; } -/// Return true if two vectors are parallel +// Return true if two vectors are parallel bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { return vector1.cross(vector2).lengthSquare() < decimal(0.00001); } -/// Return true if two vectors are orthogonal +// Return true if two vectors are orthogonal bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { return std::abs(vector1.dot(vector2)) < decimal(0.00001); } -/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { const Vector3 ab = segPointB - segPointA; @@ -95,9 +108,9 @@ Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, c return segPointA + t * ab; } -/// Compute the closest points between two segments -/// This method uses the technique described in the book Real-Time -/// collision detection by Christer Ericson. +// Compute the closest points between two segments +// This method uses the technique described in the book Real-Time +// collision detection by Christer Ericson. void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, const Vector3& seg2PointA, const Vector3& seg2PointB, Vector3& closestPointSeg1, Vector3& closestPointSeg2) { @@ -175,7 +188,7 @@ void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1Po closestPointSeg2 = seg2PointA + d2 * t; } -/// Compute the intersection between a plane and a segment +// Compute the intersection between a plane and a segment // Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method // computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such // that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, @@ -282,8 +295,8 @@ std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, return outputVertices; } -/// Clip a polygon against multiple planes and return the clipped polygon vertices -/// This method implements the Sutherland–Hodgman clipping algorithm +// Clip a polygon against multiple planes and return the clipped polygon vertices +// This method implements the Sutherland–Hodgman clipping algorithm std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, const std::vector& planesNormals) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 78b5ba67..9f062826 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -38,6 +38,7 @@ namespace reactphysics3d { struct Vector3; +struct Vector2; // ---------- Mathematics functions ---------- // @@ -47,6 +48,12 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) return (std::fabs(a - b) < epsilon); } +/// Function to test if two vectors are (almost) equal +bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); + +/// Function to test if two vectors are (almost) equal +bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); + /// Function that returns the result of the "value" clamped by /// two others values "lowerLimit" and "upperLimit" inline int clamp(int value, int lowerLimit, int upperLimit) { diff --git a/test/main.cpp b/test/main.cpp index 54bfbb8f..26d62803 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -38,6 +38,7 @@ #include "tests/collision/TestAABB.h" #include "tests/collision/TestDynamicAABBTree.h" #include "tests/collision/TestHalfEdgeStructure.h" +#include "tests/collision/TestTriangleVertexArray.h" using namespace reactphysics3d; @@ -59,6 +60,7 @@ int main() { testSuite.addTest(new TestAABB("AABB")); testSuite.addTest(new TestPointInside("IsPointInside")); + testSuite.addTest(new TestTriangleVertexArray("TriangleVertexArray")); testSuite.addTest(new TestRaycast("Raycasting")); testSuite.addTest(new TestCollisionWorld("CollisionWorld")); testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 81013570..f2ae3d6b 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -277,9 +277,8 @@ class TestRaycast : public Test { TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE; mConcaveMeshVertexArray = new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), - 12, &(mConcaveMeshIndices[0]), sizeof(uint), - vertexType, - TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + 12, &(mConcaveMeshIndices[0]), 3 * sizeof(uint), + vertexType, TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); // Add the triangle vertex array of the subpart to the triangle mesh diff --git a/test/tests/collision/TestTriangleVertexArray.h b/test/tests/collision/TestTriangleVertexArray.h new file mode 100644 index 00000000..282b7d55 --- /dev/null +++ b/test/tests/collision/TestTriangleVertexArray.h @@ -0,0 +1,261 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_TRIANGLE_VERTEX_ARRAY_H +#define TEST_TRIANGLE_VERTEX_ARRAY_H + +// Libraries +#include "reactphysics3d.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + + +// Class TestTriangleVertexArray +/** + * Unit test for the TestTriangleArray class. + */ +class TestTriangleVertexArray : public Test { + + private : + + // ---------- Atributes ---------- // + + float mVertices1[4*3]; + double mVertices2[4*3]; + float mNormals2[4*3]; + uint mIndices1[6]; + short mIndices2[6]; + TriangleVertexArray* mTriangleVertexArray1; + TriangleVertexArray* mTriangleVertexArray2; + + Vector3 mVertex0; + Vector3 mVertex1; + Vector3 mVertex2; + Vector3 mVertex3; + Vector3 mVertex4; + Vector3 mVertex5; + Vector3 mVertex6; + Vector3 mVertex7; + + Vector3 mNormal0; + Vector3 mNormal1; + Vector3 mNormal2; + Vector3 mNormal3; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestTriangleVertexArray(const std::string& name) : Test(name) { + + mVertex0 = Vector3(0, 0, 4); + mVertex1 = Vector3(0, 0, -3); + mVertex2 = Vector3(-2, 0, 0); + mVertex3 = Vector3(0, -5, 0); + + // Initialize data + mVertices1[0] = mVertex0.x; mVertices1[1] = mVertex0.y; mVertices1[2] = mVertex0.z; + mVertices1[3] = mVertex1.x; mVertices1[4] = mVertex1.y; mVertices1[5] = mVertex1.z; + mVertices1[6] = mVertex2.x; mVertices1[7] = mVertex2.y; mVertices1[8] = mVertex2.z; + mVertices1[9] = mVertex3.x; mVertices1[10] = mVertex3.y; mVertices1[11] = mVertex3.z; + + mIndices1[0] = 0; mIndices1[1] = 1; mIndices1[2] = 2; + mIndices1[3] = 0; mIndices1[4] = 3; mIndices1[5] = 1; + + mVertex4 = Vector3(0, 0, 5); + mVertex5 = Vector3(0, 0, -7); + mVertex6 = Vector3(-2, 0, 0); + mVertex7 = Vector3(0, -5, 0); + + mVertices2[0] = static_cast(mVertex4.x); mVertices2[1] = static_cast(mVertex4.y); mVertices2[2] = static_cast(mVertex4.z); + mVertices2[3] = static_cast(mVertex5.x); mVertices2[4] = static_cast(mVertex5.y); mVertices2[5] = static_cast(mVertex5.z); + mVertices2[6] = static_cast(mVertex6.x); mVertices2[7] = static_cast(mVertex6.y); mVertices2[8] = static_cast(mVertex6.z); + mVertices2[9] = static_cast(mVertex7.x); mVertices2[10] = static_cast(mVertex7.y); mVertices2[11] = static_cast(mVertex7.z); + + mIndices2[0] = 0; mIndices2[1] = 1; mIndices2[2] = 2; + mIndices2[3] = 0; mIndices2[4] = 3; mIndices2[5] = 1; + + mNormal0 = Vector3(2, 4, 6); + mNormal1 = Vector3(1, 6, -3); + mNormal2 = Vector3(-2, 4, 7); + mNormal3 = Vector3(-5, 2, 9); + mNormal0.normalize(); + mNormal1.normalize(); + mNormal2.normalize(); + mNormal3.normalize(); + mNormals2[0] = mNormal0.x; mNormals2[1] = mNormal0.y; mNormals2[2] = mNormal0.z; + mNormals2[3] = mNormal1.x; mNormals2[4] = mNormal1.y; mNormals2[5] = mNormal1.z; + mNormals2[6] = mNormal2.x; mNormals2[7] = mNormal2.y; mNormals2[8] = mNormal2.z; + mNormals2[9] = mNormal3.x; mNormals2[10] = mNormal3.y; mNormals2[11] = mNormal3.z; + + // Create triangle vertex array with automatic normals computation + mTriangleVertexArray1 = new TriangleVertexArray(4, static_cast(mVertices1), 3 * sizeof(float), + 2, static_cast(mIndices1), 3 * sizeof(uint), + TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create triangle vertex array with normals defined by the user + mTriangleVertexArray2 = new TriangleVertexArray(4, static_cast(mVertices2), 3 * sizeof(double), + static_cast(mNormals2), 3 * sizeof(float), + 2, static_cast(mIndices2), 3 * sizeof(short), + TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE, + TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE, + TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE); + + } + + /// Destructor + virtual ~TestTriangleVertexArray() { + delete mTriangleVertexArray1; + delete mTriangleVertexArray2; + } + + /// Run the tests + void run() { + + // ----- First triangle vertex array ----- // + + test(mTriangleVertexArray1->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE); + test(mTriangleVertexArray1->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + test(mTriangleVertexArray1->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE); + test(mTriangleVertexArray1->getNbTriangles() == 2); + test(mTriangleVertexArray1->getNbVertices() == 4); + test(mTriangleVertexArray1->getIndicesStart() == static_cast(mIndices1)); + test(mTriangleVertexArray1->getVerticesStart() == static_cast(mVertices1)); + test(mTriangleVertexArray1->getIndicesStride() == (3 * sizeof(uint))); + test(mTriangleVertexArray1->getVerticesStride() == (3 * sizeof(float))); + + // Get triangle indices + + uint triangle0Indices[3]; + mTriangleVertexArray1->getTriangleVerticesIndices(0, triangle0Indices); + + test(triangle0Indices[0] == mIndices1[0]); + test(triangle0Indices[1] == mIndices1[1]); + test(triangle0Indices[2] == mIndices1[2]); + + uint triangle1Indices[3]; + mTriangleVertexArray1->getTriangleVerticesIndices(1, triangle1Indices); + + test(triangle1Indices[0] == mIndices1[3]); + test(triangle1Indices[1] == mIndices1[4]); + test(triangle1Indices[2] == mIndices1[5]); + + // Get triangle vertices + + Vector3 triangle0Vertices[3]; + mTriangleVertexArray1->getTriangleVertices(0, triangle0Vertices); + + test(approxEqual(triangle0Vertices[0], mVertex0, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[1], mVertex1, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[2], mVertex2, decimal(0.0000001))); + + Vector3 triangle1Vertices[3]; + mTriangleVertexArray1->getTriangleVertices(1, triangle1Vertices); + + test(approxEqual(triangle1Vertices[0], mVertex0, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[1], mVertex3, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[2], mVertex1, decimal(0.0000001))); + + // Get triangle normals + + Vector3 triangle0Normals[3]; + mTriangleVertexArray1->getTriangleVerticesNormals(0, triangle0Normals); + + Vector3 triangle1Normals[3]; + mTriangleVertexArray1->getTriangleVerticesNormals(1, triangle1Normals); + + const Vector3 normal0Test(decimal(0.9792), decimal(0.20268), 0); + const Vector3 normal2Test(0, 1, 0); + const Vector3 normal3Test(1, 0, 0); + + test(approxEqual(triangle0Normals[0], normal0Test, decimal(0.0001))); + test(approxEqual(triangle0Normals[2], normal2Test, decimal(0.0001))); + test(approxEqual(triangle1Normals[1], normal3Test, decimal(0.0001))); + + // ----- Second triangle vertex array ----- // + + test(mTriangleVertexArray2->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE); + test(mTriangleVertexArray2->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE); + test(mTriangleVertexArray2->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE); + test(mTriangleVertexArray2->getNbTriangles() == 2); + test(mTriangleVertexArray2->getNbVertices() == 4); + test(mTriangleVertexArray2->getIndicesStart() == static_cast(mIndices2)); + test(mTriangleVertexArray2->getVerticesStart() == static_cast(mVertices2)); + test(mTriangleVertexArray2->getVerticesNormalsStart() == static_cast(mNormals2)); + test(mTriangleVertexArray2->getIndicesStride() == (3 * sizeof(short))); + test(mTriangleVertexArray2->getVerticesStride() == (3 * sizeof(double))); + test(mTriangleVertexArray2->getVerticesNormalsStride() == (3 * sizeof(float))); + + // Get triangle indices + + mTriangleVertexArray2->getTriangleVerticesIndices(0, triangle0Indices); + + test(triangle0Indices[0] == mIndices2[0]); + test(triangle0Indices[1] == mIndices2[1]); + test(triangle0Indices[2] == mIndices2[2]); + + mTriangleVertexArray2->getTriangleVerticesIndices(1, triangle1Indices); + + test(triangle1Indices[0] == mIndices2[3]); + test(triangle1Indices[1] == mIndices2[4]); + test(triangle1Indices[2] == mIndices2[5]); + + // Get triangle vertices + + mTriangleVertexArray2->getTriangleVertices(0, triangle0Vertices); + + test(approxEqual(triangle0Vertices[0], mVertex4, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[1], mVertex5, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[2], mVertex6, decimal(0.0000001))); + + mTriangleVertexArray2->getTriangleVertices(1, triangle1Vertices); + + test(approxEqual(triangle1Vertices[0], mVertex4, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[1], mVertex7, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[2], mVertex5, decimal(0.0000001))); + + // Get triangle normals + + mTriangleVertexArray2->getTriangleVerticesNormals(0, triangle0Normals); + mTriangleVertexArray2->getTriangleVerticesNormals(1, triangle1Normals); + + test(approxEqual(triangle0Normals[0], mNormal0, decimal(0.000001))); + test(approxEqual(triangle0Normals[1], mNormal1, decimal(0.000001))); + test(approxEqual(triangle0Normals[2], mNormal2, decimal(0.000001))); + + test(approxEqual(triangle1Normals[0], mNormal0, decimal(0.000001))); + test(approxEqual(triangle1Normals[1], mNormal3, decimal(0.000001))); + test(approxEqual(triangle1Normals[2], mNormal1, decimal(0.000001))); + } + +}; + +} + +#endif + From 17d1ee36813334749bc044cd44326e1c2d1618d4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 9 Oct 2017 23:35:30 +0200 Subject: [PATCH 061/105] Change in the ConcaveMesh class of the testbed application --- testbed/common/ConcaveMesh.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index d6a6bbe3..c7e162a3 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -46,7 +46,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, // Vertex and Indices array for the triangle mesh (data in shared and not copied) rp3d::TriangleVertexArray* vertexArray = new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(i), &(mIndices[i][0]), sizeof(int), + getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int), rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); @@ -97,7 +97,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, // Vertex and Indices array for the triangle mesh (data in shared and not copied) rp3d::TriangleVertexArray* vertexArray = new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(i), &(mIndices[i][0]), sizeof(int), + getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int), rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); From d5617526ff91baa77ca2b759994f981713a5e4a1 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 12 Oct 2017 20:07:39 +0200 Subject: [PATCH 062/105] Modify the policy to drop contact manifolds. First drop the old ones before the new ones --- src/collision/CollisionDetection.cpp | 5 ++- src/collision/ContactManifold.cpp | 3 +- src/collision/ContactManifold.h | 21 ++++++++++++ src/collision/ContactManifoldSet.cpp | 51 ++++++++++++++++++++++------ src/collision/ContactManifoldSet.h | 18 ++++++++-- src/engine/OverlappingPair.h | 9 +++++ 6 files changed, 92 insertions(+), 15 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f27b5ddf..6e0e6ed6 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -71,7 +71,7 @@ void CollisionDetection::computeCollisionDetection() { computeMiddlePhase(); // Compute the narrow-phase collision detection - computeNarrowPhase(); + computeNarrowPhase(); // Reset the linked list of narrow-phase info mNarrowPhaseInfoList = nullptr; @@ -410,6 +410,9 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { potentialManifold = potentialManifold->mNext; } + // Reset the isNew status of the manifolds + pair->resetIsNewManifoldStatus(); + // Clear the obsolete contact manifolds and contact points pair->clearObsoleteManifoldsAndContactPoints(); diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 62ca7a0c..37ad3ad8 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -34,7 +34,7 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) { // For each contact point info in the manifold const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); @@ -85,6 +85,7 @@ void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) mContactPoints = contactPoint; mNbContactPoints++; + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } // Clear the obsolete contact points diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index e34c4543..b24ed378 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -143,6 +143,9 @@ class ContactManifold { /// True if the contact manifold is obsolete bool mIsObsolete; + /// True if the contact manifold is new (has just been added from potential contacts) + bool mIsNew; + // -------------------- Methods -------------------- // /// Return true if the contact manifold has already been added into an island @@ -204,6 +207,12 @@ class ContactManifold { /// Return the friction twist accumulated impulse decimal getFrictionTwistImpulse() const; + + /// Return true if the manifold has just been created + bool getIsNew() const; + + /// Set the isNew attribute + void setIsNew(bool isNew); public: @@ -408,6 +417,18 @@ inline short ContactManifold::getContactNormalId() const { return mContactNormalId; } + +// Return true if the manifold has just been created +inline bool ContactManifold::getIsNew() const { + return mIsNew; +} + +// Set the isNew attribute +inline void ContactManifold::setIsNew(bool isNew) { + mIsNew = isNew; +} + + } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 4e73aa9f..06ae4963 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -66,8 +66,9 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa if (mNbManifolds >= mNbMaxManifolds) { // We need to remove a manifold from the set. - // We seach for the one with the smallest maximum penetration depth among its contact points - ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth()); + // We search for an old manifold with the smallest maximum penetration depth among its contact points. + // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth. + ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth()); // If the manifold with the minimum penetration depth is an existing one if (minDepthManifold != nullptr) { @@ -135,25 +136,53 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, oldManifold->setIsObsolete(false, false); } -// Return the manifold with the smallest contact penetration depth among its points -ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const { +// Return the manifold to remove (because it is too old or has not the largest penetration depth) +ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const { assert(mNbManifolds == mNbMaxManifolds); + assert(mManifolds != nullptr); - ContactManifold* minDepthManifold = nullptr; - decimal minDepth = initDepth; + // Look for a manifold that is not new and with the smallest contact penetration depth. + // At the same time, we also look for a new manifold with the smallest contact penetration depth + // in case no old manifold exists. + ContactManifold* minDepthOldManifold = nullptr; + ContactManifold* minDepthNewManifold = nullptr; + decimal minDepthOld = DECIMAL_LARGEST; + decimal minDepthNew = penDepthNewManifold; ContactManifold* manifold = mManifolds; while (manifold != nullptr) { - decimal depth = manifold->getLargestContactDepth(); - if (depth < minDepth) { - minDepth = depth; - minDepthManifold = manifold; + + // Get the largest contact point penetration depth of the manifold + const decimal depth = manifold->getLargestContactDepth(); + + // If it is a new manifold + if (manifold->getIsNew()) { + + if (depth < minDepthNew) { + minDepthNew = depth; + minDepthNewManifold = manifold; + } + } + else { + + if (depth < minDepthOld) { + minDepthOld = depth; + minDepthOldManifold = manifold; + } } manifold = manifold->getNext(); } - return minDepthManifold; + // If there was a contact manifold that was not new + if (minDepthOldManifold != nullptr) { + + // Return the old manifold with the smallest penetration depth + return minDepthOldManifold; + } + + // Otherwise, we return the new manifold with the smallest penetration depth + return minDepthNewManifold; } // Return the contact manifold with a similar average normal. diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 1676f5a2..ea832cdb 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -74,8 +74,8 @@ class ContactManifoldSet { // Return the contact manifold with a similar average normal. ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const; - /// Return the manifold with the smallest contact penetration depth - ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const; + /// Return the manifold to remove (because it is too old or has not the largest penetration depth) + ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const; /// Update a previous similar manifold with a new one void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); @@ -124,6 +124,9 @@ class ContactManifoldSet { /// Clear the obsolete contact manifolds and contact points void clearObsoleteManifoldsAndContactPoints(); + /// Reset the isNew status of all the manifolds + void resetIsNewManifoldStatus(); + // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the // normal vector into of the of the bucket and returns a unique Id for the bucket @@ -177,6 +180,17 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape } } + +// Reset the isNew status of all the manifolds +inline void ContactManifoldSet::resetIsNewManifoldStatus() { + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + manifold->setIsNew(false); + manifold = manifold->getNext(); + } +} + } #endif diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index bb7c1048..2725a8d5 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -172,6 +172,9 @@ class OverlappingPair { /// Clear the obsolete contact manifold and contact points void clearObsoleteManifoldsAndContactPoints(); + /// Reset the isNew status of all the manifolds + void resetIsNewManifoldStatus(); + /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -269,6 +272,12 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() { mContactManifoldSet.clearObsoleteManifoldsAndContactPoints(); } + +// Reset the isNew status of all the manifolds +inline void OverlappingPair::resetIsNewManifoldStatus() { + mContactManifoldSet.resetIsNewManifoldStatus(); +} + } #endif From de494bb0fd74bf5f09bde09f7eac8ea64ce4ea35 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 18 Oct 2017 00:41:32 +0200 Subject: [PATCH 063/105] Changes and bug fixes in ContactManifold and ContactManifoldSet --- src/collision/CollisionDetection.cpp | 8 +- src/collision/ContactManifold.cpp | 126 ++++++++++++++++++++------- src/collision/ContactManifold.h | 32 +++---- src/collision/ContactManifoldSet.cpp | 98 ++++++++------------- src/collision/ContactManifoldSet.h | 19 +--- src/collision/ContactPointInfo.h | 2 + src/constraint/ContactPoint.cpp | 16 +--- src/constraint/ContactPoint.h | 49 +++++------ src/engine/ContactSolver.cpp | 9 +- src/engine/OverlappingPair.h | 23 ++--- 10 files changed, 189 insertions(+), 193 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 6e0e6ed6..f650351a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() { OverlappingPair* pair = it->second; // Make all the contact manifolds and contact points of the pair obsolete - pair->makeContactsObselete(); + pair->makeContactsObsolete(); ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); @@ -410,12 +410,12 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { potentialManifold = potentialManifold->mNext; } - // Reset the isNew status of the manifolds - pair->resetIsNewManifoldStatus(); - // Clear the obsolete contact manifolds and contact points pair->clearObsoleteManifoldsAndContactPoints(); + // Reduce the contact manifolds and contact points if there are too many of them + pair->reduceContactManifolds(); + // Reset the potential contacts of the pair pair->clearPotentialContactManifolds(); } diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 37ad3ad8..55ce8ba3 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -34,20 +34,14 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) { // For each contact point info in the manifold const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); while(pointInfo != nullptr) { - // Create the new contact point - ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); - - // Add the new contact point into the manifold - contact->setNext(mContactPoints); - mContactPoints = contact; - mNbContactPoints++; + // Add the new contact point + addContactPoint(pointInfo); pointInfo = pointInfo->next; } @@ -73,52 +67,122 @@ ContactManifold::~ContactManifold() { } } +// Remove a contact point +void ContactManifold::removeContactPoint(ContactPoint* contactPoint) { + + assert(mNbContactPoints > 0); + assert(mContactPoints != nullptr); + assert(contactPoint != nullptr); + + ContactPoint* previous = contactPoint->getPrevious(); + ContactPoint* next = contactPoint->getNext(); + + if (previous != nullptr) { + previous->setNext(next); + } + else { + mContactPoints = next; + } + + if (next != nullptr) { + next->setPrevious(previous); + } + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + mNbContactPoints--; + assert(mNbContactPoints >= 0); +} + // Add a contact point void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) { + assert(contactPointInfo != nullptr); + // Create the new contact point - ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo); // Add the new contact point into the manifold contactPoint->setNext(mContactPoints); + contactPoint->setPrevious(nullptr); + if (mContactPoints != nullptr) { + mContactPoints->setPrevious(contactPoint); + } mContactPoints = contactPoint; mNbContactPoints++; - assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); } // Clear the obsolete contact points -void ContactManifold::clearObseleteContactPoints() { +void ContactManifold::clearObsoleteContactPoints() { + assert(mContactPoints != nullptr); + + // For each contact point of the manifold ContactPoint* contactPoint = mContactPoints; - ContactPoint* previousContactPoint = nullptr; while (contactPoint != nullptr) { ContactPoint* nextContactPoint = contactPoint->getNext(); + // If the contact point is obsolete if (contactPoint->getIsObsolete()) { - // Delete the contact point - contactPoint->~ContactPoint(); - mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); - - if (previousContactPoint != nullptr) { - previousContactPoint->setNext(nextContactPoint); - } - else { - mContactPoints = nextContactPoint; - } - - mNbContactPoints--; - } - else { - previousContactPoint = contactPoint; + // Remove the contact point + removeContactPoint(contactPoint); } contactPoint = nextContactPoint; } - assert(mNbContactPoints >= 0); - assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(mNbContactPoints > 0); + assert(mContactPoints != nullptr); +} + +// Make sure we do not have too much contact points by keeping only the best +// contact points of the manifold (with largest penetration depth) +void ContactManifold::reduce() { + + assert(mContactPoints != nullptr); + + // Remove contact points while there is too much contact points + while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { + removeNonOptimalContactPoint(); + } + + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0); + assert(mContactPoints != nullptr); +} + +// Remove a contact point that is not optimal (with a small penetration depth) +void ContactManifold::removeNonOptimalContactPoint() { + + assert(mContactPoints != nullptr); + assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); + + // Get the contact point with the minimum penetration depth among all points + ContactPoint* contactPoint = mContactPoints; + ContactPoint* minContactPoint = nullptr; + decimal minPenetrationDepth = DECIMAL_LARGEST; + while (contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + if (contactPoint->getPenetrationDepth() < minPenetrationDepth) { + + minContactPoint = contactPoint; + minPenetrationDepth = contactPoint->getPenetrationDepth(); + } + + contactPoint = nextContactPoint; + } + + assert(minContactPoint != nullptr); + + // Remove the non optimal contact point + removeContactPoint(minContactPoint); + + assert(mNbContactPoints > 0); + assert(mContactPoints != nullptr); } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index b24ed378..b5c25cb9 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -143,9 +143,6 @@ class ContactManifold { /// True if the contact manifold is obsolete bool mIsObsolete; - /// True if the contact manifold is new (has just been added from potential contacts) - bool mIsNew; - // -------------------- Methods -------------------- // /// Return true if the contact manifold has already been added into an island @@ -161,7 +158,7 @@ class ContactManifold { void setIsObsolete(bool isObselete, bool setContactPoints); /// Clear the obsolete contact points - void clearObseleteContactPoints(); + void clearObsoleteContactPoints(); /// Return the contact normal direction Id of the manifold short getContactNormalId() const; @@ -184,6 +181,15 @@ class ContactManifold { /// Add a contact point void addContactPoint(const ContactPointInfo* contactPointInfo); + /// Make sure we do not have too much contact points by keeping only the best ones + void reduce(); + + /// Remove a contact point that is not optimal (with a small penetration depth) + void removeNonOptimalContactPoint(); + + /// Remove a contact point + void removeContactPoint(ContactPoint* contactPoint); + /// Set the friction twist accumulated impulse void setFrictionTwistImpulse(decimal frictionTwistImpulse); @@ -208,12 +214,6 @@ class ContactManifold { /// Return the friction twist accumulated impulse decimal getFrictionTwistImpulse() const; - /// Return true if the manifold has just been created - bool getIsNew() const; - - /// Set the isNew attribute - void setIsNew(bool isNew); - public: // -------------------- Methods -------------------- // @@ -417,18 +417,6 @@ inline short ContactManifold::getContactNormalId() const { return mContactNormalId; } - -// Return true if the manifold has just been created -inline bool ContactManifold::getIsNew() const { - return mIsNew; -} - -// Set the isNew attribute -inline void ContactManifold::setIsNew(bool isNew) { - mIsNew = isNew; -} - - } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 06ae4963..9b8bf58b 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -60,43 +60,16 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa } else { - bool addNewManifold = true; - - // If there are too much contact manifolds in the set - if (mNbManifolds >= mNbMaxManifolds) { - - // We need to remove a manifold from the set. - // We search for an old manifold with the smallest maximum penetration depth among its contact points. - // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth. - ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth()); - - // If the manifold with the minimum penetration depth is an existing one - if (minDepthManifold != nullptr) { - - // Remove the manifold - removeManifold(minDepthManifold); - } - else { - - // The manifold we do not want to get is the new one. Therefore, we do not add it to the set - addNewManifold = false; - } - } - - // If we need to add the new contact manifold - if (addNewManifold) { - - // Create a new contact manifold - createManifold(contactManifoldInfo); - } + // Create a new contact manifold + createManifold(contactManifoldInfo); } } // Update a previous similar manifold with a new one void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) { - assert(oldManifold != nullptr); - assert(newManifold != nullptr); + assert(oldManifold != nullptr); + assert(newManifold != nullptr); // For each contact point of the new manifold ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo(); @@ -114,7 +87,7 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { // Replace (update) the old contact point with the new one - oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform()); + oldContactPoint->update(contactPointInfo); isSimilarPointFound = true; break; } @@ -136,53 +109,34 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, oldManifold->setIsObsolete(false, false); } -// Return the manifold to remove (because it is too old or has not the largest penetration depth) -ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const { +// Remove a contact manifold that is the least optimal (smaller penetration depth) +void ContactManifoldSet::removeNonOptimalManifold() { - assert(mNbManifolds == mNbMaxManifolds); + assert(mNbManifolds > mNbMaxManifolds); assert(mManifolds != nullptr); // Look for a manifold that is not new and with the smallest contact penetration depth. // At the same time, we also look for a new manifold with the smallest contact penetration depth // in case no old manifold exists. - ContactManifold* minDepthOldManifold = nullptr; - ContactManifold* minDepthNewManifold = nullptr; - decimal minDepthOld = DECIMAL_LARGEST; - decimal minDepthNew = penDepthNewManifold; + ContactManifold* minDepthManifold = nullptr; + decimal minDepth = DECIMAL_LARGEST; ContactManifold* manifold = mManifolds; while (manifold != nullptr) { // Get the largest contact point penetration depth of the manifold const decimal depth = manifold->getLargestContactDepth(); - // If it is a new manifold - if (manifold->getIsNew()) { - - if (depth < minDepthNew) { - minDepthNew = depth; - minDepthNewManifold = manifold; - } - } - else { - - if (depth < minDepthOld) { - minDepthOld = depth; - minDepthOldManifold = manifold; - } + if (depth < minDepth) { + minDepth = depth; + minDepthManifold = manifold; } manifold = manifold->getNext(); } - // If there was a contact manifold that was not new - if (minDepthOldManifold != nullptr) { - - // Return the old manifold with the smallest penetration depth - return minDepthOldManifold; - } - - // Otherwise, we return the new manifold with the smallest penetration depth - return minDepthNewManifold; + // Remove the non optimal manifold + assert(minDepthManifold != nullptr); + removeManifold(minDepthManifold); } // Return the contact manifold with a similar average normal. @@ -262,7 +216,6 @@ void ContactManifoldSet::clear() { // Create a new contact manifold and add it to the set void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) { - assert(mNbManifolds < mNbMaxManifolds); ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator); @@ -333,9 +286,26 @@ void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() { else { // Clear the obsolete contact points of the manifold - manifold->clearObseleteContactPoints(); + manifold->clearObsoleteContactPoints(); } manifold = nextManifold; } } + + +// Remove some contact manifolds and contact points if there are too many of them +void ContactManifoldSet::reduce() { + + // Remove non optimal contact manifold while there are too many manifolds in the set + while (mNbManifolds > mNbMaxManifolds) { + removeNonOptimalManifold(); + } + + // Reduce all the contact manifolds in case they have too many contact points + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + manifold->reduce(); + manifold = manifold->getNext(); + } +} diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index ea832cdb..4b33eb8c 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -74,8 +74,8 @@ class ContactManifoldSet { // Return the contact manifold with a similar average normal. ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const; - /// Return the manifold to remove (because it is too old or has not the largest penetration depth) - ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const; + /// Remove a contact manifold that is the least optimal (smaller penetration depth) + void removeNonOptimalManifold(); /// Update a previous similar manifold with a new one void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); @@ -124,8 +124,8 @@ class ContactManifoldSet { /// Clear the obsolete contact manifolds and contact points void clearObsoleteManifoldsAndContactPoints(); - /// Reset the isNew status of all the manifolds - void resetIsNewManifoldStatus(); + // Remove some contact manifolds and contact points if there are too many of them + void reduce(); // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the @@ -180,17 +180,6 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape } } - -// Reset the isNew status of all the manifolds -inline void ContactManifoldSet::resetIsNewManifoldStatus() { - - ContactManifold* manifold = mManifolds; - while (manifold != nullptr) { - manifold->setIsNew(false); - manifold = manifold->getNext(); - } -} - } #endif diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h index 1ff8bfdd..6fbf9834 100644 --- a/src/collision/ContactPointInfo.h +++ b/src/collision/ContactPointInfo.h @@ -77,6 +77,8 @@ struct ContactPointInfo { : normal(contactNormal), penetrationDepth(penDepth), localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) { + assert(contactNormal.lengthSquare() > decimal(0.8)); + assert(penDepth > decimal(0.0)); } /// Destructor diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 2cd951b4..b99d52f1 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -31,26 +31,22 @@ using namespace reactphysics3d; using namespace std; // Constructor -ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform) +ContactPoint::ContactPoint(const ContactPointInfo* contactInfo) : mNormal(contactInfo->normal), mPenetrationDepth(contactInfo->penetrationDepth), mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), - mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) { + mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) { assert(mPenetrationDepth > decimal(0.0)); - - // Compute the world position of the contact points - mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; - mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; + assert(mNormal.lengthSquare() > decimal(0.8)); mIsObsolete = false; } // Update the contact point with a new one that is similar (very close) /// The idea is to keep the cache impulse (for warm starting the contact solver) -void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) { +void ContactPoint::update(const ContactPointInfo* contactInfo) { assert(isSimilarWithContactPoint(contactInfo)); assert(contactInfo->penetrationDepth > decimal(0.0)); @@ -60,9 +56,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& mLocalPointOnBody1 = contactInfo->localPoint1; mLocalPointOnBody2 = contactInfo->localPoint2; - // Compute the world position of the contact points - mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; - mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; - mIsObsolete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 288abdcc..e2a9a5e5 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -59,12 +59,6 @@ class ContactPoint { /// Contact point on body 2 in local space of body 2 Vector3 mLocalPointOnBody2; - /// Contact point on body 1 in world space - Vector3 mWorldPointOnBody1; - - /// Contact point on body 2 in world space - Vector3 mWorldPointOnBody2; - /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; @@ -74,14 +68,16 @@ class ContactPoint { /// True if the contact point is obsolete bool mIsObsolete; - /// Pointer to the next contact point in the linked-list + /// Pointer to the next contact point in the double linked-list ContactPoint* mNext; + /// Pointer to the previous contact point in the double linked-list + ContactPoint* mPrevious; + // -------------------- Methods -------------------- // /// Update the contact point with a new one that is similar (very close) - void update(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform); + void update(const ContactPointInfo* contactInfo); /// Return true if the contact point is similar (close enougth) to another given contact point bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; @@ -99,6 +95,9 @@ class ContactPoint { /// Set the next contact point in the linked list void setNext(ContactPoint* next); + /// Set the previous contact point in the linked list + void setPrevious(ContactPoint* previous); + /// Return true if the contact point is obsolete bool getIsObsolete() const; @@ -107,8 +106,7 @@ class ContactPoint { // -------------------- Methods -------------------- // /// Constructor - ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform, - const Transform& body2Transform); + ContactPoint(const ContactPointInfo* contactInfo); /// Destructor ~ContactPoint() = default; @@ -128,18 +126,15 @@ class ContactPoint { /// Return the contact local point on body 2 Vector3 getLocalPointOnBody2() const; - /// Return the contact world point on body 1 - Vector3 getWorldPointOnBody1() const; - - /// Return the contact world point on body 2 - Vector3 getWorldPointOnBody2() const; - /// Return the cached penetration impulse decimal getPenetrationImpulse() const; /// Return true if the contact is a resting contact bool getIsRestingContact() const; + /// Return the previous contact point in the linked list + inline ContactPoint* getPrevious() const; + /// Return the next contact point in the linked list ContactPoint* getNext() const; @@ -170,16 +165,6 @@ inline Vector3 ContactPoint::getLocalPointOnBody2() const { return mLocalPointOnBody2; } -// Return the contact world point on body 1 -inline Vector3 ContactPoint::getWorldPointOnBody1() const { - return mWorldPointOnBody1; -} - -// Return the contact world point on body 2 -inline Vector3 ContactPoint::getWorldPointOnBody2() const { - return mWorldPointOnBody2; -} - // Return the cached penetration impulse inline decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; @@ -226,6 +211,16 @@ inline void ContactPoint::setNext(ContactPoint* next) { mNext = next; } +// Return the previous contact point in the linked list +inline ContactPoint* ContactPoint::getPrevious() const { + return mPrevious; +} + +// Set the previous contact point in the linked list +inline void ContactPoint::setPrevious(ContactPoint* previous) { + mPrevious = previous; +} + // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 94a82b14..819a1b76 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -120,6 +120,10 @@ void ContactSolver::initializeForIsland(Island* island) { assert(body1 != nullptr); assert(body2 != nullptr); + // Get the two contact shapes + const ProxyShape* shape1 = externalManifold->getShape1(); + const ProxyShape* shape2 = externalManifold->getShape2(); + // Get the position of the two bodies const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x2 = body2->mCenterOfMassWorld; @@ -149,11 +153,12 @@ void ContactSolver::initializeForIsland(Island* island) { // For each contact point of the contact manifold ContactPoint* externalContact = externalManifold->getContactPoints(); + assert(externalContact != nullptr); while (externalContact != nullptr) { // Get the contact point on the two bodies - Vector3 p1 = externalContact->getWorldPointOnBody1(); - Vector3 p2 = externalContact->getWorldPointOnBody2(); + Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1(); + Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2(); new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = externalContact; diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 2725a8d5..47f13bfb 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -136,9 +136,6 @@ class OverlappingPair { /// Return the last frame collision info LastFrameCollisionInfo& getLastFrameCollisionInfo(); - /// Return the number of contacts in the cache - uint getNbContactPoints() const; - /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); @@ -167,13 +164,13 @@ class OverlappingPair { void reducePotentialContactManifolds(); /// Make the contact manifolds and contact points obsolete - void makeContactsObselete(); + void makeContactsObsolete(); /// Clear the obsolete contact manifold and contact points void clearObsoleteManifoldsAndContactPoints(); - /// Reset the isNew status of all the manifolds - void resetIsNewManifoldStatus(); + /// Reduce the contact manifolds that have too many contact points + void reduceContactManifolds(); /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -206,18 +203,13 @@ inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() { return mLastFrameCollisionInfo; } -// Return the number of contact points in the contact manifold -inline uint OverlappingPair::getNbContactPoints() const { - return mContactManifoldSet.getTotalNbContactPoints(); -} - // Return the contact manifold inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { return mContactManifoldSet; } // Make the contact manifolds and contact points obsolete -inline void OverlappingPair::makeContactsObselete() { +inline void OverlappingPair::makeContactsObsolete() { mContactManifoldSet.makeContactsObsolete(); } @@ -272,10 +264,9 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() { mContactManifoldSet.clearObsoleteManifoldsAndContactPoints(); } - -// Reset the isNew status of all the manifolds -inline void OverlappingPair::resetIsNewManifoldStatus() { - mContactManifoldSet.resetIsNewManifoldStatus(); +// Reduce the contact manifolds that have too many contact points +inline void OverlappingPair::reduceContactManifolds() { + mContactManifoldSet.reduce(); } } From 0250d8c4bdaca0c7eaf581e5f85839058d72055c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 18 Oct 2017 19:35:20 +0200 Subject: [PATCH 064/105] Fix issue in SAT algorithm --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index a663791c..4411187b 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -731,7 +731,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, narrowPhaseInfo, minPenetrationDepth); - assert(contactsFound); + + // There should be clipping points here. If it is not the case, it might be + // because of a numerical issue + if (!contactsFound) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + + // Return no collision + return false; + } } lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; From cdec7413c516ffe2671d0cbde45f5b60ef1d68e5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 18 Oct 2017 21:26:53 +0200 Subject: [PATCH 065/105] Improve capsule resting on another shape stability --- .../narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp | 5 +++-- src/mathematics/mathematics_functions.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 72ba2e06..d6de739a 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -86,14 +86,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + capsuleInnerSegmentDirection.normalize(); bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal // is in direction of the contact normal (from the polyhedron point of view). - if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection) && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index fd813cdc..37335abe 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -80,7 +80,7 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v // Return true if two vectors are orthogonal bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { - return std::abs(vector1.dot(vector2)) < decimal(0.00001); + return std::abs(vector1.dot(vector2)) < decimal(0.001); } // Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" From c1295f1d7aa71f092dce03a5be2207d6f67bbe1f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 19 Oct 2017 07:26:11 +0200 Subject: [PATCH 066/105] Remove contactNormaldId attribute and fix typo --- src/collision/ContactManifold.cpp | 2 +- src/collision/ContactManifold.h | 8 ---- src/collision/ContactManifoldInfo.cpp | 11 ++---- src/collision/ContactManifoldInfo.h | 13 +----- src/collision/ContactManifoldSet.cpp | 57 +++++++-------------------- src/collision/ContactManifoldSet.h | 9 +---- src/configuration.h | 9 ++++- src/engine/DynamicsWorld.cpp | 2 +- src/engine/OverlappingPair.cpp | 13 +++--- 9 files changed, 38 insertions(+), 86 deletions(-) diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 55ce8ba3..c2d60422 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator) - : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()), + : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) { diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index b5c25cb9..80a9f456 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -104,9 +104,6 @@ class ContactManifold { /// Contact points in the manifold ContactPoint* mContactPoints; - /// Normal direction Id (Unique Id representing the normal direction) - short int mContactNormalId; - /// Number of contacts in the cache int8 mNbContactPoints; @@ -412,11 +409,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint } } -// Return the contact normal direction Id of the manifold -inline short ContactManifold::getContactNormalId() const { - return mContactNormalId; -} - } #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index a6b8c6df..f22593ac 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -30,8 +30,9 @@ using namespace reactphysics3d; // Constructor ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator) - : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator), - mContactNormalId(-1) {} + : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) { + +} // Destructor ContactManifoldInfo::~ContactManifoldInfo() { @@ -41,13 +42,9 @@ ContactManifoldInfo::~ContactManifoldInfo() { } // Add a new contact point into the manifold -void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) { +void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) { assert(contactPointInfo->penetrationDepth > decimal(0.0)); - assert(contactNormalId >= 0); - assert(mContactNormalId == -1 || contactNormalId == mContactNormalId); - - mContactNormalId = contactNormalId; // Add it into the linked list of contact points contactPointInfo->next = mContactPointsList; diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index 5851c375..4126f4c8 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -59,9 +59,6 @@ class ContactManifoldInfo { /// Reference the the memory allocator where the contact point infos have been allocated Allocator& mAllocator; - /// Contact normal direction Id (Identify the contact normal direction of points in manifold) - short mContactNormalId; - public: // -------------------- Methods -------------------- // @@ -79,7 +76,7 @@ class ContactManifoldInfo { ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; /// Add a new contact point into the manifold - void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId); + void addContactPoint(ContactPointInfo* contactPointInfo); /// Remove all the contact points void reset(); @@ -93,9 +90,6 @@ class ContactManifoldInfo { /// Return the pointer to the next manifold info in the linked-list ContactManifoldInfo* getNext(); - /// Return the contact normal Id - short getContactNormalId() const; - /// Reduce the number of contact points of the currently computed manifold void reduce(const Transform& shape1ToWorldTransform); @@ -114,11 +108,6 @@ inline ContactManifoldInfo* ContactManifoldInfo::getNext() { return mNext; } -// Return the contact normal Id -inline short ContactManifoldInfo::getContactNormalId() const { - return mContactNormalId; -} - } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 9b8bf58b..ce3e4492 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -50,7 +50,7 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr); // Try to find an existing contact manifold with similar contact normal - ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId()); + ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo); // If a similar manifold has been found if (similarManifold != nullptr) { @@ -139,15 +139,25 @@ void ContactManifoldSet::removeNonOptimalManifold() { removeManifold(minDepthManifold); } -// Return the contact manifold with a similar average normal. -// If no manifold has close enough average normal, it returns nullptr -ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { +// Return the contact manifold with a similar contact normal. +// If no manifold has close enough contact normal, it returns nullptr +ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const { + + // Get the contact normal of the first point of the manifold + const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo(); + assert(contactPoint != nullptr); ContactManifold* manifold = mManifolds; // Return the Id of the manifold with the same normal direction id (if exists) while (manifold != nullptr) { - if (normalDirectionId == manifold->getContactNormalId()) { + + // Get the first contact point of the current manifold + const ContactPoint* point = manifold->getContactPoints(); + assert(point != nullptr); + + // If the contact normal of the two manifolds are close enough + if (contactPoint->normal.dot(point->getNormal()) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) { return manifold; } @@ -157,43 +167,6 @@ ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int n return nullptr; } -// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) -// Each face of the cube is divided into 4x4 buckets. This method maps the -// normal vector into of the of the bucket and returns a unique Id for the bucket -short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) { - - assert(normal.lengthSquare() > MACHINE_EPSILON); - - int faceNo; - decimal u, v; - decimal max = max3(std::fabs(normal.x), std::fabs(normal.y), std::fabs(normal.z)); - Vector3 normalScaled = normal / max; - - if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { - faceNo = normalScaled.x > 0 ? 0 : 1; - u = normalScaled.y; - v = normalScaled.z; - } - else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { - faceNo = normalScaled.y > 0 ? 2 : 3; - u = normalScaled.x; - v = normalScaled.z; - } - else { - faceNo = normalScaled.z > 0 ? 4 : 5; - u = normalScaled.x; - v = normalScaled.y; - } - - int indexU = std::floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - int indexV = std::floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; - if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; - - const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; - return faceNo * 200 + indexU * nbSubDivInFace + indexV; -} - // Clear the contact manifold set void ContactManifoldSet::clear() { diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 4b33eb8c..13d27b70 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -71,8 +71,8 @@ class ContactManifoldSet { /// Create a new contact manifold and add it to the set void createManifold(const ContactManifoldInfo* manifoldInfo); - // Return the contact manifold with a similar average normal. - ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const; + // Return the contact manifold with a similar contact normal. + ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const; /// Remove a contact manifold that is the least optimal (smaller penetration depth) void removeNonOptimalManifold(); @@ -126,11 +126,6 @@ class ContactManifoldSet { // Remove some contact manifolds and contact points if there are too many of them void reduce(); - - // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) - // Each face of the cube is divided into 4x4 buckets. This method maps the - // normal vector into of the of the bucket and returns a unique Id for the bucket - static short int computeCubemapNormalId(const Vector3& normal); }; // Return the first proxy shape diff --git a/src/configuration.h b/src/configuration.h index 444bd1db..779b45e2 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -99,8 +99,8 @@ constexpr decimal DEFAULT_BOUNCINESS = decimal(0.5); /// Default rolling resistance constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); -/// True if the spleeping technique is enabled -constexpr bool SPLEEPING_ENABLED = true; +/// True if the sleeping technique is enabled +constexpr bool SLEEPING_ENABLED = true; /// Distance threshold for two contact points for a valid persistent contact (in meters) constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); @@ -143,6 +143,11 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; /// least one concave collision shape. constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +/// This is used to test if two contact manifold are similar (same contact normal) in order to +/// merge them. If the cosine of the angle between the normals of the two manifold are larger +/// than the value bellow, the manifold are considered to be similar. +constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95); + /// Size (in bytes) of the single frame allocator constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 136d34f6..09712c3c 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -44,7 +44,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity), + mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index f3f85fa1..0055e683 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -54,21 +54,22 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo ContactPointInfo* nextContactPoint = contactPoint->next; - // Compute the contact normal id - short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal); - // Look if the contact point correspond to an existing potential manifold // (if the contact point normal is similar to the normal of an existing manifold) ContactManifoldInfo* manifold = mPotentialContactManifolds; bool similarManifoldFound = false; while(manifold != nullptr) { + // Get the first contact point + const ContactPointInfo* point = manifold->getFirstContactPointInfo(); + assert(point != nullptr); + // If we have found a corresponding manifold for the new contact point // (a manifold with a similar contact normal direction) - if (manifold->getContactNormalId() == contactNormalId) { + if (point->normal.dot(contactPoint->normal) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) { // Add the contact point to the manifold - manifold->addContactPoint(contactPoint, contactNormalId); + manifold->addContactPoint(contactPoint); similarManifoldFound = true; @@ -90,7 +91,7 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo mPotentialContactManifolds = manifoldInfo; // Add the contact point to the manifold - manifoldInfo->addContactPoint(contactPoint, contactNormalId); + manifoldInfo->addContactPoint(contactPoint); } contactPoint = nextContactPoint; From cdaa297a7800a10ecd14dc0ddf7fbc4561502887 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 19 Oct 2017 17:19:33 +0200 Subject: [PATCH 067/105] Remove unused variable --- src/engine/OverlappingPair.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 47f13bfb..ce22bb7e 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -56,10 +56,6 @@ struct LastFrameCollisionInfo { /// True if we were using SAT algorithm to check for collision in the previous frame bool wasUsingSAT; - /// True if there was a narrow-phase collision - /// in the previous frame - bool wasCollidingLastFrame; - // ----- GJK Algorithm ----- /// Previous separating axis From b1aad2b7c493e6b46cf28e138b65d522ae624333 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 19 Oct 2017 17:42:21 +0200 Subject: [PATCH 068/105] Refactor the testbed application --- testbed/common/Box.h | 2 +- testbed/common/Capsule.h | 3 +- testbed/common/ConvexMesh.h | 3 +- testbed/common/Dumbbell.h | 4 +- testbed/common/PhysicsObject.h | 3 + testbed/common/Sphere.h | 3 +- .../CollisionDetectionScene.cpp | 60 ++---- .../CollisionDetectionScene.h | 10 +- .../collisionshapes/CollisionShapesScene.cpp | 154 +------------- .../collisionshapes/CollisionShapesScene.h | 9 +- testbed/scenes/cubes/CubesScene.cpp | 35 +--- testbed/scenes/cubes/CubesScene.h | 7 - .../scenes/heightfield/HeightFieldScene.cpp | 191 +++++++++++++----- testbed/scenes/heightfield/HeightFieldScene.h | 47 ++++- testbed/scenes/joints/JointsScene.cpp | 27 +-- testbed/scenes/joints/JointsScene.h | 3 - testbed/scenes/raycast/RaycastScene.cpp | 33 +-- testbed/scenes/raycast/RaycastScene.h | 6 +- testbed/src/SceneDemo.cpp | 32 ++- testbed/src/SceneDemo.h | 10 +- testbed/src/TestbedApplication.cpp | 2 +- 21 files changed, 281 insertions(+), 363 deletions(-) mode change 100644 => 100755 testbed/scenes/cubes/CubesScene.cpp mode change 100644 => 100755 testbed/scenes/cubes/CubesScene.h diff --git a/testbed/common/Box.h b/testbed/common/Box.h index a1f05d5e..bd7a6024 100644 --- a/testbed/common/Box.h +++ b/testbed/common/Box.h @@ -86,7 +86,7 @@ class Box : public PhysicsObject { ~Box(); /// Render the cube at the correct position and with the correct orientation - void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h index f117984f..5c99120b 100644 --- a/testbed/common/Capsule.h +++ b/testbed/common/Capsule.h @@ -94,8 +94,7 @@ class Capsule : public PhysicsObject { ~Capsule(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index e2ba5836..1b55611b 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -90,8 +90,7 @@ class ConvexMesh : public PhysicsObject { ~ConvexMesh(); /// Render the mesh at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index ea2d3d88..aa6bded1 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -89,13 +89,11 @@ class Dumbbell : public PhysicsObject { Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, const std::string& meshFolderPath); - /// Destructor ~Dumbbell(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index 0da224c3..7fcc612d 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -62,6 +62,9 @@ class PhysicsObject : public openglframework::Mesh { /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor)=0; + /// Render the sphere at the correct position and with the correct orientation + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe)=0; + /// Set the color of the box void setColor(const openglframework::Color& color); diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h index 62f1809e..000cc776 100644 --- a/testbed/common/Sphere.h +++ b/testbed/common/Sphere.h @@ -90,8 +90,7 @@ class Sphere : public PhysicsObject { ~Sphere(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 7606a61e..3b186943 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -59,6 +59,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mSphere1->setColor(mGreyColorDemo); mSphere1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mSphere1); // ---------- Sphere 2 ---------- // openglframework::Vector3 position2(12, 8, 0); @@ -70,6 +71,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mSphere2->setColor(mGreyColorDemo); mSphere2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mSphere2); // ---------- Capsule 1 ---------- // openglframework::Vector3 position3(-6, 7, 0); @@ -81,6 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mCapsule1->setColor(mGreyColorDemo); mCapsule1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule1); // ---------- Capsule 2 ---------- // openglframework::Vector3 position4(11, -8, 0); @@ -92,6 +95,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mCapsule2->setColor(mGreyColorDemo); mCapsule2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule2); // ---------- Box 1 ---------- // openglframework::Vector3 position5(-4, -7, 0); @@ -103,6 +107,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mBox1->setColor(mGreyColorDemo); mBox1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox1); // ---------- Box 2 ---------- // openglframework::Vector3 position6(0, 8, 0); @@ -114,6 +119,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mBox2->setColor(mGreyColorDemo); mBox2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox2); // ---------- Convex Mesh ---------- // openglframework::Vector3 position7(-5, 0, 0); @@ -125,9 +131,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mConvexMesh->setColor(mGreyColorDemo); mConvexMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConvexMesh); // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 0, 0); + openglframework::Vector3 position8(0, 100, 0); // Create a convex mesh and a corresponding collision body in the dynamics world mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); @@ -136,16 +143,18 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Set the color mConcaveMesh->setColor(mGreyColorDemo); mConcaveMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); // ---------- Heightfield ---------- // - //openglframework::Vector3 position9(0, 0, 0); + openglframework::Vector3 position9(0, -12, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - //mHeightField = new HeightField(position9, mCollisionWorld); + mHeightField = new HeightField(position9, mCollisionWorld); // Set the color - //mHeightField->setColor(mGreyColorDemo); - //mHeightField->setSleepingColor(mRedColorDemo); + mHeightField->setColor(mGreyColorDemo); + mHeightField->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mHeightField); mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); } @@ -187,6 +196,9 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); delete mConcaveMesh; + mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + delete mHeightField; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -210,11 +222,7 @@ CollisionDetectionScene::~CollisionDetectionScene() { // Destroy the dumbbell delete mDumbbell; - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); - - // Destroy the convex mesh - delete mHeightField; + */ mContactManager.resetPoints(); @@ -226,12 +234,6 @@ CollisionDetectionScene::~CollisionDetectionScene() { delete mCollisionWorld; } -// Update the physics world (take a simulation step) -void CollisionDetectionScene::updatePhysics() { - - -} - // Take a step for the simulation void CollisionDetectionScene::update() { @@ -242,32 +244,6 @@ void CollisionDetectionScene::update() { SceneDemo::update(); } -// Render the scene -void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Render the shapes - if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - /* - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); - */ - - shader.unbind(); -} - void CollisionDetectionScene::selectNextShape() { int previousIndex = mSelectedShapeIndex; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 8e94eaf5..8473ae73 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -148,7 +148,7 @@ class CollisionDetectionScene : public SceneDemo { ConvexMesh* mConvexMesh; //Dumbbell* mDumbbell; ConcaveMesh* mConcaveMesh; - //HeightField* mHeightField; + HeightField* mHeightField; std::vector mAllShapes; @@ -170,17 +170,9 @@ class CollisionDetectionScene : public SceneDemo { /// Destructor virtual ~CollisionDetectionScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Take a step for the simulation virtual void update() override; - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index 746942ff..e63f39d4 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -71,6 +71,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the mesh the list of dumbbells in the scene mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); } // Create all the boxes of the scene @@ -95,6 +96,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the sphere the list of sphere in the scene mBoxes.push_back(box); + mPhysicsObjects.push_back(box); } // Create all the spheres of the scene @@ -123,6 +125,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the sphere the list of sphere in the scene mSpheres.push_back(sphere); + mPhysicsObjects.push_back(sphere); } // Create all the capsules of the scene @@ -150,6 +153,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the cylinder the list of sphere in the scene mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); } // Create all the convex meshes of the scene @@ -174,12 +178,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the mesh the list of sphere in the scene mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); } // ---------- Create the floor --------- openglframework::Vector3 floorPosition(0, 0, 0); mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); + mPhysicsObjects.push_back(mFloor); // Set the box color mFloor->setColor(mGreyColorDemo); @@ -230,68 +236,16 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Destructor CollisionShapesScene::~CollisionShapesScene() { - // Destroy all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { // Destroy the corresponding rigid body from the dynamics world mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - // Destroy the box + // Destroy the object delete (*it); } - // Destroy all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the convex mesh - delete (*it); - } - - // Destroy all the dumbbell of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the convex mesh - delete (*it); - } - - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - //mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); - - // Destroy the floor - delete mFloor; - - // Destroy the convex mesh - //delete mConcaveMesh; - // Destroy the dynamics world delete mDynamicsWorld; } @@ -315,96 +269,6 @@ void CollisionShapesScene::updatePhysics() { mDynamicsWorld->update(mEngineSettings.timeStep); } -// Take a step for the simulation -void CollisionShapesScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the sphere - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the capsules - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the convex meshes - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the dumbbells - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - //mConcaveMesh->updateTransform(mInterpolationFactor); - - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene -void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render all the dumbbells of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - //mConcaveMesh->render(shader, worldToCameraMatrix); - - // Unbind the shader - shader.unbind(); -} - /// Reset the scene void CollisionShapesScene::reset() { diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h index b253acef..70e28755 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -71,7 +71,7 @@ class CollisionShapesScene : public SceneDemo { // -------------------- Attributes -------------------- // - /// All the spheres of the scene + /// All the boxes of the scene std::vector mBoxes; std::vector mSpheres; @@ -104,13 +104,6 @@ class CollisionShapesScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Take a step for the simulation - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp old mode 100644 new mode 100755 index 81929e58..3ddb1d61 --- a/testbed/scenes/cubes/CubesScene.cpp +++ b/testbed/scenes/cubes/CubesScene.cpp @@ -82,6 +82,7 @@ CubesScene::CubesScene(const std::string& name) // The floor must be a static rigid body mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mFloor); // Change the material properties of the floor rigid body //rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); @@ -102,6 +103,7 @@ CubesScene::CubesScene(const std::string& name) // Add the box the list of box in the scene mBoxes.push_back(cube); + mPhysicsObjects.push_back(cube); // Get the physics engine parameters mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); @@ -157,39 +159,6 @@ void CubesScene::updatePhysics() { mDynamicsWorld->update(mEngineSettings.timeStep); } -// Update the scene -void CubesScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene in a single pass -void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - // Unbind the shader - shader.unbind(); -} - // Reset the scene void CubesScene::reset() { diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h old mode 100644 new mode 100755 index 4ad57f77..aa4b7073 --- a/testbed/scenes/cubes/CubesScene.h +++ b/testbed/scenes/cubes/CubesScene.h @@ -72,13 +72,6 @@ class CubesScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 3e4061c9..329712e9 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -33,6 +33,8 @@ using namespace heightfieldscene; // Constructor HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { + std::string meshFolderPath("meshes/"); + // Compute the radius and the center of the scene openglframework::Vector3 center(0, 5, 0); @@ -45,25 +47,138 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // ---------- Create the boxes ----------- // + float radius = 3.0f; - // For each box - for (int i=0; isetColor(mDemoColors[2]); - mBoxes[i]->setSleepingColor(mRedColorDemo); + // Set the box color + dumbbell->setColor(mDemoColors[i % mNbDemoColors]); + dumbbell->setSleepingColor(mRedColorDemo); - // Change the material properties of the rigid body - rp3d::Material& boxMaterial = mBoxes[i]->getRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } + // Change the material properties of the rigid body + rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of dumbbells in the scene + mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); + } + + // Create all the boxes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + box->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = box->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mBoxes.push_back(box); + mPhysicsObjects.push_back(box); + } + + // Create all the spheres of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + + // Set the box color + sphere->setColor(mDemoColors[i % mNbDemoColors]); + sphere->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mSpheres.push_back(sphere); + mPhysicsObjects.push_back(sphere); + } + + // Create all the capsules of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + + // Set the box color + capsule->setColor(mDemoColors[i % mNbDemoColors]); + capsule->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = capsule->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the cylinder the list of sphere in the scene + mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); + } + + // Create all the convex meshes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + mesh->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of sphere in the scene + mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); + } // ---------- Create the height field ---------- // @@ -77,6 +192,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Set the mesh as beeing static mHeightField->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mHeightField); + // Set the color mHeightField->setColor(mGreyColorDemo); mHeightField->setSleepingColor(mGreyColorDemo); @@ -101,18 +218,15 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Destructor HeightFieldScene::~HeightFieldScene() { - // Destroy the corresponding rigid body from the dynamics world - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - } - mDynamicsWorld->destroyRigidBody(mHeightField->getRigidBody()); + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { - for (int i=0; idestroyRigidBody((*it)->getRigidBody()); - // Destroy the convex mesh - delete mHeightField; + // Destroy the object + delete (*it); + } // Destroy the dynamics world delete mDynamicsWorld; @@ -137,35 +251,6 @@ void HeightFieldScene::updatePhysics() { mDynamicsWorld->update(mEngineSettings.timeStep); } -// Update the scene -void HeightFieldScene::update() { - - SceneDemo::update(); - - // Update the transform used for the rendering - mHeightField->updateTransform(mInterpolationFactor); - - for (int i=0; iupdateTransform(mInterpolationFactor); - } -} - -// Render the scene in a single pass -void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - for (int i=0; irender(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Unbind the shader - shader.unbind(); -} - // Reset the scene void HeightFieldScene::reset() { diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h index 7eb9527d..7306ae2c 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.h +++ b/testbed/scenes/heightfield/HeightFieldScene.h @@ -30,6 +30,10 @@ #include "openglframework.h" #include "reactphysics3d.h" #include "Box.h" +#include "Sphere.h" +#include "ConvexMesh.h" +#include "Capsule.h" +#include "Dumbbell.h" #include "SceneDemo.h" #include "HeightField.h" @@ -37,17 +41,47 @@ namespace heightfieldscene { // Constants const float SCENE_RADIUS = 50.0f; +static const int NB_BOXES = 10; +static const int NB_SPHERES = 5; +static const int NB_CAPSULES = 5; +static const int NB_MESHES = 3; +static const int NB_COMPOUND_SHAPES = 3; +const openglframework::Vector3 BOX_SIZE(2, 2, 2); +const float SPHERE_RADIUS = 1.5f; +const float CONE_RADIUS = 2.0f; +const float CONE_HEIGHT = 3.0f; +const float CYLINDER_RADIUS = 1.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; +const float DUMBBELL_HEIGHT = 1.0f; +const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; +const float CONE_MASS = 1.0f; +const float CYLINDER_MASS = 1.0f; +const float CAPSULE_MASS = 1.0f; +const float MESH_MASS = 1.0f; +const float FLOOR_MASS = 100.0f; // Class HeightFieldScene class HeightFieldScene : public SceneDemo { - static const int NB_BOXES = 10; - protected : // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES]; + /// All the boxes of the scene + std::vector mBoxes; + + std::vector mSpheres; + + std::vector mCapsules; + + /// All the convex meshes of the scene + std::vector mConvexMeshes; + + /// All the dumbbell of the scene + std::vector mDumbbells; /// Height field HeightField* mHeightField; @@ -69,13 +103,6 @@ class HeightFieldScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override ; - /// Reset the scene virtual void reset() override ; diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index 23bcdb93..282f7e51 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -136,25 +136,6 @@ void JointsScene::updatePhysics() { mDynamicsWorld->update(mEngineSettings.timeStep); } -// Take a step for the simulation -void JointsScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - mSliderJointBottomBox->updateTransform(mInterpolationFactor); - mSliderJointTopBox->updateTransform(mInterpolationFactor); - mPropellerBox->updateTransform(mInterpolationFactor); - mFixedJointBox1->updateTransform(mInterpolationFactor); - mFixedJointBox2->updateTransform(mInterpolationFactor); - for (int i=0; iupdateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the floor - mFloor->updateTransform(mInterpolationFactor); -} - // Render the scene void JointsScene::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { @@ -281,6 +262,8 @@ void JointsScene::createBallAndSocketJoints() { rp3d::Material& material = mBallAndSocketJointChainBoxes[i]->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mBallAndSocketJointChainBoxes[i]); + positionBox.y -= boxDimension.y + 0.5f; } @@ -324,6 +307,7 @@ void JointsScene::createSliderJoint() { // Change the material properties of the rigid body rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); material1.setBounciness(0.4f); + mPhysicsObjects.push_back(mSliderJointBottomBox); // --------------- Create the second box --------------- // @@ -341,6 +325,7 @@ void JointsScene::createSliderJoint() { // Change the material properties of the rigid body rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); material2.setBounciness(0.4f); + mPhysicsObjects.push_back(mSliderJointTopBox); // --------------- Create the joint --------------- // @@ -381,6 +366,7 @@ void JointsScene::createPropellerHingeJoint() { // Change the material properties of the rigid body rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mPropellerBox); // --------------- Create the Hinge joint --------------- // @@ -420,6 +406,7 @@ void JointsScene::createFixedJoints() { // Change the material properties of the rigid body rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); material1.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mFixedJointBox1); // --------------- Create the second box --------------- // @@ -436,6 +423,7 @@ void JointsScene::createFixedJoints() { // Change the material properties of the rigid body rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); material2.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mFixedJointBox2); // --------------- Create the first fixed joint --------------- // @@ -478,4 +466,5 @@ void JointsScene::createFloor() { // Change the material properties of the rigid body rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.3)); + mPhysicsObjects.push_back(mFloor); } diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h index a9474393..16ccfe8c 100644 --- a/testbed/scenes/joints/JointsScene.h +++ b/testbed/scenes/joints/JointsScene.h @@ -126,9 +126,6 @@ class JointsScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Take a step for the simulation - virtual void update() override; - /// Render the scene in a single pass virtual void renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index d420073f..2ded75d6 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -56,6 +56,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the box color mDumbbell->setColor(mGreyColorDemo); mDumbbell->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mDumbbell); // ---------- Box ---------- // openglframework::Vector3 position2(0, 0, 0); @@ -67,6 +68,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the box color mBox->setColor(mGreyColorDemo); mBox->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox); // ---------- Sphere ---------- // openglframework::Vector3 position3(0, 0, 0); @@ -78,6 +80,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the color mSphere->setColor(mGreyColorDemo); mSphere->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mSphere); // ---------- Capsule ---------- // openglframework::Vector3 position6(0, 0, 0); @@ -89,6 +92,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the color mCapsule->setColor(mGreyColorDemo); mCapsule->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule); // ---------- Convex Mesh ---------- // openglframework::Vector3 position7(0, 0, 0); @@ -99,6 +103,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the color mConvexMesh->setColor(mGreyColorDemo); mConvexMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConvexMesh); // ---------- Concave Mesh ---------- // openglframework::Vector3 position8(0, 0, 0); @@ -109,6 +114,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the color mConcaveMesh->setColor(mGreyColorDemo); mConcaveMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); // ---------- Heightfield ---------- // openglframework::Vector3 position9(0, 0, 0); @@ -119,6 +125,7 @@ RaycastScene::RaycastScene(const std::string& name) // Set the color mHeightField->setColor(mGreyColorDemo); mHeightField->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mHeightField); // Create the lines that will be used for raycasting createLines(); @@ -256,12 +263,6 @@ RaycastScene::~RaycastScene() { mVAO.destroy(); } -// Update the physics world (take a simulation step) -void RaycastScene::updatePhysics() { - - -} - // Take a step for the simulation void RaycastScene::update() { @@ -327,14 +328,18 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg mColorShader.unbind(); - // Render the shapes - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + // Bind the shader + shader.bind(); + + // Render all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + if ((*it)->getCollisionBody()->isActive()) { + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + } + } + + // Unbind the shader + shader.unbind(); } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index 39b73799..1400fcf7 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -57,7 +57,7 @@ const float CAPSULE_HEIGHT = 5.0f; const float DUMBBELL_HEIGHT = 5.0f; const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; -const int NB_BODIES = 9; +const int NB_BODIES = 7; // Raycast manager class RaycastManager : public rp3d::RaycastCallback { @@ -175,10 +175,6 @@ class RaycastScene : public SceneDemo { /// Destructor virtual ~RaycastScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Take a step for the simulation virtual void update() override; diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 85e8a66f..d406bcea 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -80,7 +80,7 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa // Destructor SceneDemo::~SceneDemo() { - + mShadowMapTexture.destroy(); mFBOShadowMap.destroy(); mVBOQuad.destroy(); @@ -101,6 +101,19 @@ void SceneDemo::update() { // Update the contact points updateContactPoints(); + + // Update the position and orientation of the physics objects + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } +} + +// Update the physics world (take a simulation step) +// Can be called several times per frame +void SceneDemo::updatePhysics() { + } // Render the scene (in multiple passes for shadow mapping) @@ -202,6 +215,21 @@ void SceneDemo::render() { //drawTextureQuad(); } +// Render the scene in a single pass +void SceneDemo::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { + + // Bind the shader + shader.bind(); + + // Render all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + } + + // Unbind the shader + shader.unbind(); +} + // Create the Shadow map FBO and texture void SceneDemo::createShadowMapFBOAndTexture() { @@ -345,7 +373,7 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn rp3d::ContactPoint* contactPoint = manifold->getContactPoints(); while (contactPoint != nullptr) { - rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); + rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnBody1(); rp3d::Vector3 normalWorld = contactPoint->getNormal(); openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h index f7b5f7ce..a8b1941f 100644 --- a/testbed/src/SceneDemo.h +++ b/testbed/src/SceneDemo.h @@ -30,6 +30,7 @@ #include "Scene.h" #include "VisualContactPoint.h" #include "reactphysics3d.h" +#include "PhysicsObject.h" // Constants const int SHADOWMAP_WIDTH = 2048; @@ -95,6 +96,8 @@ class SceneDemo : public Scene { std::string mMeshFolderPath; + std::vector mPhysicsObjects; + // -------------------- Methods -------------------- // // Create the Shadow map FBO and texture @@ -128,12 +131,15 @@ class SceneDemo : public Scene { /// 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; /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix)=0 ; + virtual void renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); /// Enabled/Disable the shadow mapping virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index 09f464f3..2b9195e5 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -125,7 +125,7 @@ void TestbedApplication::createScenes() { mScenes.push_back(concaveMeshScene); assert(mScenes.size() > 0); - mCurrentScene = mScenes[0]; + mCurrentScene = mScenes[5]; // Get the engine settings from the scene mEngineSettings = mCurrentScene->getEngineSettings(); From 5da57a96c80be764d77e38478c33b64de2d9bc34 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 24 Oct 2017 22:47:35 +0200 Subject: [PATCH 069/105] Fix issue with sphere and capsule SAT collision detection --- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 4 ++++ src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index d6de739a..89336c4e 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -114,6 +114,10 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + if (isCapsuleShape1) { + faceNormalWorld = -faceNormalWorld; + } + // Compute and create two contact points satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4411187b..1ba5143b 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -97,10 +97,12 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow if (reportContacts) { const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); - Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); - Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); + Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal; + Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius()); Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, @@ -243,6 +245,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + if (isCapsuleShape1) { + normalWorld = -normalWorld; + } const decimal capsuleRadius = capsuleShape->getRadius(); // If the separating axis is a face normal @@ -391,12 +396,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); - if (isCapsuleShape1) { - normalWorld = -normalWorld; - } - // For each of the two clipped points - for (int i = 0; i<2; i++) { + for (uint i = 0; i<2; i++) { // Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth) const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal); From 6a69ef76c5cd6467f90c4b3b20ba30704d960696 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 1 Nov 2017 23:07:56 +0100 Subject: [PATCH 070/105] Fix issue with ProxyShape::mBroadPhaseId not set when body was sleeping or inactive --- src/body/CollisionBody.cpp | 26 +- src/collision/CollisionDetection.cpp | 245 ++++++++++-------- src/collision/CollisionDetection.h | 8 +- .../broadphase/BroadPhaseAlgorithm.cpp | 6 + .../broadphase/BroadPhaseAlgorithm.h | 3 + src/engine/CollisionWorld.cpp | 11 + src/engine/CollisionWorld.h | 3 + 7 files changed, 178 insertions(+), 124 deletions(-) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 39d47de4..64ce6467 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -111,7 +111,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { if (current == proxyShape) { mProxyCollisionShapes = current->mNext; - if (mIsActive) { + if (mIsActive && proxyShape->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(current); } @@ -131,7 +131,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { ProxyShape* elementToRemove = current->mNext; current->mNext = elementToRemove->mNext; - if (mIsActive) { + if (mIsActive && proxyShape->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); } @@ -157,7 +157,7 @@ void CollisionBody::removeAllCollisionShapes() { // Remove the proxy collision shape ProxyShape* nextElement = current->mNext; - if (mIsActive) { + if (mIsActive && current->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(current); } @@ -202,12 +202,15 @@ void CollisionBody::updateBroadPhaseState() const { // Update the broad-phase state of a proxy collision shape of the body void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const { - // Recompute the world-space AABB of the collision shape - AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + if (proxyShape->mBroadPhaseID != -1) { - // Update the broad-phase state for the proxy collision shape - mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); + // Recompute the world-space AABB of the collision shape + AABB aabb; + proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ; + } } // Set whether or not the body is active @@ -240,8 +243,11 @@ void CollisionBody::setIsActive(bool isActive) { // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { - // Remove the proxy shape from the collision detection - mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + if (shape->mBroadPhaseID != -1) { + + // Remove the proxy shape from the collision detection + mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + } } // Reset the contact manifold list of the body diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f650351a..baaf0db5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -109,6 +109,8 @@ void CollisionDetection::computeMiddlePhase() { ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); + assert(shape1->mBroadPhaseID != -1); + assert(shape2->mBroadPhaseID != -1); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); // Check if the two shapes are still overlapping. Otherwise, we destroy the @@ -282,6 +284,8 @@ void CollisionDetection::computeNarrowPhase() { /// This method is called by the broad-phase collision detection algorithm void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { + assert(shape1->mBroadPhaseID != -1); + assert(shape2->mBroadPhaseID != -1); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); // Check if the collision filtering allows collision between the two shapes @@ -313,6 +317,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Remove a body from the collision detection void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { + assert(proxyShape->mBroadPhaseID != -1); + // Remove all the overlapping pairs involving this proxy shape std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { @@ -683,87 +689,90 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mPoolAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // If the proxy shape is from a body that we have not already reported collision and the - // two proxy collision shapes are not from the same body - if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() && - proxyShape->getBody()->getID() != bodyId) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // If the proxy shape is from a body that we have not already reported collision and the + // two proxy collision shapes are not from the same body + if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() && + proxyShape->getBody()->getID() != bodyId) { - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); - bool isColliding = false; + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + bool isColliding = false; - // If we have not found a collision yet - if (!isColliding) { + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // If we have not found a collision yet + if (!isColliding) { - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + } } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Return if we have found a narrow-phase collision + if (isColliding) { - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); + CollisionBody* overlapBody = proxyShape->getBody(); - // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } + // Add the body into the set of reported bodies + reportedBodies.insert(overlapBody->getID()); - // Return if we have found a narrow-phase collision - if (isColliding) { - - CollisionBody* overlapBody = proxyShape->getBody(); - - // Add the body into the set of reported bodies - reportedBodies.insert(overlapBody->getID()); - - // Notify the overlap to the user - overlapCallback->notifyOverlap(overlapBody); + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } } } - } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next overlapping proxy shape + element = element->next; + } } // Go to the next proxy shape @@ -858,85 +867,88 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mPoolAllocator); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // If the two proxy collision shapes are not from the same body - if (proxyShape->getBody()->getID() != bodyId) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // If the two proxy collision shapes are not from the same body + if (proxyShape->getBody()->getID() != bodyId) { - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { - // Add the contact points as a potential contact manifold into the pair - narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + } } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Process the potential contacts + processPotentialContacts(&pair); - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); + if (pair.hasContacts()) { - // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + callback->notifyContact(collisionInfo); + } } - - // Process the potential contacts - processPotentialContacts(&pair); - - if (pair.hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - callback->notifyContact(collisionInfo); - } } + + // Go to the next overlapping proxy shape + element = element->next; } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); } - - // Go to the next proxy shape - bodyProxyShape = bodyProxyShape->getNext(); } } @@ -1031,7 +1043,14 @@ EventListener* CollisionDetection::getWorldEventListener() { return mWorld->mEventListener; } -/// Return a reference to the world memory allocator +// Return a reference to the world memory allocator PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { return mWorld->mPoolAllocator; } + + +// Return the world-space AABB of a given proxy shape +const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const { + assert(proxyShape->mBroadPhaseID > -1); + return mBroadPhaseAlgorithm.getFatAABB(proxyShape->mBroadPhaseID); +} diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 848e7d09..6f28f788 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -219,6 +219,9 @@ class CollisionDetection { /// Return a reference to the world memory allocator PoolAllocator& getWorldMemoryAllocator(); + /// Return the world-space AABB of a given proxy shape + const AABB getWorldAABB(const ProxyShape* proxyShape) const; + // -------------------- Friendship -------------------- // friend class DynamicsWorld; @@ -259,7 +262,10 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, /// We simply put the shape in the list of collision shape that have moved in the /// previous frame so that it is tested for collision again in the broad-phase. inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { - mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + + if (shape->mBroadPhaseID != -1) { + mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + } } // Update a proxy collision shape (that has moved for instance) diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index bc03f44d..c86cd0bc 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -117,6 +117,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { // Add a proxy collision shape into the broad-phase collision detection void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { + assert(proxyShape->mBroadPhaseID == -1); + // Add the collision shape into the dynamic AABB tree and get its broad-phase ID int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape); @@ -131,8 +133,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A // Remove a proxy collision shape from the broad-phase collision detection void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { + assert(proxyShape->mBroadPhaseID != -1); + int broadPhaseID = proxyShape->mBroadPhaseID; + proxyShape->mBroadPhaseID = -1; + // Remove the collision shape from the dynamic AABB tree mDynamicAABBTree.removeObject(broadPhaseID); diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 8d2cda35..94499d44 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -232,6 +232,9 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad // Return true if the two broad-phase collision shapes are overlapping inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const { + + if (shape1->mBroadPhaseID == -1 || shape2->mBroadPhaseID == -1) return false; + // Get the two AABBs of the collision shapes const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID); const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID); diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index d160021c..e8f4155a 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -164,3 +164,14 @@ bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) { return mCollisionDetection.testOverlap(body1, body2); } + +// Return the current world-space AABB of given proxy shape +AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const { + + if (proxyShape->mBroadPhaseID == -1) { + return AABB(); + } + + return mCollisionDetection.getWorldAABB(proxyShape); +} + diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 82d2601f..d27b8018 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -147,6 +147,9 @@ class CollisionWorld { /// Test and report collisions between all shapes of the world void testCollision(CollisionCallback* callback); + /// Return the current world-space AABB of given proxy shape + AABB getWorldAABB(const ProxyShape* proxyShape) const; + // -------------------- Friendship -------------------- // friend class CollisionDetection; From ad0f805f53be29b0f3c7e17b5fdc1986c51a759c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 1 Nov 2017 23:09:02 +0100 Subject: [PATCH 071/105] Fix robustness issue in SAT Algorithm (convex polyhedron vs capsule) --- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 5 ++++- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 13 +++++++++---- src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 89336c4e..e895eef7 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -119,10 +119,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh } // Compute and create two contact points - satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, narrowPhaseInfo, isCapsuleShape1); + if (!contactsFound) { + return false; + } break; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 1ba5143b..e6d09dd1 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -256,7 +256,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro if (reportContacts) { - computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, + return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, narrowPhaseInfo, isCapsuleShape1); @@ -349,7 +349,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe // Compute the two contact points between a polyhedron and a capsule when the separating // axis is a face normal of the polyhedron -void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, +bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, @@ -391,13 +391,14 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // First we clip the inner segment of the capsule with the four planes of the adjacent faces std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); - assert(clipSegment.size() == 2); // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); + bool contactFound = false; + // For each of the two clipped points - for (uint i = 0; i<2; i++) { + for (uint i = 0; i penetrationDepth - capsuleRadius - decimal(0.001)) { + contactFound = true; + Vector3 contactPointPolyhedron = clipSegment[i] + delta; // Project the clipped point into the capsule bounds @@ -426,6 +429,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); } } + + return contactFound; } // This method returns true if an edge of a polyhedron and a capsule forms a diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 37e4f7d2..5c721a00 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -126,7 +126,7 @@ class SATAlgorithm { bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron - void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, From 7495ff6598d218ae8d8549dd7320047791f4d5a3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 Nov 2017 22:58:41 +0100 Subject: [PATCH 072/105] Refactor the testbed application and display objects AABBs --- testbed/CMakeLists.txt | 2 + testbed/common/AABB.cpp | 202 +++++++++++++ testbed/common/AABB.h | 79 +++++ testbed/common/Box.cpp | 44 +-- testbed/common/Box.h | 10 +- testbed/common/Capsule.cpp | 40 +-- testbed/common/Capsule.h | 10 +- testbed/common/ConcaveMesh.cpp | 40 +-- testbed/common/ConcaveMesh.h | 10 +- testbed/common/ConvexMesh.cpp | 40 +-- testbed/common/ConvexMesh.h | 10 +- testbed/common/Dumbbell.cpp | 42 +-- testbed/common/Dumbbell.h | 10 +- testbed/common/HeightField.cpp | 38 +-- testbed/common/HeightField.h | 10 +- testbed/common/PhysicsObject.h | 2 +- testbed/common/Sphere.cpp | 39 +-- testbed/common/Sphere.h | 10 +- .../CollisionDetectionScene.cpp | 125 ++++---- .../CollisionDetectionScene.h | 7 +- .../collisionshapes/CollisionShapesScene.cpp | 208 ++++---------- .../collisionshapes/CollisionShapesScene.h | 13 +- .../scenes/concavemesh/ConcaveMeshScene.cpp | 270 +++++++++++------- testbed/scenes/concavemesh/ConcaveMeshScene.h | 57 ++-- testbed/scenes/cubes/CubesScene.cpp | 132 +++------ testbed/scenes/cubes/CubesScene.h | 13 +- .../scenes/heightfield/HeightFieldScene.cpp | 169 +++++------ testbed/scenes/heightfield/HeightFieldScene.h | 13 +- testbed/scenes/joints/JointsScene.cpp | 135 ++++----- testbed/scenes/joints/JointsScene.h | 11 +- testbed/scenes/raycast/RaycastScene.cpp | 56 ++-- testbed/scenes/raycast/RaycastScene.h | 5 +- testbed/src/Gui.cpp | 8 + testbed/src/Scene.cpp | 16 +- testbed/src/Scene.h | 53 ++-- testbed/src/SceneDemo.cpp | 86 +++++- testbed/src/SceneDemo.h | 25 +- testbed/src/TestbedApplication.cpp | 34 ++- testbed/src/TestbedApplication.h | 6 +- 39 files changed, 1066 insertions(+), 1014 deletions(-) create mode 100644 testbed/common/AABB.cpp create mode 100644 testbed/common/AABB.h diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index a5620028..e5e3b312 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -94,6 +94,8 @@ SET(COMMON_SOURCES common/VisualContactPoint.cpp common/PerlinNoise.h common/PerlinNoise.cpp + common/AABB.h + common/AABB.cpp ) # Examples scenes source files diff --git a/testbed/common/AABB.cpp b/testbed/common/AABB.cpp new file mode 100644 index 00000000..8c43700c --- /dev/null +++ b/testbed/common/AABB.cpp @@ -0,0 +1,202 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "AABB.h" + +// Macros +#define MEMBER_OFFSET(s,m) ((char *)nullptr + (offsetof(s,m))) + +// Initialize static variables +openglframework::VertexBufferObject AABB::mVBOVertices(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject AABB::mVBONormals(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject AABB::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); +openglframework::VertexArrayObject AABB::mVAO; + + +// Initialize the data to render AABBs +void AABB::init() { + createVBOAndVAO(); +} + +// Destroy the data used to render AABBs +void AABB::destroy() { + + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVAO.destroy(); +} + +// Render the AABB +void AABB::render(const openglframework::Vector3& position, const openglframework::Vector3& dimension, + openglframework::Color color, openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) { + + // Render in wireframe mode + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + + // Bind the shader + shader.bind(); + + // Transform matrix + openglframework::Matrix4 transformMatrix = openglframework::Matrix4::identity(); + transformMatrix = openglframework::Matrix4::translationMatrix(position) * transformMatrix; + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * transformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + + // Compute the scaling matrix + openglframework::Matrix4 scalingMatrix = openglframework::Matrix4(dimension.x, 0, 0, 0, + 0, dimension.y, 0, 0, + 0, 0, dimension.z, 0, + 0, 0, 0, 1); + + transformMatrix = transformMatrix * scalingMatrix; + + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", transformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the vertex color + openglframework::Vector4 colorVec(color.r, color.g, color.b, color.a); + shader.setVector4Uniform("vertexColor", colorVec, false); + + // Bind the VAO + mVAO.bind(); + + mVBOVertices.bind(); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); + + mVBONormals.bind(); + + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + + // Draw the geometry + glDrawElements(GL_LINES, 12 * 2, GL_UNSIGNED_INT, (char*)nullptr); + + glDisableVertexAttribArray(vertexPositionLoc); + if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc); + + mVBONormals.unbind(); + mVBOVertices.unbind(); + + // Unbind the VAO + mVAO.unbind(); + + // Unbind the shader + shader.unbind(); + + // Disable wireframe mode + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); +} + +// Create the Vertex Buffer Objects used to render to box with OpenGL. +/// We create two VBOs (one for vertices and one for indices) +void AABB::createVBOAndVAO() { + + std::vector vertices; + std::vector normals; + std::vector indices; + + // Vertices + vertices.push_back(openglframework::Vector3(-0.5, -0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, -0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, 0.5, 0.5)); + vertices.push_back(openglframework::Vector3(-0.5, 0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, -0.5, -0.5)); + vertices.push_back(openglframework::Vector3(0.5, 0.5, -0.5)); + vertices.push_back(openglframework::Vector3(-0.5, 0.5, -0.5)); + vertices.push_back(openglframework::Vector3(-0.5, -0.5, -0.5)); + + // Normals + normals.push_back(openglframework::Vector3(1, -1, 1)); + normals.push_back(openglframework::Vector3(1, 1, 1)); + normals.push_back(openglframework::Vector3(-1, 1, 1)); + normals.push_back(openglframework::Vector3(-1, -1, 1)); + normals.push_back(openglframework::Vector3(1, -1, -1)); + normals.push_back(openglframework::Vector3(1, 1, -1)); + normals.push_back(openglframework::Vector3(-1, 1, -1)); + normals.push_back(openglframework::Vector3(-1, -1, -1)); + + // Indices + indices.push_back(0); indices.push_back(1); indices.push_back(1); indices.push_back(2); + indices.push_back(2); indices.push_back(3); indices.push_back(3); indices.push_back(0); + + indices.push_back(4); indices.push_back(5); indices.push_back(5); indices.push_back(6); + indices.push_back(6); indices.push_back(7); indices.push_back(7); indices.push_back(4); + + indices.push_back(1); indices.push_back(4); indices.push_back(2); indices.push_back(5); + indices.push_back(0); indices.push_back(7); indices.push_back(3); indices.push_back(6); + + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + GLsizei sizeVertices = static_cast(vertices.size() * sizeof(openglframework::Vector3)); + mVBOVertices.copyDataIntoVBO(sizeVertices, &(vertices[0]), GL_STATIC_DRAW); + mVBOVertices.unbind(); + + // Create the VBO for the normals data + mVBONormals.create(); + mVBONormals.bind(); + GLsizei sizeNormals = static_cast(normals.size() * sizeof(openglframework::Vector3)); + mVBONormals.copyDataIntoVBO(sizeNormals, &(normals[0]), GL_STATIC_DRAW); + mVBONormals.unbind(); + + // Create th VBO for the indices data + mVBOIndices.create(); + mVBOIndices.bind(); + GLsizei sizeIndices = static_cast(indices.size() * sizeof(unsigned int)); + mVBOIndices.copyDataIntoVBO(sizeIndices, &(indices[0]), GL_STATIC_DRAW); + mVBOIndices.unbind(); + + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); + + // Bind the VBO of vertices + mVBOVertices.bind(); + + // Bind the VBO of normals + mVBONormals.bind(); + + // Bind the VBO of indices + mVBOIndices.bind(); + + // Unbind the VAO + mVAO.unbind(); +} diff --git a/testbed/common/AABB.h b/testbed/common/AABB.h new file mode 100644 index 00000000..e73326b5 --- /dev/null +++ b/testbed/common/AABB.h @@ -0,0 +1,79 @@ +/******************************************************************************** + * ReactPhysics3D physics library, http://www.reactphysics3d.com * + * Copyright (c) 2010-2016 Daniel Chappuis * + ********************************************************************************* + * * + * This software is provided 'as-is', without any express or implied warranty. * + * In no event will the authors be held liable for any damages arising from the * + * use of this software. * + * * + * Permission is granted to anyone to use this software for any purpose, * + * including commercial applications, and to alter it and redistribute it * + * freely, subject to the following restrictions: * + * * + * 1. The origin of this software must not be misrepresented; you must not claim * + * that you wrote the original software. If you use this software in a * + * product, an acknowledgment in the product documentation would be * + * appreciated but is not required. * + * * + * 2. Altered source versions must be plainly marked as such, and must not be * + * misrepresented as being the original software. * + * * + * 3. This notice may not be removed or altered from any source distribution. * + * * + ********************************************************************************/ + +#ifndef AABB_H +#define AABB_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" + +// Class AABB +class AABB { + + private : + + // -------------------- Attributes -------------------- // + + /// Size of each side of the box + float mSize[3]; + + /// Scaling matrix (applied to a cube to obtain the correct box dimensions) + openglframework::Matrix4 mScalingMatrix; + + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; + + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; + + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; + + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; + + // -------------------- Methods -------------------- // + + /// Create a the VAO and VBOs to render to box with OpenGL + static void createVBOAndVAO(); + + public : + + // -------------------- Methods -------------------- // + + /// Initialize the data to render AABBs + static void init(); + + /// Destroy the data used to render AABBs + static void destroy(); + + /// Render the cube at the correct position and with the correct orientation + static void render(const openglframework::Vector3& position, const openglframework::Vector3& dimension, + openglframework::Color color, openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); +}; + +#endif diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp index 09dc388e..508fa516 100644 --- a/testbed/common/Box.cpp +++ b/testbed/common/Box.cpp @@ -38,8 +38,8 @@ openglframework::VertexArrayObject Box::mVAO; int Box::totalNbBoxes = 0; // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) +Box::Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "cube.obj") { // Initialize the size of the box @@ -53,23 +53,16 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p 0, 0, mSize[2], 0, 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + //mBoxShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add the collision shape to the body mProxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity()); @@ -87,8 +80,8 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p } // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) +Box::Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld* world, + const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "cube.obj") { // Load the mesh from a file @@ -108,23 +101,15 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p 0, 0, mSize[2], 0, 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform); // Add the collision shape to the body mProxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass); @@ -158,12 +143,7 @@ Box::~Box() { } // Render the cube at the correct position and with the correct orientation -void Box::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } +void Box::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -217,10 +197,6 @@ void Box::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render to box with OpenGL. diff --git a/testbed/common/Box.h b/testbed/common/Box.h index bd7a6024..42d234f9 100644 --- a/testbed/common/Box.h +++ b/testbed/common/Box.h @@ -75,24 +75,22 @@ class Box : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); + Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath); + Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath); /// Destructor ~Box(); /// Render the cube at the correct position and with the correct orientation - virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp index 413d6eb2..b48baf69 100644 --- a/testbed/common/Capsule.cpp +++ b/testbed/common/Capsule.cpp @@ -34,9 +34,7 @@ openglframework::VertexArrayObject Capsule::mVAO; int Capsule::totalNbCapsules = 0; // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) +Capsule::Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix @@ -45,23 +43,16 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos 0, 0, mRadius, 0, 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); + //mCapsuleShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity()); @@ -77,8 +68,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos } // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, +Capsule::Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { @@ -88,21 +78,15 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos 0, 0, mRadius, 0, 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); + mPreviousTransform = rp3d::Transform::identity(); // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass); @@ -140,11 +124,7 @@ Capsule::~Capsule() { // Render the sphere at the correct position and with the correct orientation void Capsule::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -198,10 +178,6 @@ void Capsule::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h index 5c99120b..d0e74436 100644 --- a/testbed/common/Capsule.h +++ b/testbed/common/Capsule.h @@ -82,25 +82,23 @@ class Capsule : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); + Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, + Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor ~Capsule(); /// Render the sphere at the correct position and with the correct orientation - virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index c7e162a3..f6b04b9c 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -27,16 +27,11 @@ #include "ConcaveMesh.h" // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) +ConcaveMesh::ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath) : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -58,15 +53,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, // do not forget to delete it at the end mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity()); @@ -78,16 +68,11 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, } // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) +ConcaveMesh::ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -109,13 +94,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, // do not forget to delete it at the end mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass); @@ -151,11 +133,7 @@ ConcaveMesh::~ConcaveMesh() { // Render the sphere at the correct position and with the correct orientation void ConcaveMesh::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -209,10 +187,6 @@ void ConcaveMesh::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h index afda3d45..17755352 100644 --- a/testbed/common/ConcaveMesh.h +++ b/testbed/common/ConcaveMesh.h @@ -76,25 +76,23 @@ class ConcaveMesh : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConcaveMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConcaveMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor ~ConcaveMesh(); /// Render the mesh at the correct position and with the correct orientation void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index b3cb3b95..a59c2f3d 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -27,16 +27,11 @@ #include "ConvexMesh.h" // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) +ConvexMesh::ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath) : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -64,15 +59,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, // do not forget to delete it at the end mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity()); @@ -84,16 +74,11 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, } // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) +ConvexMesh::ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -121,13 +106,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, // not forget to delete it at the end mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass); @@ -161,11 +143,7 @@ ConvexMesh::~ConvexMesh() { // Render the sphere at the correct position and with the correct orientation void ConvexMesh::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -219,10 +197,6 @@ void ConvexMesh::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index 1b55611b..865921ca 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -79,24 +79,22 @@ class ConvexMesh : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConvexMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConvexMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor ~ConvexMesh(); /// Render the mesh at the correct position and with the correct orientation - virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp index 02afe091..f90f0cc9 100644 --- a/testbed/common/Dumbbell.cpp +++ b/testbed/common/Dumbbell.cpp @@ -34,8 +34,7 @@ openglframework::VertexArrayObject Dumbbell::mVAO; int Dumbbell::totalNbDumbbells = 0; // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) +Dumbbell::Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "dumbbell.obj") { // Identity scaling matrix @@ -43,9 +42,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() @@ -61,13 +57,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, const rp3d::decimal massCylinder = rp3d::decimal(1.0); mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); - - mPreviousTransform = transformBody; + mPreviousTransform = rp3d::Transform::identity(); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); @@ -79,7 +69,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); // Create a rigid body corresponding to the dumbbell in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere); @@ -99,8 +89,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, } // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) +Dumbbell::Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "dumbbell.obj"){ // Identity scaling matrix @@ -108,9 +97,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() @@ -124,12 +110,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, const rp3d::decimal heightCapsule = rp3d::decimal(7.0); mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); - // Initial transform of the first sphere collision shape of the dumbbell (in local-space) rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); @@ -139,8 +119,10 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Initial transform of the cylinder collision shape of the dumbell (in local-space) rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + mPreviousTransform = rp3d::Transform::identity(); + // Create a rigid body corresponding to the dumbbell in the dynamics world - mBody = world->createCollisionBody(transformBody); + mBody = world->createCollisionBody(mPreviousTransform); // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1); @@ -179,11 +161,7 @@ Dumbbell::~Dumbbell() { // Render the sphere at the correct position and with the correct orientation void Dumbbell::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -237,10 +215,6 @@ void Dumbbell::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index aa6bded1..135beabc 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -82,24 +82,22 @@ class Dumbbell : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath); + Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, - const std::string& meshFolderPath); + Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Destructor ~Dumbbell(); /// Render the sphere at the correct position and with the correct orientation - virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp index 377cab07..e979936d 100644 --- a/testbed/common/HeightField.cpp +++ b/testbed/common/HeightField.cpp @@ -28,15 +28,11 @@ #include "PerlinNoise.h" // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) +HeightField::HeightField(rp3d::CollisionWorld* world) : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -51,15 +47,10 @@ HeightField::HeightField(const openglframework::Vector3 &position, mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity()); @@ -71,15 +62,11 @@ HeightField::HeightField(const openglframework::Vector3 &position, } // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld) +HeightField::HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld) : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -94,13 +81,10 @@ HeightField::HeightField(const openglframework::Vector3 &position, float mass, mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass); @@ -131,11 +115,7 @@ HeightField::~HeightField() { // Render the sphere at the correct position and with the correct orientation void HeightField::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) { - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -189,10 +169,6 @@ void HeightField::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); - - if (wireframe) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Compute the heights of the height field diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h index acb39163..1ff390ee 100644 --- a/testbed/common/HeightField.h +++ b/testbed/common/HeightField.h @@ -89,25 +89,23 @@ class HeightField : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - HeightField(const openglframework::Vector3& position, - rp3d::CollisionWorld* world); + HeightField(rp3d::CollisionWorld* world); /// Constructor - HeightField(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld); + HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld); /// Destructor ~HeightField(); /// Render the mesh at the correct position and with the correct orientation void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix, bool wireframe); + const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index 7fcc612d..e1714048 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -63,7 +63,7 @@ class PhysicsObject : public openglframework::Mesh { virtual void updateTransform(float interpolationFactor)=0; /// Render the sphere at the correct position and with the correct orientation - virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe)=0; + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix)=0; /// Set the color of the box void setColor(const openglframework::Color& color); diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp index 9eaf62c2..f7c53579 100644 --- a/testbed/common/Sphere.cpp +++ b/testbed/common/Sphere.cpp @@ -34,8 +34,7 @@ openglframework::VertexArrayObject Sphere::mVAO; int Sphere::totalNbSpheres = 0; // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, +Sphere::Sphere(float radius, rp3d::CollisionWorld* world, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { @@ -45,23 +44,16 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, 0, 0, mRadius, 0, 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCollisionShape = new rp3d::SphereShape(mRadius); + //mCollisionShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = mBody->addCollisionShape(mCollisionShape, rp3d::Transform::identity()); @@ -77,8 +69,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, } // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* world, +Sphere::Sphere(float radius, float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { @@ -88,21 +79,15 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, 0, 0, mRadius, 0, 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCollisionShape = new rp3d::SphereShape(mRadius); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = body->addCollisionShape(mCollisionShape, rp3d::Transform::identity(), mass); @@ -138,11 +123,7 @@ Sphere::~Sphere() { } // Render the sphere at the correct position and with the correct orientation -void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireFrame) { - - if (wireFrame) { - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - } +void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -196,10 +177,6 @@ void Sphere::render(openglframework::Shader& shader, const openglframework::Matr // Unbind the shader shader.unbind(); - - if (wireFrame) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - } } // Create the Vertex Buffer Objects used to render with OpenGL. diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h index 000cc776..fe2b3f83 100644 --- a/testbed/common/Sphere.h +++ b/testbed/common/Sphere.h @@ -79,24 +79,22 @@ class Sphere : public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); + Sphere(float radius, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); + Sphere(float radius, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor ~Sphere(); /// Render the sphere at the correct position and with the correct orientation - void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override; + void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 3b186943..f00dbdba 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace collisiondetectionscene; // Constructor -CollisionDetectionScene::CollisionDetectionScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), +CollisionDetectionScene::CollisionDetectionScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), mContactManager(mPhongShader, mMeshFolderPath), mAreNormalsDisplayed(false) { @@ -47,61 +47,57 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) setScenePosition(center, SCENE_RADIUS); // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); + mPhysicsWorld = new rp3d::CollisionWorld(); // ---------- Sphere 1 ---------- // - openglframework::Vector3 position1(12, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere1 = new Sphere(4, position1, mCollisionWorld, mMeshFolderPath); + mSphere1 = new Sphere(4, mPhysicsWorld, mMeshFolderPath); mAllShapes.push_back(mSphere1); // Set the color mSphere1->setColor(mGreyColorDemo); mSphere1->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mSphere1); + //mSphere1->setScaling(0.5f); + mPhysicsObjects.push_back(mSphere1); // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(12, 8, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath); + mSphere2 = new Sphere(2, mPhysicsWorld, mMeshFolderPath); mAllShapes.push_back(mSphere2); // Set the color mSphere2->setColor(mGreyColorDemo); mSphere2->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mSphere2); + mPhysicsObjects.push_back(mSphere2); - // ---------- Capsule 1 ---------- // - openglframework::Vector3 position3(-6, 7, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); - mAllShapes.push_back(mCapsule1); - - // Set the color - mCapsule1->setColor(mGreyColorDemo); - mCapsule1->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mCapsule1); - - // ---------- Capsule 2 ---------- // - openglframework::Vector3 position4(11, -8, 0); + // ---------- Capsule 1 ---------- // // Create a cylinder and a corresponding collision body in the dynamics world - mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath); + mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule1); + + // Set the color + mCapsule1->setColor(mGreyColorDemo); + mCapsule1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule1); + + // ---------- Capsule 2 ---------- // + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); mAllShapes.push_back(mCapsule2); // Set the color mCapsule2->setColor(mGreyColorDemo); mCapsule2->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mCapsule2); + mPhysicsObjects.push_back(mCapsule2); - // ---------- Box 1 ---------- // - openglframework::Vector3 position5(-4, -7, 0); + // ---------- Box 1 ---------- // // Create a cylinder and a corresponding collision body in the dynamics world - mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); + mBox1 = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath); mAllShapes.push_back(mBox1); // Set the color @@ -110,10 +106,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mPhysicsObjects.push_back(mBox1); // ---------- Box 2 ---------- // - openglframework::Vector3 position6(0, 8, 0); // Create a cylinder and a corresponding collision body in the dynamics world - mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath); + mBox2 = new Box(openglframework::Vector3(3, 2, 5), mPhysicsWorld, mMeshFolderPath); mAllShapes.push_back(mBox2); // Set the color @@ -122,10 +117,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mPhysicsObjects.push_back(mBox2); // ---------- Convex Mesh ---------- // - openglframework::Vector3 position7(-5, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj"); mAllShapes.push_back(mConvexMesh); // Set the color @@ -134,10 +128,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mPhysicsObjects.push_back(mConvexMesh); // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 100, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); mAllShapes.push_back(mConcaveMesh); // Set the color @@ -146,10 +139,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mPhysicsObjects.push_back(mConcaveMesh); // ---------- Heightfield ---------- // - openglframework::Vector3 position9(0, -12, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mHeightField = new HeightField(position9, mCollisionWorld); + mHeightField = new HeightField(mPhysicsWorld); // Set the color mHeightField->setColor(mGreyColorDemo); @@ -162,76 +154,59 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) // Reset the scene void CollisionDetectionScene::reset() { + mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(12, 0, 0), rp3d::Quaternion::identity())); + mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(12, 8, 0), rp3d::Quaternion::identity())); + mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-6, 7, 0), rp3d::Quaternion::identity())); + mCapsule2->setTransform(rp3d::Transform(rp3d::Vector3(11, -8, 0), rp3d::Quaternion::identity())); + mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity())); + mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity())); + mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity())); + mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 100, 0), rp3d::Quaternion::identity())); + mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity())); } // Destructor CollisionDetectionScene::~CollisionDetectionScene() { // Destroy the box rigid body from the dynamics world - //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + //mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody()); //delete mBox; // Destroy the spheres - mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mSphere1->getCollisionBody()); delete mSphere1; - mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mSphere2->getCollisionBody()); delete mSphere2; - mCollisionWorld->destroyCollisionBody(mCapsule1->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mCapsule1->getCollisionBody()); delete mCapsule1; - mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); delete mCapsule2; - mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mBox1->getCollisionBody()); delete mBox1; - mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mBox2->getCollisionBody()); delete mBox2; - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); delete mConvexMesh; - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); delete mConcaveMesh; - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody()); delete mHeightField; - /* - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); - delete mCone; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); - - // Destroy the sphere - delete mCylinder; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); - - // Destroy the sphere - delete mCapsule; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); - - // Destroy the dumbbell - delete mDumbbell; - - - */ - mContactManager.resetPoints(); // Destroy the static data for the visual contact points VisualContactPoint::destroyStaticData(); // Destroy the collision world - delete mCollisionWorld; + delete mPhysicsWorld; } // Take a step for the simulation @@ -239,14 +214,14 @@ void CollisionDetectionScene::update() { mContactManager.resetPoints(); - mCollisionWorld->testCollision(&mContactManager); + mPhysicsWorld->testCollision(&mContactManager); SceneDemo::update(); } void CollisionDetectionScene::selectNextShape() { - int previousIndex = mSelectedShapeIndex; + uint previousIndex = mSelectedShapeIndex; mSelectedShapeIndex++; if (mSelectedShapeIndex >= mAllShapes.size()) { mSelectedShapeIndex = 0; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index 8473ae73..4bea0c0b 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -152,10 +152,7 @@ class CollisionDetectionScene : public SceneDemo { std::vector mAllShapes; - int mSelectedShapeIndex; - - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; + uint mSelectedShapeIndex; /// Select the next shape void selectNextShape(); @@ -165,7 +162,7 @@ class CollisionDetectionScene : public SceneDemo { // -------------------- Methods -------------------- // /// Constructor - CollisionDetectionScene(const std::string& name); + CollisionDetectionScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~CollisionDetectionScene() override; diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index e63f39d4..a6fd6662 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace collisionshapesscene; // Constructor -CollisionShapesScene::CollisionShapesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { std::string meshFolderPath("meshes/"); @@ -43,23 +43,15 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, -9.81, 0); + rp3d::Vector3 gravity(0, -9.81f, 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - - float radius = 3.0f; + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -77,14 +69,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Create all the boxes of the scene for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -102,18 +88,11 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Create all the spheres of the scene for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + sphere->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); // Set the box color sphere->setColor(mDemoColors[i % mNbDemoColors]); @@ -131,17 +110,11 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Create all the capsules of the scene for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + capsule->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08f)); // Set the box color capsule->setColor(mDemoColors[i % mNbDemoColors]); @@ -159,14 +132,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Create all the convex meshes of the scene for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -183,8 +150,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // ---------- Create the floor --------- - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); mPhysicsObjects.push_back(mFloor); // Set the box color @@ -198,39 +164,16 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - // ---------- Create the triangular mesh ---------- // - - /* - // Position - openglframework::Vector3 position(0, 0, 0); - rp3d::decimal mass = 1.0; - - // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath); - - // Set the mesh as beeing static - mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); - - // Set the box color - mConcaveMesh->setColor(mDemoColors[0]); - mConcaveMesh->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); - */ - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor @@ -240,126 +183,81 @@ CollisionShapesScene::~CollisionShapesScene() { for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); // Destroy the object delete (*it); } // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void CollisionShapesScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + delete mPhysicsWorld; } /// Reset the scene void CollisionShapesScene::reset() { - float radius = 3.0f; + const float radius = 3.0f; - for (int i=0; isetTransform(transform); + mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the boxes of the scene - for (int i=0; isetTransform(transform); + mBoxes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the spheres of the scene - for (int i=0; isetTransform(transform); + mSpheres[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the capsules of the scene - for (int i=0; isetTransform(transform); + mCapsules[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the convex meshes of the scene - for (int i=0; isetTransform(transform); + mConvexMeshes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + + // ---------- Create the triangular mesh ---------- // + + mFloor->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h index 70e28755..b22d8429 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -45,7 +45,7 @@ const float SCENE_RADIUS = 30.0f; const int NB_BOXES = 5; const int NB_SPHERES = 5; const int NB_CAPSULES = 5; -const int NB_MESHES = 3; +const int NB_MESHES = 4; const int NB_COMPOUND_SHAPES = 3; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; @@ -87,23 +87,16 @@ class CollisionShapesScene : public SceneDemo { /// Box for the floor Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - CollisionShapesScene(const std::string& name); + CollisionShapesScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~CollisionShapesScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Reset the scene virtual void reset() override; @@ -113,7 +106,7 @@ class CollisionShapesScene : public SceneDemo { // Return all the contact points of the scene inline std::vector CollisionShapesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 11474819..d70a02af 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace trianglemeshscene; // Constructor -ConcaveMeshScene::ConcaveMeshScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { std::string meshFolderPath("meshes/"); @@ -46,38 +46,115 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); - // ---------- Create the boxes ----------- // + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + dumbbell->setSleepingColor(mRedColorDemo); - // Position - openglframework::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE); + // Change the material properties of the rigid body + rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); - // Create a sphere and a corresponding rigid in the dynamics world - mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld, mMeshFolderPath); + // Add the mesh the list of dumbbells in the scene + mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); + } - // Set the sphere color - mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]); - mBoxes[i * NB_BOXES_Z + j]->setSleepingColor(mRedColorDemo); + // Create all the boxes of the scene + for (int i = 0; igetRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } + // Create a sphere and a corresponding rigid in the dynamics world + Box* box = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + + // Set the box color + box->setColor(mDemoColors[i % mNbDemoColors]); + box->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = box->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mBoxes.push_back(box); + mPhysicsObjects.push_back(box); + } + + // Create all the spheres of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); + + // Set the box color + sphere->setColor(mDemoColors[i % mNbDemoColors]); + sphere->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mSpheres.push_back(sphere); + mPhysicsObjects.push_back(sphere); + } + + // Create all the capsules of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); + + // Set the box color + capsule->setColor(mDemoColors[i % mNbDemoColors]); + capsule->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = capsule->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the cylinder the list of sphere in the scene + mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); + } + + // Create all the convex meshes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + mesh->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of sphere in the scene + mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); } // ---------- Create the triangular mesh ---------- // // Position - openglframework::Vector3 position(0, 0, 0); rp3d::decimal mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(mass, getDynamicsWorld(), meshFolderPath + "city.obj"); // Set the mesh as beeing static mConcaveMesh->getRigidBody()->setType(rp3d::BodyType::STATIC); @@ -86,106 +163,107 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) mConcaveMesh->setColor(mGreyColorDemo); mConcaveMesh->setSleepingColor(mGreyColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); + // Change the material properties of the rigid body rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + material.setFrictionCoefficient(rp3d::decimal(0.1)); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor ConcaveMeshScene::~ConcaveMeshScene() { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { - // Destroy the boxes - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - delete mBoxes[i]; + // Destroy the corresponding rigid body from the dynamics world + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the object + delete (*it); } - // Destroy the convex mesh - delete mConcaveMesh; - // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void ConcaveMeshScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Update the scene -void ConcaveMeshScene::update() { - - SceneDemo::update(); - - // Update the transform used for the rendering - mConcaveMesh->updateTransform(mInterpolationFactor); - - for (int i=0; iupdateTransform(mInterpolationFactor); - } -} - -// Render the scene in a single pass -void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - for (int i=0; irender(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Unbind the shader - shader.unbind(); + delete getDynamicsWorld(); } // Reset the scene void ConcaveMeshScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); - mConcaveMesh->setTransform(transform); + const float radius = 15.0f; - for (int i=0; isetTransform(boxTransform); - } + mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + // Create all the boxes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the spheres of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the capsules of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the convex meshes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // ---------- Create the triangular mesh ---------- // + + mConcaveMesh->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h index 5943fe6c..a8d566a1 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.h +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h @@ -33,15 +33,34 @@ #include "SceneDemo.h" #include "ConcaveMesh.h" #include "Box.h" +#include "Capsule.h" +#include "Dumbbell.h" +#include "Sphere.h" +#include "ConvexMesh.h" namespace trianglemeshscene { // Constants const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters -const int NB_BOXES_X = 8; -const int NB_BOXES_Z = 8; -const float BOX_SIZE = 3.0f; -const float BOXES_SPACE = 2.0f; +static const int NB_BOXES = 10; +static const int NB_SPHERES = 5; +static const int NB_CAPSULES = 5; +static const int NB_MESHES = 3; +static const int NB_COMPOUND_SHAPES = 3; +const openglframework::Vector3 BOX_SIZE(2, 2, 2); +const float SPHERE_RADIUS = 1.5f; +const float CONE_RADIUS = 2.0f; +const float CONE_HEIGHT = 3.0f; +const float CYLINDER_RADIUS = 1.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; +const float DUMBBELL_HEIGHT = 1.0f; +const float BOX_MASS = 1.0f; +const float CONE_MASS = 1.0f; +const float CYLINDER_MASS = 1.0f; +const float CAPSULE_MASS = 1.0f; +const float MESH_MASS = 1.0f; // Class TriangleMeshScene class ConcaveMeshScene : public SceneDemo { @@ -50,35 +69,31 @@ class ConcaveMeshScene : public SceneDemo { // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES_X * NB_BOXES_Z]; + std::vector mBoxes; + + std::vector mSpheres; + + std::vector mCapsules; + + /// All the convex meshes of the scene + std::vector mConvexMeshes; + + /// All the dumbbell of the scene + std::vector mDumbbells; /// Concave triangles mesh ConcaveMesh* mConcaveMesh; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - ConcaveMeshScene(const std::string& name); + ConcaveMeshScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~ConcaveMeshScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; @@ -88,7 +103,7 @@ class ConcaveMeshScene : public SceneDemo { // Return all the contact points of the scene inline std::vector ConcaveMeshScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp index 3ddb1d61..fa7f4ba5 100755 --- a/testbed/scenes/cubes/CubesScene.cpp +++ b/testbed/scenes/cubes/CubesScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace cubesscene; // Constructor -CubesScene::CubesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +CubesScene::CubesScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { // Compute the radius and the center of the scene openglframework::Vector3 center(0, 5, 0); @@ -44,39 +44,31 @@ CubesScene::CubesScene(const std::string& name) rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); - float radius = 2.0f; + // Create all the cubes of the scene + for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); + cube->setSleepingColor(mRedColorDemo); - // // Create a cube and a corresponding rigid in the dynamics world - // Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld); + // Change the material properties of the rigid body + rp3d::Material& material = cube->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.4)); - // // 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); - //} + // Add the box the list of box in the scene + mBoxes.push_back(cube); + mPhysicsObjects.push_back(cube); + } // ------------------------- FLOOR ----------------------- // // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); mFloor->setColor(mGreyColorDemo); mFloor->setSleepingColor(mGreyColorDemo); @@ -84,37 +76,16 @@ CubesScene::CubesScene(const std::string& name) mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); mPhysicsObjects.push_back(mFloor); - // Change the material properties of the floor rigid body - //rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - //material.setBounciness(rp3d::decimal(0.3)); - - // ------------------------- BOX ----------------------- // - - // Create a cube and a corresponding rigid in the dynamics world - Box* cube = new Box(BOX_SIZE, Vector3(0, 10, 0), BOX_MASS, mDynamicsWorld, mMeshFolderPath); - - // Set the box color - cube->setColor(mDemoColors[0]); - 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); - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor @@ -124,39 +95,20 @@ CubesScene::~CubesScene() { for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); // Destroy the cube delete (*it); } // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); // Destroy the floor delete mFloor; // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void CubesScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + delete getDynamicsWorld(); } // Reset the scene @@ -164,19 +116,21 @@ void CubesScene::reset() { float radius = 2.0f; -// for (int i=0; i::iterator it; + int i = 0; + for (it = mBoxes.begin(); it != mBoxes.end(); ++it) { -// // Position of the cubes -// float angle = i * 30.0f; -// openglframework::Vector3 position(radius * cos(angle), -// 10 + i * (BOX_SIZE.y + 0.3f), -// 0); + // Position of the cubes + float angle = i * 30.0f; + rp3d::Vector3 position(radius * std::cos(angle), + 10 + i * (BOX_SIZE.y + 0.3f), + 0); -// // Initial position and orientation of the rigid body -// rp3d::Vector3 initPosition(position.x, position.y, position.z); -// rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); -// rp3d::Transform transform(initPosition, initOrientation); + (*it)->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); -// mBoxes[i]->resetTransform(transform); -// } + i++; + } + + mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); } diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h index aa4b7073..6d553835 100755 --- a/testbed/scenes/cubes/CubesScene.h +++ b/testbed/scenes/cubes/CubesScene.h @@ -38,7 +38,7 @@ namespace cubesscene { const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters const int NB_CUBES = 30; // Number of boxes in the scene const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters -const openglframework::Vector3 FLOOR_SIZE(5, 5.0, 5); // Floor dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters const float BOX_MASS = 1.0f; // Box mass in kilograms const float FLOOR_MASS = 100.0f; // Floor mass in kilograms @@ -55,23 +55,16 @@ class CubesScene : public SceneDemo { /// Box for the floor Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - CubesScene(const std::string& name); + CubesScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~CubesScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Reset the scene virtual void reset() override; @@ -81,7 +74,7 @@ class CubesScene : public SceneDemo { // Return all the contact points of the scene inline std::vector CubesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 329712e9..238018dd 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -31,7 +31,8 @@ using namespace openglframework; using namespace heightfieldscene; // Constructor -HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { +HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { std::string meshFolderPath("meshes/"); @@ -45,20 +46,12 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - - float radius = 3.0f; + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -76,14 +69,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create all the boxes of the scene for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -101,18 +88,11 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create all the spheres of the scene for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + sphere->getRigidBody()->getMaterial().setRollingResistance(0.08f); // Set the box color sphere->setColor(mDemoColors[i % mNbDemoColors]); @@ -130,17 +110,11 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create all the capsules of the scene for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + capsule->getRigidBody()->getMaterial().setRollingResistance(0.08f); // Set the box color capsule->setColor(mDemoColors[i % mNbDemoColors]); @@ -158,14 +132,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create all the convex meshes of the scene for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -183,11 +151,10 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // ---------- Create the height field ---------- // // Position - openglframework::Vector3 position(0, 0, 0); rp3d::decimal mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world - mHeightField = new HeightField(position, mass, mDynamicsWorld); + mHeightField = new HeightField(mass, getDynamicsWorld()); // Set the mesh as beeing static mHeightField->getRigidBody()->setType(rp3d::BodyType::STATIC); @@ -201,18 +168,18 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Change the material properties of the rigid body rp3d::Material& material = mHeightField->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + material.setFrictionCoefficient(0.1f); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor @@ -222,47 +189,81 @@ HeightFieldScene::~HeightFieldScene() { for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); // Destroy the object delete (*it); } // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void HeightFieldScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); + delete getDynamicsWorld(); } // Reset the scene void HeightFieldScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - mHeightField->setTransform(transform); + const float radius = 3.0f; - float heightFieldWidth = 10.0f; - float stepDist = heightFieldWidth / (NB_BOXES + 1); - for (int i=0; isetTransform(boxTransform); + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + + // Create all the boxes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the spheres of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the capsules of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the convex meshes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // ---------- Create the triangular mesh ---------- // + + mHeightField->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h index 7306ae2c..2a3e68f5 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.h +++ b/testbed/scenes/heightfield/HeightFieldScene.h @@ -44,7 +44,7 @@ const float SCENE_RADIUS = 50.0f; static const int NB_BOXES = 10; static const int NB_SPHERES = 5; static const int NB_CAPSULES = 5; -static const int NB_MESHES = 3; +static const int NB_MESHES = 4; static const int NB_COMPOUND_SHAPES = 3; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; @@ -86,23 +86,16 @@ class HeightFieldScene : public SceneDemo { /// Height field HeightField* mHeightField; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - HeightFieldScene(const std::string& name); + HeightFieldScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~HeightFieldScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Reset the scene virtual void reset() override ; @@ -112,7 +105,7 @@ class HeightFieldScene : public SceneDemo { // Return all the contact points of the scene inline std::vector HeightFieldScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index 282f7e51..3252e90e 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -32,8 +32,8 @@ using namespace openglframework; using namespace jointsscene; // Constructor -JointsScene::JointsScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +JointsScene::JointsScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { // Compute the radius and the center of the scene openglframework::Vector3 center(0, 5, 0); @@ -45,7 +45,7 @@ JointsScene::JointsScene(const std::string& name) rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); // Create the Ball-and-Socket joint createBallAndSocketJoints(); @@ -63,37 +63,37 @@ JointsScene::JointsScene(const std::string& name) createFloor(); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor JointsScene::~JointsScene() { // Destroy the joints - mDynamicsWorld->destroyJoint(mSliderJoint); - mDynamicsWorld->destroyJoint(mPropellerHingeJoint); - mDynamicsWorld->destroyJoint(mFixedJoint1); - mDynamicsWorld->destroyJoint(mFixedJoint2); + getDynamicsWorld()->destroyJoint(mSliderJoint); + getDynamicsWorld()->destroyJoint(mPropellerHingeJoint); + getDynamicsWorld()->destroyJoint(mFixedJoint1); + getDynamicsWorld()->destroyJoint(mFixedJoint2); for (int i=0; idestroyJoint(mBallAndSocketJoints[i]); + getDynamicsWorld()->destroyJoint(mBallAndSocketJoints[i]); } // Destroy all the rigid bodies of the scene - mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mSliderJointTopBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mPropellerBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFixedJointBox1->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFixedJointBox2->getRigidBody()); for (int i=0; idestroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); } delete mSliderJointBottomBox; @@ -106,58 +106,21 @@ JointsScene::~JointsScene() { } // Destroy the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); delete mFloor; // Destroy the dynamics world - delete mDynamicsWorld; + delete getDynamicsWorld(); } // Update the physics world (take a simulation step) void JointsScene::updatePhysics() { - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Update the motor speed of the Slider Joint (to move up and down) - long double motorSpeed = 2 * cos(mEngineSettings.elapsedTime * 1.5); + double motorSpeed = 2.0 * std::cos(static_cast(mEngineSettings.elapsedTime) * 1.5); mSliderJoint->setMotorSpeed(rp3d::decimal(motorSpeed)); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Render the scene -void JointsScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the boxes - mSliderJointBottomBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - mSliderJointTopBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - mPropellerBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - mFixedJointBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - mFixedJointBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - for (int i=0; irender(shader, worldToCameraMatrix, mIsWireframeEnabled); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled); - - // Unbind the shader - shader.unbind(); + SceneDemo::updatePhysics(); } // Reset the scene @@ -236,15 +199,16 @@ void JointsScene::createBallAndSocketJoints() { // --------------- Create the boxes --------------- // - openglframework::Vector3 positionBox(0, 15, 5); + rp3d::Vector3 positionBox(0, 15, 5); openglframework::Vector3 boxDimension(1, 1, 1); const float boxMass = 0.5f; for (int i=0; isetTransform(rp3d::Transform(positionBox, rp3d::Quaternion::identity())); // Set the box color mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]); @@ -281,7 +245,7 @@ void JointsScene::createBallAndSocketJoints() { // Create the joint in the dynamics world mBallAndSocketJoints[i] = dynamic_cast( - mDynamicsWorld->createJoint(jointInfo)); + getDynamicsWorld()->createJoint(jointInfo)); } } @@ -291,11 +255,12 @@ void JointsScene::createSliderJoint() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); + rp3d::Vector3 positionBox1(0, 2.1f, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box1Dimension(2, 4, 2); - mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); + mSliderJointBottomBox = new Box(box1Dimension , BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mSliderJointBottomBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mSliderJointBottomBox->setColor(mBlueColorDemo); @@ -312,11 +277,12 @@ void JointsScene::createSliderJoint() { // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); + rp3d::Vector3 positionBox2(0, 4.2f, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); - mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld, mMeshFolderPath); + mSliderJointTopBox = new Box(box2Dimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mSliderJointTopBox->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity())); // Set the box color mSliderJointTopBox->setColor(mOrangeColorDemo); @@ -344,7 +310,7 @@ void JointsScene::createSliderJoint() { jointInfo.isCollisionEnabled = false; // Create the joint in the dynamics world - mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + mSliderJoint = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo)); } /// Create the boxes and joint for the Hinge joint example @@ -353,11 +319,12 @@ void JointsScene::createPropellerHingeJoint() { // --------------- Create the propeller box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 7, 0); + rp3d::Vector3 positionBox1(0, 7, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(10, 1, 1); - mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); + mPropellerBox = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mPropellerBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mPropellerBox->setColor(mYellowColorDemo); @@ -384,7 +351,7 @@ void JointsScene::createPropellerHingeJoint() { jointInfo.isCollisionEnabled = false; // Create the joint in the dynamics world - mPropellerHingeJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + mPropellerHingeJoint = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo)); } /// Create the boxes and joints for the fixed joints @@ -393,11 +360,12 @@ void JointsScene::createFixedJoints() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(5, 7, 0); + rp3d::Vector3 positionBox1(5, 7, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); - mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); + mFixedJointBox1 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mFixedJointBox1->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mFixedJointBox1->setColor(mPinkColorDemo); @@ -411,10 +379,11 @@ void JointsScene::createFixedJoints() { // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(-5, 7, 0); + rp3d::Vector3 positionBox2(-5, 7, 0); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld, mMeshFolderPath); + mFixedJointBox2 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mFixedJointBox2->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity())); // Set the box color mFixedJointBox2->setColor(mBlueColorDemo); @@ -435,7 +404,7 @@ void JointsScene::createFixedJoints() { jointInfo1.isCollisionEnabled = false; // Create the joint in the dynamics world - mFixedJoint1 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo1)); + mFixedJoint1 = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo1)); // --------------- Create the second fixed joint --------------- // @@ -446,15 +415,15 @@ void JointsScene::createFixedJoints() { jointInfo2.isCollisionEnabled = false; // Create the joint in the dynamics world - mFixedJoint2 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo2)); + mFixedJoint2 = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo2)); } // Create the floor void JointsScene::createFloor() { // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath); + rp3d::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); // Set the box color mFloor->setColor(mGreyColorDemo); diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h index 16ccfe8c..dad9af38 100644 --- a/testbed/scenes/joints/JointsScene.h +++ b/testbed/scenes/joints/JointsScene.h @@ -92,9 +92,6 @@ class JointsScene : public SceneDemo { /// Box for the floor Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - // -------------------- Methods -------------------- // /// Create the boxes and joints for the Ball-and-Socket joint example @@ -117,7 +114,7 @@ class JointsScene : public SceneDemo { // -------------------- Methods -------------------- // /// Constructor - JointsScene(const std::string& name); + JointsScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~JointsScene() override ; @@ -126,10 +123,6 @@ class JointsScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; @@ -139,7 +132,7 @@ class JointsScene : public SceneDemo { // Return all the contact points of the scene inline std::vector JointsScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index 2ded75d6..bf806183 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace raycastscene; // Constructor -RaycastScene::RaycastScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), +RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1), mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { @@ -45,13 +45,12 @@ RaycastScene::RaycastScene(const std::string& name) setScenePosition(center, SCENE_RADIUS); // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); + mPhysicsWorld = new rp3d::CollisionWorld(); // ---------- Dumbbell ---------- // - openglframework::Vector3 position1(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath); + mDumbbell = new Dumbbell(mPhysicsWorld, mMeshFolderPath); // Set the box color mDumbbell->setColor(mGreyColorDemo); @@ -59,10 +58,9 @@ RaycastScene::RaycastScene(const std::string& name) mPhysicsObjects.push_back(mDumbbell); // ---------- Box ---------- // - openglframework::Vector3 position2(0, 0, 0); // Create a box and a corresponding collision body in the dynamics world - mBox = new Box(BOX_SIZE, position2, mCollisionWorld, mMeshFolderPath); + mBox = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath); mBox->getCollisionBody()->setIsActive(false); // Set the box color @@ -71,11 +69,9 @@ RaycastScene::RaycastScene(const std::string& name) mPhysicsObjects.push_back(mBox); // ---------- Sphere ---------- // - openglframework::Vector3 position3(0, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld, - mMeshFolderPath); + mSphere = new Sphere(SPHERE_RADIUS, mPhysicsWorld, mMeshFolderPath); // Set the color mSphere->setColor(mGreyColorDemo); @@ -86,8 +82,7 @@ RaycastScene::RaycastScene(const std::string& name) openglframework::Vector3 position6(0, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world - mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , - mCollisionWorld, mMeshFolderPath); + mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); // Set the color mCapsule->setColor(mGreyColorDemo); @@ -95,10 +90,9 @@ RaycastScene::RaycastScene(const std::string& name) mPhysicsObjects.push_back(mCapsule); // ---------- Convex Mesh ---------- // - openglframework::Vector3 position7(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj"); // Set the color mConvexMesh->setColor(mGreyColorDemo); @@ -106,10 +100,9 @@ RaycastScene::RaycastScene(const std::string& name) mPhysicsObjects.push_back(mConvexMesh); // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); // Set the color mConcaveMesh->setColor(mGreyColorDemo); @@ -117,10 +110,9 @@ RaycastScene::RaycastScene(const std::string& name) mPhysicsObjects.push_back(mConcaveMesh); // ---------- Heightfield ---------- // - openglframework::Vector3 position9(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mHeightField = new HeightField(position9, mCollisionWorld); + mHeightField = new HeightField(mPhysicsWorld); // Set the color mHeightField->setColor(mGreyColorDemo); @@ -139,7 +131,7 @@ RaycastScene::RaycastScene(const std::string& name) // Create the raycast lines void RaycastScene::createLines() { - int nbRaysOneDimension = std::sqrt(float(NB_RAYS)); + int nbRaysOneDimension = static_cast(std::sqrt(float(NB_RAYS))); for (int i=0; i::iterator it; + for (it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + (*it)->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); + } } // Destructor RaycastScene::~RaycastScene() { // Destroy the box rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody()); delete mBox; // Destroy the sphere - mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mSphere->getCollisionBody()); delete mSphere; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mCapsule->getCollisionBody()); // Destroy the sphere delete mCapsule; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); // Destroy the convex mesh delete mConvexMesh; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); // Destroy the dumbbell delete mDumbbell; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); // Destroy the convex mesh delete mConcaveMesh; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody()); // Destroy the convex mesh delete mHeightField; @@ -250,7 +246,7 @@ RaycastScene::~RaycastScene() { VisualContactPoint::destroyStaticData(); // Destroy the collision world - delete mCollisionWorld; + delete mPhysicsWorld; // Destroy the lines for (std::vector::iterator it = mLines.begin(); it != mLines.end(); @@ -284,7 +280,7 @@ void RaycastScene::update() { // Perform a raycast query on the physics world by passing a raycast // callback class in argument. - mCollisionWorld->raycast(ray, &mRaycastManager); + mPhysicsWorld->raycast(ray, &mRaycastManager); } SceneDemo::update(); @@ -307,7 +303,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg mColorShader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); // Set the vertex color - openglframework::Vector4 color(1, 0.55, 0, 1); + openglframework::Vector4 color(1, 0.55f, 0, 1); mColorShader.setVector4Uniform("vertexColor", color, false); // Get the location of shader attribute variables @@ -334,7 +330,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg // Render all the physics objects of the scene for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { if ((*it)->getCollisionBody()->isActive()) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + (*it)->render(shader, worldToCameraMatrix); } } diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index 1400fcf7..8024ff1a 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -146,9 +146,6 @@ class RaycastScene : public SceneDemo { ConcaveMesh* mConcaveMesh; HeightField* mHeightField; - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; - /// All the points to render the lines std::vector mLinePoints; @@ -170,7 +167,7 @@ class RaycastScene : public SceneDemo { // -------------------- Methods -------------------- // /// Constructor - RaycastScene(const std::string& name); + RaycastScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~RaycastScene() override; diff --git a/testbed/src/Gui.cpp b/testbed/src/Gui.cpp index cd9f8f4e..6ad24335 100644 --- a/testbed/src/Gui.cpp +++ b/testbed/src/Gui.cpp @@ -388,6 +388,14 @@ void Gui::createSettingsPanel() { mApp->mIsContactPointsDisplayed = value; }); + + // Display/Hide the AABBs + CheckBox* checkboxAABBs = new CheckBox(mRenderingPanel, "AABBs"); + checkboxAABBs->setChecked(mApp->mIsAABBsDisplayed); + checkboxAABBs->setCallback([&](bool value) { + mApp->mIsAABBsDisplayed = value; + }); + // Enabled/Disable VSync CheckBox* checkboxVSync = new CheckBox(mRenderingPanel, "V-Sync"); checkboxVSync->setChecked(mApp->mIsVSyncEnabled); diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp index e872b3bc..b6668649 100644 --- a/testbed/src/Scene.cpp +++ b/testbed/src/Scene.cpp @@ -30,10 +30,10 @@ using namespace openglframework; // Constructor -Scene::Scene(const std::string& name, bool isShadowMappingEnabled) - : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), +Scene::Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled) + : mName(name), mEngineSettings(engineSettings), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled), - mIsContactPointsDisplayed(true), mIsWireframeEnabled(false) { + mIsContactPointsDisplayed(true), mIsAABBsDisplayed(false), mIsWireframeEnabled(false) { } @@ -74,14 +74,14 @@ bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse, if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) { float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth); float y = float(0.5f * mWindowHeight - yMouse) / float(mWindowHeight); - float sinx = sin(PI * x * 0.5f); - float siny = sin(PI * y * 0.5f); + float sinx = std::sin(PI * x * 0.5f); + float siny = std::sin(PI * y * 0.5f); float sinx2siny2 = sinx * sinx + siny * siny; // Compute the point on the sphere spherePoint.x = sinx; spherePoint.y = siny; - spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; + spherePoint.z = (sinx2siny2 < 1.0f) ? std::sqrt(1.0f - sinx2siny2) : 0.0f; return true; } @@ -175,9 +175,9 @@ void Scene::rotate(int xMouse, int yMouse) { float cosAngle = mLastPointOnSphere.dot(newPoint3D); float epsilon = std::numeric_limits::epsilon(); - if (fabs(cosAngle) < 1.0f && axis.length() > epsilon) { + if (std::abs(cosAngle) < 1.0f && axis.length() > epsilon) { axis.normalize(); - float angle = 2.0f * acos(cosAngle); + float angle = 2.0f * std::acos(cosAngle); // Rotate the camera around the center of the scene mCamera.rotateAroundLocalPoint(axis, -angle, mCenterScene); diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index aa29b9e5..e33e9da9 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -28,6 +28,7 @@ // Libraries #include "openglframework.h" +#include "reactphysics3d.h" // Structure ContactPoint struct ContactPoint { @@ -52,8 +53,8 @@ struct EngineSettings { long double elapsedTime; // Elapsed time (in seconds) float timeStep; // Current time step (in seconds) - int nbVelocitySolverIterations; // Nb of velocity solver iterations - int nbPositionSolverIterations; // Nb of position solver iterations + uint nbVelocitySolverIterations; // Nb of velocity solver iterations + uint nbPositionSolverIterations; // Nb of position solver iterations bool isSleepingEnabled; // True if sleeping technique is enabled float timeBeforeSleep; // Time of inactivity before a body sleep float sleepLinearVelocity; // Sleep linear velocity @@ -65,6 +66,23 @@ struct EngineSettings { EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) { } + + /// Return default engine settings + static EngineSettings defaultSettings() { + + EngineSettings defaultSettings; + + defaultSettings.timeStep = 1.0f / 60.0f; + defaultSettings.nbVelocitySolverIterations = rp3d::DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS; + defaultSettings.nbPositionSolverIterations = rp3d::DEFAULT_POSITION_SOLVER_NB_ITERATIONS; + defaultSettings.isSleepingEnabled = rp3d::SLEEPING_ENABLED; + defaultSettings.timeBeforeSleep = rp3d::DEFAULT_TIME_BEFORE_SLEEP; + defaultSettings.sleepLinearVelocity = rp3d::DEFAULT_SLEEP_LINEAR_VELOCITY; + defaultSettings.sleepAngularVelocity = rp3d::DEFAULT_SLEEP_ANGULAR_VELOCITY; + defaultSettings.isGravityEnabled = true; + + return defaultSettings; + } }; // Class Scene @@ -79,7 +97,7 @@ class Scene { std::string mName; /// Physics engine settings - EngineSettings mEngineSettings; + EngineSettings& mEngineSettings; /// Camera openglframework::Camera mCamera; @@ -111,6 +129,9 @@ class Scene { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if the AABBs of the phycis objects are displayed + bool mIsAABBsDisplayed; + /// True if we render shapes in wireframe mode bool mIsWireframeEnabled; @@ -140,7 +161,7 @@ class Scene { // -------------------- Methods -------------------- // /// Constructor - Scene(const std::string& name, bool isShadowMappingEnabled = false); + Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled = false); /// Destructor virtual ~Scene(); @@ -184,12 +205,6 @@ class Scene { /// Return a reference to the camera const openglframework::Camera& getCamera() const; - /// Get the engine settings - EngineSettings getEngineSettings() const; - - /// Set the engine settings - void setEngineSettings(const EngineSettings& settings); - /// Set the interpolation factor void setInterpolationFactor(float interpolationFactor); @@ -205,6 +220,9 @@ class Scene { /// Display/Hide the contact points void virtual setIsContactPointsDisplayed(bool display); + /// Display/Hide the AABBs + void setIsAABBsDisplayed(bool display); + /// Return true if wireframe rendering is enabled bool getIsWireframeEnabled() const; @@ -244,16 +262,6 @@ inline void Scene::setViewport(int x, int y, int width, int height) { mViewportHeight = height; } -// Get the engine settings -inline EngineSettings Scene::getEngineSettings() const { - return mEngineSettings; -} - -// Set the engine settings -inline void Scene::setEngineSettings(const EngineSettings& settings) { - mEngineSettings = settings; -} - // Set the interpolation factor inline void Scene::setInterpolationFactor(float interpolationFactor) { mInterpolationFactor = interpolationFactor; @@ -279,6 +287,11 @@ inline void Scene::setIsContactPointsDisplayed(bool display) { mIsContactPointsDisplayed = display; } +// Display/Hide the AABBs +inline void Scene::setIsAABBsDisplayed(bool display) { + mIsAABBsDisplayed = display; +} + // Return true if wireframe rendering is enabled inline bool Scene::getIsWireframeEnabled() const { return mIsWireframeEnabled; diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index d406bcea..db5d030e 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -26,27 +26,28 @@ // Libraries #include "SceneDemo.h" #include +#include "AABB.h" using namespace openglframework; int SceneDemo::shadowMapTextureLevel = 0; -openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0); -openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9, 0.88, 0.145, 1.0); -openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66, 0.95, 1.0); -openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9, 0.35, 0, 1.0); -openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83, 0.48, 0.64, 1.0); -openglframework::Color SceneDemo::mRedColorDemo = Color(0.95, 0, 0, 1.0); +openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0f); +openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9f, 0.88f, 0.145f, 1.0f); +openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66f, 0.95f, 1.0f); +openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9f, 0.35f, 0, 1.0f); +openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83f, 0.48f, 0.64f, 1.0f); +openglframework::Color SceneDemo::mRedColorDemo = Color(0.95f, 0, 0, 1.0f); int SceneDemo::mNbDemoColors = 4; openglframework::Color SceneDemo::mDemoColors[] = {SceneDemo::mYellowColorDemo, SceneDemo::mBlueColorDemo, SceneDemo::mOrangeColorDemo, SceneDemo::mPinkColorDemo}; // Constructor -SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled) - : Scene(name, isShadowMappingEnabled), mIsShadowMappingInitialized(false), +SceneDemo::SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled) + : Scene(name, settings, isShadowMappingEnabled), mIsShadowMappingInitialized(false), mDepthShader("shaders/depth.vert", "shaders/depth.frag"), mPhongShader("shaders/phong.vert", "shaders/phong.frag"), - mQuadShader("shaders/quad.vert", "shaders/quad.frag"), mColorShader("shaders/color.vert", "shaders/color.frag"), + mQuadShader("shaders/quad.vert", "shaders/quad.frag"), mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") { shadowMapTextureLevel++; @@ -75,6 +76,9 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa createQuadVBO(); + // Init rendering for the AABBs + AABB::init(); + VisualContactPoint::createStaticData(mMeshFolderPath); } @@ -93,6 +97,9 @@ SceneDemo::~SceneDemo() { // Destroy the contact points removeAllContactPoints(); + // Destroy rendering data for the AABB + AABB::destroy(); + VisualContactPoint::destroyStaticData(); } @@ -114,6 +121,23 @@ void SceneDemo::update() { // Can be called several times per frame void SceneDemo::updatePhysics() { + if (getDynamicsWorld() != nullptr) { + + // Update the physics engine parameters + getDynamicsWorld()->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + getDynamicsWorld()->setGravity(gravity); + getDynamicsWorld()->enableSleeping(mEngineSettings.isSleepingEnabled); + getDynamicsWorld()->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + getDynamicsWorld()->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + getDynamicsWorld()->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + getDynamicsWorld()->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + getDynamicsWorld()->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + + // Take a simulation step + getDynamicsWorld()->update(mEngineSettings.timeStep); + } } // Render the scene (in multiple passes for shadow mapping) @@ -209,6 +233,11 @@ void SceneDemo::render() { renderContactPoints(mPhongShader, worldToCameraMatrix); } + // Render the AABBs + if (mIsAABBsDisplayed) { + renderAABBs(worldToCameraMatrix); + } + if (mIsShadowMappingEnabled) mShadowMapTexture.unbind(); mPhongShader.unbind(); @@ -218,16 +247,24 @@ void SceneDemo::render() { // Render the scene in a single pass void SceneDemo::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Bind the shader + if (mIsWireframeEnabled) { + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + } + + // Bind the shader shader.bind(); // Render all the physics objects of the scene for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + (*it)->render(mIsWireframeEnabled ? mColorShader : shader, worldToCameraMatrix); } // Unbind the shader shader.unbind(); + + if (mIsWireframeEnabled) { + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + } } // Create the Shadow map FBO and texture @@ -338,13 +375,38 @@ void SceneDemo::updateContactPoints() { // Render the contact points void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Render all the raycast hit points + // Render all the contact points for (std::vector::iterator it = mContactPoints.begin(); it != mContactPoints.end(); ++it) { (*it)->render(mColorShader, worldToCameraMatrix); } } +// Render the AABBs +void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix) { + + // For each physics object of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + + // For each proxy shape of the object + rp3d::ProxyShape* proxyShape = (*it)->getCollisionBody()->getProxyShapesList(); + while (proxyShape != nullptr) { + + // Get the broad-phase AABB corresponding to the proxy shape + rp3d::AABB aabb = mPhysicsWorld->getWorldAABB(proxyShape); + + openglframework::Vector3 aabbCenter(aabb.getCenter().x, aabb.getCenter().y, aabb.getCenter().z); + openglframework::Vector3 aabbMin(aabb.getMin().x, aabb.getMin().y, aabb.getMin().z); + openglframework::Vector3 aabbMax(aabb.getMax().x, aabb.getMax().y, aabb.getMax().z); + + // Render the AABB + AABB::render(aabbCenter, aabbMax - aabbMin, Color::green(), mColorShader, worldToCameraMatrix); + + proxyShape = proxyShape->getNext(); + } + } +} + void SceneDemo::removeAllContactPoints() { // Destroy all the visual contact points diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h index a8b1941f..359c2374 100644 --- a/testbed/src/SceneDemo.h +++ b/testbed/src/SceneDemo.h @@ -98,6 +98,8 @@ class SceneDemo : public Scene { std::vector mPhysicsObjects; + rp3d::CollisionWorld* mPhysicsWorld; + // -------------------- Methods -------------------- // // Create the Shadow map FBO and texture @@ -116,14 +118,25 @@ class SceneDemo : public Scene { void renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); + + /// Render the AABBs + void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix); + + /// Remove all contact points void removeAllContactPoints(); + /// Return a reference to the dynamics world + rp3d::DynamicsWorld* getDynamicsWorld(); + + /// Return a reference to the dynamics world + const rp3d::DynamicsWorld* getDynamicsWorld() const; + public: // -------------------- Methods -------------------- // /// Constructor - SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled = true); + SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled = true); /// Destructor virtual ~SceneDemo() override; @@ -158,6 +171,16 @@ inline void SceneDemo::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { } } +// Return a reference to the dynamics world +inline rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() { + return dynamic_cast(mPhysicsWorld); +} + +// Return a reference to the dynamics world +inline const rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() const { + return dynamic_cast(mPhysicsWorld); +} + #endif diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index 2b9195e5..d94103ef 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -59,12 +59,14 @@ TestbedApplication::TestbedApplication(bool isFullscreen) mIsMultisamplingActive = true; mWidth = 1280; mHeight = 720; + mEngineSettings = EngineSettings::defaultSettings(); mSinglePhysicsStepEnabled = false; mSinglePhysicsStepDone = false; mWindowToFramebufferRatio = Vector2(1, 1); mIsShadowMappingEnabled = true; mIsVSyncEnabled = false; mIsContactPointsDisplayed = false; + mIsAABBsDisplayed = false; mIsWireframeEnabled = false; init(); @@ -97,39 +99,38 @@ void TestbedApplication::init() { void TestbedApplication::createScenes() { // Cubes scene - CubesScene* cubeScene = new CubesScene("Cubes"); + CubesScene* cubeScene = new CubesScene("Cubes", mEngineSettings); mScenes.push_back(cubeScene); // Joints scene - JointsScene* jointsScene = new JointsScene("Joints"); + JointsScene* jointsScene = new JointsScene("Joints", mEngineSettings); mScenes.push_back(jointsScene); // Collision shapes scene - CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes"); + CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes", mEngineSettings); mScenes.push_back(collisionShapesScene); // Heightfield shape scene - HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield"); + HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield", mEngineSettings); mScenes.push_back(heightFieldScene); // Raycast scene - RaycastScene* raycastScene = new RaycastScene("Raycast"); + RaycastScene* raycastScene = new RaycastScene("Raycast", mEngineSettings); mScenes.push_back(raycastScene); // Collision Detection scene - CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection"); + CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection", mEngineSettings); mScenes.push_back(collisionDetectionScene); // Concave Mesh scene - ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh"); + ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh", mEngineSettings); mScenes.push_back(concaveMeshScene); assert(mScenes.size() > 0); - mCurrentScene = mScenes[5]; - // Get the engine settings from the scene - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = DEFAULT_TIMESTEP; + const int firstSceneIndex = 0; + + switchScene(mScenes[firstSceneIndex]); } // Remove all the scenes @@ -152,9 +153,8 @@ void TestbedApplication::updateSinglePhysicsStep() { // Update the physics of the current scene void TestbedApplication::updatePhysics() { - // Set the engine settings + // Update the elapsed time mEngineSettings.elapsedTime = mTimer.getPhysicsTime(); - mCurrentScene->setEngineSettings(mEngineSettings); if (mTimer.isRunning()) { @@ -202,6 +202,9 @@ void TestbedApplication::update() { // Display/Hide contact points mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed); + // Display/Hide the AABBs + mCurrentScene->setIsAABBsDisplayed(mIsAABBsDisplayed); + // Enable/Disable wireframe mode mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled); @@ -259,11 +262,6 @@ void TestbedApplication::switchScene(Scene* newScene) { mCurrentScene = newScene; - // Get the engine settings of the scene - float currentTimeStep = mEngineSettings.timeStep; - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = currentTimeStep; - // Reset the scene mCurrentScene->reset(); diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h index 8790a1f7..3a9fe071 100644 --- a/testbed/src/TestbedApplication.h +++ b/testbed/src/TestbedApplication.h @@ -38,9 +38,6 @@ using namespace nanogui; // Macro for OpenGL errors #define checkOpenGLErrors() checkOpenGLErrorsInternal(__FILE__,__LINE__) -// Constants -const float DEFAULT_TIMESTEP = 1.0f / 60.0f; - /// Class TestbedApplication class TestbedApplication : public Screen { @@ -109,6 +106,9 @@ class TestbedApplication : public Screen { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if the AABBs of physics objects are displayed + bool mIsAABBsDisplayed; + /// True if the wireframe rendering is enabled bool mIsWireframeEnabled; From 002264a5a14dbcaad67ce5364b4aabd32d29d184 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 Nov 2017 23:01:32 +0100 Subject: [PATCH 073/105] Remove unused files --- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 302 ------------------ .../narrowphase/ConcaveVsConvexAlgorithm.h | 196 ------------ 2 files changed, 498 deletions(-) delete mode 100644 src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp delete mode 100644 src/collision/narrowphase/ConcaveVsConvexAlgorithm.h diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp deleted file mode 100644 index 8a2475a2..00000000 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ -/* -// Libraries -#include "collision/shapes/ConcaveShape.h" -#include "collision/shapes/TriangleShape.h" -#include "ConcaveVsConvexAlgorithm.h" -#include "collision/CollisionDetection.h" -#include "engine/CollisionWorld.h" -#include - -using namespace reactphysics3d; - -// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) -void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) { - - // Create a triangle collision shape - decimal margin = mConcaveShape->getTriangleMargin(); - TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex, margin); - - // Create a narrow phase info for the narrow-phase collision detection - NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; - narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), - triangleShape, mConvexProxyShape->getLocalToWorldTransform(), - mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), - mConcaveProxyShape->getCachedCollisionData()); - narrowPhaseInfoList->next = firstNarrowPhaseInfo; -} - -// Return true and compute a contact info if the two bounding volumes collide -void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - NarrowPhaseCallback* narrowPhaseCallback) { - -// ProxyShape* convexProxyShape; -// ProxyShape* concaveProxyShape; -// const ConvexShape* convexShape; -// const ConcaveShape* concaveShape; - -// // Collision shape 1 is convex, collision shape 2 is concave -// if (shape1Info.collisionShape->isConvex()) { -// convexProxyShape = shape1Info.proxyShape; -// convexShape = static_cast(shape1Info.collisionShape); -// concaveProxyShape = shape2Info.proxyShape; -// concaveShape = static_cast(shape2Info.collisionShape); -// } -// else { // Collision shape 2 is convex, collision shape 1 is concave -// convexProxyShape = shape2Info.proxyShape; -// convexShape = static_cast(shape2Info.collisionShape); -// concaveProxyShape = shape1Info.proxyShape; -// concaveShape = static_cast(shape1Info.collisionShape); -// } - -// // Set the parameters of the callback object -// ConvexVsTriangleCallback convexVsTriangleCallback; -// convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); -// convexVsTriangleCallback.setConvexShape(convexShape); -// convexVsTriangleCallback.setConcaveShape(concaveShape); -// convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); -// convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); - -// // Compute the convex shape AABB in the local-space of the convex shape -// AABB aabb; -// convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - -// // If smooth mesh collision is enabled for the concave mesh -// if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - -// std::vector contactPoints; - -// SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); - -// convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); - -// // Call the convex vs triangle callback for each triangle of the concave shape -// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - -// // Run the smooth mesh collision algorithm -// processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); -// } -// else { - -// convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); - -// // Call the convex vs triangle callback for each triangle of the concave shape -// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); -// } -} - -//// Test collision between a triangle and the convex mesh shape -//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { - -// // Create a triangle collision shape -// decimal margin = mConcaveShape->getTriangleMargin(); -// TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - -// // Select the collision algorithm to use between the triangle and the convex shape -// NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), -// mConvexShape->getType()); - -// // If there is no collision algorithm between those two kinds of shapes -// if (algo == nullptr) return; - -// // Notify the narrow-phase algorithm about the overlapping pair we are going to test -// algo->setCurrentOverlappingPair(mOverlappingPair); - -// // Create the CollisionShapeInfo objects -// CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), -// mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); -// CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, -// mConcaveProxyShape->getLocalToWorldTransform(), -// mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); - -// // Use the collision algorithm to test collision between the triangle and the other convex shape -// algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); -//} - -// Process the concave triangle mesh collision using the smooth mesh collision algorithm described -// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision -// issue with some internal edges. -//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, -// std::vector contactPoints, -// NarrowPhaseCallback* narrowPhaseCallback) { - -// // Set with the triangle vertices already processed to void further contacts with same triangle -// std::unordered_multimap processTriangleVertices; - -// // Sort the list of narrow-phase contacts according to their penetration depth -// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); - -// // For each contact point (from smaller penetration depth to larger) -// std::vector::const_iterator it; -// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - -// const SmoothMeshContactInfo info = *it; -// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - -// // Compute the barycentric coordinates of the point in the triangle -// decimal u, v, w; -// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], -// info.triangleVertices[1], -// info.triangleVertices[2], -// contactPoint, u, v, w); -// int nbZeros = 0; -// bool isUZero = approxEqual(u, 0, 0.0001); -// bool isVZero = approxEqual(v, 0, 0.0001); -// bool isWZero = approxEqual(w, 0, 0.0001); -// if (isUZero) nbZeros++; -// if (isVZero) nbZeros++; -// if (isWZero) nbZeros++; - -// // If it is a vertex contact -// if (nbZeros == 2) { - -// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - -// // Check that this triangle vertex has not been processed yet -// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - -// // Keep the contact as it is and report it -// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); -// } -// } -// else if (nbZeros == 1) { // If it is an edge contact - -// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); -// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); - -// // Check that this triangle edge has not been processed yet -// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && -// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - -// // Keep the contact as it is and report it -// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); -// } - -// } -// else { // If it is a face contact - -// ContactPointInfo newContactInfo(info.contactInfo); - -// ProxyShape* firstShape; -// ProxyShape* secondShape; -// if (info.isFirstShapeTriangle) { -// firstShape = overlappingPair->getShape1(); -// secondShape = overlappingPair->getShape2(); -// } -// else { -// firstShape = overlappingPair->getShape2(); -// secondShape = overlappingPair->getShape1(); -// } - -// // We use the triangle normal as the contact normal -// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; -// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; -// Vector3 localNormal = a.cross(b); -// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; -// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; -// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; -// newContactInfo.normal.normalize(); -// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { -// newContactInfo.normal = -newContactInfo.normal; -// } - -// // We recompute the contact point on the second body with the new normal as described in -// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and -// // Dirk Gregorius) to avoid adding torque -// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); -// if (info.isFirstShapeTriangle) { -// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; -// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; -// } -// else { -// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; -// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; -// } - -// // Report the contact -// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); -// } - -// // Add the three vertices of the triangle to the set of processed -// // triangle vertices -// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); -// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); -// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); -// } -//} - -// Return true if the vertex is in the set of already processed vertices -bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { - - int key = int(vertex.x * vertex.y * vertex.z); - - auto range = processTriangleVertices.equal_range(key); - for (auto it = range.first; it != range.second; ++it) { - if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true; - } - - return false; -} - - -//// Called by a narrow-phase collision algorithm when a new contact has been found -//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, -// const ContactPointInfo& contactInfo) { -// Vector3 triangleVertices[3]; -// bool isFirstShapeTriangle; - -// // If the collision shape 1 is the triangle -// if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) { -// assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE); - -// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); -// triangleVertices[0] = triangleShape->getVertex(0); -// triangleVertices[1] = triangleShape->getVertex(1); -// triangleVertices[2] = triangleShape->getVertex(2); - -// isFirstShapeTriangle = true; -// } -// else { // If the collision shape 2 is the triangle -// assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE); - -// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); -// triangleVertices[0] = triangleShape->getVertex(0); -// triangleVertices[1] = triangleShape->getVertex(1); -// triangleVertices[2] = triangleShape->getVertex(2); - -// isFirstShapeTriangle = false; -// } -// SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); - -// // Add the narrow-phase contact into the list of contact to process for -// // smooth mesh collision -// mContactPoints.push_back(smoothContactInfo); -//} -*/ diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h deleted file mode 100644 index 70cd3dbc..00000000 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ /dev/null @@ -1,196 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ -/* -#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H -#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H - -// Libraries -#include "NarrowPhaseAlgorithm.h" -#include "collision/shapes/ConvexShape.h" -#include "collision/shapes/ConcaveShape.h" -#include "memory/SingleFrameAllocator.h" -#include - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class ConvexVsTriangleCallback -class MiddlePhaseTriangleCallback : public TriangleCallback { - - protected: - - /// Broadphase overlapping pair - OverlappingPair* mOverlappingPair; - - /// Pointer to the concave proxy shape - ProxyShape* mConcaveProxyShape; - - /// Pointer to the convex proxy shape - ProxyShape* mConvexProxyShape; - - /// Pointer to the concave collision shape - const ConcaveShape* mConcaveShape; - - /// Reference to the single-frame memory allocator - Allocator& mAllocator; - - public: - - /// Pointer to the first element of the linked-list of narrow-phase info - NarrowPhaseInfo* narrowPhaseInfoList; - - /// Constructor - MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, - ProxyShape* concaveProxyShape, - ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, - Allocator& allocator) - :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), - mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), - mAllocator(allocator), narrowPhaseInfoList(nullptr) { - - } - - /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) override; -}; - -// Class SmoothMeshContactInfo -struct SmoothMeshContactInfo { - - public: - - ContactManifoldInfo* contactManifoldInfo; - ContactPointInfo* contactInfo; - bool isFirstShapeTriangle; - Vector3 triangleVertices[3]; - bool isUVWZero[3]; - - /// Constructor - SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo, - bool firstShapeTriangle, - const Vector3& trianglePoint1, const Vector3& trianglePoint2, - const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero) - : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) { - - isFirstShapeTriangle = firstShapeTriangle; - - triangleVertices[0] = trianglePoint1; - triangleVertices[1] = trianglePoint2; - triangleVertices[2] = trianglePoint3; - - isUVWZero[0] = isUZero; - isUVWZero[1] = isVZero; - isUVWZero[2] = isWZero; - } - -}; - -struct ContactsDepthCompare { - bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) - { - return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth; - } -}; - -/// Method used to compare two smooth mesh contact info to sort them -//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1, -// const SmoothMeshContactInfo& contact2) { -// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; -//} - -// TODO : Delete this -// Class SmoothCollisionNarrowPhaseCallback -class SmoothCollisionNarrowPhaseCallback { - - private: - - std::vector& mContactPoints; - - - public: - - // Constructor - SmoothCollisionNarrowPhaseCallback(std::vector& contactPoints) - : mContactPoints(contactPoints) { - - } - -}; - -// TODO : Delete this -// Class ConcaveVsConvexAlgorithm -class ConcaveVsConvexAlgorithm { - - protected : - - // -------------------- Attributes -------------------- // - - // -------------------- Methods -------------------- // - - /// Process the concave triangle mesh collision using the smooth mesh collision algorithm - void processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback); - - /// Add a triangle vertex into the set of processed triangles - void addProcessedVertex(std::unordered_multimap& processTriangleVertices, - const Vector3& vertex); - - /// Return true if the vertex is in the set of already processed vertices - bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, - const Vector3& vertex) const; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ConcaveVsConvexAlgorithm() = default; - - /// Destructor - ~ConcaveVsConvexAlgorithm() = default; - - /// Private copy-constructor - ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete; - - /// Private assignment operator - ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - void testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - NarrowPhaseCallback* narrowPhaseCallback); -}; - -// Add a triangle vertex into the set of processed triangles -inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const Vector3& vertex) { - processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex)); -} - -} - -#endif - -*/ From fd427c0337b58422d1a4bd5e575ba10fc29f0968 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 3 Nov 2017 07:11:19 +0100 Subject: [PATCH 074/105] Fix compilation errors because of removed files --- CMakeLists.txt | 2 -- src/collision/narrowphase/DefaultCollisionDispatch.h | 1 - 2 files changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dcc2e4bf..c4cfde6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,8 +83,6 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp" "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h" "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp" - "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" - "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 7b3945d3..779f7048 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -28,7 +28,6 @@ // Libraries #include "CollisionDispatch.h" -#include "ConcaveVsConvexAlgorithm.h" #include "SphereVsSphereAlgorithm.h" #include "SphereVsConvexPolyhedronAlgorithm.h" #include "SphereVsCapsuleAlgorithm.h" From 6e322882eb21e66a44a1f055beb75f4b52f9408e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 5 Nov 2017 23:10:55 +0100 Subject: [PATCH 075/105] Fix wrong world AABB computation that caused broad-phase collision misses --- src/collision/shapes/CollisionShape.cpp | 50 +++++++++++++++++-------- 1 file changed, 34 insertions(+), 16 deletions(-) diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index d03b4a1c..9622e795 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -32,15 +32,19 @@ using namespace reactphysics3d; // Constructor -CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) : mName(name), mType(type), mScaling(1.0, 1.0, 1.0) { +CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) + : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) { } -// Compute the world-space AABB of the collision shape given a transform +// Compute the world-space AABB of the collision shape given a transform from shape +// local-space to world-space. The technique is described in the book Real-Time Collision +// Detection by Christer Ericson. /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * computed in world-space coordinates - * @param transform Transform used to compute the AABB of the collision shape + * @param transform Transform from shape local-space to world-space used to compute + * the AABB of the collision shape */ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { @@ -51,20 +55,34 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { Vector3 maxBounds; getLocalBounds(minBounds, maxBounds); - // Rotate the local bounds according to the orientation of the body - Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); - Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), - worldAxis.getColumn(1).dot(minBounds), - worldAxis.getColumn(2).dot(minBounds)); - Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), - worldAxis.getColumn(1).dot(maxBounds), - worldAxis.getColumn(2).dot(maxBounds)); + const Vector3 translation = transform.getPosition(); + Matrix3x3 matrix = transform.getOrientation().getMatrix(); + Vector3 resultMin; + Vector3 resultMax; - // Compute the minimum and maximum coordinates of the rotated extents - Vector3 minCoordinates = transform.getPosition() + worldMinBounds; - Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; + // For each of the three axis + for (int i=0; i<3; i++) { + + // Add translation component + resultMin[i] = translation[i]; + resultMax[i] = translation[i]; + + for (int j=0; j<3; j++) { + decimal e = matrix[i][j] * minBounds[j]; + decimal f = matrix[i][j] * maxBounds[j]; + + if (e < f) { + resultMin[i] += e; + resultMax[i] += f; + } + else { + resultMin[i] += f; + resultMax[i] += e; + } + } + } // Update the AABB with the new minimum and maximum coordinates - aabb.setMin(minCoordinates); - aabb.setMax(maxCoordinates); + aabb.setMin(resultMin); + aabb.setMax(resultMax); } From e91cded83161106314db4d5f9680595d66a6b2c0 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 5 Nov 2017 23:15:47 +0100 Subject: [PATCH 076/105] Update code documentation and fix warnings --- src/collision/shapes/BoxShape.cpp | 1 - src/collision/shapes/BoxShape.h | 15 ++++----------- src/collision/shapes/ConcaveMeshShape.h | 2 +- src/mathematics/Matrix3x3.h | 6 +++--- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 01c4d965..6b8c8d6b 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -35,7 +35,6 @@ using namespace reactphysics3d; // Constructor /** * @param extent The vector with the three extents of the box (in meters) - * @param margin The collision margin (in meters) around the collision shape */ BoxShape::BoxShape(const Vector3& extent) : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) { diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 56c14797..5643a5c8 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -41,14 +41,7 @@ namespace reactphysics3d { * This class represents a 3D box shape. Those axis are unit length. * The three extents are half-widths of the box along the three * axis x, y, z local axis. The "transform" of the corresponding - * rigid body will give an orientation and a position to the box. This - * collision shape uses an extra margin distance around it for collision - * detection purpose. The default margin is 4cm (if your units are meters, - * which is recommended). In case, you want to simulate small objects - * (smaller than the margin distance), you might want to reduce the margin by - * specifying your own margin distance using the "margin" parameter in the - * constructor of the box shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. + * body will give an orientation and a position to the box. */ class BoxShape : public ConvexPolyhedronShape { @@ -171,9 +164,9 @@ inline size_t BoxShape::getSizeInBytes() const { // Return a local support point in a given direction without the objec margin inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { - return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, - direction.y < 0.0 ? -mExtent.y : mExtent.y, - direction.z < 0.0 ? -mExtent.z : mExtent.z); + return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x, + direction.y < decimal(0.0) ? -mExtent.y : mExtent.y, + direction.z < decimal(0.0) ? -mExtent.z : mExtent.z); } // Return true if a point is inside the collision shape diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index b99f2b4a..82c08794 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -103,7 +103,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { // Class ConcaveMeshShape /** * This class represents a static concave mesh shape. Note that collision detection - * with a concave mesh shape can be very expensive. You should use only use + * with a concave mesh shape can be very expensive. You should only use * this shape for a static mesh. */ class ConcaveMeshShape : public ConcaveShape { diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index dfa23085..ebb8792d 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -233,9 +233,9 @@ inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vect // Return the matrix with absolute values inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), - fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), - fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); + return Matrix3x3(std::fabs(mRows[0][0]), std::fabs(mRows[0][1]), std::fabs(mRows[0][2]), + std::fabs(mRows[1][0]), std::fabs(mRows[1][1]), std::fabs(mRows[1][2]), + std::fabs(mRows[2][0]), std::fabs(mRows[2][1]), std::fabs(mRows[2][2])); } // Overloaded operator for addition From c6f93789975013df309f735516515ff6c2b91636 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 Nov 2017 21:13:02 +0100 Subject: [PATCH 077/105] Enable vsync in the testbed application --- testbed/src/TestbedApplication.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index d94103ef..f2a43bc1 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -64,7 +64,7 @@ TestbedApplication::TestbedApplication(bool isFullscreen) mSinglePhysicsStepDone = false; mWindowToFramebufferRatio = Vector2(1, 1); mIsShadowMappingEnabled = true; - mIsVSyncEnabled = false; + mIsVSyncEnabled = true; mIsContactPointsDisplayed = false; mIsAABBsDisplayed = false; mIsWireframeEnabled = false; From b39bb3ba37f4ae068d31afeb8ce169fa2b1f5bb6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 Nov 2017 21:14:21 +0100 Subject: [PATCH 078/105] Initialize the timestep in the DynamicsWorld constructor --- src/engine/DynamicsWorld.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 09712c3c..26835166 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -44,7 +44,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), + mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), From 013431487eb633f4df09ed6c4292091ae0ef6e4e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 Nov 2017 21:17:16 +0100 Subject: [PATCH 079/105] Rename setScaling() to setLocalScaling() --- src/collision/ProxyShape.h | 2 +- src/collision/shapes/CollisionShape.h | 4 ++-- src/collision/shapes/ConvexMeshShape.h | 21 ++++++--------------- 3 files changed, 9 insertions(+), 18 deletions(-) diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 36bec740..8282af01 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -332,7 +332,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit * @return The local scaling vector */ inline Vector3 ProxyShape::getLocalScaling() const { - return mCollisionShape->getScaling(); + return mCollisionShape->getLocalScaling(); } // Set the local scaling vector of the collision shape diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 77f96dfb..f6690c58 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -113,7 +113,7 @@ class CollisionShape { virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; /// Return the scaling vector of the collision shape - Vector3 getScaling() const; + Vector3 getLocalScaling() const; /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); @@ -147,7 +147,7 @@ inline CollisionShapeType CollisionShape::getType() const { } // Return the scaling vector of the collision shape -inline Vector3 CollisionShape::getScaling() const { +inline Vector3 CollisionShape::getLocalScaling() const { return mScaling; } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 4ceabe11..71077af2 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -46,18 +46,9 @@ class CollisionWorld; // Class ConvexMeshShape /** * This class represents a convex mesh shape. In order to create a convex mesh shape, you - * need to indicate the local-space position of the mesh vertices. You do it either by - * passing a vertices array to the constructor or using the addVertex() method. Make sure - * that the set of vertices that you use to create the shape are indeed part of a convex - * mesh. The center of mass of the shape will be at the origin of the local-space geometry - * that you use to create the mesh. The method used for collision detection with a convex - * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. - * Therefore, you should try not to use too many vertices. However, it is possible to speed - * up the collision detection by using the edges information of your mesh. The running time - * of the collision detection that uses the edges is almost O(1) constant time at the cost - * of additional memory used to store the vertices. You can indicate edges information - * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method - * in order to use the edges information for collision detection. + * need to indicate the local-space position of the mesh vertices. The center of mass + * of the shape will be at the origin of the local-space geometry that you use to create + * the mesh. */ class ConvexMeshShape : public ConvexPolyhedronShape { @@ -79,9 +70,6 @@ class ConvexMeshShape : public ConvexPolyhedronShape { /// Recompute the bounds of the mesh void recalculateBounds(); - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - /// Return a local support point in a given direction without the object margin. virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; @@ -110,6 +98,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape { /// Deleted assignment operator ConvexMeshShape& operator=(const ConvexMeshShape& shape) = delete; + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling) override; + /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const override; From 8bfa6dd137d49e448cc5a5565e5deffac6b8388c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 Nov 2017 21:26:15 +0100 Subject: [PATCH 080/105] Fix issue : the scaling factor was not used when recomputing AABB tree for concave mesh shape --- src/collision/shapes/ConcaveMeshShape.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 84e95e2c..566b7bc4 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -57,6 +57,11 @@ void ConcaveMeshShape::initBVHTree() { // Get the triangle vertices triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints); + // Apply the scaling factor to the vertices + trianglePoints[0] *= mScaling.x; + trianglePoints[1] *= mScaling.y; + trianglePoints[2] *= mScaling.z; + // Create the AABB for the triangle AABB aabb = AABB::createAABBForTriangle(trianglePoints); From de95e15147e8edf8d72a2f1177e7f8de06ebb0c5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 8 Nov 2017 21:28:00 +0100 Subject: [PATCH 081/105] Fix issue with the shape order in concave vs convex middle-phase collision detection --- src/collision/MiddlePhaseTriangleCallback.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index de6c22ea..9d163799 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -38,12 +38,19 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], verticesNormals, meshSubPart, triangleIndex); + bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex(); + ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape; + ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape; + // Create a narrow phase info for the narrow-phase collision detection NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), - triangleShape, mConvexProxyShape->getLocalToWorldTransform(), - mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), - mConcaveProxyShape->getCachedCollisionData(), mAllocator); + NarrowPhaseInfo(mOverlappingPair, + isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape, + isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), + shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), + shape1->getCachedCollisionData(), + shape2->getCachedCollisionData(), mAllocator); narrowPhaseInfoList->next = firstNarrowPhaseInfo; } From 222636391e89ad3718cb00985120de8ba1eb18c9 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 10 Nov 2017 17:51:02 +0100 Subject: [PATCH 082/105] Use the true triangle face normal if contact is not on an edge in smooth triangle contact --- src/collision/shapes/TriangleShape.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 263595b3..142ba181 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -133,13 +133,22 @@ void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, /// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface /// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 /// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the -/// mesh are either provided by the user or precomputed if the user did not provide them. +/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only +/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the +/// middle of the triangle, we return the true triangle normal. Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { // Compute the barycentric coordinates of the point in the triangle decimal u, v, w; computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); + // If the contact is in the middle of the triangle face (not on the edges) + if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { + + // We return the true triangle face normal (not the interpolated one) + return mNormal; + } + // We compute the contact normal as the barycentric interpolation of the three vertices normals return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit(); } From e9709c3db50add07cb4b9de05aedb4d8dbba983f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 10 Nov 2017 19:57:50 +0100 Subject: [PATCH 083/105] Refactor the Profiler. Now there is one profiler instance per CollisionWorld/DynamicsWorld instance instead of a static one --- src/body/CollisionBody.cpp | 7 ++ src/body/CollisionBody.h | 24 ++++ src/body/RigidBody.cpp | 28 ++++- src/body/RigidBody.h | 7 ++ src/collision/CollisionDetection.cpp | 106 +++--------------- src/collision/CollisionDetection.h | 36 +++++- src/collision/MiddlePhaseTriangleCallback.cpp | 7 ++ src/collision/MiddlePhaseTriangleCallback.h | 17 +++ src/collision/ProxyShape.h | 26 +++++ .../broadphase/BroadPhaseAlgorithm.cpp | 7 ++ .../broadphase/BroadPhaseAlgorithm.h | 30 ++++- src/collision/broadphase/DynamicAABBTree.cpp | 4 +- src/collision/broadphase/DynamicAABBTree.h | 24 ++++ .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 10 +- src/collision/narrowphase/CollisionDispatch.h | 29 ++++- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 7 ++ .../narrowphase/DefaultCollisionDispatch.h | 25 +++++ .../narrowphase/GJK/GJKAlgorithm.cpp | 2 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 24 ++++ .../narrowphase/NarrowPhaseAlgorithm.h | 24 ++++ .../narrowphase/SAT/SATAlgorithm.cpp | 6 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 25 +++++ .../SphereVsConvexPolyhedronAlgorithm.cpp | 14 +++ src/collision/shapes/CollisionShape.cpp | 2 +- src/collision/shapes/CollisionShape.h | 25 +++++ src/collision/shapes/ConcaveMeshShape.cpp | 16 ++- src/collision/shapes/ConcaveMeshShape.h | 35 ++++++ src/collision/shapes/HeightFieldShape.cpp | 16 ++- src/collision/shapes/HeightFieldShape.h | 17 +++ src/collision/shapes/TriangleShape.cpp | 3 +- src/engine/CollisionWorld.cpp | 13 +++ src/engine/CollisionWorld.h | 21 ++++ src/engine/ConstraintSolver.cpp | 12 +- src/engine/ConstraintSolver.h | 23 ++++ src/engine/ContactSolver.cpp | 12 +- src/engine/ContactSolver.h | 24 ++++ src/engine/DynamicsWorld.cpp | 43 ++++--- src/engine/DynamicsWorld.h | 14 --- src/engine/Profiler.cpp | 35 +++++- src/engine/Profiler.h | 82 +++++++++----- .../CollisionDetectionScene.cpp | 69 +++++++----- .../CollisionDetectionScene.h | 2 +- .../collisionshapes/CollisionShapesScene.cpp | 9 +- .../scenes/concavemesh/ConcaveMeshScene.cpp | 7 ++ testbed/scenes/cubes/CubesScene.cpp | 6 + .../scenes/heightfield/HeightFieldScene.cpp | 9 +- testbed/scenes/joints/JointsScene.cpp | 6 + testbed/scenes/raycast/RaycastScene.cpp | 6 + testbed/src/Scene.h | 4 +- 49 files changed, 790 insertions(+), 210 deletions(-) mode change 100755 => 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.h mode change 100755 => 100644 testbed/scenes/cubes/CubesScene.cpp diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 64ce6467..e588f93c 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -74,6 +74,13 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, decimal(1)); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + proxyShape->setProfiler(mProfiler); + +#endif + // Add it to the list of proxy collision shapes of the body if (mProxyCollisionShapes == nullptr) { mProxyCollisionShapes = proxyShape; diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 2d5101b9..9943dc86 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -36,6 +36,7 @@ #include "collision/RaycastInfo.h" #include "memory/PoolAllocator.h" #include "configuration.h" +#include "engine/Profiler.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -85,6 +86,13 @@ class CollisionBody : public Body { /// Reference to the world the body belongs to CollisionWorld& mWorld; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Reset the contact manifold lists @@ -177,6 +185,13 @@ class CollisionBody : public Body { /// Return the body local-space coordinates of a vector given in the world-space coordinates Vector3 getLocalVector(const Vector3& worldVector) const; +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif + // -------------------- Friendship -------------------- // friend class CollisionWorld; @@ -313,6 +328,15 @@ inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getAABB()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionBody::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 6e6a55e6..d0f0c782 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -228,6 +228,13 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, mass); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + proxyShape->setProfiler(mProfiler); + +#endif + // Add it to the list of proxy collision shapes of the body if (mProxyCollisionShapes == nullptr) { mProxyCollisionShapes = proxyShape; @@ -410,7 +417,7 @@ void RigidBody::recomputeMassInformation() { // Update the broad-phase state for this body (because it has moved for instance) void RigidBody::updateBroadPhaseState() const { - PROFILE("RigidBody::updateBroadPhaseState()"); + PROFILE("RigidBody::updateBroadPhaseState()", mProfiler); DynamicsWorld& world = static_cast(mWorld); const Vector3 displacement = world.mTimeStep * mLinearVelocity; @@ -427,3 +434,22 @@ void RigidBody::updateBroadPhaseState() const { } } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +void RigidBody::setProfiler(Profiler* profiler) { + + CollisionBody::setProfiler(profiler); + + // Set the profiler for each proxy shape + ProxyShape* proxyShape = getProxyShapesList(); + while (proxyShape != nullptr) { + + proxyShape->setProfiler(profiler); + + proxyShape = proxyShape->getNext(); + } +} + +#endif + diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index a38b53d5..6ba7a596 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -227,6 +227,13 @@ class RigidBody : public CollisionBody { /// the collision shapes attached to the body. void recomputeMassInformation(); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) override; + +#endif + // -------------------- Friendship -------------------- // friend class DynamicsWorld; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index baaf0db5..7cb865f9 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -57,12 +57,19 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& mem // Fill-in the collision detection matrix with algorithms fillInCollisionMatrix(); + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + } // Compute the collision detection void CollisionDetection::computeCollisionDetection() { - PROFILE("CollisionDetection::computeCollisionDetection()"); + PROFILE("CollisionDetection::computeCollisionDetection()", mProfiler); // Compute the broad-phase collision detection computeBroadPhase(); @@ -80,7 +87,7 @@ void CollisionDetection::computeCollisionDetection() { // Compute the broad-phase collision detection void CollisionDetection::computeBroadPhase() { - PROFILE("CollisionDetection::computeBroadPhase()"); + PROFILE("CollisionDetection::computeBroadPhase()", mProfiler); // If new collision shapes have been added to bodies if (mIsCollisionShapesAdded) { @@ -95,7 +102,7 @@ void CollisionDetection::computeBroadPhase() { // Compute the middle-phase collision detection void CollisionDetection::computeMiddlePhase() { - PROFILE("CollisionDetection::computeMiddlePhase()"); + PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); // For each possible collision pair of bodies map::iterator it; @@ -219,6 +226,13 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, concaveShape, allocator); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + middlePhaseCallback.setProfiler(mProfiler); + +#endif + // Compute the convex shape AABB in the local-space of the convex shape const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() * convexProxyShape->getLocalToWorldTransform(); @@ -236,7 +250,7 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { - PROFILE("CollisionDetection::computeNarrowPhase()"); + PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; while (currentNarrowPhaseInfo != nullptr) { @@ -444,90 +458,6 @@ void CollisionDetection::reportAllContacts() { } } -// Process the potential contacts where one collion is a concave shape. -// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described -// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision -// issue with some internal edges. -void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) { - -// // Set with the triangle vertices already processed to void further contacts with same triangle -// std::unordered_multimap processTriangleVertices; - -// std::vector smoothContactPoints; - -// // If the collision shape 1 is the triangle -// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE; -// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE); -// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE); - -// const TriangleShape* triangleShape = nullptr; -// if (isFirstShapeTriangle) { -// triangleShape = static_cast(pair->getShape1()->getCollisionShape()); -// } -// else { -// triangleShape = static_cast(pair->getShape2()->getCollisionShape()); -// } -// assert(triangleShape != nullptr); - -// // Get the temporary memory allocator -// Allocator& allocator = pair->getTemporaryAllocator(); - -// // For each potential contact manifold of the pair -// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); -// while (potentialManifold != nullptr) { - -// // For each contact point of the potential manifold -// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo(); -// while (contactPointInfo != nullptr) { - -// // Compute the barycentric coordinates of the point in the triangle -// decimal u, v, w; -// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0), -// triangleShape->getVertexPosition(1), -// triangleShape->getVertexPosition(2), -// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2, -// u, v, w); -// int nbZeros = 0; -// bool isUZero = approxEqual(u, 0, 0.0001); -// bool isVZero = approxEqual(v, 0, 0.0001); -// bool isWZero = approxEqual(w, 0, 0.0001); -// if (isUZero) nbZeros++; -// if (isVZero) nbZeros++; -// if (isWZero) nbZeros++; - -// // If the triangle contact point is on a triangle vertex of a triangle edge -// if (nbZeros == 1 || nbZeros == 2) { - - -// // Create a smooth mesh contact info -// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo))) -// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle, -// triangleShape->getVertexPosition(0), -// triangleShape->getVertexPosition(1), -// triangleShape->getVertexPosition(2)); - -// smoothContactPoints.push_back(smoothContactInfo); - -// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we -// // will put the contact point back in the manifold. -// ... -// } - -// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle -// // from the contact manifold because we know we will keep to contact points. We only remove the vertices -// // and edges contact points for the moment. If those points will be kept in the future, we will have to -// // put them back again in the contact manifold -// } - -// potentialManifold = potentialManifold->mNext; -// } - -// // Sort the list of narrow-phase contacts according to their penetration depth -// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare()); - -// ... -} - // Compute the middle-phase collision detection between two proxy shapes NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 6f28f788..fc33c00f 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -100,6 +100,13 @@ class CollisionDetection { /// True if some collision shapes have been added previously bool mIsCollisionShapesAdded; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Compute the broad-phase collision detection @@ -143,6 +150,7 @@ class CollisionDetection { /// Process the potential contacts where one collion is a concave shape void processSmoothMeshContacts(OverlappingPair* pair); + public : @@ -219,6 +227,13 @@ class CollisionDetection { /// Return a reference to the world memory allocator PoolAllocator& getWorldMemoryAllocator(); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + /// Return the world-space AABB of a given proxy shape const AABB getWorldAABB(const ProxyShape* proxyShape) const; @@ -234,6 +249,14 @@ inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisio // Fill-in the collision matrix with the new algorithms to use fillInCollisionMatrix(); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mCollisionDispatch->setProfiler(mProfiler); + +#endif + } // Add a body to the collision detection @@ -298,7 +321,7 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { - PROFILE("CollisionDetection::raycast()"); + PROFILE("CollisionDetection::raycast()", mProfiler); RaycastTest rayCastTest(raycastCallback); @@ -312,6 +335,17 @@ inline CollisionWorld* CollisionDetection::getWorld() { return mWorld; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionDetection::setProfiler(Profiler* profiler) { + mProfiler = profiler; + mBroadPhaseAlgorithm.setProfiler(profiler); + mCollisionDispatch->setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index 9d163799..0bda49d4 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -38,6 +38,13 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], verticesNormals, meshSubPart, triangleIndex); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape->setProfiler(mProfiler); + +#endif + bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex(); ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape; ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape; diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h index 36bfac24..4927c6e0 100644 --- a/src/collision/MiddlePhaseTriangleCallback.h +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -58,6 +58,13 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { /// Reference to the single-frame memory allocator Allocator& mAllocator; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public: /// Pointer to the first element of the linked-list of narrow-phase info @@ -77,6 +84,16 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { /// Test collision between a triangle and the convex mesh shape virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, const Vector3* verticesNormals) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif + }; } diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 8282af01..574758d4 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -85,6 +85,13 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Return the collision shape @@ -170,6 +177,13 @@ class ProxyShape { /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + // -------------------- Friendship -------------------- // friend class OverlappingPair; @@ -359,6 +373,18 @@ inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getWorldAABB()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ProxyShape::setProfiler(Profiler* profiler) { + + mProfiler = profiler; + + mCollisionShape->setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index c86cd0bc..c1484434 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -44,6 +44,13 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) // Allocate memory for the array of potential overlapping pairs mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); assert(mPotentialPairs != nullptr); + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + } // Destructor diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 94499d44..7e1ff262 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -162,6 +162,13 @@ class BroadPhaseAlgorithm { /// Reference to the collision detection object CollisionDetection& mCollisionDetection; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public : // -------------------- Methods -------------------- // @@ -215,8 +222,15 @@ class BroadPhaseAlgorithm { const AABB& getFatAABB(int broadPhaseId) const; /// Ray casting method - void raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const; + void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Method used to compare two pairs for sorting algorithm @@ -252,7 +266,7 @@ inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const { inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { - PROFILE("BroadPhaseAlgorithm::raycast()"); + PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler); BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); @@ -264,6 +278,16 @@ inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPh return static_cast(mDynamicAABBTree.getNodeDataPointer(broadPhaseId)); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void BroadPhaseAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; + mDynamicAABBTree.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index f829c000..5fa33d9d 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -171,7 +171,7 @@ void DynamicAABBTree::removeObject(int nodeID) { /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { - PROFILE("DynamicAABBTree::updateObject()"); + PROFILE("DynamicAABBTree::updateObject()", mProfiler); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); @@ -633,7 +633,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, // Ray casting method void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { - PROFILE("DynamicAABBTree::raycast()"); + PROFILE("DynamicAABBTree::raycast()", mProfiler); decimal maxFraction = ray.maxFraction; diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 8025e4f9..54751138 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -155,6 +155,13 @@ class DynamicAABBTree { /// without triggering a large modification of the tree which can be costly decimal mExtraAABBGap; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Allocate and return a node to use in the tree @@ -237,6 +244,14 @@ class DynamicAABBTree { /// Clear all the nodes and reset the tree void reset(); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Return true if the node is a leaf of the tree @@ -292,6 +307,15 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { return nodeId; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void DynamicAABBTree::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index e895eef7..315dcf7a 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -41,7 +41,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm; + +#ifdef IS_PROFILING_ACTIVE + + gjkAlgorithm.setProfiler(mProfiler); + satAlgorithm.setProfiler(mProfiler); + +#endif + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 230ebbb3..d8351d6a 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -41,6 +41,13 @@ class CollisionDispatch { protected: +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public: /// Constructor @@ -49,13 +56,29 @@ class CollisionDispatch { /// Destructor virtual ~CollisionDispatch() = default; - /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, - int shape2Type)=0; + virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionDispatch::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index c6297fb2..9f2bb47e 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -38,6 +38,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; + +#ifdef IS_PROFILING_ACTIVE + + satAlgorithm.setProfiler(mProfiler); + +#endif + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 779f7048..fe22b4a9 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -77,8 +77,33 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler) override; + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) { + + CollisionDispatch::setProfiler(profiler); + + mSphereVsSphereAlgorithm.setProfiler(profiler); + mCapsuleVsCapsuleAlgorithm.setProfiler(profiler); + mSphereVsCapsuleAlgorithm.setProfiler(profiler); + mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler); + mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler); + mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 629811eb..c0f436a6 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -49,7 +49,7 @@ using namespace reactphysics3d; /// the correct penetration depth and contact points between the enlarged objects. GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { - PROFILE("GJKAlgorithm::testCollision()"); + PROFILE("GJKAlgorithm::testCollision()", mProfiler); Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index b8c7319c..a9daa897 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -62,6 +62,13 @@ class GJKAlgorithm { // -------------------- Attributes -------------------- // +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // public : @@ -94,8 +101,25 @@ class GJKAlgorithm { /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void GJKAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 6a8746c6..34fa5962 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -67,6 +67,13 @@ class NarrowPhaseAlgorithm { // -------------------- Attributes -------------------- // +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public : // -------------------- Methods -------------------- // @@ -85,8 +92,25 @@ class NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volume collide virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index e6d09dd1..47953f1a 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -47,7 +47,7 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { - PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()"); + PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler); bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; @@ -139,7 +139,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { - PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()"); + PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler); bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; @@ -448,7 +448,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c // Test collision between two convex polyhedrons bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { - PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()"); + PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler); assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 5c721a00..60f7a091 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -50,6 +50,13 @@ class SATAlgorithm { /// make sure the contact manifold does not change too much between frames. static const decimal SAME_SEPARATING_AXIS_BIAS; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) @@ -138,8 +145,26 @@ class SATAlgorithm { /// Test collision between two convex meshes bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void SATAlgorithm::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 7ecda325..ae3764ab 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -43,6 +43,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; + +#ifdef IS_PROFILING_ACTIVE + + gjkAlgorithm.setProfiler(mProfiler); + +#endif + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; @@ -60,6 +67,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; + +#ifdef IS_PROFILING_ACTIVE + + satAlgorithm.setProfiler(mProfiler); + +#endif + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index 9622e795..a89747c9 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -48,7 +48,7 @@ CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) */ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { - PROFILE("CollisionShape::computeAABB()"); + PROFILE("CollisionShape::computeAABB()", mProfiler); // Get the local bounds in x,y and z direction Vector3 minBounds; diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index f6690c58..feb4c502 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -35,6 +35,7 @@ #include "AABB.h" #include "collision/RaycastInfo.h" #include "memory/PoolAllocator.h" +#include "engine/Profiler.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -69,6 +70,13 @@ class CollisionShape { /// Scaling vector of the collision shape Vector3 mScaling; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif // -------------------- Methods -------------------- // @@ -124,6 +132,13 @@ class CollisionShape { /// Compute the world-space AABB of the collision shape given a transform virtual void computeAABB(AABB& aabb, const Transform& transform) const; +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif + // -------------------- Friendship -------------------- // friend class ProxyShape; @@ -156,6 +171,16 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionShape::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 566b7bc4..646638dd 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -113,11 +113,18 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& /// the ray hits many triangles. bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - PROFILE("ConcaveMeshShape::raycast()"); + PROFILE("ConcaveMeshShape::raycast()", mProfiler); // Create the callback object that will compute ray casting against triangles ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + raycastCallback.setProfiler(mProfiler); + +#endif + // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. // The raycastCallback object will then compute ray casting against the triangles // in the hit AABBs. @@ -159,6 +166,13 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], verticesNormals, data[0], data[1]); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif // Ray casting test against the collision shape RaycastInfo raycastInfo; diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 82c08794..678d5325 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -78,6 +78,13 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { const Ray& mRay; bool mIsHit; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public: // Constructor @@ -98,6 +105,15 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { bool getIsHit() const { return mIsHit; } + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif }; // Class ConcaveMeshShape @@ -165,6 +181,13 @@ class ConcaveMeshShape : public ConcaveShape { /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override; +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler) override; + +#endif + // ---------- Friendship ----------- // friend class ConvexTriangleAABBOverlapCallback; @@ -239,6 +262,18 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ConcaveMeshShape::setProfiler(Profiler* profiler) { + + CollisionShape::setProfiler(profiler); + + mDynamicAABBTree.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 2afba401..08ec19a4 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -217,10 +217,17 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead - PROFILE("HeightFieldShape::raycast()"); + PROFILE("HeightFieldShape::raycast()", mProfiler); TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + triangleCallback.setProfiler(mProfiler); + +#endif + // Compute the AABB for the ray const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd)); @@ -264,6 +271,13 @@ void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, verticesNormals, meshSubPart, triangleIndex); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif + // Ray casting test against the collision shape RaycastInfo raycastInfo; bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index aa0cd787..80c18d9c 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -49,6 +49,13 @@ class TriangleOverlapCallback : public TriangleCallback { bool mIsHit; decimal mSmallestHitFraction; const HeightFieldShape& mHeightFieldShape; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif public: @@ -66,6 +73,16 @@ class TriangleOverlapCallback : public TriangleCallback { /// Raycast test between a ray and a triangle of the heightfield virtual void testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, const Vector3* verticesNormals) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif + }; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 142ba181..020053b6 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -153,13 +153,12 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit(); } - // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - PROFILE("TriangleShape::raycast()"); + PROFILE("TriangleShape::raycast()", mProfiler); const Vector3 pq = ray.point2 - ray.point1; const Vector3 pa = mPoints[0] - ray.point1; diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index e8f4155a..eeb06305 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -37,6 +37,13 @@ CollisionWorld::CollisionWorld() mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0), mEventListener(nullptr) { +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mCollisionDetection.setProfiler(&mProfiler); + +#endif + } // Destructor @@ -75,6 +82,12 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { // Add the collision body to the world mBodies.insert(collisionBody); +#ifdef IS_PROFILING_ACTIVE + + collisionBody->setProfiler(&mProfiler); + +#endif + // Return the pointer to the rigid body return collisionBody; } diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index d27b8018..f523d169 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -82,6 +82,12 @@ class CollisionWorld { /// Pointer to an event listener object EventListener* mEventListener; +#ifdef IS_PROFILING_ACTIVE + + /// Real-time hierarchical profiler + Profiler mProfiler; +#endif + // -------------------- Methods -------------------- // /// Return the next available body ID @@ -147,6 +153,12 @@ class CollisionWorld { /// Test and report collisions between all shapes of the world void testCollision(CollisionCallback* callback); +#ifdef IS_PROFILING_ACTIVE + + /// Set the name of the profiler + void setProfilerName(std::string name); +#endif + /// Return the current world-space AABB of given proxy shape AABB getWorldAABB(const ProxyShape* proxyShape) const; @@ -236,6 +248,15 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits); } +#ifdef IS_PROFILING_ACTIVE + +// Set the name of the profiler +inline void CollisionWorld::setProfilerName(std::string name) { + mProfiler.setName(name); +} + +#endif + } #endif diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 547aea6e..458c39a7 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,12 +34,18 @@ ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVe : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + } // Initialize the constraint solver for a given island void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { - PROFILE("ConstraintSolver::initializeForIsland()"); + PROFILE("ConstraintSolver::initializeForIsland()", mProfiler); assert(island != nullptr); assert(island->getNbBodies() > 0); @@ -69,7 +75,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { // Solve the velocity constraints void ConstraintSolver::solveVelocityConstraints(Island* island) { - PROFILE("ConstraintSolver::solveVelocityConstraints()"); + PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler); assert(island != nullptr); assert(island->getNbJoints() > 0); @@ -86,7 +92,7 @@ void ConstraintSolver::solveVelocityConstraints(Island* island) { // Solve the position constraints void ConstraintSolver::solvePositionConstraints(Island* island) { - PROFILE("ConstraintSolver::solvePositionConstraints()"); + PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler); assert(island != nullptr); assert(island->getNbJoints() > 0); diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index b40cc983..fbb54ed1 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -165,6 +165,12 @@ class ConstraintSolver { /// Constraint solver data used to initialize and solve the constraints ConstraintSolverData mConstraintSolverData; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; +#endif + public : // -------------------- Methods -------------------- // @@ -197,6 +203,14 @@ class ConstraintSolver { /// Set the constrained positions/orientations arrays void setConstrainedPositionsArrays(Vector3* constrainedPositions, Quaternion* constrainedOrientations); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Set the constrained velocities arrays @@ -221,6 +235,15 @@ inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrained mConstraintSolverData.orientations = constrainedOrientations; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ConstraintSolver::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 819a1b76..1e5b5401 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -52,7 +52,7 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity // Initialize the contact constraints void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { - PROFILE("ContactSolver::init()"); + PROFILE("ContactSolver::init()", mProfiler); mTimeStep = timeStep; @@ -98,7 +98,7 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { // Initialize the constraint solver for a given island void ContactSolver::initializeForIsland(Island* island) { - PROFILE("ContactSolver::initializeForIsland()"); + PROFILE("ContactSolver::initializeForIsland()", mProfiler); assert(island != nullptr); assert(island->getNbBodies() > 0); @@ -270,7 +270,7 @@ void ContactSolver::initializeForIsland(Island* island) { /// the solution of the linear system void ContactSolver::warmStart() { - PROFILE("ContactSolver::warmStart()"); + PROFILE("ContactSolver::warmStart()", mProfiler); uint contactPointIndex = 0; @@ -387,7 +387,7 @@ void ContactSolver::warmStart() { // Solve the contacts void ContactSolver::solve() { - PROFILE("ContactSolver::solve()"); + PROFILE("ContactSolver::solve()", mProfiler); decimal deltaLambda; decimal lambdaTemp; @@ -586,7 +586,7 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { - PROFILE("ContactSolver::storeImpulses()"); + PROFILE("ContactSolver::storeImpulses()", mProfiler); uint contactPointIndex = 0; @@ -614,7 +614,7 @@ void ContactSolver::storeImpulses() { void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { - PROFILE("ContactSolver::computeFrictionVectors()"); + PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); assert(contact.normal.length() > decimal(0.0)); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 40e0721d..0a2850f5 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -308,6 +308,13 @@ class ContactSolver { /// True if the split impulse position correction is active bool mIsSplitImpulseActive; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Compute the collision restitution factor from the restitution factor of each body @@ -367,6 +374,13 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif }; // Set the split velocities arrays @@ -425,6 +439,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ContactSolver::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 26835166..bb824228 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -29,6 +29,7 @@ #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" #include "constraint/FixedJoint.h" +#include // Namespaces using namespace reactphysics3d; @@ -53,6 +54,14 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mConstraintSolver.setProfiler(&mProfiler); + mContactSolver.setProfiler(&mProfiler); + +#endif + } // Destructor @@ -80,10 +89,10 @@ DynamicsWorld::~DynamicsWorld() { #ifdef IS_PROFILING_ACTIVE // Print the profiling report - Profiler::printReport(std::cout); - - // Destroy the profiler (release the allocated memory) - Profiler::destroy(); + ofstream myfile; + myfile.open(mProfiler.getName() + ".txt"); + mProfiler.printReport(myfile); + myfile.close(); #endif } @@ -96,10 +105,10 @@ void DynamicsWorld::update(decimal timeStep) { #ifdef IS_PROFILING_ACTIVE // Increment the frame counter of the profiler - Profiler::incrementFrameCounter(); + mProfiler.incrementFrameCounter(); #endif - PROFILE("DynamicsWorld::update()"); + PROFILE("DynamicsWorld::update()", &mProfiler); mTimeStep = timeStep; @@ -147,7 +156,7 @@ void DynamicsWorld::update(decimal timeStep) { /// the sympletic Euler time stepping scheme. void DynamicsWorld::integrateRigidBodiesPositions() { - PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); + PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", &mProfiler); // For each island of the world for (uint i=0; i < mNbIslands; i++) { @@ -186,7 +195,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // Update the postion/orientation of the bodies void DynamicsWorld::updateBodiesState() { - PROFILE("DynamicsWorld::updateBodiesState()"); + PROFILE("DynamicsWorld::updateBodiesState()", &mProfiler); // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { @@ -223,7 +232,7 @@ void DynamicsWorld::updateBodiesState() { // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { - PROFILE("DynamicsWorld::initVelocityArrays()"); + PROFILE("DynamicsWorld::initVelocityArrays()", &mProfiler); // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); @@ -266,7 +275,7 @@ void DynamicsWorld::initVelocityArrays() { /// contact solver. void DynamicsWorld::integrateRigidBodiesVelocities() { - PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); + PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", &mProfiler); // Initialize the bodies velocity arrays initVelocityArrays(); @@ -328,7 +337,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // Solve the contacts and constraints void DynamicsWorld::solveContactsAndConstraints() { - PROFILE("DynamicsWorld::solveContactsAndConstraints()"); + PROFILE("DynamicsWorld::solveContactsAndConstraints()", &mProfiler); // Set the velocities arrays mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); @@ -401,7 +410,7 @@ void DynamicsWorld::solveContactsAndConstraints() { // Solve the position error correction of the constraints void DynamicsWorld::solvePositionCorrection() { - PROFILE("DynamicsWorld::solvePositionCorrection()"); + PROFILE("DynamicsWorld::solvePositionCorrection()", &mProfiler); // Do not continue if there is no constraints if (mJoints.empty()) return; @@ -442,6 +451,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { mBodies.insert(rigidBody); mRigidBodies.insert(rigidBody); +#ifdef IS_PROFILING_ACTIVE + + rigidBody->setProfiler(&mProfiler); + +#endif + // Return the pointer to the rigid body return rigidBody; } @@ -613,7 +628,7 @@ void DynamicsWorld::addJointToBody(Joint* joint) { /// it). Then, we create an island with this group of connected bodies. void DynamicsWorld::computeIslands() { - PROFILE("DynamicsWorld::computeIslands()"); + PROFILE("DynamicsWorld::computeIslands()", &mProfiler); uint nbBodies = mRigidBodies.size(); @@ -757,7 +772,7 @@ void DynamicsWorld::computeIslands() { /// time, we put all the bodies of the island to sleep. void DynamicsWorld::updateSleepingBodies() { - PROFILE("DynamicsWorld::updateSleepingBodies()"); + PROFILE("DynamicsWorld::updateSleepingBodies()", &mProfiler); const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b632dcf2..366b2770 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -124,19 +124,9 @@ class DynamicsWorld : public CollisionWorld { /// Integrate the positions and orientations of rigid bodies. void integrateRigidBodiesPositions(); - /// Update the AABBs of the bodies - void updateRigidBodiesAABB(); - /// Reset the external force and torque applied to the bodies void resetBodiesForceAndTorque(); - /// Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, - Vector3 newAngVelocity); - - /// Compute and set the interpolation factor to all bodies - void setInterpolationFactorToAllBodies(); - /// Initialize the bodies velocities arrays for the next simulation step. void initVelocityArrays(); @@ -149,9 +139,6 @@ class DynamicsWorld : public CollisionWorld { /// Solve the position error correction of the constraints void solvePositionCorrection(); - /// Cleanup the constrained velocities array at each step - void cleanupConstrainedVelocitiesArray(); - /// Compute the islands of awake bodies. void computeIslands(); @@ -478,7 +465,6 @@ inline void DynamicsWorld::setEventListener(EventListener* eventListener) { mEventListener = eventListener; } - } #endif diff --git a/src/engine/Profiler.cpp b/src/engine/Profiler.cpp index 9437dca0..3212e4a1 100644 --- a/src/engine/Profiler.cpp +++ b/src/engine/Profiler.cpp @@ -27,14 +27,12 @@ // Libraries #include "Profiler.h" +#include using namespace reactphysics3d; // Initialization of static variables -ProfileNode Profiler::mRootNode("Root", nullptr); -ProfileNode* Profiler::mCurrentNode = &Profiler::mRootNode; -long double Profiler::mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; -uint Profiler::mFrameCounter = 0; +int Profiler::mNbProfilers = 0; // Constructor ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) @@ -157,6 +155,35 @@ void ProfileNodeIterator::enterParent() { mCurrentChildNode = mCurrentParentNode->getChildNode(); } +// Constructor +Profiler::Profiler(std::string name) :mRootNode("Root", nullptr) { + + // Set the name of the profiler + if (name == "") { + + if (mNbProfilers == 0) { + mName = "profiler"; + } + else { + mName = std::string("profiler") + std::to_string(mNbProfilers); + } + } + else { + mName = name; + } + + mCurrentNode = &mRootNode; + mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; + mFrameCounter = 0; + + mNbProfilers++; +} + +// Destructor +Profiler::~Profiler() { + destroy(); +} + // Method called when we want to start profiling a block of code. void Profiler::startProfilingBlock(const char* name) { diff --git a/src/engine/Profiler.h b/src/engine/Profiler.h index 818d6cc8..45b9b2e9 100644 --- a/src/engine/Profiler.h +++ b/src/engine/Profiler.h @@ -184,57 +184,73 @@ class Profiler { // -------------------- Attributes -------------------- // + /// Profiler name + std::string mName; + + /// Total number of profilers + static int mNbProfilers; + /// Root node of the profiler tree - static ProfileNode mRootNode; + ProfileNode mRootNode; /// Current node in the current execution - static ProfileNode* mCurrentNode; + ProfileNode* mCurrentNode; /// Frame counter - static uint mFrameCounter; + uint mFrameCounter; /// Starting profiling time - static long double mProfilingStartTime; + long double mProfilingStartTime; /// Recursively print the report of a given node of the profiler tree - static void printRecursiveNodeReport(ProfileNodeIterator* iterator, - int spacing, - std::ostream& outputStream); + void printRecursiveNodeReport(ProfileNodeIterator* iterator, int spacing, std::ostream& outputStream); + + /// Destroy a previously allocated iterator + void destroyIterator(ProfileNodeIterator* iterator); + + /// Destroy the profiler (release the memory) + void destroy(); public : // -------------------- Methods -------------------- // + /// Constructor + Profiler(std::string name = ""); + + /// Destructor + ~Profiler(); + /// Method called when we want to start profiling a block of code. - static void startProfilingBlock(const char *name); + void startProfilingBlock(const char *name); /// Method called at the end of the scope where the /// startProfilingBlock() method has been called. - static void stopProfilingBlock(); + void stopProfilingBlock(); /// Reset the timing data of the profiler (but not the profiler tree structure) - static void reset(); + void reset(); /// Return the number of frames - static uint getNbFrames(); + uint getNbFrames(); + + /// Get the name of the profiler + std::string getName() const; + + /// Set the name of the profiler + void setName(std::string name); /// Return the elasped time since the start/reset of the profiling - static long double getElapsedTimeSinceStart(); + long double getElapsedTimeSinceStart(); /// Increment the frame counter - static void incrementFrameCounter(); + void incrementFrameCounter(); /// Return an iterator over the profiler tree starting at the root - static ProfileNodeIterator* getIterator(); + ProfileNodeIterator* getIterator(); /// Print the report of the profiler in a given output stream - static void printReport(std::ostream& outputStream); - - /// Destroy a previously allocated iterator - static void destroyIterator(ProfileNodeIterator* iterator); - - /// Destroy the profiler (release the memory) - static void destroy(); + void printReport(std::ostream& outputStream); }; // Class ProfileSample @@ -245,27 +261,33 @@ class Profiler { */ class ProfileSample { + private: + + Profiler* mProfiler; + public : // -------------------- Methods -------------------- // /// Constructor - ProfileSample(const char* name) { + ProfileSample(const char* name, Profiler* profiler) :mProfiler(profiler) { + + assert(profiler != nullptr); // Ask the profiler to start profiling a block of code - Profiler::startProfilingBlock(name); + mProfiler->startProfilingBlock(name); } /// Destructor ~ProfileSample() { // Tell the profiler to stop profiling a block of code - Profiler::stopProfilingBlock(); + mProfiler->stopProfilingBlock(); } }; // Use this macro to start profile a block of code -#define PROFILE(name) ProfileSample profileSample(name) +#define PROFILE(name, profiler) ProfileSample profileSample(name, profiler) // Return true if we are at the root of the profiler tree inline bool ProfileNodeIterator::isRoot() { @@ -352,6 +374,16 @@ inline uint Profiler::getNbFrames() { return mFrameCounter; } +// Get the name of the profiler +inline std::string Profiler::getName() const { + return mName; +} + +// Set the name of the profiler +inline void Profiler::setName(std::string name) { + mName = name; +} + // Return the elasped time since the start/reset of the profiling inline long double Profiler::getElapsedTimeSinceStart() { long double currentTime = Timer::getCurrentSystemTime() * 1000.0; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index f00dbdba..1241b701 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -49,6 +49,12 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine // Create the dynamics world for the physics simulation mPhysicsWorld = new rp3d::CollisionWorld(); +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + // ---------- Sphere 1 ---------- // // Create a sphere and a corresponding collision body in the dynamics world @@ -72,6 +78,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine mSphere2->setSleepingColor(mRedColorDemo); mPhysicsObjects.push_back(mSphere2); + // ---------- Capsule 1 ---------- // // Create a cylinder and a corresponding collision body in the dynamics world @@ -94,49 +101,49 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine mCapsule2->setSleepingColor(mRedColorDemo); mPhysicsObjects.push_back(mCapsule2); + // ---------- Concave Mesh ---------- // + + // Create a convex mesh and a corresponding collision body in the dynamics world + mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); + mAllShapes.push_back(mConcaveMesh); + + // Set the color + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); + // ---------- Box 1 ---------- // - // Create a cylinder and a corresponding collision body in the dynamics world + // Create a cylinder and a corresponding collision body in the dynamics world mBox1 = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath); - mAllShapes.push_back(mBox1); + mAllShapes.push_back(mBox1); - // Set the color - mBox1->setColor(mGreyColorDemo); - mBox1->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mBox1); + // Set the color + mBox1->setColor(mGreyColorDemo); + mBox1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox1); - // ---------- Box 2 ---------- // + // ---------- Box 2 ---------- // - // Create a cylinder and a corresponding collision body in the dynamics world + // Create a cylinder and a corresponding collision body in the dynamics world mBox2 = new Box(openglframework::Vector3(3, 2, 5), mPhysicsWorld, mMeshFolderPath); - mAllShapes.push_back(mBox2); + mAllShapes.push_back(mBox2); - // Set the color - mBox2->setColor(mGreyColorDemo); - mBox2->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mBox2); + // Set the color + mBox2->setColor(mGreyColorDemo); + mBox2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox2); // ---------- Convex Mesh ---------- // // Create a convex mesh and a corresponding collision body in the dynamics world mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj"); - mAllShapes.push_back(mConvexMesh); + mAllShapes.push_back(mConvexMesh); // Set the color mConvexMesh->setColor(mGreyColorDemo); mConvexMesh->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mConvexMesh); - - // ---------- Concave Mesh ---------- // - - // Create a convex mesh and a corresponding collision body in the dynamics world - mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); - mAllShapes.push_back(mConcaveMesh); - - // Set the color - mConcaveMesh->setColor(mGreyColorDemo); - mConcaveMesh->setSleepingColor(mRedColorDemo); - mPhysicsObjects.push_back(mConcaveMesh); + mPhysicsObjects.push_back(mConvexMesh); // ---------- Heightfield ---------- // @@ -154,14 +161,14 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine // Reset the scene void CollisionDetectionScene::reset() { - mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(12, 0, 0), rp3d::Quaternion::identity())); - mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(12, 8, 0), rp3d::Quaternion::identity())); - mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-6, 7, 0), rp3d::Quaternion::identity())); + mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(15, 5, 0), rp3d::Quaternion::identity())); + mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(0, 6, 0), rp3d::Quaternion::identity())); + mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-8, 7, 0), rp3d::Quaternion::identity())); mCapsule2->setTransform(rp3d::Transform(rp3d::Vector3(11, -8, 0), rp3d::Quaternion::identity())); mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity())); - mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity())); + mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 9, 0), rp3d::Quaternion::identity())); mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity())); - mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 100, 0), rp3d::Quaternion::identity())); + mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity())); mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity())); } diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h old mode 100755 new mode 100644 index 4bea0c0b..e5bd8db9 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -152,7 +152,7 @@ class CollisionDetectionScene : public SceneDemo { std::vector mAllShapes; - uint mSelectedShapeIndex; + unsigned int mSelectedShapeIndex; /// Select the next shape void selectNextShape(); diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index a6fd6662..8efc13de 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -46,7 +46,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin rp3d::Vector3 gravity(0, -9.81f, 0); // Create the dynamics world for the physics simulation - mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + for (int i=0; isetProfilerName(name + "_profiler"); + +#endif + + // ---------- Create the boxes ----------- // for (int i = 0; isetProfilerName(name + "_profiler"); + +#endif + // Create all the cubes of the scene for (int i=0; isetProfilerName(name + "_profiler"); + +#endif + for (int i = 0; isetProfilerName(name + "_profiler"); + +#endif + // Create the Ball-and-Socket joint createBallAndSocketJoints(); diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index bf806183..5705627d 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -47,6 +47,12 @@ RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings) // Create the dynamics world for the physics simulation mPhysicsWorld = new rp3d::CollisionWorld(); +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + // ---------- Dumbbell ---------- // // Create a convex mesh and a corresponding collision body in the dynamics world diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index e33e9da9..89a9afdd 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -53,8 +53,8 @@ struct EngineSettings { long double elapsedTime; // Elapsed time (in seconds) float timeStep; // Current time step (in seconds) - uint nbVelocitySolverIterations; // Nb of velocity solver iterations - uint nbPositionSolverIterations; // Nb of position solver iterations + unsigned int nbVelocitySolverIterations; // Nb of velocity solver iterations + unsigned int nbPositionSolverIterations; // Nb of position solver iterations bool isSleepingEnabled; // True if sleeping technique is enabled float timeBeforeSleep; // Time of inactivity before a body sleep float sleepLinearVelocity; // Sleep linear velocity From 38bd462b91843834a529527b96cad3f1f7ff2ee6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 13 Nov 2017 18:42:39 +0100 Subject: [PATCH 084/105] Fix issue in SAT algorithm, use the correct penetration depth for each contact point --- src/collision/narrowphase/SAT/SATAlgorithm.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 47953f1a..327f4465 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -871,8 +871,12 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene bool contactPointsFound = false; for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + // Compute the penetration depth of this contact point (can be different from the minPenetration depth which is + // the maximal penetration depth of any contact point for this separating axis + decimal penetrationDepth = (referenceFaceVertex - (*itPoints)).dot(axisReferenceSpace); + // If the clip point is bellow the reference face - if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) { + if (penetrationDepth > decimal(0.0)) { contactPointsFound = true; @@ -889,10 +893,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, - minPenetrationDepth, outWorldNormal); + penetrationDepth, outWorldNormal); // Create a new contact point - narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(outWorldNormal, penetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } From f403a6e8040832a222ff17ad648889b802aacef5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 Nov 2017 22:43:27 +0100 Subject: [PATCH 085/105] Add temporal coherence for convex vs triangle collision detection --- src/collision/CollisionCallback.h | 2 + src/collision/CollisionDetection.cpp | 18 +- src/collision/ContactManifold.h | 1 + src/collision/ContactManifoldInfo.cpp | 3 + src/collision/MiddlePhaseTriangleCallback.cpp | 6 +- src/collision/MiddlePhaseTriangleCallback.h | 3 +- src/collision/NarrowPhaseInfo.cpp | 7 +- src/collision/NarrowPhaseInfo.h | 17 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 7 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 10 +- .../narrowphase/SAT/SATAlgorithm.cpp | 300 +++++++++--------- .../SphereVsConvexPolyhedronAlgorithm.cpp | 11 +- src/collision/shapes/CollisionShape.cpp | 2 +- src/collision/shapes/CollisionShape.h | 11 + src/collision/shapes/ConcaveMeshShape.cpp | 21 +- src/collision/shapes/ConcaveMeshShape.h | 5 +- src/collision/shapes/ConcaveShape.h | 3 +- src/collision/shapes/HeightFieldShape.cpp | 10 +- src/collision/shapes/HeightFieldShape.h | 12 +- src/collision/shapes/SphereShape.cpp | 3 +- src/collision/shapes/TriangleShape.cpp | 15 +- src/collision/shapes/TriangleShape.h | 28 +- src/constraint/ContactPoint.h | 3 +- src/engine/CollisionWorld.h | 2 +- src/engine/OverlappingPair.cpp | 73 ++++- src/engine/OverlappingPair.h | 50 ++- test/tests/collision/TestRaycast.h | 9 +- 28 files changed, 400 insertions(+), 247 deletions(-) diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h index f860b42d..54208fe4 100644 --- a/src/collision/CollisionCallback.h +++ b/src/collision/CollisionCallback.h @@ -32,6 +32,8 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +class OverlappingPair; + // Class CollisionCallback /** * This class can be used to register a callback for collision test queries. diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7cb865f9..4808ea65 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -113,6 +113,9 @@ void CollisionDetection::computeMiddlePhase() { // Make all the contact manifolds and contact points of the pair obsolete pair->makeContactsObsolete(); + // Make all the last frame collision info obsolete + pair->makeLastFrameCollisionInfosObsolete(); + ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); @@ -192,6 +195,9 @@ void CollisionDetection::computeMiddlePhase() { // Not handled continue; } + + // Remove the obsolete last frame collision infos + pair->clearObsoleteLastFrameCollisionInfos(); } } } @@ -263,6 +269,8 @@ void CollisionDetection::computeNarrowPhase() { // If there is no collision algorithm between those two kinds of shapes, skip it if (narrowPhaseAlgorithm != nullptr) { + LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo(); + // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. @@ -271,14 +279,14 @@ void CollisionDetection::computeNarrowPhase() { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; + lastCollisionFrameInfo->wasColliding = true; } else { - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false; + lastCollisionFrameInfo->wasColliding = false; } // The previous frame collision info is now valid - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; + lastCollisionFrameInfo->isValid = true; } currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; @@ -471,6 +479,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin NarrowPhaseInfo* narrowPhaseInfo = nullptr; + pair->makeLastFrameCollisionInfosObsolete(); + // If both shapes are convex if ((isShape1Convex && isShape2Convex)) { @@ -490,6 +500,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo); } + pair->clearObsoleteLastFrameCollisionInfos(); + return narrowPhaseInfo; } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 80a9f456..8303f35c 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -410,5 +410,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint } } + #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index f22593ac..6740fe4f 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -62,6 +62,9 @@ void ContactManifoldInfo::reset() { ContactPointInfo* elementToDelete = element; element = element->next; + // Call the constructor + elementToDelete->~ContactPointInfo(); + // Delete the current element mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); } diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index 0bda49d4..58604087 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -29,14 +29,12 @@ using namespace reactphysics3d; // Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) -void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) { +void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex); + TriangleShape(trianglePoints, verticesNormals, shapeId); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h index 4927c6e0..e04d7a9c 100644 --- a/src/collision/MiddlePhaseTriangleCallback.h +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -82,8 +82,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { } /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) override; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index b4a92a38..660f6704 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -35,12 +35,15 @@ using namespace reactphysics3d; // Constructor NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator) + const Transform& shape2Transform, void** cachedData1, void** cachedData2, + Allocator& shapeAllocator) : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), contactPoints(nullptr), cachedCollisionData1(cachedData1), - cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) { + cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) { + // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); } // Destructor diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index c9b18094..b100939f 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -29,11 +29,12 @@ // Libraries #include "shapes/CollisionShape.h" #include "collision/ContactManifoldInfo.h" +#include "engine/OverlappingPair.h" /// Namespace ReactPhysics3D namespace reactphysics3d { -class OverlappingPair; +struct LastFrameCollisionInfo; // Class NarrowPhaseInfo /** @@ -70,12 +71,12 @@ struct NarrowPhaseInfo { // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 void** cachedCollisionData2; - /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) - Allocator& collisionShapeAllocator; - /// Pointer to the next element in the linked list NarrowPhaseInfo* next; + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + Allocator& collisionShapeAllocator; + /// Constructor NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, @@ -93,8 +94,16 @@ struct NarrowPhaseInfo { /// Reset the remaining contact points void resetContactPoints(); + + /// Get the last collision frame info for temporal coherence + LastFrameCollisionInfo* getLastFrameCollisionInfo() const; }; +// Get the last collision frame info for temporal coherence +inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const { + return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId()); +} + } #endif diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 315dcf7a..300ba5f2 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -50,10 +50,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh #endif + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -140,8 +143,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh } } - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = false; // Return true return true; @@ -153,8 +156,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // Run the SAT algorithm to find the separating axis and compute contact point bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; return isColliding; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 9f2bb47e..507f6043 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -45,10 +45,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* #endif + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; return isColliding; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index c0f436a6..dcfff45e 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -88,11 +88,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Create a simplex set VoronoiSimplex simplex; + // Get the last collision frame info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + // Get the previous point V (last cached separating axis) Vector3 v; - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) { - v = lastFrameInfo.gjkSeparatingAxis; + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) { + v = lastFrameCollisionInfo->gjkSeparatingAxis; assert(v.lengthSquare() > decimal(0.000001)); } else { @@ -117,7 +119,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v; + lastFrameCollisionInfo->gjkSeparatingAxis = v; // No intersection, we return return GJKResult::SEPARATED; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 327f4465..761bd5ec 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -74,7 +74,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow decimal minPenetrationDepth = DECIMAL_LARGEST; uint minFaceIndex = 0; - // For each face of the convex mesh for (uint f = 0; f < polyhedron->getNbFaces(); f++) { @@ -110,7 +109,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, minPenetrationDepth, normalWorld); - // Create the contact info object narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, @@ -468,156 +466,152 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 separatingEdge2A, separatingEdge2B; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // True if the shapes were overlapping in the previous frame and are // still overlapping on the same axis in this frame bool isTemporalCoherenceValid = false; - // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous - // frame collision data per triangle) - if (polyhedron1->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) { + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { - // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating - // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If - // the shapes are still separated along this axis, we directly exit with no collision. + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // If the previous separating axis (or axis with minimum penetration depth) - // was a face normal of polyhedron 1 - if (lastFrameInfo.satIsAxisFacePolyhedron1) { + // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 1 + if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) { - decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, - lastFrameInfo.satMinAxisFaceIndex); - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + lastFrameCollisionInfo->satMinAxisFaceIndex); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - isMinPenetrationFaceNormal = true; - isMinPenetrationFaceNormalPolyhedron1 = true; - - // Compute the contact points between two faces of two convex polyhedra. - // If contact points have been found, we report them without running the whole SAT algorithm - if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, - polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, - narrowPhaseInfo, minPenetrationDepth)) { - - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; - - return true; - } - else { // Contact points have not been found (the set of clipped points was empty) - - // Therefore, we need to run the whole SAT algorithm again - isTemporalCoherenceValid = false; - } - } + // Return no collision + return false; } - else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) - // was a face normal of polyhedron 2 - decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, - lastFrameInfo.satMinAxisFaceIndex); - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; - // Return no collision - return false; + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = true; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; } + else { // Contact points have not been found (the set of clipped points was empty) - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - if (isTemporalCoherenceValid) { - - minPenetrationDepth = penetrationDepth; - minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; - isMinPenetrationFaceNormal = true; - isMinPenetrationFaceNormalPolyhedron1 = false; - - // Compute the contact points between two faces of two convex polyhedra. - // If contact points have been found, we report them without running the whole SAT algorithm - if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, - polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, - narrowPhaseInfo, minPenetrationDepth)) { - - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; - - return true; - } - else { // Contact points have not been found (the set of clipped points was empty) - - // Therefore, we need to run the whole SAT algorithm again - isTemporalCoherenceValid = false; - } - } - } - else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges - - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index); - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index); - - Vector3 separatingAxisPolyhedron2Space; - - const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); - const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); - const Vector3 edge1Direction = edge1B - edge1A; - const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); - const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); - const Vector3 edge2Direction = edge2B - edge2A; - - // Compute the penetration depth - decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), - edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); - - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { - - // Return no collision - return false; - } - - // The two shapes are overlapping as in the previous frame and on the same axis, therefore - // we will skip the entire SAT algorithm because the minimum separating axis did not change - isTemporalCoherenceValid = lastFrameInfo.wasColliding; - - // Temporal coherence is valid only if the two edges build a minkowski - // face (and the cross product is therefore a candidate for separating axis - if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + // Therefore, we need to run the whole SAT algorithm again isTemporalCoherenceValid = false; } + } + } + else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 2 - if (isTemporalCoherenceValid) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, + lastFrameCollisionInfo->satMinAxisFaceIndex); - minPenetrationDepth = penetrationDepth; - isMinPenetrationFaceNormal = false; - isMinPenetrationFaceNormalPolyhedron1 = false; - minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; - minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; - separatingEdge1A = edge1A; - separatingEdge1B = edge1B; - separatingEdge2A = edge2A; - separatingEdge2B = edge2B; - minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = false; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = false; + } + } + } + else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges + + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); + + Vector3 separatingAxisPolyhedron2Space; + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; + + // Temporal coherence is valid only if the two edges build a minkowski + // face (and the cross product is therefore a candidate for separating axis + if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + isTemporalCoherenceValid = false; + } + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index; + minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; } } } @@ -631,9 +625,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = true; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinAxisFaceIndex = faceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; // We have found a separating axis return false; @@ -649,9 +643,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = true; - lastFrameInfo.satMinAxisFaceIndex = faceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; // We have found a separating axis return false; @@ -694,10 +688,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinEdge1Index = i; - lastFrameInfo.satMinEdge2Index = j; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = i; + lastFrameCollisionInfo->satMinEdge2Index = j; // We have found a separating axis return false; @@ -742,18 +736,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // because of a numerical issue if (!contactsFound) { - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; // Return no collision return false; } } - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; } else { // If we have an edge vs edge contact @@ -781,10 +775,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); } - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index; - lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index; + lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; } return true; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index ae3764ab..28a565c1 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; @@ -52,8 +55,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -76,8 +79,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; return isColliding; } diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index a89747c9..0b64190e 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Constructor CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) - : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) { + : mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) { } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index feb4c502..6792cc09 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -71,6 +71,9 @@ class CollisionShape { /// Scaling vector of the collision shape Vector3 mScaling; + /// Unique identifier of the shape inside an overlapping pair + uint mId; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -126,6 +129,9 @@ class CollisionShape { /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); + /// Return the id of the shape + uint getId() const; + /// Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; @@ -171,6 +177,11 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } +// Return the id of the shape +inline uint CollisionShape::getId() const { + return mId; +} + #ifdef IS_PROFILING_ACTIVE // Set the profiler diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 646638dd..ee90ce46 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -135,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh return raycastCallback.getIsHit(); } +// Compute the shape Id for a given triangle of the mesh +uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const { + + uint shapeId = 0; + + uint i=0; + while (i < subPart) { + + shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles(); + + i++; + } + + return shapeId + triangleIndex; +} + // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { @@ -162,9 +178,9 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Get the vertices normals of the triangle Vector3 verticesNormals[3]; mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); + // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, data[0], data[1]); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -194,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { smallestHitFraction = raycastInfo.hitFraction; mIsHit = true; } + } } diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 678d5325..2d0a3e9c 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -155,6 +155,9 @@ class ConcaveMeshShape : public ConcaveShape { /// Return the three vertex normals (in the array outVerticesNormals) of a triangle void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const; + /// Compute the shape Id for a given triangle of the mesh + uint computeTriangleShapeId(uint subPart, uint triangleIndex) const; + public: /// Constructor @@ -259,7 +262,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Call the callback to test narrow-phase collision with this triangle - mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals); + mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); } #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index abc3759b..eb00d8f0 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -46,8 +46,7 @@ class TriangleCallback { virtual ~TriangleCallback() = default; /// Report a triangle - virtual void testTriangle(uint meshSubPart, uint triangleIndex, - const Vector3* trianglePoints, const Vector3* verticesNormals)=0; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0; }; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 08ec19a4..74276a75 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -154,7 +154,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal}; // Test collision against the first triangle - callback.testTriangle(0, 0, trianglePoints, verticesNormals1); + callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0)); // Generate the second triangle for the current grid rectangle trianglePoints[0] = p3; @@ -174,7 +174,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal}; // Test collision against the second triangle - callback.testTriangle(0, 0, trianglePoints, verticesNormals2); + callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1)); } } } @@ -263,12 +263,10 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { } // Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) { +void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex); + TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 80c18d9c..f8b498d5 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -71,8 +71,7 @@ class TriangleOverlapCallback : public TriangleCallback { bool getIsHit() const {return mIsHit;} /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(uint meshSubPart, uint triangleIndex, - const Vector3* trianglePoints, const Vector3* verticesNormals) override; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; #ifdef IS_PROFILING_ACTIVE @@ -169,6 +168,9 @@ class HeightFieldShape : public ConcaveShape { /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; + /// Compute the shape Id for a given triangle + uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const; + public: /// Constructor @@ -270,6 +272,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim 0, 0, mass); } +// Compute the shape Id for a given triangle +inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const { + + return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement; +} + } #endif diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 8583e80d..fa6b4f6a 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -35,7 +35,8 @@ using namespace reactphysics3d; /** * @param radius Radius of the sphere (in meters) */ -SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { +SphereShape::SphereShape(decimal radius) + : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { assert(radius > decimal(0.0)); } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 020053b6..77d0929c 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -43,16 +43,15 @@ using namespace reactphysics3d; * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) { - mPoints[0] = point1; - mPoints[1] = point2; - mPoints[2] = point3; + mPoints[0] = vertices[0]; + mPoints[1] = vertices[1]; + mPoints[2] = vertices[2]; // Compute the triangle normal - mNormal = (point2 - point1).cross(point3 - point1); + mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); mNormal.normalize(); mVerticesNormals[0] = verticesNormals[0]; @@ -60,6 +59,8 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const mVerticesNormals[2] = verticesNormals[2]; mRaycastTestType = TriangleRaycastSide::FRONT; + + mId = shapeId; } // This method compute the smooth mesh contact with a triangle in case one of the two collision diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 8360c32b..60ec216b 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -72,12 +72,6 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; - /// Index of the mesh sub part in the original mesh - uint mMeshSubPart; - - /// Triangle index of the triangle in the sub mesh - uint mTriangleIndex; - // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -95,6 +89,9 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; + /// Generate the id of the shape (used for temporal coherence) + void generateId(); + // -------------------- Methods -------------------- // /// This method implements the technique described in Game Physics Pearl book @@ -107,8 +104,7 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId); /// Destructor virtual ~TriangleShape() override = default; @@ -164,12 +160,6 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const override; - /// Return the index of the sub part mesh of the original mesh - uint getMeshSubPart() const; - - /// Return the triangle index in the original mesh - uint getTriangleIndex() const; - /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, Vector3& localContactPointShape1, Vector3& localContactPointShape2, @@ -319,16 +309,6 @@ inline Vector3 TriangleShape::getCentroid() const { return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); } -// Return the index of the sub part mesh of the original mesh -inline uint TriangleShape::getMeshSubPart() const { - return mMeshSubPart; -} - -// Return the triangle index in the original mesh -inline uint TriangleShape::getTriangleIndex() const { - return mTriangleIndex; -} - // Return the number of half-edges of the polyhedron inline uint TriangleShape::getNbHalfEdges() const { return 6; diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index e2a9a5e5..739a9efb 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -28,7 +28,6 @@ // Libraries #include "body/CollisionBody.h" -#include "collision/NarrowPhaseInfo.h" #include "collision/ContactPointInfo.h" #include "configuration.h" #include "mathematics/mathematics.h" @@ -36,6 +35,8 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +struct NarrowPhaseInfo; + // Class ContactPoint /** * This class represents a collision contact point between two diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index f523d169..6c558d6d 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -259,4 +259,4 @@ inline void CollisionWorld::setProfilerName(std::string name) { } - #endif +#endif diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 0055e683..eba2fb80 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -27,20 +27,31 @@ #include #include "OverlappingPair.h" #include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator) - : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr), - mTempMemoryAllocator(temporaryMemoryAllocator) { + Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator) + : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr), + mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) { } // Destructor OverlappingPair::~OverlappingPair() { assert(mPotentialContactManifolds == nullptr); + + // Remove all the remaining last frame collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + + // Call the constructor + it->second->~LastFrameCollisionInfo(); + + // Release memory + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + } } // Create a new potential contact manifold using contact-points from narrow-phase @@ -137,3 +148,59 @@ void OverlappingPair::reducePotentialContactManifolds() { manifold = manifold->getNext(); } } + + +// Add a new last frame collision info if it does not exist for the given shapes already +void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { + + // Try to get the corresponding last frame collision info + auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2)); + + // If there is no collision info for those two shapes already + if (it == mLastFrameCollisionInfos.end()) { + + // Create a new collision info + LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) + LastFrameCollisionInfo(); + + // Add it into the map of collision infos + std::map, LastFrameCollisionInfo*>::iterator it; + auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo)); + assert(ret.second); + } + else { + + // The existing collision info is not obsolete + it->second->isObsolete = false; + } +} + + +// Delete all the obsolete last frame collision info +void OverlappingPair::clearObsoleteLastFrameCollisionInfos() { + + // For each collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Delete it + it->second->~LastFrameCollisionInfo(); + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + + mLastFrameCollisionInfos.erase(it++); + } + else { + ++it; + } + } +} + +// Make all the last frame collision infos obsolete +void OverlappingPair::makeLastFrameCollisionInfosObsolete() { + + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + it->second->isObsolete = true; + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index ce22bb7e..8c65e633 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -30,6 +30,7 @@ #include "collision/ContactManifoldSet.h" #include "collision/ProxyShape.h" #include "collision/shapes/CollisionShape.h" +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -47,6 +48,9 @@ struct LastFrameCollisionInfo { /// True if we have information about the previous frame bool isValid; + /// True if the frame info is obsolete (the collision shape are not overlapping in middle phase) + bool isObsolete; + /// True if the two shapes were colliding in the previous frame bool wasColliding; @@ -72,6 +76,7 @@ struct LastFrameCollisionInfo { LastFrameCollisionInfo() { isValid = false; + isObsolete = false; wasColliding = false; wasUsingSAT = false; wasUsingGJK = false; @@ -97,12 +102,19 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Collision information about the last frame (for temporal coherence) - LastFrameCollisionInfo mLastFrameCollisionInfo; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + std::map, LastFrameCollisionInfo*> mLastFrameCollisionInfos; /// Linked-list of potential contact manifold ContactManifoldInfo* mPotentialContactManifolds; + /// Persistent memory allocator + Allocator& mPersistentAllocator; + /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo Allocator& mTempMemoryAllocator; @@ -111,8 +123,8 @@ class OverlappingPair { // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator); + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, Allocator& persistentMemoryAllocator, + Allocator& temporaryMemoryAllocator); /// Destructor ~OverlappingPair(); @@ -130,7 +142,7 @@ class OverlappingPair { ProxyShape* getShape2() const; /// Return the last frame collision info - LastFrameCollisionInfo& getLastFrameCollisionInfo(); + LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair shapeIds); /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); @@ -168,6 +180,18 @@ class OverlappingPair { /// Reduce the contact manifolds that have too many contact points void reduceContactManifolds(); + /// Add a new last frame collision info if it does not exist for the given shapes already + void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2); + + /// Return the last frame collision info for a given pair of shape ids + LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const; + + /// Delete all the obsolete last frame collision info + void clearObsoleteLastFrameCollisionInfos(); + + /// Make all the last frame collision infos obsolete + void makeLastFrameCollisionInfosObsolete(); + /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -194,9 +218,14 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta mContactManifoldSet.addContactManifold(contactManifoldInfo); } -// Return the last frame collision info -inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() { - return mLastFrameCollisionInfo; +// Return the last frame collision info for a given shape id or nullptr if none is found +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair shapeIds) { + std::map, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds); + if (it != mLastFrameCollisionInfos.end()) { + return it->second; + } + + return nullptr; } // Return the contact manifold @@ -265,6 +294,11 @@ inline void OverlappingPair::reduceContactManifolds() { mContactManifoldSet.reduce(); } +// Return the last frame collision info for a given pair of shape ids +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const { + return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2)); +} + } #endif diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index f2ae3d6b..c62ca179 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -198,11 +198,12 @@ class TestRaycast : public Test { mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - const Vector3 triangleVertex1(100, 100, 0); - const Vector3 triangleVertex2(105, 100, 0); - const Vector3 triangleVertex3(100, 103, 0); + Vector3 triangleVertices[3]; + triangleVertices[0] = Vector3(100, 100, 0); + triangleVertices[1] = Vector3(105, 100, 0); + triangleVertices[2] = Vector3(100, 103, 0); Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; - mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0); + mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5); From f09331c185ddc81a3415ed26e565fd94ae2e71fa Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 Nov 2017 22:58:31 +0100 Subject: [PATCH 086/105] Remove unused cachedCollisionData variable --- src/collision/CollisionDetection.cpp | 6 ++---- src/collision/MiddlePhaseTriangleCallback.cpp | 3 +-- src/collision/NarrowPhaseInfo.cpp | 6 ++---- src/collision/NarrowPhaseInfo.h | 10 +--------- src/collision/ProxyShape.cpp | 7 +------ src/collision/ProxyShape.h | 11 ----------- 6 files changed, 7 insertions(+), 36 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 4808ea65..f1a7b80a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -169,8 +169,7 @@ void CollisionDetection::computeMiddlePhase() { mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData(), mSingleFrameAllocator); + shape2->getLocalToWorldTransform(), mSingleFrameAllocator); mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; } @@ -488,8 +487,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // for the narrow-phase collision detection narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData(), mPoolAllocator); + shape2->getLocalToWorldTransform(), mPoolAllocator); } // Concave vs Convex algorithm diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index 58604087..f8736a0b 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -55,7 +55,6 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), - shape1->getCachedCollisionData(), - shape2->getCachedCollisionData(), mAllocator); + mAllocator); narrowPhaseInfoList->next = firstNarrowPhaseInfo; } diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index 660f6704..a6440367 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -35,12 +35,10 @@ using namespace reactphysics3d; // Constructor NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2, - Allocator& shapeAllocator) + const Transform& shape2Transform, Allocator& shapeAllocator) : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), - contactPoints(nullptr), cachedCollisionData1(cachedData1), - cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) { + contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) { // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index b100939f..b732480c 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -63,14 +63,6 @@ struct NarrowPhaseInfo { /// Linked-list of contact points created during the narrow-phase ContactPointInfo* contactPoints; - /// Cached collision data of the proxy shape - // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 - void** cachedCollisionData1; - - /// Cached collision data of the proxy shape - // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 - void** cachedCollisionData2; - /// Pointer to the next element in the linked list NarrowPhaseInfo* next; @@ -80,7 +72,7 @@ struct NarrowPhaseInfo { /// Constructor NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator); + const Transform& shape2Transform, Allocator& shapeAllocator); /// Destructor ~NarrowPhaseInfo(); diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 50418011..6e6eefbd 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -37,18 +37,13 @@ using namespace reactphysics3d; */ ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(nullptr), mBroadPhaseID(-1), mCachedCollisionData(nullptr), mUserData(nullptr), - mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { + mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { } // Destructor ProxyShape::~ProxyShape() { - // Release the cached collision data memory - if (mCachedCollisionData != nullptr) { - free(mCachedCollisionData); - } } // Return true if a point is inside the collision shape diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 574758d4..8e9a9e9f 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -66,9 +66,6 @@ class ProxyShape { /// Broad-phase ID (node ID in the dynamic AABB tree) int mBroadPhaseID; - /// Cached collision data - void* mCachedCollisionData; - /// Pointer to user data void* mUserData; @@ -168,9 +165,6 @@ class ProxyShape { /// Return the next proxy shape in the linked list of proxy shapes const ProxyShape* getNext() const; - /// Return the pointer to the cached collision data - void** getCachedCollisionData(); - /// Return the local scaling vector of the collision shape Vector3 getLocalScaling() const; @@ -201,11 +195,6 @@ class ProxyShape { }; -// Return the pointer to the cached collision data -inline void** ProxyShape::getCachedCollisionData() { - return &mCachedCollisionData; -} - // Return the collision shape /** * @return Pointer to the internal collision shape From fea467f112e29f53dad44e8ada4077f8c4450505 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 25 Nov 2017 17:51:14 +0100 Subject: [PATCH 087/105] Add profiling in SAT algorithm methods --- src/collision/CollisionDetection.cpp | 6 +++++ .../narrowphase/SAT/SATAlgorithm.cpp | 22 +++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index f1a7b80a..a2c3344d 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -366,6 +366,8 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::addAllContactManifoldsToBodies() { + PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { @@ -413,6 +415,8 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { /// Convert the potential contact into actual contacts void CollisionDetection::processAllPotentialContacts() { + PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { @@ -450,6 +454,8 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { // Report contacts for all the colliding overlapping pairs void CollisionDetection::reportAllContacts() { + PROFILE("CollisionDetection::reportAllContacts()", mProfiler); + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 761bd5ec..f12964cf 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -122,6 +122,8 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron, const SphereShape* sphere, const Vector3& sphereCenter) const { + PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler); + // Get the face HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex); @@ -297,6 +299,8 @@ decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const Con const Vector3& edgeDirectionCapsuleSpace, const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const { + PROFILE("SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth)", mProfiler); + decimal penetrationDepth = DECIMAL_LARGEST; // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) @@ -329,6 +333,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform, Vector3& outFaceNormalCapsuleSpace) const { + PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler); + // Get the face HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex); @@ -353,6 +359,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { + PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler); + HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); // Get the face normal @@ -791,6 +799,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const { + PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler); + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; @@ -903,6 +913,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // This is used to find the incident face on a polyhedron of a given reference face of another polyhedron uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { + PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler); + decimal minDotProduct = DECIMAL_LARGEST; uint mostAntiParallelFace = 0; @@ -925,6 +937,8 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V const Vector3& edge1Direction, const Vector3& edge2Direction, Vector3& outSeparatingAxisPolyhedron2Space) const { + PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler); + // If the two edges are parallel if (areParallelVectors(edge1Direction, edge2Direction)) { @@ -952,6 +966,8 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex const Transform& polyhedron1ToPolyhedron2, uint faceIndex) const { + PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler); + HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex); // Get the face normal @@ -976,6 +992,8 @@ decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyh const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const { + PROFILE("SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron", mProfiler); + decimal minPenetrationDepth = DECIMAL_LARGEST; // For each face of the first polyhedron @@ -1006,6 +1024,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, const Transform& polyhedron1ToPolyhedron2) const { + PROFILE("SATAlgorithm::testEdgesBuildMinkowskiFace", mProfiler); + const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex); const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); @@ -1037,6 +1057,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& bCrossA, const Vector3& dCrossC) const { + PROFILE("SATAlgorithm::testGaussMapArcsIntersect", mProfiler); + const decimal cba = c.dot(bCrossA); const decimal dba = d.dot(bCrossA); const decimal adc = a.dot(dCrossC); From c8e9cca9123295e2994a3530a1114917aaf4dc8a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 26 Nov 2017 12:07:58 +0100 Subject: [PATCH 088/105] Compute the inverse quaternion using its conjugate --- src/mathematics/Quaternion.h | 24 +++++++----------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 28f7b636..b8e75306 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -224,16 +224,11 @@ inline void Quaternion::normalize() { // Inverse the quaternion inline void Quaternion::inverse() { - // Get the square length of the quaternion - decimal lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - x /= -lengthSquareQuaternion; - y /= -lengthSquareQuaternion; - z /= -lengthSquareQuaternion; - w /= lengthSquareQuaternion; + // Use the conjugate of the current quaternion + x = -x; + y = -y; + z = -z; + w = w; } // Return the unit quaternion @@ -261,13 +256,8 @@ inline Quaternion Quaternion::getConjugate() const { // Return the inverse of the quaternion (inline) inline Quaternion Quaternion::getInverse() const { - decimal lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, - -z / lengthSquareQuaternion, w / lengthSquareQuaternion); + // Return the conjugate quaternion + return Quaternion(-x, -y, -z, w); } // Scalar product between two quaternions From 317dea90bd5287085f3b4cd34718c06682db260f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Nov 2017 17:26:13 +0100 Subject: [PATCH 089/105] Remove Quaternion constructor with Euler angles and replace it by static fromEulerAngles() method --- src/mathematics/Quaternion.cpp | 66 ++++++++++--------- src/mathematics/Quaternion.h | 14 ++-- test/tests/collision/TestPointInside.h | 6 +- test/tests/collision/TestRaycast.h | 6 +- test/tests/mathematics/TestQuaternion.h | 8 +-- .../CollisionDetectionScene.cpp | 12 ++-- 6 files changed, 59 insertions(+), 53 deletions(-) diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 0b2587f5..d0e1ae61 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -47,14 +47,28 @@ Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), } -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) { - initWithEulerAngles(angleX, angleY, angleZ); +// Constructor with the component w and the vector v=(x y z) +Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { + } -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(const Vector3& eulerAngles) { - initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { + + Quaternion quaternion; + quaternion.initWithEulerAngles(angleX, angleY, angleZ); + + return quaternion; +} + + +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(const Vector3& eulerAngles) { + + Quaternion quaternion; + quaternion.initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); + + return quaternion; } // Copy-constructor @@ -72,10 +86,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { decimal r; decimal s; - if (trace < 0.0) { + if (trace < decimal(0.0)) { if (matrix[1][1] > matrix[0][0]) { if(matrix[2][2] > matrix[1][1]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -85,7 +99,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); + r = std::sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -96,7 +110,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else if (matrix[2][2] > matrix[0][0]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -106,7 +120,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); + r = std::sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -117,7 +131,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else { - r = sqrt(trace + decimal(1.0)); + r = std::sqrt(trace + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -132,22 +146,12 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { /// This method is used to get the rotation angle (in radian) and the unit /// rotation axis of an orientation quaternion. void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const { - Quaternion quaternion; - - // If the quaternion is unit - if (length() == 1.0) { - quaternion = *this; - } - else { - // We compute the unit quaternion - quaternion = getUnit(); - } // Compute the roation angle - angle = acos(quaternion.w) * decimal(2.0); + angle = std::acos(w) * decimal(2.0); // Compute the 3D rotation axis - Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); + Vector3 rotationAxis(x, y, z); // Normalize the rotation axis rotationAxis = rotationAxis.getUnit(); @@ -162,7 +166,7 @@ Matrix3x3 Quaternion::getMatrix() const { decimal nQ = x*x + y*y + z*z + w*w; decimal s = 0.0; - if (nQ > 0.0) { + if (nQ > decimal(0.0)) { s = decimal(2.0) / nQ; } @@ -190,7 +194,7 @@ Matrix3x3 Quaternion::getMatrix() const { /// The t argument has to be such that 0 <= t <= 1. This method is static. Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) { - assert(t >= 0.0 && t <= 1.0); + assert(t >= decimal(0.0) && t <= decimal(1.0)); decimal invert = 1.0; @@ -198,7 +202,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, decimal cosineTheta = quaternion1.dot(quaternion2); // Take care of the sign of cosineTheta - if (cosineTheta < 0.0) { + if (cosineTheta < decimal(0.0)) { cosineTheta = -cosineTheta; invert = -1.0; } @@ -212,14 +216,14 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, } // Compute the theta angle - decimal theta = acos(cosineTheta); + decimal theta = std::acos(cosineTheta); // Compute sin(theta) - decimal sineTheta = sin(theta); + decimal sineTheta = std::sin(theta); // Compute the two coefficients that are in the spherical linear interpolation formula - decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta; - decimal coeff2 = sin(t*theta) / sineTheta * invert; + decimal coeff1 = std::sin((decimal(1.0)-t)*theta) / sineTheta; + decimal coeff2 = std::sin(t*theta) / sineTheta * invert; // Compute and return the interpolated quaternion return quaternion1 * coeff1 + quaternion2 * coeff2; diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index b8e75306..74415eb8 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -69,11 +69,8 @@ struct Quaternion { /// Constructor with the component w and the vector v=(x y z) Quaternion(decimal newW, const Vector3& v); - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(decimal angleX, decimal angleY, decimal angleZ); - - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(const Vector3& eulerAngles); + /// Constructor with the component w and the vector v=(x y z) + Quaternion(const Vector3& v, decimal newW); /// Copy-constructor Quaternion(const Quaternion& quaternion); @@ -123,6 +120,12 @@ struct Quaternion { /// Return the identity quaternion static Quaternion identity(); + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ); + + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(const Vector3& eulerAngles); + /// Dot product between two quaternions decimal dot(const Quaternion& quaternion) const; @@ -228,7 +231,6 @@ inline void Quaternion::inverse() { x = -x; y = -y; z = -z; - w = w; } // Return the unit quaternion diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 608b439b..afdb415c 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -93,7 +93,7 @@ class TestPointInside : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -108,7 +108,7 @@ class TestPointInside : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -165,7 +165,7 @@ class TestPointInside : public Test { // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index c62ca179..97dd002a 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -169,7 +169,7 @@ class TestRaycast : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -185,7 +185,7 @@ class TestRaycast : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space @@ -246,7 +246,7 @@ class TestRaycast : public Test { // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 5fe0ccd0..dfc65955 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -94,7 +94,7 @@ class TestQuaternion : public Test { const decimal PI_OVER_2 = PI * decimal(0.5); const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); - Quaternion quaternion5(PI_OVER_2, 0, 0); + Quaternion quaternion5 = Quaternion::fromEulerAngles(PI_OVER_2, 0, 0); Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); quaternionTest5.normalize(); test(approxEqual(quaternion5.x, quaternionTest5.x)); @@ -102,7 +102,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion5.z, quaternionTest5.z)); test(approxEqual(quaternion5.w, quaternionTest5.w)); - Quaternion quaternion6(0, PI_OVER_2, 0); + Quaternion quaternion6 = Quaternion::fromEulerAngles(0, PI_OVER_2, 0); Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); quaternionTest6.normalize(); test(approxEqual(quaternion6.x, quaternionTest6.x)); @@ -110,7 +110,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion6.z, quaternionTest6.z)); test(approxEqual(quaternion6.w, quaternionTest6.w)); - Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); + Quaternion quaternion7 = Quaternion::fromEulerAngles(Vector3(0, 0, PI_OVER_2)); Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); quaternionTest7.normalize(); test(approxEqual(quaternion7.x, quaternionTest7.x)); @@ -124,7 +124,7 @@ class TestQuaternion : public Test { // Test method that returns the length Quaternion quaternion(2, 3, -4, 5); - test(approxEqual(quaternion.length(), sqrt(decimal(54.0)))); + test(approxEqual(quaternion.length(), std::sqrt(decimal(54.0)))); // Test method that returns a unit quaternion test(approxEqual(quaternion.getUnit().length(), 1.0)); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 1241b701..6f2c65ae 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -282,32 +282,32 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i } else if (key == GLFW_KEY_A && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, stepAngle, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_D && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, -stepAngle, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_W && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(stepAngle, 0, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_S && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(-stepAngle, 0, 0) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_F && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, stepAngle) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } else if (key == GLFW_KEY_G && action == GLFW_PRESS) { rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); - transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation()); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, -stepAngle) * transform.getOrientation()); mAllShapes[mSelectedShapeIndex]->setTransform(transform); } From e754711a84264c6f0db646d1bff620c5918ea2ec Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Nov 2017 17:46:45 +0100 Subject: [PATCH 090/105] Remove unnecessary calls to Quaternion.getMatrix() --- src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 5 ++--- src/mathematics/Transform.h | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index dcfff45e..62685867 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -75,10 +75,9 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase Transform transform1Inverse = transform1.getInverse(); Transform body2Tobody1 = transform1Inverse * transform2; - // Matrix that transform a direction from local + // Quaternion that transform a direction from local // space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); + Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Initialize the margin (sum of margins of both objects) decimal margin = shape1->getMargin() + shape2->getMargin(); diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index a93734cd..ab17a620 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -169,8 +169,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { // Return the inverse of the transform inline Transform Transform::getInverse() const { const Quaternion& invQuaternion = mOrientation.getInverse(); - Matrix3x3 invMatrix = invQuaternion.getMatrix(); - return Transform(invMatrix * (-mPosition), invQuaternion); + return Transform(invQuaternion * (-mPosition), invQuaternion); } // Return an interpolated transform @@ -200,7 +199,7 @@ inline Vector3 Transform::operator*(const Vector3& vector) const { // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(mPosition + mOrientation.getMatrix() * transform2.mPosition, + return Transform(mPosition + mOrientation * transform2.mPosition, mOrientation * transform2.mOrientation); } From ebd715d2e080a7547641a1065677e64256ec2f03 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Nov 2017 17:53:50 +0100 Subject: [PATCH 091/105] Add data types --- src/configuration.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/configuration.h b/src/configuration.h index 779b45e2..a25c1502 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -54,9 +54,12 @@ using luint = long unsigned int; using bodyindex = luint; using bodyindexpair = std::pair; -using int8 = int8_t; -using int16 = int16_t; -using int32 = int32_t; +using int8 = std::int8_t; +using uint8 = std::uint8_t; +using int16 = std::int16_t; +using uint16 = std::uint16_t; +using int32 = std::int32_t; +using uint32 = std::uint32_t; // ------------------- Enumerations ------------------- // From 4cc024b85e9d384f423ec4632975292ac45bdada Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 29 Nov 2017 23:43:55 +0100 Subject: [PATCH 092/105] Rename fields and methods in ContactPoint class --- src/constraint/ContactPoint.cpp | 8 ++--- src/constraint/ContactPoint.h | 30 +++++++++---------- src/engine/ContactSolver.cpp | 4 +-- .../CollisionDetectionScene.h | 4 +-- testbed/src/SceneDemo.cpp | 2 +- 5 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index b99d52f1..37dc34a3 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -34,8 +34,8 @@ using namespace std; ContactPoint::ContactPoint(const ContactPointInfo* contactInfo) : mNormal(contactInfo->normal), mPenetrationDepth(contactInfo->penetrationDepth), - mLocalPointOnBody1(contactInfo->localPoint1), - mLocalPointOnBody2(contactInfo->localPoint2), + mLocalPointOnShape1(contactInfo->localPoint1), + mLocalPointOnShape2(contactInfo->localPoint2), mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) { assert(mPenetrationDepth > decimal(0.0)); @@ -53,8 +53,8 @@ void ContactPoint::update(const ContactPointInfo* contactInfo) { mNormal = contactInfo->normal; mPenetrationDepth = contactInfo->penetrationDepth; - mLocalPointOnBody1 = contactInfo->localPoint1; - mLocalPointOnBody2 = contactInfo->localPoint2; + mLocalPointOnShape1 = contactInfo->localPoint1; + mLocalPointOnShape2 = contactInfo->localPoint2; mIsObsolete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 739a9efb..40a3d13c 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -54,11 +54,11 @@ class ContactPoint { /// Penetration depth decimal mPenetrationDepth; - /// Contact point on body 1 in local space of body 1 - Vector3 mLocalPointOnBody1; + /// Contact point on proxy shape 1 in local-space of proxy shape 1 + Vector3 mLocalPointOnShape1; - /// Contact point on body 2 in local space of body 2 - Vector3 mLocalPointOnBody2; + /// Contact point on proxy shape 2 in local-space of proxy shape 2 + Vector3 mLocalPointOnShape2; /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; @@ -121,11 +121,11 @@ class ContactPoint { /// Return the normal vector of the contact Vector3 getNormal() const; - /// Return the contact local point on body 1 - Vector3 getLocalPointOnBody1() const; + /// Return the contact point on the first proxy shape in the local-space of the proxy shape + Vector3 getLocalPointOnShape1() const; - /// Return the contact local point on body 2 - Vector3 getLocalPointOnBody2() const; + /// Return the contact point on the second proxy shape in the local-space of the proxy shape + Vector3 getLocalPointOnShape2() const; /// Return the cached penetration impulse decimal getPenetrationImpulse() const; @@ -156,14 +156,14 @@ inline Vector3 ContactPoint::getNormal() const { return mNormal; } -// Return the contact point on body 1 -inline Vector3 ContactPoint::getLocalPointOnBody1() const { - return mLocalPointOnBody1; +// Return the contact point on the first proxy shape in the local-space of the proxy shape +inline Vector3 ContactPoint::getLocalPointOnShape1() const { + return mLocalPointOnShape1; } -// Return the contact point on body 2 -inline Vector3 ContactPoint::getLocalPointOnBody2() const { - return mLocalPointOnBody2; +// Return the contact point on the second proxy shape in the local-space of the proxy shape +inline Vector3 ContactPoint::getLocalPointOnShape2() const { + return mLocalPointOnShape2; } // Return the cached penetration impulse @@ -173,7 +173,7 @@ inline decimal ContactPoint::getPenetrationImpulse() const { // Return true if the contact point is similar (close enougth) to another given contact point inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const { - return (localContactPointBody1->localPoint1 - mLocalPointOnBody1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD * + return (localContactPointBody1->localPoint1 - mLocalPointOnShape1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD); } diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 1e5b5401..167d37b8 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -157,8 +157,8 @@ void ContactSolver::initializeForIsland(Island* island) { while (externalContact != nullptr) { // Get the contact point on the two bodies - Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1(); - Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2(); + Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1(); + Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2(); new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = externalContact; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index e5bd8db9..9a90f25c 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -94,13 +94,13 @@ class ContactManager : public rp3d::CollisionCallback { rp3d::Vector3 normal = contactPoint->getNormal(); openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); - rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1(); + rp3d::Vector3 point1 = contactPoint->getLocalPointOnShape1(); point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; openglframework::Vector3 position1(point1.x, point1.y, point1.z); mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); - rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2(); + rp3d::Vector3 point2 = contactPoint->getLocalPointOnShape2(); point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; openglframework::Vector3 position2(point2.x, point2.y, point2.z); mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index db5d030e..5ef6fb95 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -435,7 +435,7 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn rp3d::ContactPoint* contactPoint = manifold->getContactPoints(); while (contactPoint != nullptr) { - rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnBody1(); + rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnShape1(); rp3d::Vector3 normalWorld = contactPoint->getNormal(); openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); From 4f76553c59ae7410afa89f649a7aa089bb34a302 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 4 Dec 2017 22:14:52 +0100 Subject: [PATCH 093/105] Many small optimizations --- src/collision/CollisionDetection.cpp | 12 +-- src/collision/CollisionDetection.h | 1 - src/collision/HalfEdgeStructure.h | 18 ++-- src/collision/PolyhedronMesh.cpp | 2 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 3 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 8 +- .../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +- ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/NarrowPhaseAlgorithm.h | 5 +- .../narrowphase/SAT/SATAlgorithm.cpp | 61 ++++++------ src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 5 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 3 +- .../narrowphase/SphereVsSphereAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 7 +- src/collision/shapes/BoxShape.h | 14 +-- src/collision/shapes/ConvexMeshShape.h | 8 +- src/collision/shapes/ConvexPolyhedronShape.h | 4 +- src/collision/shapes/TriangleShape.cpp | 99 ++++++++++--------- src/collision/shapes/TriangleShape.h | 37 +++---- src/mathematics/Quaternion.h | 27 ++++- src/mathematics/Transform.h | 2 +- src/mathematics/mathematics_functions.cpp | 31 +++--- src/mathematics/mathematics_functions.h | 5 +- .../mathematics/TestMathematicsFunctions.h | 62 ++++++------ 30 files changed, 235 insertions(+), 204 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index a2c3344d..c0b4d4e9 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -273,7 +273,7 @@ void CollisionDetection::computeNarrowPhase() { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -593,7 +593,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); } } @@ -688,7 +688,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); } } @@ -767,7 +767,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -859,7 +859,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -943,7 +943,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index fc33c00f..d728a95f 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -150,7 +150,6 @@ class CollisionDetection { /// Process the potential contacts where one collion is a concave shape void processSmoothMeshContacts(OverlappingPair* pair); - public : diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index 6b4e3aa1..ec4fff35 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -106,13 +106,13 @@ class HalfEdgeStructure { uint getNbVertices() const; /// Return a given face - Face getFace(uint index) const; + const Face& getFace(uint index) const; /// Return a given edge - Edge getHalfEdge(uint index) const; + const Edge& getHalfEdge(uint index) const; /// Return a given vertex - Vertex getVertex(uint index) const; + const Vertex& getVertex(uint index) const; }; @@ -133,33 +133,33 @@ inline void HalfEdgeStructure::addFace(std::vector faceVertices) { // Return the number of faces inline uint HalfEdgeStructure::getNbFaces() const { - return mFaces.size(); + return static_cast(mFaces.size()); } // Return the number of edges inline uint HalfEdgeStructure::getNbHalfEdges() const { - return mEdges.size(); + return static_cast(mEdges.size()); } // Return the number of vertices inline uint HalfEdgeStructure::getNbVertices() const { - return mVertices.size(); + return static_cast(mVertices.size()); } // Return a given face -inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const { +inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const { assert(index < mFaces.size()); return mFaces[index]; } // Return a given edge -inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const { +inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const { assert(index < mEdges.size()); return mEdges[index]; } // Return a given vertex -inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const { +inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const { assert(index < mVertices.size()); return mVertices[index]; } diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index b4f82fad..ec8b661d 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -123,7 +123,7 @@ void PolyhedronMesh::computeFacesNormals() { // For each face for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { - HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f); + const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f); assert(face.faceVertices.size() >= 3); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index b15cb186..bf9eefc2 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -33,7 +33,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two capsules // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 0b19338f..df9a8854 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 300ba5f2..f943312b 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -37,11 +37,12 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a capsule and a polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE @@ -85,9 +86,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // For each face of the polyhedron for (uint f = 0; f < polyhedron->getNbFaces(); f++) { - // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(f); - const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index c0dd46d5..d7f96d63 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 507f6043..5cf10b17 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -34,10 +34,11 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between two convex polyhedra // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index 1e0c9748..204bc269 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 34fa5962..40d3fbc6 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -90,8 +90,9 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0; + /// Compute a contact info if the two bounding volumes collide + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator)=0; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index f12964cf..7ef99e1f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -44,6 +44,11 @@ using namespace reactphysics3d; // Static variables initialization const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); +// Constructor +SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { + +} + // Test collision between a sphere and a convex mesh bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { @@ -125,7 +130,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler); // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex); // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex); @@ -200,12 +205,12 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { // Get an edge from the polyhedron (convert it into the capsule local-space) - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(e); const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); @@ -336,7 +341,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler); // Get the face - HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex); // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex); @@ -361,7 +366,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler); - HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); + const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex); // Get the face normal Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); @@ -375,8 +380,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // For each adjacent edge of the separating face of the polyhedron do { - HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); - HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex); + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); // Compute the edge vertices and edge direction Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex); @@ -575,8 +580,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn } else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); Vector3 separatingAxisPolyhedron2Space; @@ -669,7 +674,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { // Get an edge of polyhedron 1 - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i); const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); @@ -678,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { // Get an edge of polyhedron 2 - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j); const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); @@ -816,23 +821,24 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); // Get the reference face - HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex); + const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); // Find the incident face on the other polyhedron (most anti-parallel face) uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); // Get the incident face - HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); - std::vector polygonVertices; // Vertices to clip of the incident face - std::vector planesNormals; // Normals of the clipping planes - std::vector planesPoints; // Points on the clipping planes + uint nbIncidentFaceVertices = static_cast(incidentFace.faceVertices.size()); + List polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face + List planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes + List planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes // Get all the vertices of the incident face (in the reference local-space) std::vector::const_iterator it; for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); - polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace); + polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); } // Get the reference face clipping planes @@ -841,10 +847,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene do { // Get the adjacent edge - HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); // Compute the edge vertices and edge direction Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); @@ -855,8 +861,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // The clipping plane is perpendicular to the edge direction and the reference face normal Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - planesNormals.push_back(clipPlaneNormal); - planesPoints.push_back(edgeV1); + planesNormals.add(clipPlaneNormal); + planesPoints.add(edgeV1); // Go to the next adjacent edge of the reference face currentEdgeIndex = edge.nextEdgeIndex; @@ -867,17 +873,16 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene assert(planesNormals.size() == planesPoints.size()); // Clip the reference faces with the adjacent planes of the reference face - std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); - std::vector::const_iterator itPoints; bool contactPointsFound = false; - for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) { + for (uint i=0; i decimal(0.0)) { @@ -887,10 +892,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene Vector3 outWorldNormal = normalWorld; // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i]; // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex); + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, @@ -968,7 +973,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler); - HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex); + const HalfEdgeStructure::Face& face = polyhedron1->getFace(faceIndex); // Get the face normal const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 60f7a091..af49ca09 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -50,6 +50,9 @@ class SATAlgorithm { /// make sure the contact manifold does not change too much between frames. static const decimal SAME_SEPARATING_AXIS_BIAS; + /// Memory allocator + Allocator& mMemoryAllocator; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -115,7 +118,7 @@ class SATAlgorithm { // -------------------- Methods -------------------- // /// Constructor - SATAlgorithm() = default; + SATAlgorithm(Allocator& memoryAllocator); /// Destructor ~SATAlgorithm() = default; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a5d7a6ac..f5bfc11d 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -34,7 +34,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a capsule // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index bb9e9866..b51bba57 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 28a565c1..6cc3e578 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -34,7 +34,8 @@ using namespace reactphysics3d; // Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. -bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -69,7 +70,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 2d9fa801..17055431 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 0b520213..f8820106 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -30,7 +30,8 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + Allocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 1fd885a5..e0741ab8 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; }; } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 6b8c8d6b..37667958 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -84,10 +84,9 @@ BoxShape::BoxShape(const Vector3& extent) */ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { decimal factor = (decimal(1.0) / decimal(3.0)) * mass; - Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); - decimal xSquare = realExtent.x * realExtent.x; - decimal ySquare = realExtent.y * realExtent.y; - decimal zSquare = realExtent.z * realExtent.z; + decimal xSquare = mExtent.x * mExtent.x; + decimal ySquare = mExtent.y * mExtent.y; + decimal zSquare = mExtent.z * mExtent.z; tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 5643a5c8..d1aa9810 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -101,7 +101,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -113,7 +113,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the position of a given vertex virtual Vector3 getVertexPosition(uint vertexIndex) const override; @@ -130,7 +130,7 @@ class BoxShape : public ConvexPolyhedronShape { * @return The vector with the three extents of the box shape (in meters) */ inline Vector3 BoxShape::getExtent() const { - return mExtent + Vector3(mMargin, mMargin, mMargin); + return mExtent; } // Set the scaling vector of the collision shape @@ -150,7 +150,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) { inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds - max = mExtent + Vector3(mMargin, mMargin, mMargin); + max = mExtent; // Minimum bounds min = -max; @@ -161,7 +161,7 @@ inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); } -// Return a local support point in a given direction without the objec margin +// Return a local support point in a given direction without the object margin inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x, @@ -182,7 +182,7 @@ inline uint BoxShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const { +inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const { assert(faceIndex < mHalfEdgeStructure.getNbFaces()); return mHalfEdgeStructure.getFace(faceIndex); } @@ -243,7 +243,7 @@ inline uint BoxShape::getNbHalfEdges() const { } // Return a given half-edge of the polyhedron -inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const { +inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const { assert(edgeIndex < getNbHalfEdges()); return mHalfEdgeStructure.getHalfEdge(edgeIndex); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 71077af2..0ce696af 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -111,7 +111,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -123,7 +123,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the position of a given vertex virtual Vector3 getVertexPosition(uint vertexIndex) const override; @@ -191,7 +191,7 @@ inline uint ConvexMeshShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const { +inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const { assert(faceIndex < getNbFaces()); return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); } @@ -213,7 +213,7 @@ inline uint ConvexMeshShape::getNbHalfEdges() const { } // Return a given half-edge of the polyhedron -inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const { +inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const { assert(edgeIndex < getNbHalfEdges()); return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); } diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 96707d69..1d944cb8 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -62,7 +62,7 @@ class ConvexPolyhedronShape : public ConvexShape { virtual uint getNbFaces() const=0; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const=0; @@ -80,7 +80,7 @@ class ConvexPolyhedronShape : public ConvexShape { virtual uint getNbHalfEdges() const=0; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0; /// Return true if the collision shape is a polyhedron virtual bool isPolyhedron() const override; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 77d0929c..e43cc674 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -58,6 +58,57 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor mVerticesNormals[1] = verticesNormals[1]; mVerticesNormals[2] = verticesNormals[2]; + // Faces + for (uint i=0; i<2; i++) { + mFaces[i].faceVertices.push_back(0); + mFaces[i].faceVertices.push_back(1); + mFaces[i].faceVertices.push_back(2); + mFaces[i].edgeIndex = i; + } + + // Edges + for (uint i=0; i<6; i++) { + switch(i) { + case 0: + mEdges[0].vertexIndex = 0; + mEdges[0].twinEdgeIndex = 1; + mEdges[0].faceIndex = 0; + mEdges[0].nextEdgeIndex = 2; + break; + case 1: + mEdges[1].vertexIndex = 1; + mEdges[1].twinEdgeIndex = 0; + mEdges[1].faceIndex = 1; + mEdges[1].nextEdgeIndex = 5; + break; + case 2: + mEdges[2].vertexIndex = 1; + mEdges[2].twinEdgeIndex = 3; + mEdges[2].faceIndex = 0; + mEdges[2].nextEdgeIndex = 4; + break; + case 3: + mEdges[3].vertexIndex = 2; + mEdges[3].twinEdgeIndex = 2; + mEdges[3].faceIndex = 1; + mEdges[3].nextEdgeIndex = 1; + break; + case 4: + mEdges[4].vertexIndex = 2; + mEdges[4].twinEdgeIndex = 5; + mEdges[4].faceIndex = 0; + mEdges[4].nextEdgeIndex = 0; + break; + case 5: + mEdges[5].vertexIndex = 0; + mEdges[5].twinEdgeIndex = 4; + mEdges[5].faceIndex = 1; + mEdges[5].nextEdgeIndex = 3; + break; + } + } + + mRaycastTestType = TriangleRaycastSide::FRONT; mId = shapeId; @@ -227,51 +278,3 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape return true; } -// Return a given half-edge of the polyhedron -HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const { - assert(edgeIndex < getNbHalfEdges()); - - HalfEdgeStructure::Edge edge; - - switch(edgeIndex) { - case 0: - edge.vertexIndex = 0; - edge.twinEdgeIndex = 1; - edge.faceIndex = 0; - edge.nextEdgeIndex = 2; - break; - case 1: - edge.vertexIndex = 1; - edge.twinEdgeIndex = 0; - edge.faceIndex = 1; - edge.nextEdgeIndex = 5; - break; - case 2: - edge.vertexIndex = 1; - edge.twinEdgeIndex = 3; - edge.faceIndex = 0; - edge.nextEdgeIndex = 4; - break; - case 3: - edge.vertexIndex = 2; - edge.twinEdgeIndex = 2; - edge.faceIndex = 1; - edge.nextEdgeIndex = 1; - break; - case 4: - edge.vertexIndex = 2; - edge.twinEdgeIndex = 5; - edge.faceIndex = 0; - edge.nextEdgeIndex = 0; - break; - case 5: - edge.vertexIndex = 0; - edge.twinEdgeIndex = 4; - edge.faceIndex = 1; - edge.nextEdgeIndex = 3; - break; - } - - return edge; - -} diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 60ec216b..a4ce7726 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -72,6 +72,12 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; + /// Faces information for the two faces of the triangle + HalfEdgeStructure::Face mFaces[2]; + + /// Edges information for the six edges of the triangle + HalfEdgeStructure::Edge mEdges[6]; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -137,7 +143,7 @@ class TriangleShape : public ConvexPolyhedronShape { virtual uint getNbFaces() const override; /// Return a given face of the polyhedron - virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override; + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; /// Return the number of vertices of the polyhedron virtual uint getNbVertices() const override; @@ -155,7 +161,7 @@ class TriangleShape : public ConvexPolyhedronShape { virtual uint getNbHalfEdges() const override; /// Return a given half-edge of the polyhedron - virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override; + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const override; @@ -252,26 +258,9 @@ inline uint TriangleShape::getNbFaces() const { } // Return a given face of the polyhedron -inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const { +inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const { assert(faceIndex < 2); - - HalfEdgeStructure::Face face; - - if (faceIndex == 0) { - face.faceVertices.push_back(0); - face.faceVertices.push_back(1); - face.faceVertices.push_back(2); - face.edgeIndex = 0; - - } - else { - face.faceVertices.push_back(0); - face.faceVertices.push_back(2); - face.faceVertices.push_back(1); - face.edgeIndex = 1; - } - - return face; + return mFaces[faceIndex]; } // Return the number of vertices of the polyhedron @@ -292,6 +281,12 @@ inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) cons return vertex; } +// Return a given half-edge of the polyhedron +inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mEdges[edgeIndex]; +} + // Return the position of a given vertex inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { assert(vertexIndex < 3); diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 74415eb8..d5e771e2 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -306,16 +306,35 @@ inline Quaternion Quaternion::operator*(decimal nb) const { // Overloaded operator for the multiplication of two quaternions inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { + + /* The followin code is equivalent to this return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), - w * quaternion.getVectorV() + quaternion.w * getVectorV() + - getVectorV().cross(quaternion.getVectorV())); + w * quaternion.getVectorV() + quaternion.w * getVectorV() + + getVectorV().cross(quaternion.getVectorV())); + */ + + return Quaternion(w * quaternion.x + quaternion.w * x + y * quaternion.z - z * quaternion.y, + w * quaternion.y + quaternion.w * y + z * quaternion.x - x * quaternion.z, + w * quaternion.z + quaternion.w * z + x * quaternion.y - y * quaternion.x, + w * quaternion.w - x * quaternion.x - y * quaternion.y - z * quaternion.z); } // Overloaded operator for the multiplication with a vector. /// This methods rotates a point given the rotation of a quaternion. inline Vector3 Quaternion::operator*(const Vector3& point) const { - Quaternion p(point.x, point.y, point.z, 0.0); - return (((*this) * p) * getConjugate()).getVectorV(); + + /* The following code is equivalent to this + * Quaternion p(point.x, point.y, point.z, 0.0); + * return (((*this) * p) * getConjugate()).getVectorV(); + */ + + const decimal prodX = w * point.x + y * point.z - z * point.y; + const decimal prodY = w * point.y + z * point.x - x * point.z; + const decimal prodZ = w * point.z + x * point.y - y * point.x; + const decimal prodW = -x * point.x - y * point.y - z * point.z; + return Vector3(w * prodX - prodY * z + prodZ * y - prodW * x, + w * prodY - prodZ * x + prodX * z - prodW * y, + w * prodZ - prodX * y + prodY * x - prodW * z); } // Overloaded operator for the assignment diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index ab17a620..52ee663e 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -194,7 +194,7 @@ inline Transform Transform::identity() { // Return the transformed vector inline Vector3 Transform::operator*(const Vector3& vector) const { - return (mOrientation.getMatrix() * vector) + mPosition; + return (mOrientation * vector) + mPosition; } // Operator of multiplication of a transform with another one diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 37335abe..e7c9f371 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -198,10 +198,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con const decimal parallelEpsilon = decimal(0.0001); decimal t = decimal(-1); - // Segment AB - const Vector3 ab = segB - segA; - - decimal nDotAB = planeNormal.dot(ab); + decimal nDotAB = planeNormal.dot(segB - segA); // If the segment is not parallel to the plane if (std::abs(nDotAB) > parallelEpsilon) { @@ -297,23 +294,27 @@ std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, // Clip a polygon against multiple planes and return the clipped polygon vertices // This method implements the Sutherland–Hodgman clipping algorithm -std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, - const std::vector& planesNormals) { +List reactphysics3d::clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, Allocator& allocator) { assert(planesPoints.size() == planesNormals.size()); - std::vector inputVertices(polygonVertices); - std::vector outputVertices; + uint nbMaxElements = polygonVertices.size() + planesPoints.size(); + List inputVertices(allocator, nbMaxElements); + List outputVertices(allocator, nbMaxElements); + + inputVertices.addRange(polygonVertices); // For each clipping plane for (uint p=0; p reactphysics3d::clipPolygonWithPlanes(const std::vector= decimal(0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } else { - outputVertices.push_back(v2); + outputVertices.add(v2); } } // Add the second vertex - outputVertices.push_back(v2); + outputVertices.add(v2); } else { // If the second vertex is behind the clipping plane @@ -350,10 +351,10 @@ std::vector reactphysics3d::clipPolygonWithPlanes(const std::vector= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices.add(v1 + t * (v2 - v1)); } else { - outputVertices.push_back(v1); + outputVertices.add(v1); } } } diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 9f062826..ff6ac6f4 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -33,6 +33,7 @@ #include #include #include +#include "containers/List.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -116,8 +117,8 @@ std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& s const std::vector& planesNormals); /// Clip a polygon against multiple planes and return the clipped polygon vertices -std::vector clipPolygonWithPlanes(const std::vector& polygonVertices, const std::vector& planesPoints, - const std::vector& planesNormals); +List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, Allocator& allocator); /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index b45415ee..793a79fb 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -27,6 +27,8 @@ #define TEST_MATHEMATICS_FUNCTIONS_H // Libraries +#include "containers/List.h" +#include "memory/DefaultAllocator.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -41,7 +43,7 @@ class TestMathematicsFunctions : public Test { // ---------- Atributes ---------- // - + DefaultAllocator mAllocator; public : @@ -223,37 +225,37 @@ class TestMathematicsFunctions : public Test { test(clipSegmentVertices.size() == 0); // Test clipPolygonWithPlanes() - std::vector polygonVertices; - polygonVertices.push_back(Vector3(-4, 2, 0)); - polygonVertices.push_back(Vector3(7, 2, 0)); - polygonVertices.push_back(Vector3(7, 4, 0)); - polygonVertices.push_back(Vector3(-4, 4, 0)); + List polygonVertices(mAllocator); + polygonVertices.add(Vector3(-4, 2, 0)); + polygonVertices.add(Vector3(7, 2, 0)); + polygonVertices.add(Vector3(7, 4, 0)); + polygonVertices.add(Vector3(-4, 4, 0)); - planesNormals.clear(); - planesPoints.clear(); - planesNormals.push_back(Vector3(1, 0, 0)); - planesPoints.push_back(Vector3(0, 0, 0)); - planesNormals.push_back(Vector3(0, 1, 0)); - planesPoints.push_back(Vector3(0, 0, 0)); - planesNormals.push_back(Vector3(-1, 0, 0)); - planesPoints.push_back(Vector3(10, 0, 0)); - planesNormals.push_back(Vector3(0, -1, 0)); - planesPoints.push_back(Vector3(10, 5, 0)); + List polygonPlanesNormals(mAllocator); + List polygonPlanesPoints(mAllocator); + polygonPlanesNormals.add(Vector3(1, 0, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(0, 1, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(-1, 0, 0)); + polygonPlanesPoints.add(Vector3(10, 0, 0)); + polygonPlanesNormals.add(Vector3(0, -1, 0)); + polygonPlanesPoints.add(Vector3(10, 5, 0)); - clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); - test(clipSegmentVertices.size() == 4); - test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001)); - test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001)); - test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001)); - test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001)); - test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001)); - test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001)); - test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001)); - test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001)); + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator); + test(clipPolygonVertices.size() == 4); + test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001)); } From 9d761291d620efdf23749d0ed2d82daee054c45a Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 6 Dec 2017 21:55:50 +0100 Subject: [PATCH 094/105] Small optimizations --- CMakeLists.txt | 2 ++ .../narrowphase/SAT/SATAlgorithm.cpp | 24 +-------------- src/collision/narrowphase/SAT/SATAlgorithm.h | 3 -- .../shapes/ConvexPolyhedronShape.cpp | 22 ++++++++++++++ src/collision/shapes/ConvexPolyhedronShape.h | 5 ++++ src/constraint/ContactPoint.h | 8 ++--- src/mathematics/mathematics_functions.cpp | 29 ++++++++++--------- 7 files changed, 50 insertions(+), 43 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4cfde6e..e7e2df87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -192,8 +192,10 @@ SET (REACTPHYSICS3D_SOURCES "src/memory/PoolAllocator.cpp" "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" + "src/memory/DefaultAllocator.h" "src/containers/Stack.h" "src/containers/LinkedList.h" + "src/containers/List.h" ) # Create the library diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 7ef99e1f..5e40f45f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -824,7 +824,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); // Find the incident face on the other polyhedron (most anti-parallel face) - uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace); + uint incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(axisIncidentSpace); // Get the incident face const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); @@ -914,28 +914,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene return contactPointsFound; } -// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector -// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron -uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const { - - PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler); - - decimal minDotProduct = DECIMAL_LARGEST; - uint mostAntiParallelFace = 0; - - // For each face of the polyhedron - for (uint i=0; i < polyhedron->getNbFaces(); i++) { - - // Get the face normal - decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction); - if (dotProduct < minDotProduct) { - minDotProduct = dotProduct; - mostAntiParallelFace = i; - } - } - - return mostAntiParallelFace; -} // Compute and return the distance between the two edges in the direction of the candidate separating axis decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index af49ca09..52884f8e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -72,9 +72,6 @@ class SATAlgorithm { const Vector3& c, const Vector3& d, const Vector3& bCrossA, const Vector3& dCrossC) const; - // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector - uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const; - /// Compute and return the distance between the two edges in the direction of the candidate separating axis decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, const Vector3& edge1Direction, const Vector3& edge2Direction, diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp index ab93584d..24f80de6 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.cpp +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -35,3 +35,25 @@ ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name) : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) { } + +// Find and return the index of the polyhedron face with the most anti-parallel face +// normal given a direction vector. This is used to find the incident face on +// a polyhedron of a given reference face of another polyhedron +uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + for (uint i=0; i < getNbFaces(); i++) { + + // Get the face normal + decimal dotProduct = getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 1d944cb8..50c6df89 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -87,6 +87,10 @@ class ConvexPolyhedronShape : public ConvexShape { /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const=0; + + /// Find and return the index of the polyhedron face with the most anti-parallel face + /// normal given a direction vector + uint findMostAntiParallelFace(const Vector3& direction) const; }; // Return true if the collision shape is a polyhedron @@ -94,6 +98,7 @@ inline bool ConvexPolyhedronShape::isPolyhedron() const { return true; } + } #endif diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 40a3d13c..d78659f8 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -122,10 +122,10 @@ class ContactPoint { Vector3 getNormal() const; /// Return the contact point on the first proxy shape in the local-space of the proxy shape - Vector3 getLocalPointOnShape1() const; + const Vector3& getLocalPointOnShape1() const; /// Return the contact point on the second proxy shape in the local-space of the proxy shape - Vector3 getLocalPointOnShape2() const; + const Vector3& getLocalPointOnShape2() const; /// Return the cached penetration impulse decimal getPenetrationImpulse() const; @@ -157,12 +157,12 @@ inline Vector3 ContactPoint::getNormal() const { } // Return the contact point on the first proxy shape in the local-space of the proxy shape -inline Vector3 ContactPoint::getLocalPointOnShape1() const { +inline const Vector3& ContactPoint::getLocalPointOnShape1() const { return mLocalPointOnShape1; } // Return the contact point on the second proxy shape in the local-space of the proxy shape -inline Vector3 ContactPoint::getLocalPointOnShape2() const { +inline const Vector3& ContactPoint::getLocalPointOnShape2() const { return mLocalPointOnShape2; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index e7c9f371..b82c9fbd 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -300,24 +300,26 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon assert(planesPoints.size() == planesNormals.size()); uint nbMaxElements = polygonVertices.size() + planesPoints.size(); - List inputVertices(allocator, nbMaxElements); - List outputVertices(allocator, nbMaxElements); + List list1(allocator, nbMaxElements); + List list2(allocator, nbMaxElements); + List dotProducts(allocator, nbMaxElements * 2); - inputVertices.addRange(polygonVertices); + const List* inputVertices = &polygonVertices; + List* outputVertices = &list2; // For each clipping plane for (uint p=0; pclear(); - uint nbInputVertices = inputVertices.size(); + uint nbInputVertices = inputVertices->size(); uint vStart = nbInputVertices - 1; // For each edge of the polygon for (uint vEnd = 0; vEnd reactphysics3d::clipPolygonWithPlanes(const List& polygon decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } else { - outputVertices.add(v2); + outputVertices->add(v2); } } // Add the second vertex - outputVertices.add(v2); + outputVertices->add(v2); } else { // If the second vertex is behind the clipping plane @@ -351,10 +353,10 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.add(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } else { - outputVertices.add(v1); + outputVertices->add(v1); } } } @@ -363,9 +365,10 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon } inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; } - return outputVertices; + return *outputVertices; } // Project a point onto a plane that is given by a point and its unit length normal From cf42e9f04c35cdacb3b7ae54881812d8bd187d7d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 07:29:29 +0100 Subject: [PATCH 095/105] Optimizations in contact solver --- src/engine/ContactSolver.cpp | 300 +++++++++++++++++++++++++++-------- 1 file changed, 233 insertions(+), 67 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 167d37b8..7bd74e8a 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -163,22 +163,48 @@ void ContactSolver::initializeForIsland(Island* island) { new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = externalContact; mContactPoints[mNbContactPoints].normal = externalContact->getNormal(); - mContactPoints[mNbContactPoints].r1 = p1 - x1; - mContactPoints[mNbContactPoints].r2 = p2 - x2; + mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x; + mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y; + mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z; + mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x; + mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y; + mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z; mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; - mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; - mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; + mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x; + mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y; + mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z; + mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x; + mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y; + mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z; // Compute the velocity difference - Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y + - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y, + v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z + - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z, + v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x + - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x); - Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); - Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x); + // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x); mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN; mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN; @@ -194,13 +220,18 @@ void ContactSolver::initializeForIsland(Island* island) { // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias mContactPoints[mNbContactPoints].restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x + + deltaV.y * mContactPoints[mNbContactPoints].normal.y + + deltaV.z * mContactPoints[mNbContactPoints].normal.z; const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } - mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; + mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x; + mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y; + mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z; mNbContactPoints++; @@ -209,8 +240,12 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); - mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1; - mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2; + mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x; + mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y; + mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z; + mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x; + mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y; + mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z; mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1(); mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); @@ -230,8 +265,20 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - - v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + // v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z - + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y - + v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z - + w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y, + v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x - + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z - + v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x - + w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z, + v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y - + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x - + v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y - + w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); @@ -289,13 +336,25 @@ void ContactSolver::warmStart() { // --------- Penetration --------- // // Update the velocities of the body 1 by applying the impulse P - Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse; - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -312,20 +371,27 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 + - mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2; + Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z); mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1); mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2); // ------ First friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * - mContactConstraints[c].friction1Impulse; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * - mContactConstraints[c].friction1Impulse; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * - mContactConstraints[c].friction1Impulse; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); // Update the velocities of the body 1 by applying the impulse P mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; @@ -338,23 +404,40 @@ void ContactSolver::warmStart() { // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; - angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; + + angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; @@ -409,8 +492,15 @@ void ContactSolver::solve() { // --------- Penetration --------- // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); - decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal); + //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x - + w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y, + v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y - + w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z, + v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z - + w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x); + decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y + + deltaV.z * mContactPoints[contactPointIndex].normal.z; decimal Jv = deltaVDotN; // Compute the bias "b" of the constraint @@ -433,15 +523,27 @@ void ContactSolver::solve() { deltaLambda, decimal(0.0)); deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda, + mContactPoints[contactPointIndex].normal.y * deltaLambda, + mContactPoints[contactPointIndex].normal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -453,9 +555,17 @@ void ContactSolver::solve() { const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1]; const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); - decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal); + + //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - + w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y, + v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y - + w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z, + v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z - + w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x); + decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x + + deltaVSplit.y * mContactPoints[contactPointIndex].normal.y + + deltaVSplit.z * mContactPoints[contactPointIndex].normal.z; decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * mContactPoints[contactPointIndex].inversePenetrationMass; decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; @@ -464,15 +574,27 @@ void ContactSolver::solve() { deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; } contactPointIndex++; @@ -481,9 +603,16 @@ void ContactSolver::solve() { // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - - v1 - w1.cross(mContactConstraints[c].r1Friction); - decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1); + // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y, + v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z, + v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x); + decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x + + deltaV.y * mContactConstraints[c].frictionVector1.y + + deltaV.z * mContactConstraints[c].frictionVector1.z; // Compute the Lagrange multiplier lambda decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; @@ -495,23 +624,42 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda, + -mContactConstraints[c].r1CrossT1.y * deltaLambda, + -mContactConstraints[c].r1CrossT1.z * deltaLambda); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda, + mContactConstraints[c].frictionVector1.y * deltaLambda, + mContactConstraints[c].frictionVector1.z * deltaLambda); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda, + mContactConstraints[c].r2CrossT1.y * deltaLambda, + mContactConstraints[c].r2CrossT1.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); - Jv = deltaV.dot(mContactConstraints[c].frictionVector2); + //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y; + deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z; + deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x; + Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y + + deltaV.z * mContactConstraints[c].frictionVector2.z; // Compute the Lagrange multiplier lambda deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; @@ -523,23 +671,36 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda; + + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda; + + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; - Jv = deltaV.dot(mContactConstraints[c].normal); + Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y + + deltaV.z * mContactConstraints[c].normal.z; deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; @@ -550,7 +711,9 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; + angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; @@ -619,8 +782,11 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, + deltaVelocity.y * contact.normal.y * contact.normal.y, + deltaVelocity.z * contact.normal.z * contact.normal.z); + Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, + deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero decimal lengthTangenVelocity = tangentVelocity.length(); From 9066264189e27c85bd7a16b04cd3bb32c6a8ef8c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 22:36:19 +0100 Subject: [PATCH 096/105] Remove commented code --- src/engine/DynamicsWorld.cpp | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index bb824228..66b13a8e 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -358,20 +358,6 @@ void DynamicsWorld::solveContactsAndConstraints() { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - //if (!isConstraintsToSolve && !isContactsToSolve) continue; - - // If there are contacts in the current island -// if (isContactsToSolve) { - -// // Initialize the solver -// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - -// // Warm start the contact solver -// if (mContactSolver.IsWarmStartingActive()) { -// mContactSolver.warmStart(); -// } -// } // If there are constraints if (isConstraintsToSolve) { @@ -393,15 +379,6 @@ void DynamicsWorld::solveContactsAndConstraints() { } mContactSolver.solve(); - - // Solve the contacts -// if (isContactsToSolve) { - -// mContactSolver.resetTotalPenetrationImpulse(); - -// mContactSolver.solvePenetrationConstraints(); -// mContactSolver.solveFrictionConstraints(); -// } } mContactSolver.storeImpulses(); From 2d0cb2753832059abb460ebb317bbbfd9e8f0eab Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 12 Dec 2017 07:38:56 +0100 Subject: [PATCH 097/105] Add List and DefaultAllocator classes --- src/containers/List.h | 191 ++++++++++++++++++++++++++++++++++ src/memory/DefaultAllocator.h | 66 ++++++++++++ 2 files changed, 257 insertions(+) create mode 100644 src/containers/List.h create mode 100644 src/memory/DefaultAllocator.h diff --git a/src/containers/List.h b/src/containers/List.h new file mode 100644 index 00000000..4915f32d --- /dev/null +++ b/src/containers/List.h @@ -0,0 +1,191 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_LIST_H +#define REACTPHYSICS3D_LIST_H + +// Libraries +#include "configuration.h" +#include "memory/Allocator.h" +#include + +namespace reactphysics3d { + +// Class List +/** + * This class represents a simple generic list with custom memory allocator. + */ +template +class List { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer to the first element of the list + T* mElements; + + /// Number of elements in the list + uint mSize; + + /// Number of allocated elements in the list + uint mCapacity; + + /// Memory allocator + Allocator& mAllocator; + + // -------------------- Methods -------------------- // + + /// Allocate more memory for the elements of the list + void allocateMemory(uint nbElementsToAllocate) { + + assert(nbElementsToAllocate > mCapacity); + + // Allocate memory for the new array + void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T)); + + if (mElements != nullptr) { + + // Copy the elements to the new allocated memory location + std::memcpy(newMemory, static_cast(mElements), mSize * sizeof(T)); + + // Release the previously allocated memory + mAllocator.release(mElements, mCapacity * sizeof(T)); + } + + mElements = static_cast(newMemory); + + mCapacity = nbElementsToAllocate; + } + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + List(Allocator& allocator, uint capacity = 0) + : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { + + if (capacity > 0) { + + // Allocate memory + allocateMemory(capacity); + } + } + + /// Copy constructor + List(const List& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + + // All all the elements of the list to the current one + addRange(list); + } + + /// Destructor + ~List() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Release the memory allocated on the heap + mAllocator.release(static_cast(mElements), mCapacity * sizeof(T)); + } + } + + /// Add an element into the list + void add(const T& element) { + + // If we need to allocate more memory + if (mSize == mCapacity) { + allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2); + } + + mElements[mSize] = element; + mSize++; + } + + /// Append another list to the current one + void addRange(const List& list) { + + // If we need to allocate more memory + if (mSize + list.size() > mCapacity) { + + // Allocate memory + allocateMemory(mSize + list.size()); + } + + // Add the elements of the list to the current one + for(uint i=0; i= 0 && index < mSize); + return mElements[index]; + } + + /// Overloaded const index operator + const T& operator[](const uint index) const { + assert(index >= 0 && index < mSize); + return mElements[index]; + } + + /// Overloaded assignment operator + List& operator=(const List& list) { + + // Clear all the elements + clear(); + + // Add all the elements of the list to the current one + addRange(list); + + return *this; + } +}; + +} + +#endif diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h new file mode 100644 index 00000000..367171bd --- /dev/null +++ b/src/memory/DefaultAllocator.h @@ -0,0 +1,66 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_DEFAULT_ALLOCATOR_H +#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H + +// Libraries +#include "memory/Allocator.h" +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class DefaultAllocator +/** + * This class represents a default memory allocator that uses default malloc/free methods + */ +class DefaultAllocator : public Allocator { + + public: + + /// Destructor + virtual ~DefaultAllocator() = default; + + /// Allocate memory of a given size (in bytes) and return a pointer to the + /// allocated memory. + virtual void* allocate(size_t size) override { + return malloc(size); + } + + /// Release previously allocated memory. + virtual void release(void* pointer, size_t size) override { + free(pointer); + } + + /// Return true if memory needs to be release with this allocator + virtual bool isReleaseNeeded() const override { + return true; + } +}; + +} + +#endif From 5392948518ef415991feb55e0595bc15b48d737c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 13 Dec 2017 17:51:38 +0100 Subject: [PATCH 098/105] Use inline constructors for mathematics objects (vectors, quaternion, matrices, ...) --- src/mathematics/Matrix2x2.cpp | 25 ------------------ src/mathematics/Matrix2x2.h | 25 ++++++++++++++++++ src/mathematics/Matrix3x3.cpp | 26 ------------------- src/mathematics/Matrix3x3.h | 26 +++++++++++++++++++ src/mathematics/Quaternion.cpp | 21 ---------------- src/mathematics/Quaternion.h | 25 ++++++++++++++++-- src/mathematics/Transform.cpp | 22 ---------------- src/mathematics/Transform.h | 46 ++++++++++++++++++++++++++++++++-- src/mathematics/Vector2.cpp | 15 ----------- src/mathematics/Vector2.h | 16 ++++++++++++ src/mathematics/Vector3.cpp | 15 ----------- src/mathematics/Vector3.h | 15 +++++++++++ 12 files changed, 149 insertions(+), 128 deletions(-) diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp index c501c1db..bf4adbe5 100644 --- a/src/mathematics/Matrix2x2.cpp +++ b/src/mathematics/Matrix2x2.cpp @@ -28,31 +28,6 @@ using namespace reactphysics3d; -// Constructor of the class Matrix2x2 -Matrix2x2::Matrix2x2() { - - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix2x2::Matrix2x2(decimal value) { - setAllValues(value, value, value, value); -} - -// Constructor with arguments -Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { - - // Initialize the matrix with the values - setAllValues(a1, a2, b1, b2); -} - -// Copy-constructor -Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); -} - // Assignment operator Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index ee31b07a..8315eb99 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -147,6 +147,31 @@ class Matrix2x2 { Vector2& operator[](int row); }; +// Constructor of the class Matrix2x2 +inline Matrix2x2::Matrix2x2() { + + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix2x2::Matrix2x2(decimal value) { + setAllValues(value, value, value, value); +} + +// Constructor with arguments +inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { + + // Initialize the matrix with the values + setAllValues(a1, a2, b1, b2); +} + +// Copy-constructor +inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); +} + // Method to set all the values in the matrix inline void Matrix2x2::setAllValues(decimal a1, decimal a2, decimal b1, decimal b2) { diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index a5491ed4..e86e63a1 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -30,32 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Matrix3x3 -Matrix3x3::Matrix3x3() { - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix3x3::Matrix3x3(decimal value) { - setAllValues(value, value, value, value, value, value, value, value, value); -} - -// Constructor with arguments -Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, - decimal b1, decimal b2, decimal b3, - decimal c1, decimal c2, decimal c3) { - // Initialize the matrix with the values - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); -} - -// Copy-constructor -Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], - matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], - matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); -} - // Assignment operator Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index ebb8792d..6897a2cf 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -155,6 +155,32 @@ class Matrix3x3 { Vector3& operator[](int row); }; +// Constructor of the class Matrix3x3 +inline Matrix3x3::Matrix3x3() { + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix3x3::Matrix3x3(decimal value) { + setAllValues(value, value, value, value, value, value, value, value, value); +} + +// Constructor with arguments +inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, + decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3) { + // Initialize the matrix with the values + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); +} + +// Copy-constructor +inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], + matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], + matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); +} + // Method to set all the values in the matrix inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index d0e1ae61..0e877612 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -31,27 +31,6 @@ // Namespace using namespace reactphysics3d; -// Constructor of the class -Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { - -} - -// Constructor with arguments -Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) - :x(newX), y(newY), z(newZ), w(newW) { - -} - -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { - -} - -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { - -} - // Return a quaternion constructed from Euler angles (in radians) Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index d5e771e2..dd0726dc 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -169,7 +169,28 @@ struct Quaternion { void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ); }; -/// Set all the values +// Constructor of the class +inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { + +} + +// Constructor with arguments +inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) + :x(newX), y(newY), z(newZ), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Set all the values inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { x = newX; y = newY; @@ -177,7 +198,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d w = newW; } -/// Set the quaternion to zero +// Set the quaternion to zero inline void Quaternion::setToZero() { x = 0; y = 0; diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index 4bc91253..d5cf0399 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -29,25 +29,3 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Matrix3x3& orientation) - : mPosition(position), mOrientation(Quaternion(orientation)) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Quaternion& orientation) - : mPosition(position), mOrientation(orientation) { - -} - -// Copy-constructor -Transform::Transform(const Transform& transform) - : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { - -} diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index 52ee663e..a5248fc2 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -118,6 +118,29 @@ class Transform { Transform& operator=(const Transform& transform); }; +// Constructor +inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation) + : mPosition(position), mOrientation(Quaternion(orientation)) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Quaternion& orientation) + : mPosition(position), mOrientation(orientation) { + +} + +// Copy-constructor +inline Transform::Transform(const Transform& transform) + : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { + +} + // Return the position of the transform inline const Vector3& Transform::getPosition() const { return mPosition; @@ -199,8 +222,27 @@ inline Vector3 Transform::operator*(const Vector3& vector) const { // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(mPosition + mOrientation * transform2.mPosition, - mOrientation * transform2.mOrientation); + + const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z + - mOrientation.z * transform2.mPosition.y; + const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x + - mOrientation.x * transform2.mPosition.z; + const decimal prodZ = mOrientation.w * transform2.mPosition.z + mOrientation.x * transform2.mPosition.y + - mOrientation.y * transform2.mPosition.x; + const decimal prodW = -mOrientation.x * transform2.mPosition.x - mOrientation.y * transform2.mPosition.y + - mOrientation.z * transform2.mPosition.z; + + return Transform(Vector3(mPosition.x + mOrientation.w * prodX - prodY * mOrientation.z + prodZ * mOrientation.y - prodW * mOrientation.x, + mPosition.y + mOrientation.w * prodY - prodZ * mOrientation.x + prodX * mOrientation.z - prodW * mOrientation.y, + mPosition.z + mOrientation.w * prodZ - prodX * mOrientation.y + prodY * mOrientation.x - prodW * mOrientation.z), + Quaternion(mOrientation.w * transform2.mOrientation.x + transform2.mOrientation.w * mOrientation.x + + mOrientation.y * transform2.mOrientation.z - mOrientation.z * transform2.mOrientation.y, + mOrientation.w * transform2.mOrientation.y + transform2.mOrientation.w * mOrientation.y + + mOrientation.z * transform2.mOrientation.x - mOrientation.x * transform2.mOrientation.z, + mOrientation.w * transform2.mOrientation.z + transform2.mOrientation.w * mOrientation.z + + mOrientation.x * transform2.mOrientation.y - mOrientation.y * transform2.mOrientation.x, + mOrientation.w * transform2.mOrientation.w - mOrientation.x * transform2.mOrientation.x + - mOrientation.y * transform2.mOrientation.y - mOrientation.z * transform2.mOrientation.z)); } // Return true if the two transforms are equal diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp index 2f225636..271fd82f 100644 --- a/src/mathematics/Vector2.cpp +++ b/src/mathematics/Vector2.cpp @@ -30,21 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Vector2::Vector2() : x(0.0), y(0.0) { - -} - -// Constructor with arguments -Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { - -} - -// Copy-constructor -Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { - -} - // Return the corresponding unit vector Vector2 Vector2::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector2.h b/src/mathematics/Vector2.h index 4b0e32f1..bc5f8872 100644 --- a/src/mathematics/Vector2.h +++ b/src/mathematics/Vector2.h @@ -156,6 +156,22 @@ struct Vector2 { friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); }; +// Constructor +inline Vector2::Vector2() : x(0.0), y(0.0) { + +} + +// Constructor with arguments +inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { + +} + +// Copy-constructor +inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { + +} + + // Set the vector to zero inline void Vector2::setToZero() { x = 0; diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index ab2d126d..33b760ba 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -31,21 +31,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Vector3D -Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { - -} - -// Constructor with arguments -Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { - -} - -// Copy-constructor -Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { - -} - // Return the corresponding unit vector Vector3 Vector3::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 1ca1adfd..95255fdb 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -168,6 +168,21 @@ struct Vector3 { friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); }; +// Constructor of the class Vector3D +inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { + +} + +// Constructor with arguments +inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { + +} + +// Copy-constructor +inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { + +} + // Set the vector to zero inline void Vector3::setToZero() { x = 0; From 7d20a746e9a8822d4530e6b7b44ef3286d8aed68 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 15:09:56 +0100 Subject: [PATCH 099/105] Do not std::map to store mapping from rigid body to index in array --- src/body/RigidBody.cpp | 2 +- src/body/RigidBody.h | 7 ++++++- src/constraint/BallAndSocketJoint.cpp | 4 ++-- src/constraint/FixedJoint.cpp | 4 ++-- src/constraint/HingeJoint.cpp | 4 ++-- src/constraint/SliderJoint.cpp | 4 ++-- src/engine/ConstraintSolver.cpp | 4 +--- src/engine/ConstraintSolver.h | 16 +++------------- src/engine/ContactSolver.cpp | 8 +++----- src/engine/ContactSolver.h | 6 +----- src/engine/DynamicsWorld.cpp | 26 +++++++++----------------- src/engine/DynamicsWorld.h | 3 --- 12 files changed, 32 insertions(+), 56 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index d0f0c782..01e1694b 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr) { + mJointsList(nullptr), mArrayIndex(0) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6ba7a596..e12cab05 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -50,6 +50,11 @@ class DynamicsWorld; */ class RigidBody : public CollisionBody { + private : + + /// Index of the body in arrays for contact/constraint solver + uint mArrayIndex; + protected : // -------------------- Attributes -------------------- // @@ -102,7 +107,7 @@ class RigidBody : public CollisionBody { decimal mAngularDamping; /// First element of the linked list of joints involving this body - JointListElement* mJointsList; + JointListElement* mJointsList; // -------------------- Methods -------------------- // diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 7a5fce1b..272d922e 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies center of mass and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 8f0b105c..01fe3957 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 1634bf27..7b400a2a 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 919d91c0..7697013d 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the veloc ity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 458c39a7..5345cbaf 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -30,9 +30,7 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) - : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { +ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) { #ifdef IS_PROFILING_ACTIVE diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index fbb54ed1..105045e0 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -60,18 +60,12 @@ struct ConstraintSolverData { /// Reference to the bodies orientations Quaternion* orientations; - /// Reference to the map that associates rigid body to their index - /// in the constrained velocities array - const std::map& mapBodyToConstrainedVelocityIndex; - /// True if warm starting of the solver is active bool isWarmStartingActive; /// Constructor - ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(nullptr), angularVelocities(nullptr), - positions(nullptr), orientations(nullptr), - mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr), + positions(nullptr), orientations(nullptr) { } @@ -152,10 +146,6 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // - /// Reference to the map that associates rigid body to their index in - /// the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// Current time step decimal mTimeStep; @@ -176,7 +166,7 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(const std::map& mapBodyToVelocityIndex); + ConstraintSolver(); /// Destructor ~ConstraintSolver() = default; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 7bd74e8a..6361badd 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator) +ContactSolver::ContactSolver(SingleFrameAllocator& allocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mContactConstraints(nullptr), mSingleFrameAllocator(allocator), mLinearVelocities(nullptr), mAngularVelocities(nullptr), - mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsSplitImpulseActive(true) { } @@ -131,8 +129,8 @@ void ContactSolver::initializeForIsland(Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex; + mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex; mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 0a2850f5..05bd569c 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -302,9 +302,6 @@ class ContactSolver { /// Array of angular velocities Vector3* mAngularVelocities; - /// Reference to the map of rigid body to their index in the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the split impulse position correction is active bool mIsSplitImpulseActive; @@ -342,8 +339,7 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator); + ContactSolver(SingleFrameAllocator& allocator); /// Destructor ~ContactSolver() = default; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 66b13a8e..054c4dc5 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -41,8 +41,7 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), - mConstraintSolver(mMapBodyToConstrainedVelocityIndex), + mContactSolver(mSingleFrameAllocator), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), @@ -167,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Get the constrained velocity - uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexArray = bodies[b]->mArrayIndex; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; @@ -205,7 +204,7 @@ void DynamicsWorld::updateBodiesState() { for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint index = bodies[b]->mArrayIndex; // Update the linear and angular velocity of the body bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; @@ -250,21 +249,14 @@ void DynamicsWorld::initVelocityArrays() { assert(mConstrainedPositions != nullptr); assert(mConstrainedOrientations != nullptr); - // Reset the velocities arrays - for (uint i=0; i::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + mSplitLinearVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero(); - } - // Initialize the map of body indexes in the velocity arrays - mMapBodyToConstrainedVelocityIndex.clear(); - std::set::const_iterator it; - uint indexBody = 0; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - - // Add the body into the map - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); - indexBody++; + (*it)->mArrayIndex = i++; } } @@ -289,7 +281,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Insert the body into the map of constrained velocities - uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexBody = bodies[b]->mArrayIndex; assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 366b2770..0c614b65 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld { /// Array of constrained rigid bodies orientation (for position error correction) Quaternion* mConstrainedOrientations; - /// Map body to their index in the constrained velocities array - std::map mMapBodyToConstrainedVelocityIndex; - /// Number of islands in the world uint mNbIslands; From 47869627d13d71f3997adea53731890e55a10de4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 20:24:19 +0100 Subject: [PATCH 100/105] Fix issue in Transform --- src/mathematics/Transform.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index a5248fc2..aeaf4234 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -223,7 +223,11 @@ inline Vector3 Transform::operator*(const Vector3& vector) const { // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z + // The following code is equivalent to this + //return Transform(mPosition + mOrientation * transform2.mPosition, + // mOrientation * transform2.mOrientation); + + const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.y * transform2.mPosition.z - mOrientation.z * transform2.mPosition.y; const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x - mOrientation.x * transform2.mPosition.z; From f2ee00ca68b409234b84d134cdc9845c9c55e2c3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 14 Dec 2017 22:25:52 +0100 Subject: [PATCH 101/105] Use List instead of std::vector compute segment clipping in SAT algorithm --- src/body/RigidBody.cpp | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 10 ++--- src/mathematics/mathematics_functions.cpp | 43 +++++++++++-------- src/mathematics/mathematics_functions.h | 7 +-- 4 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 01e1694b..e4ce4458 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -39,10 +39,10 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), + : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), - mJointsList(nullptr), mArrayIndex(0) { + mJointsList(nullptr) { // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 5e40f45f..2605eaf9 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -374,8 +374,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI uint firstEdgeIndex = face.edgeIndex; uint edgeIndex = firstEdgeIndex; - std::vector planesPoints; - std::vector planesNormals; + List planesPoints(mMemoryAllocator, 2); + List planesNormals(mMemoryAllocator, 2); // For each adjacent edge of the separating face of the polyhedron do { @@ -393,15 +393,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection); // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron - planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); - planesNormals.push_back(clipPlaneNormal); + planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex)); + planesNormals.add(clipPlaneNormal); edgeIndex = edge.nextEdgeIndex; } while(edgeIndex != firstEdgeIndex); // First we clip the inner segment of the capsule with the four planes of the adjacent faces - std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); + List clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator); // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index b82c9fbd..0d8994f4 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -222,27 +222,34 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co // Clip a segment against multiple planes and return the clipped segment vertices // This method implements the Sutherland–Hodgman clipping algorithm -std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const std::vector& planesPoints, - const std::vector& planesNormals) { +List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + Allocator& allocator) { assert(planesPoints.size() == planesNormals.size()); - std::vector inputVertices = {segA, segB}; - std::vector outputVertices; + List list1(allocator, 2); + List list2(allocator, 2); + + List* inputVertices = &list1; + List* outputVertices = &list2; + + inputVertices->add(segA); + inputVertices->add(segB); // For each clipping plane for (uint p=0; psize() == 0) return *inputVertices; - assert(inputVertices.size() == 2); + assert(inputVertices->size() == 2); - outputVertices.clear(); + outputVertices->clear(); - Vector3& v1 = inputVertices[0]; - Vector3& v2 = inputVertices[1]; + Vector3& v1 = (*inputVertices)[0]; + Vector3& v2 = (*inputVertices)[1]; decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); @@ -257,39 +264,40 @@ std::vector reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); if (t >= decimal(0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } else { - outputVertices.push_back(v2); + outputVertices->add(v2); } } else { - outputVertices.push_back(v1); + outputVertices->add(v1); } // Add the second vertex - outputVertices.push_back(v2); + outputVertices->add(v2); } else { // If the second vertex is behind the clipping plane // If the first vertex is in front of the clippling plane if (v1DotN >= decimal(0.0)) { - outputVertices.push_back(v1); + outputVertices->add(v1); // The first point we keep is the intersection between the segment v1, v2 and the clipping plane decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); if (t >= decimal(0.0) && t <= decimal(1.0)) { - outputVertices.push_back(v1 + t * (v2 - v1)); + outputVertices->add(v1 + t * (v2 - v1)); } } } inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; } - return outputVertices; + return *outputVertices; } // Clip a polygon against multiple planes and return the clipped polygon vertices @@ -302,7 +310,6 @@ List reactphysics3d::clipPolygonWithPlanes(const List& polygon uint nbMaxElements = polygonVertices.size() + planesPoints.size(); List list1(allocator, nbMaxElements); List list2(allocator, nbMaxElements); - List dotProducts(allocator, nbMaxElements * 2); const List* inputVertices = &polygonVertices; List* outputVertices = &list2; diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index ff6ac6f4..5081d741 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -112,9 +112,10 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); /// Clip a segment against multiple planes and return the clipped segment vertices -std::vector clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, - const std::vector& planesPoints, - const std::vector& planesNormals); +List clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + Allocator& allocator); /// Clip a polygon against multiple planes and return the clipped polygon vertices List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, From 8f126a75d6d38dd4152739fab4257bd0f427e514 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 27 Dec 2017 20:53:09 +0100 Subject: [PATCH 102/105] Use List in HalfEdgeStructure with some changes in memory allocation --- CMakeLists.txt | 2 + src/body/CollisionBody.cpp | 2 +- src/body/RigidBody.cpp | 2 +- src/collision/CollisionDetection.cpp | 7 + src/collision/ContactManifoldSet.cpp | 1 - src/collision/HalfEdgeStructure.cpp | 4 +- src/collision/HalfEdgeStructure.h | 29 +- src/collision/MiddlePhaseTriangleCallback.cpp | 2 +- src/collision/NarrowPhaseInfo.cpp | 6 +- src/collision/PolyhedronMesh.cpp | 11 +- src/collision/PolyhedronMesh.h | 1 + src/collision/ProxyShape.cpp | 6 +- src/collision/ProxyShape.h | 5 +- .../narrowphase/SAT/SATAlgorithm.cpp | 5 +- src/collision/shapes/BoxShape.cpp | 33 +- src/collision/shapes/BoxShape.h | 4 +- src/collision/shapes/CapsuleShape.cpp | 2 +- src/collision/shapes/CapsuleShape.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConcaveMeshShape.cpp | 8 +- src/collision/shapes/ConcaveMeshShape.h | 7 +- src/collision/shapes/ConvexMeshShape.cpp | 2 +- src/collision/shapes/ConvexMeshShape.h | 2 +- src/collision/shapes/HeightFieldShape.cpp | 8 +- src/collision/shapes/HeightFieldShape.h | 7 +- src/collision/shapes/SphereShape.cpp | 2 +- src/collision/shapes/SphereShape.h | 2 +- src/collision/shapes/TriangleShape.cpp | 15 +- src/collision/shapes/TriangleShape.h | 7 +- src/containers/List.h | 105 ++-- src/engine/CollisionWorld.cpp | 1 - src/engine/CollisionWorld.h | 1 + src/engine/OverlappingPair.cpp | 20 +- src/memory/MemoryManager.cpp | 32 ++ src/memory/MemoryManager.h | 74 +++ src/memory/PoolAllocator.cpp | 2 +- src/memory/PoolAllocator.h | 2 +- test/tests/collision/TestCollisionWorld.h | 499 +++++++++++++----- test/tests/collision/TestDynamicAABBTree.h | 29 + test/tests/collision/TestHalfEdgeStructure.h | 47 +- test/tests/collision/TestRaycast.h | 4 +- .../mathematics/TestMathematicsFunctions.h | 18 +- 42 files changed, 730 insertions(+), 290 deletions(-) create mode 100644 src/memory/MemoryManager.cpp create mode 100644 src/memory/MemoryManager.h mode change 100644 => 100755 test/tests/collision/TestCollisionWorld.h mode change 100644 => 100755 test/tests/collision/TestDynamicAABBTree.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e7e2df87..e585e0d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,8 @@ SET (REACTPHYSICS3D_SOURCES "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" "src/memory/DefaultAllocator.h" + "src/memory/MemoryManager.h" + "src/memory/MemoryManager.cpp" "src/containers/Stack.h" "src/containers/LinkedList.h" "src/containers/List.h" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index e588f93c..56493d28 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -72,7 +72,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, decimal(1)); + transform, decimal(1), mWorld.mPoolAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index e4ce4458..7419fc4c 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -226,7 +226,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, mass); + transform, mass, mWorld.mPoolAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index c0b4d4e9..e57c5fd8 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -288,7 +288,14 @@ void CollisionDetection::computeNarrowPhase() { lastCollisionFrameInfo->isValid = true; } + NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo; currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; + + // Call the destructor + narrowPhaseInfoToDelete->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); } // Convert the potential contact into actual contacts diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index ce3e4492..5f70bbc0 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -225,7 +225,6 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) { // Delete the contact manifold manifold->~ContactManifold(); mMemoryAllocator.release(manifold, sizeof(ContactManifold)); - mNbManifolds--; } diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp index 0f7b8f6e..70a731d5 100644 --- a/src/collision/HalfEdgeStructure.cpp +++ b/src/collision/HalfEdgeStructure.cpp @@ -96,8 +96,8 @@ void HalfEdgeStructure::init() { mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); - mEdges.push_back(itEdge->second); - mEdges.push_back(edge); + mEdges.add(itEdge->second); + mEdges.add(edge); } currentFaceEdges.push_back(pairV1V2); diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index ec4fff35..e398913f 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -50,14 +50,14 @@ class HalfEdgeStructure { }; struct Face { - uint edgeIndex; // Index of an half-edge of the face - std::vector faceVertices; // Index of the vertices of the face + uint edgeIndex; // Index of an half-edge of the face + List faceVertices; // Index of the vertices of the face /// Constructor - Face() {} + Face(Allocator& allocator) : faceVertices(allocator) {} /// Constructor - Face(std::vector vertices) : faceVertices(vertices) {} + Face(List vertices) : faceVertices(vertices) {} }; struct Vertex { @@ -70,19 +70,24 @@ class HalfEdgeStructure { private: + /// Reference to a memory allocator + Allocator& mAllocator; + /// All the faces - std::vector mFaces; + List mFaces; /// All the vertices - std::vector mVertices; + List mVertices; /// All the half-edges - std::vector mEdges; + List mEdges; public: /// Constructor - HalfEdgeStructure() = default; + HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity, + uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity), + mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {} /// Destructor ~HalfEdgeStructure() = default; @@ -94,7 +99,7 @@ class HalfEdgeStructure { uint addVertex(uint vertexPointIndex); /// Add a face - void addFace(std::vector faceVertices); + void addFace(List faceVertices); /// Return the number of faces uint getNbFaces() const; @@ -119,16 +124,16 @@ class HalfEdgeStructure { // Add a vertex inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { Vertex vertex(vertexPointIndex); - mVertices.push_back(vertex); + mVertices.add(vertex); return mVertices.size() - 1; } // Add a face -inline void HalfEdgeStructure::addFace(std::vector faceVertices) { +inline void HalfEdgeStructure::addFace(List faceVertices) { // Create a new face Face face(faceVertices); - mFaces.push_back(face); + mFaces.add(face); } // Return the number of faces diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index f8736a0b..a53271a4 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -34,7 +34,7 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints, verticesNormals, shapeId); + TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index a6440367..0189db99 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -52,11 +52,13 @@ NarrowPhaseInfo::~NarrowPhaseInfo() { // Release the memory of the TriangleShape (this memory was allocated in the // MiddlePhaseTriangleCallback::testTriangle() method) if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + collisionShape1->~CollisionShape(); collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape)); } if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) { - collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); - } + collisionShape2->~CollisionShape(); + collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); + } } // Add a new contact point diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index ec8b661d..08a84e7d 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -25,6 +25,7 @@ // Libraries #include "PolyhedronMesh.h" +#include "memory/MemoryManager.h" using namespace reactphysics3d; @@ -34,7 +35,11 @@ using namespace reactphysics3d; * Create a polyhedron mesh given an array of polygons. * @param polygonVertexArray Pointer to the array of polygons and their vertices */ -PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { +PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) + : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), + polygonVertexArray->getNbFaces(), + polygonVertexArray->getNbVertices(), + (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) { mPolygonVertexArray = polygonVertexArray; @@ -70,11 +75,11 @@ void PolyhedronMesh::createHalfEdgeStructure() { // Get the polygon face PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); - std::vector faceVertices; + List faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices); // For each vertex of the face for (uint v=0; v < face->nbVertices; v++) { - faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v)); + faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v)); } assert(faceVertices.size() >= 3); diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index a64db670..b459723a 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -30,6 +30,7 @@ #include "mathematics/mathematics.h" #include "HalfEdgeStructure.h" #include "collision/PolygonVertexArray.h" +#include "memory/DefaultAllocator.h" #include namespace reactphysics3d { diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 6e6eefbd..f782b2a8 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -35,9 +35,9 @@ using namespace reactphysics3d; * @param transform Transformation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator) :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { + mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) { } @@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { worldToLocalTransform * ray.point2, ray.maxFraction); - bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator); // Convert the raycast info into world-space raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 8e9a9e9f..0ba34698 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -82,6 +82,9 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; + /// Memory allocator + Allocator& mAllocator; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -100,7 +103,7 @@ class ProxyShape { /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, decimal mass); + const Transform& transform, decimal mass, Allocator& allocator); /// Destructor virtual ~ProxyShape(); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 2605eaf9..1992cbc7 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -835,9 +835,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene List planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes // Get all the vertices of the incident face (in the reference local-space) - std::vector::const_iterator it; - for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) { - const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it); + for (uint i=0; i < incidentFace.faceVertices.size(); i++) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]); polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 37667958..079b15c5 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -27,6 +27,7 @@ #include "BoxShape.h" #include "collision/ProxyShape.h" #include "configuration.h" +#include "memory/MemoryManager.h" #include #include @@ -37,7 +38,9 @@ using namespace reactphysics3d; * @param extent The vector with the three extents of the box (in meters) */ BoxShape::BoxShape(const Vector3& extent) - : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) { + : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent), + mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) { + assert(extent.x > decimal(0.0)); assert(extent.y > decimal(0.0)); assert(extent.z > decimal(0.0)); @@ -52,19 +55,21 @@ BoxShape::BoxShape(const Vector3& extent) mHalfEdgeStructure.addVertex(6); mHalfEdgeStructure.addVertex(7); + DefaultAllocator& allocator = MemoryManager::getDefaultAllocator(); + // Faces - std::vector face0; - face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); - std::vector face1; - face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); - std::vector face2; - face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5); - std::vector face3; - face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); - std::vector face4; - face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0); - std::vector face5; - face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); + List face0(allocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(allocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(allocator, 4); + face2.add(4); face2.add(7); face2.add(6); face2.add(5); + List face3(allocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(allocator, 4); + face4.add(4); face4.add(5); face4.add(1); face4.add(0); + List face5(allocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); mHalfEdgeStructure.addFace(face0); mHalfEdgeStructure.addFace(face1); @@ -93,7 +98,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const } // Raycast method with feedback information -bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { Vector3 rayDirection = ray.point2 - ray.point1; decimal tMin = DECIMAL_SMALLEST; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index d1aa9810..b3e094bd 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -31,7 +31,7 @@ #include "ConvexPolyhedronShape.h" #include "body/CollisionBody.h" #include "mathematics/mathematics.h" - +#include "memory/DefaultAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index 32ea146c..da29fede 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS } // Raycast method with feedback information -bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { const Vector3 n = ray.point2 - ray.point1; diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 8aebff58..0da2d1f4 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Raycasting method between a ray one of the two spheres end cap of the capsule bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 6792cc09..4381b05c 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -87,7 +87,7 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index ee90ce46..0b49f20c 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -111,12 +111,12 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { PROFILE("ConcaveMeshShape::raycast()", mProfiler); // Create the callback object that will compute ray casting against triangles - ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); + ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator); #ifdef IS_PROFILING_ACTIVE @@ -180,7 +180,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -192,7 +192,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 2d0a3e9c..52d49a02 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -77,6 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { RaycastInfo& mRaycastInfo; const Ray& mRay; bool mIsHit; + Allocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -89,9 +90,9 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, - ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator) : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), - mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) { + mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) { } @@ -141,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index f6011375..19ef5016 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() { } // Raycast method with feedback information -bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( ray, proxyShape, raycastInfo); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 0ce696af..1c33d919 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 74276a75..f067a827 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -212,14 +212,14 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead PROFILE("HeightFieldShape::raycast()", mProfiler); - TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); + TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator); #ifdef IS_PROFILING_ACTIVE @@ -266,7 +266,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId); + TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -278,7 +278,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index f8b498d5..4190c3c7 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -49,6 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback { bool mIsHit; decimal mSmallestHitFraction; const HeightFieldShape& mHeightFieldShape; + Allocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -61,9 +62,9 @@ class TriangleOverlapCallback : public TriangleCallback { // Constructor TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape) + const HeightFieldShape& heightFieldShape, Allocator& allocator) : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), - mHeightFieldShape (heightFieldShape) { + mHeightFieldShape (heightFieldShape), mAllocator(allocator) { mIsHit = false; mSmallestHitFraction = mRay.maxFraction; } @@ -143,7 +144,7 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index fa6b4f6a..7d155f97 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius) } // Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { const Vector3 m = ray.point1; decimal c = m.dot(m) - mMargin * mMargin; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 70f651de..18883d79 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -55,7 +55,7 @@ class SphereShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index e43cc674..4a7eb013 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -33,6 +33,7 @@ using namespace reactphysics3d; + // Constructor /** * Do not use this constructor. It is supposed to be used internally only. @@ -43,8 +44,9 @@ using namespace reactphysics3d; * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) { +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, + Allocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} { mPoints[0] = vertices[0]; mPoints[1] = vertices[1]; @@ -60,9 +62,10 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor // Faces for (uint i=0; i<2; i++) { - mFaces[i].faceVertices.push_back(0); - mFaces[i].faceVertices.push_back(1); - mFaces[i].faceVertices.push_back(2); + mFaces[i].faceVertices.reserve(3); + mFaces[i].faceVertices.add(0); + mFaces[i].faceVertices.add(1); + mFaces[i].faceVertices.add(2); mFaces[i].edgeIndex = i; } @@ -208,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. -bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { PROFILE("TriangleShape::raycast()", mProfiler); diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index a4ce7726..9a2c756e 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -60,6 +60,7 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Attribute -------------------- // + /// Three points of the triangle Vector3 mPoints[3]; @@ -90,7 +91,8 @@ class TriangleShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + Allocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -110,7 +112,8 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, + uint shapeId, Allocator& allocator); /// Destructor virtual ~TriangleShape() override = default; diff --git a/src/containers/List.h b/src/containers/List.h index 4915f32d..71ae7ef7 100644 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -44,59 +44,38 @@ class List { // -------------------- Attributes -------------------- // - /// Pointer to the first element of the list - T* mElements; + /// Buffer for the list elements + void* mBuffer; /// Number of elements in the list - uint mSize; + size_t mSize; /// Number of allocated elements in the list - uint mCapacity; + size_t mCapacity; /// Memory allocator Allocator& mAllocator; // -------------------- Methods -------------------- // - /// Allocate more memory for the elements of the list - void allocateMemory(uint nbElementsToAllocate) { - - assert(nbElementsToAllocate > mCapacity); - - // Allocate memory for the new array - void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T)); - - if (mElements != nullptr) { - - // Copy the elements to the new allocated memory location - std::memcpy(newMemory, static_cast(mElements), mSize * sizeof(T)); - - // Release the previously allocated memory - mAllocator.release(mElements, mCapacity * sizeof(T)); - } - - mElements = static_cast(newMemory); - - mCapacity = nbElementsToAllocate; - } public: // -------------------- Methods -------------------- // /// Constructor - List(Allocator& allocator, uint capacity = 0) - : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { + List(Allocator& allocator, size_t capacity = 0) + : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { if (capacity > 0) { // Allocate memory - allocateMemory(capacity); + reserve(capacity); } } /// Copy constructor - List(const List& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + List(const List& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { // All all the elements of the list to the current one addRange(list); @@ -112,22 +91,66 @@ class List { clear(); // Release the memory allocated on the heap - mAllocator.release(static_cast(mElements), mCapacity * sizeof(T)); + mAllocator.release(mBuffer, mCapacity * sizeof(T)); } } + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + // Allocate memory for the new array + void* newMemory = mAllocator.allocate(capacity * sizeof(T)); + + if (mBuffer != nullptr) { + + // Copy the elements to the new allocated memory location + std::memcpy(newMemory, mBuffer, mSize * sizeof(T)); + + // Release the previously allocated memory + mAllocator.release(mBuffer, mCapacity * sizeof(T)); + } + + mBuffer = newMemory; + assert(mBuffer != nullptr); + + mCapacity = capacity; + } + /// Add an element into the list void add(const T& element) { // If we need to allocate more memory if (mSize == mCapacity) { - allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2); + reserve(mCapacity == 0 ? 1 : mCapacity * 2); } - mElements[mSize] = element; + // Use the copy-constructor to construct the element + new (static_cast(mBuffer) + mSize * sizeof(T)) T(element); + mSize++; } + /// Remove an element from the list at a given index + void remove(uint index) { + + assert(index >= 0 && index < mSize); + + // Call the destructor + (static_cast(mBuffer)[index]).~T(); + + mSize--; + + if (index != mSize) { + + // Move the elements to fill in the empty slot + char* dest = static_cast(mBuffer) + index * sizeof(T); + char* src = dest + sizeof(T); + std::memcpy(static_cast(dest), static_cast(src), (mSize - index) * sizeof(T)); + } + } + /// Append another list to the current one void addRange(const List& list) { @@ -135,12 +158,13 @@ class List { if (mSize + list.size() > mCapacity) { // Allocate memory - allocateMemory(mSize + list.size()); + reserve(mSize + list.size()); } // Add the elements of the list to the current one for(uint i=0; i(mBuffer) + mSize * sizeof(T)) T(list[i]); mSize++; } } @@ -150,27 +174,32 @@ class List { // Call the destructor of each element for (uint i=0; i < mSize; i++) { - mElements[i].~T(); + (static_cast(mBuffer)[i]).~T(); } mSize = 0; } /// Return the number of elments in the list - uint size() const { + size_t size() const { return mSize; } + /// Return the capacity of the list + size_t capacity() const { + return mCapacity; + } + /// Overloaded index operator T& operator[](const uint index) { assert(index >= 0 && index < mSize); - return mElements[index]; + return (static_cast(mBuffer)[index]); } /// Overloaded const index operator const T& operator[](const uint index) const { assert(index >= 0 && index < mSize); - return mElements[index]; + return (static_cast(mBuffer)[index]); } /// Overloaded assignment operator diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index eeb06305..8b29e4a1 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -187,4 +187,3 @@ AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const { return mCollisionDetection.getWorldAABB(proxyShape); } - diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 6c558d6d..ead42710 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,6 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" +#include "memory/DefaultAllocator.h" #include "memory/PoolAllocator.h" #include "EventListener.h" diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index eba2fb80..431476ed 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -116,20 +116,16 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo // Clear all the potential contact manifolds void OverlappingPair::clearPotentialContactManifolds() { - // Do we need to release memory - if (mTempMemoryAllocator.isReleaseNeeded()) { + ContactManifoldInfo* element = mPotentialContactManifolds; + while(element != nullptr) { - ContactManifoldInfo* element = mPotentialContactManifolds; - while(element != nullptr) { + // Remove the proxy collision shape + ContactManifoldInfo* elementToRemove = element; + element = element->getNext(); - // Remove the proxy collision shape - ContactManifoldInfo* elementToRemove = element; - element = element->getNext(); - - // Delete the element - elementToRemove->~ContactManifoldInfo(); - mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); - } + // Delete the element + elementToRemove->~ContactManifoldInfo(); + mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); } mPotentialContactManifolds = nullptr; diff --git a/src/memory/MemoryManager.cpp b/src/memory/MemoryManager.cpp new file mode 100644 index 00000000..85832cb1 --- /dev/null +++ b/src/memory/MemoryManager.cpp @@ -0,0 +1,32 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2015 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "MemoryManager.h" + +using namespace reactphysics3d; + +// Static variables +DefaultAllocator MemoryManager::mDefaultAllocator; diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h new file mode 100644 index 00000000..922a5a17 --- /dev/null +++ b/src/memory/MemoryManager.h @@ -0,0 +1,74 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MEMORY_MANAGER_H +#define REACTPHYSICS3D_MEMORY_MANAGER_H + +// Libraries +#include "memory/DefaultAllocator.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class MemoryManager +/** + * The memory manager is used to store the different memory allocators that are used + * by the library. + */ +class MemoryManager { + + private: + + /// Default memory allocator + static DefaultAllocator mDefaultAllocator; + + public: + + /// Memory allocation types + enum class AllocationType { + Default, // Default memory allocator + Pool, // Memory pool allocator + Frame, // Single frame memory allocator + }; + + /// Constructor + MemoryManager(); + + /// Destructor + ~MemoryManager(); + + /// Return the default memory allocator + static DefaultAllocator& getDefaultAllocator(); +}; + +// Return the default memory allocator +inline DefaultAllocator& MemoryManager::getDefaultAllocator() { + return mDefaultAllocator; +} + +} + +#endif + diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp index dc1306bc..53cee0a1 100644 --- a/src/memory/PoolAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -126,7 +126,7 @@ void* PoolAllocator::allocate(size_t size) { } else { // If there is no more free memory units in the corresponding heap - // If we need to allocate more memory to containsthe blocks + // If we need to allocate more memory to contains the blocks if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) { // Allocate more memory to contain the blocks diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 0664e559..b06778d5 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -94,7 +94,7 @@ class PoolAllocator : public Allocator { /// Size of the memory units that each heap is responsible to allocate static size_t mUnitSizes[NB_HEAPS]; - /// Lookup table that mape size to allocate to the index of the + /// Lookup table that map the size to allocate to the index of the /// corresponding heap we will use for the allocation. static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h old mode 100644 new mode 100755 index 678b5f21..ad0c92cf --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -40,21 +40,108 @@ enum CollisionCategory { CATEGORY_3 = 0x0004 }; -// TODO : Add test for concave shape collision here +// Contact point collision data +struct CollisionPointData { + + Vector3 localPointBody1; + Vector3 localPointBody2; + decimal penetrationDepth; + + CollisionPointData(const Vector3& point1, const Vector3& point2, decimal penDepth) { + localPointBody1 = point1; + localPointBody2 = point2; + penetrationDepth = penDepth; + } + + bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) { + + return approxEqual(pointBody1, localPointBody1, epsilon) && + approxEqual(pointBody2, localPointBody2, epsilon) && + approxEqual(penetrationDepth, penDepth, epsilon); + } +}; + +// Contact manifold collision data +struct CollisionManifoldData { + + std::vector contactPoints; + + int getNbContactPoints() const { + return contactPoints.size(); + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + + if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; + +// Collision data between two proxy shapes +struct CollisionData { + + std::pair proxyShapes; + std::pair bodies; + std::vector contactManifolds; + + int getNbContactManifolds() const { + return contactManifolds.size(); + } + + int getTotalNbContactPoints() const { + + int nbPoints = 0; + + std::vector::const_iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + nbPoints += it->getNbContactPoints(); + } + + return nbPoints; + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; // Class class WorldCollisionCallback : public CollisionCallback { + private: + + std::vector> mCollisionData; + + std::pair getCollisionKeyPair(std::pair pair) const { + + if (pair.first > pair.second) { + return std::make_pair(pair.second, pair.first); + } + + return pair; + } + public: - bool boxCollideWithSphere1; - bool sphere1CollideWithSphere2; - - CollisionBody* boxBody; - CollisionBody* sphere1Body; - CollisionBody* sphere2Body; - CollisionBody* cylinderBody; - WorldCollisionCallback() { reset(); @@ -62,30 +149,79 @@ class WorldCollisionCallback : public CollisionCallback void reset() { - boxCollideWithSphere1 = false; - sphere1CollideWithSphere2 = false; + mCollisionData.clear(); } - // This method will be called for contact + bool hasContacts() const { + return mCollisionData.size() > 0; + } + + bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) { + return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end(); + } + + // This method will be called for each contact virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) { - boxCollideWithSphere1 = true; - } - else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) { - sphere1CollideWithSphere2 = true; - } - } + CollisionData collisionData; + collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2); + collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2); - bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2, - const CollisionCallbackInfo& collisionCallbackInfo) { - return (collisionCallbackInfo.body1->getID() == body1->getID() && - collisionCallbackInfo.body2->getID() == body2->getID()) || - (collisionCallbackInfo.body2->getID() == body1->getID() && - collisionCallbackInfo.body1->getID() == body2->getID()); + ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements; + while (element != nullptr) { + + ContactManifold* contactManifold = element->getContactManifold(); + + CollisionManifoldData collisionManifold; + + ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { + + CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth()); + collisionManifold.contactPoints.push_back(collisionPoint); + + contactPoint = contactPoint->getNext(); + } + + collisionData.contactManifolds.push_back(collisionManifold); + + element = element->getNext(); + } } }; +/// Overlap callback +class WorldOverlapCallback : public OverlapCallback { + + private: + + CollisionBody* mOverlapBody; + + public: + + /// Destructor + virtual ~WorldOverlapCallback() { + reset(); + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody) override { + + } + + void reset() { + mOverlapBody = nullptr; + } + + bool hasOverlap() const { + return mOverlapBody != nullptr; + } + + CollisionBody* getOverlapBody() { + return mOverlapBody; + } +}; + // Class TestCollisionWorld /** * Unit test for the CollisionWorld class. @@ -100,22 +236,29 @@ class TestCollisionWorld : public Test { CollisionWorld* mWorld; // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphere1Body; - CollisionBody* mSphere2Body; + CollisionBody* mBoxBody1; + CollisionBody* mBoxBody2; + CollisionBody* mSphereBody1; + CollisionBody* mSphereBody2; // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; + BoxShape* mBoxShape1; + BoxShape* mBoxShape2; + SphereShape* mSphereShape1; + SphereShape* mSphereShape2; // Proxy shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphere1ProxyShape; - ProxyShape* mSphere2ProxyShape; + ProxyShape* mBoxProxyShape1; + ProxyShape* mBoxProxyShape2; + ProxyShape* mSphereProxyShape1; + ProxyShape* mSphereProxyShape2; - // Collision callback class + // Collision callback WorldCollisionCallback mCollisionCallback; + // Overlap callback + WorldOverlapCallback mOverlapCallback; + public : // ---------- Methods ---------- // @@ -123,147 +266,243 @@ class TestCollisionWorld : public Test { /// Constructor TestCollisionWorld(const std::string& name) : Test(name) { - // Create the world + // Create the collision world mWorld = new CollisionWorld(); - // Create the bodies - Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); - mBoxBody = mWorld->createCollisionBody(boxTransform); - mBoxShape = new BoxShape(Vector3(3, 3, 3)); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); + // ---------- Boxes ---------- // + Transform boxTransform1(Vector3(-20, 20, 0), Quaternion::identity()); + mBoxBody1 = mWorld->createCollisionBody(boxTransform1); + mBoxShape1 = new BoxShape(Vector3(3, 3, 3)); + mBoxProxyShape1 = mBoxBody1->addCollisionShape(mBoxShape1, Transform::identity()); - mSphereShape = new SphereShape(3.0); - Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); - mSphere1Body = mWorld->createCollisionBody(sphere1Transform); - mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); + Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity()); + mBoxBody2 = mWorld->createCollisionBody(boxTransform2); + mBoxShape2 = new BoxShape(Vector3(2, 2, 2)); + mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity()); - Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); - mSphere2Body = mWorld->createCollisionBody(sphere2Transform); - mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); + // ---------- Spheres ---------- // + mSphereShape1 = new SphereShape(3.0); + Transform sphereTransform1(Vector3(10, 20, 0), Quaternion::identity()); + mSphereBody1 = mWorld->createCollisionBody(sphereTransform1); + mSphereProxyShape1 = mSphereBody1->addCollisionShape(mSphereShape1, Transform::identity()); - // Assign collision categories to proxy shapes - mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); - - mCollisionCallback.boxBody = mBoxBody; - mCollisionCallback.sphere1Body = mSphere1Body; - mCollisionCallback.sphere2Body = mSphere2Body; + mSphereShape2 = new SphereShape(5.0); + Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity()); + mSphereBody2 = mWorld->createCollisionBody(sphereTransform2); + mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity()); } /// Destructor virtual ~TestCollisionWorld() { - delete mBoxShape; - delete mSphereShape; + + delete mBoxShape1; + delete mBoxShape2; + + delete mSphereShape1; + delete mSphereShape2; + + delete mWorld; } /// Run the tests void run() { - testCollisions(); + testNoCollisions(); + testNoOverlap(); + testNoAABBOverlap(); + + testAABBOverlap(); + + testSphereVsSphereCollision(); + testSphereVsBoxCollision(); + + testMultipleCollisions(); } - void testCollisions() { + void testNoCollisions() { - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no collision at all. - test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); + // ---------- Global test ---------- // - test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // ---------- Single body test ---------- // - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // Two bodies test - mCollisionCallback.reset(); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + } + + void testNoOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no overlap at all. + + // ---------- Single body test ---------- // + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + // Two bodies test + + test(!mWorld->testOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody2)); + } + + void testNoAABBOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no AABB overlap at all. + + // Two bodies test + + test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2)); + } + + void testAABBOverlap() { + + // TODO : Test the CollisionWorld::testAABBOverlap() method here + } + + void testSphereVsSphereCollision() { + + + + // Move sphere 1 to collide with sphere 2 + mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + + } + + void testSphereVsBoxCollision() { // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); // --------- Test collision with inactive bodies --------- // mCollisionCallback.reset(); - mBoxBody->setIsActive(false); - mSphere1Body->setIsActive(false); - mSphere2Body->setIsActive(false); + mBoxBody1->setIsActive(false); + mSphereBody1->setIsActive(false); + mSphereBody2->setIsActive(false); mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + - test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); - - test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); - - mBoxBody->setIsActive(true); - mSphere1Body->setIsActive(true); - mSphere2Body->setIsActive(true); + mBoxBody1->setIsActive(true); + mSphereBody1->setIsActive(true); + mSphereBody2->setIsActive(true); // --------- Test collision with collision filtering -------- // - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + //// Move sphere 1 to collide with sphere 2 + //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(mCollisionCallback.sphere1CollideWithSphere2); - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + //// Move sphere 1 to collide with box + //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); - mBoxProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); + //mBoxProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); } + + void testMultipleCollisions() { + + // TODO : Test collisions without categories set + + // TODO : Test colliisons with categories set + + // Assign collision categories to proxy shapes + //mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); + } }; } diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h old mode 100644 new mode 100755 index 7ab3202b..53f5e618 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -114,6 +114,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; + +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif int object1Data = 56; int object2Data = 23; @@ -152,6 +158,10 @@ class TestDynamicAABBTree : public Test { test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data); test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data); test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testOverlapping() { @@ -161,6 +171,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -342,6 +358,9 @@ class TestDynamicAABBTree : public Test { test(!mOverlapCallback.isOverlapping(object3Id)); test(!mOverlapCallback.isOverlapping(object4Id)); +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testRaycast() { @@ -351,6 +370,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -513,6 +538,10 @@ class TestDynamicAABBTree : public Test { test(!mRaycastCallback.isHit(object2Id)); test(mRaycastCallback.isHit(object3Id)); test(mRaycastCallback.isHit(object4Id)); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } }; diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h index 4c4a2b39..35f357b3 100644 --- a/test/tests/collision/TestHalfEdgeStructure.h +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -19,6 +19,9 @@ class TestHalfEdgeStructure : public Test { // ---------- Atributes ---------- // + /// Memory allocator + DefaultAllocator mAllocator; + public : @@ -43,7 +46,7 @@ class TestHalfEdgeStructure : public Test { void testCube() { // Create the half-edge structure for a cube - rp3d::HalfEdgeStructure cubeStructure; + rp3d::HalfEdgeStructure cubeStructure(mAllocator, 6, 8, 24); rp3d::Vector3 vertices[8] = { rp3d::Vector3(-0.5, -0.5, 0.5), @@ -67,18 +70,18 @@ class TestHalfEdgeStructure : public Test { cubeStructure.addVertex(7); // Faces - std::vector face0; - face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); - std::vector face1; - face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); - std::vector face2; - face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6); - std::vector face3; - face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); - std::vector face4; - face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1); - std::vector face5; - face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); + List face0(mAllocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(mAllocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(mAllocator, 4); + face2.add(5); face2.add(4); face2.add(7); face2.add(6); + List face3(mAllocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(mAllocator, 4); + face4.add(0); face4.add(4); face4.add(5); face4.add(1); + List face5(mAllocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); cubeStructure.addFace(face0); cubeStructure.addFace(face1); @@ -168,7 +171,7 @@ class TestHalfEdgeStructure : public Test { // Create the half-edge structure for a tetrahedron std::vector> faces; - rp3d::HalfEdgeStructure tetrahedron; + rp3d::HalfEdgeStructure tetrahedron(mAllocator, 4, 4, 12); // Vertices rp3d::Vector3 vertices[4] = { @@ -184,14 +187,14 @@ class TestHalfEdgeStructure : public Test { tetrahedron.addVertex(3); // Faces - std::vector face0; - face0.push_back(0); face0.push_back(1); face0.push_back(2); - std::vector face1; - face1.push_back(0); face1.push_back(3); face1.push_back(1); - std::vector face2; - face2.push_back(1); face2.push_back(3); face2.push_back(2); - std::vector face3; - face3.push_back(0); face3.push_back(2); face3.push_back(3); + List face0(mAllocator, 3); + face0.add(0); face0.add(1); face0.add(2); + List face1(mAllocator, 3); + face1.add(0); face1.add(3); face1.add(1); + List face2(mAllocator, 3); + face2.add(1); face2.add(3); face2.add(2); + List face3(mAllocator, 3); + face3.add(0); face3.add(2); face3.add(3); tetrahedron.addFace(face0); tetrahedron.addFace(face1); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 97dd002a..d02ab847 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -99,6 +99,8 @@ class TestRaycast : public Test { // Raycast callback class WorldRaycastCallback mCallback; + DefaultAllocator mAllocator; + // Epsilon decimal epsilon; @@ -203,7 +205,7 @@ class TestRaycast : public Test { triangleVertices[1] = Vector3(105, 100, 0); triangleVertices[2] = Vector3(100, 103, 0); Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; - mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0); + mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0, mAllocator); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5); diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 793a79fb..b76cb78c 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -176,13 +176,13 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(-6, 3, 0)); segmentVertices.push_back(Vector3(8, 3, 0)); - std::vector planesNormals; - std::vector planesPoints; - planesNormals.push_back(Vector3(-1, 0, 0)); - planesPoints.push_back(Vector3(4, 0, 0)); + List planesNormals(mAllocator, 2); + List planesPoints(mAllocator, 2); + planesNormals.add(Vector3(-1, 0, 0)); + planesPoints.add(Vector3(4, 0, 0)); - std::vector clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], - planesPoints, planesNormals); + List clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], + planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -195,7 +195,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(8, 3, 0)); segmentVertices.push_back(Vector3(-6, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -208,7 +208,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(-6, 3, 0)); segmentVertices.push_back(Vector3(3, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 2); test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); @@ -221,7 +221,7 @@ class TestMathematicsFunctions : public Test { segmentVertices.push_back(Vector3(5, 3, 0)); segmentVertices.push_back(Vector3(8, 3, 0)); - clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals); + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); test(clipSegmentVertices.size() == 0); // Test clipPolygonWithPlanes() From 261ffef616b8d6af37eefd0beec52d2c3bb7a5ff Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 1 Jan 2018 18:35:57 +0100 Subject: [PATCH 103/105] Refactor memory allocation --- CMakeLists.txt | 2 +- src/body/CollisionBody.cpp | 12 +-- src/body/RigidBody.cpp | 12 ++- src/body/RigidBody.h | 4 +- src/collision/CollisionCallback.cpp | 12 ++- src/collision/CollisionCallback.h | 6 +- src/collision/CollisionDetection.cpp | 97 ++++++++++--------- src/collision/CollisionDetection.h | 19 ++-- src/collision/ContactManifold.cpp | 2 +- src/collision/ContactManifold.h | 4 +- src/collision/ContactManifoldInfo.cpp | 2 +- src/collision/ContactManifoldInfo.h | 6 +- src/collision/ContactManifoldSet.cpp | 2 +- src/collision/ContactManifoldSet.h | 4 +- src/collision/HalfEdgeStructure.h | 6 +- src/collision/MiddlePhaseTriangleCallback.h | 4 +- src/collision/NarrowPhaseInfo.cpp | 6 +- src/collision/NarrowPhaseInfo.h | 4 +- src/collision/PolyhedronMesh.cpp | 4 +- src/collision/ProxyShape.cpp | 8 +- src/collision/ProxyShape.h | 9 +- .../broadphase/BroadPhaseAlgorithm.cpp | 4 +- .../broadphase/BroadPhaseAlgorithm.h | 2 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 2 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 2 +- ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/NarrowPhaseAlgorithm.h | 2 +- .../narrowphase/SAT/SATAlgorithm.cpp | 2 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 4 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 2 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 2 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 2 +- .../narrowphase/SphereVsSphereAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 6 +- src/collision/shapes/BoxShape.h | 2 +- src/collision/shapes/CapsuleShape.cpp | 2 +- src/collision/shapes/CapsuleShape.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConcaveMeshShape.cpp | 2 +- src/collision/shapes/ConcaveMeshShape.h | 6 +- src/collision/shapes/ConvexMeshShape.cpp | 2 +- src/collision/shapes/ConvexMeshShape.h | 2 +- src/collision/shapes/HeightFieldShape.cpp | 2 +- src/collision/shapes/HeightFieldShape.h | 6 +- src/collision/shapes/SphereShape.cpp | 2 +- src/collision/shapes/SphereShape.h | 2 +- src/collision/shapes/TriangleShape.cpp | 4 +- src/collision/shapes/TriangleShape.h | 4 +- src/configuration.h | 2 +- src/containers/LinkedList.h | 6 +- src/containers/List.h | 6 +- src/engine/CollisionWorld.cpp | 8 +- src/engine/CollisionWorld.h | 10 +- src/engine/ContactSolver.cpp | 12 ++- src/engine/ContactSolver.h | 9 +- src/engine/DynamicsWorld.cpp | 63 +++++++----- src/engine/Island.cpp | 11 ++- src/engine/Island.h | 2 +- src/engine/OverlappingPair.cpp | 2 +- src/engine/OverlappingPair.h | 12 +-- src/mathematics/mathematics_functions.cpp | 4 +- src/mathematics/mathematics_functions.h | 4 +- src/memory/DefaultAllocator.h | 12 +-- src/memory/{Allocator.h => MemoryAllocator.h} | 19 ++-- src/memory/MemoryManager.cpp | 2 + src/memory/MemoryManager.h | 92 +++++++++++++++--- src/memory/PoolAllocator.cpp | 35 ++++--- src/memory/PoolAllocator.h | 16 +-- src/memory/SingleFrameAllocator.cpp | 75 ++++++++++++-- src/memory/SingleFrameAllocator.h | 34 ++++--- 75 files changed, 449 insertions(+), 296 deletions(-) rename src/memory/{Allocator.h => MemoryAllocator.h} (87%) diff --git a/CMakeLists.txt b/CMakeLists.txt index e585e0d1..bc11aebd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -187,7 +187,7 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" - "src/memory/Allocator.h" + "src/memory/MemoryAllocator.h" "src/memory/PoolAllocator.h" "src/memory/PoolAllocator.cpp" "src/memory/SingleFrameAllocator.h" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 56493d28..bb8b0408 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -70,9 +70,9 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, const Transform& transform) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, decimal(1), mWorld.mPoolAllocator); + transform, decimal(1), mWorld.mMemoryManager); #ifdef IS_PROFILING_ACTIVE @@ -123,7 +123,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } current->~ProxyShape(); - mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -143,7 +143,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } elementToRemove->~ProxyShape(); - mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, elementToRemove, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -169,7 +169,7 @@ void CollisionBody::removeAllCollisionShapes() { } current->~ProxyShape(); - mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape)); // Get the next element in the list current = nextElement; @@ -188,7 +188,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the current element currentElement->~ContactManifoldListElement(); - mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 7419fc4c..36c362dc 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -176,7 +176,7 @@ void RigidBody::setMass(decimal mass) { } // Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) { +void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) { assert(joint != nullptr); assert(mJointsList != nullptr); @@ -186,7 +186,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const JointListElement* elementToRemove = mJointsList; mJointsList = elementToRemove->next; elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + memoryManager.release(MemoryManager::AllocationType::Pool, + elementToRemove, sizeof(JointListElement)); } else { // If the element to remove is not the first one in the list JointListElement* currentElement = mJointsList; @@ -195,7 +196,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const JointListElement* elementToRemove = currentElement->next; currentElement->next = elementToRemove->next; elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + memoryManager.release(MemoryManager::AllocationType::Pool, + elementToRemove, sizeof(JointListElement)); break; } currentElement = currentElement->next; @@ -224,9 +226,9 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, decimal mass) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, mass, mWorld.mPoolAllocator); + transform, mass, mWorld.mMemoryManager); #ifdef IS_PROFILING_ACTIVE diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index e12cab05..23603bda 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -31,7 +31,7 @@ #include "CollisionBody.h" #include "engine/Material.h" #include "mathematics/mathematics.h" -#include "memory/PoolAllocator.h" +#include "memory/MemoryManager.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -112,7 +112,7 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // /// Remove a joint from the joints list - void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint); + void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint); /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp index db820db4..1702e40a 100644 --- a/src/collision/CollisionCallback.cpp +++ b/src/collision/CollisionCallback.cpp @@ -26,18 +26,18 @@ // Libraries #include "collision/CollisionCallback.h" #include "engine/OverlappingPair.h" -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" #include "collision/ContactManifold.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) : +CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) : contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()), body2(pair->getShape2()->getBody()), proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()), - mMemoryAllocator(allocator) { + mMemoryManager(memoryManager) { assert(pair != nullptr); @@ -52,7 +52,8 @@ CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement))) + ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, contactManifoldElements); contactManifoldElements = element; @@ -72,7 +73,8 @@ CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() { // Delete and release memory element->~ContactManifoldListElement(); - mMemoryAllocator.release(element, sizeof(ContactManifoldListElement)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, element, + sizeof(ContactManifoldListElement)); element = nextElement; } diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h index 54208fe4..57ae0f04 100644 --- a/src/collision/CollisionCallback.h +++ b/src/collision/CollisionCallback.h @@ -69,11 +69,11 @@ class CollisionCallback { /// Pointer to the proxy shape of second body const ProxyShape* proxyShape2; - /// Memory allocator - Allocator& mMemoryAllocator; + /// Memory manager + MemoryManager& mMemoryManager; // Constructor - CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator); + CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager); // Destructor ~CollisionCallbackInfo(); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index e57c5fd8..66cf1eb5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -47,9 +47,8 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator) - : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), - mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), +CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) + : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { // Set the default collision dispatch configuration @@ -95,7 +94,7 @@ void CollisionDetection::computeBroadPhase() { // Ask the broad-phase to recompute the overlapping pairs of collision // shapes. This call can only add new overlapping pairs in the collision // detection. - mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator); + mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryManager); } } @@ -133,7 +132,7 @@ void CollisionDetection::computeMiddlePhase() { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -166,10 +165,10 @@ void CollisionDetection::computeMiddlePhase() { // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) + mNarrowPhaseInfoList = new (mMemoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), mSingleFrameAllocator); + shape2->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator()); mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; } @@ -177,7 +176,7 @@ void CollisionDetection::computeMiddlePhase() { else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { NarrowPhaseInfo* narrowPhaseInfo = nullptr; - computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), &narrowPhaseInfo); // Add all the narrow-phase info object reported by the callback into the // list of all the narrow-phase info object @@ -202,7 +201,7 @@ void CollisionDetection::computeMiddlePhase() { } // Compute the concave vs convex middle-phase algorithm for a given pair of bodies -void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator, +void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, NarrowPhaseInfo** firstNarrowPhaseInfo) { ProxyShape* shape1 = pair->getShape1(); @@ -273,7 +272,7 @@ void CollisionDetection::computeNarrowPhase() { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -295,7 +294,7 @@ void CollisionDetection::computeNarrowPhase() { narrowPhaseInfoToDelete->~NarrowPhaseInfo(); // Release the allocated memory for the narrow phase info - mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); } // Convert the potential contact into actual contacts @@ -327,8 +326,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator); + OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), + mMemoryManager.getSingleFrameAllocator()); assert(newPair != nullptr); #ifndef NDEBUG @@ -359,7 +359,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); } else { @@ -403,14 +403,16 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) + ContactManifoldListElement* listElement1 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); body1->mContactManifoldsList = listElement1; // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement))) + ContactManifoldListElement* listElement2 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); body2->mContactManifoldsList = listElement2; @@ -470,7 +472,7 @@ void CollisionDetection::reportAllContacts() { // If there is a user callback if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { - CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator); + CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mMemoryManager); // Trigger a callback event to report the new contact to the user mWorld->mEventListener->newContact(collisionInfo); @@ -498,9 +500,10 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), + narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), mPoolAllocator); + shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator()); } // Concave vs Convex algorithm @@ -508,7 +511,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin // Run the middle-phase collision detection algorithm to find the triangles of the concave // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), &narrowPhaseInfo); } pair->clearObsoleteLastFrameCollisionInfos(); @@ -524,7 +527,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over std::unordered_set reportedBodies; // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); // For each overlaping proxy shape @@ -575,7 +578,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) if (aabb1.testCollision(aabb2)) { // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -600,7 +604,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); } } @@ -611,7 +615,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) currentNarrowPhaseInfo->~NarrowPhaseInfo(); // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -648,7 +652,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -670,7 +674,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -695,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); } } @@ -706,7 +711,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl currentNarrowPhaseInfo->~NarrowPhaseInfo(); // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -754,7 +759,8 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body if (aabb1.testCollision(aabb2)) { // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator); + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -774,7 +780,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -788,7 +794,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body currentNarrowPhaseInfo->~NarrowPhaseInfo(); // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Process the potential contacts @@ -797,7 +803,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body if (pair.hasContacts()) { // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); collisionCallback->notifyContact(collisionInfo); } } @@ -826,7 +832,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mPoolAllocator); + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); const bodyindex bodyId = body->getID(); @@ -846,7 +852,8 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator); + OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); // Compute the middle-phase collision detection between the two shapes NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); @@ -866,7 +873,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -880,7 +887,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c currentNarrowPhaseInfo->~NarrowPhaseInfo(); // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Process the potential contacts @@ -889,7 +896,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c if (pair.hasContacts()) { // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); callback->notifyContact(collisionInfo); } } @@ -920,8 +927,8 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { OverlappingPair* originalPair = it->second; // Create a new overlapping pair so that we do not work on the original one - OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator, - mPoolAllocator); + OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); ProxyShape* shape1 = pair.getShape1(); ProxyShape* shape2 = pair.getShape2(); @@ -950,7 +957,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { // Add the contact points as a potential contact manifold into the pair narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); @@ -964,7 +971,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { currentNarrowPhaseInfo->~NarrowPhaseInfo(); // Release the allocated memory - mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Process the potential contacts @@ -973,7 +980,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { if (pair.hasContacts()) { // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); callback->notifyContact(collisionInfo); } } @@ -996,12 +1003,6 @@ EventListener* CollisionDetection::getWorldEventListener() { return mWorld->mEventListener; } -// Return a reference to the world memory allocator -PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { - return mWorld->mPoolAllocator; -} - - // Return the world-space AABB of a given proxy shape const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const { assert(proxyShape->mBroadPhaseID > -1); diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index d728a95f..6aeac709 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -32,8 +32,7 @@ #include "engine/OverlappingPair.h" #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" -#include "memory/PoolAllocator.h" -#include "memory/SingleFrameAllocator.h" +#include "memory/MemoryManager.h" #include "constraint/ContactPoint.h" #include #include @@ -62,6 +61,9 @@ class CollisionDetection { // -------------------- Attributes -------------------- // + /// Memory manager + MemoryManager& mMemoryManager; + /// Collision Detection Dispatch configuration CollisionDispatch* mCollisionDispatch; @@ -71,12 +73,6 @@ class CollisionDetection { /// Collision detection matrix (algorithms to use) NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; - /// Reference to the memory allocator - PoolAllocator& mPoolAllocator; - - /// Reference to the single frame memory allocator - SingleFrameAllocator& mSingleFrameAllocator; - /// Pointer to the physics world CollisionWorld* mWorld; @@ -133,7 +129,7 @@ class CollisionDetection { void addAllContactManifoldsToBodies(); /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies - void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator, + void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, NarrowPhaseInfo** firstNarrowPhaseInfo); /// Compute the middle-phase collision detection between two proxy shapes @@ -156,7 +152,7 @@ class CollisionDetection { // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator); + CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager); /// Destructor ~CollisionDetection() = default; @@ -223,9 +219,6 @@ class CollisionDetection { /// Return the world event listener EventListener* getWorldEventListener(); - /// Return a reference to the world memory allocator - PoolAllocator& getWorldMemoryAllocator(); - #ifdef IS_PROFILING_ACTIVE /// Set the profiler diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index c2d60422..ba1bcb1e 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator) +ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator) : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 8303f35c..f513ad82 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -129,7 +129,7 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - Allocator& mMemoryAllocator; + MemoryAllocator& mMemoryAllocator; /// Pointer to the next contact manifold in the linked-list ContactManifold* mNext; @@ -217,7 +217,7 @@ class ContactManifold { /// Constructor ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - Allocator& memoryAllocator); + MemoryAllocator& memoryAllocator); /// Destructor ~ContactManifold(); diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index 6740fe4f..a28e3817 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -29,7 +29,7 @@ using namespace reactphysics3d; // Constructor -ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator) +ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator) : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) { } diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index 4126f4c8..b2687da5 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -28,7 +28,7 @@ // Libraries #include "collision/ContactPointInfo.h" -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -57,14 +57,14 @@ class ContactManifoldInfo { ContactManifoldInfo* mNext; /// Reference the the memory allocator where the contact point infos have been allocated - Allocator& mAllocator; + MemoryAllocator& mAllocator; public: // -------------------- Methods -------------------- // /// Constructor - ContactManifoldInfo(Allocator& allocator); + ContactManifoldInfo(MemoryAllocator& allocator); /// Destructor ~ContactManifoldInfo(); diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 5f70bbc0..c63e6a11 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - Allocator& memoryAllocator) + MemoryAllocator& memoryAllocator) : mNbManifolds(0), mShape1(shape1), mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) { diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 13d27b70..56b09b04 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -61,7 +61,7 @@ class ContactManifoldSet { ProxyShape* mShape2; /// Reference to the memory allocator for the contact manifolds - Allocator& mMemoryAllocator; + MemoryAllocator& mMemoryAllocator; /// Contact manifolds of the set ContactManifold* mManifolds; @@ -95,7 +95,7 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - Allocator& memoryAllocator); + MemoryAllocator& memoryAllocator); /// Destructor ~ContactManifoldSet(); diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h index e398913f..15658a93 100644 --- a/src/collision/HalfEdgeStructure.h +++ b/src/collision/HalfEdgeStructure.h @@ -54,7 +54,7 @@ class HalfEdgeStructure { List faceVertices; // Index of the vertices of the face /// Constructor - Face(Allocator& allocator) : faceVertices(allocator) {} + Face(MemoryAllocator& allocator) : faceVertices(allocator) {} /// Constructor Face(List vertices) : faceVertices(vertices) {} @@ -71,7 +71,7 @@ class HalfEdgeStructure { private: /// Reference to a memory allocator - Allocator& mAllocator; + MemoryAllocator& mAllocator; /// All the faces List mFaces; @@ -85,7 +85,7 @@ class HalfEdgeStructure { public: /// Constructor - HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity, + HalfEdgeStructure(MemoryAllocator& allocator, uint facesCapacity, uint verticesCapacity, uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity), mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {} diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h index e04d7a9c..b4798a2c 100644 --- a/src/collision/MiddlePhaseTriangleCallback.h +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -56,7 +56,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { const ConcaveShape* mConcaveShape; /// Reference to the single-frame memory allocator - Allocator& mAllocator; + MemoryAllocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -74,7 +74,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, ProxyShape* concaveProxyShape, ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, - Allocator& allocator) + MemoryAllocator& allocator) :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), mAllocator(allocator), narrowPhaseInfoList(nullptr) { diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index 0189db99..2c7a226c 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // Constructor NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, Allocator& shapeAllocator) + const Transform& shape2Transform, MemoryAllocator& shapeAllocator) : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) { @@ -68,7 +68,7 @@ void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penD assert(penDepth > decimal(0.0)); // Get the memory allocator - Allocator& allocator = overlappingPair->getTemporaryAllocator(); + MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); // Create the contact point info ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) @@ -89,7 +89,7 @@ void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() { void NarrowPhaseInfo::resetContactPoints() { // Get the memory allocator - Allocator& allocator = overlappingPair->getTemporaryAllocator(); + MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); // For each remaining contact point info ContactPointInfo* element = contactPoints; diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index b732480c..f9ea1ac9 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -67,12 +67,12 @@ struct NarrowPhaseInfo { NarrowPhaseInfo* next; /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) - Allocator& collisionShapeAllocator; + MemoryAllocator& collisionShapeAllocator; /// Constructor NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, Allocator& shapeAllocator); + const Transform& shape2Transform, MemoryAllocator& shapeAllocator); /// Destructor ~NarrowPhaseInfo(); diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 08a84e7d..64db474e 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -36,7 +36,7 @@ using namespace reactphysics3d; * @param polygonVertexArray Pointer to the array of polygons and their vertices */ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) - : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), + : mHalfEdgeStructure(MemoryManager::getBaseAllocator(), polygonVertexArray->getNbFaces(), polygonVertexArray->getNbVertices(), (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) { @@ -75,7 +75,7 @@ void PolyhedronMesh::createHalfEdgeStructure() { // Get the polygon face PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); - List faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices); + List faceVertices(MemoryManager::getBaseAllocator(), face->nbVertices); // For each vertex of the face for (uint v=0; v < face->nbVertices; v++) { diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index f782b2a8..8847d094 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -35,9 +35,9 @@ using namespace reactphysics3d; * @param transform Transformation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator) - :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) { +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, MemoryManager& memoryManager) + :mMemoryManager(memoryManager), mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), + mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { } @@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { worldToLocalTransform * ray.point2, ray.maxFraction); - bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator); + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator()); // Convert the raycast info into world-space raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 0ba34698..0da79e28 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -29,6 +29,7 @@ // Libraries #include "body/CollisionBody.h" #include "shapes/CollisionShape.h" +#include "memory/MemoryManager.h" namespace reactphysics3d { @@ -48,6 +49,9 @@ class ProxyShape { // -------------------- Attributes -------------------- // + /// Reference to the memory manager + MemoryManager& mMemoryManager; + /// Pointer to the parent body CollisionBody* mBody; @@ -82,9 +86,6 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; - /// Memory allocator - Allocator& mAllocator; - #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -103,7 +104,7 @@ class ProxyShape { /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, decimal mass, Allocator& allocator); + const Transform& transform, decimal mass, MemoryManager& memoryManager); /// Destructor virtual ~ProxyShape(); diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index c1484434..8998859e 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -185,14 +185,14 @@ void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb, } // Compute all the overlapping pairs of collision shapes -void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) { +void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager) { // TODO : Try to see if we can allocate potential pairs in single frame allocator // Reset the potential overlapping pairs mNbPotentialPairs = 0; - LinkedList overlappingNodes(allocator); + LinkedList overlappingNodes(memoryManager.getPoolAllocator()); // For all collision shapes that have moved (or have been created) during the // last simulation step diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 7e1ff262..30a67920 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -210,7 +210,7 @@ class BroadPhaseAlgorithm { void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList& overlappingNodes) const; /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(Allocator& allocator); + void computeOverlappingPairs(MemoryManager& memoryManager); /// Return the proxy shape corresponding to the broad-phase node id in parameter ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const; diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index bf9eefc2..dce9b04b 100755 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -34,7 +34,7 @@ using namespace reactphysics3d; // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index df9a8854..b122ca14 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two capsules - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index f943312b..33a381f0 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -38,7 +38,7 @@ using namespace reactphysics3d; // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index d7f96d63..58bb14fb 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a capsule and a polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 5cf10b17..375814cd 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm(memoryAllocator); diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h index 204bc269..ddd10585 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between two convex polyhedra - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 40d3fbc6..3088d874 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -92,7 +92,7 @@ class NarrowPhaseAlgorithm { /// Compute a contact info if the two bounding volumes collide virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator)=0; + MemoryAllocator& memoryAllocator)=0; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 1992cbc7..66be7d06 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); // Constructor -SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { +SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 52884f8e..0402067f 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -51,7 +51,7 @@ class SATAlgorithm { static const decimal SAME_SEPARATING_AXIS_BIAS; /// Memory allocator - Allocator& mMemoryAllocator; + MemoryAllocator& mMemoryAllocator; #ifdef IS_PROFILING_ACTIVE @@ -115,7 +115,7 @@ class SATAlgorithm { // -------------------- Methods -------------------- // /// Constructor - SATAlgorithm(Allocator& memoryAllocator); + SATAlgorithm(MemoryAllocator& memoryAllocator); /// Destructor ~SATAlgorithm() = default; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index f5bfc11d..2e86dfe3 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index b51bba57..c7c4b39e 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a capsule - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 6cc3e578..8d4f562c 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 17055431..1929f43c 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index f8820106..8103380e 100755 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, - Allocator& memoryAllocator) { + MemoryAllocator& memoryAllocator) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index e0741ab8..142053c4 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override; + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; }; } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 079b15c5..c13e32f4 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -39,7 +39,7 @@ using namespace reactphysics3d; */ BoxShape::BoxShape(const Vector3& extent) : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent), - mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) { + mHalfEdgeStructure(MemoryManager::getBaseAllocator(), 6, 8, 24) { assert(extent.x > decimal(0.0)); assert(extent.y > decimal(0.0)); @@ -55,7 +55,7 @@ BoxShape::BoxShape(const Vector3& extent) mHalfEdgeStructure.addVertex(6); mHalfEdgeStructure.addVertex(7); - DefaultAllocator& allocator = MemoryManager::getDefaultAllocator(); + MemoryAllocator& allocator = MemoryManager::getBaseAllocator(); // Faces List face0(allocator, 4); @@ -98,7 +98,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const } // Raycast method with feedback information -bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { Vector3 rayDirection = ray.point2 - ray.point1; decimal tMin = DECIMAL_SMALLEST; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index b3e094bd..2ebcdce1 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index da29fede..3f89d090 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS } // Raycast method with feedback information -bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { const Vector3 n = ray.point2 - ray.point1; diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 0da2d1f4..7843f139 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Raycasting method between a ray one of the two spheres end cap of the capsule bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 4381b05c..8d84ab55 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -87,7 +87,7 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const=0; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 0b49f20c..8f215fc2 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -111,7 +111,7 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { PROFILE("ConcaveMeshShape::raycast()", mProfiler); diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 52d49a02..c8c91972 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -77,7 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { RaycastInfo& mRaycastInfo; const Ray& mRay; bool mIsHit; - Allocator& mAllocator; + MemoryAllocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -90,7 +90,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, - ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator) + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator) : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) { @@ -142,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 19ef5016..5b6c70d6 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() { } // Raycast method with feedback information -bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( ray, proxyShape, raycastInfo); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 1c33d919..44d01df4 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index f067a827..badfb68d 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -212,7 +212,7 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 4190c3c7..fc587d4c 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -49,7 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback { bool mIsHit; decimal mSmallestHitFraction; const HeightFieldShape& mHeightFieldShape; - Allocator& mAllocator; + MemoryAllocator& mAllocator; #ifdef IS_PROFILING_ACTIVE @@ -62,7 +62,7 @@ class TriangleOverlapCallback : public TriangleCallback { // Constructor TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape, Allocator& allocator) + const HeightFieldShape& heightFieldShape, MemoryAllocator& allocator) : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), mHeightFieldShape (heightFieldShape), mAllocator(allocator) { mIsHit = false; @@ -144,7 +144,7 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 7d155f97..3ad7afe7 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius) } // Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { const Vector3 m = ray.point1; decimal c = m.dot(m) - mMargin * mMargin; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 18883d79..90b20d2d 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -55,7 +55,7 @@ class SphereShape : public ConvexShape { virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 4a7eb013..2257c0e6 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; * @param margin The collision margin (in meters) around the collision shape */ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, - Allocator& allocator) + MemoryAllocator& allocator) : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} { mPoints[0] = vertices[0]; @@ -211,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. -bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const { +bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { PROFILE("TriangleShape::raycast()", mProfiler); diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 9a2c756e..23f26adc 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -92,7 +92,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, - Allocator& allocator) const override; + MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -113,7 +113,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// Constructor TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, - uint shapeId, Allocator& allocator); + uint shapeId, MemoryAllocator& allocator); /// Destructor virtual ~TriangleShape() override = default; diff --git a/src/configuration.h b/src/configuration.h index a25c1502..96391e76 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -152,7 +152,7 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95); /// Size (in bytes) of the single frame allocator -constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb +constexpr size_t INIT_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; // 1Mb } diff --git a/src/containers/LinkedList.h b/src/containers/LinkedList.h index 281c134c..997035b7 100644 --- a/src/containers/LinkedList.h +++ b/src/containers/LinkedList.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_LINKED_LIST_H // Libraries -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" namespace reactphysics3d { @@ -64,14 +64,14 @@ class LinkedList { ListElement* mListHead; /// Memory allocator used to allocate the list elements - Allocator& mAllocator; + MemoryAllocator& mAllocator; public: // -------------------- Methods -------------------- // /// Constructor - LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) { + LinkedList(MemoryAllocator& allocator) : mListHead(nullptr), mAllocator(allocator) { } diff --git a/src/containers/List.h b/src/containers/List.h index 71ae7ef7..176766a5 100644 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -28,7 +28,7 @@ // Libraries #include "configuration.h" -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" #include namespace reactphysics3d { @@ -54,7 +54,7 @@ class List { size_t mCapacity; /// Memory allocator - Allocator& mAllocator; + MemoryAllocator& mAllocator; // -------------------- Methods -------------------- // @@ -64,7 +64,7 @@ class List { // -------------------- Methods -------------------- // /// Constructor - List(Allocator& allocator, size_t capacity = 0) + List(MemoryAllocator& allocator, size_t capacity = 0) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { if (capacity > 0) { diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 8b29e4a1..da5bf609 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,8 +33,7 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), - mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0), + : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0), mEventListener(nullptr) { #ifdef IS_PROFILING_ACTIVE @@ -74,7 +73,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the collision body - CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody))) + CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); assert(collisionBody != nullptr); @@ -111,7 +111,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { mBodies.erase(collisionBody); // Free the object from the memory allocator - mPoolAllocator.release(collisionBody, sizeof(CollisionBody)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody)); } // Return the next available body ID diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index ead42710..9f1fea58 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,8 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" -#include "memory/DefaultAllocator.h" -#include "memory/PoolAllocator.h" +#include "memory/MemoryManager.h" #include "EventListener.h" /// Namespace reactphysics3d @@ -62,11 +61,8 @@ class CollisionWorld { // -------------------- Attributes -------------------- // - /// Pool Memory allocator - PoolAllocator mPoolAllocator; - - /// Single frame Memory allocator - SingleFrameAllocator mSingleFrameAllocator; + /// Memory manager + MemoryManager mMemoryManager; /// Reference to the collision detection CollisionDetection mCollisionDetection; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 6361badd..f4762c49 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,9 +39,9 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(SingleFrameAllocator& allocator) - :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mSingleFrameAllocator(allocator), +ContactSolver::ContactSolver(MemoryManager& memoryManager) + :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr), + mSplitAngularVelocities(nullptr), mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mIsSplitImpulseActive(true) { @@ -75,10 +75,12 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { if (nbContactManifolds == 0 || nbContactPoints == 0) return; // TODO : Count exactly the number of constraints to allocate here - mContactPoints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints)); + mContactPoints = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactPointSolver) * nbContactPoints)); assert(mContactPoints != nullptr); - mContactConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds)); + mContactConstraints = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactManifoldSolver) * nbContactManifolds)); assert(mContactConstraints != nullptr); // For each island of the world diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 05bd569c..8799a1ad 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -272,6 +272,10 @@ class ContactSolver { // -------------------- Attributes -------------------- // + /// Memory manager + MemoryManager& mMemoryManager; + + /// Split linear velocities for the position contact solver (split impulse) Vector3* mSplitLinearVelocities; @@ -293,9 +297,6 @@ class ContactSolver { /// Number of contact constraints uint mNbContactManifolds; - /// Single frame memory allocator - SingleFrameAllocator& mSingleFrameAllocator; - /// Array of linear velocities Vector3* mLinearVelocities; @@ -339,7 +340,7 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(SingleFrameAllocator& allocator); + ContactSolver(MemoryManager& memoryManager); /// Destructor ~ContactSolver() = default; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 054c4dc5..35b1ba04 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -41,7 +41,7 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mSingleFrameAllocator), + mContactSolver(mMemoryManager), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), @@ -147,7 +147,7 @@ void DynamicsWorld::update(decimal timeStep) { resetBodiesForceAndTorque(); // Reset the single frame memory allocator - mSingleFrameAllocator.reset(); + mMemoryManager.resetFrameAllocator(); } // Integrate position and orientation of the rigid bodies. @@ -236,12 +236,18 @@ void DynamicsWorld::initVelocityArrays() { // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); - mSplitLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mSplitAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedPositions = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedOrientations = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Quaternion))); + mSplitLinearVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mSplitAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedLinearVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedPositions = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedOrientations = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Quaternion))); assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); assert(mConstrainedLinearVelocities != nullptr); @@ -412,8 +418,8 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the rigid body - RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, - *this, bodyID); + RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(RigidBody))) RigidBody(transform, *this, bodyID); assert(rigidBody != nullptr); // Add the rigid body to the physics world @@ -459,7 +465,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mRigidBodies.erase(rigidBody); // Free the object from the memory allocator - mPoolAllocator.release(rigidBody, sizeof(RigidBody)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint @@ -477,7 +483,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(info); @@ -487,7 +494,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(info); break; @@ -496,7 +504,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(info); break; @@ -505,7 +514,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(info); break; @@ -558,8 +568,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mJoints.erase(joint); // Remove the joint from the joint list of the bodies involved in the joint - joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint); - joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint); + joint->mBody1->removeJointFromJointsList(mMemoryManager, joint); + joint->mBody2->removeJointFromJointsList(mMemoryManager, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -567,7 +577,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->~Joint(); // Release the allocated memory - mPoolAllocator.release(joint, nbBytes); + mMemoryManager.release(MemoryManager::AllocationType::Pool, joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint @@ -576,13 +586,15 @@ void DynamicsWorld::addJointToBody(Joint* joint) { assert(joint != nullptr); // Add the joint at the beginning of the linked list of joints of the first body - void* allocatedMemory1 = mPoolAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, joint->mBody1->mJointsList); joint->mBody1->mJointsList = jointListElement1; // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; @@ -603,7 +615,8 @@ void DynamicsWorld::computeIslands() { // Allocate and create the array of islands pointer. This memory is allocated // in the single frame allocator - mIslands = static_cast(mSingleFrameAllocator.allocate(sizeof(Island*) * nbBodies)); + mIslands = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Island*) * nbBodies)); mNbIslands = 0; int nbContactManifolds = 0; @@ -619,7 +632,8 @@ void DynamicsWorld::computeIslands() { // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = static_cast(mSingleFrameAllocator.allocate(nbBytesStack)); + RigidBody** stackBodiesToVisit = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBytesStack)); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -642,9 +656,10 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island)); + void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Island)); mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(), - mSingleFrameAllocator); + mMemoryManager); // While there are still some bodies to visit in the stack while (stackIndex > 0) { diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 19eef304..34b65540 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -29,14 +29,17 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator) +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryManager& memoryManager) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), mNbContactManifolds(0), mNbJoints(0) { // Allocate memory for the arrays on the single frame allocator - mBodies = static_cast(allocator.allocate(sizeof(RigidBody*) * nbMaxBodies)); - mContactManifolds = static_cast(allocator.allocate(sizeof(ContactManifold*) * nbMaxContactManifolds)); - mJoints = static_cast(allocator.allocate(sizeof(Joint*) * nbMaxJoints)); + mBodies = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(RigidBody*) * nbMaxBodies)); + mContactManifolds = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactManifold*) * nbMaxContactManifolds)); + mJoints = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Joint*) * nbMaxJoints)); } // Destructor diff --git a/src/engine/Island.h b/src/engine/Island.h index 5f370004..ca1e6fdd 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -69,7 +69,7 @@ class Island { /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - SingleFrameAllocator& allocator); + MemoryManager& memoryManager); /// Destructor ~Island(); diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 431476ed..7a95be95 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator) + MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator) : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr), mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 8c65e633..f3631775 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -113,18 +113,18 @@ class OverlappingPair { ContactManifoldInfo* mPotentialContactManifolds; /// Persistent memory allocator - Allocator& mPersistentAllocator; + MemoryAllocator& mPersistentAllocator; /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo - Allocator& mTempMemoryAllocator; + MemoryAllocator& mTempMemoryAllocator; public: // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, Allocator& persistentMemoryAllocator, - Allocator& temporaryMemoryAllocator); + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator, + MemoryAllocator& temporaryMemoryAllocator); /// Destructor ~OverlappingPair(); @@ -157,7 +157,7 @@ class OverlappingPair { void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); /// Return a reference to the temporary memory allocator - Allocator& getTemporaryAllocator(); + MemoryAllocator& getTemporaryAllocator(); /// Return true if one of the shapes of the pair is a concave shape bool hasConcaveShape() const; @@ -264,7 +264,7 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body } // Return a reference to the temporary memory allocator -inline Allocator& OverlappingPair::getTemporaryAllocator() { +inline MemoryAllocator& OverlappingPair::getTemporaryAllocator() { return mTempMemoryAllocator; } diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 0d8994f4..9fa13083 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -225,7 +225,7 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, const List& planesPoints, const List& planesNormals, - Allocator& allocator) { + MemoryAllocator& allocator) { assert(planesPoints.size() == planesNormals.size()); @@ -303,7 +303,7 @@ List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V // Clip a polygon against multiple planes and return the clipped polygon vertices // This method implements the Sutherland–Hodgman clipping algorithm List reactphysics3d::clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, - const List& planesNormals, Allocator& allocator) { + const List& planesNormals, MemoryAllocator& allocator) { assert(planesPoints.size() == planesNormals.size()); diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 5081d741..eda0fdd3 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -115,11 +115,11 @@ decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& lin List clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, const List& planesPoints, const List& planesNormals, - Allocator& allocator); + MemoryAllocator& allocator); /// Clip a polygon against multiple planes and return the clipped polygon vertices List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, - const List& planesNormals, Allocator& allocator); + const List& planesNormals, MemoryAllocator& allocator); /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h index 367171bd..ac4ddd04 100644 --- a/src/memory/DefaultAllocator.h +++ b/src/memory/DefaultAllocator.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H // Libraries -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" #include /// ReactPhysics3D namespace @@ -37,13 +37,16 @@ namespace reactphysics3d { /** * This class represents a default memory allocator that uses default malloc/free methods */ -class DefaultAllocator : public Allocator { +class DefaultAllocator : public MemoryAllocator { public: /// Destructor virtual ~DefaultAllocator() = default; + /// Assignment operator + DefaultAllocator& operator=(DefaultAllocator& allocator) = default; + /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. virtual void* allocate(size_t size) override { @@ -54,11 +57,6 @@ class DefaultAllocator : public Allocator { virtual void release(void* pointer, size_t size) override { free(pointer); } - - /// Return true if memory needs to be release with this allocator - virtual bool isReleaseNeeded() const override { - return true; - } }; } diff --git a/src/memory/Allocator.h b/src/memory/MemoryAllocator.h similarity index 87% rename from src/memory/Allocator.h rename to src/memory/MemoryAllocator.h index 0440a5c9..c0d86a33 100644 --- a/src/memory/Allocator.h +++ b/src/memory/MemoryAllocator.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_ALLOCATOR_H -#define REACTPHYSICS3D_ALLOCATOR_H +#ifndef REACTPHYSICS3D_MEMORY_ALLOCATOR_H +#define REACTPHYSICS3D_MEMORY_ALLOCATOR_H // Libraries #include @@ -32,16 +32,22 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Class Allocator +// Class MemoryAllocator /** * Abstract class with the basic interface of all the derived memory allocators */ -class Allocator { +class MemoryAllocator { public: + /// Constructor + MemoryAllocator() = default; + /// Destructor - virtual ~Allocator() = default; + virtual ~MemoryAllocator() = default; + + /// Assignment operator + MemoryAllocator& operator=(MemoryAllocator& allocator) = default; /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. @@ -49,9 +55,6 @@ class Allocator { /// Release previously allocated memory. virtual void release(void* pointer, size_t size)=0; - - /// Return true if memory needs to be release with this allocator - virtual bool isReleaseNeeded() const=0; }; } diff --git a/src/memory/MemoryManager.cpp b/src/memory/MemoryManager.cpp index 85832cb1..99a32655 100644 --- a/src/memory/MemoryManager.cpp +++ b/src/memory/MemoryManager.cpp @@ -30,3 +30,5 @@ using namespace reactphysics3d; // Static variables DefaultAllocator MemoryManager::mDefaultAllocator; +MemoryAllocator* MemoryManager::mBaseAllocator = &mDefaultAllocator; + diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h index 922a5a17..8d986a31 100644 --- a/src/memory/MemoryManager.h +++ b/src/memory/MemoryManager.h @@ -27,7 +27,10 @@ #define REACTPHYSICS3D_MEMORY_MANAGER_H // Libraries +#include "memory/MemoryAllocator.h" #include "memory/DefaultAllocator.h" +#include "memory/PoolAllocator.h" +#include "memory/SingleFrameAllocator.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -41,31 +44,98 @@ class MemoryManager { private: - /// Default memory allocator + /// Default malloc/free memory allocator static DefaultAllocator mDefaultAllocator; + /// Pointer to the base memory allocator to use + static MemoryAllocator* mBaseAllocator; + + /// Memory pool allocator + PoolAllocator mPoolAllocator; + + /// Single frame stack allocator + SingleFrameAllocator mSingleFrameAllocator; + public: /// Memory allocation types enum class AllocationType { - Default, // Default memory allocator + Base, // Base memory allocator Pool, // Memory pool allocator Frame, // Single frame memory allocator }; - /// Constructor - MemoryManager(); + /// Constructor + MemoryManager() = default; - /// Destructor - ~MemoryManager(); + /// Destructor + ~MemoryManager() = default; - /// Return the default memory allocator - static DefaultAllocator& getDefaultAllocator(); + /// Allocate memory of a given type + void* allocate(AllocationType allocationType, size_t size); + + /// Release previously allocated memory. + void release(AllocationType allocationType, void* pointer, size_t size); + + /// Return the pool allocator + PoolAllocator& getPoolAllocator(); + + /// Return the single frame stack allocator + SingleFrameAllocator& getSingleFrameAllocator(); + + /// Return the base memory allocator + static MemoryAllocator& getBaseAllocator(); + + /// Set the base memory allocator + static void setBaseAllocator(MemoryAllocator* memoryAllocator); + + /// Reset the single frame allocator + void resetFrameAllocator(); }; -// Return the default memory allocator -inline DefaultAllocator& MemoryManager::getDefaultAllocator() { - return mDefaultAllocator; +// Allocate memory of a given type +inline void* MemoryManager::allocate(AllocationType allocationType, size_t size) { + + switch (allocationType) { + case AllocationType::Base: return mBaseAllocator->allocate(size); break; + case AllocationType::Pool: return mPoolAllocator.allocate(size); break; + case AllocationType::Frame: return mSingleFrameAllocator.allocate(size); break; + } +} + +// Release previously allocated memory. +inline void MemoryManager::release(AllocationType allocationType, void* pointer, size_t size) { + + switch (allocationType) { + case AllocationType::Base: mBaseAllocator->release(pointer, size); break; + case AllocationType::Pool: mPoolAllocator.release(pointer, size); break; + case AllocationType::Frame: mSingleFrameAllocator.release(pointer, size); break; + } +} + +// Return the pool allocator +inline PoolAllocator& MemoryManager::getPoolAllocator() { + return mPoolAllocator; +} + +// Return the single frame stack allocator +inline SingleFrameAllocator& MemoryManager::getSingleFrameAllocator() { + return mSingleFrameAllocator; +} + +// Return the base memory allocator +inline MemoryAllocator& MemoryManager::getBaseAllocator() { + return *mBaseAllocator; +} + +// Set the base memory allocator +inline void MemoryManager::setBaseAllocator(MemoryAllocator* baseAllocator) { + mBaseAllocator = baseAllocator; +} + +// Reset the single frame allocator +inline void MemoryManager::resetFrameAllocator() { + mSingleFrameAllocator.reset(); } } diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp index 53cee0a1..2a9507c4 100644 --- a/src/memory/PoolAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -25,6 +25,7 @@ // Libraries #include "PoolAllocator.h" +#include "MemoryManager.h" #include #include @@ -42,7 +43,7 @@ PoolAllocator::PoolAllocator() { mNbAllocatedMemoryBlocks = 64; mNbCurrentMemoryBlocks = 0; const size_t sizeToAllocate = mNbAllocatedMemoryBlocks * sizeof(MemoryBlock); - mMemoryBlocks = (MemoryBlock*) malloc(sizeToAllocate); + mMemoryBlocks = static_cast(MemoryManager::getBaseAllocator().allocate(sizeToAllocate)); memset(mMemoryBlocks, 0, sizeToAllocate); memset(mFreeMemoryUnits, 0, sizeof(mFreeMemoryUnits)); @@ -55,7 +56,7 @@ PoolAllocator::PoolAllocator() { // Initialize the array that contains the sizes the memory units that will // be allocated in each different heap - for (int i=0; i < NB_HEAPS; i++) { + for (uint i=0; i < NB_HEAPS; i++) { mUnitSizes[i] = (i+1) * 8; } @@ -82,16 +83,17 @@ PoolAllocator::~PoolAllocator() { // Release the memory allocated for each block for (uint i=0; i MAX_UNIT_SIZE) { - // Allocate memory using standard malloc() function - return malloc(size); + // Allocate memory using default allocation + return MemoryManager::getBaseAllocator().allocate(size); } // Get the index of the heap that will take care of the allocation request @@ -132,7 +134,7 @@ void* PoolAllocator::allocate(size_t size) { // Allocate more memory to contain the blocks MemoryBlock* currentMemoryBlocks = mMemoryBlocks; mNbAllocatedMemoryBlocks += 64; - mMemoryBlocks = (MemoryBlock*) malloc(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock)); + mMemoryBlocks = static_cast(MemoryManager::getBaseAllocator().allocate(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock))); memcpy(mMemoryBlocks, currentMemoryBlocks,mNbCurrentMemoryBlocks * sizeof(MemoryBlock)); memset(mMemoryBlocks + mNbCurrentMemoryBlocks, 0, 64 * sizeof(MemoryBlock)); free(currentMemoryBlocks); @@ -141,17 +143,22 @@ void* PoolAllocator::allocate(size_t size) { // Allocate a new memory blocks for the corresponding heap and divide it in many // memory units MemoryBlock* newBlock = mMemoryBlocks + mNbCurrentMemoryBlocks; - newBlock->memoryUnits = (MemoryUnit*) malloc(BLOCK_SIZE); + newBlock->memoryUnits = static_cast(MemoryManager::getBaseAllocator().allocate(BLOCK_SIZE)); assert(newBlock->memoryUnits != nullptr); size_t unitSize = mUnitSizes[indexHeap]; uint nbUnits = BLOCK_SIZE / unitSize; assert(nbUnits * unitSize <= BLOCK_SIZE); + void* memoryUnitsStart = static_cast(newBlock->memoryUnits); + char* memoryUnitsStartChar = static_cast(memoryUnitsStart); for (size_t i=0; i < nbUnits - 1; i++) { - MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i); - MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1)); + void* unitPointer = static_cast(memoryUnitsStartChar + unitSize * i); + void* nextUnitPointer = static_cast(memoryUnitsStartChar + unitSize * (i+1)); + MemoryUnit* unit = static_cast(unitPointer); + MemoryUnit* nextUnit = static_cast(nextUnitPointer); unit->nextUnit = nextUnit; } - MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1)); + void* lastUnitPointer = static_cast(memoryUnitsStartChar + unitSize*(nbUnits-1)); + MemoryUnit* lastUnit = static_cast(lastUnitPointer); lastUnit->nextUnit = nullptr; // Add the new allocated block into the list of free memory units in the heap @@ -176,8 +183,8 @@ void PoolAllocator::release(void* pointer, size_t size) { // If the size is larger than the maximum memory unit size if (size > MAX_UNIT_SIZE) { - // Release the memory using the standard free() function - free(pointer); + // Release the memory using the default deallocation + MemoryManager::getBaseAllocator().release(pointer, size); return; } @@ -187,7 +194,7 @@ void PoolAllocator::release(void* pointer, size_t size) { // Insert the released memory unit into the list of free memory units of the // corresponding heap - MemoryUnit* releasedUnit = (MemoryUnit*) pointer; + MemoryUnit* releasedUnit = static_cast(pointer); releasedUnit->nextUnit = mFreeMemoryUnits[indexHeap]; mFreeMemoryUnits[indexHeap] = releasedUnit; } diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index b06778d5..25f31198 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -28,7 +28,7 @@ // Libraries #include "configuration.h" -#include "Allocator.h" +#include "MemoryAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -40,7 +40,7 @@ namespace reactphysics3d { * efficiently. This implementation is inspired by the small block allocator * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp */ -class PoolAllocator : public Allocator { +class PoolAllocator : public MemoryAllocator { private : @@ -131,23 +131,17 @@ class PoolAllocator : public Allocator { /// Destructor virtual ~PoolAllocator() override; + /// Assignment operator + PoolAllocator& operator=(PoolAllocator& allocator) = default; + /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. virtual void* allocate(size_t size) override; /// Release previously allocated memory. virtual void release(void* pointer, size_t size) override; - - /// Return true if memory needs to be release with this allocator - virtual bool isReleaseNeeded() const override; - }; -// Return true if memory needs to be release with this allocator -inline bool PoolAllocator::isReleaseNeeded() const { - return true; -} - } #endif diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp index a2e436cd..c9d6533a 100644 --- a/src/memory/SingleFrameAllocator.cpp +++ b/src/memory/SingleFrameAllocator.cpp @@ -25,17 +25,19 @@ // Libraries #include "SingleFrameAllocator.h" +#include "MemoryManager.h" #include #include using namespace reactphysics3d; // Constructor -SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) - : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) { +SingleFrameAllocator::SingleFrameAllocator() + : mTotalSizeBytes(INIT_SINGLE_FRAME_ALLOCATOR_BYTES), + mCurrentOffset(0), mNbFramesTooMuchAllocated(0), mNeedToAllocatedMore(false) { // Allocate a whole block of memory at the beginning - mMemoryBufferStart = static_cast(malloc(mTotalSizeBytes)); + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); assert(mMemoryBufferStart != nullptr); } @@ -43,7 +45,7 @@ SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) SingleFrameAllocator::~SingleFrameAllocator() { // Release the memory allocated at the beginning - free(mMemoryBufferStart); + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); } @@ -52,14 +54,13 @@ SingleFrameAllocator::~SingleFrameAllocator() { void* SingleFrameAllocator::allocate(size_t size) { // Check that there is enough remaining memory in the buffer - if (static_cast(mCurrentOffset) + size > mTotalSizeBytes) { + if (mCurrentOffset + size > mTotalSizeBytes) { - // This should never occur. If it does, you must increase the initial - // size of memory of this allocator - assert(false); + // We need to allocate more memory next time reset() is called + mNeedToAllocatedMore = true; - // Return null - return nullptr; + // Return default memory allocation + return MemoryManager::getBaseAllocator().allocate(size); } // Next available memory location @@ -72,9 +73,63 @@ void* SingleFrameAllocator::allocate(size_t size) { return nextAvailableMemory; } +// Release previously allocated memory. +void SingleFrameAllocator::release(void* pointer, size_t size) { + + // If allocated memory is not within the single frame allocation range + char* p = static_cast(pointer); + if (p < mMemoryBufferStart || p > mMemoryBufferStart + mTotalSizeBytes) { + + // Use default deallocation + MemoryManager::getBaseAllocator().release(pointer, size); + } +} + // Reset the marker of the current allocated memory void SingleFrameAllocator::reset() { + // If too much memory is allocated + if (mCurrentOffset < mTotalSizeBytes / 2) { + + mNbFramesTooMuchAllocated++; + + if (mNbFramesTooMuchAllocated > NB_FRAMES_UNTIL_SHRINK) { + + // Release the memory allocated at the beginning + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); + + // Divide the total memory to allocate by two + mTotalSizeBytes /= 2; + if (mTotalSizeBytes <= 0) mTotalSizeBytes = 1; + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); + + mNbFramesTooMuchAllocated = 0; + } + } + else { + mNbFramesTooMuchAllocated = 0; + } + + // If we need to allocate more memory + if (mNeedToAllocatedMore) { + + // Release the memory allocated at the beginning + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); + + // Multiply the total memory to allocate by two + mTotalSizeBytes *= 2; + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); + + mNeedToAllocatedMore = false; + mNbFramesTooMuchAllocated = 0; + } + // Reset the current offset at the beginning of the block mCurrentOffset = 0; } diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h index 426c2a2d..d3142908 100644 --- a/src/memory/SingleFrameAllocator.h +++ b/src/memory/SingleFrameAllocator.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H // Libraries -#include "Allocator.h" +#include "MemoryAllocator.h" #include "configuration.h" /// ReactPhysics3D namespace @@ -38,10 +38,16 @@ namespace reactphysics3d { * This class represent a memory allocator used to efficiently allocate * memory on the heap that is used during a single frame. */ -class SingleFrameAllocator : public Allocator { +class SingleFrameAllocator : public MemoryAllocator { private : + // -------------------- Constants -------------------- // + + /// Number of frames to wait before shrinking the allocated + /// memory if too much is allocated + static const int NB_FRAMES_UNTIL_SHRINK = 120; + // -------------------- Attributes -------------------- // /// Total size (in bytes) of memory of the allocator @@ -51,36 +57,38 @@ class SingleFrameAllocator : public Allocator { char* mMemoryBufferStart; /// Pointer to the next available memory location in the buffer - int mCurrentOffset; + size_t mCurrentOffset; + + /// Current number of frames since we detected too much memory + /// is allocated + size_t mNbFramesTooMuchAllocated; + + /// True if we need to allocate more memory in the next reset() call + bool mNeedToAllocatedMore; public : // -------------------- Methods -------------------- // /// Constructor - SingleFrameAllocator(size_t totalSizeBytes); + SingleFrameAllocator(); /// Destructor virtual ~SingleFrameAllocator() override; + /// Assignment operator + SingleFrameAllocator& operator=(SingleFrameAllocator& allocator) = default; + /// Allocate memory of a given size (in bytes) virtual void* allocate(size_t size) override; /// Release previously allocated memory. - virtual void release(void* pointer, size_t size) override { } + virtual void release(void* pointer, size_t size) override; /// Reset the marker of the current allocated memory virtual void reset(); - - /// Return true if memory needs to be release with this allocator - virtual bool isReleaseNeeded() const override; }; -// Return true if memory needs to be release with this allocator -inline bool SingleFrameAllocator::isReleaseNeeded() const { - return false; -} - } #endif From ceb27760cb15830c006fe2d939e8628557f74896 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 14 Jan 2018 10:47:39 +0100 Subject: [PATCH 104/105] Working on containers --- CMakeLists.txt | 1 + src/containers/List.h | 133 +++++- src/containers/Map.h | 532 ++++++++++++++++++++++ src/mathematics/mathematics_functions.cpp | 24 + src/mathematics/mathematics_functions.h | 3 + test/main.cpp | 7 + test/tests/containers/TestList.h | 331 ++++++++++++++ test/tests/containers/TestMap.h | 319 +++++++++++++ test/tests/mathematics/TestTransform.h | 15 +- 9 files changed, 1353 insertions(+), 12 deletions(-) create mode 100644 src/containers/Map.h create mode 100644 test/tests/containers/TestList.h create mode 100644 test/tests/containers/TestMap.h diff --git a/CMakeLists.txt b/CMakeLists.txt index bc11aebd..169476c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -198,6 +198,7 @@ SET (REACTPHYSICS3D_SOURCES "src/containers/Stack.h" "src/containers/LinkedList.h" "src/containers/List.h" + "src/containers/Map.h" ) # Create the library diff --git a/src/containers/List.h b/src/containers/List.h index 176766a5..6391d52b 100644 --- a/src/containers/List.h +++ b/src/containers/List.h @@ -30,6 +30,7 @@ #include "configuration.h" #include "memory/MemoryAllocator.h" #include +#include namespace reactphysics3d { @@ -61,6 +62,102 @@ class List { public: + /// Class Iterator + /** + * This class represents an iterator for the List + */ + class Iterator { + + private: + + size_t mCurrentIndex; + T* mBuffer; + size_t mSize; + + public: + + // Iterator traits + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = T*; + using reference = T&; + using iterator_category = std::bidirectional_iterator_tag; + + /// Constructor + Iterator() = default; + + /// Constructor + Iterator(T* buffer, size_t index, size_t size) + :mCurrentIndex(index), mBuffer(buffer), mSize(size) { + + } + + /// Copy constructor + Iterator(const Iterator& it) + :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) { + + } + + /// Deferencable + reference operator*() const { + assert(mCurrentIndex >= 0 && mCurrentIndex < mSize); + return mBuffer[mCurrentIndex]; + } + + /// Deferencable + pointer operator->() const { + assert(mCurrentIndex >= 0 && mCurrentIndex < mSize); + return &(mBuffer[mCurrentIndex]); + } + + /// Post increment (it++) + Iterator& operator++() { + assert(mCurrentIndex < mSize - 1); + mCurrentIndex++; + return *this; + } + + /// Pre increment (++it) + Iterator operator++(int number) { + assert(mCurrentIndex < mSize - 1); + Iterator tmp = *this; + mCurrentIndex++; + return tmp; + } + + /// Post decrement (it--) + Iterator& operator--() { + assert(mCurrentIndex > 0); + mCurrentIndex--; + return *this; + } + + /// Pre decrement (--it) + Iterator operator--(int number) { + assert(mCurrentIndex > 0); + Iterator tmp = *this; + mCurrentIndex--; + return tmp; + } + + /// Equality operator (it == end()) + bool operator==(const Iterator& iterator) const { + assert(mCurrentIndex >= 0 && mCurrentIndex <= mSize); + + // If both iterators points to the end of the list + if (mCurrentIndex == mSize && iterator.mCurrentIndex == iterator.mSize) { + return true; + } + + return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]); + } + + /// Inequality operator (it != end()) + bool operator!=(const Iterator& iterator) const { + return !(*this == iterator); + } + }; + // -------------------- Methods -------------------- // /// Constructor @@ -151,7 +248,7 @@ class List { } } - /// Append another list to the current one + /// Append another list at the end of the current one void addRange(const List& list) { // If we need to allocate more memory @@ -180,7 +277,7 @@ class List { mSize = 0; } - /// Return the number of elments in the list + /// Return the number of elements in the list size_t size() const { return mSize; } @@ -202,14 +299,38 @@ class List { return (static_cast(mBuffer)[index]); } + /// Overloaded equality operator + bool operator==(const List& list) const { + + if (mSize != list.mSize) return false; + + T* items = static_cast(mBuffer); + for (int i=0; i < mSize; i++) { + if (items[i] != list[i]) { + return false; + } + } + + return true; + } + + /// Overloaded not equal operator + bool operator!=(const List& list) const { + + return !((*this) == list); + } + /// Overloaded assignment operator List& operator=(const List& list) { - // Clear all the elements - clear(); + if (this != &list) { - // Add all the elements of the list to the current one - addRange(list); + // Clear all the elements + clear(); + + // Add all the elements of the list to the current one + addRange(list); + } return *this; } diff --git a/src/containers/Map.h b/src/containers/Map.h new file mode 100644 index 00000000..5510b98d --- /dev/null +++ b/src/containers/Map.h @@ -0,0 +1,532 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MAP_H +#define REACTPHYSICS3D_MAP_H + +// Libraries +#include "memory/MemoryAllocator.h" +#include "mathematics/mathematics_functions.h" +#include +#include + +namespace reactphysics3d { + +// Class Map +/** + * This class represents a simple generic associative map + */ +template +class Map { + + private: + + /// An entry of the map + struct Entry { + + size_t hashCode; // Hash code of the entry + int next; // Index of the next entry + std::pair* keyValue; // Pointer to the pair with key and value + + /// Constructor + Entry() { + next = -1; + keyValue = nullptr; + } + + /// Destructor + ~Entry() { + + assert(keyValue == nullptr); + } + + }; + + // -------------------- Constants -------------------- // + + /// Number of prime numbers in array + static constexpr int NB_PRIMES = 70; + + /// Array of prime numbers for the size of the map + static const int PRIMES[NB_PRIMES]; + + /// Largest prime number + static int LARGEST_PRIME; + + // -------------------- Attributes -------------------- // + + /// Current number of used entries in the map + int mNbUsedEntries; + + /// Number of free entries among the used ones + int mNbFreeEntries; + + /// Current capacity of the map + int mCapacity; + + /// Array with all the buckets + int* mBuckets; + + /// Array with all the entries + Entry* mEntries; + + /// Memory allocator + MemoryAllocator& mAllocator; + + /// Index to the fist free entry + int mFreeIndex; + + // -------------------- Methods -------------------- // + + /// Initialize the map + void initialize(int capacity) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(capacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Initialize the buckets and entries + for (int i=0; i mCapacity); + assert(isPrimeNumber(newCapacity)); + + // Allocate memory for the buckets + int* newBuckets = static_cast(mAllocator.allocate(newCapacity * sizeof(int))); + + // Allocate memory for the entries + Entry* newEntries = static_cast(mAllocator.allocate(newCapacity * sizeof(Entry))); + + // Initialize the new buckets + for (int i=0; i(&newEntries[i])) Entry(); + } + + // For each used entry + for (int i=0; i 0) { + + size_t hashCode = std::hash()(key); + int bucket = hashCode % mCapacity; + + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) { + return i; + } + } + } + + return -1; + } + + /// Return the prime number that is larger or equal to the number in parameter + /// for the size of the map + static int getPrimeSize(int number) { + + // Check if the next larger prime number is in the precomputed array of primes + for (int i = 0; i < NB_PRIMES; i++) { + if (PRIMES[i] >= number) return PRIMES[i]; + } + + // Manually compute the next larger prime number + for (int i = (number | 1); i < std::numeric_limits::max(); i+=2) { + + if (isPrimeNumber(i)) { + return i; + } + } + + return number; + } + + /// Clear and reset the map + void reset() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Destroy the entries + for (int i=0; i < mCapacity; i++) { + mEntries[i].~Entry(); + } + + mAllocator.release(mBuckets, mCapacity * sizeof(int)); + mAllocator.release(mEntries, mCapacity * sizeof(Entry)); + + mNbUsedEntries = 0; + mNbFreeEntries = 0; + mCapacity = 0; + mBuckets = nullptr; + mEntries = nullptr; + mFreeIndex = -1; + } + } + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + Map(MemoryAllocator& allocator, size_t capacity = 0) + : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), + mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { + + // If the largest prime has not been computed yet + if (LARGEST_PRIME == -1) { + + // Compute the largest prime number (largest map capacity) + LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2); + } + + if (capacity > 0) { + + initialize(capacity); + } + } + + /// Copy constructor + Map(const Map& map) + :mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity), + mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) { + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + } + + /// Destructor + ~Map() { + + reset(); + } + + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) { + capacity = LARGEST_PRIME; + } + else { + capacity = getPrimeSize(capacity); + } + + expand(capacity); + } + + /// Return true if the map contains an item with the given key + bool containsKey(const K& key) const { + return findEntry(key) != -1; + } + + /// Add an element into the map + void add(const std::pair& keyValue) { + + if (mCapacity == 0) { + initialize(0); + } + + // Compute the hash code of the key + size_t hashCode = std::hash()(keyValue.first); + + // Compute the corresponding bucket index + int bucket = hashCode % mCapacity; + + // Check if the item is already in the map + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + + // If there is already an item with the same key in the map + if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) { + + throw std::runtime_error("The key and value pair already exists in the map"); + } + } + + size_t entryIndex; + + // If there are free entries to use + if (mNbFreeEntries > 0) { + assert(mFreeIndex >= 0); + entryIndex = mFreeIndex; + mFreeIndex = mEntries[entryIndex].next; + mNbFreeEntries--; + } + else { + + // If we need to allocator more entries + if (mNbUsedEntries == mCapacity) { + + // Allocate more memory + reserve(mCapacity * 2); + + // Recompute the bucket index + bucket = hashCode % mCapacity; + } + + entryIndex = mNbUsedEntries; + mNbUsedEntries++; + } + + assert(mEntries[entryIndex].keyValue == nullptr); + mEntries[entryIndex].hashCode = hashCode; + mEntries[entryIndex].next = mBuckets[bucket]; + mEntries[entryIndex].keyValue = static_cast*>(mAllocator.allocate(sizeof(std::pair))); + assert(mEntries[entryIndex].keyValue != nullptr); + new (mEntries[entryIndex].keyValue) std::pair(keyValue); + mBuckets[bucket] = entryIndex; + } + + /// Remove the element from the map with a given key + bool remove(const K& key) { + + if (mCapacity > 0) { + + size_t hashcode = std::hash()(key); + int bucket = hashcode % mCapacity; + int last = -1; + for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { + + if (mEntries[i].hashCode == hashcode && mEntries[i].keyValue->first == key) { + + if (last < 0 ) { + mBuckets[bucket] = mEntries[i].next; + } + else { + mEntries[last].next = mEntries[i].next; + } + + // Release memory for the key/value pair if any + if (mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue->~pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue = nullptr; + } + mEntries[i].hashCode = -1; + mEntries[i].next = mFreeIndex; + mFreeIndex = i; + mNbFreeEntries++; + + return true; + } + } + } + + return false; + } + + /// Clear the list + void clear() { + + if (mNbUsedEntries > 0) { + + for (int i=0; i < mCapacity; i++) { + mBuckets[i] = -1; + mEntries[i].next = -1; + if (mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue->~pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue = nullptr; + } + } + + mFreeIndex = -1; + mNbUsedEntries = 0; + mNbFreeEntries = 0; + } + + assert(size() == 0); + } + + /// Return the number of elements in the map + int size() const { + return mNbUsedEntries - mNbFreeEntries; + } + + /// Return the capacity of the map + int capacity() const { + return mCapacity; + } + + /// Overloaded index operator + V& operator[](const K& key) { + + int entry = -1; + + if (mCapacity > 0) { + entry = findEntry(key); + } + + if (entry == -1) { + throw std::runtime_error("No item with given key has been found in the map"); + } + + assert(mEntries[entry].keyValue != nullptr); + + return mEntries[entry].keyValue->second; + } + + /// Overloaded index operator + const V& operator[](const K& key) const { + + int entry = -1; + + if (mCapacity > 0) { + entry = findEntry(key); + } + + if (entry == -1) { + throw std::runtime_error("No item with given key has been found in the map"); + } + + return mEntries[entry]; + } + + /// Overloaded equality operator + bool operator==(const Map& map) const { + + // TODO : Implement this + return false; + } + + /// Overloaded not equal operator + bool operator!=(const Map& map) const { + + return !((*this) == map); + } + + /// Overloaded assignment operator + Map& operator=(const Map& map) { + + // Check for self assignment + if (this != &map) { + + // Reset the map + reset(); + + if (map.mCapacity > 0) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(map.mCapacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + + mNbUsedEntries = map.mNbUsedEntries; + mNbFreeEntries = map.mNbFreeEntries; + mFreeIndex = map.mFreeIndex; + } + } + + return *this; + } +}; + +template +const int Map::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, + 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, + 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, + 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, + 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; + +template +int Map::LARGEST_PRIME = -1; + +} + +#endif diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 9fa13083..7e4a1427 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -383,4 +383,28 @@ Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; } +// Return true if the given number is prime +bool reactphysics3d::isPrimeNumber(int number) { + + // If it's a odd number + if ((number & 1) != 0) { + + int limit = static_cast(std::sqrt(number)); + + for (int divisor = 3; divisor <= limit; divisor += 2) { + + // If we have found a divisor + if ((number % divisor) == 0) { + + // It is not a prime number + return false; + } + } + + return true; + } + + return number == 2; +} + diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index eda0fdd3..a72b0077 100755 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -124,6 +124,9 @@ List clipPolygonWithPlanes(const List& polygonVertices, const /// Project a point onto a plane that is given by a point and its unit length normal Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); +/// Return true if the given number is prime +bool isPrimeNumber(int number); + } diff --git a/test/main.cpp b/test/main.cpp index 26d62803..8bb62ea4 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -39,6 +39,8 @@ #include "tests/collision/TestDynamicAABBTree.h" #include "tests/collision/TestHalfEdgeStructure.h" #include "tests/collision/TestTriangleVertexArray.h" +#include "tests/containers/TestList.h" +#include "tests/containers/TestMap.h" using namespace reactphysics3d; @@ -46,6 +48,11 @@ int main() { TestSuite testSuite("ReactPhysics3D Tests"); + // ---------- Containers tests ---------- // + + testSuite.addTest(new TestList("List")); + testSuite.addTest(new TestMap("Map")); + // ---------- Mathematics tests ---------- // testSuite.addTest(new TestVector2("Vector2")); diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h new file mode 100644 index 00000000..ba1fbf61 --- /dev/null +++ b/test/tests/containers/TestList.h @@ -0,0 +1,331 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_LIST_H +#define TEST_LIST_H + +// Libraries +#include "Test.h" +#include "containers/List.h" +#include "memory/DefaultAllocator.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestList +/** + * Unit test for the List class + */ +class TestList : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestList(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testAddRemoveClear(); + testAssignment(); + testIndexing(); + testEquality(); + testReserve(); + testIteration(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + List list1(mAllocator); + test(list1.capacity() == 0); + test(list1.size() == 0); + + List list2(mAllocator, 100); + test(list2.capacity() == 100); + test(list2.size() == 0); + + List list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + test(list3.capacity() == 4); + test(list3.size() == 3); + + // ----- Copy Constructors ----- // + + List list4(list1); + test(list4.capacity() == 0); + test(list4.size() == 0); + + List list5(list3); + test(list5.capacity() == list3.size()); + test(list5.size() == list3.size()); + for (uint i=0; i list6(mAllocator, 20); + test(list6.capacity() == 20); + for (uint i=0; i<20; i++) { + list6.add("test"); + } + test(list6.capacity() == 20); + list6.add("test"); + test(list6.capacity() == 40); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + List list1(mAllocator); + list1.add(4); + test(list1.size() == 1); + test(list1[0] == 4); + list1.add(9); + test(list1.size() == 2); + test(list1[0] == 4); + test(list1[1] == 9); + + const int arraySize = 15; + int arrayTest[arraySize] = {3, 145, -182, 34, 12, 95, -1834, 4143, -111, -111, 4343, 234, 22983, -3432, 753}; + List list2(mAllocator); + for (uint i=0; i list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + list3.add(4); + + list3.remove(3); + test(list3.size() == 3); + test(list3.capacity() == 4); + test(list3[0] = 1); + test(list3[1] = 2); + test(list3[2] = 3); + + list3.remove(1); + test(list3.size() == 2); + test(list3.capacity() == 4); + test(list3[0] = 1); + test(list3[1] = 3); + + list3.remove(0); + test(list3.size() == 1); + test(list3.capacity() == 4); + test(list3[0] = 3); + + list3.remove(0); + test(list3.size() == 0); + test(list3.capacity() == 4); + + // ----- Test addRange() ----- // + + List list4(mAllocator); + list4.add(1); + list4.add(2); + list4.add(3); + + List list5(mAllocator); + list5.add(4); + list5.add(5); + + List list6(mAllocator); + list6.addRange(list5); + test(list6.size() == list5.size()); + test(list6[0] == 4); + test(list6[1] == 5); + + list4.addRange(list5); + test(list4.size() == 3 + list5.size()); + test(list4[0] == 1); + test(list4[1] == 2); + test(list4[2] == 3); + test(list4[3] == 4); + test(list4[4] == 5); + + // ----- Test clear() ----- // + + List list7(mAllocator); + list7.add("test1"); + list7.add("test2"); + list7.add("test3"); + list7.clear(); + test(list7.size() == 0); + list7.add("new"); + test(list7.size() == 1); + test(list7[0] == "new"); + } + + void testAssignment() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + List list2(mAllocator); + list2.add(5); + list2.add(6); + + List list3(mAllocator); + List list4(mAllocator); + list4.add(1); + list4.add(2); + + List list5(mAllocator); + list5.add(1); + list5.add(2); + list5.add(3); + + list3 = list2; + test(list2.size() == list3.size()); + test(list2[0] == list3[0]); + test(list2[1] == list3[1]); + + list4 = list1; + test(list4.size() == list1.size()) + test(list4[0] = list1[0]); + test(list4[1] = list1[1]); + test(list4[2] = list1[2]); + + list5 = list2; + test(list5.size() == list2.size()); + test(list5[0] = list2[0]); + test(list5[1] = list2[1]); + } + + void testIndexing() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + test(list1[0] == 1); + test(list1[1] == 2); + test(list1[2] == 3); + + list1[0] = 6; + list1[1] = 7; + list1[2] = 8; + + test(list1[0] == 6); + test(list1[1] == 7); + test(list1[2] == 8); + + const int a = list1[0]; + const int b = list1[1]; + test(a == 6); + test(b == 7); + + list1[0]++; + list1[1]++; + test(list1[0] == 7); + test(list1[1] == 8); + } + + void testEquality() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + List list2(mAllocator); + list2.add(1); + list2.add(2); + + List list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + + List list4(mAllocator); + list4.add(1); + list4.add(5); + list4.add(3); + + test(list1 == list1); + test(list1 != list2); + test(list1 == list3); + test(list1 != list4); + test(list2 != list4); + } + + void testReserve() { + + List list1(mAllocator); + list1.reserve(10); + test(list1.size() == 0); + test(list1.capacity() == 10); + list1.add(1); + list1.add(2); + test(list1.capacity() == 10); + test(list1.size() == 2); + test(list1[0] == 1); + test(list1[1] == 2); + + list1.reserve(1); + test(list1.capacity() == 10); + + list1.reserve(100); + test(list1.capacity() == 100); + test(list1[0] == 1); + test(list1[1] == 2); + } + + void testIteration() { + // TODO : Implement this + } + + }; + +} + +#endif diff --git a/test/tests/containers/TestMap.h b/test/tests/containers/TestMap.h new file mode 100644 index 00000000..d7e084dc --- /dev/null +++ b/test/tests/containers/TestMap.h @@ -0,0 +1,319 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_MAP_H +#define TEST_MAP_H + +// Libraries +#include "Test.h" +#include "containers/Map.h" +#include "memory/DefaultAllocator.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMap +/** + * Unit test for the Map class + */ +class TestMap : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMap(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testReserve(); + testAddRemoveClear(); + testContainsKey(); + testIndexing(); + testEquality(); + testAssignment(); + testIteration(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + Map map1(mAllocator); + test(map1.capacity() == 0); + test(map1.size() == 0); + + Map map2(mAllocator, 100); + test(map2.capacity() >= 100); + test(map2.size() == 0); + + // ----- Copy Constructors ----- // +/* + Map map3(map1); + test(map3.capacity() == map1.capacity()); + test(map3.size() == map1.size()); + + Map map4(mAllocator); + map4.add(std::make_pair(1, 10)); + map4.add(std::make_pair(2, 20)); + map4.add(std::make_pair(3, 30)); + test(map4.capacity() >= 3); + test(map4.size() == 3); + + Map map5(map4); + test(map5.capacity() == map4.capacity()); + test(map5.size() == map4.size()); + test(map5[1] == 10); + test(map5[2] == 20); + test(map5[3] == 30); + */ + } + + void testReserve() { + + Map map1(mAllocator); + map1.reserve(15); + test(map1.capacity() >= 15); + map1.add(std::make_pair(1, "test1")); + map1.add(std::make_pair(2, "test2")); + test(map1.capacity() >= 15); + + map1.reserve(10); + test(map1.capacity() >= 15); + + map1.reserve(100); + test(map1.capacity() >= 100); + test(map1[1] == "test1"); + test(map1[2] == "test2"); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + Map map1(mAllocator); + map1.add(std::make_pair(1, 10)); + map1.add(std::make_pair(8, 80)); + map1.add(std::make_pair(13, 130)); + test(map1[1] == 10); + test(map1[8] == 80); + test(map1[13] == 130); + test(map1.size() == 3); + + Map map2(mAllocator, 15); + for (int i = 0; i < 1000000; i++) { + map2.add(std::make_pair(i, i * 100)); + } + bool isValid = true; + for (int i = 0; i < 1000000; i++) { + if (map2[i] != i * 100) isValid = false; + } + test(isValid); + + map1.remove(1); + map1.add(std::make_pair(1, 10)); + test(map1.size() == 3); + test(map1[1] == 10); + + // ----- Test remove() ----- // + + map1.remove(1); + test(!map1.containsKey(1)); + test(map1.containsKey(8)); + test(map1.containsKey(13)); + test(map1.size() == 2); + + map1.remove(13); + test(!map1.containsKey(8)); + test(map1.containsKey(13)); + test(map1.size() == 1); + + map1.remove(8); + test(!map1.containsKey(8)); + test(map1.size() == 0); + + isValid = true; + for (int i = 0; i < 1000000; i++) { + map2.remove(i); + } + for (int i = 0; i < 1000000; i++) { + if (map2.containsKey(i)) isValid = false; + } + test(isValid); + test(map2.size() == 0); + + Map map3(mAllocator); + for (int i=0; i < 1000000; i++) { + map3.add(std::make_pair(i, i * 10)); + map3.remove(i); + } + + // ----- Test clear() ----- // + + Map map4(mAllocator); + map4.add(std::make_pair(2, 20)); + map4.add(std::make_pair(4, 40)); + map4.add(std::make_pair(6, 60)); + map4.clear(); + test(map4.size() == 0); + map4.add(std::make_pair(2, 20)); + test(map4.size() == 1); + test(map4[2] == 20); + map4.clear(); + test(map4.size() == 0); + + Map map5(mAllocator); + map5.clear(); + test(map5.size() == 0); + } + + void testContainsKey() { + + Map map1(mAllocator); + + test(!map1.containsKey(2)); + test(!map1.containsKey(4)); + test(!map1.containsKey(6)); + + map1.add(std::make_pair(2, 20)); + map1.add(std::make_pair(4, 40)); + map1.add(std::make_pair(6, 60)); + + test(map1.containsKey(2)); + test(map1.containsKey(4)); + test(map1.containsKey(6)); + + map1.remove(4); + test(!map1.containsKey(4)); + test(map1.containsKey(2)); + test(map1.containsKey(6)); + + map1.clear(); + test(!map1.containsKey(2)); + test(!map1.containsKey(6)); + } + + void testIndexing() { + + Map map1(mAllocator); + map1.add(std::make_pair(2, 20)); + map1.add(std::make_pair(4, 40)); + map1.add(std::make_pair(6, 60)); + test(map1[2] == 20); + test(map1[4] == 40); + test(map1[6] == 60); + + map1[2] = 10; + map1[4] = 20; + map1[6] = 30; + + test(map1[2] == 10); + test(map1[4] == 20); + test(map1[6] == 30); + } + + void testEquality() { + + Map map1(mAllocator, 10); + Map map2(mAllocator, 2); + + test(map1 == map2); + + map1.add(std::make_pair("a", 1)); + map1.add(std::make_pair("b", 2)); + map1.add(std::make_pair("c", 3)); + + map2.add(std::make_pair("a", 1)); + map2.add(std::make_pair("b", 2)); + map2.add(std::make_pair("c", 4)); + + test(map1 == map1); + test(map2 == map2); + test(map1 != map2); + + map2["c"] = 3; + + test(map1 == map2); + + Map map3(mAllocator); + map3.add(std::make_pair("a", 1)); + + test(map1 != map3); + test(map2 != map3); + } + + void testAssignment() { + + Map map1(mAllocator); + map1.add(std::make_pair(1, 3)); + map1.add(std::make_pair(2, 6)); + map1.add(std::make_pair(10, 30)); +/* + Map map2(mAllocator); + map2 = map1; + test(map2.size() == map1.size()); + test(map2[1] == 3); + test(map2[2] == 6); + test(map2[10] == 30); + + Map map3(mAllocator, 100); + map3 = map1; + test(map3.size() == map1.size()); + test(map3[1] == 3); + test(map3[2] == 6); + test(map3[10] == 30); + + Map map4(mAllocator); + map3 = map4; + test(map3.size() == 0); +*/ + Map map5(mAllocator); + map5.add(std::make_pair(7, 8)); + map5.add(std::make_pair(19, 70)); + map1 = map5; + test(map5.size() == map1.size()); + test(map1[7] == 8); + test(map1[19] == 70); + } + + void testIteration() { + + } + }; + +} + +#endif diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index 581b9c5e..815e3d54 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -61,13 +61,16 @@ class TestTransform : public Test { mIdentityTransform.setToIdentity(); - decimal sinA = sin(PI/8.0f); - decimal cosA = cos(PI/8.0f); - mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA)); + Vector3 unitVec(1, 1, 1); + unitVec.normalize(); - decimal sinB = sin(PI/3.0f); - decimal cosB = cos(PI/3.0f); - mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB)); + decimal sinA = std::sin(PI/8.0f); + decimal cosA = std::cos(PI/8.0f); + mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA * unitVec, cosA)); + + decimal sinB = std::sin(PI/3.0f); + decimal cosB = std::cos(PI/3.0f); + mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB * unitVec, cosB)); } /// Run the tests From 584b28a91c67745030df1482a3ab573063f2f6ad Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 14 Jan 2018 10:51:38 +0100 Subject: [PATCH 105/105] Add Cube stack scene in the testbed application --- testbed/CMakeLists.txt | 2 + testbed/common/AABB.h | 3 - .../CollisionDetectionScene.cpp | 4 +- testbed/scenes/concavemesh/ConcaveMeshScene.h | 8 +- testbed/scenes/cubestack/CubeStackScene.cpp | 146 ++++++++++++++++++ testbed/scenes/cubestack/CubeStackScene.h | 82 ++++++++++ testbed/src/TestbedApplication.cpp | 6 + 7 files changed, 242 insertions(+), 9 deletions(-) create mode 100644 testbed/scenes/cubestack/CubeStackScene.cpp create mode 100644 testbed/scenes/cubestack/CubeStackScene.h diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index e5e3b312..818cb026 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -114,6 +114,8 @@ SET(SCENES_SOURCES scenes/concavemesh/ConcaveMeshScene.cpp scenes/heightfield/HeightFieldScene.h scenes/heightfield/HeightFieldScene.cpp + scenes/cubestack/CubeStackScene.h + scenes/cubestack/CubeStackScene.cpp ) # Add .user file to set debug path in Visual Studio diff --git a/testbed/common/AABB.h b/testbed/common/AABB.h index e73326b5..2e8f82e1 100644 --- a/testbed/common/AABB.h +++ b/testbed/common/AABB.h @@ -37,9 +37,6 @@ class AABB { // -------------------- Attributes -------------------- // - /// Size of each side of the box - float mSize[3]; - /// Scaling matrix (applied to a cube to obtain the correct box dimensions) openglframework::Matrix4 mScalingMatrix; diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 6f2c65ae..02e8acc1 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -168,8 +168,8 @@ void CollisionDetectionScene::reset() { mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity())); mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 9, 0), rp3d::Quaternion::identity())); mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity())); - mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity())); - mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity())); + mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 15, 0), rp3d::Quaternion::identity())); + mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -22, 0), rp3d::Quaternion::identity())); } // Destructor diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h index a8d566a1..beca8f71 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.h +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h @@ -42,10 +42,10 @@ namespace trianglemeshscene { // Constants const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters -static const int NB_BOXES = 10; -static const int NB_SPHERES = 5; -static const int NB_CAPSULES = 5; -static const int NB_MESHES = 3; +static const int NB_BOXES = 50; +static const int NB_SPHERES = 40; +static const int NB_CAPSULES = 20; +static const int NB_MESHES = 15; static const int NB_COMPOUND_SHAPES = 3; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; diff --git a/testbed/scenes/cubestack/CubeStackScene.cpp b/testbed/scenes/cubestack/CubeStackScene.cpp new file mode 100644 index 00000000..e4a0f859 --- /dev/null +++ b/testbed/scenes/cubestack/CubeStackScene.cpp @@ -0,0 +1,146 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CubeStackScene.h" + +// Namespaces +using namespace openglframework; +using namespace cubestackscene; + +// Constructor +CubeStackScene::CubeStackScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + // Create the dynamics world for the physics simulation + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + + // Create all the cubes of the scene + for (int i=1; i<=NB_FLOORS; i++) { + + for (int j=0; jsetColor(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); + } + } + + // ------------------------- FLOOR ----------------------- // + + // Create the floor + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); + + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mFloor); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); +} + +// Destructor +CubeStackScene::~CubeStackScene() { + + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Destroy the corresponding rigid body from the dynamics world + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the cube + delete (*it); + } + + // Destroy the rigid body of the floor + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); + + // Destroy the floor + delete mFloor; + + // Destroy the dynamics world + delete getDynamicsWorld(); +} + +// Reset the scene +void CubeStackScene::reset() { + + int index = 0; + for (int i=NB_FLOORS; i > 0; i--) { + + for (int j=0; jsetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + + index++; + } + } + + mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); +} diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h new file mode 100644 index 00000000..9bbc1c01 --- /dev/null +++ b/testbed/scenes/cubestack/CubeStackScene.h @@ -0,0 +1,82 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CUBESTACK_SCENE_H +#define CUBESTACK_SCENE_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" +#include "Box.h" +#include "SceneDemo.h" + +namespace cubestackscene { + +// Constants +const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters +const int NB_FLOORS = 15; // Number of boxes in the scene +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms + +// Class CubeStackScene +class CubeStackScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// All the boxes of the scene + std::vector mBoxes; + + /// Box for the floor + Box* mFloor; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CubeStackScene(const std::string& name, EngineSettings& settings); + + /// Destructor + virtual ~CubeStackScene() override; + + /// Reset the scene + virtual void reset() override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Return all the contact points of the scene +inline std::vector CubeStackScene::getContactPoints() const { + return computeContactPointsOfWorld(getDynamicsWorld()); +} + +} + +#endif diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index f2a43bc1..63230e0c 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -36,6 +36,7 @@ #include "heightfield/HeightFieldScene.h" #include "raycast/RaycastScene.h" #include "concavemesh/ConcaveMeshScene.h" +#include "cubestack/CubeStackScene.h" using namespace openglframework; using namespace jointsscene; @@ -45,6 +46,7 @@ using namespace collisionshapesscene; using namespace trianglemeshscene; using namespace heightfieldscene; using namespace collisiondetectionscene; +using namespace cubestackscene; // Initialization of static variables const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; @@ -102,6 +104,10 @@ void TestbedApplication::createScenes() { 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);