From ddccc2797abf398afd23cafe25f5ffe97dcfa149 Mon Sep 17 00:00:00 2001 From: rponom Date: Fri, 12 Sep 2008 02:55:03 +0000 Subject: [PATCH] More stable btConeTwistConstraint to fix the RagdollDemo --- .../btConeTwistConstraint.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index e11b49d94..61dad522a 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -114,17 +114,34 @@ void btConeTwistConstraint::buildJacobian() btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + btScalar swx=btScalar(0.),swy = btScalar(0.); + btScalar thresh = btScalar(10.); + btScalar fact; + // Get Frame into world space if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); - swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); +// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing1 *= fact; + } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); - swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); +// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing2 *= fact; } btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);