Modifications for smooth concave mesh contacts
This commit is contained in:
Daniel Chappuis
parent
4362033018
commit
8be408ccec
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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(),
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@
|
|||
#include "ConcaveVsConvexAlgorithm.h"
|
||||
#include "collision/CollisionDetection.h"
|
||||
#include "engine/CollisionWorld.h"
|
||||
#include <algorithm>
|
||||
|
||||
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<SmoothMeshContactInfo> 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<SmoothMeshContactInfo> contactPoints,
|
||||
NarrowPhaseCallback* narrowPhaseCallback) {
|
||||
|
||||
// Set with the triangle vertices already processed to void further contacts with same triangle
|
||||
std::unordered_multimap<int, Vector3> 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<SmoothMeshContactInfo>::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<int, Vector3>& 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<const TriangleShape*>(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<const TriangleShape*>(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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include "NarrowPhaseAlgorithm.h"
|
||||
#include "collision/shapes/ConvexShape.h"
|
||||
#include "collision/shapes/ConcaveShape.h"
|
||||
#include <unordered_map>
|
||||
|
||||
/// 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<SmoothMeshContactInfo>& mContactPoints;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
// Constructor
|
||||
SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& 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<SmoothMeshContactInfo> contactPoints,
|
||||
NarrowPhaseCallback* narrowPhaseCallback);
|
||||
|
||||
/// Add a triangle vertex into the set of processed triangles
|
||||
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
|
||||
const Vector3& vertex);
|
||||
|
||||
/// Return true if the vertex is in the set of already processed vertices
|
||||
bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& 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<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
|
||||
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include "Simplex.h"
|
||||
#include "constraint/ContactPoint.h"
|
||||
#include "configuration.h"
|
||||
#include "engine/Profiler.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
ConcaveShape::ConcaveShape(CollisionShapeType type) : CollisionShape(type) {
|
||||
|
||||
mIsSmoothMeshCollisionEnabled = false;
|
||||
}
|
||||
|
||||
// Destructor
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
51
src/mathematics/mathematics_functions.cpp
Normal file
51
src/mathematics/mathematics_functions.cpp
Normal file
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -30,6 +30,8 @@
|
|||
#include "configuration.h"
|
||||
#include "decimal.h"
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
/// 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user