Add clippling segment/polygons methods, fix issues and add convex vs capsule algorithm
This commit is contained in:
Daniel Chappuis
parent
57da79492f
commit
f61fea8b8a
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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<const uint>(shape1->getCollisionShape()->getType());
|
||||
const uint shape2TypeIndex = static_cast<const uint>(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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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<unsigned int>(shape1Type);
|
||||
const unsigned int shape2Index = static_cast<unsigned int>(shape2Type);
|
||||
uint shape1Index = static_cast<unsigned int>(shape1Type);
|
||||
uint shape2Index = static_cast<unsigned int>(shape2Type);
|
||||
|
||||
// Swap the shape types if necessary
|
||||
if (shape1Index > shape2Index) {
|
||||
const uint tempIndex = shape1Index;
|
||||
shape1Index = shape2Index;
|
||||
shape2Index = tempIndex;
|
||||
}
|
||||
|
||||
assert(shape1Index <= shape2Index);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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<const SphereShape*>(narrowPhaseInfo->collisionShape1);
|
||||
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
|
||||
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
|
||||
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<const SphereShape*>(narrowPhaseInfo->collisionShape1);
|
||||
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
|
||||
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
|
||||
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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<uint>(shape1->getCollisionShape()->getType()) <=
|
||||
static_cast<uint>(shape2->getCollisionShape()->getType()));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include "mathematics_functions.h"
|
||||
#include "Vector3.h"
|
||||
#include <cassert>
|
||||
#include <vector>
|
||||
|
||||
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<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
|
||||
const std::vector<Vector3>& planesPoints,
|
||||
const std::vector<Vector3>& planesNormals) {
|
||||
|
||||
assert(planesPoints.size() == planesNormals.size());
|
||||
|
||||
std::vector<Vector3> inputVertices = {segA, segB};
|
||||
std::vector<Vector3> outputVertices;
|
||||
|
||||
// For each clipping plane
|
||||
for (uint p=0; p<planesPoints.size(); p++) {
|
||||
|
||||
// If there is no more vertices, stop
|
||||
if (inputVertices.empty()) return inputVertices;
|
||||
|
||||
assert(inputVertices.size() == 2);
|
||||
|
||||
outputVertices.clear();
|
||||
|
||||
Vector3& v1 = inputVertices[0];
|
||||
Vector3& v2 = inputVertices[1];
|
||||
|
||||
decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
|
||||
decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
|
||||
|
||||
// If the second vertex is in front of the clippling plane
|
||||
if (v2DotN >= 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<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
|
||||
const std::vector<Vector3>& planesNormals) {
|
||||
|
||||
assert(planesPoints.size() == planesNormals.size());
|
||||
|
||||
std::vector<Vector3> inputVertices(polygonVertices);
|
||||
std::vector<Vector3> outputVertices;
|
||||
|
||||
// For each clipping plane
|
||||
for (uint p=0; p<planesPoints.size(); p++) {
|
||||
|
||||
outputVertices.clear();
|
||||
|
||||
uint vStart = inputVertices.size() - 1;
|
||||
|
||||
// For each edge of the polygon
|
||||
for (uint vEnd = 0; vEnd<inputVertices.size(); vEnd++) {
|
||||
|
||||
Vector3& v1 = inputVertices[vStart];
|
||||
Vector3& v2 = inputVertices[vEnd];
|
||||
|
||||
decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
|
||||
decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
|
||||
|
||||
// If the second vertex is in front of the clippling plane
|
||||
if (v2DotN >= 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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
/// 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<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
|
||||
const std::vector<Vector3>& planesPoints,
|
||||
const std::vector<Vector3>& planesNormals);
|
||||
|
||||
/// Clip a polygon against multiple planes and return the clipped polygon vertices
|
||||
std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
|
||||
const std::vector<Vector3>& planesNormals);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<Vector3> segmentVertices;
|
||||
segmentVertices.push_back(Vector3(-6, 3, 0));
|
||||
segmentVertices.push_back(Vector3(8, 3, 0));
|
||||
|
||||
std::vector<Vector3> planesNormals;
|
||||
std::vector<Vector3> planesPoints;
|
||||
planesNormals.push_back(Vector3(-1, 0, 0));
|
||||
planesPoints.push_back(Vector3(4, 0, 0));
|
||||
|
||||
std::vector<Vector3> 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<Vector3> 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));
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user