atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] refacto

Browse Source
This commit is contained in:
Edouard DUPIN 2018-05-14 21:49:37 +02:00
parent b85098fcaa
commit b1fe6eebb4
16 changed files with 387 additions and 559 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -71,26 +71,20 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t _broadPhaseID) {
*/ */
} }
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* _proxyShape, const AABB& _aabb) {
// Add the collision shape int32_to the dynamic AABB tree and get its broad-phase ID // Add the collision shape int32_to the dynamic AABB tree and get its broad-phase ID
int32_t nodeId = m_dynamicAABBTree.addObject(aabb, proxyShape); int32_t nodeId = m_dynamicAABBTree.addObject(_aabb, _proxyShape);
// Set the broad-phase ID of the proxy shape // Set the broad-phase ID of the proxy shape
proxyShape->m_broadPhaseID = nodeId; _proxyShape->m_broadPhaseID = nodeId;
// Add the collision shape int32_to the array of bodies that have moved (or have been created) // Add the collision shape int32_to the array of bodies that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(proxyShape->m_broadPhaseID); addMovedCollisionShape(_proxyShape->m_broadPhaseID);
} }
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* _proxyShape) {
int32_t broadPhaseID = _proxyShape->m_broadPhaseID;
int32_t broadPhaseID = proxyShape->m_broadPhaseID;
// Remove the collision shape from the dynamic AABB tree // Remove the collision shape from the dynamic AABB tree
m_dynamicAABBTree.removeObject(broadPhaseID); m_dynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape int32_to the array of shapes that have moved (or have been created) // Remove the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step // during the last simulation step
removeMovedCollisionShape(broadPhaseID); removeMovedCollisionShape(broadPhaseID);
@ -127,18 +121,17 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Ask the dynamic AABB tree to report all collision shapes that overlap with // Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair. // by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t nodeId) mutable { m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t _nodeId) mutable {
// If both the nodes are the same, we do not create store the overlapping pair // If both the nodes are the same, we do not create store the overlapping pair
if (it == nodeId) { if (it == _nodeId) {
return; return;
} }
// Add the new potential pair int32_to the array of potential overlapping pairs // Add the new potential pair int32_to the array of potential overlapping pairs
m_potentialPairs.pushBack(etk::makePair(etk::min(it, nodeId), etk::max(it, nodeId) )); m_potentialPairs.pushBack(etk::makePair(etk::min(it, _nodeId), etk::max(it, _nodeId) ));
}); });
} }
// Reset the array of collision shapes that have move (or have been created) during the last simulation step // Reset the array of collision shapes that have move (or have been created) during the last simulation step
m_movedShapes.clear(); m_movedShapes.clear();
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
m_potentialPairs.sort(0, m_potentialPairs.sort(0,
m_potentialPairs.size()-1, m_potentialPairs.size()-1,
@ -151,7 +144,6 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
} }
return false; return false;
}); });
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
uint32_t iii=0; uint32_t iii=0;
@ -178,22 +170,17 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
} }
} }
float BroadPhaseRaycastCallback::operator()(int32_t nodeId, const Ray& ray) { float BroadPhaseRaycastCallback::operator()(int32_t _nodeId, const Ray& _ray) {
float hitFraction = float(-1.0); float hitFraction = float(-1.0);
// Get the proxy shape from the node // Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(nodeId)); ProxyShape* proxyShape = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(_nodeId));
// Check if the raycast filtering mask allows raycast against this shape // Check if the raycast filtering mask allows raycast against this shape
if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against // Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping // the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase // with the shape in the broad-phase
hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray); hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, _ray);
} }
return hitFraction; return hitFraction;
} }
@ -202,7 +189,6 @@ bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* _shape1,
// Get the two AABBs of the collision shapes // Get the two AABBs of the collision shapes
const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID); const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID);
const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(_shape2->m_broadPhaseID); const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(_shape2->m_broadPhaseID);
// Check if the two AABBs are overlapping // Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2); return aabb1.testCollision(aabb2);
} }

84
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -18,29 +18,25 @@ ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
} }
ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() { void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& _shape1Info,
const CollisionShapeInfo& _shape2Info,
} NarrowPhaseCallback* _callback) {
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
ProxyShape* convexProxyShape; ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape; ProxyShape* concaveProxyShape;
const ConvexShape* convexShape; const ConvexShape* convexShape;
const ConcaveShape* concaveShape; const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave // Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) { if (_shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape; convexProxyShape = _shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape); convexShape = static_cast<const ConvexShape*>(_shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape; concaveProxyShape = _shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(_shape2Info.collisionShape);
} else { } else {
// Collision shape 2 is convex, collision shape 1 is concave // Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape; convexProxyShape = _shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape); convexShape = static_cast<const ConvexShape*>(_shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape; concaveProxyShape = _shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(_shape1Info.collisionShape);
} }
// Set the parameters of the callback object // Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback; ConvexVsTriangleCallback convexVsTriangleCallback;
@ -48,7 +44,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
convexVsTriangleCallback.setConvexShape(convexShape); convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape); convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); convexVsTriangleCallback.setOverlappingPair(_shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape // Compute the convex shape AABB in the local-space of the convex shape
AABB aabb; AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
@ -60,21 +56,20 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm // Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); processSmoothMeshCollision(_shape1Info.overlappingPair, contactPoints, _callback);
} else { } else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(_callback);
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
} }
} }
void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) { void ConvexVsTriangleCallback::testTriangle(const vec3* _trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
float margin = m_concaveShape->getTriangleMargin(); float margin = m_concaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(), NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(), m_convexShape->getType());
m_convexShape->getType());
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (algo == nullptr) { if (algo == nullptr) {
return; return;
@ -82,29 +77,34 @@ void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) {
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(m_overlappingPair); algo->setCurrentOverlappingPair(m_overlappingPair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(m_convexProxyShape, m_convexShape, m_convexProxyShape->getLocalToWorldTransform(), CollisionShapeInfo shapeConvexInfo(m_convexProxyShape,
m_overlappingPair, m_convexProxyShape->getCachedCollisionData()); m_convexShape,
CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape, &triangleShape, m_convexProxyShape->getLocalToWorldTransform(),
m_concaveProxyShape->getLocalToWorldTransform(), m_overlappingPair,
m_overlappingPair, m_concaveProxyShape->getCachedCollisionData()); m_convexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape,
&triangleShape,
m_concaveProxyShape->getLocalToWorldTransform(),
m_overlappingPair,
m_concaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape // Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, m_narrowPhaseCallback); algo->testCollision(shapeConvexInfo, shapeConcaveInfo, m_narrowPhaseCallback);
} }
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* _overlappingPair,
etk::Vector<SmoothMeshContactInfo> contactPoints, etk::Vector<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle // Set with the triangle vertices already processed to void further contacts with same triangle
etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices; etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth // Sort the list of narrow-phase contacts according to their penetration depth
contactPoints.sort(0, _contactPoints.sort(0,
contactPoints.size()-1, _contactPoints.size()-1,
[](const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) { [](const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) {
return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth; return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth;
}); });
// For each contact point (from smaller penetration depth to larger) // For each contact point (from smaller penetration depth to larger)
etk::Vector<SmoothMeshContactInfo>::Iterator it; etk::Vector<SmoothMeshContactInfo>::Iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { for (it = _contactPoints.begin(); it != _contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it; const SmoothMeshContactInfo info = *it;
const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle // Compute the barycentric coordinates of the point in the triangle
@ -132,7 +132,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
// Check that this triangle vertex has not been processed yet // Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); _callback->notifyContact(_overlappingPair, info.contactInfo);
} }
} else if (nbZeros == 1) { } else if (nbZeros == 1) {
// If it is an edge contact // If it is an edge contact
@ -142,7 +142,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); _callback->notifyContact(_overlappingPair, info.contactInfo);
} }
} else { } else {
// If it is a face contact // If it is a face contact
@ -150,11 +150,11 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
ProxyShape* firstShape; ProxyShape* firstShape;
ProxyShape* secondShape; ProxyShape* secondShape;
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1(); firstShape = _overlappingPair->getShape1();
secondShape = overlappingPair->getShape2(); secondShape = _overlappingPair->getShape2();
} else { } else {
firstShape = overlappingPair->getShape2(); firstShape = _overlappingPair->getShape2();
secondShape = overlappingPair->getShape1(); secondShape = _overlappingPair->getShape1();
} }
// We use the triangle normal as the contact normal // We use the triangle normal as the contact normal
vec3 a = info.triangleVertices[1] - info.triangleVertices[0]; vec3 a = info.triangleVertices[1] - info.triangleVertices[0];
@ -179,7 +179,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
} }
// Report the contact // Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); _callback->notifyContact(_overlappingPair, newContactInfo);
} }
// Add the three vertices of the triangle to the set of processed // Add the three vertices of the triangle to the set of processed
// triangle vertices // triangle vertices
@ -214,7 +214,7 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const etk::Vector<etk::Pai
} }
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* _overlappingPair, void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* _overlappingPair,
const ContactPointInfo& _contactInfo) { const ContactPointInfo& _contactInfo) {
vec3 triangleVertices[3]; vec3 triangleVertices[3];
bool isFirstShapeTriangle; bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle // If the collision shape 1 is the triangle

2
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp
View File

@ -138,8 +138,6 @@ namespace ephysics {
public : public :
/// Constructor /// Constructor
ConcaveVsConvexAlgorithm(); ConcaveVsConvexAlgorithm();
/// Destructor
virtual ~ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& _shape1Info, virtual void testCollision(const CollisionShapeInfo& _shape1Info,
const CollisionShapeInfo& _shape2Info, const CollisionShapeInfo& _shape2Info,

20
ephysics/collision/shapes/AABB.cpp
View File

@ -151,13 +151,19 @@ void AABB::inflate(float _dx, float _dy, float _dz) {
m_minCoordinates -= vec3(_dx, _dy, _dz); m_minCoordinates -= vec3(_dx, _dy, _dz);
} }
bool AABB::testCollision(const AABB& aabb) const { bool AABB::testCollision(const AABB& _aabb) const {
if (m_maxCoordinates.x() < aabb.m_minCoordinates.x() || if ( m_maxCoordinates.x() < _aabb.m_minCoordinates.x()
aabb.m_maxCoordinates.x() < m_minCoordinates.x()) return false; || _aabb.m_maxCoordinates.x() < m_minCoordinates.x()) {
if (m_maxCoordinates.y() < aabb.m_minCoordinates.y() || return false;
aabb.m_maxCoordinates.y() < m_minCoordinates.y()) return false; }
if (m_maxCoordinates.z() < aabb.m_minCoordinates.z()|| if ( m_maxCoordinates.y() < _aabb.m_minCoordinates.y()
aabb.m_maxCoordinates.z() < m_minCoordinates.z()) return false; || _aabb.m_maxCoordinates.y() < m_minCoordinates.y()) {
return false;
}
if ( m_maxCoordinates.z() < _aabb.m_minCoordinates.z()
|| _aabb.m_maxCoordinates.z() < m_minCoordinates.z()) {
return false;
}
return true; return true;
} }

95
ephysics/collision/shapes/BoxShape.cpp
View File

@ -14,11 +14,6 @@
using namespace ephysics; using namespace ephysics;
// Constructor
/**
* @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const vec3& _extent, float _margin): BoxShape::BoxShape(const vec3& _extent, float _margin):
ConvexShape(BOX, _margin), ConvexShape(BOX, _margin),
m_extent(_extent - vec3(_margin, _margin, _margin)) { m_extent(_extent - vec3(_margin, _margin, _margin)) {
@ -27,27 +22,19 @@ BoxShape::BoxShape(const vec3& _extent, float _margin):
assert(_extent.z() > 0.0f && _extent.z() > _margin); assert(_extent.z() > 0.0f && _extent.z() > _margin);
} }
// Return the local inertia tensor of the collision shape void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
/** float factor = (1.0f / float(3.0)) * _mass;
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass;
vec3 realExtent = m_extent + vec3(m_margin, m_margin, m_margin); vec3 realExtent = m_extent + vec3(m_margin, m_margin, m_margin);
float xSquare = realExtent.x() * realExtent.x(); float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y(); float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z(); float zSquare = realExtent.z() * realExtent.z();
tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, _tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0, 0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
// Raycast method with feedback information bool BoxShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { vec3 rayDirection = _ray.point2 - _ray.point1;
vec3 rayDirection = ray.point2 - ray.point1;
float tMin = FLT_MIN; float tMin = FLT_MIN;
float tMax = FLT_MAX; float tMax = FLT_MAX;
vec3 normalDirection(0,0,0); vec3 normalDirection(0,0,0);
@ -57,14 +44,14 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
// If ray is parallel to the slab // If ray is parallel to the slab
if (etk::abs(rayDirection[iii]) < FLT_EPSILON) { if (etk::abs(rayDirection[iii]) < FLT_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit // If the ray's origin is not inside the slab, there is no hit
if (ray.point1[iii] > m_extent[iii] || ray.point1[iii] < -m_extent[iii]) { if (_ray.point1[iii] > m_extent[iii] || _ray.point1[iii] < -m_extent[iii]) {
return false; return false;
} }
} else { } else {
// Compute the intersection of the ray with the near and far plane of the slab // Compute the intersection of the ray with the near and far plane of the slab
float oneOverD = 1.0f / rayDirection[iii]; float oneOverD = 1.0f / rayDirection[iii];
float t1 = (-m_extent[iii] - ray.point1[iii]) * oneOverD; float t1 = (-m_extent[iii] - _ray.point1[iii]) * oneOverD;
float t2 = (m_extent[iii] - ray.point1[iii]) * oneOverD; float t2 = (m_extent[iii] - _ray.point1[iii]) * oneOverD;
currentNormal[0] = (iii == 0) ? -m_extent[iii] : 0.0f; currentNormal[0] = (iii == 0) ? -m_extent[iii] : 0.0f;
currentNormal[1] = (iii == 1) ? -m_extent[iii] : 0.0f; currentNormal[1] = (iii == 1) ? -m_extent[iii] : 0.0f;
currentNormal[2] = (iii == 2) ? -m_extent[iii] : 0.0f; currentNormal[2] = (iii == 2) ? -m_extent[iii] : 0.0f;
@ -81,7 +68,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
} }
tMax = etk::min(tMax, t2); tMax = etk::min(tMax, t2);
// If tMin is larger than the maximum raycasting fraction, we return no hit // If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) { if (tMin > _ray.maxFraction) {
return false; return false;
} }
// If the slabs intersection is empty, there is no hit // If the slabs intersection is empty, there is no hit
@ -92,71 +79,55 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
} }
// If tMin is negative, we return no hit // If tMin is negative, we return no hit
if ( tMin < 0.0f if ( tMin < 0.0f
|| tMin > ray.maxFraction) { || tMin > _ray.maxFraction) {
return false; return false;
} }
if (normalDirection == vec3(0,0,0)) { if (normalDirection == vec3(0,0,0)) {
return false; return false;
} }
// The ray int32_tersects the three slabs, we compute the hit point // The ray int32_tersects the three slabs, we compute the hit point
vec3 localHitPoint = ray.point1 + tMin * rayDirection; vec3 localHitPoint = _ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = tMin; _raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localHitPoint; _raycastInfo.worldPoint = localHitPoint;
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
// Return the extents of the box
/**
* @return The vector with the three extents of the box shape (in meters)
*/
vec3 BoxShape::getExtent() const { vec3 BoxShape::getExtent() const {
return m_extent + vec3(m_margin, m_margin, m_margin); return m_extent + vec3(m_margin, m_margin, m_margin);
} }
// Set the scaling vector of the collision shape void BoxShape::setLocalScaling(const vec3& _scaling) {
void BoxShape::setLocalScaling(const vec3& scaling) { m_extent = (m_extent / m_scaling) * _scaling;
CollisionShape::setLocalScaling(_scaling);
m_extent = (m_extent / m_scaling) * scaling;
CollisionShape::setLocalScaling(scaling);
} }
// Return the local bounds of the shape in x, y and z directions
/// This method is used to compute the AABB of the box
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void BoxShape::getLocalBounds(vec3& _min, vec3& _max) const { void BoxShape::getLocalBounds(vec3& _min, vec3& _max) const {
// Maximum bounds // Maximum bounds
_max = m_extent + vec3(m_margin, m_margin, m_margin); _max = m_extent + vec3(m_margin, m_margin, m_margin);
// Minimum bounds // Minimum bounds
_min = -_max; _min = -_max;
} }
// Return the number of bytes used by the collision shape
size_t BoxShape::getSizeInBytes() const { size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape); return sizeof(BoxShape);
} }
// Return a local support point in a given direction without the objec margin vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction, void** _cachedCollisionData) const {
void** cachedCollisionData) const { return vec3(_direction.x() < 0.0 ? -m_extent.x() : m_extent.x(),
_direction.y() < 0.0 ? -m_extent.y() : m_extent.y(),
return vec3(direction.x() < 0.0 ? -m_extent.x() : m_extent.x(), _direction.z() < 0.0 ? -m_extent.z() : m_extent.z());
direction.y() < 0.0 ? -m_extent.y() : m_extent.y(),
direction.z() < 0.0 ? -m_extent.z() : m_extent.z());
} }
// Return true if a point is inside the collision shape bool BoxShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
bool BoxShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { return ( _localPoint.x() < m_extent[0]
return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] && && _localPoint.x() > -m_extent[0]
localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] && && _localPoint.y() < m_extent[1]
localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]); && _localPoint.y() > -m_extent[1]
&& _localPoint.z() < m_extent[2]
&& _localPoint.z() > -m_extent[2]);
} }

33
ephysics/collision/shapes/BoxShape.hpp
View File

@ -28,24 +28,31 @@ namespace ephysics {
* default margin distance by not using the "margin" parameter in the constructor. * default margin distance by not using the "margin" parameter in the constructor.
*/ */
class BoxShape : public ConvexShape { class BoxShape : public ConvexShape {
protected : public:
vec3 m_extent; //!< Extent sizes of the box in the x, y and z direction /**
/// Private copy-constructor * @brief Constructor
BoxShape(const BoxShape& _shape) = delete; * @param extent The vector with the three extents of the box (in meters)
/// Private assignment operator * @param margin The collision margin (in meters) around the collision shape
BoxShape& operator=(const BoxShape& _shape) = delete; */
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
public :
/// Constructor
BoxShape(const vec3& _extent, float _margin = OBJECT_MARGIN); BoxShape(const vec3& _extent, float _margin = OBJECT_MARGIN);
/// Return the extents of the box /// DELETE copy-constructor
BoxShape(const BoxShape& _shape) = delete;
/// DELETE assignment operator
BoxShape& operator=(const BoxShape& _shape) = delete;
/**
* @brief Return the extents of the box
* @return The vector with the three extents of the box shape (in meters)
*/
vec3 getExtent() const; vec3 getExtent() const;
void setLocalScaling(const vec3& _scaling) override; void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) const override; void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
protected:
vec3 m_extent; //!< Extent sizes of the box in the x, y and z direction
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
}; };
} }

289
ephysics/collision/shapes/CapsuleShape.cpp
View File

@ -12,11 +12,6 @@
using namespace ephysics; using namespace ephysics;
// Constructor
/**
* @param _radius The radius of the capsule (in meters)
* @param _height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(float _radius, float _height): CapsuleShape::CapsuleShape(float _radius, float _height):
ConvexShape(CAPSULE, _radius), ConvexShape(CAPSULE, _radius),
m_halfHeight(_height * 0.5f) { m_halfHeight(_height * 0.5f) {
@ -24,21 +19,8 @@ CapsuleShape::CapsuleShape(float _radius, float _height):
assert(_height > 0.0f); assert(_height > 0.0f);
} }
// Destructor void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
CapsuleShape::~CapsuleShape() {
}
// Return the local inertia tensor of the capsule
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
float height = m_halfHeight + m_halfHeight; float height = m_halfHeight + m_halfHeight;
float radiusSquare = m_margin * m_margin; float radiusSquare = m_margin * m_margin;
float heightSquare = height * height; float heightSquare = height * height;
@ -48,292 +30,231 @@ void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass)
float sum1 = float(0.4) * radiusSquareDouble; float sum1 = float(0.4) * radiusSquareDouble;
float sum2 = float(0.75) * height * m_margin + 0.5f * heightSquare; float sum2 = float(0.75) * height * m_margin + 0.5f * heightSquare;
float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare; float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3; float IxxAndzz = factor1 * _mass * (sum1 + sum2) + factor2 * _mass * sum3;
float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble; float Iyy = factor1 * _mass * sum1 + factor2 * _mass * float(0.25) * radiusSquareDouble;
tensor.setValue(IxxAndzz, 0.0, 0.0, _tensor.setValue(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0, 0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz); 0.0, 0.0, IxxAndzz);
} }
// Return true if a point is inside the collision shape bool CapsuleShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
bool CapsuleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { const float diffYCenterSphere1 = _localPoint.y() - m_halfHeight;
const float diffYCenterSphere2 = _localPoint.y() + m_halfHeight;
const float diffYCenterSphere1 = localPoint.y() - m_halfHeight; const float xSquare = _localPoint.x() * _localPoint.x();
const float diffYCenterSphere2 = localPoint.y() + m_halfHeight; const float zSquare = _localPoint.z() * _localPoint.z();
const float xSquare = localPoint.x() * localPoint.x();
const float zSquare = localPoint.z() * localPoint.z();
const float squareRadius = m_margin * m_margin; const float squareRadius = m_margin * m_margin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule // Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius && return ((xSquare + zSquare) < squareRadius &&
localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) || _localPoint.y() < m_halfHeight && _localPoint.y() > -m_halfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
} }
// Raycast method with feedback information bool CapsuleShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { const vec3 n = _ray.point2 - _ray.point1;
const vec3 n = ray.point2 - ray.point1;
const float epsilon = float(0.01); const float epsilon = float(0.01);
vec3 p(float(0), -m_halfHeight, float(0)); vec3 p(float(0), -m_halfHeight, float(0));
vec3 q(float(0), m_halfHeight, float(0)); vec3 q(float(0), m_halfHeight, float(0));
vec3 d = q - p; vec3 d = q - p;
vec3 m = ray.point1 - p; vec3 m = _ray.point1 - p;
float t; float t;
float mDotD = m.dot(d); float mDotD = m.dot(d);
float nDotD = n.dot(d); float nDotD = n.dot(d);
float dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // Test if the segment is outside the cylinder
float vec1DotD = (ray.point1 - vec3(0.0f, -m_halfHeight - m_margin, float(0.0))).dot(d); float vec1DotD = (_ray.point1 - vec3(0.0f, -m_halfHeight - m_margin, float(0.0))).dot(d);
if (vec1DotD < 0.0f && vec1DotD + nDotD < float(0.0)) return false; if ( vec1DotD < 0.0f
&& vec1DotD + nDotD < float(0.0)) {
return false;
}
float ddotDExtraCaps = float(2.0) * m_margin * d.y(); float ddotDExtraCaps = float(2.0) * m_margin * d.y();
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false; if ( vec1DotD > dDotD + ddotDExtraCaps
&& vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
return false;
}
float nDotN = n.dot(n); float nDotN = n.dot(n);
float mDotN = m.dot(n); float mDotN = m.dot(n);
float a = dDotD * nDotN - nDotD * nDotD; float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - m_margin * m_margin; float k = m.dot(m) - m_margin * m_margin;
float c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis // If the ray is parallel to the capsule axis
if (etk::abs(a) < epsilon) { if (etk::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit // If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > 0.0f) return false; if (c > 0.0f) {
return false;
}
// Here we know that the segment int32_tersect an endcap of the capsule // Here we know that the segment int32_tersect an endcap of the capsule
// If the ray int32_tersects with the "p" endcap of the capsule // If the ray int32_tersects with the "p" endcap of the capsule
if (mDotD < 0.0f) { if (mDotD < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint; vec3 hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = hitFraction; _raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; _raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p; vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} } else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
// Check int32_tersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
vec3 hitLocalPoint; vec3 hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = hitFraction; _raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; _raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q; vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} } else {
else { // If the origin is inside the cylinder, we return no hit // If the origin is inside the cylinder, we return no hit
return false; return false;
} }
} }
float b = dDotD * mDotN - nDotD * mDotD; float b = dDotD * mDotN - nDotD * mDotD;
float discriminant = b * b - a * c; float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit // If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < 0.0f) return false; if (discriminant < 0.0f) {
return false;
}
// Compute the smallest root (first int32_tersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - etk::sqrt(discriminant)) / a; float t0 = t = (-b - etk::sqrt(discriminant)) / a;
// If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < 0.0f) { if (value < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule // Check int32_tersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint; vec3 hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = hitFraction; _raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; _raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p; vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} } else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side
else if (value > dDotD) { // If the int32_tersection is outside the finite cylinder on the "q" side
// Check int32_tersection between the ray and the "q" sphere endcap of the capsule // Check int32_tersection between the ray and the "q" sphere endcap of the capsule
vec3 hitLocalPoint; vec3 hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = hitFraction; _raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; _raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q; vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} }
t = t0; t = t0;
// If the int32_tersection is behind the origin of the ray or beyond the maximum // If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit // raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false; if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; vec3 localHitPoint = _ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = t; _raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; _raycastInfo.worldPoint = localHitPoint;
vec3 v = localHitPoint - p; vec3 v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d; vec3 w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized(); vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized();
raycastInfo.worldNormal = normalDirection; _raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
// Raycasting method between a ray one of the two spheres end cap of the capsule bool CapsuleShape::raycastWithSphereEndCap(const vec3& _point1,
bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point2, const vec3& _point2,
const vec3& sphereCenter, float maxFraction, const vec3& _sphereCenter,
vec3& hitLocalPoint, float& hitFraction) const { float _maxFraction,
vec3& _hitLocalPoint,
const vec3 m = point1 - sphereCenter; float& _hitFraction) const {
const vec3 m = _point1 - _sphereCenter;
float c = m.dot(m) - m_margin * m_margin; float c = m.dot(m) - m_margin * m_margin;
// If the origin of the ray is inside the sphere, we return no int32_tersection // If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < 0.0f) return false; if (c < 0.0f) {
return false;
const vec3 rayDirection = point2 - point1; }
const vec3 rayDirection = _point2 - _point1;
float b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no int32_tersection // is pointing away from the sphere, there is no int32_tersection
if (b > 0.0f) return false; if (b > 0.0f) {
return false;
}
float raySquareLength = rayDirection.length2(); float raySquareLength = rayDirection.length2();
// Compute the discriminant of the quadratic equation // Compute the discriminant of the quadratic equation
float discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < 0.0f || raySquareLength < FLT_EPSILON) return false; if ( discriminant < 0.0f
|| raySquareLength < FLT_EPSILON) {
return false;
}
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
float t = -b - etk::sqrt(discriminant); float t = -b - etk::sqrt(discriminant);
assert(t >= 0.0f); assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) { if (t < _maxFraction * raySquareLength) {
// Compute the int32_tersection information // Compute the int32_tersection information
t /= raySquareLength; t /= raySquareLength;
hitFraction = t; _hitFraction = t;
hitLocalPoint = point1 + t * rayDirection; _hitLocalPoint = _point1 + t * rayDirection;
return true; return true;
} }
return false; return false;
} }
// Get the radius of the capsule
/**
* @return The radius of the capsule shape (in meters)
*/
float CapsuleShape::getRadius() const { float CapsuleShape::getRadius() const {
return m_margin; return m_margin;
} }
// Return the height of the capsule
/**
* @return The height of the capsule shape (in meters)
*/
float CapsuleShape::getHeight() const { float CapsuleShape::getHeight() const {
return m_halfHeight + m_halfHeight; return m_halfHeight + m_halfHeight;
} }
// Set the scaling vector of the collision shape void CapsuleShape::setLocalScaling(const vec3& _scaling) {
void CapsuleShape::setLocalScaling(const vec3& scaling) { m_halfHeight = (m_halfHeight / m_scaling.y()) * _scaling.y();
m_margin = (m_margin / m_scaling.x()) * _scaling.x();
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); CollisionShape::setLocalScaling(_scaling);
m_margin = (m_margin / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape
size_t CapsuleShape::getSizeInBytes() const { size_t CapsuleShape::getSizeInBytes() const {
return sizeof(CapsuleShape); return sizeof(CapsuleShape);
} }
// Return the local bounds of the shape in x, y and z directions void CapsuleShape::getLocalBounds(vec3& _min, vec3& _max) const {
// This method is used to compute the AABB of the box
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void CapsuleShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds // Maximum bounds
max.setX(m_margin); _max.setX(m_margin);
max.setY(m_halfHeight + m_margin); _max.setY(m_halfHeight + m_margin);
max.setZ(m_margin); _max.setZ(m_margin);
// Minimum bounds // Minimum bounds
min.setX(-m_margin); _min.setX(-m_margin);
min.setY(-max.y()); _min.setY(-_max.y());
min.setZ(min.x()); _min.setZ(_min.x());
} }
// Return a local support point in a given direction without the object margin. vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" void** _cachedCollisionData) const {
/// of the convex hull of a set of convex objects is the support point "p" in the set of all
/// support points from all the convex objects with the maximum dot product with the direction "d".
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
// Support point top sphere // Support point top sphere
float dotProductTop = m_halfHeight * direction.y(); float dotProductTop = m_halfHeight * _direction.y();
// Support point bottom sphere // Support point bottom sphere
float dotProductBottom = -m_halfHeight * direction.y(); float dotProductBottom = -m_halfHeight * _direction.y();
// Return the point with the maximum dot product // Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) { if (dotProductTop > dotProductBottom) {
return vec3(0, m_halfHeight, 0); return vec3(0, m_halfHeight, 0);
} }
else { return vec3(0, -m_halfHeight, 0);
return vec3(0, -m_halfHeight, 0); }
}
}

51
ephysics/collision/shapes/CapsuleShape.hpp
View File

@ -23,31 +23,44 @@ namespace ephysics {
* capsule shape. * capsule shape.
*/ */
class CapsuleShape : public ConvexShape { class CapsuleShape : public ConvexShape {
protected:
float m_halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
/// Private copy-constructor
CapsuleShape(const CapsuleShape& _shape);
/// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& _shape);
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const vec3& _point1, const vec3& _point2,
const vec3& _sphereCenter, float _maxFraction,
vec3& _hitLocalPoint, float& _hitFraction) const;
size_t getSizeInBytes() const override;
public : public :
/// Constructor /**
* @brief Constructor
* @param _radius The radius of the capsule (in meters)
* @param _height The height of the capsule (in meters)
*/
CapsuleShape(float _radius, float _height); CapsuleShape(float _radius, float _height);
/// Destructor /// DELETE copy-constructor
virtual ~CapsuleShape(); CapsuleShape(const CapsuleShape& _shape) = delete;
/// Return the radius of the capsule /// DELETE assignment operator
CapsuleShape& operator=(const CapsuleShape& _shape) = delete;
/**
* Get the radius of the capsule
* @return The radius of the capsule shape (in meters)
*/
float getRadius() const; float getRadius() const;
/// Return the height of the capsule /**
* @brief Return the height of the capsule
* @return The height of the capsule shape (in meters)
*/
float getHeight() const; float getHeight() const;
void setLocalScaling(const vec3& _scaling) override; void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) const override; void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
protected:
float m_halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
/**
* @brief Raycasting method between a ray one of the two spheres end cap of the capsule
*/
bool raycastWithSphereEndCap(const vec3& _point1,
const vec3& _point2,
const vec3& _sphereCenter,
float _maxFraction,
vec3& _hitLocalPoint,
float& _hitFraction) const;
size_t getSizeInBytes() const override;
}; };
} }

41
ephysics/collision/shapes/CollisionShape.cpp
View File

@ -19,50 +19,35 @@ CollisionShape::CollisionShape(CollisionShapeType type) :
} }
CollisionShape::~CollisionShape() { void CollisionShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const {
}
// Compute the world-space AABB of the collision shape given a transform
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform etk::Transform3D used to compute the AABB of the collision shape
*/
void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const {
PROFILE("CollisionShape::computeAABB()"); PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction // Get the local bounds in x,y and z direction
vec3 minBounds(0,0,0); vec3 minBounds(0,0,0);
vec3 maxBounds(0,0,0); vec3 maxBounds(0,0,0);
getLocalBounds(minBounds, maxBounds); getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body // Rotate the local bounds according to the orientation of the body
etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute(); etk::Matrix3x3 worldAxis = _transform.getOrientation().getMatrix().getAbsolute();
vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds)); worldAxis.getColumn(2).dot(minBounds));
vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds)); worldAxis.getColumn(2).dot(maxBounds));
// Compute the minimum and maximum coordinates of the rotated extents // Compute the minimum and maximum coordinates of the rotated extents
vec3 minCoordinates = transform.getPosition() + worldMinBounds; vec3 minCoordinates = _transform.getPosition() + worldMinBounds;
vec3 maxCoordinates = transform.getPosition() + worldMaxBounds; vec3 maxCoordinates = _transform.getPosition() + worldMaxBounds;
// Update the AABB with the new minimum and maximum coordinates // Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates); _aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates); _aabb.setMax(maxCoordinates);
} }
int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType _shapeType1,
CollisionShapeType shapeType2) { CollisionShapeType _shapeType2) {
// If both shapes are convex // If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) { if (isConvex(_shapeType1) && isConvex(_shapeType2)) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
} }
} // If there is at least one concave shape
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}

28
ephysics/collision/shapes/CollisionShape.hpp
View File

@ -28,24 +28,13 @@ class CollisionBody;
* body that is used during the narrow-phase collision detection. * body that is used during the narrow-phase collision detection.
*/ */
class CollisionShape { class CollisionShape {
protected :
CollisionShapeType m_type; //!< Type of the collision shape
vec3 m_scaling; //!< Scaling vector of the collision shape
/// Private copy-constructor
CollisionShape(const CollisionShape& shape) = delete;
/// Private assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
public : public :
/// Constructor /// Constructor
CollisionShape(CollisionShapeType _type); CollisionShape(CollisionShapeType _type);
/// Destructor /// DELETE copy-constructor
virtual ~CollisionShape(); CollisionShape(const CollisionShape& shape) = delete;
/// DELETE assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/** /**
* @brief Get the type of the collision shapes * @brief Get the type of the collision shapes
* @return The type of the collision shape (box, sphere, cylinder, ...) * @return The type of the collision shape (box, sphere, cylinder, ...)
@ -106,6 +95,15 @@ class CollisionShape {
CollisionShapeType _shapeType2); CollisionShapeType _shapeType2);
friend class ProxyShape; friend class ProxyShape;
friend class CollisionWorld; friend class CollisionWorld;
protected :
CollisionShapeType m_type; //!< Type of the collision shape
vec3 m_scaling; //!< Scaling vector of the collision shape
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const = 0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const = 0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
}; };

1
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -42,7 +42,6 @@ void ConcaveMeshShape::initBVHTree() {
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int32_t _triangleIndex, vec3* _outTriangleVertices) const { void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int32_t _triangleIndex, vec3* _outTriangleVertices) const {
EPHY_ASSERT(_outTriangleVertices != nullptr, "Input check error"); EPHY_ASSERT(_outTriangleVertices != nullptr, "Input check error");
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart);
if (triangleVertexArray == nullptr) { if (triangleVertexArray == nullptr) {

29
ephysics/collision/shapes/ConcaveMeshShape.hpp
View File

@ -17,7 +17,7 @@
namespace ephysics { namespace ephysics {
class ConcaveMeshShape; class ConcaveMeshShape;
class ConcaveMeshRaycastCallback { class ConcaveMeshRaycastCallback {
private : private:
etk::Vector<int32_t> m_hitAABBNodes; etk::Vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
const ConcaveMeshShape& m_concaveMeshShape; const ConcaveMeshShape& m_concaveMeshShape;
@ -55,13 +55,22 @@ namespace ephysics {
* this shape for a static mesh. * this shape for a static mesh.
*/ */
class ConcaveMeshShape : public ConcaveShape { class ConcaveMeshShape : public ConcaveShape {
public:
/// Constructor
ConcaveMeshShape(TriangleMesh* _triangleMesh);
/// DELETE copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& _shape) = delete;
/// DELETE assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& _shape) = delete;
virtual void getLocalBounds(vec3& _min, vec3& _max) const override;
virtual void setLocalScaling(const vec3& _scaling) override;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
virtual void testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const override;
friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback;
protected: protected:
TriangleMesh* m_triangleMesh; //!< Triangle mesh TriangleMesh* m_triangleMesh; //!< Triangle mesh
DynamicAABBTree m_dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles DynamicAABBTree m_dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
/// Private copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& _shape) = delete;
/// Private assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& _shape) = delete;
virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
virtual size_t getSizeInBytes() const override; virtual size_t getSizeInBytes() const override;
/// Insert all the triangles int32_to the dynamic AABB tree /// Insert all the triangles int32_to the dynamic AABB tree
@ -71,16 +80,6 @@ namespace ephysics {
void getTriangleVerticesWithIndexPointer(int32_t _subPart, void getTriangleVerticesWithIndexPointer(int32_t _subPart,
int32_t _triangleIndex, int32_t _triangleIndex,
vec3* _outTriangleVertices) const; vec3* _outTriangleVertices) const;
public:
/// Constructor
ConcaveMeshShape(TriangleMesh* triangleMesh);
virtual void getLocalBounds(vec3& min, vec3& max) const override;
virtual void setLocalScaling(const vec3& scaling) override;
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const override;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override;
friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback;
}; };
} }

38
ephysics/collision/shapes/ConcaveShape.cpp
View File

@ -13,54 +13,38 @@
using namespace ephysics; using namespace ephysics;
// Constructor // Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type) ConcaveShape::ConcaveShape(CollisionShapeType _type):
: CollisionShape(type), m_isSmoothMeshCollisionEnabled(false), CollisionShape(_type),
m_triangleMargin(0), m_raycastTestType(FRONT) { m_isSmoothMeshCollisionEnabled(false),
m_triangleMargin(0),
m_raycastTestType(FRONT) {
} }
// Destructor
ConcaveShape::~ConcaveShape() {
}
// Return the triangle margin
float ConcaveShape::getTriangleMargin() const { float ConcaveShape::getTriangleMargin() const {
return m_triangleMargin; return m_triangleMargin;
} }
/// Return true if the collision shape is convex, false if it is concave
bool ConcaveShape::isConvex() const { bool ConcaveShape::isConvex() const {
return false; return false;
} }
// Return true if a point is inside the collision shape bool ConcaveShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
bool ConcaveShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
return false; return false;
} }
// Return true if the smooth mesh collision is enabled
bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
return m_isSmoothMeshCollisionEnabled; return m_isSmoothMeshCollisionEnabled;
} }
// Enable/disable the smooth mesh collision algorithm void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool _isEnabled) {
/// Smooth mesh collision is used to avoid collisions against some int32_ternal edges m_isSmoothMeshCollisionEnabled = _isEnabled;
/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
/// but collisions computation is a bit more expensive.
void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
m_isSmoothMeshCollisionEnabled = isEnabled;
} }
// Return the raycast test type (front, back, front-back)
TriangleRaycastSide ConcaveShape::getRaycastTestType() const { TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
return m_raycastTestType; return m_raycastTestType;
} }
// Set the raycast test type (front, back, front-back) void ConcaveShape::setRaycastTestType(TriangleRaycastSide _testType) {
/** m_raycastTestType = _testType;
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
m_raycastTestType = testType;
} }

31
ephysics/collision/shapes/ConcaveShape.hpp
View File

@ -28,25 +28,29 @@ namespace ephysics {
* body that is used during the narrow-phase collision detection. * body that is used during the narrow-phase collision detection.
*/ */
class ConcaveShape : public CollisionShape { class ConcaveShape : public CollisionShape {
protected :
bool m_isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
float m_triangleMargin; //!< Margin use for collision detection for each triangle
TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
/// Private copy-constructor
ConcaveShape(const ConcaveShape& _shape) = delete;
/// Private assignment operator
ConcaveShape& operator=(const ConcaveShape& _shape) = delete;
virtual bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
public : public :
/// Constructor /// Constructor
ConcaveShape(CollisionShapeType _type); ConcaveShape(CollisionShapeType _type);
/// Destructor /// Destructor
virtual ~ConcaveShape(); virtual ~ConcaveShape();
/// DELETE copy-constructor
ConcaveShape(const ConcaveShape& _shape) = delete;
/// DELETE assignment operator
ConcaveShape& operator=(const ConcaveShape& _shape) = delete;
protected :
bool m_isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
float m_triangleMargin; //!< Margin use for collision detection for each triangle
TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
public:
/// Return the triangle margin /// Return the triangle margin
float getTriangleMargin() const; float getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back) /// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const; TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back) /**
* @brief Set the raycast test type (front, back, front-back)
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
void setRaycastTestType(TriangleRaycastSide _testType); void setRaycastTestType(TriangleRaycastSide _testType);
/// Return true if the collision shape is convex, false if it is concave /// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const override; virtual bool isConvex() const override;
@ -54,7 +58,12 @@ namespace ephysics {
virtual void testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const=0; virtual void testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const=0;
/// Return true if the smooth mesh collision is enabled /// Return true if the smooth mesh collision is enabled
bool getIsSmoothMeshCollisionEnabled() const; bool getIsSmoothMeshCollisionEnabled() const;
/// Enable/disable the smooth mesh collision algorithm /**
* @brief Enable/disable the smooth mesh collision algorithm
*
* Smooth mesh collision is used to avoid collisions against some int32_ternal edges of the triangle mesh.
* If it is enabled, collsions with the mesh will be smoother but collisions computation is a bit more expensive.
*/
void setIsSmoothMeshCollisionEnabled(bool _isEnabled); void setIsSmoothMeshCollisionEnabled(bool _isEnabled);
}; };

136
ephysics/collision/shapes/ConeShape.cpp
View File

@ -12,56 +12,36 @@
using namespace ephysics; using namespace ephysics;
// Constructor ConeShape::ConeShape(float _radius, float _height, float _margin):
/** ConvexShape(CONE, _margin),
* @param radius Radius of the cone (in meters) m_radius(_radius),
* @param height Height of the cone (in meters) m_halfHeight(_height * 0.5f) {
* @param margin Collision margin (in meters) around the collision shape
*/
ConeShape::ConeShape(float radius, float height, float margin):
ConvexShape(CONE, margin),
m_radius(radius),
m_halfHeight(height * 0.5f) {
assert(m_radius > 0.0f); assert(m_radius > 0.0f);
assert(m_halfHeight > 0.0f); assert(m_halfHeight > 0.0f);
// Compute the sine of the semi-angle at the apex point // Compute the sine of the semi-angle at the apex point
m_sinTheta = m_radius / (sqrt(m_radius * m_radius + height * height)); m_sinTheta = m_radius / (sqrt(m_radius * m_radius + _height * _height));
} }
// Return a local support point in a given direction without the object margin vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction, const vec3& v = _direction;
void** cachedCollisionData) const {
const vec3& v = direction;
float sinThetaTimesLengthV = m_sinTheta * v.length(); float sinThetaTimesLengthV = m_sinTheta * v.length();
vec3 supportPoint; vec3 supportPoint;
if (v.y() > sinThetaTimesLengthV) { if (v.y() > sinThetaTimesLengthV) {
supportPoint = vec3(0.0, m_halfHeight, 0.0); supportPoint = vec3(0.0, m_halfHeight, 0.0);
} } else {
else {
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
if (projectedLength > FLT_EPSILON) { if (projectedLength > FLT_EPSILON) {
float d = m_radius / projectedLength; float d = m_radius / projectedLength;
supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d); supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d);
} } else {
else {
supportPoint = vec3(0.0, -m_halfHeight, 0.0); supportPoint = vec3(0.0, -m_halfHeight, 0.0);
} }
} }
return supportPoint; return supportPoint;
} }
// Raycast method with feedback information
// This implementation is based on the technique described by David Eberly in the article
// "Intersection of a Line and a Cone" that can be found at
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const vec3 r = ray.point2 - ray.point1; const vec3 r = ray.point2 - ray.point1;
const float epsilon = float(0.00001); const float epsilon = float(0.00001);
vec3 V(0, m_halfHeight, 0); vec3 V(0, m_halfHeight, 0);
vec3 centerBase(0, -m_halfHeight, 0); vec3 centerBase(0, -m_halfHeight, 0);
@ -70,56 +50,46 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius); float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius);
float factor = 1.0f - cosThetaSquare; float factor = 1.0f - cosThetaSquare;
vec3 delta = ray.point1 - V; vec3 delta = ray.point1 - V;
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
cosThetaSquare * delta.z() * delta.z();
float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z(); float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z(); float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
float tHit[] = {float(-1.0), float(-1.0), float(-1.0)}; float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
vec3 localHitPoint[3]; vec3 localHitPoint[3];
vec3 localNormal[3]; vec3 localNormal[3];
// If c2 is different from zero // If c2 is different from zero
if (etk::abs(c2) > FLT_EPSILON) { if (etk::abs(c2) > FLT_EPSILON) {
float gamma = c1 * c1 - c0 * c2; float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation // If there is no real roots in the quadratic equation
if (gamma < 0.0f) { if (gamma < 0.0f) {
return false; return false;
} } else if (gamma > 0.0f) { // The equation has two real roots
else if (gamma > 0.0f) { // The equation has two real roots
// Compute two int32_tersections // Compute two int32_tersections
float sqrRoot = etk::sqrt(gamma); float sqrRoot = etk::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2; tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2;
} } else { // If the equation has a single real root
else { // If the equation has a single real root
// Compute the int32_tersection // Compute the int32_tersection
tHit[0] = -c1 / c2; tHit[0] = -c1 / c2;
} }
} } else {
else { // If c2 == 0 // If c2 == 0
// If c2 = 0 and c1 != 0
if (etk::abs(c1) > FLT_EPSILON) { if (etk::abs(c1) > FLT_EPSILON) {
// If c2 = 0 and c1 != 0
tHit[0] = -c0 / (float(2.0) * c1); tHit[0] = -c0 / (float(2.0) * c1);
} } else {
else { // If c2 = c1 = 0 // If c2 = c1 = 0
// If c0 is different from zero, no solution and if c0 = 0, we have a // If c0 is different from zero, no solution and if c0 = 0, we have a
// degenerate case, the whole ray is contained in the cone side // degenerate case, the whole ray is contained in the cone side
// but we return no hit in this case // but we return no hit in this case
return false; return false;
} }
} }
// If the origin of the ray is inside the cone, we return no hit // If the origin of the ray is inside the cone, we return no hit
if (testPointInside(ray.point1, NULL)) return false; if (testPointInside(ray.point1, NULL)) {
return false;
}
localHitPoint[0] = ray.point1 + tHit[0] * r; localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * r; localHitPoint[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are int32_terested in) // Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < 0.0f) { if (axis.dot(localHitPoint[0] - V) < 0.0f) {
tHit[0] = float(-1.0); tHit[0] = float(-1.0);
@ -127,7 +97,6 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (axis.dot(localHitPoint[1] - V) < 0.0f) { if (axis.dot(localHitPoint[1] - V) < 0.0f) {
tHit[1] = float(-1.0); tHit[1] = float(-1.0);
} }
// Only keep hit points that are within the correct height of the cone // Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y() < float(-m_halfHeight)) { if (localHitPoint[0].y() < float(-m_halfHeight)) {
tHit[0] = float(-1.0); tHit[0] = float(-1.0);
@ -135,43 +104,40 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (localHitPoint[1].y() < float(-m_halfHeight)) { if (localHitPoint[1].y() < float(-m_halfHeight)) {
tHit[1] = float(-1.0); tHit[1] = float(-1.0);
} }
// If the ray is in direction of the base plane of the cone // If the ray is in direction of the base plane of the cone
if (r.y() > epsilon) { if (r.y() > epsilon) {
// Compute the int32_tersection with the base plane of the cone // Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y()); tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y());
// Only keep this int32_tersection if it is inside the cone radius // Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r; localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) { if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) {
tHit[2] = float(-1.0); tHit[2] = float(-1.0);
} }
// Compute the normal direction // Compute the normal direction
localNormal[2] = axis; localNormal[2] = axis;
} }
// Find the smallest positive t value // Find the smallest positive t value
int32_t hitIndex = -1; int32_t hitIndex = -1;
float t = FLT_MAX; float t = FLT_MAX;
for (int32_t i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
if (tHit[i] < 0.0f) continue; if (tHit[i] < 0.0f) {
continue;
}
if (tHit[i] < t) { if (tHit[i] < t) {
hitIndex = i; hitIndex = i;
t = tHit[hitIndex]; t = tHit[hitIndex];
} }
} }
if (hitIndex < 0) {
if (hitIndex < 0) return false; return false;
if (t > ray.maxFraction) return false; }
if (t > ray.maxFraction) {
return false;
}
// Compute the normal direction for hit against side of the cone // Compute the normal direction for hit against side of the cone
if (hitIndex != 2) { if (hitIndex != 2) {
float h = float(2.0) * m_halfHeight; float h = float(2.0) * m_halfHeight;
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float rOverH = m_radius / h; float rOverH = m_radius / h;
float value2 = 1.0f + rOverH * rOverH; float value2 = 1.0f + rOverH * rOverH;
float factor = 1.0f / etk::sqrt(value1 * value2); float factor = 1.0f / etk::sqrt(value1 * value2);
@ -181,82 +147,56 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH); localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z); localNormal[hitIndex].setZ(z);
} }
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex]; raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex]; raycastInfo.worldNormal = localNormal[hitIndex];
return true; return true;
} }
// Return the radius
/**
* @return Radius of the cone (in meters)
*/
float ConeShape::getRadius() const { float ConeShape::getRadius() const {
return m_radius; return m_radius;
} }
// Return the height
/**
* @return Height of the cone (in meters)
*/
float ConeShape::getHeight() const { float ConeShape::getHeight() const {
return float(2.0) * m_halfHeight; return float(2.0) * m_halfHeight;
} }
// Set the scaling vector of the collision shape
void ConeShape::setLocalScaling(const vec3& scaling) { void ConeShape::setLocalScaling(const vec3& scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
m_radius = (m_radius / m_scaling.x()) * scaling.x(); m_radius = (m_radius / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
// Return the number of bytes used by the collision shape
size_t ConeShape::getSizeInBytes() const { size_t ConeShape::getSizeInBytes() const {
return sizeof(ConeShape); return sizeof(ConeShape);
} }
// Return the local bounds of the shape in x, y and z directions
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void ConeShape::getLocalBounds(vec3& min, vec3& max) const { void ConeShape::getLocalBounds(vec3& min, vec3& max) const {
// Maximum bounds // Maximum bounds
max.setX(m_radius + m_margin); max.setX(m_radius + m_margin);
max.setY(m_halfHeight + m_margin); max.setY(m_halfHeight + m_margin);
max.setZ(max.x()); max.setZ(max.x());
// Minimum bounds // Minimum bounds
min.setX(-max.x()); min.setX(-max.x());
min.setY(-max.y()); min.setY(-max.y());
min.setZ(min.x()); min.setZ(min.x());
} }
// Return the local inertia tensor of the collision shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float rSquare = m_radius * m_radius; float rSquare = m_radius * m_radius;
float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight);
tensor.setValue(diagXZ, 0.0, 0.0, tensor.setValue(diagXZ, 0.0, 0.0,
0.0, float(0.3) * mass * rSquare, 0.0, float(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ); 0.0, 0.0, 0.0, diagXZ);
} }
// Return true if a point is inside the collision shape
bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
const float radiusHeight = m_radius * (-localPoint.y() + m_halfHeight) / const float radiusHeight = m_radius
(m_halfHeight * float(2.0)); * (-localPoint.y() + m_halfHeight)
return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) && / (m_halfHeight * float(2.0));
(localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); return ( localPoint.y() < m_halfHeight
&& localPoint.y() > -m_halfHeight)
&& (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
} }

30
ephysics/collision/shapes/ConeShape.hpp
View File

@ -28,24 +28,36 @@ namespace ephysics {
* default margin distance by not using the "margin" parameter in the constructor. * default margin distance by not using the "margin" parameter in the constructor.
*/ */
class ConeShape : public ConvexShape { class ConeShape : public ConvexShape {
public :
/**
* @brief Constructor
* @param _radius Radius of the cone (in meters)
* @param _height Height of the cone (in meters)
* @param _margin Collision margin (in meters) around the collision shape
*/
ConeShape(float _radius, float _height, float _margin = OBJECT_MARGIN);
/// DELETE copy-constructor
ConeShape(const ConeShape& _shape) = delete;
/// DELETE assignment operator
ConeShape& operator=(const ConeShape& _shape) = delete;
protected : protected :
float m_radius; //!< Radius of the base float m_radius; //!< Radius of the base
float m_halfHeight; //!< Half height of the cone float m_halfHeight; //!< Half height of the cone
float m_sinTheta; //!< sine of the semi angle at the apex point float m_sinTheta; //!< sine of the semi angle at the apex point
/// Private copy-constructor
ConeShape(const ConeShape& _shape) = delete;
/// Private assignment operator
ConeShape& operator=(const ConeShape& _shape) = delete;
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override; virtual vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override; bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override; size_t getSizeInBytes() const override;
public : public:
/// Constructor /**
ConeShape(float _radius, float _height, float _margin = OBJECT_MARGIN); * @brief Return the radius
/// Return the radius * @return Radius of the cone (in meters)
*/
float getRadius() const; float getRadius() const;
/// Return the height /**
* @brief Return the height
* @return Height of the cone (in meters)
*/
float getHeight() const; float getHeight() const;
void setLocalScaling(const vec3& _scaling) override; void setLocalScaling(const vec3& _scaling) override;