diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp | 43 |
1 files changed, 26 insertions, 17 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 755544f0dee..15a4c92de20 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -53,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; + m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); init(); } @@ -136,6 +137,9 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); @@ -304,7 +308,7 @@ void btConeTwistConstraint::buildJacobian() -void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) @@ -506,7 +510,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBo m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accTwistLimitImpulse - temp; - btVector3 impulse = m_twistAxis * impulseMag; + // btVector3 impulse = m_twistAxis * impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); @@ -725,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr { if(m_swingSpan1 < m_fixThresh) { // hinge around Y axis - if(!(btFuzzyZero(y))) +// if(!(btFuzzyZero(y))) + if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; if(m_swingSpan2 >= m_fixThresh) @@ -747,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } else { // hinge around Z axis - if(!btFuzzyZero(z)) +// if(!btFuzzyZero(z)) + if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) { m_solveSwingLimit = true; if(m_swingSpan1 >= m_fixThresh) @@ -828,12 +834,11 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, { vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); vSwingAxis.normalize(); - if (fabs(vSwingAxis.x()) > SIMD_EPSILON) - { - // non-zero twist?! this should never happen. - int wtf = 0; wtf = wtf; - } - +#if 0 + // non-zero twist?! this should never happen. + btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON)); +#endif + // Compute limit for given swing. tricky: // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) @@ -877,8 +882,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, else if (swingAngle < 0) { // this should never happen! - int wtf = 0; wtf = wtf; - } +#if 0 + btAssert(0); +#endif + } } btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const @@ -929,7 +936,9 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, if (twistAngle < 0) { // this should never happen - int wtf = 0; wtf = wtf; +#if 0 + btAssert(0); +#endif } vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); @@ -976,10 +985,10 @@ void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) { btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btTransform trABCur = trBCur.inverse() * trACur; - btQuaternion qABCur = trABCur.getRotation(); - btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); - btQuaternion qConstraintCur = trConstraintCur.getRotation(); +// btTransform trABCur = trBCur.inverse() * trACur; +// btQuaternion qABCur = trABCur.getRotation(); +// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + //btQuaternion qConstraintCur = trConstraintCur.getRotation(); btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); setMotorTargetInConstraintSpace(qConstraint); |