From f61fea8b8ab3d592c42a3074200307ebd7014d36 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Apr 2017 22:09:59 +0200 Subject: [PATCH] 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)); + } };