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
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
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)
// during the last simulation step
addMovedCollisionShape(proxyShape->m_broadPhaseID);
addMovedCollisionShape(_proxyShape->m_broadPhaseID);
}
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int32_t broadPhaseID = proxyShape->m_broadPhaseID;
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* _proxyShape) {
int32_t broadPhaseID = _proxyShape->m_broadPhaseID;
// Remove the collision shape from the dynamic AABB tree
m_dynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape int32_to the array of shapes that have moved (or have been created)
// during the last simulation step
removeMovedCollisionShape(broadPhaseID);
@ -127,18 +121,17 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// 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 (it == nodeId) {
if (it == _nodeId) {
return;
}
// 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
m_movedShapes.clear();
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
m_potentialPairs.sort(0,
m_potentialPairs.size()-1,
@ -151,7 +144,6 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
}
return false;
});
// Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs
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);
// 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
if ((m_raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, ray);
hitFraction = m_raycastTest.raycastAgainstShape(proxyShape, _ray);
}
return hitFraction;
}
@ -202,7 +189,6 @@ bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* _shape1,
// Get the two AABBs of the collision shapes
const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID);
const AABB& aabb2 = m_dynamicAABBTree.getFatAABB(_shape2->m_broadPhaseID);
// Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2);
}

78
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* narrowPhaseCallback) {
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& _shape1Info,
const CollisionShapeInfo& _shape2Info,
NarrowPhaseCallback* _callback) {
ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape;
const ConvexShape* convexShape;
const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
if (_shape1Info.collisionShape->isConvex()) {
convexProxyShape = _shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(_shape1Info.collisionShape);
concaveProxyShape = _shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(_shape2Info.collisionShape);
} else {
// Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
convexProxyShape = _shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(_shape2Info.collisionShape);
concaveProxyShape = _shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(_shape1Info.collisionShape);
}
// Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback;
@ -48,7 +44,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape);
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
AABB aabb;
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
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
processSmoothMeshCollision(_shape1Info.overlappingPair, contactPoints, _callback);
} else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
convexVsTriangleCallback.setNarrowPhaseCallback(_callback);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
}
}
void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) {
void ConvexVsTriangleCallback::testTriangle(const vec3* _trianglePoints) {
// Create a triangle collision shape
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
NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(),
m_convexShape->getType());
NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(), m_convexShape->getType());
// If there is no collision algorithm between those two kinds of shapes
if (algo == nullptr) {
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
algo->setCurrentOverlappingPair(m_overlappingPair);
// Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(m_convexProxyShape, m_convexShape, m_convexProxyShape->getLocalToWorldTransform(),
m_overlappingPair, m_convexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape, &triangleShape,
CollisionShapeInfo shapeConvexInfo(m_convexProxyShape,
m_convexShape,
m_convexProxyShape->getLocalToWorldTransform(),
m_overlappingPair,
m_convexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape,
&triangleShape,
m_concaveProxyShape->getLocalToWorldTransform(),
m_overlappingPair, m_concaveProxyShape->getCachedCollisionData());
m_overlappingPair,
m_concaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, m_narrowPhaseCallback);
}
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
etk::Vector<SmoothMeshContactInfo> contactPoints,
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* _overlappingPair,
etk::Vector<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
contactPoints.sort(0,
contactPoints.size()-1,
_contactPoints.sort(0,
_contactPoints.size()-1,
[](const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) {
return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth;
});
// For each contact point (from smaller penetration depth to larger)
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 vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// 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
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
_callback->notifyContact(_overlappingPair, info.contactInfo);
}
} else if (nbZeros == 1) {
// If it is an edge contact
@ -142,7 +142,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
_callback->notifyContact(_overlappingPair, info.contactInfo);
}
} else {
// If it is a face contact
@ -150,11 +150,11 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
ProxyShape* firstShape;
ProxyShape* secondShape;
if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2();
firstShape = _overlappingPair->getShape1();
secondShape = _overlappingPair->getShape2();
} else {
firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1();
firstShape = _overlappingPair->getShape2();
secondShape = _overlappingPair->getShape1();
}
// We use the triangle normal as the contact normal
vec3 a = info.triangleVertices[1] - info.triangleVertices[0];
@ -179,7 +179,7 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
}
// Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
_callback->notifyContact(_overlappingPair, newContactInfo);
}
// Add the three vertices of the triangle to the set of processed
// triangle vertices

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

@ -138,8 +138,6 @@ namespace ephysics {
public :
/// Constructor
ConcaveVsConvexAlgorithm();
/// Destructor
virtual ~ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& _shape1Info,
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);
}
bool AABB::testCollision(const AABB& aabb) const {
if (m_maxCoordinates.x() < aabb.m_minCoordinates.x() ||
aabb.m_maxCoordinates.x() < m_minCoordinates.x()) return false;
if (m_maxCoordinates.y() < aabb.m_minCoordinates.y() ||
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;
bool AABB::testCollision(const AABB& _aabb) const {
if ( m_maxCoordinates.x() < _aabb.m_minCoordinates.x()
|| _aabb.m_maxCoordinates.x() < m_minCoordinates.x()) {
return false;
}
if ( m_maxCoordinates.y() < _aabb.m_minCoordinates.y()
|| _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;
}

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

@ -14,11 +14,6 @@
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):
ConvexShape(BOX, _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);
}
// 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 BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass;
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);
float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y();
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, 0.0, factor * (xSquare + ySquare));
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
vec3 rayDirection = ray.point2 - ray.point1;
bool BoxShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
vec3 rayDirection = _ray.point2 - _ray.point1;
float tMin = FLT_MIN;
float tMax = FLT_MAX;
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 (etk::abs(rayDirection[iii]) < FLT_EPSILON) {
// 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;
}
} else {
// Compute the intersection of the ray with the near and far plane of the slab
float oneOverD = 1.0f / rayDirection[iii];
float t1 = (-m_extent[iii] - ray.point1[iii]) * oneOverD;
float t2 = (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;
currentNormal[0] = (iii == 0) ? -m_extent[iii] : 0.0f;
currentNormal[1] = (iii == 1) ? -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);
// If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) {
if (tMin > _ray.maxFraction) {
return false;
}
// 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 < 0.0f
|| tMin > ray.maxFraction) {
|| tMin > _ray.maxFraction) {
return false;
}
if (normalDirection == vec3(0,0,0)) {
return false;
}
// The ray int32_tersects the three slabs, we compute the hit point
vec3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localHitPoint;
raycastInfo.worldNormal = normalDirection;
vec3 localHitPoint = _ray.point1 + tMin * rayDirection;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = tMin;
_raycastInfo.worldPoint = localHitPoint;
_raycastInfo.worldNormal = normalDirection;
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 {
return m_extent + vec3(m_margin, m_margin, m_margin);
}
// Set the scaling vector of the collision shape
void BoxShape::setLocalScaling(const vec3& scaling) {
m_extent = (m_extent / m_scaling) * scaling;
CollisionShape::setLocalScaling(scaling);
void BoxShape::setLocalScaling(const vec3& _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 {
// Maximum bounds
_max = m_extent + vec3(m_margin, m_margin, m_margin);
// Minimum bounds
_min = -_max;
}
// Return the number of bytes used by the collision shape
size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);
}
// Return a local support point in a given direction without the objec margin
vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction,
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(),
direction.z() < 0.0 ? -m_extent.z() : m_extent.z());
vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
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(),
_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 {
return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] &&
localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] &&
localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]);
bool BoxShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
return ( _localPoint.x() < m_extent[0]
&& _localPoint.x() > -m_extent[0]
&& _localPoint.y() < m_extent[1]
&& _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.
*/
class BoxShape : public ConvexShape {
protected :
vec3 m_extent; //!< Extent sizes of the box in the x, y and z direction
/// Private copy-constructor
BoxShape(const BoxShape& _shape) = delete;
/// Private assignment operator
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
public:
/**
* @brief 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(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;
void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) 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;
};
}

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

@ -12,11 +12,6 @@
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):
ConvexShape(CAPSULE, _radius),
m_halfHeight(_height * 0.5f) {
@ -24,21 +19,8 @@ CapsuleShape::CapsuleShape(float _radius, float _height):
assert(_height > 0.0f);
}
// Destructor
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 {
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
float height = m_halfHeight + m_halfHeight;
float radiusSquare = m_margin * m_margin;
float heightSquare = height * height;
@ -48,292 +30,231 @@ void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass)
float sum1 = float(0.4) * radiusSquareDouble;
float sum2 = float(0.75) * height * m_margin + 0.5f * heightSquare;
float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
tensor.setValue(IxxAndzz, 0.0, 0.0,
float IxxAndzz = factor1 * _mass * (sum1 + sum2) + factor2 * _mass * sum3;
float Iyy = factor1 * _mass * sum1 + factor2 * _mass * float(0.25) * radiusSquareDouble;
_tensor.setValue(IxxAndzz, 0.0, 0.0,
0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz);
}
// Return true if a point is inside the collision shape
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 xSquare = localPoint.x() * localPoint.x();
const float zSquare = localPoint.z() * localPoint.z();
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 xSquare = _localPoint.x() * _localPoint.x();
const float zSquare = _localPoint.z() * _localPoint.z();
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 ((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 + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const vec3 n = ray.point2 - ray.point1;
bool CapsuleShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
const vec3 n = _ray.point2 - _ray.point1;
const float epsilon = float(0.01);
vec3 p(float(0), -m_halfHeight, float(0));
vec3 q(float(0), m_halfHeight, float(0));
vec3 d = q - p;
vec3 m = ray.point1 - p;
vec3 m = _ray.point1 - p;
float t;
float mDotD = m.dot(d);
float nDotD = n.dot(d);
float dDotD = d.dot(d);
// Test if the segment is outside the cylinder
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;
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;
}
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 mDotN = m.dot(n);
float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - m_margin * m_margin;
float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis
if (etk::abs(a) < epsilon) {
// 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
// If the ray int32_tersects with the "p" endcap of the capsule
if (mDotD < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = hitFraction;
_raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
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
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = hitFraction;
_raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
return false;
}
else { // If the origin is inside the cylinder, we return no hit
} else {
// If the origin is inside the cylinder, we return no hit
return false;
}
}
float b = dDotD * mDotN - nDotD * mDotD;
float discriminant = b * b - a * c;
// 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)
float t0 = t = (-b - etk::sqrt(discriminant)) / a;
// If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD;
if (value < 0.0f) {
// Check int32_tersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = hitFraction;
_raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
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
vec3 hitLocalPoint;
float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint;
if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = hitFraction;
_raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
return false;
}
t = t0;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// 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
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized();
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
// Raycasting method between a ray one of the two spheres end cap of the capsule
bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point2,
const vec3& sphereCenter, float maxFraction,
vec3& hitLocalPoint, float& hitFraction) const {
const vec3 m = point1 - sphereCenter;
bool CapsuleShape::raycastWithSphereEndCap(const vec3& _point1,
const vec3& _point2,
const vec3& _sphereCenter,
float _maxFraction,
vec3& _hitLocalPoint,
float& _hitFraction) const {
const vec3 m = _point1 - _sphereCenter;
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 (c < 0.0f) return false;
const vec3 rayDirection = point2 - point1;
if (c < 0.0f) {
return false;
}
const vec3 rayDirection = _point2 - _point1;
float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// 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();
// Compute the discriminant of the quadratic equation
float discriminant = b * b - raySquareLength * c;
// 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
float t = -b - etk::sqrt(discriminant);
assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) {
if (t < _maxFraction * raySquareLength) {
// Compute the int32_tersection information
t /= raySquareLength;
hitFraction = t;
hitLocalPoint = point1 + t * rayDirection;
_hitFraction = t;
_hitLocalPoint = _point1 + t * rayDirection;
return true;
}
return false;
}
// Get the radius of the capsule
/**
* @return The radius of the capsule shape (in meters)
*/
float CapsuleShape::getRadius() const {
return m_margin;
}
// Return the height of the capsule
/**
* @return The height of the capsule shape (in meters)
*/
float CapsuleShape::getHeight() const {
return m_halfHeight + m_halfHeight;
}
// Set the scaling vector of the collision shape
void CapsuleShape::setLocalScaling(const vec3& scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
m_margin = (m_margin / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(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();
CollisionShape::setLocalScaling(_scaling);
}
// Return the number of bytes used by the collision shape
size_t CapsuleShape::getSizeInBytes() const {
return sizeof(CapsuleShape);
}
// 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 CapsuleShape::getLocalBounds(vec3& min, vec3& max) const {
void CapsuleShape::getLocalBounds(vec3& _min, vec3& _max) const {
// Maximum bounds
max.setX(m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(m_margin);
_max.setX(m_margin);
_max.setY(m_halfHeight + m_margin);
_max.setZ(m_margin);
// Minimum bounds
min.setX(-m_margin);
min.setY(-max.y());
min.setZ(min.x());
_min.setX(-m_margin);
_min.setY(-_max.y());
_min.setZ(_min.x());
}
// Return a local support point in a given direction without the object margin.
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
/// 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 {
vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
void** _cachedCollisionData) const {
// Support point top sphere
float dotProductTop = m_halfHeight * direction.y();
float dotProductTop = m_halfHeight * _direction.y();
// Support point bottom sphere
float dotProductBottom = -m_halfHeight * direction.y();
float dotProductBottom = -m_halfHeight * _direction.y();
// Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) {
return vec3(0, m_halfHeight, 0);
}
else {
return vec3(0, -m_halfHeight, 0);
}
}

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

@ -23,31 +23,44 @@ namespace ephysics {
* capsule shape.
*/
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 :
/// 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);
/// Destructor
virtual ~CapsuleShape();
/// Return the radius of the capsule
/// DELETE copy-constructor
CapsuleShape(const CapsuleShape& _shape) = delete;
/// 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;
/// 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;
void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) 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;
};
}

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

@ -19,50 +19,35 @@ CollisionShape::CollisionShape(CollisionShapeType type) :
}
CollisionShape::~CollisionShape() {
}
// 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 {
void CollisionShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const {
PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction
vec3 minBounds(0,0,0);
vec3 maxBounds(0,0,0);
getLocalBounds(minBounds, maxBounds);
// 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),
worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds));
vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds));
// Compute the minimum and maximum coordinates of the rotated extents
vec3 minCoordinates = transform.getPosition() + worldMinBounds;
vec3 maxCoordinates = transform.getPosition() + worldMaxBounds;
vec3 minCoordinates = _transform.getPosition() + worldMinBounds;
vec3 maxCoordinates = _transform.getPosition() + worldMaxBounds;
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates);
_aabb.setMin(minCoordinates);
_aabb.setMax(maxCoordinates);
}
int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) {
int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType _shapeType1,
CollisionShapeType _shapeType2) {
// If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) {
if (isConvex(_shapeType1) && isConvex(_shapeType2)) {
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.
*/
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 :
/// Constructor
CollisionShape(CollisionShapeType _type);
/// Destructor
virtual ~CollisionShape();
/// DELETE copy-constructor
CollisionShape(const CollisionShape& shape) = delete;
/// DELETE assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/**
* @brief Get the type of the collision shapes
* @return The type of the collision shape (box, sphere, cylinder, ...)
@ -106,6 +95,15 @@ class CollisionShape {
CollisionShapeType _shapeType2);
friend class ProxyShape;
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 {
EPHY_ASSERT(_outTriangleVertices != nullptr, "Input check error");
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart);
if (triangleVertexArray == nullptr) {

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

@ -17,7 +17,7 @@
namespace ephysics {
class ConcaveMeshShape;
class ConcaveMeshRaycastCallback {
private :
private:
etk::Vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree;
const ConcaveMeshShape& m_concaveMeshShape;
@ -55,13 +55,22 @@ namespace ephysics {
* this shape for a static mesh.
*/
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:
TriangleMesh* m_triangleMesh; //!< Triangle mesh
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 size_t getSizeInBytes() const override;
/// Insert all the triangles int32_to the dynamic AABB tree
@ -71,16 +80,6 @@ namespace ephysics {
void getTriangleVerticesWithIndexPointer(int32_t _subPart,
int32_t _triangleIndex,
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;
};
}

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

@ -13,54 +13,38 @@
using namespace ephysics;
// Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type)
: CollisionShape(type), m_isSmoothMeshCollisionEnabled(false),
m_triangleMargin(0), m_raycastTestType(FRONT) {
ConcaveShape::ConcaveShape(CollisionShapeType _type):
CollisionShape(_type),
m_isSmoothMeshCollisionEnabled(false),
m_triangleMargin(0),
m_raycastTestType(FRONT) {
}
// Destructor
ConcaveShape::~ConcaveShape() {
}
// Return the triangle margin
float ConcaveShape::getTriangleMargin() const {
return m_triangleMargin;
}
/// Return true if the collision shape is convex, false if it is concave
bool ConcaveShape::isConvex() const {
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 true if the smooth mesh collision is enabled
bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
return m_isSmoothMeshCollisionEnabled;
}
// 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 ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
m_isSmoothMeshCollisionEnabled = isEnabled;
void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool _isEnabled) {
m_isSmoothMeshCollisionEnabled = _isEnabled;
}
// Return the raycast test type (front, back, front-back)
TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
return m_raycastTestType;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
m_raycastTestType = testType;
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.
*/
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 :
/// Constructor
ConcaveShape(CollisionShapeType _type);
/// Destructor
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
float getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back)
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);
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const override;
@ -54,7 +58,12 @@ namespace ephysics {
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
/**
* @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);
};

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

@ -12,56 +12,36 @@
using namespace ephysics;
// 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::ConeShape(float radius, float height, float margin):
ConvexShape(CONE, margin),
m_radius(radius),
m_halfHeight(height * 0.5f) {
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_halfHeight > 0.0f);
// 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 {
const vec3& v = direction;
vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
const vec3& v = _direction;
float sinThetaTimesLengthV = m_sinTheta * v.length();
vec3 supportPoint;
if (v.y() > sinThetaTimesLengthV) {
supportPoint = vec3(0.0, m_halfHeight, 0.0);
}
else {
} else {
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
if (projectedLength > FLT_EPSILON) {
float d = m_radius / projectedLength;
supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d);
}
else {
} else {
supportPoint = vec3(0.0, -m_halfHeight, 0.0);
}
}
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 {
const vec3 r = ray.point2 - ray.point1;
const float epsilon = float(0.00001);
vec3 V(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 factor = 1.0f - cosThetaSquare;
vec3 delta = ray.point1 - V;
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() -
cosThetaSquare * delta.z() * delta.z();
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.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 tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
vec3 localHitPoint[3];
vec3 localNormal[3];
// If c2 is different from zero
if (etk::abs(c2) > FLT_EPSILON) {
float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation
if (gamma < 0.0f) {
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
float sqrRoot = etk::sqrt(gamma);
tHit[0] = (-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
tHit[0] = -c1 / c2;
}
}
else { // If c2 == 0
// If c2 = 0 and c1 != 0
} else {
// If c2 == 0
if (etk::abs(c1) > FLT_EPSILON) {
// If c2 = 0 and c1 != 0
tHit[0] = -c0 / (float(2.0) * c1);
}
else { // If c2 = c1 = 0
} else {
// If c2 = c1 = 0
// 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
// but we return no hit in this case
return false;
}
}
// 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[1] = ray.point1 + tHit[1] * r;
// 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) {
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) {
tHit[1] = float(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y() < float(-m_halfHeight)) {
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)) {
tHit[1] = float(-1.0);
}
// If the ray is in direction of the base plane of the cone
if (r.y() > epsilon) {
// Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y());
// Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) {
tHit[2] = float(-1.0);
}
// Compute the normal direction
localNormal[2] = axis;
}
// Find the smallest positive t value
int32_t hitIndex = -1;
float t = FLT_MAX;
for (int32_t i=0; i<3; i++) {
if (tHit[i] < 0.0f) continue;
if (tHit[i] < 0.0f) {
continue;
}
if (tHit[i] < t) {
hitIndex = i;
t = tHit[hitIndex];
}
}
if (hitIndex < 0) return false;
if (t > ray.maxFraction) return false;
if (hitIndex < 0) {
return false;
}
if (t > ray.maxFraction) {
return false;
}
// Compute the normal direction for hit against side of the cone
if (hitIndex != 2) {
float h = float(2.0) * m_halfHeight;
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() +
localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float rOverH = m_radius / h;
float value2 = 1.0f + rOverH * rOverH;
float factor = 1.0f / etk::sqrt(value1 * value2);
@ -181,70 +147,43 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z);
}
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex];
return true;
}
// Return the radius
/**
* @return Radius of the cone (in meters)
*/
float ConeShape::getRadius() const {
return m_radius;
}
// Return the height
/**
* @return Height of the cone (in meters)
*/
float ConeShape::getHeight() const {
return float(2.0) * m_halfHeight;
}
// Set the scaling vector of the collision shape
void ConeShape::setLocalScaling(const vec3& scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
m_radius = (m_radius / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
size_t ConeShape::getSizeInBytes() const {
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 {
// Maximum bounds
max.setX(m_radius + m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(max.x());
// Minimum bounds
min.setX(-max.x());
min.setY(-max.y());
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 {
float rSquare = m_radius * m_radius;
float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight);
@ -253,10 +192,11 @@ void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) co
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 {
const float radiusHeight = m_radius * (-localPoint.y() + m_halfHeight) /
(m_halfHeight * float(2.0));
return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) &&
(localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
const float radiusHeight = m_radius
* (-localPoint.y() + m_halfHeight)
/ (m_halfHeight * float(2.0));
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.
*/
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 :
float m_radius; //!< Radius of the base
float m_halfHeight; //!< Half height of the cone
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;
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
ConeShape(float _radius, float _height, float _margin = OBJECT_MARGIN);
/// Return the radius
public:
/**
* @brief Return the radius
* @return Radius of the cone (in meters)
*/
float getRadius() const;
/// Return the height
/**
* @brief Return the height
* @return Height of the cone (in meters)
*/
float getHeight() const;
void setLocalScaling(const vec3& _scaling) override;