diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 54914d27c..03a851219 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -163,7 +163,7 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt btScalar *J2 = info->m_J2angularAxis; if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { - btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btTransform trA = transA*m_rbAFrame; btVector3 p = trA.getBasis().getColumn(1); btVector3 q = trA.getBasis().getColumn(2); int srow1 = srow + info->rowskip; @@ -296,6 +296,7 @@ void btConeTwistConstraint::buildJacobian() void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { + #ifndef __SPU__ if (m_useSolveConstraintObsolete) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); @@ -502,11 +503,14 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver } } } - +#else +btAssert(0); +#endif //__SPU__ } + void btConeTwistConstraint::updateRHS(btScalar timeStep) { (void)timeStep; @@ -514,7 +518,7 @@ void btConeTwistConstraint::updateRHS(btScalar timeStep) } - +#ifndef __SPU__ void btConeTwistConstraint::calcAngleInfo() { m_swingCorrection = btScalar(0.); @@ -600,7 +604,7 @@ void btConeTwistConstraint::calcAngleInfo() } } } - +#endif //__SPU__ static btVector3 vTwist(1,0,0); // twist axis in constraint's space @@ -677,8 +681,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_twistAxisA.setValue(0,0,0); m_kSwing = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + + computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); } } else @@ -780,8 +784,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr m_twistAxis = quatRotate(qB, -twistAxis); m_kTwist = btScalar(1.) / - (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + + computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); } if (m_solveSwingLimit) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7e6323402..c637231c4 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -461,11 +461,22 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) void btHingeConstraint::getInfo2 (btConstraintInfo2* info) { - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); + getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); } -void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) + +void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) { + ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now + testLimit(transA,transB); + + getInfo2Internal(info,transA,transB,angVelA,angVelB); +} + + +void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) +{ + btAssert(!m_useSolveConstraintObsolete); int i, skip = info->rowskip; // transforms in world space @@ -474,7 +485,27 @@ void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTra // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); +#if 0 + if (0) + { + for (i=0;i<6;i++) + { + info->m_J1linearAxis[i*skip]=0; + info->m_J1linearAxis[i*skip+1]=0; + info->m_J1linearAxis[i*skip+2]=0; + info->m_J1angularAxis[i*skip]=0; + info->m_J1angularAxis[i*skip+1]=0; + info->m_J1angularAxis[i*skip+2]=0; + + info->m_J2angularAxis[i*skip]=0; + info->m_J2angularAxis[i*skip+1]=0; + info->m_J2angularAxis[i*skip+2]=0; + + info->m_constraintError[i*skip]=0.f; + } + } +#endif //#if 0 // linear (all fixed) info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[skip + 1] = 1; @@ -483,6 +514,7 @@ void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTra + btVector3 a1 = pivotAInW - transA.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index ce11d388a..270f3f85f 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -86,6 +86,8 @@ public: virtual void getInfo2 (btConstraintInfo2* info); void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); + + void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);