diff --git a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp index fbc004e16..970913197 100644 --- a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp @@ -27,12 +27,27 @@ Generic6DofConstraint::Generic6DofConstraint() { } -Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB ) +Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB) : TypedConstraint(rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB) { - +m_accumulatedLinearImpulse[0] = 0.0f; +m_accumulatedLinearImpulse[1] = 0.0f; +m_accumulatedLinearImpulse[2] = 0.0f; + +m_accumulatedAngularImpulse[0] = 0.0f; +m_accumulatedAngularImpulse[1] = 0.0f; +m_accumulatedAngularImpulse[2] = 0.0f; + +m_angularLowerLimit[0] = 0.0f; +m_angularLowerLimit[1] = 0.0f; +m_angularLowerLimit[2] = 0.0f; + +m_angularUpperLimit[0] = 0.0f; +m_angularUpperLimit[1] = 0.0f; +m_angularUpperLimit[2] = 0.0f; + } @@ -43,12 +58,19 @@ void Generic6DofConstraint::BuildJacobian() const SimdVector3& pivotInA = m_frameInA.getOrigin(); const SimdVector3& pivotInB = m_frameInB.getOrigin(); + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + int i; //linear part for (i=0;i<3;i++) { normal[i] = 1; - + + // Create linear atom new (&m_jac[i]) JacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), @@ -60,37 +82,36 @@ void Generic6DofConstraint::BuildJacobian() m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedLinearImpulse[i] * normal; + + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + normal[i] = 0; } // angular part for (i=0;i<3;i++) { - #if 0 - - SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i); - SimdVector3 axisInB = m_frameInB.getBasis().getColumn(i); - - new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB, - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - #else - SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel + // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe SimdVector3 axis = kSign[i] * axisA.cross(axisB); + // Create angular atom new (&m_jacAng[i]) JacobianEntry(axis, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - #endif + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedAngularImpulse[i] * axis; + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); } } @@ -126,6 +147,7 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + m_accumulatedLinearImpulse[i] += impulse; SimdVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse( impulse_vector, rel_pos1); @@ -154,6 +176,7 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) //impulse SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + m_accumulatedAngularImpulse[i] += impulse; // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) SimdVector3 axis = kSign[i] * axisA.cross(axisB); @@ -169,3 +192,52 @@ void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep) } +SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const + { + SimdScalar angle; + + switch (axis) + { + case 0: + { + SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); + SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + + SimdScalar s = v1.dot(w2); + SimdScalar c = v1.dot(v2); + + angle = SimdAtan2( s, c ); + } + break; + + case 1: + { + SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); + SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + + SimdScalar s = w1.dot(u2); + SimdScalar c = w1.dot(w2); + + angle = SimdAtan2( s, c ); + } + break; + + case 2: + { + SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); + SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + + SimdScalar s = u1.dot(v2); + SimdScalar c = u1.dot(u2); + + angle = SimdAtan2( s, c ); + } + break; + } + + return angle; + } + diff --git a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h index 01284521d..ba7fe3421 100644 --- a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h +++ b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h @@ -33,12 +33,14 @@ class Generic6DofConstraint : public TypedConstraint JacobianEntry m_jac[3]; // 3 orthogonal linear constraints JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints - SimdTransform m_frameInA; // the constraint space w.r.t body A SimdTransform m_frameInB; // the constraint space w.r.t body B - SimdScalar m_lowerLimit[6]; // the constraint lower limits - SimdScalar m_upperLimit[6]; // the constraint upper limits + SimdScalar m_accumulatedLinearImpulse[3]; + SimdScalar m_accumulatedAngularImpulse[3]; + + SimdScalar m_angularLowerLimit[3]; // the constraint lower limits + SimdScalar m_angularUpperLimit[3]; // the constraint upper limits public: @@ -52,6 +54,14 @@ public: void UpdateRHS(SimdScalar timeStep); + SimdScalar ComputeAngle(int axis) const; + + void SetAngularLimit(int axis, SimdScalar lo, SimdScalar hi) + { + m_angularLowerLimit[axis] = lo; + m_angularUpperLimit[axis] = hi; + } + const RigidBody& GetRigidBodyA() const { return m_rbA;