From 8be408ccec4235f3900406c5b9706676c9279dca Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 5 Jan 2016 18:39:22 +0100 Subject: [PATCH] Modifications for smooth concave mesh contacts --- CMakeLists.txt | 9 +- src/collision/CollisionDetection.cpp | 2 + .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 185 +++++++++++++++++- .../narrowphase/ConcaveVsConvexAlgorithm.h | 89 +++++++++ .../narrowphase/EPA/EPAAlgorithm.cpp | 9 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 21 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 4 +- src/collision/shapes/ConcaveShape.cpp | 2 +- src/collision/shapes/ConcaveShape.h | 22 ++- src/collision/shapes/TriangleShape.h | 12 ++ src/constraint/ContactPoint.h | 34 ++-- src/mathematics/mathematics_functions.cpp | 51 +++++ src/mathematics/mathematics_functions.h | 8 +- 13 files changed, 411 insertions(+), 37 deletions(-) create mode 100644 src/mathematics/mathematics_functions.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 272c1186..9a3f24f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # Minimum cmake version required -CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0 FATAL_ERROR) # Project configuration PROJECT(REACTPHYSICS3D) @@ -147,6 +147,7 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/Timer.cpp" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" + "src/mathematics/mathematics_functions.cpp" "src/mathematics/Matrix2x2.h" "src/mathematics/Matrix2x2.cpp" "src/mathematics/Matrix3x3.h" @@ -166,7 +167,11 @@ SET (REACTPHYSICS3D_SOURCES ) # Create the library -ADD_LIBRARY (reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES}) +ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES}) + +# Enable C++11 features +set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD 11) +set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD_REQUIRED ON) # If we need to compile the testbed application IF(COMPILE_TESTBED) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index d23a90d7..5044224c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -132,6 +132,8 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac // Create the contact info object for the contact ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), + manifold->getShape1()->getCollisionShape(), + manifold->getShape2()->getCollisionShape(), contactPoint->getNormal(), contactPoint->getPenetrationDepth(), contactPoint->getLocalPointOnBody1(), diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index de78d8f1..21c68478 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -29,6 +29,7 @@ #include "ConcaveVsConvexAlgorithm.h" #include "collision/CollisionDetection.h" #include "engine/CollisionWorld.h" +#include using namespace reactphysics3d; @@ -67,7 +68,6 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf } // Set the parameters of the callback object - mConvexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); mConvexVsTriangleCallback.setCollisionDetection(mCollisionDetection); mConvexVsTriangleCallback.setConvexShape(convexShape); mConvexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); @@ -77,8 +77,28 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf AABB aabb; convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(mConvexVsTriangleCallback, aabb); + // If smooth mesh collision is enabled for the concave mesh + if (concaveShape->getIsSmoothMeshCollisionEnabled()) { + + std::vector contactPoints; + + SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); + + mConvexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); + + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(mConvexVsTriangleCallback, aabb); + + // Run the smooth mesh collision algorithm + processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); + } + else { + + mConvexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); + + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(mConvexVsTriangleCallback, aabb); + } } // Test collision between a triangle and the convex mesh shape @@ -107,3 +127,162 @@ void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { // 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() == TRIANGLE) { + assert(contactInfo.collisionShape2->getType() != 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() == 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 index 68dceb2f..b395cb5a 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h @@ -30,6 +30,7 @@ #include "NarrowPhaseAlgorithm.h" #include "collision/shapes/ConvexShape.h" #include "collision/shapes/ConcaveShape.h" +#include /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -62,6 +63,10 @@ class ConvexVsTriangleCallback : public TriangleCallback { /// Broadphase overlapping pair OverlappingPair* mOverlappingPair; + /// Used to sort ContactPointInfos according to their penetration depth + static bool contactsDepthCompare(const ContactPointInfo& contact1, + const ContactPointInfo& contact2); + public: /// Set the collision detection pointer @@ -95,6 +100,72 @@ class ConvexVsTriangleCallback : public TriangleCallback { }; +// Class SmoothMeshContactInfo +/** + * This class is used to store data about a contact with a triangle for the smooth + * mesh algorithm. + */ +class SmoothMeshContactInfo { + + public: + + ContactPointInfo contactInfo; + bool isFirstShapeTriangle; + Vector3 triangleVertices[3]; + + /// Constructor + SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, + const Vector3& trianglePoint2, const Vector3& trianglePoint3) + : contactInfo(contact) { + isFirstShapeTriangle = firstShapeTriangle; + triangleVertices[0] = trianglePoint1; + triangleVertices[1] = trianglePoint2; + triangleVertices[2] = trianglePoint3; + } + +}; + +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; +//} + +// 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 : public NarrowPhaseCallback { + + private: + + std::vector& mContactPoints; + + + public: + + // Constructor + SmoothCollisionNarrowPhaseCallback(std::vector& contactPoints) + : mContactPoints(contactPoints) { + + } + + + /// Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* overlappingPair, + const ContactPointInfo& contactInfo); + +}; + // Class ConcaveVsConvexAlgorithm /** * This class is used to compute the narrow-phase collision detection @@ -119,6 +190,19 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { /// Private assignment operator ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm); + /// 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 -------------------- // @@ -135,6 +219,11 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { 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 diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 7b56949d..4442265e 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -25,6 +25,7 @@ // Libraries #include "EPAAlgorithm.h" +#include "engine/Profiler.h" #include "collision/narrowphase//GJK/GJKAlgorithm.h" #include "TrianglesStore.h" @@ -89,6 +90,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple Vector3& v, NarrowPhaseCallback* narrowPhaseCallback) { + PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); + assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); @@ -425,10 +428,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple Vector3 normal = v.getUnit(); decimal penetrationDepth = v.length(); assert(penetrationDepth > 0.0); + + if (normal.lengthSquare() < MACHINE_EPSILON) return; // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - normal, penetrationDepth, pALocal, pBLocal); + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index ddfedf36..a2ec298c 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -28,6 +28,7 @@ #include "Simplex.h" #include "constraint/ContactPoint.h" #include "configuration.h" +#include "engine/Profiler.h" #include #include #include @@ -58,6 +59,8 @@ GJKAlgorithm::~GJKAlgorithm() { void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, const CollisionShapeInfo& shape2Info, NarrowPhaseCallback* narrowPhaseCallback) { + + PROFILE("GJKAlgorithm::testCollision()"); Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B @@ -146,8 +149,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, if (penetrationDepth <= 0.0) return; // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - normal, penetrationDepth, pA, pB); + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); @@ -179,8 +182,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, if (penetrationDepth <= 0.0) return; // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - normal, penetrationDepth, pA, pB); + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); @@ -210,8 +213,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, if (penetrationDepth <= 0.0) return; // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - normal, penetrationDepth, pA, pB); + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); @@ -248,8 +251,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, if (penetrationDepth <= 0.0) return; // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - normal, penetrationDepth, pA, pB); + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); @@ -278,6 +281,8 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap const Transform& transform2, NarrowPhaseCallback* narrowPhaseCallback, Vector3& v) { + PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); + Simplex simplex; Vector3 suppA; Vector3 suppB; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index d469fc79..958c60ee 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -70,8 +70,8 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, - vectorBetweenCenters.getUnit(), penetrationDepth, + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth, intersectionOnBody1, intersectionOnBody2); // Notify about the new contact diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp index 99dd2215..81643482 100644 --- a/src/collision/shapes/ConcaveShape.cpp +++ b/src/collision/shapes/ConcaveShape.cpp @@ -32,7 +32,7 @@ using namespace reactphysics3d; // Constructor ConcaveShape::ConcaveShape(CollisionShapeType type) : CollisionShape(type) { - + mIsSmoothMeshCollisionEnabled = false; } // Destructor diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index e15ee22e..1a654180 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -58,7 +58,8 @@ class ConcaveShape : public CollisionShape { // -------------------- Attributes -------------------- // - + /// True if the smooth mesh collision algorithm is enabled + bool mIsSmoothMeshCollisionEnabled; // -------------------- Methods -------------------- // @@ -86,6 +87,12 @@ 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 true if the collision shape is convex, false if it is concave @@ -98,6 +105,19 @@ 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; +} + } #endif diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index a6cb7c7a..25532fd6 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -117,6 +117,9 @@ 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; + // ---------- Friendship ---------- // friend class ConcaveMeshRaycastCallback; @@ -213,6 +216,15 @@ 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]; +} + } #endif diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index c4240143..afa8e37b 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -49,12 +49,6 @@ struct ContactPointInfo { // -------------------- Methods -------------------- // - /// Private copy-constructor - ContactPointInfo(const ContactPointInfo& contactInfo); - - /// Private assignment operator - ContactPointInfo& operator=(const ContactPointInfo& contactInfo); - public: // -------------------- Attributes -------------------- // @@ -65,26 +59,32 @@ struct ContactPointInfo { /// Second proxy shape of the contact ProxyShape* shape2; - /// Normalized normal vector the the collision contact in world space - const Vector3 normal; + /// First collision shape + const CollisionShape* collisionShape1; + + /// Second collision shape + const CollisionShape* collisionShape2; + + /// Normalized normal vector of the collision contact in world space + Vector3 normal; /// Penetration depth of the contact - const decimal penetrationDepth; + decimal penetrationDepth; /// Contact point of body 1 in local space of body 1 - const Vector3 localPoint1; + Vector3 localPoint1; /// Contact point of body 2 in local space of body 2 - const Vector3 localPoint2; + Vector3 localPoint2; // -------------------- Methods -------------------- // /// Constructor - ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, - const Vector3& normal, decimal penetrationDepth, const Vector3& localPoint1, - const Vector3& localPoint2) - : shape1(proxyShape1), shape2(proxyShape2), normal(normal), - penetrationDepth(penetrationDepth), localPoint1(localPoint1), + ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1, + const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth, + const Vector3& localPoint1, const Vector3& localPoint2) + : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2), + normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1), localPoint2(localPoint2) { } @@ -107,7 +107,7 @@ class ContactPoint { /// Second rigid body of the contact CollisionBody* mBody2; - /// Normalized normal vector of the contact (From body1 toward body2) in world space + /// Normalized normal vector of the contact (from body1 toward body2) in world space const Vector3 mNormal; /// Penetration depth diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp new file mode 100644 index 00000000..95bc3d65 --- /dev/null +++ b/src/mathematics/mathematics_functions.cpp @@ -0,0 +1,51 @@ +/******************************************************************************** +* 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 "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) - u - w; +} diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 0ac2bc81..64dc2a61 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -30,6 +30,8 @@ #include "configuration.h" #include "decimal.h" #include +#include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -41,7 +43,7 @@ struct Vector3; /// 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 (fabs(a - b) < epsilon); + return (std::fabs(a - b) < epsilon); } /// Function that returns the result of the "value" clamped by @@ -66,6 +68,10 @@ inline bool sameSign(decimal a, decimal b) { return a * b >= decimal(0.0); } +/// 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