From 46d19bf5507123f7128b6cf4ee4bd2330ae1ed3f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 21 Nov 2019 23:23:03 +0100 Subject: [PATCH] Some optimizations --- src/collision/shapes/CollisionShape.cpp | 2 +- src/collision/shapes/ConcaveMeshShape.cpp | 1 + src/components/ProxyShapeComponents.h | 1 + src/systems/CollisionDetectionSystem.cpp | 75 ++++---- src/systems/CollisionDetectionSystem.h | 2 +- src/systems/ContactSolverSystem.cpp | 205 +++++++++++----------- src/systems/ContactSolverSystem.h | 4 +- src/systems/DynamicsSystem.cpp | 33 ++-- 8 files changed, 161 insertions(+), 162 deletions(-) diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index f9cc40ec..5a49ecb1 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -59,7 +59,7 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { Vector3 maxBounds; getLocalBounds(minBounds, maxBounds); - const Vector3 translation = transform.getPosition(); + const Vector3& translation = transform.getPosition(); Matrix3x3 matrix = transform.getOrientation().getMatrix(); Vector3 resultMin; Vector3 resultMax; diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 18518bad..90e26078 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -35,6 +35,7 @@ using namespace reactphysics3d; // Constructor ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling) + // TODO : Do not use the default base allocator here : ConcaveShape(CollisionShapeName::TRIANGLE_MESH), mDynamicAABBTree(MemoryManager::getBaseAllocator()), mScaling(scaling) { mTriangleMesh = triangleMesh; diff --git a/src/components/ProxyShapeComponents.h b/src/components/ProxyShapeComponents.h index 18ab9053..0514e8b7 100644 --- a/src/components/ProxyShapeComponents.h +++ b/src/components/ProxyShapeComponents.h @@ -195,6 +195,7 @@ class ProxyShapeComponents : public Components { friend class BroadPhaseSystem; friend class CollisionDetectionSystem; + friend class DynamicsSystem; }; // Return the body entity of a given proxy-shape diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 165de8ec..d0ea855e 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -91,7 +91,7 @@ void CollisionDetectionSystem::computeCollisionDetection() { computeBroadPhase(); // Compute the middle-phase collision detection - computeMiddlePhase(mOverlappingPairs, mNarrowPhaseInput); + computeMiddlePhase(mNarrowPhaseInput); // Compute the narrow-phase collision detection computeNarrowPhase(); @@ -204,7 +204,7 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List& const Entity proxyShape1Entity = mOverlappingPairs.mProxyShapes1[pairIndex]; const Entity proxyShape2Entity = mOverlappingPairs.mProxyShapes2[pairIndex]; + const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1Entity); + const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2Entity); + assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); - CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity); - CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity); + CollisionShape* collisionShape1 = mProxyShapesComponents.mCollisionShapes[proxyShape1Index]; + CollisionShape* collisionShape2 = mProxyShapesComponents.mCollisionShapes[proxyShape2Index]; NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex]; // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection narrowPhaseInput.addNarrowPhaseTest(pairId, pairIndex, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, - mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), - mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), + mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index], + mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index], algorithmType, mMemoryManager.getSingleFrameAllocator()); } @@ -314,6 +317,7 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& computeConvexVsConcaveMiddlePhase(pairIndex, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); } } + // Compute the concave vs convex middle-phase algorithm for a given pair of bodies void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairIndex, MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) { @@ -325,32 +329,29 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1); const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2); - ProxyShape* shape1 = mProxyShapesComponents.mProxyShapes[proxyShape1Index]; - ProxyShape* shape2 = mProxyShapesComponents.mProxyShapes[proxyShape2Index]; + Transform& shape1LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index]; + Transform& shape2LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index]; - ConvexShape* convexShape; - ConcaveShape* concaveShape; - - const bool isShape1Convex = mOverlappingPairs.mIsShape1Convex[pairIndex]; + Transform convexToConcaveTransform; // Collision shape 1 is convex, collision shape 2 is concave + ConvexShape* convexShape; + ConcaveShape* concaveShape; + const bool isShape1Convex = mOverlappingPairs.mIsShape1Convex[pairIndex]; if (isShape1Convex) { - convexShape = static_cast(shape1->getCollisionShape()); - concaveShape = static_cast(shape2->getCollisionShape()); + convexShape = static_cast(mProxyShapesComponents.mCollisionShapes[proxyShape1Index]); + concaveShape = static_cast(mProxyShapesComponents.mCollisionShapes[proxyShape2Index]); + convexToConcaveTransform = shape2LocalToWorldTransform.getInverse() * shape1LocalToWorldTransform; } else { // Collision shape 2 is convex, collision shape 1 is concave - convexShape = static_cast(shape2->getCollisionShape()); - concaveShape = static_cast(shape1->getCollisionShape()); + convexShape = static_cast(mProxyShapesComponents.mCollisionShapes[proxyShape1Index]); + concaveShape = static_cast(mProxyShapesComponents.mCollisionShapes[proxyShape2Index]); + convexToConcaveTransform = shape1LocalToWorldTransform.getInverse() * shape2LocalToWorldTransform; } assert(mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex] != NarrowPhaseAlgorithmType::None); - Transform& shape1LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index]; - Transform& shape2LocalToWorldTransform = mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index]; - // Compute the convex shape AABB in the local-space of the convex shape - const Transform convexToConcaveTransform = (isShape1Convex ? shape2LocalToWorldTransform : shape1LocalToWorldTransform).getInverse() * - (isShape1Convex ? shape1LocalToWorldTransform : shape2LocalToWorldTransform); AABB aabb; convexShape->computeAABB(aabb, convexToConcaveTransform); @@ -1360,7 +1361,7 @@ void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) { computeBroadPhase(); // Compute the middle-phase collision detection - computeMiddlePhase(mOverlappingPairs, narrowPhaseInput); + computeMiddlePhase(narrowPhaseInput); // Compute the narrow-phase collision detection and report overlapping shapes computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback); @@ -1444,7 +1445,7 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) { computeBroadPhase(); // Compute the middle-phase collision detection - computeMiddlePhase(mOverlappingPairs, narrowPhaseInput); + computeMiddlePhase(narrowPhaseInput); // Compute the narrow-phase collision detection and report contacts computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback); diff --git a/src/systems/CollisionDetectionSystem.h b/src/systems/CollisionDetectionSystem.h index 8128c03e..582d6454 100644 --- a/src/systems/CollisionDetectionSystem.h +++ b/src/systems/CollisionDetectionSystem.h @@ -176,7 +176,7 @@ class CollisionDetectionSystem { void computeBroadPhase(); /// Compute the middle-phase collision detection - void computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput); + void computeMiddlePhase(NarrowPhaseInput& narrowPhaseInput); // Compute the middle-phase collision detection void computeMiddlePhaseCollisionSnapshot(List& convexPairs, List& concavePairs, NarrowPhaseInput& narrowPhaseInput); diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 3d783e35..a6c9ea0c 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -124,19 +124,21 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2)); - // Get the position of the two bodies - const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity1); - const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity2); + const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1); + const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2); - // Initialize the internal contact manifold structure using the external - // contact manifold + // Get the position of the two bodies + const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1]; + const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex2]; + + // Initialize the internal contact manifold structure using the external contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity()); - mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity()); + mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1; + mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2; mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2); - mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity()); - mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity()); + mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1]; + mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); @@ -146,15 +148,15 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero(); // Get the velocities of the bodies - const Vector3& v1 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity1); - const Vector3& w1 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity1); - const Vector3& v2 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity2); - const Vector3& w2 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity2); + const Vector3& v1 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex1]; + const Vector3& w1 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex1]; + const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2]; + const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2]; // For each contact point of the contact manifold assert(externalManifold.nbContactPoints > 0); uint contactPointsStartIndex = externalManifold.contactPointsIndex; - uint nbContactPoints = externalManifold.nbContactPoints; + uint nbContactPoints = static_cast(externalManifold.nbContactPoints); for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) { ContactPoint& externalContact = (*mAllContactPoints)[c]; @@ -350,22 +352,22 @@ void ContactSolverSystem::warmStart() { Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -405,12 +407,12 @@ void ContactSolverSystem::warmStart() { mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifold ----- // @@ -426,18 +428,18 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // @@ -451,10 +453,10 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Rolling resistance at the center of the contact manifold ------ // @@ -462,10 +464,10 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; } else { // If it is a new contact manifold @@ -495,10 +497,10 @@ void ContactSolverSystem::solve() { decimal sumPenetrationImpulse = 0.0; // Get the constrained velocities - const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; - const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1]; - const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; - const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2]; + const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; + const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; + const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; + const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; for (short int i=0; igetMaterial().getBounciness(); decimal restitution2 = body2->getMaterial().getBounciness(); @@ -768,16 +769,14 @@ decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1, } // Compute the mixed friction coefficient from the friction coefficient of each body -decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody *body1, - RigidBody *body2) const { +decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) const { // Use the geometric mean to compute the mixed friction coefficient return std::sqrt(body1->getMaterial().getFrictionCoefficient() * body2->getMaterial().getFrictionCoefficient()); } // Compute th mixed rolling resistance factor between two bodies -inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1, - RigidBody* body2) const { +inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const { return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } @@ -825,12 +824,12 @@ void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { + decimal lengthTangentVelocity = tangentVelocity.length(); + if (lengthTangentVelocity > MACHINE_EPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + contact.frictionVector1 = tangentVelocity / lengthTangentVelocity; } else { diff --git a/src/systems/ContactSolverSystem.h b/src/systems/ContactSolverSystem.h index f6c9a2f5..83b7faba 100644 --- a/src/systems/ContactSolverSystem.h +++ b/src/systems/ContactSolverSystem.h @@ -176,10 +176,10 @@ class ContactSolverSystem { ContactManifold* externalContactManifold; /// Index of body 1 in the dynamics components arrays - uint32 dynamicsComponentIndexBody1; + uint32 rigidBodyComponentIndexBody1; /// Index of body 2 in the dynamics components arrays - uint32 dynamicsComponentIndexBody2; + uint32 rigidBodyComponentIndexBody2; /// Inverse of the mass of body 1 decimal massInverseBody1; diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index 1b7278a5..63bd575e 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -77,8 +77,8 @@ void DynamicsSystem::updateBodiesState() { for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { // Update the linear and angular velocity of the body - mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]); - mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]); + mRigidBodyComponents.mLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i]; + mRigidBodyComponents.mAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i]; // Update the position of the center of mass of the body mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i]; @@ -98,17 +98,11 @@ void DynamicsSystem::updateBodiesState() { } // Update the local-to-world transform of the proxy-shapes - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + for (uint32 i=0; i < mProxyShapeComponents.getNbEnabledComponents(); i++) { - // For all the proxy collision shapes of the body - const List& proxyShapesEntities = mCollisionBodyComponents.getProxyShapes(mRigidBodyComponents.mBodiesEntities[i]); - for (uint j=0; j < proxyShapesEntities.size(); j++) { - - // Update the local-to-world transform of the proxy-shape - mProxyShapeComponents.setLocalToWorldTransform(proxyShapesEntities[j], - mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]) * - mProxyShapeComponents.getLocalToBodyTransform(proxyShapesEntities[j])); - } + // Update the local-to-world transform of the proxy-shape + mProxyShapeComponents.mLocalToWorldTransforms[i] = mTransformComponents.getTransform(mProxyShapeComponents.mBodiesEntities[i]) * + mProxyShapeComponents.mLocalToBodyTransforms[i]; } } @@ -141,14 +135,17 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { } // Apply gravity force - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + if (mIsGravityEnabled) { - // If the gravity has to be applied to this rigid body - if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) { + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - // Integrate the gravity force - mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep * - mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity; + // If the gravity has to be applied to this rigid body + if (mRigidBodyComponents.mIsGravityEnabled[i]) { + + // Integrate the gravity force + mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep * + mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity; + } } }