mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix two issues

Browse Source
This commit is contained in:
Daniel Chappuis 2013-05-02 23:55:10 +02:00
parent 0071ed16a8
commit af2fcaeb82
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/constraint/BallAndSocketJoint.cpp
View File

@ -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;

14
src/engine/DynamicsWorld.cpp
View File

@ -219,11 +219,15 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
RigidBody* rigidBody = *it;
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(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++;
}