From 696a979395e27cc415ed46c5e0d4d81a73c1c1f2 Mon Sep 17 00:00:00 2001 From: Edouard DUPIN Date: Fri, 18 May 2018 22:17:49 +0200 Subject: [PATCH] [DEV] continue rework --- ephysics/collision/ContactManifold.cpp | 115 +++++++----------------- ephysics/collision/ContactManifold.hpp | 50 ++++++++--- ephysics/collision/shapes/ConeShape.cpp | 70 +++++++-------- 3 files changed, 104 insertions(+), 131 deletions(-) diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index 95c0787..5004cc5 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -10,7 +10,6 @@ using namespace ephysics; -// Constructor ContactManifold::ContactManifold(ProxyShape* _shape1, ProxyShape* _shape2, short _normalDirectionId): @@ -25,21 +24,16 @@ ContactManifold::ContactManifold(ProxyShape* _shape1, } -// Destructor ContactManifold::~ContactManifold() { clear(); } -// Add a contact point in the manifold void ContactManifold::addContactPoint(ContactPoint* contact) { - // For contact already in the manifold for (uint32_t i=0; igetWorldPointOnBody1() - - contact->getWorldPointOnBody1()).length2(); + float distance = (m_contactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2(); if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { // Delete the new contact ETK_DELETE(ContactPoint, contact); @@ -47,26 +41,21 @@ void ContactManifold::addContactPoint(ContactPoint* contact) { return; } } - // If the contact manifold is full if (m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact); int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); removeContactPoint(indexToRemove); } - // Add the new contact point in the manifold m_contactPoints[m_nbContactPoints] = contact; m_nbContactPoints++; - assert(m_nbContactPoints > 0); } -// Remove a contact point from the manifold void ContactManifold::removeContactPoint(uint32_t index) { assert(index < m_nbContactPoints); assert(m_nbContactPoints > 0); - // Call the destructor explicitly and tell the memory allocator that // the corresponding memory block is now free ETK_DELETE(ContactPoint, m_contactPoints[index]); @@ -75,156 +64,116 @@ void ContactManifold::removeContactPoint(uint32_t index) { if (index < m_nbContactPoints - 1) { m_contactPoints[index] = m_contactPoints[m_nbContactPoints - 1]; } - m_nbContactPoints--; } -// Update the contact manifold -/// First the world space coordinates of the current contacts in the manifold are recomputed from -/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts -/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also -/// the contacts with a too large distance between the contact points in the plane orthogonal to the -/// contact normal. void ContactManifold::update(const etk::Transform3D& transform1, const etk::Transform3D& transform2) { - - if (m_nbContactPoints == 0) return; - + if (m_nbContactPoints == 0) { + return; + } // Update the world coordinates and penetration depth of the contact points in the manifold for (uint32_t i=0; isetWorldPointOnBody1(transform1 * - m_contactPoints[i]->getLocalPointOnBody1()); - m_contactPoints[i]->setWorldPointOnBody2(transform2 * - m_contactPoints[i]->getLocalPointOnBody2()); - m_contactPoints[i]->setPenetrationDepth((m_contactPoints[i]->getWorldPointOnBody1() - - m_contactPoints[i]->getWorldPointOnBody2()).dot(m_contactPoints[i]->getNormal())); + m_contactPoints[i]->setWorldPointOnBody1(transform1 * m_contactPoints[i]->getLocalPointOnBody1()); + m_contactPoints[i]->setWorldPointOnBody2(transform2 * m_contactPoints[i]->getLocalPointOnBody2()); + m_contactPoints[i]->setPenetrationDepth((m_contactPoints[i]->getWorldPointOnBody1() - m_contactPoints[i]->getWorldPointOnBody2()).dot(m_contactPoints[i]->getNormal())); } - - const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * - PERSISTENT_CONTACT_DIST_THRESHOLD; - + const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD; // Remove the contact points that don't represent very well the contact manifold for (int32_t i=static_cast(m_nbContactPoints)-1; i>=0; i--) { assert(i < static_cast(m_nbContactPoints)); - // Compute the distance between contact points in the normal direction float distanceNormal = -m_contactPoints[i]->getPenetrationDepth(); - // If the contacts points are too far from each other in the normal direction if (distanceNormal > squarePersistentContactThreshold) { removeContactPoint(i); - } - else { + } else { // Compute the distance of the two contact points in the plane // orthogonal to the contact normal - vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + - m_contactPoints[i]->getNormal() * distanceNormal; + vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + m_contactPoints[i]->getNormal() * distanceNormal; vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - // If the orthogonal distance is larger than the valid distance // threshold, we remove the contact if (projDifference.length2() > squarePersistentContactThreshold) { removeContactPoint(i); } } - } + } } -// Return the index of the contact point with the larger penetration depth. -/// This corresponding contact will be kept in the cache. The method returns -1 is -/// the new contact is the deepest. int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); int32_t indexMaxPenetrationDepth = -1; float maxPenetrationDepth = newContact->getPenetrationDepth(); - // For each contact in the cache for (uint32_t i=0; igetPenetrationDepth() > maxPenetrationDepth) { maxPenetrationDepth = m_contactPoints[i]->getPenetrationDepth(); indexMaxPenetrationDepth = i; } } - // Return the index of largest penetration depth return indexMaxPenetrationDepth; } -// Return the index that will be removed. -/// The index of the contact point with the larger penetration -/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute -/// the different area and we want to keep the contacts with the largest area. The new point is also -/// kept. In order to compute the area of a quadrilateral, we use the formula : -/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that -/// when we compute this area, we do not calculate it exactly but we -/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, -/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library -/// by Erwin Coumans (http://wwww.bulletphysics.org). int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const { - assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - - float area0 = 0.0; // Area with contact 1,2,3 and newPoint - float area1 = 0.0; // Area with contact 0,2,3 and newPoint - float area2 = 0.0; // Area with contact 0,1,3 and newPoint - float area3 = 0.0; // Area with contact 0,1,2 and newPoint - + float area0 = 0.0f; // Area with contact 1,2,3 and newPoint + float area1 = 0.0f; // Area with contact 0,2,3 and newPoint + float area2 = 0.0f; // Area with contact 0,1,3 and newPoint + float area3 = 0.0f; // Area with contact 0,1,2 and newPoint if (indexMaxPenetration != 0) { // Compute the area vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1(); - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - - m_contactPoints[2]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1(); vec3 crossProduct = vector1.cross(vector2); area0 = crossProduct.length2(); } if (indexMaxPenetration != 1) { // Compute the area vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - - m_contactPoints[2]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1(); vec3 crossProduct = vector1.cross(vector2); area1 = crossProduct.length2(); } if (indexMaxPenetration != 2) { // Compute the area vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - - m_contactPoints[1]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1(); vec3 crossProduct = vector1.cross(vector2); area2 = crossProduct.length2(); } if (indexMaxPenetration != 3) { // Compute the area vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - - m_contactPoints[1]->getLocalPointOnBody1(); + vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1(); vec3 crossProduct = vector1.cross(vector2); area3 = crossProduct.length2(); } - // Return the index of the contact to remove return getMaxArea(area0, area1, area2, area3); } -// Return the index of maximum area int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const { if (area0 < area1) { if (area1 < area2) { - if (area2 < area3) return 3; - else return 2; + if (area2 < area3) { + return 3; + } else { + return 2; + } + } else { + if (area1 < area3) { + return 3; + } else { + return 1; + } } - else { - if (area1 < area3) return 3; - else return 1; - } - } - else { + } else { if (area0 < area2) { if (area2 < area3) return 3; else return 2; - } - else { + } else { if (area0 < area3) return 3; else return 0; } diff --git a/ephysics/collision/ContactManifold.hpp b/ephysics/collision/ContactManifold.hpp index 6da6757..749cc6f 100644 --- a/ephysics/collision/ContactManifold.hpp +++ b/ephysics/collision/ContactManifold.hpp @@ -49,6 +49,17 @@ namespace ephysics { * The new added point is always kept. */ class ContactManifold { + public: + /// Constructor + ContactManifold(ProxyShape* _shape1, + ProxyShape* _shape2, + int16_t _normalDirectionId); + /// Destructor + ~ContactManifold(); + /// DELETE copy-constructor + ContactManifold(const ContactManifold& _contactManifold) = delete; + /// DELETE assignment operator + ContactManifold& operator=(const ContactManifold& _contactManifold) = delete; private: ProxyShape* m_shape1; //!< Pointer to the first proxy shape of the contact ProxyShape* m_shape2; //!< Pointer to the second proxy shape of the contact @@ -62,27 +73,32 @@ namespace ephysics { float m_frictionTwistImpulse; //!< Twist friction constraint accumulated impulse vec3 m_rollingResistanceImpulse; //!< Accumulated rolling resistance impulse bool m_isAlreadyInIsland; //!< True if the contact manifold has already been added int32_to an island - /// Private copy-constructor - ContactManifold(const ContactManifold& _contactManifold) = delete; - /// Private assignment operator - ContactManifold& operator=(const ContactManifold& _contactManifold) = delete; /// Return the index of maximum area int32_t getMaxArea(float _area0, float _area1, float _area2, float _area3) const; - /// Return the index of the contact with the larger penetration depth. + /** + * @brief Return the index of the contact with the larger penetration depth. + * + * This corresponding contact will be kept in the cache. The method returns -1 is + * the new contact is the deepest. + */ int32_t getIndexOfDeepestPenetration(ContactPoint* _newContact) const; - /// Return the index that will be removed. + /** + * @brief Return the index that will be removed. + * The index of the contact point with the larger penetration + * depth is given as a parameter. This contact won't be removed. Given this contact, we compute + * the different area and we want to keep the contacts with the largest area. The new point is also + * kept. In order to compute the area of a quadrilateral, we use the formula : + * Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that + * when we compute this area, we do not calculate it exactly but we + * only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, + * this is only a guess that is faster to compute. This idea comes from the Bullet Physics library + * by Erwin Coumans (http://wwww.bulletphysics.org). int32_t getIndexToRemove(int32_t _indexMaxPenetration, const vec3& _newPoint) const; /// Remove a contact point from the manifold void removeContactPoint(uint32_t _index); /// Return true if the contact manifold has already been added int32_to an island bool isAlreadyInIsland() const; public: - /// Constructor - ContactManifold(ProxyShape* _shape1, - ProxyShape* _shape2, - int16_t _normalDirectionId); - /// Destructor - ~ContactManifold(); /// Return a pointer to the first proxy shape of the contact ProxyShape* getShape1() const; /// Return a pointer to the second proxy shape of the contact @@ -95,7 +111,15 @@ namespace ephysics { int16_t getNormalDirectionId() const; /// Add a contact point to the manifold void addContactPoint(ContactPoint* _contact); - /// Update the contact manifold. + /** + * @brief Update the contact manifold. + * + * First the world space coordinates of the current contacts in the manifold are recomputed from + * the corresponding transforms of the bodies because they have moved. Then we remove the contacts + * with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also + * the contacts with a too large distance between the contact points in the plane orthogonal to the + * contact normal. + */ void update(const etk::Transform3D& _transform1, const etk::Transform3D& _transform2); /// Clear the contact manifold diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 24e713b..8c82f35 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -40,8 +40,8 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** return supportPoint; } -bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - const vec3 r = ray.point2 - ray.point1; +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); @@ -49,7 +49,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr float heightSquare = float(4.0) * m_halfHeight * m_halfHeight; float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius); float factor = 1.0f - cosThetaSquare; - vec3 delta = ray.point1 - V; + vec3 delta = _ray.point1 - V; float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - 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(); @@ -85,11 +85,11 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr } } // If the origin of the ray is inside the cone, we return no hit - if (testPointInside(ray.point1, NULL)) { + if (testPointInside(_ray.point1, NULL)) { return false; } - localHitPoint[0] = ray.point1 + tHit[0] * r; - localHitPoint[1] = ray.point1 + tHit[1] * r; + 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); @@ -107,9 +107,9 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr // 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()); + 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; + localHitPoint[2] = _ray.point1 + tHit[2] * r; if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) { tHit[2] = float(-1.0); } @@ -131,7 +131,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr if (hitIndex < 0) { return false; } - if (t > ray.maxFraction) { + if (t > _ray.maxFraction) { return false; } // Compute the normal direction for hit against side of the cone @@ -147,11 +147,11 @@ 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]; + _raycastInfo.body = _proxyShape->getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint[hitIndex]; + _raycastInfo.worldNormal = localNormal[hitIndex]; return true; } @@ -163,40 +163,40 @@ float ConeShape::getHeight() const { return float(2.0) * m_halfHeight; } -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); +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); } size_t ConeShape::getSizeInBytes() const { return sizeof(ConeShape); } -void ConeShape::getLocalBounds(vec3& min, vec3& max) const { +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()); + _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()); + _min.setX(-_max.x()); + _min.setY(-_max.y()); + _min.setZ(_min.x()); } -void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { +void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { float rSquare = m_radius * m_radius; - float 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); + 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); } -bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { +bool ConeShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const { const float radiusHeight = m_radius - * (-localPoint.y() + m_halfHeight) + * (-_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); + return ( _localPoint.y() < m_halfHeight + && _localPoint.y() > -m_halfHeight) + && (_localPoint.x() * _localPoint.x() + _localPoint.z() * _localPoint.z() < radiusHeight *radiusHeight); }