diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index d3af59c..06b920e 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -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(m_dynamicAABBTree.getNodeDataPointer(nodeId)); - + ProxyShape* proxyShape = static_cast(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); } diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index d1f3c9b..caabc5c 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -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(shape1Info.collisionShape); - concaveProxyShape = shape2Info.proxyShape; - concaveShape = static_cast(shape2Info.collisionShape); + if (_shape1Info.collisionShape->isConvex()) { + convexProxyShape = _shape1Info.proxyShape; + convexShape = static_cast(_shape1Info.collisionShape); + concaveProxyShape = _shape2Info.proxyShape; + concaveShape = static_cast(_shape2Info.collisionShape); } else { // Collision shape 2 is convex, collision shape 1 is concave - convexProxyShape = shape2Info.proxyShape; - convexShape = static_cast(shape2Info.collisionShape); - concaveProxyShape = shape1Info.proxyShape; - concaveShape = static_cast(shape1Info.collisionShape); + convexProxyShape = _shape2Info.proxyShape; + convexShape = static_cast(_shape2Info.collisionShape); + concaveProxyShape = _shape1Info.proxyShape; + concaveShape = static_cast(_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, - m_concaveProxyShape->getLocalToWorldTransform(), - m_overlappingPair, m_concaveProxyShape->getCachedCollisionData()); + 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()); // 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 contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { +void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* _overlappingPair, + etk::Vector _contactPoints, + NarrowPhaseCallback* narrowPhaseCallback) { // Set with the triangle vertices already processed to void further contacts with same triangle etk::Vector> 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::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 @@ -214,7 +214,7 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const etk::Vector 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, - 0.0, factor * (xSquare + zSquare), 0.0, - 0.0, 0.0, factor * (xSquare + ySquare)); + _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]); } diff --git a/ephysics/collision/shapes/BoxShape.hpp b/ephysics/collision/shapes/BoxShape.hpp index 2920e2e..383202e 100644 --- a/ephysics/collision/shapes/BoxShape.hpp +++ b/ephysics/collision/shapes/BoxShape.hpp @@ -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; }; } diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index aa355ed..75a89ef 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -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, - 0.0, Iyy, 0.0, - 0.0, 0.0, IxxAndzz); + 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); - } -} \ No newline at end of file + return vec3(0, -m_halfHeight, 0); +} diff --git a/ephysics/collision/shapes/CapsuleShape.hpp b/ephysics/collision/shapes/CapsuleShape.hpp index 6f7c86e..e082bb9 100644 --- a/ephysics/collision/shapes/CapsuleShape.hpp +++ b/ephysics/collision/shapes/CapsuleShape.hpp @@ -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; }; } diff --git a/ephysics/collision/shapes/CollisionShape.cpp b/ephysics/collision/shapes/CollisionShape.cpp index 1b4f8c1..c680e08 100644 --- a/ephysics/collision/shapes/CollisionShape.cpp +++ b/ephysics/collision/shapes/CollisionShape.cpp @@ -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; } -} \ No newline at end of file + // If there is at least one concave shape + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; +} + diff --git a/ephysics/collision/shapes/CollisionShape.hpp b/ephysics/collision/shapes/CollisionShape.hpp index d3df785..721b09d 100644 --- a/ephysics/collision/shapes/CollisionShape.hpp +++ b/ephysics/collision/shapes/CollisionShape.hpp @@ -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; }; diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index 986439e..6ee14f1 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -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) { diff --git a/ephysics/collision/shapes/ConcaveMeshShape.hpp b/ephysics/collision/shapes/ConcaveMeshShape.hpp index ed28e04..38bf11f 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.hpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.hpp @@ -17,7 +17,7 @@ namespace ephysics { class ConcaveMeshShape; class ConcaveMeshRaycastCallback { - private : + private: etk::Vector 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; }; } diff --git a/ephysics/collision/shapes/ConcaveShape.cpp b/ephysics/collision/shapes/ConcaveShape.cpp index df91b6d..e107c5f 100644 --- a/ephysics/collision/shapes/ConcaveShape.cpp +++ b/ephysics/collision/shapes/ConcaveShape.cpp @@ -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; } diff --git a/ephysics/collision/shapes/ConcaveShape.hpp b/ephysics/collision/shapes/ConcaveShape.hpp index 4e05040..1cf75de 100644 --- a/ephysics/collision/shapes/ConcaveShape.hpp +++ b/ephysics/collision/shapes/ConcaveShape.hpp @@ -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); }; diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 5b18477..24e713b 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -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,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].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); tensor.setValue(diagXZ, 0.0, 0.0, - 0.0, float(0.3) * mass * rSquare, - 0.0, 0.0, 0.0, diagXZ); + 0.0, float(0.3) * mass * rSquare, + 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); } diff --git a/ephysics/collision/shapes/ConeShape.hpp b/ephysics/collision/shapes/ConeShape.hpp index e800d1a..fdd7a58 100644 --- a/ephysics/collision/shapes/ConeShape.hpp +++ b/ephysics/collision/shapes/ConeShape.hpp @@ -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;