From 14644e4f5ffc8e5c93008b8964aa674be75a62ee Mon Sep 17 00:00:00 2001 From: Edouard DUPIN Date: Wed, 18 Oct 2017 22:52:23 +0200 Subject: [PATCH] [DEV] update to the system allocator ETK_NEW --- ephysics/body/Body.hpp | 2 + ephysics/body/CollisionBody.cpp | 12 +- ephysics/body/RigidBody.cpp | 6 +- ephysics/collision/CollisionDetection.cpp | 24 +- ephysics/collision/ContactManifold.cpp | 6 +- ephysics/collision/ContactManifoldSet.cpp | 6 +- ephysics/engine/CollisionWorld.cpp | 13 +- ephysics/engine/ContactSolver.cpp | 686 +++++++--------------- ephysics/engine/ContactSolver.hpp | 186 ++++-- ephysics/engine/DynamicsWorld.cpp | 94 ++- ephysics/engine/DynamicsWorld.hpp | 12 +- ephysics/engine/Island.cpp | 87 ++- ephysics/engine/Island.hpp | 87 +-- ephysics/engine/Profiler.cpp | 17 +- test/testCollisionWorld.cpp | 45 +- test/testPointInside.cpp | 35 +- test/testRaycast.cpp | 48 +- 17 files changed, 583 insertions(+), 783 deletions(-) diff --git a/ephysics/body/Body.hpp b/ephysics/body/Body.hpp index b441809..5045c78 100644 --- a/ephysics/body/Body.hpp +++ b/ephysics/body/Body.hpp @@ -154,7 +154,9 @@ namespace ephysics { bool operator!=(const Body& _obj) const { return (m_id != _obj.m_id); } + // TODO : remove this friend class DynamicsWorld; + friend class Island; }; } diff --git a/ephysics/body/CollisionBody.cpp b/ephysics/body/CollisionBody.cpp index 05a72fb..c4e5ba1 100644 --- a/ephysics/body/CollisionBody.cpp +++ b/ephysics/body/CollisionBody.cpp @@ -43,8 +43,8 @@ inline void CollisionBody::setType(BodyType _type) { ProxyShape* CollisionBody::addCollisionShape(CollisionShape* _collisionShape, const etk::Transform3D& _transform) { - // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new ProxyShape(this, _collisionShape,_transform, float(1)); + // Create a proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = ETK_NEW(ProxyShape, this, _collisionShape,_transform, float(1)); // Add it to the list of proxy collision shapes of the body if (m_proxyCollisionShapes == nullptr) { m_proxyCollisionShapes = proxyShape; @@ -67,7 +67,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* _proxyShape) { if (m_isActive) { m_world.m_collisionDetection.removeProxyCollisionShape(current); } - delete current; + ETK_DELETE(ProxyShape, current); current = nullptr; m_numberCollisionShapes--; return; @@ -82,7 +82,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* _proxyShape) { if (m_isActive) { m_world.m_collisionDetection.removeProxyCollisionShape(elementToRemove); } - delete elementToRemove; + ETK_DELETE(ProxyShape, elementToRemove); elementToRemove = nullptr; m_numberCollisionShapes--; return; @@ -102,7 +102,7 @@ void CollisionBody::removeAllCollisionShapes() { if (m_isActive) { m_world.m_collisionDetection.removeProxyCollisionShape(current); } - delete current; + ETK_DELETE(ProxyShape, current); // Get the next element in the list current = nextElement; } @@ -116,7 +116,7 @@ void CollisionBody::resetContactManifoldsList() { while (currentElement != nullptr) { ContactManifoldListElement* nextElement = currentElement->next; // Delete the current element - delete currentElement; + ETK_DELETE(ContactManifoldListElement, currentElement); currentElement = nextElement; } m_contactManifoldsList = nullptr; diff --git a/ephysics/body/RigidBody.cpp b/ephysics/body/RigidBody.cpp index 84b79a7..5e54a7f 100644 --- a/ephysics/body/RigidBody.cpp +++ b/ephysics/body/RigidBody.cpp @@ -103,7 +103,7 @@ void RigidBody::removeJointFrom_jointsList(const Joint* _joint) { if (m_jointsList->joint == _joint) { // If the first element is the one to remove JointListElement* elementToRemove = m_jointsList; m_jointsList = elementToRemove->next; - delete elementToRemove; + ETK_DELETE(JointListElement, elementToRemove); elementToRemove = nullptr; } else { // If the element to remove is not the first one in the list @@ -112,7 +112,7 @@ void RigidBody::removeJointFrom_jointsList(const Joint* _joint) { if (currentElement->next->joint == _joint) { JointListElement* elementToRemove = currentElement->next; currentElement->next = elementToRemove->next; - delete elementToRemove; + ETK_DELETE(JointListElement, elementToRemove); elementToRemove = nullptr; break; } @@ -127,7 +127,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* _collisionShape, float _mass) { assert(_mass > 0.0f); // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new ProxyShape(this, _collisionShape, _transform, _mass); + ProxyShape* proxyShape = ETK_NEW(ProxyShape, this, _collisionShape, _transform, _mass); // Add it to the list of proxy collision shapes of the body if (m_proxyCollisionShapes == nullptr) { m_proxyCollisionShapes = proxyShape; diff --git a/ephysics/collision/CollisionDetection.cpp b/ephysics/collision/CollisionDetection.cpp index 472e15e..3f24e1f 100644 --- a/ephysics/collision/CollisionDetection.cpp +++ b/ephysics/collision/CollisionDetection.cpp @@ -137,7 +137,7 @@ void CollisionDetection::computeNarrowPhase() { !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - delete it->second; + ETK_DELETE(OverlappingPair, it->second); it->second = nullptr; it = m_overlappingPairs.erase(it); continue; @@ -228,7 +228,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - delete it->second; + ETK_DELETE(OverlappingPair, it->second); it->second = nullptr; it = m_overlappingPairs.erase(it); continue; @@ -294,7 +294,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), shape2->getCollisionShape()->getType()); // Create the overlapping pair and add it int32_to the set of overlapping pairs - OverlappingPair* newPair = new OverlappingPair(shape1, shape2, nbMaxManifolds); + OverlappingPair* newPair = ETK_NEW(OverlappingPair, shape1, shape2, nbMaxManifolds); assert(newPair != nullptr); m_overlappingPairs.set(pairID, newPair); // Wake up the two bodies @@ -310,7 +310,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { it->second->getShape2()->m_broadPhaseID == proxyShape->m_broadPhaseID) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - delete it->second; + ETK_DELETE(OverlappingPair, it->second); it->second = nullptr; it = m_overlappingPairs.erase(it); } else { @@ -325,22 +325,26 @@ void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const C // If it is the first contact since the pairs are overlapping if (overlappingPair->getNbContactPoints() == 0) { // Trigger a callback event - if (m_world->m_eventListener != NULL) m_world->m_eventListener->beginContact(contactInfo); + if (m_world->m_eventListener != NULL) { + m_world->m_eventListener->beginContact(contactInfo); + } } // Create a new contact createContact(overlappingPair, contactInfo); // Trigger a callback event for the new contact - if (m_world->m_eventListener != NULL) m_world->m_eventListener->newContact(contactInfo); + if (m_world->m_eventListener != NULL) { + m_world->m_eventListener->newContact(contactInfo); + } } void CollisionDetection::createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { // Create a new contact - ContactPoint* contact = new ContactPoint(contactInfo); + ContactPoint* contact = ETK_NEW(ContactPoint, contactInfo); // Add the contact to the contact manifold set of the corresponding overlapping pair overlappingPair->addContact(contact); // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), - overlappingPair->getShape2()); + overlappingPair->getShape2()); m_contactOverlappingPairs.set(pairId, overlappingPair); } @@ -365,10 +369,10 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { assert(contactManifold->getNbContactPoints() > 0); // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - body1->m_contactManifoldsList = new ContactManifoldListElement(contactManifold, body1->m_contactManifoldsList);; + body1->m_contactManifoldsList = ETK_NEW(ContactManifoldListElement, contactManifold, body1->m_contactManifoldsList);; // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - body2->m_contactManifoldsList = new ContactManifoldListElement(contactManifold, body2->m_contactManifoldsList);; + body2->m_contactManifoldsList = ETK_NEW(ContactManifoldListElement, contactManifold, body2->m_contactManifoldsList);; } } diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index ae3989b..95c0787 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -42,7 +42,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) { contact->getWorldPointOnBody1()).length2(); if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { // Delete the new contact - delete contact; + ETK_DELETE(ContactPoint, contact); assert(m_nbContactPoints > 0); return; } @@ -69,7 +69,7 @@ void ContactManifold::removeContactPoint(uint32_t index) { // Call the destructor explicitly and tell the memory allocator that // the corresponding memory block is now free - delete m_contactPoints[index]; + ETK_DELETE(ContactPoint, m_contactPoints[index]); m_contactPoints[index] = nullptr; // If we don't remove the last index if (index < m_nbContactPoints - 1) { @@ -237,7 +237,7 @@ void ContactManifold::clear() { // Call the destructor explicitly and tell the memory allocator that // the corresponding memory block is now free - delete m_contactPoints[iii]; + ETK_DELETE(ContactPoint, m_contactPoints[iii]); m_contactPoints[iii] = nullptr; } m_nbContactPoints = 0; diff --git a/ephysics/collision/ContactManifoldSet.cpp b/ephysics/collision/ContactManifoldSet.cpp index 2291acb..bd461ae 100644 --- a/ephysics/collision/ContactManifoldSet.cpp +++ b/ephysics/collision/ContactManifoldSet.cpp @@ -76,7 +76,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { // new contact point if (smallestDepthIndex == -1) { // Delete the new contact - delete contact; + ETK_DELETE(ContactPoint, contact); contact = nullptr; return; } @@ -155,7 +155,7 @@ void ContactManifoldSet::clear() { void ContactManifoldSet::createManifold(int16_t normalDirectionId) { assert(m_nbManifolds < m_nbMaxManifolds); - m_manifolds[m_nbManifolds] = new ContactManifold(m_shape1, m_shape2, normalDirectionId); + m_manifolds[m_nbManifolds] = ETK_NEW(ContactManifold, m_shape1, m_shape2, normalDirectionId); m_nbManifolds++; } @@ -163,7 +163,7 @@ void ContactManifoldSet::removeManifold(int32_t index) { assert(m_nbManifolds > 0); assert(index >= 0 && index < m_nbManifolds); // Delete the new contact - delete m_manifolds[index]; + ETK_DELETE(ContactManifold, m_manifolds[index]); m_manifolds[index] = nullptr; for (int32_t i=index; (i+1) < m_nbManifolds; i++) { m_manifolds[i] = m_manifolds[i+1]; diff --git a/ephysics/engine/CollisionWorld.cpp b/ephysics/engine/CollisionWorld.cpp index 81e85c4..40ad1bd 100644 --- a/ephysics/engine/CollisionWorld.cpp +++ b/ephysics/engine/CollisionWorld.cpp @@ -20,14 +20,9 @@ CollisionWorld::CollisionWorld() : } CollisionWorld::~CollisionWorld() { - // Destroy all the collision bodies that have not been removed - etk::Set::Iterator itBodies; - for (itBodies = m_bodies.begin(); itBodies != m_bodies.end(); ) { - etk::Set::Iterator itToRemove = itBodies; - ++itBodies; - destroyCollisionBody(*itToRemove); + while(m_bodies.size() != 0) { + destroyCollisionBody(m_bodies[0]); } - assert(m_bodies.empty()); } /** @@ -41,7 +36,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans // Largest index cannot be used (it is used for invalid index) EPHY_ASSERT(bodyID < UINT64_MAX, "index too big"); // Create the collision body - CollisionBody* collisionBody = new CollisionBody(transform, *this, bodyID); + CollisionBody* collisionBody = ETK_NEW(CollisionBody, transform, *this, bodyID); EPHY_ASSERT(collisionBody != nullptr, "empty Body collision"); // Add the collision body to the world m_bodies.add(collisionBody); @@ -64,7 +59,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { // Remove the collision body from the list of bodies m_bodies.erase(m_bodies.find(collisionBody)); - delete collisionBody; + ETK_DELETE(CollisionBody, collisionBody); collisionBody = nullptr; } diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index 8a1df55..58e0911 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -12,67 +12,47 @@ #include using namespace ephysics; -using namespace std; -// Constants initialization const float ContactSolver::BETA = float(0.2); const float ContactSolver::BETA_SPLIT_IMPULSE = float(0.2); -const float ContactSolver::SLOP= float(0.01); - -// Constructor -ContactSolver::ContactSolver(const etk::Map& mapBodyToVelocityIndex) - :m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr), - m_contactConstraints(nullptr), m_linearVelocities(nullptr), m_angularVelocities(nullptr), - m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - m_isWarmStartingActive(true), m_isSplitImpulseActive(true), - m_isSolveFrictionAtContactManifoldCenterActive(true) { +const float ContactSolver::SLOP = float(0.01); +ContactSolver::ContactSolver(const etk::Map& _mapBodyToVelocityIndex) : + m_splitLinearVelocities(nullptr), + m_splitAngularVelocities(nullptr), + m_linearVelocities(nullptr), + m_angularVelocities(nullptr), + m_mapBodyToConstrainedVelocityIndex(_mapBodyToVelocityIndex), + m_isWarmStartingActive(true), + m_isSplitImpulseActive(true), + m_isSolveFrictionAtContactManifoldCenterActive(true) { + } -// Destructor -ContactSolver::~ContactSolver() { - -} - -// Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(float dt, Island* island) { - +void ContactSolver::initializeForIsland(float _dt, Island* _island) { PROFILE("ContactSolver::initializeForIsland()"); - - assert(island != nullptr); - assert(island->getNbBodies() > 0); - assert(island->getNbContactManifolds() > 0); + assert(_island != nullptr); + assert(_island->getNbBodies() > 0); + assert(_island->getNbContactManifolds() > 0); assert(m_splitLinearVelocities != nullptr); assert(m_splitAngularVelocities != nullptr); - // Set the current time step - m_timeStep = dt; - - m_numberContactManifolds = island->getNbContactManifolds(); - - m_contactConstraints = new ContactManifoldSolver[m_numberContactManifolds]; - assert(m_contactConstraints != nullptr); - + m_timeStep = _dt; + m_contactConstraints.resize(_island->getNbContactManifolds()); // For each contact manifold of the island - ContactManifold** contactManifolds = island->getContactManifold(); - for (uint32_t i=0; igetContactManifold(); + for (uint32_t iii=0; iiigetNbContactPoints() > 0); - // Get the two bodies of the contact RigidBody* body1 = static_cast(externalManifold->getContactPoint(0)->getBody1()); RigidBody* body2 = static_cast(externalManifold->getContactPoint(0)->getBody2()); assert(body1 != nullptr); assert(body2 != nullptr); - // Get the position of the two bodies const vec3& x1 = body1->m_centerOfMassWorld; const vec3& x2 = body2->m_centerOfMassWorld; - // Initialize the int32_ternal contact manifold structure using the external // contact manifold int32_ternalManifold.indexBody1 = m_mapBodyToConstrainedVelocityIndex.find(body1)->second; @@ -88,25 +68,19 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { int32_ternalManifold.externalContactManifold = externalManifold; int32_ternalManifold.isBody1DynamicType = body1->getType() == DYNAMIC; int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { int32_ternalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f); int32_ternalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f); } - // For each contact point of the contact manifold - for (uint32_t c=0; cgetNbContactPoints(); c++) { - - ContactPointSolver& contactPoint = int32_ternalManifold.contacts[c]; - + for (uint32_t ccc=0; cccgetNbContactPoints(); ++ccc) { + ContactPointSolver& contactPoint = int32_ternalManifold.contacts[ccc]; // Get a contact point - ContactPoint* externalContact = externalManifold->getContactPoint(c); - + ContactPoint* externalContact = externalManifold->getContactPoint(ccc); // Get the contact point on the two bodies vec3 p1 = externalContact->getWorldPointOnBody1(); vec3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.externalContact = externalContact; contactPoint.normal = externalContact->getNormal(); contactPoint.r1 = p1 - x1; @@ -120,34 +94,27 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { int32_ternalManifold.frictionPointBody1 += p1; int32_ternalManifold.frictionPointBody2 += p2; } } - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { - int32_ternalManifold.frictionPointBody1 /=static_cast(int32_ternalManifold.nbContacts); int32_ternalManifold.frictionPointBody2 /=static_cast(int32_ternalManifold.nbContacts); int32_ternalManifold.r1Friction = int32_ternalManifold.frictionPointBody1 - x1; int32_ternalManifold.r2Friction = int32_ternalManifold.frictionPointBody2 - x2; int32_ternalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2(); - // If warm starting is active if (m_isWarmStartingActive) { - // Initialize the accumulated impulses with the previous step accumulated impulses int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); int32_ternalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); int32_ternalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); - } - else { - + } else { // Initialize the accumulated impulses to zero int32_ternalManifold.friction1Impulse = 0.0; int32_ternalManifold.friction2Impulse = 0.0; @@ -156,85 +123,67 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { } } } - // Fill-in all the matrices needed to solve the LCP problem initializeContactConstraints(); } -// Initialize the contact constraints before solving the system void ContactSolver::initializeContactConstraints() { - // For each contact constraint - for (uint32_t c=0; c 0.0 ? contactPoint.inversePenetrationMass = 1.0f / - massPenetration : - 0.0f; - + float massPenetration = manifold.massInverseBody1 + + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + + ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); + if (massPenetration > 0.0) { + contactPoint.inversePenetrationMass = 1.0f / massPenetration; + } // If we do not solve the friction constraints at the center of the contact manifold if (!m_isSolveFrictionAtContactManifoldCenterActive) { - // Compute the friction vectors computeFrictionVectors(deltaV, contactPoint); - contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2); contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2); - // Compute the inverse mass matrix K for the friction // constraints at each contact point - float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( - contactPoint.frictionVector1) + - ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( - contactPoint.frictionVector1); - float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( - contactPoint.frictionvec2) + - ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( - contactPoint.frictionvec2); - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = 1.0f / - friction1Mass : - 0.0f; - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = 1.0f / - friction2Mass : - 0.0f; + float friction1Mass = manifold.massInverseBody1 + + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1) + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); + float friction2Mass = manifold.massInverseBody1 + + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionvec2) + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionvec2); + if (friction1Mass > 0.0) { + contactPoint.inverseFriction1Mass = 1.0f / friction1Mass; + } + if (friction2Mass > 0.0) { + contactPoint.inverseFriction2Mass = 1.0f / friction2Mass; + } } - // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference // at the beginning of the contact. Note that if it is a resting contact (normal @@ -244,156 +193,113 @@ void ContactSolver::initializeContactConstraints() { if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } - // If the warm starting of the contact solver is active if (m_isWarmStartingActive) { - // Get the cached accumulated impulses from the previous step contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); } - // Initialize the split impulses to zero contactPoint.penetrationSplitImpulse = 0.0; - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { manifold.normal += contactPoint.normal; } } - // Compute the inverse K matrix for the rolling resistance constraint manifold.inverseRollingResistance.setZero(); if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); } - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { - manifold.normal.normalize(); - - vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - - v1 - w1.cross(manifold.r1Friction); - + vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) + - v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, manifold); - // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2); - float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( - manifold.frictionVector1) + - ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( - manifold.frictionVector1); - float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( - manifold.frictionvec2) + - ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( - manifold.frictionvec2); - float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * - manifold.normal) + - manifold.normal.dot(manifold.inverseInertiaTensorBody2 * - manifold.normal); - friction1Mass > 0.0 ? manifold.inverseFriction1Mass = 1.0f/friction1Mass - : 0.0f; - friction2Mass > 0.0 ? manifold.inverseFriction2Mass = 1.0f/friction2Mass - : 0.0f; - frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = 1.0f / - frictionTwistMass : - 0.0f; + float friction1Mass = manifold.massInverseBody1 + + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(manifold.frictionVector1) + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(manifold.frictionVector1); + float friction2Mass = manifold.massInverseBody1 + + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(manifold.frictionvec2) + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(manifold.frictionvec2); + float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * manifold.normal) + + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal); + if (friction1Mass > 0.0) { + manifold.inverseFriction1Mass = 1.0f/friction1Mass; + } + if (friction2Mass > 0.0) { + manifold.inverseFriction2Mass = 1.0f/friction2Mass; + } + if (frictionTwistMass > 0.0) { + manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass; + } } } } -// Warm start the solver. -/// For each constraint, we apply the previous impulse (from the previous step) -/// at the beginning. With this technique, we will converge faster towards -/// the solution of the linear system void ContactSolver::warmStart() { - // Check that warm starting is active - if (!m_isWarmStartingActive) return; - + if (!m_isWarmStartingActive) { + return; + } // For each constraint - for (uint32_t c=0; c 0) { - // Compute the impulse P = J^T * lambda const Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse, - vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); - + vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); } } - } - else { // If it is a new contact point - + } else { + // If it is a new contact point // Initialize the accumulated impulses to zero contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; @@ -401,82 +307,57 @@ void ContactSolver::warmStart() { contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); } } - // If we solve the friction constraints at the center of the contact manifold and there is // at least one resting contact point in the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { - // Project the old friction impulses (with old friction vectors) int32_to the new friction // vectors to get the new friction impulses vec3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionvec2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionvec2); - + contactManifold.oldFrictionVector1 + + contactManifold.friction2Impulse * + contactManifold.oldFrictionvec2; + contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); + contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2); // ------ First friction constraint at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda - vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * - contactManifold.friction1Impulse; - vec3 linearImpulseBody2 = contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * - contactManifold.friction1Impulse; + vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse; + vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; + vec3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; + vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, contactManifold); - // ------ Second friction constraint at the center of the contact manifold ----- // - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionvec2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionvec2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; + linearImpulseBody1 = -contactManifold.frictionvec2 * contactManifold.friction2Impulse; + angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse; + linearImpulseBody2 = contactManifold.frictionvec2 * contactManifold.friction2Impulse; + angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse; const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, contactManifold); - // ------ Twist friction constraint at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; linearImpulseBody2 = vec3(0.0, 0.0, 0.0); angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseTwistFriction, contactManifold); - // ------ Rolling resistance at the center of the contact manifold ------ // - // Compute the impulse P = J^T * lambda angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; const Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), angularImpulseBody1, - vec3(0.0f,0.0f,0.0f), angularImpulseBody2); - + vec3(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRollingResistance, contactManifold); - } - else { // If it is a new contact manifold - + } else { + // If it is a new contact manifold // Initialize the accumulated impulses to zero contactManifold.friction1Impulse = 0.0; contactManifold.friction2Impulse = 0.0; @@ -486,258 +367,179 @@ void ContactSolver::warmStart() { } } -// Solve the contacts void ContactSolver::solve() { - PROFILE("ContactSolver::solve()"); - float deltaLambda; float lambdaTemp; - // For each contact manifold - for (uint32_t c=0; c SLOP) biasPenetrationDepth = -(beta/m_timeStep) * - etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + if (contactPoint.penetrationDepth > SLOP) { + biasPenetrationDepth = -(beta/m_timeStep) * etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + } float b = biasPenetrationDepth + contactPoint.restitutionBias; - // Compute the Lagrange multiplier lambda if (m_isSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; - } - else { + deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass; + } else { deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; } lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = etk::max(contactPoint.penetrationImpulse + - deltaLambda, 0.0f); + contactPoint.penetrationImpulse = etk::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); - + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); // Apply the impulse to the bodies of the constraint applyImpulse(impulsePenetration, contactManifold); - sum_penetrationImpulse += contactPoint.penetrationImpulse; - // If the split impulse position correction is active if (m_isSplitImpulseActive) { - // Split impulse (position correction) const vec3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1]; const vec3& w1Split = m_splitAngularVelocities[contactManifold.indexBody1]; const vec3& v2Split = m_splitLinearVelocities[contactManifold.indexBody2]; const vec3& w2Split = m_splitAngularVelocities[contactManifold.indexBody2]; - vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); + vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1); float JvSplit = deltaVSplit.dot(contactPoint.normal); - float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; + float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; float lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = etk::max( - contactPoint.penetrationSplitImpulse + - deltaLambdaSplit, 0.0f); + contactPoint.penetrationSplitImpulse = etk::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; - // Compute the impulse P=J^T * lambda - const Impulse splitImpulsePenetration = computePenetrationImpulse( - deltaLambdaSplit, contactPoint); - + const Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint); applySplitImpulse(splitImpulsePenetration, contactManifold); } - // If we do not solve the friction constraints at the center of the contact manifold if (!m_isSolveFrictionAtContactManifoldCenterActive) { - // --------- Friction 1 --------- // - // Compute J*v deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Jv = deltaV.dot(contactPoint.frictionVector1); - // Compute the Lagrange multiplier lambda deltaLambda = -Jv; deltaLambda *= contactPoint.inverseFriction1Mass; - float frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; + float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction1Impulse; contactPoint.friction1Impulse = etk::max(-frictionLimit, - etk::min(contactPoint.friction1Impulse - + deltaLambda, frictionLimit)); + etk::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, - contactPoint); - + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, contactManifold); - // --------- Friction 2 --------- // - // Compute J*v deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Jv = deltaV.dot(contactPoint.frictionvec2); - // Compute the Lagrange multiplier lambda deltaLambda = -Jv; deltaLambda *= contactPoint.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; + frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction2Impulse; - contactPoint.friction2Impulse = etk::max(-frictionLimit, - etk::min(contactPoint.friction2Impulse - + deltaLambda, frictionLimit)); + contactPoint.friction2Impulse = etk::max(-frictionLimit, etk::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, - contactPoint); - + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, contactManifold); - // --------- Rolling resistance constraint --------- // - if (contactManifold.rollingResistanceFactor > 0) { - // Compute J*v const vec3 JvRolling = w2 - w1; - // Compute the Lagrange multiplier lambda vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; - contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); + contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; - // Compute the impulse P=J^T * lambda const Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), -deltaLambdaRolling, - vec3(0.0f,0.0f,0.0f), deltaLambdaRolling); - + vec3(0.0f,0.0f,0.0f), deltaLambdaRolling); // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); } } } - // If we solve the friction constraints at the center of the contact manifold if (m_isSolveFrictionAtContactManifoldCenterActive) { - // ------ First friction constraint at the center of the contact manifol ------ // - // Compute J*v - vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); + vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); float Jv = deltaV.dot(contactManifold.frictionVector1); - // Compute the Lagrange multiplier lambda float deltaLambda = -Jv * contactManifold.inverseFriction1Mass; float frictionLimit = contactManifold.frictionCoefficient * sum_penetrationImpulse; lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = etk::max(-frictionLimit, - etk::min(contactManifold.friction1Impulse + - deltaLambda, frictionLimit)); + contactManifold.friction1Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction1Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, contactManifold); - // ------ Second friction constraint at the center of the contact manifol ----- // - // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); + deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); Jv = deltaV.dot(contactManifold.frictionvec2); - // Compute the Lagrange multiplier lambda deltaLambda = -Jv * contactManifold.inverseFriction2Mass; frictionLimit = contactManifold.frictionCoefficient * sum_penetrationImpulse; lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = etk::max(-frictionLimit, - etk::min(contactManifold.friction2Impulse + - deltaLambda, frictionLimit)); + contactManifold.friction2Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction2Impulse - lambdaTemp; - // Compute the impulse P=J^T * lambda linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda; angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda; angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, contactManifold); - // ------ Twist friction constraint at the center of the contact manifol ------ // - // Compute J*v deltaV = w2 - w1; Jv = deltaV.dot(contactManifold.normal); - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); frictionLimit = contactManifold.frictionCoefficient * sum_penetrationImpulse; lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = etk::max(-frictionLimit, - etk::min(contactManifold.frictionTwistImpulse - + deltaLambda, frictionLimit)); + contactManifold.frictionTwistImpulse = etk::max(-frictionLimit, etk::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * deltaLambda; linearImpulseBody2 = vec3(0.0, 0.0, 0.0);; angularImpulseBody2 = contactManifold.normal * deltaLambda; const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - + linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseTwistFriction, contactManifold); - // --------- Rolling resistance constraint at the center of the contact manifold --------- // - if (contactManifold.rollingResistanceFactor > 0) { - // Compute J*v const vec3 JvRolling = w2 - w1; - // Compute the Lagrange multiplier lambda vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * sum_penetrationImpulse; @@ -745,7 +547,6 @@ void ContactSolver::solve() { contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; - // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; @@ -753,7 +554,6 @@ void ContactSolver::solve() { angularImpulseBody1, vec3(0.0f,0.0f,0.0f), angularImpulseBody2); - // Apply the impulses to the bodies of the constraint applyImpulse(impulseRolling, contactManifold); } @@ -761,28 +561,19 @@ void ContactSolver::solve() { } } -// Store the computed impulses to use them to -// warm start the solver at the next iteration void ContactSolver::storeImpulses() { - // For each contact manifold - for (uint32_t c=0; csetPenetrationImpulse(contactPoint.penetrationImpulse); contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); contactPoint.externalContact->setFrictionvec2(contactPoint.frictionvec2); } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); @@ -792,191 +583,130 @@ void ContactSolver::storeImpulses() { } } -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - +void ContactSolver::applyImpulse(const Impulse& _impulse, const ContactManifoldSolver& _manifold) { // Update the velocities of the body 1 by applying the impulse P - m_linearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - m_angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - + m_linearVelocities[_manifold.indexBody1] += _manifold.massInverseBody1 * _impulse.linearImpulseBody1; + m_angularVelocities[_manifold.indexBody1] += _manifold.inverseInertiaTensorBody1 * _impulse.angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P - m_linearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - m_angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; + m_linearVelocities[_manifold.indexBody2] += _manifold.massInverseBody2 * _impulse.linearImpulseBody2; + m_angularVelocities[_manifold.indexBody2] += _manifold.inverseInertiaTensorBody2 * _impulse.angularImpulseBody2; } -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - +void ContactSolver::applySplitImpulse(const Impulse& _impulse, const ContactManifoldSolver& _manifold) { // Update the velocities of the body 1 by applying the impulse P - m_splitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - m_splitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - + m_splitLinearVelocities[_manifold.indexBody1] += _manifold.massInverseBody1 * _impulse.linearImpulseBody1; + m_splitAngularVelocities[_manifold.indexBody1] += _manifold.inverseInertiaTensorBody1 * _impulse.angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P - m_splitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - m_splitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; + m_splitLinearVelocities[_manifold.indexBody2] += _manifold.massInverseBody2 * _impulse.linearImpulseBody2; + m_splitAngularVelocities[_manifold.indexBody2] += _manifold.inverseInertiaTensorBody2 * _impulse.angularImpulseBody2; } -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, - ContactPointSolver& contactPoint) const { - - assert(contactPoint.normal.length() > 0.0); - +void ContactSolver::computeFrictionVectors(const vec3& _deltaVelocity, ContactPointSolver& _contactPoint) const { + assert(_contactPoint.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - vec3 tangentVelocity = deltaVelocity - normalVelocity; - + vec3 normalVelocity = _deltaVelocity.dot(_contactPoint.normal) * _contactPoint.normal; + vec3 tangentVelocity = _deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLT_EPSILON) { - // Compute the first friction vector in the direction of the tangent // velocity difference - contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { - + _contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } else { // Get any orthogonal vector to the normal as the first friction vector - contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector(); + _contactPoint.frictionVector1 = _contactPoint.normal.getOrthoVector(); } - // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized(); + _contactPoint.frictionvec2 =_contactPoint.normal.cross(_contactPoint.frictionVector1).safeNormalized(); } -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, - ContactManifoldSolver& contact) const { - - assert(contact.normal.length() > 0.0); - +void ContactSolver::computeFrictionVectors(const vec3& _deltaVelocity, ContactManifoldSolver& _contactManifold) const { + assert(_contactManifold.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - vec3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - vec3 tangentVelocity = deltaVelocity - normalVelocity; - + vec3 normalVelocity = _deltaVelocity.dot(_contactManifold.normal) * _contactManifold.normal; + vec3 tangentVelocity = _deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLT_EPSILON) { - // Compute the first friction vector in the direction of the tangent // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { - + _contactManifold.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } else { // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOrthoVector(); + _contactManifold.frictionVector1 = _contactManifold.normal.getOrthoVector(); } - // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contact.frictionvec2 = contact.normal.cross(contact.frictionVector1).safeNormalized(); + _contactManifold.frictionvec2 = _contactManifold.normal.cross(_contactManifold.frictionVector1).safeNormalized(); } -// Clean up the constraint solver void ContactSolver::cleanup() { - - if (m_contactConstraints != nullptr) { - delete[] m_contactConstraints; - m_contactConstraints = nullptr; - } + m_contactConstraints.clear(); } -// Set the split velocities arrays -void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, - vec3* splitAngularVelocities) { - assert(splitLinearVelocities != NULL); - assert(splitAngularVelocities != NULL); - m_splitLinearVelocities = splitLinearVelocities; - m_splitAngularVelocities = splitAngularVelocities; +void ContactSolver::setSplitVelocitiesArrays(vec3* _splitLinearVelocities, vec3* _splitAngularVelocities) { + assert(_splitLinearVelocities != NULL); + assert(_splitAngularVelocities != NULL); + m_splitLinearVelocities = _splitLinearVelocities; + m_splitAngularVelocities = _splitAngularVelocities; } -// Set the constrained velocities arrays -void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities) { - assert(constrainedLinearVelocities != NULL); - assert(constrainedAngularVelocities != NULL); - m_linearVelocities = constrainedLinearVelocities; - m_angularVelocities = constrainedAngularVelocities; +void ContactSolver::setConstrainedVelocitiesArrays(vec3* _constrainedLinearVelocities, vec3* _constrainedAngularVelocities) { + assert(_constrainedLinearVelocities != NULL); + assert(_constrainedAngularVelocities != NULL); + m_linearVelocities = _constrainedLinearVelocities; + m_angularVelocities = _constrainedAngularVelocities; } -// Return true if the split impulses position correction technique is used for contacts bool ContactSolver::isSplitImpulseActive() const { return m_isSplitImpulseActive; } -// Activate or Deactivate the split impulses for contacts -void ContactSolver::setIsSplitImpulseActive(bool isActive) { - m_isSplitImpulseActive = isActive; +void ContactSolver::setIsSplitImpulseActive(bool _isActive) { + m_isSplitImpulseActive = _isActive; } -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - m_isSolveFrictionAtContactManifoldCenterActive = isActive; +void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool _isActive) { + m_isSolveFrictionAtContactManifoldCenterActive = _isActive; } -// Compute the collision restitution factor from the restitution factor of each body -float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, - RigidBody* body2) const { - float restitution1 = body1->getMaterial().getBounciness(); - float restitution2 = body2->getMaterial().getBounciness(); - +float ContactSolver::computeMixedRestitutionFactor(RigidBody* _body1, RigidBody* _body2) const { + float restitution1 = _body1->getMaterial().getBounciness(); + float restitution2 = _body2->getMaterial().getBounciness(); // Return the largest restitution factor return (restitution1 > restitution2) ? restitution1 : restitution2; } -// Compute the mixed friction coefficient from the friction coefficient of each body -float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, - RigidBody *body2) const { +float ContactSolver::computeMixedFrictionCoefficient(RigidBody* _body1, RigidBody* _body2) const { // Use the geometric mean to compute the mixed friction coefficient - return sqrt(body1->getMaterial().getFrictionCoefficient() * - body2->getMaterial().getFrictionCoefficient()); + return sqrt(_body1->getMaterial().getFrictionCoefficient() * + _body2->getMaterial().getFrictionCoefficient()); } -// Compute th mixed rolling resistance factor between two bodies -float ContactSolver::computeMixedRollingResistance(RigidBody* body1, - RigidBody* body2) const { - return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); +float ContactSolver::computeMixedRollingResistance(RigidBody* _body1, RigidBody* _body2) const { + return 0.5f + * (_body1->getMaterial().getRollingResistance() + + _body2->getMaterial().getRollingResistance()); } -// Compute a penetration constraint impulse -const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, - contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +const Impulse ContactSolver::computePenetrationImpulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const { + return Impulse(-_contactPoint.normal * _deltaLambda, + -_contactPoint.r1CrossN * _deltaLambda, + _contactPoint.normal * _deltaLambda, + _contactPoint.r2CrossN * _deltaLambda); } -// Compute the first friction constraint impulse -const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, - -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, - contactPoint.r2CrossT1 * deltaLambda); +const Impulse ContactSolver::computeFriction1Impulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const { + return Impulse(-_contactPoint.frictionVector1 * _deltaLambda, + -_contactPoint.r1CrossT1 * _deltaLambda, + _contactPoint.frictionVector1 * _deltaLambda, + _contactPoint.r2CrossT1 * _deltaLambda); } -// Compute the second friction constraint impulse -const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionvec2 * deltaLambda, - -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionvec2 * deltaLambda, - contactPoint.r2CrossT2 * deltaLambda); +const Impulse ContactSolver::computeFriction2Impulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const { + return Impulse(-_contactPoint.frictionvec2 * _deltaLambda, + -_contactPoint.r1CrossT2 * _deltaLambda, + _contactPoint.frictionvec2 * _deltaLambda, + _contactPoint.r2CrossT2 * _deltaLambda); } diff --git a/ephysics/engine/ContactSolver.hpp b/ephysics/engine/ContactSolver.hpp index 6ebe889..28d0f0a 100644 --- a/ephysics/engine/ContactSolver.hpp +++ b/ephysics/engine/ContactSolver.hpp @@ -118,7 +118,6 @@ namespace ephysics { bool isRestingContact; //!< True if the contact was existing last time step ContactPoint* externalContact; //!< Pointer to the external contact }; - /** * @brief Contact solver int32_ternal data structure to store all the information relative to a contact manifold. */ @@ -166,76 +165,147 @@ namespace ephysics { vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) float m_timeStep; //!< Current time step - ContactManifoldSolver* m_contactConstraints; //!< Contact constraints - uint32_t m_numberContactManifolds; //!< Number of contact constraints + etk::Vector m_contactConstraints; //!< Contact constraints vec3* m_linearVelocities; //!< Array of linear velocities vec3* m_angularVelocities; //!< Array of angular velocities const etk::Map& m_mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the constrained velocities array bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active bool m_isSplitImpulseActive; //!< True if the split impulse position correction is active bool m_isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction constraints at the contact manifold center only instead of 2 friction constraints at each contact point - /// Initialize the contact constraints before solving the system + /** + * @brief Initialize the contact constraints before solving the system + */ void initializeContactConstraints(); - /// Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); - /// Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold); - /// Compute the collision restitution factor from the restitution factor of each body - float computeMixedRestitutionFactor(RigidBody *body1, - RigidBody *body2) const; - /// Compute the mixed friction coefficient from the friction coefficient of each body - float computeMixedFrictionCoefficient(RigidBody* body1, - RigidBody* body2) const; - /// Compute th mixed rolling resistance factor between two bodies - float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; - /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - /// plane for a contact point. The two vectors have to be - /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const vec3& deltaVelocity, - ContactPointSolver &contactPoint) const; - /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - /// plane for a contact manifold. The two vectors have to be - /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const vec3& deltaVelocity, - ContactManifoldSolver& contactPoint) const; - /// Compute a penetration constraint impulse - const Impulse computePenetrationImpulse(float deltaLambda, - const ContactPointSolver& contactPoint) const; - /// Compute the first friction constraint impulse - const Impulse computeFriction1Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) const; - /// Compute the second friction constraint impulse - const Impulse computeFriction2Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) const; + /** + * @brief Apply an impulse to the two bodies of a constraint + * @param[in] _impulse Impulse to apply + * @param[in] _manifold Constraint to apply the impulse + */ + void applyImpulse(const Impulse& _impulse, const ContactManifoldSolver& _manifold); + /** + * @brief Apply an impulse to the two bodies of a constraint + * @param[in] _impulse Impulse to apply + * @param[in] _manifold Constraint to apply the impulse + */ + void applySplitImpulse(const Impulse& _impulse, const ContactManifoldSolver& _manifold); + /** + * @brief Compute the collision restitution factor from the restitution factor of each body + * @param[in] _body1 First body to compute + * @param[in] _body2 Second body to compute + * @return Collision restitution factor + */ + float computeMixedRestitutionFactor(RigidBody* _body1, RigidBody* _body2) const; + /** + * @brief Compute the mixed friction coefficient from the friction coefficient of each body + * @param[in] _body1 First body to compute + * @param[in] _body2 Second body to compute + * @return Mixed friction coefficient + */ + float computeMixedFrictionCoefficient(RigidBody* _body1, RigidBody* _body2) const; + /** + * @brief Compute the mixed rolling resistance factor between two bodies + * @param[in] _body1 First body to compute + * @param[in] _body2 Second body to compute + * @return Mixed rolling resistance + */ + float computeMixedRollingResistance(RigidBody* _body1, RigidBody* _body2) const; + /** + * @brief Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + * plane for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. + * @param[in] _deltaVelocity Velocity ratio (with the delta time step) + * @param[in,out] _contactPoint Contact point property + */ + void computeFrictionVectors(const vec3& _deltaVelocity, ContactPointSolver& _contactPoint) const; + /** + * @brief Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + * plane for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. + * @param[in] _deltaVelocity Velocity ratio (with the delta time step) + * @param[in,out] _contactPoint Contact point property + */ + void computeFrictionVectors(const vec3& _deltaVelocity, ContactManifoldSolver& _contactPoint) const; + /** + * @brief Compute a penetration constraint impulse + * @param[in] _deltaLambda Ratio to apply at the calculation. + * @param[in,out] _contactPoint Contact point property + * @return Impulse of the penetration result + */ + const Impulse computePenetrationImpulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const; + /** + * @brief Compute the first friction constraint impulse + * @param[in] _deltaLambda Ratio to apply at the calculation. + * @param[in] _contactPoint Contact point property + * @return Impulse of the friction result + */ + const Impulse computeFriction1Impulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const; + /** + * @brief Compute the second friction constraint impulse + * @param[in] _deltaLambda Ratio to apply at the calculation. + * @param[in] _contactPoint Contact point property + * @return Impulse of the friction result + */ + const Impulse computeFriction2Impulse(float _deltaLambda, const ContactPointSolver& _contactPoint) const; public: - /// Constructor - ContactSolver(const etk::Map& mapBodyToVelocityIndex); - /// Destructor - virtual ~ContactSolver(); - /// Initialize the constraint solver for a given island - void initializeForIsland(float dt, Island* island); - /// Set the split velocities arrays - void setSplitVelocitiesArrays(vec3* splitLinearVelocities, - vec3* splitAngularVelocities); - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities); - /// Warm start the solver. + /** + * @brief Constructor + * @param[in] _mapBodyToVelocityIndex + */ + ContactSolver(const etk::Map& _mapBodyToVelocityIndex); + /** + * @brief Virtualize the destructor + */ + virtual ~ContactSolver() = default; + /** + * @brief Initialize the constraint solver for a given island + * @param[in] _dt Delta step time + * @param[in] _island Island list property + */ + void initializeForIsland(float _dt, Island* _island); + /** + * @brief Set the split velocities arrays + * @param[in] _splitLinearVelocities Split linear velocities Table pointer (not free) + * @param[in] _splitAngularVelocities Split angular velocities Table pointer (not free) + */ + void setSplitVelocitiesArrays(vec3* _splitLinearVelocities, vec3* _splitAngularVelocities); + /** + * @brief Set the constrained velocities arrays + * @param[in] _constrainedLinearVelocities Constrained Linear velocities Table pointer (not free) + * @param[in] _constrainedAngularVelocities Constrained angular velocities Table pointer (not free) + */ + void setConstrainedVelocitiesArrays(vec3* _constrainedLinearVelocities, vec3* _constrainedAngularVelocities); + /** + * @brief Warm start the solver. + * For each constraint, we apply the previous impulse (from the previous step) + * at the beginning. With this technique, we will converge faster towards the solution of the linear system + */ void warmStart(); - /// Store the computed impulses to use them to - /// warm start the solver at the next iteration + /** + * @brief Store the computed impulses to use them to warm start the solver at the next iteration + */ void storeImpulses(); - /// Solve the contacts + /** + * @brief Solve the contacts + */ void solve(); - /// Return true if the split impulses position correction technique is used for contacts + /** + * @brief Get the split impulses position correction technique is used for contacts + * @return true the split status is Enable + * @return true the split status is Disable + */ bool isSplitImpulseActive() const; - /// Activate or Deactivate the split impulses for contacts - void setIsSplitImpulseActive(bool isActive); - /// Activate or deactivate the solving of friction constraints at the center of + /** + * @brief Activate or Deactivate the split impulses for contacts + * @param[in] _isActive status to set. + */ + void setIsSplitImpulseActive(bool _isActive); + /** + * @brief Activate or deactivate the solving of friction constraints at the center of /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Clean up the constraint solver + * @param[in] _isActive Enable or not the center inertie + */ + void setIsSolveFrictionAtContactManifoldCenterActive(bool _isActive); + /** + * @brief Clean up the constraint solver + */ void cleanup(); }; diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index 4bd523e..7750859 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -22,13 +22,6 @@ ephysics::DynamicsWorld::DynamicsWorld(const vec3& _gravity): m_isSleepingEnabled(SPLEEPING_ENABLED), m_gravity(_gravity), m_isGravityEnabled(true), - m_constrainedLinearVelocities(nullptr), - m_constrainedAngularVelocities(nullptr), - m_splitLinearVelocities(nullptr), - m_splitAngularVelocities(nullptr), - m_constrainedPositions(nullptr), - m_constrainedOrientations(nullptr), - m_islands(), m_numberBodiesCapacity(0), m_sleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), m_sleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), @@ -54,18 +47,18 @@ ephysics::DynamicsWorld::~DynamicsWorld() { // Release the memory allocated for the islands for (auto &it: m_islands) { // Call the island destructor - delete it; + ETK_DELETE(Island, it); it = nullptr; } m_islands.clear(); // Release the memory allocated for the bodies velocity arrays if (m_numberBodiesCapacity > 0) { - delete[] m_splitLinearVelocities; - delete[] m_splitAngularVelocities; - delete[] m_constrainedLinearVelocities; - delete[] m_constrainedAngularVelocities; - delete[] m_constrainedPositions; - delete[] m_constrainedOrientations; + m_splitLinearVelocities.clear(); + m_splitAngularVelocities.clear(); + m_constrainedLinearVelocities.clear(); + m_constrainedAngularVelocities.clear(); + m_constrainedPositions.clear(); + m_constrainedOrientations.clear(); } assert(m_joints.size() == 0); assert(m_rigidBodies.size() == 0); @@ -180,23 +173,22 @@ void ephysics::DynamicsWorld::initVelocityArrays() { uint32_t nbBodies = m_rigidBodies.size(); if (m_numberBodiesCapacity != nbBodies && nbBodies > 0) { if (m_numberBodiesCapacity > 0) { - delete[] m_splitLinearVelocities; - delete[] m_splitAngularVelocities; + m_splitLinearVelocities.clear(); + m_splitAngularVelocities.clear(); } m_numberBodiesCapacity = nbBodies; - // TODO : Use better memory allocation here - m_splitLinearVelocities = new vec3[m_numberBodiesCapacity]; - m_splitAngularVelocities = new vec3[m_numberBodiesCapacity]; - m_constrainedLinearVelocities = new vec3[m_numberBodiesCapacity]; - m_constrainedAngularVelocities = new vec3[m_numberBodiesCapacity]; - m_constrainedPositions = new vec3[m_numberBodiesCapacity]; - m_constrainedOrientations = new etk::Quaternion[m_numberBodiesCapacity]; - assert(m_splitLinearVelocities != nullptr); - assert(m_splitAngularVelocities != nullptr); - assert(m_constrainedLinearVelocities != nullptr); - assert(m_constrainedAngularVelocities != nullptr); - assert(m_constrainedPositions != nullptr); - assert(m_constrainedOrientations != nullptr); + m_splitLinearVelocities.clear(); + m_splitAngularVelocities.clear(); + m_constrainedLinearVelocities.clear(); + m_constrainedAngularVelocities.clear(); + m_constrainedPositions.clear(); + m_constrainedOrientations.clear(); + m_splitLinearVelocities.resize(m_numberBodiesCapacity, vec3(0,0,0)); + m_splitAngularVelocities.resize(m_numberBodiesCapacity, vec3(0,0,0)); + m_constrainedLinearVelocities.resize(m_numberBodiesCapacity, vec3(0,0,0)); + m_constrainedAngularVelocities.resize(m_numberBodiesCapacity, vec3(0,0,0)); + m_constrainedPositions.resize(m_numberBodiesCapacity, vec3(0,0,0)); + m_constrainedOrientations.resize(m_numberBodiesCapacity, etk::Quaternion::identity()); } // Reset the velocities arrays for (uint32_t i=0; i(_jointInfo)); + newJoint = ETK_NEW(BallAndSocketJoint, static_cast(_jointInfo)); break; // Slider joint case SLIDERJOINT: - newJoint = new SliderJoint(static_cast(_jointInfo)); + newJoint = ETK_NEW(SliderJoint, static_cast(_jointInfo)); break; // Hinge joint case HINGEJOINT: - newJoint = new HingeJoint(static_cast(_jointInfo)); + newJoint = ETK_NEW(HingeJoint, static_cast(_jointInfo)); break; // Fixed joint case FIXEDJOINT: - newJoint = new FixedJoint(static_cast(_jointInfo)); + newJoint = ETK_NEW(FixedJoint, static_cast(_jointInfo)); break; default: assert(false); @@ -420,7 +412,7 @@ void ephysics::DynamicsWorld::destroyJoint(Joint* _joint) { _joint->m_body2->removeJointFrom_jointsList(_joint); size_t nbBytes = _joint->getSizeInBytes(); // Call the destructor of the joint - delete _joint; + ETK_DELETE(Joint, _joint); _joint = nullptr; } @@ -430,9 +422,9 @@ void ephysics::DynamicsWorld::addJointToBody(ephysics::Joint* _joint) { return; } // Add the joint at the beginning of the linked list of joints of the first body - _joint->m_body1->m_jointsList = new JointListElement(_joint, _joint->m_body1->m_jointsList); + _joint->m_body1->m_jointsList = ETK_NEW(JointListElement, _joint, _joint->m_body1->m_jointsList); // Add the joint at the beginning of the linked list of joints of the second body - _joint->m_body2->m_jointsList = new JointListElement(_joint, _joint->m_body2->m_jointsList); + _joint->m_body2->m_jointsList = ETK_NEW(JointListElement, _joint, _joint->m_body2->m_jointsList); } void ephysics::DynamicsWorld::computeIslands() { @@ -440,7 +432,7 @@ void ephysics::DynamicsWorld::computeIslands() { uint32_t nbBodies = m_rigidBodies.size(); // Clear all the islands for (auto &it: m_islands) { - delete it; + ETK_DELETE(Island, it); it = nullptr; } // Call the island destructor @@ -478,7 +470,7 @@ void ephysics::DynamicsWorld::computeIslands() { stackIndex++; body->m_isAlreadyInIsland = true; // Create the new island - m_islands.pushBack(new Island(nbBodies, nbContactManifolds, m_joints.size())); + m_islands.pushBack(ETK_NEW(Island, nbBodies, nbContactManifolds, m_joints.size())); // While there are still some bodies to visit in the stack while (stackIndex > 0) { // Get the next body to visit from the stack @@ -544,13 +536,7 @@ void ephysics::DynamicsWorld::computeIslands() { otherBody->m_isAlreadyInIsland = true; } } - // Reset the isAlreadyIsland variable of the static bodies so that they - // can also be included in the other islands - for (uint32_t i=0; i < m_islands.back()->m_numberBodies; i++) { - if (m_islands.back()->m_bodies[i]->getType() == STATIC) { - m_islands.back()->m_bodies[i]->m_isAlreadyInIsland = false; - } - } + m_islands.back()->resetStaticBobyNotInIsland(); } } diff --git a/ephysics/engine/DynamicsWorld.hpp b/ephysics/engine/DynamicsWorld.hpp index 0ea6cf1..e75523c 100644 --- a/ephysics/engine/DynamicsWorld.hpp +++ b/ephysics/engine/DynamicsWorld.hpp @@ -34,12 +34,12 @@ namespace ephysics { vec3 m_gravity; //!< Gravity vector of the world float m_timeStep; //!< Current frame time step (in seconds) bool m_isGravityEnabled; //!< True if the gravity force is on - vec3* m_constrainedLinearVelocities; //!< Array of constrained linear velocities (state of the linear velocities after solving the constraints) - vec3* m_constrainedAngularVelocities; //!< Array of constrained angular velocities (state of the angular velocities after solving the constraints) - vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) - vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) - vec3* m_constrainedPositions; //!< Array of constrained rigid bodies position (for position error correction) - etk::Quaternion* m_constrainedOrientations; //!< Array of constrained rigid bodies orientation (for position error correction) + etk::Vector m_constrainedLinearVelocities; //!< Array of constrained linear velocities (state of the linear velocities after solving the constraints) + etk::Vector m_constrainedAngularVelocities; //!< Array of constrained angular velocities (state of the angular velocities after solving the constraints) + etk::Vector m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + etk::Vector m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + etk::Vector m_constrainedPositions; //!< Array of constrained rigid bodies position (for position error correction) + etk::Vector m_constrainedOrientations; //!< Array of constrained rigid bodies orientation (for position error correction) etk::Map m_mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the constrained velocities array etk::Vector m_islands; //!< Array with all the islands of awaken bodies uint32_t m_numberBodiesCapacity; //!< Current allocated capacity for the bodies diff --git a/ephysics/engine/Island.cpp b/ephysics/engine/Island.cpp index 48211e2..2c584da 100644 --- a/ephysics/engine/Island.cpp +++ b/ephysics/engine/Island.cpp @@ -7,79 +7,64 @@ * @license MPL v2.0 (see license file) */ #include +#include using namespace ephysics; -ephysics::Island::Island(uint32_t _nbMaxBodies, - uint32_t _nbMaxContactManifolds, - uint32_t _nbMaxJoints): - m_bodies(nullptr), - m_contactManifolds(nullptr), - m_joints(nullptr), - m_numberBodies(0), - m_numberContactManifolds(0), - m_numberJoints(0) { +ephysics::Island::Island(size_t _nbMaxBodies, + size_t _nbMaxContactManifolds, + size_t _nbMaxJoints) { // Allocate memory for the arrays - m_numberAllocatedBytesBodies = sizeof(RigidBody*) * _nbMaxBodies; - m_bodies = new RigidBody*[_nbMaxBodies]; - m_numberAllocatedBytesContactManifolds = sizeof(ContactManifold*) * _nbMaxContactManifolds; - m_contactManifolds = new ContactManifold*[_nbMaxContactManifolds]; - m_numberAllocatedBytesJoints = sizeof(Joint*) * _nbMaxJoints; - m_joints = new Joint*[_nbMaxJoints]; + m_bodies.reserve(_nbMaxBodies); + m_contactManifolds.reserve(_nbMaxContactManifolds); + m_joints.reserve(_nbMaxJoints); } -Island::~Island() { - // Release the memory of the arrays - delete[] m_bodies; - delete[] m_contactManifolds; - delete[] m_joints; + +void ephysics::Island::addBody(ephysics::RigidBody* _body) { + if (_body->isSleeping() == true) { + EPHY_ERROR("Try to add a body that is sleeping ..."); + return; + } + m_bodies.pushBack(_body); } -// Add a body int32_to the island -void Island::addBody(RigidBody* body) { - assert(!body->isSleeping()); - m_bodies[m_numberBodies] = body; - m_numberBodies++; +void ephysics::Island::addContactManifold(ephysics::ContactManifold* _contactManifold) { + m_contactManifolds.pushBack(_contactManifold); } -// Add a contact manifold int32_to the island -void Island::addContactManifold(ContactManifold* contactManifold) { - m_contactManifolds[m_numberContactManifolds] = contactManifold; - m_numberContactManifolds++; +void ephysics::Island::addJoint(ephysics::Joint* _joint) { + m_joints.pushBack(_joint); } -// Add a joint int32_to the island -void Island::addJoint(Joint* joint) { - m_joints[m_numberJoints] = joint; - m_numberJoints++; +size_t ephysics::Island::getNbBodies() const { + return m_bodies.size(); } -// Return the number of bodies in the island -uint32_t Island::getNbBodies() const { - return m_numberBodies; +size_t ephysics::Island::getNbContactManifolds() const { + return m_contactManifolds.size(); } -// Return the number of contact manifolds in the island -uint32_t Island::getNbContactManifolds() const { - return m_numberContactManifolds; +size_t ephysics::Island::getNbJoints() const { + return m_joints.size(); } -// Return the number of joints in the island -uint32_t Island::getNbJoints() const { - return m_numberJoints; +ephysics::RigidBody** ephysics::Island::getBodies() { + return &m_bodies[0]; } -// Return a pointer to the array of bodies -RigidBody** Island::getBodies() { - return m_bodies; +ephysics::ContactManifold** ephysics::Island::getContactManifold() { + return &m_contactManifolds[0]; } -// Return a pointer to the array of contact manifolds -ContactManifold** Island::getContactManifold() { - return m_contactManifolds; +ephysics::Joint** ephysics::Island::getJoints() { + return &m_joints[0]; } -// Return a pointer to the array of joints -Joint** Island::getJoints() { - return m_joints; +void ephysics::Island::resetStaticBobyNotInIsland() { + for (auto &it: m_bodies) { + if (it->getType() == STATIC) { + it->m_isAlreadyInIsland = false; + } + } } diff --git a/ephysics/engine/Island.hpp b/ephysics/engine/Island.hpp index 0e144d5..7fafaa9 100644 --- a/ephysics/engine/Island.hpp +++ b/ephysics/engine/Island.hpp @@ -19,43 +19,64 @@ namespace ephysics { */ class Island { private: - RigidBody** m_bodies; //!< Array with all the bodies of the island - ContactManifold** m_contactManifolds; //!< Array with all the contact manifolds between bodies of the island - Joint** m_joints; //!< Array with all the joints between bodies of the island - uint32_t m_numberBodies; //!< Current number of bodies in the island - uint32_t m_numberContactManifolds; //!< Current number of contact manifold in the island - uint32_t m_numberJoints; //!< Current number of joints in the island - size_t m_numberAllocatedBytesBodies; //!< Number of bytes allocated for the bodies array - size_t m_numberAllocatedBytesContactManifolds; //!< Number of bytes allocated for the contact manifolds array - size_t m_numberAllocatedBytesJoints; //!< Number of bytes allocated for the joints array - /// Private assignment operator - Island& operator=(const Island& island); - /// Private copy-constructor - Island(const Island& island); + etk::Vector m_bodies; //!< Array with all the bodies of the island + etk::Vector m_contactManifolds; //!< Array with all the contact manifolds between bodies of the island + etk::Vector m_joints; //!< Array with all the joints between bodies of the island + //! Remove assignment operator + Island& operator=(const Island& island) = delete; + //! Remove copy-constructor + Island(const Island& island) = delete; public: - /// Constructor - Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints); - /// Destructor - ~Island(); - /// Add a body int32_to the island - void addBody(RigidBody* body); - /// Add a contact manifold int32_to the island - void addContactManifold(ContactManifold* contactManifold); - /// Add a joint int32_to the island - void addJoint(Joint* joint); - /// Return the number of bodies in the island - uint32_t getNbBodies() const; - /// Return the number of contact manifolds in the island - uint32_t getNbContactManifolds() const; - /// Return the number of joints in the island - uint32_t getNbJoints() const; - /// Return a pointer to the array of bodies + /** + * @brief Constructor + */ + Island(size_t nbMaxBodies, size_t nbMaxContactManifolds, size_t nbMaxJoints); + /** + * @brief Destructor + */ + ~Island() = default; + /** + * Add a body. + */ + void addBody(RigidBody* _body); + /** + * Add a contact manifold. + */ + void addContactManifold(ContactManifold* _contactManifold); + /** + * Add a joint. + */ + void addJoint(Joint* _joint); + /** + * @brief Get the number of body + * @return Number of bodies. + */ + size_t getNbBodies() const; + /** + * @ Get the number of contact manifolds + * Return the number of contact manifolds in the island + */ + size_t getNbContactManifolds() const; + /** + * Return the number of joints in the island + */ + size_t getNbJoints() const; + /** + * Return a pointer to the array of bodies + */ RigidBody** getBodies(); - /// Return a pointer to the array of contact manifolds + /** + * Return a pointer to the array of contact manifolds + */ ContactManifold** getContactManifold(); - /// Return a pointer to the array of joints + /** + * Return a pointer to the array of joints + */ Joint** getJoints(); - friend class DynamicsWorld; + /** + * @brief Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands + */ + void resetStaticBobyNotInIsland(); }; diff --git a/ephysics/engine/Profiler.cpp b/ephysics/engine/Profiler.cpp index a1aae4d..321ac6a 100644 --- a/ephysics/engine/Profiler.cpp +++ b/ephysics/engine/Profiler.cpp @@ -30,9 +30,10 @@ ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) // Destructor ProfileNode::~ProfileNode() { - - delete m_childNode; - delete m_siblingNode; + ETK_DELETE(ProfileNode, m_childNode); + m_childNode = nullptr; + ETK_DELETE(ProfileNode, m_siblingNode); + m_siblingNode = nullptr; } // Return a pointer to a sub node with a given name @@ -49,7 +50,7 @@ ProfileNode* ProfileNode::findSubNode(const char* name) { // The nose has not been found. Therefore, we create it // and add it to the profiler tree - ProfileNode* newNode = new ProfileNode(name, this); + ProfileNode* newNode = ETK_NEW(ProfileNode, name, this); newNode->m_siblingNode = m_childNode; m_childNode = newNode; @@ -106,9 +107,9 @@ void ProfileNode::reset() { // Destroy the node void ProfileNode::destroy() { - delete m_childNode; + ETK_DELETE(ProfileNode, m_childNode); m_childNode = nullptr; - delete m_siblingNode; + ETK_DELETE(ProfileNode, m_siblingNode); m_siblingNode = nullptr; } @@ -337,12 +338,12 @@ void Profiler::incrementFrameCounter() { // Return an iterator over the profiler tree starting at the root ProfileNodeIterator* Profiler::getIterator() { - return new ProfileNodeIterator(&m_rootNode); + return ETK_NEW(ProfileNodeIterator(&m_rootNode)); } // Destroy a previously allocated iterator void Profiler::destroyIterator(ProfileNodeIterator* iterator) { - delete iterator; + ETK_DELETE(ProfileNodeIterator, iterator); } // Destroy the profiler (release the memory) diff --git a/test/testCollisionWorld.cpp b/test/testCollisionWorld.cpp index e3b5a8b..bc227d4 100644 --- a/test/testCollisionWorld.cpp +++ b/test/testCollisionWorld.cpp @@ -6,6 +6,8 @@ #include #include +#include + // Enumeration for categories enum CollisionCategory { CATEGORY_1 = 0x0001, @@ -32,24 +34,28 @@ class WorldCollisionCallback : public ephysics::CollisionCallback { sphere1CollideWithSphere2 = false; } // This method will be called for contact - virtual void notifyContact(const ephysics::ContactPointInfo& contactPointInfo) { - if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) { + virtual void notifyContact(const ephysics::ContactPointInfo& _contactPointInfo) { + if (isContactBetweenBodies(boxBody, sphere1Body, _contactPointInfo)) { + TEST_WARNING("plop 1 boxCollideWithSphere1"); boxCollideWithSphere1 = true; - } else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) { + } else if (isContactBetweenBodies(boxBody, cylinderBody, _contactPointInfo)) { + TEST_WARNING("plop 2 boxCollideWithCylinder"); boxCollideWithCylinder = true; - } else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) { + } else if (isContactBetweenBodies(sphere1Body, cylinderBody, _contactPointInfo)) { + TEST_WARNING("plop 3 sphere1CollideWithCylinder"); sphere1CollideWithCylinder = true; - } else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) { + } else if (isContactBetweenBodies(sphere1Body, sphere2Body, _contactPointInfo)) { + TEST_WARNING("plop 4 sphere1CollideWithSphere2"); sphere1CollideWithSphere2 = true; } } - bool isContactBetweenBodies(const ephysics::CollisionBody* body1, - const ephysics::CollisionBody* body2, - const ephysics::ContactPointInfo& contactPointInfo) { - return ( contactPointInfo.shape1->getBody()->getID() == body1->getID() - && contactPointInfo.shape2->getBody()->getID() == body2->getID() ) - || ( contactPointInfo.shape2->getBody()->getID() == body1->getID() - && contactPointInfo.shape1->getBody()->getID() == body2->getID() ); + bool isContactBetweenBodies(const ephysics::CollisionBody* _body1, + const ephysics::CollisionBody* _body2, + const ephysics::ContactPointInfo& _contactPointInfo) { + return ( _contactPointInfo.shape1->getBody()->getID() == _body1->getID() + && _contactPointInfo.shape2->getBody()->getID() == _body2->getID() ) + || ( _contactPointInfo.shape2->getBody()->getID() == _body1->getID() + && _contactPointInfo.shape1->getBody()->getID() == _body2->getID() ); } }; @@ -79,13 +85,13 @@ class TestCollisionWorld { public : TestCollisionWorld() { // Create the world - m_world = new ephysics::CollisionWorld(); + m_world = ETK_NEW(ephysics::CollisionWorld); // Create the bodies etk::Transform3D boxTransform(vec3(10, 0, 0), etk::Quaternion::identity()); m_boxBody = m_world->createCollisionBody(boxTransform); - m_boxShape = new ephysics::BoxShape(vec3(3, 3, 3)); + m_boxShape = ETK_NEW(ephysics::BoxShape, vec3(3,3,3)); m_boxProxyShape = m_boxBody->addCollisionShape(m_boxShape, etk::Transform3D::identity()); - m_sphereShape = new ephysics::SphereShape(3.0); + m_sphereShape = ETK_NEW(ephysics::SphereShape, 3.0); etk::Transform3D sphere1Transform(vec3(10,5, 0), etk::Quaternion::identity()); m_sphere1Body = m_world->createCollisionBody(sphere1Transform); m_sphere1ProxyShape = m_sphere1Body->addCollisionShape(m_sphereShape, etk::Transform3D::identity()); @@ -94,7 +100,7 @@ class TestCollisionWorld { m_sphere2ProxyShape = m_sphere2Body->addCollisionShape(m_sphereShape, etk::Transform3D::identity()); etk::Transform3D cylinderTransform(vec3(10, -5, 0), etk::Quaternion::identity()); m_cylinderBody = m_world->createCollisionBody(cylinderTransform); - m_cylinderShape = new ephysics::CylinderShape(2, 5); + m_cylinderShape = ETK_NEW(ephysics::CylinderShape, 2, 5); m_cylinderProxyShape = m_cylinderBody->addCollisionShape(m_cylinderShape, etk::Transform3D::identity()); // Assign collision categories to proxy shapes m_boxProxyShape->setCollisionCategoryBits(CATEGORY_1); @@ -107,9 +113,10 @@ class TestCollisionWorld { m_collisionCallback.cylinderBody = m_cylinderBody; } ~TestCollisionWorld() { - delete m_boxShape; - delete m_sphereShape; - delete m_cylinderShape; + ETK_DELETE(ephysics::BoxShape, m_boxShape); + ETK_DELETE(ephysics::SphereShape, m_sphereShape); + ETK_DELETE(ephysics::CylinderShape, m_cylinderShape); + ETK_DELETE(ephysics::CollisionWorld, m_world); } }; diff --git a/test/testPointInside.cpp b/test/testPointInside.cpp index 6b2fe2f..19adc39 100644 --- a/test/testPointInside.cpp +++ b/test/testPointInside.cpp @@ -50,7 +50,7 @@ class TestPointInside { public : TestPointInside() { // Create the world - m_world = new ephysics::CollisionWorld(); + m_world = ETK_NEW(ephysics::CollisionWorld); // Body transform vec3 position(-3, 2, 7); etk::Quaternion orientation(M_PI / 5, M_PI / 6, M_PI / 7, 1); @@ -71,15 +71,15 @@ class TestPointInside { // Compute the the transform from a local shape point to world-space m_localShapeToWorld = m_bodyTransform * m_shapeTransform; // Create collision shapes - m_boxShape = new ephysics::BoxShape(vec3(2, 3, 4), 0); + m_boxShape = ETK_NEW(ephysics::BoxShape, vec3(2, 3, 4), 0); m_boxProxyShape = m_boxBody->addCollisionShape(m_boxShape, m_shapeTransform); - m_sphereShape = new ephysics::SphereShape(3); + m_sphereShape = ETK_NEW(ephysics::SphereShape, 3); m_sphereProxyShape = m_sphereBody->addCollisionShape(m_sphereShape, m_shapeTransform); - m_capsuleShape = new ephysics::CapsuleShape(2, 10); + m_capsuleShape = ETK_NEW(ephysics::CapsuleShape, 2, 10); m_capsuleProxyShape = m_capsuleBody->addCollisionShape(m_capsuleShape, m_shapeTransform); - m_coneShape = new ephysics::ConeShape(2, 6, 0); + m_coneShape = ETK_NEW(ephysics::ConeShape, 2, 6, 0); m_coneProxyShape = m_coneBody->addCollisionShape(m_coneShape, m_shapeTransform); - m_convexMeshShape = new ephysics::ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) + m_convexMeshShape = ETK_NEW(ephysics::ConvexMeshShape, 0.0f); // Box of dimension (2, 3, 4) m_convexMeshShape->addVertex(vec3(-2, -3, -4)); m_convexMeshShape->addVertex(vec3(2, -3, -4)); m_convexMeshShape->addVertex(vec3(2, -3, 4)); @@ -89,7 +89,7 @@ class TestPointInside { m_convexMeshShape->addVertex(vec3(2, 3, 4)); m_convexMeshShape->addVertex(vec3(-2, 3, 4)); m_convexMeshProxyShape = m_convexMeshBody->addCollisionShape(m_convexMeshShape, m_shapeTransform); - m_convexMeshShapeBodyEdgesInfo = new ephysics::ConvexMeshShape(0.0); + m_convexMeshShapeBodyEdgesInfo = ETK_NEW(ephysics::ConvexMeshShape, 0.0f); m_convexMeshShapeBodyEdgesInfo->addVertex(vec3(-2, -3, -4)); m_convexMeshShapeBodyEdgesInfo->addVertex(vec3(2, -3, -4)); m_convexMeshShapeBodyEdgesInfo->addVertex(vec3(2, -3, 4)); @@ -111,10 +111,8 @@ class TestPointInside { m_convexMeshShapeBodyEdgesInfo->addEdge(2, 6); m_convexMeshShapeBodyEdgesInfo->addEdge(3, 7); m_convexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true); - m_convexMeshProxyShapeEdgesInfo = m_convexMeshBodyEdgesInfo->addCollisionShape( - m_convexMeshShapeBodyEdgesInfo, - m_shapeTransform); - m_cylinderShape = new ephysics::CylinderShape(3, 8, 0); + m_convexMeshProxyShapeEdgesInfo = m_convexMeshBodyEdgesInfo->addCollisionShape(m_convexMeshShapeBodyEdgesInfo, m_shapeTransform); + m_cylinderShape = ETK_NEW(ephysics::CylinderShape, 3, 8, 0); m_cylinderProxyShape = m_cylinderBody->addCollisionShape(m_cylinderShape, m_shapeTransform); // Compound shape is a cylinder and a sphere vec3 positionShape2(vec3(4, 2, -3)); @@ -126,13 +124,14 @@ class TestPointInside { } /// Destructor ~TestPointInside() { - delete m_boxShape; - delete m_sphereShape; - delete m_capsuleShape; - delete m_coneShape; - delete m_convexMeshShape; - delete m_convexMeshShapeBodyEdgesInfo; - delete m_cylinderShape; + ETK_DELETE(ephysics::BoxShape, m_boxShape); + ETK_DELETE(ephysics::SphereShape, m_sphereShape); + ETK_DELETE(ephysics::CapsuleShape, m_capsuleShape); + ETK_DELETE(ephysics::ConeShape, m_coneShape); + ETK_DELETE(ephysics::ConvexMeshShape, m_convexMeshShape); + ETK_DELETE(ephysics::ConvexMeshShape, m_convexMeshShapeBodyEdgesInfo); + ETK_DELETE(ephysics::CylinderShape, m_cylinderShape); + ETK_DELETE(ephysics::CollisionWorld, m_world); } }; diff --git a/test/testRaycast.cpp b/test/testRaycast.cpp index 41beab3..7e713b9 100644 --- a/test/testRaycast.cpp +++ b/test/testRaycast.cpp @@ -116,7 +116,7 @@ class TestRaycast { TestRaycast() { epsilon = float(0.0001); // Create the world - m_world = new ephysics::CollisionWorld(); + m_world = ETK_NEW(ephysics::CollisionWorld); // Body transform vec3 position(-3, 2, 7); etk::Quaternion orientation(M_PI / 5, M_PI / 6, M_PI / 7, 1.0f); @@ -144,26 +144,26 @@ class TestRaycast { m_localShapeToWorld = m_bodyTransform * m_shapeTransform; // Create collision shapes - m_boxShape = new ephysics::BoxShape(vec3(2, 3, 4), 0); + m_boxShape = ETK_NEW(ephysics::BoxShape, vec3(2, 3, 4), 0); m_boxProxyShape = m_boxBody->addCollisionShape(m_boxShape, m_shapeTransform); - m_sphereShape = new ephysics::SphereShape(3); + m_sphereShape = ETK_NEW(ephysics::SphereShape, 3); m_sphereProxyShape = m_sphereBody->addCollisionShape(m_sphereShape, m_shapeTransform); const vec3 triangleVertex1(100, 100, 0); const vec3 triangleVertex2(105, 100, 0); const vec3 triangleVertex3(100, 103, 0); - m_triangleShape = new ephysics::TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); + m_triangleShape = ETK_NEW(ephysics::TriangleShape, triangleVertex1, triangleVertex2, triangleVertex3); m_triangleProxyShape = m_triangleBody->addCollisionShape(m_triangleShape, m_shapeTransform); - m_capsuleShape = new ephysics::CapsuleShape(2, 5); + m_capsuleShape = ETK_NEW(ephysics::CapsuleShape, 2, 5); m_capsuleProxyShape = m_capsuleBody->addCollisionShape(m_capsuleShape, m_shapeTransform); - m_coneShape = new ephysics::ConeShape(2, 6, 0); + m_coneShape = ETK_NEW(ephysics::ConeShape, 2, 6, 0); m_coneProxyShape = m_coneBody->addCollisionShape(m_coneShape, m_shapeTransform); // Box of dimension (2, 3, 4) - m_convexMeshShape = new ephysics::ConvexMeshShape(0.0); + m_convexMeshShape = ETK_NEW(ephysics::ConvexMeshShape, 0.0f); m_convexMeshShape->addVertex(vec3(-2, -3, -4)); m_convexMeshShape->addVertex(vec3(2, -3, -4)); m_convexMeshShape->addVertex(vec3(2, -3, 4)); @@ -174,7 +174,7 @@ class TestRaycast { m_convexMeshShape->addVertex(vec3(-2, 3, 4)); m_convexMeshProxyShape = m_convexMeshBody->addCollisionShape(m_convexMeshShape, m_shapeTransform); - m_convexMeshShapeEdgesInfo = new ephysics::ConvexMeshShape(0.0); + m_convexMeshShapeEdgesInfo = ETK_NEW(ephysics::ConvexMeshShape, 0.0f); m_convexMeshShapeEdgesInfo->addVertex(vec3(-2, -3, -4)); m_convexMeshShapeEdgesInfo->addVertex(vec3(2, -3, -4)); m_convexMeshShapeEdgesInfo->addVertex(vec3(2, -3, 4)); @@ -200,7 +200,7 @@ class TestRaycast { m_convexMeshShapeEdgesInfo, m_shapeTransform); - m_cylinderShape = new ephysics::CylinderShape(2, 5, 0); + m_cylinderShape = ETK_NEW(ephysics::CylinderShape, 2, 5, 0); m_cylinderProxyShape = m_cylinderBody->addCollisionShape(m_cylinderShape, m_shapeTransform); // Compound shape is a cylinder and a sphere @@ -233,18 +233,18 @@ class TestRaycast { m_concaveMeshIndices.pushBack(1); m_concaveMeshIndices.pushBack(4); m_concaveMeshIndices.pushBack(5); m_concaveMeshIndices.pushBack(5); m_concaveMeshIndices.pushBack(7); m_concaveMeshIndices.pushBack(6); m_concaveMeshIndices.pushBack(4); m_concaveMeshIndices.pushBack(7); m_concaveMeshIndices.pushBack(5); - m_concaveMeshVertexArray = new ephysics::TriangleVertexArray(m_concaveMeshVertices, m_concaveMeshIndices); + m_concaveMeshVertexArray = ETK_NEW(ephysics::TriangleVertexArray, m_concaveMeshVertices, m_concaveMeshIndices); // Add the triangle vertex array of the subpart to the triangle mesh m_concaveTriangleMesh.addSubpart(m_concaveMeshVertexArray); - m_concaveMeshShape = new ephysics::ConcaveMeshShape(&m_concaveTriangleMesh); + m_concaveMeshShape = ETK_NEW(ephysics::ConcaveMeshShape, &m_concaveTriangleMesh); m_concaveMeshProxyShape = m_concaveMeshBody->addCollisionShape(m_concaveMeshShape, m_shapeTransform); // Heightfield shape (plane height field at height=4) for (int32_t i=0; i<100; i++) m_heightFieldData[i] = 4; - m_heightFieldShape = new ephysics::HeightFieldShape(10, 10, 0, 4, m_heightFieldData, ephysics::HeightFieldShape::HEIGHT_FLOAT_TYPE); + m_heightFieldShape = ETK_NEW(ephysics::HeightFieldShape, 10, 10, 0, 4, m_heightFieldData, ephysics::HeightFieldShape::HEIGHT_FLOAT_TYPE); m_heightFieldProxyShape = m_heightFieldBody->addCollisionShape(m_heightFieldShape, m_shapeTransform); // Assign proxy shapes to the different categories @@ -264,18 +264,18 @@ class TestRaycast { /// Destructor ~TestRaycast() { - delete m_boxShape; - delete m_sphereShape; - delete m_capsuleShape; - delete m_coneShape; - delete m_convexMeshShape; - delete m_convexMeshShapeEdgesInfo; - delete m_cylinderShape; - delete m_triangleShape; - delete m_concaveMeshShape; - delete m_heightFieldShape; - - delete m_concaveMeshVertexArray; + ETK_DELETE(ephysics::BoxShape, m_boxShape); + ETK_DELETE(ephysics::SphereShape, m_sphereShape); + ETK_DELETE(ephysics::CapsuleShape, m_capsuleShape); + ETK_DELETE(ephysics::ConeShape, m_coneShape); + ETK_DELETE(ephysics::ConvexMeshShape, m_convexMeshShape); + ETK_DELETE(ephysics::ConvexMeshShape, m_convexMeshShapeEdgesInfo); + ETK_DELETE(ephysics::CylinderShape, m_cylinderShape); + ETK_DELETE(ephysics::TriangleShape, m_triangleShape); + ETK_DELETE(ephysics::ConcaveShape, m_concaveMeshShape); + ETK_DELETE(ephysics::HeightFieldShape, m_heightFieldShape); + ETK_DELETE(ephysics::TriangleVertexArray, m_concaveMeshVertexArray); + ETK_DELETE(ephysics::CollisionWorld, m_world); } };