From b690019e392866bac088b8e164c2f8a612473f1f Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 13 Sep 2011 23:37:05 +0000 Subject: [PATCH] Removed global operator- in btQuaternion, and rewrote the 'getAxis' method Add a unit test for testQuaternionGetAxisAngle Thanks to ejtttje, fixes Issue 538 --- Demos/ConstraintDemo/CMakeLists.txt | 4 +- UnitTests/BulletUnitTests/TestLinearMath.h | 100 ++++++++++++++++++ .../btConeTwistConstraint.cpp | 2 +- .../ConstraintSolver/btHingeConstraint.cpp | 2 +- src/LinearMath/btQuaternion.h | 13 +-- 5 files changed, 108 insertions(+), 13 deletions(-) diff --git a/Demos/ConstraintDemo/CMakeLists.txt b/Demos/ConstraintDemo/CMakeLists.txt index d11359c26..ad920d8c5 100644 --- a/Demos/ConstraintDemo/CMakeLists.txt +++ b/Demos/ConstraintDemo/CMakeLists.txt @@ -33,13 +33,13 @@ IF (USE_GLUT) IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) IF (CMAKE_CL_64) ADD_CUSTOM_COMMAND( - TARGET AppConcaveDemo + TARGET AppConstraintDemo POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR} ) ELSE(CMAKE_CL_64) ADD_CUSTOM_COMMAND( - TARGET AppConcaveDemo + TARGET AppConstraintDemo POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} ) diff --git a/UnitTests/BulletUnitTests/TestLinearMath.h b/UnitTests/BulletUnitTests/TestLinearMath.h index 13d01d5f4..43a6e6e4d 100644 --- a/UnitTests/BulletUnitTests/TestLinearMath.h +++ b/UnitTests/BulletUnitTests/TestLinearMath.h @@ -6,10 +6,60 @@ #include "LinearMath/btScalar.h" +#define TEST_NUM_UNITSPHERE_POINTS 42 + +static btVector3 sPenetrationDirections[TEST_NUM_UNITSPHERE_POINTS] = + { + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) + }; + + // --------------------------------------------------------------------------- class TestLinearMath : public CppUnit::TestFixture { + public: @@ -54,8 +104,58 @@ public: } + void testQuaternionGetAxisAngle() + { + int steps=100; + + for (int j=0;j=0) + CPPUNIT_ASSERT_DOUBLES_EQUAL( angle, compAngle, 1e-5 ); + else + { + CPPUNIT_ASSERT_DOUBLES_EQUAL( btFabs(angle), btFabs(compAngle), 1e-5 ); + } + btVector3 compAxis = quat.getAxis(); + + if (compAngle>SIMD_EPSILON) + { + if (i>=0) + { + CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getX(), compAxis.getX(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getY(), compAxis.getY(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getZ(), compAxis.getZ(), 1e-4 ); + } else + { + btScalar sign = compAngle*angle<0? -1 : 1; + CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getX(), compAxis.getX(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getY(), compAxis.getY(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getZ(), compAxis.getZ(), 1e-4 ); + } + } else + { + CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.f, compAxis.getX(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 0.f, compAxis.getY(), 1e-4 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 0.f, compAxis.getZ(), 1e-4 ); + + } + } + } + } + + CPPUNIT_TEST_SUITE(TestLinearMath); CPPUNIT_TEST(testNormalize); + CPPUNIT_TEST(testQuaternionGetAxisAngle); + CPPUNIT_TEST_SUITE_END(); private: diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index bc371e406..aa543aabc 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -918,7 +918,7 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qMinTwist = operator-(qTwist); + qMinTwist = -(qTwist); twistAngle = qMinTwist.getAngle(); } if (twistAngle < 0) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 144beef1c..5acd0daee 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -663,7 +663,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) btScalar targetAngle = qHinge.getAngle(); if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qHinge = operator-(qHinge); + qHinge = -(qHinge); targetAngle = qHinge.getAngle(); } if (qHinge.getZ() < 0) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 42e968d59..b3196a91c 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -212,11 +212,12 @@ public: /**@brief Return the axis of the rotation represented by this quaternion */ btVector3 getAxis() const { - btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.)); + btScalar s_squared = 1.f-m_floats[3]*m_floats[3]; + if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero return btVector3(1.0, 0.0, 0.0); // Arbitrary - btScalar s = btSqrt(s_squared); - return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); + btScalar s = 1.f/btSqrt(s_squared); + return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s); } /**@brief Return the inverse of this quaternion */ @@ -315,12 +316,6 @@ public: }; -/**@brief Return the negative of a quaternion */ -SIMD_FORCE_INLINE btQuaternion -operator-(const btQuaternion& q) -{ - return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w()); -}