diff --git a/Demos/Benchmarks/BenchmarkDemo.cpp b/Demos/Benchmarks/BenchmarkDemo.cpp index 979dc5e0c..3a1955f81 100644 --- a/Demos/Benchmarks/BenchmarkDemo.cpp +++ b/Demos/Benchmarks/BenchmarkDemo.cpp @@ -713,7 +713,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); m_joints[JOINT_SPINE_HEAD] = coneC; @@ -722,7 +722,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,M_PI_4*5); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_LEFT_HIP] = coneC; @@ -739,7 +739,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(scale*btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_4); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_RIGHT_HIP] = coneC; @@ -756,7 +756,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(scale*btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_LEFT_SHOULDER] = coneC; @@ -774,7 +774,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(scale*btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_RIGHT_SHOULDER] = coneC; @@ -1181,3 +1181,4 @@ void BenchmarkDemo::exitPhysics() + diff --git a/Demos/RagdollDemo/RagdollDemo.cpp b/Demos/RagdollDemo/RagdollDemo.cpp index 7f095e29e..6f90a53fc 100644 --- a/Demos/RagdollDemo/RagdollDemo.cpp +++ b/Demos/RagdollDemo/RagdollDemo.cpp @@ -202,7 +202,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); m_joints[JOINT_SPINE_HEAD] = coneC; @@ -211,7 +211,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_LEFT_HIP] = coneC; @@ -228,7 +228,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_RIGHT_HIP] = coneC; @@ -245,7 +245,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_LEFT_SHOULDER] = coneC; @@ -263,7 +263,7 @@ public: localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_RIGHT_SHOULDER] = coneC; @@ -468,3 +468,4 @@ void RagdollDemo::exitPhysics() + diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index a8a2c4f67..e11b49d94 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -33,11 +33,6 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false) { - // flip axis for correct angles - m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_swingSpan1 = btScalar(1e30); m_swingSpan2 = btScalar(1e30); m_twistSpan = btScalar(1e30); @@ -55,15 +50,6 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& { m_rbBFrame = m_rbAFrame; - // flip axis for correct angles - m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - - m_rbBFrame.getBasis()[2][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_swingSpan1 = btScalar(1e30); m_swingSpan2 = btScalar(1e30); m_twistSpan = btScalar(1e30); @@ -143,7 +129,7 @@ void btConeTwistConstraint::buildJacobian() btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq; + btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 27ab1fcd2..2d1e79b9c 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -158,7 +158,7 @@ public: btQuaternion inverse() const { - return btQuaternion(m_x, m_y, m_z, -m_unusedW); + return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); } SIMD_FORCE_INLINE btQuaternion @@ -319,3 +319,4 @@ shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) +