Modifications in the BallAndSocketJoint to make it work
This commit is contained in:
parent
af2fcaeb82
commit
da78e5d79a
|
@ -167,7 +167,7 @@ void Scene::createBallAndSocketJoints() {
|
||||||
// --------------- Create the second box --------------- //
|
// --------------- Create the second box --------------- //
|
||||||
|
|
||||||
// Position of the box
|
// Position of the box
|
||||||
openglframework::Vector3 positionBox2(0, 10, 0);
|
openglframework::Vector3 positionBox2(5, 10, 0);
|
||||||
|
|
||||||
// Create a box and a corresponding rigid in the dynamics world
|
// Create a box and a corresponding rigid in the dynamics world
|
||||||
mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld);
|
mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld);
|
||||||
|
@ -184,7 +184,7 @@ void Scene::createBallAndSocketJoints() {
|
||||||
rp3d::BallAndSocketJointInfo jointInfo;
|
rp3d::BallAndSocketJointInfo jointInfo;
|
||||||
jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody();
|
jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody();
|
||||||
jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody();
|
jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody();
|
||||||
jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 12.5, 0);
|
jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 10, 0);
|
||||||
|
|
||||||
// Create the joint in the dynamics world
|
// Create the joint in the dynamics world
|
||||||
mBallAndSocketJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(
|
mBallAndSocketJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(
|
||||||
|
|
|
@ -135,7 +135,7 @@ class RigidBody : public CollisionBody {
|
||||||
decimal getMassInverse() const;
|
decimal getMassInverse() const;
|
||||||
|
|
||||||
/// Return the local inertia tensor of the body (in body coordinates)
|
/// Return the local inertia tensor of the body (in body coordinates)
|
||||||
Matrix3x3 getInertiaTensorLocal() const;
|
const Matrix3x3& getInertiaTensorLocal() const;
|
||||||
|
|
||||||
/// Set the local inertia tensor of the body (in body coordinates)
|
/// Set the local inertia tensor of the body (in body coordinates)
|
||||||
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
|
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
|
||||||
|
@ -222,7 +222,7 @@ inline decimal RigidBody::getMassInverse() const {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the local inertia tensor of the body (in body coordinates)
|
// Return the local inertia tensor of the body (in body coordinates)
|
||||||
inline Matrix3x3 RigidBody::getInertiaTensorLocal() const {
|
inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
|
||||||
return mInertiaTensorLocal;
|
return mInertiaTensorLocal;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,10 +34,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
|
||||||
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
|
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
mLocalAnchorPointBody1 = mBody1->getTransform().getOrientation().getInverse() *
|
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
jointInfo.anchorPointWorldSpace;
|
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
|
||||||
mLocalAnchorPointBody2 = mBody2->getTransform().getOrientation().getInverse() *
|
|
||||||
jointInfo.anchorPointWorldSpace;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Destructor
|
// Destructor
|
||||||
|
@ -53,8 +51,6 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
||||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
|
||||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
|
||||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||||
|
|
||||||
|
@ -62,13 +58,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
||||||
Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
|
Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
|
||||||
Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
|
Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute the vector from body center to anchor point in local-space
|
|
||||||
const Vector3 u1Local = mLocalAnchorPointBody1 - x1;
|
|
||||||
const Vector3 u2Local = mLocalAnchorPointBody2 - x2;
|
|
||||||
|
|
||||||
// Compute the vector from body center to the anchor point in world-space
|
// Compute the vector from body center to the anchor point in world-space
|
||||||
mU1World = orientationBody1 * u1Local;
|
mU1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||||
mU2World = orientationBody2 * u2Local;
|
mU2World = orientationBody2 * mLocalAnchorPointBody2;
|
||||||
|
|
||||||
// Compute the corresponding skew-symmetric matrices
|
// Compute the corresponding skew-symmetric matrices
|
||||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
|
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
|
||||||
|
@ -79,8 +71,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
||||||
Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
|
Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
|
||||||
0, inverseMassBodies, 0,
|
0, inverseMassBodies, 0,
|
||||||
0, 0, inverseMassBodies) +
|
0, 0, inverseMassBodies) +
|
||||||
skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * skewSymmetricMatrixU1+
|
skewSymmetricMatrixU1 * inverseInertiaTensorBody1 *
|
||||||
skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * skewSymmetricMatrixU2;
|
skewSymmetricMatrixU1.getTranspose() +
|
||||||
|
skewSymmetricMatrixU2 * inverseInertiaTensorBody2 *
|
||||||
|
skewSymmetricMatrixU2.getTranspose();
|
||||||
|
|
||||||
// Compute the inverse mass matrix K
|
// Compute the inverse mass matrix K
|
||||||
mInverseMassMatrix = massMatrix.getInverse();
|
mInverseMassMatrix = massMatrix.getInverse();
|
||||||
|
@ -109,8 +103,8 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
|
||||||
Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
|
Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
|
||||||
|
|
||||||
// Compute the bias "b" of the constraint
|
// Compute the bias "b" of the constraint
|
||||||
decimal beta = 0.7; // TODO : Use a constant here
|
decimal beta = decimal(0.2); // TODO : Use a constant here
|
||||||
decimal biasFactor = -(beta/constraintSolverData.timeStep);
|
decimal biasFactor = (beta / constraintSolverData.timeStep);
|
||||||
Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World);
|
Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World);
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
|
@ -119,9 +113,9 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda
|
// Compute the impulse P=J^T * lambda
|
||||||
Vector3 linearImpulseBody1 = -deltaLambda;
|
Vector3 linearImpulseBody1 = -deltaLambda;
|
||||||
Vector3 angularImpulseBody1 = mU1World.cross(deltaLambda);
|
Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World);
|
||||||
Vector3 linearImpulseBody2 = deltaLambda;
|
Vector3 linearImpulseBody2 = deltaLambda;
|
||||||
Vector3 angularImpulseBody2 = -mU2World.cross(deltaLambda);
|
Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World);
|
||||||
|
|
||||||
// Apply the impulse to the bodies of the joint
|
// Apply the impulse to the bodies of the joint
|
||||||
if (mBody1->getIsMotionEnabled()) {
|
if (mBody1->getIsMotionEnabled()) {
|
||||||
|
|
|
@ -407,11 +407,14 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
|
||||||
// Remove the joint from the world
|
// Remove the joint from the world
|
||||||
mJoints.erase(joint);
|
mJoints.erase(joint);
|
||||||
|
|
||||||
|
// Get the size in bytes of the joint
|
||||||
|
size_t nbBytes = joint->getSizeInBytes();
|
||||||
|
|
||||||
// Call the destructor of the joint
|
// Call the destructor of the joint
|
||||||
joint->Constraint::~Constraint();
|
joint->Constraint::~Constraint();
|
||||||
|
|
||||||
// Release the allocated memory
|
// Release the allocated memory
|
||||||
mMemoryAllocator.release(joint, joint->getSizeInBytes());
|
mMemoryAllocator.release(joint, nbBytes);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Notify the world about a new broad-phase overlapping pair
|
// Notify the world about a new broad-phase overlapping pair
|
||||||
|
|
Loading…
Reference in New Issue
Block a user