From d7ce09591061320b176a0794bb6024665cb0fa60 Mon Sep 17 00:00:00 2001 From: Brian Jensen Date: Sat, 16 Nov 2019 09:45:01 +0100 Subject: [PATCH] cache contact point and associated vectors, improves performance usage of solve call from approx. 15% to 13.5% for a specific scene --- src/engine/ContactSolver.cpp | 109 ++++++++++++++++++----------------- 1 file changed, 57 insertions(+), 52 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d114725f..ab360445 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -509,63 +509,68 @@ void ContactSolver::solve() { for (short int i=0; i SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; + if (contractPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contractPoint.penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + contractPoint.restitutionBias; // Compute the Lagrange multiplier lambda if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) * - mContactPoints[contactPointIndex].inversePenetrationMass; + deltaLambda = - (Jv + contractPoint.restitutionBias) * + contractPoint.inversePenetrationMass; } else { - deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass; + deltaLambda = - (Jv + b) * contractPoint.inversePenetrationMass; } - lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse; - mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse + + lambdaTemp = contractPoint.penetrationImpulse; + contractPoint.penetrationImpulse = std::max(contractPoint.penetrationImpulse + deltaLambda, decimal(0.0)); - deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; + deltaLambda = contractPoint.penetrationImpulse - lambdaTemp; - Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda, - mContactPoints[contactPointIndex].normal.y * deltaLambda, - mContactPoints[contactPointIndex].normal.z * deltaLambda); + Vector3 linearImpulse(contractPointNormal.x * deltaLambda, + contractPointNormal.y * deltaLambda, + contractPointNormal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P mLinearVelocities[contactConstraintIndexBody1].x -= contactConstrait.massInverseBody1 * linearImpulse.x; mLinearVelocities[contactConstraintIndexBody1].y -= contactConstrait.massInverseBody1 * linearImpulse.y; mLinearVelocities[contactConstraintIndexBody1].z -= contactConstrait.massInverseBody1 * linearImpulse.z; - mAngularVelocities[contactConstraintIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; - mAngularVelocities[contactConstraintIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; - mAngularVelocities[contactConstraintIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; + mAngularVelocities[contactConstraintIndexBody1].x -= contractPoint.i1TimesR1CrossN.x * deltaLambda; + mAngularVelocities[contactConstraintIndexBody1].y -= contractPoint.i1TimesR1CrossN.y * deltaLambda; + mAngularVelocities[contactConstraintIndexBody1].z -= contractPoint.i1TimesR1CrossN.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P mLinearVelocities[contactConstraintIndexBody2].x += contactConstrait.massInverseBody2 * linearImpulse.x; mLinearVelocities[contactConstraintIndexBody2].y += contactConstrait.massInverseBody2 * linearImpulse.y; mLinearVelocities[contactConstraintIndexBody2].z += contactConstrait.massInverseBody2 * linearImpulse.z; - mAngularVelocities[contactConstraintIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; - mAngularVelocities[contactConstraintIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; - mAngularVelocities[contactConstraintIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; + mAngularVelocities[contactConstraintIndexBody2].x += contractPoint.i2TimesR2CrossN.x * deltaLambda; + mAngularVelocities[contactConstraintIndexBody2].y += contractPoint.i2TimesR2CrossN.y * deltaLambda; + mAngularVelocities[contactConstraintIndexBody2].z += contractPoint.i2TimesR2CrossN.z * deltaLambda; - sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; + sumPenetrationImpulse += contractPoint.penetrationImpulse; // If the split impulse position correction is active if (mIsSplitImpulseActive) { @@ -576,45 +581,45 @@ void ContactSolver::solve() { const Vector3& v2Split = mSplitLinearVelocities[contactConstraintIndexBody2]; const Vector3& w2Split = mSplitAngularVelocities[contactConstraintIndexBody2]; - //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); - Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - - w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y, - v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y - - w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z, - v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z - - w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x); - decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x + - deltaVSplit.y * mContactPoints[contactPointIndex].normal.y + - deltaVSplit.z * mContactPoints[contactPointIndex].normal.z; + //Vector3 deltaVSplit = v2Split + w2Split.cross(contractPointR2) - v1Split - w1Split.cross(contractPointR1); + Vector3 deltaVSplit(v2Split.x + w2Split.y * contractPointR2.z - w2Split.z * contractPointR2.y - v1Split.x - + w1Split.y * contractPointR1.z + w1Split.z * contractPointR1.y, + v2Split.y + w2Split.z * contractPointR2.x - w2Split.x * contractPointR2.z - v1Split.y - + w1Split.z * contractPointR1.x + w1Split.x * contractPointR1.z, + v2Split.z + w2Split.x * contractPointR2.y - w2Split.y * contractPointR2.x - v1Split.z - + w1Split.x * contractPointR1.y + w1Split.y * contractPointR1.x); + decimal JvSplit = deltaVSplit.x * contractPointNormal.x + + deltaVSplit.y * contractPointNormal.y + + deltaVSplit.z * contractPointNormal.z; decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - mContactPoints[contactPointIndex].inversePenetrationMass; - decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; - mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max( - mContactPoints[contactPointIndex].penetrationSplitImpulse + + contractPoint.inversePenetrationMass; + decimal lambdaTempSplit = contractPoint.penetrationSplitImpulse; + contractPoint.penetrationSplitImpulse = std::max( + contractPoint.penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; + deltaLambdaSplit = contractPoint.penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit, - mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit, - mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); + Vector3 linearImpulse(contractPointNormal.x * deltaLambdaSplit, + contractPointNormal.y * deltaLambdaSplit, + contractPointNormal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P mSplitLinearVelocities[contactConstraintIndexBody1].x -= contactConstrait.massInverseBody1 * linearImpulse.x; mSplitLinearVelocities[contactConstraintIndexBody1].y -= contactConstrait.massInverseBody1 * linearImpulse.y; mSplitLinearVelocities[contactConstraintIndexBody1].z -= contactConstrait.massInverseBody1 * linearImpulse.z; - mSplitAngularVelocities[contactConstraintIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; - mSplitAngularVelocities[contactConstraintIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; - mSplitAngularVelocities[contactConstraintIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody1].x -= contractPoint.i1TimesR1CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody1].y -= contractPoint.i1TimesR1CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody1].z -= contractPoint.i1TimesR1CrossN.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P mSplitLinearVelocities[contactConstraintIndexBody2].x += contactConstrait.massInverseBody2 * linearImpulse.x; mSplitLinearVelocities[contactConstraintIndexBody2].y += contactConstrait.massInverseBody2 * linearImpulse.y; mSplitLinearVelocities[contactConstraintIndexBody2].z += contactConstrait.massInverseBody2 * linearImpulse.z; - mSplitAngularVelocities[contactConstraintIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; - mSplitAngularVelocities[contactConstraintIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; - mSplitAngularVelocities[contactConstraintIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody2].x += contractPoint.i2TimesR2CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody2].y += contractPoint.i2TimesR2CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[contactConstraintIndexBody2].z += contractPoint.i2TimesR2CrossN.z * deltaLambdaSplit; } contactPointIndex++;