From af2fcaeb8213c2848b95f0943a2b4ad14af89121 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 2 May 2013 23:55:10 +0200 Subject: [PATCH] Fix two issues --- src/constraint/BallAndSocketJoint.cpp | 10 ++++++---- src/engine/DynamicsWorld.cpp | 14 +++++++++----- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 214b7337..8d7ae17d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -34,8 +34,10 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo) : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) { // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody1 = mBody1->getTransform().getOrientation().getInverse() * + jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = mBody2->getTransform().getOrientation().getInverse() * + jointInfo.anchorPointWorldSpace; } // Destructor @@ -107,13 +109,13 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData) Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); // Compute the bias "b" of the constraint - decimal beta = 0.8; // TODO : Use a constant here + decimal beta = 0.7; // TODO : Use a constant here decimal biasFactor = -(beta/constraintSolverData.timeStep); Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World); // Compute the Lagrange multiplier lambda Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); - mImpulse = mImpulse + deltaLambda; + mImpulse += deltaLambda; // Compute the impulse P=J^T * lambda Vector3 linearImpulseBody1 = -deltaLambda; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index aac339ac..8dce0ece 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -219,11 +219,15 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { RigidBody* rigidBody = *it; mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(rigidBody, i)); - // Integrate the external force to get the new velocity of the body - mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + - dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + - dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + // If the body is allowed to move + if (rigidBody->getIsMotionEnabled()) { + + // Integrate the external force to get the new velocity of the body + mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); + mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + } i++; }