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"