diff --git a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp index 5bf88e2d5..65a5caeb3 100644 --- a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp @@ -63,7 +63,7 @@ void Generic6DofConstraint::BuildJacobian() for (int i=0;i<3;i++) { SimdVector3 axisInA = m_frameInA.getBasis().getColumn(i); - SimdVector3 axisInB = m_frameInA.getBasis().getColumn(i); + SimdVector3 axisInB = m_frameInB.getBasis().getColumn(i); new (&m_jacAng[i]) JacobianEntry(axisInA, axisInB, m_rbA.getInvInertiaDiagLocal(), @@ -73,7 +73,7 @@ void Generic6DofConstraint::BuildJacobian() void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) { - SimdScalar tau = 0.3f; + SimdScalar tau = 0.0f; SimdScalar damping = 1.0f; SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); @@ -82,14 +82,23 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); +//#define JACOBIAN_STYLE +#ifdef JACOBIAN_STYLE + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); +#endif + SimdVector3 normal(0,0,0); // linear for (int i=0;i<3;i++) { + #ifndef JACOBIAN_STYLE + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + #endif + normal[i] = 1; SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); @@ -112,6 +121,11 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) // angular for (int i=0;i<3;i++) { + #ifndef JACOBIAN_STYLE + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + #endif + SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); //velocity error (first order error) @@ -128,8 +142,8 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) SimdScalar impulse = (tau*rel_pos/timeStep - damping*rel_vel) * jacDiagABInv; SimdVector3 impulse_vector = axisA * impulse; - //m_rbA.applyTorqueImpulse( impulse_vector); - //m_rbB.applyTorqueImpulse(-impulse_vector); + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); } } diff --git a/BulletDynamics/ConstraintSolver/JacobianEntry.h b/BulletDynamics/ConstraintSolver/JacobianEntry.h index 02d5f2240..d2fd982aa 100644 --- a/BulletDynamics/ConstraintSolver/JacobianEntry.h +++ b/BulletDynamics/ConstraintSolver/JacobianEntry.h @@ -77,7 +77,7 @@ public: const SimdVector3& inertiaInvB) : m_linearJointAxis(SimdVector3(0.f,0.f,0.f)) , m_aJ(axisInA) - , m_bJ(axisInB) + , m_bJ(-axisInB) { m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index b44808fa0..879ba72a3 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -1156,8 +1156,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; SimdVector3 axisInA(axisX,axisY,axisZ); SimdVector3 axisInB = rb1 ? - (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : - rb0->getCenterOfMassTransform().getBasis() * -axisInA; + (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : + rb0->getCenterOfMassTransform().getBasis() * axisInA; bool angularOnly = false; @@ -1195,9 +1195,19 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl { SimdTransform frameInA; SimdTransform frameInB; + + SimdVector3 axis1, axis2; + SimdPlaneSpace1( axisInA, axis1, axis2 ); - frameInA.setIdentity(); - frameInB.setIdentity(); + frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(), + axisInA.y(), axis1.y(), axis2.y(), + axisInA.z(), axis1.z(), axis2.z() ); + + + SimdPlaneSpace1( axisInB, axis1, axis2 ); + frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(), + axisInB.y(), axis1.y(), axis2.y(), + axisInB.z(), axis1.z(), axis2.z() ); frameInA.setOrigin( pivotInA ); frameInB.setOrigin( pivotInB );