From 9bee6f6afdd3dc680cb5ee09b1c2a51f308a49a3 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Sat, 24 Jul 2010 01:16:42 +0000 Subject: [PATCH] prepared some unit test for LinearMath --- UnitTests/BulletUnitTests/CMakeLists.txt | 1 + UnitTests/BulletUnitTests/Main.cpp | 3 + UnitTests/BulletUnitTests/TestBulletOnly.h | 171 ++++++++++----------- UnitTests/BulletUnitTests/TestLinearMath.h | 65 ++++++++ src/LinearMath/btVector3.h | 13 ++ 5 files changed, 166 insertions(+), 87 deletions(-) create mode 100644 UnitTests/BulletUnitTests/TestLinearMath.h diff --git a/UnitTests/BulletUnitTests/CMakeLists.txt b/UnitTests/BulletUnitTests/CMakeLists.txt index 6b3dc519b..560a6420e 100644 --- a/UnitTests/BulletUnitTests/CMakeLists.txt +++ b/UnitTests/BulletUnitTests/CMakeLists.txt @@ -14,4 +14,5 @@ LINK_LIBRARIES( ADD_EXECUTABLE(AppBulletUnitTests Main.cpp TestBulletOnly.h + TestLinearMath.h ) \ No newline at end of file diff --git a/UnitTests/BulletUnitTests/Main.cpp b/UnitTests/BulletUnitTests/Main.cpp index 8d835a3aa..4a3fad335 100644 --- a/UnitTests/BulletUnitTests/Main.cpp +++ b/UnitTests/BulletUnitTests/Main.cpp @@ -6,9 +6,12 @@ #include #include "TestBulletOnly.h" +#include "TestLinearMath.h" +CPPUNIT_TEST_SUITE_REGISTRATION( TestLinearMath ); CPPUNIT_TEST_SUITE_REGISTRATION( TestBulletOnly ); + int main(int argc, char* argv[]) { // Create the event manager and test controller diff --git a/UnitTests/BulletUnitTests/TestBulletOnly.h b/UnitTests/BulletUnitTests/TestBulletOnly.h index ae723a0c2..b337f6b5b 100644 --- a/UnitTests/BulletUnitTests/TestBulletOnly.h +++ b/UnitTests/BulletUnitTests/TestBulletOnly.h @@ -11,112 +11,109 @@ class TestBulletOnly : public CppUnit::TestFixture { + btCollisionConfiguration * mCollisionConfig; + btCollisionDispatcher * mCollisionDispatch; + btBroadphaseInterface * mBroadphase; + btConstraintSolver * mConstraintSolver; + btDiscreteDynamicsWorld * mWorld; + btCollisionShape* mBodyShape; + btRigidBody* mRigidBody; + btDefaultMotionState* mMotionState; + public: - void setUp() - { - // empty - } + void setUp() + { + // Setup the world -- + mCollisionConfig = new btDefaultCollisionConfiguration; + mCollisionDispatch = new btCollisionDispatcher( mCollisionConfig ); + mBroadphase = new btDbvtBroadphase(); + mConstraintSolver = new btSequentialImpulseConstraintSolver(); + mWorld = new btDiscreteDynamicsWorld( mCollisionDispatch, mBroadphase, mConstraintSolver, mCollisionConfig ); + mWorld->setGravity( btVector3( 0, -9.81, 0 )); - void tearDown() - { - // empty - } + // Set up the rigid body -- + mBodyShape = new btBoxShape( btVector3( 1, 1, 1 ) ); + btScalar mass = 1; + btVector3 localInertia(0,0,0); + mBodyShape->calculateLocalInertia(mass,localInertia); + btTransform bodyInitial; - btRigidBody::btRigidBodyConstructionInfo buildConstructionInfo( btScalar mass, btScalar friction, btScalar restitution, btScalar linearDamping, - btScalar angularDamping, btDefaultMotionState & state, btCollisionShape * shape ) - { - btVector3 inertia; - shape->calculateLocalInertia( mass, inertia ); - btRigidBody::btRigidBodyConstructionInfo info( mass, &state, shape, inertia ); + mMotionState = new btDefaultMotionState(); - info.m_friction = friction; - info.m_restitution = restitution; - info.m_linearDamping = linearDamping; - info.m_angularDamping = angularDamping; - - return info; - } - - void testKinematicVelocity0() - { - // Setup the world -- - - btCollisionConfiguration * mCollisionConfig; - btCollisionDispatcher * mCollisionDispatch; - btBroadphaseInterface * mBroadphase; - btConstraintSolver * mConstraintSolver; - btDiscreteDynamicsWorld * mWorld; + bodyInitial.setIdentity(); + bodyInitial.setOrigin( btVector3( 0, 0, 0 ) ); + mRigidBody = new btRigidBody( buildConstructionInfo(mass, 0.5, 0.5, 0, 0, *mMotionState, mBodyShape ) ); - mCollisionConfig = new btDefaultCollisionConfiguration; - mCollisionDispatch = new btCollisionDispatcher( mCollisionConfig ); - mBroadphase = new btDbvtBroadphase(); - mConstraintSolver = new btSequentialImpulseConstraintSolver(); - mWorld = new btDiscreteDynamicsWorld( mCollisionDispatch, mBroadphase, mConstraintSolver, mCollisionConfig ); - - mWorld->setGravity( btVector3( 0, -9.81, 0 )); - - // -- . + mWorld->addRigidBody( mRigidBody ); + } - // Set up the rigid body -- - - btCollisionShape * bodyShape = new btBoxShape( btVector3( 1, 1, 1 ) ); + void tearDown() + { + mWorld->removeRigidBody(mRigidBody); + delete mRigidBody; + delete mMotionState; + delete mBodyShape; + delete mWorld; + delete mConstraintSolver; + delete mBroadphase; + delete mCollisionDispatch; + delete mCollisionConfig; + + } - btTransform bodyInitial; - btDefaultMotionState mMotionState; - - bodyInitial.setIdentity(); - bodyInitial.setOrigin( btVector3( 0, 0, 0 ) ); - - btRigidBody mRigidBody( buildConstructionInfo( 1, 0.5, 0.5, 0, 0, mMotionState, bodyShape ) ); - - mWorld->addRigidBody( &mRigidBody ); - - mRigidBody.setMassProps( 1, btVector3( 1, 1, 1 ) ); - mRigidBody.updateInertiaTensor(); - mRigidBody.setCollisionFlags( btCollisionObject::CF_KINEMATIC_OBJECT ); - - // -- . + btRigidBody::btRigidBodyConstructionInfo buildConstructionInfo( btScalar mass, btScalar friction, btScalar restitution, btScalar linearDamping, + btScalar angularDamping, btDefaultMotionState & state, btCollisionShape * shape ) + { + btVector3 inertia(0,0,0); + if (mass>0) + shape->calculateLocalInertia( mass, inertia ); - // Interpolate the velocity -- - - //btVector3 velocity( 1., 2., 3. ), spin( 0.1, 0.2, 0.3 ); + btRigidBody::btRigidBodyConstructionInfo info( mass, &state, shape, inertia ); + + info.m_friction = friction; + info.m_restitution = restitution; + info.m_linearDamping = linearDamping; + info.m_angularDamping = angularDamping; + + return info; + } + + void testKinematicVelocity0() + { + mRigidBody->setMassProps( 1, btVector3( 1, 1, 1 ) ); + mRigidBody->updateInertiaTensor(); + mRigidBody->setCollisionFlags( btCollisionObject::CF_KINEMATIC_OBJECT ); + // -- . + + // Interpolate the velocity -- + + //btVector3 velocity( 1., 2., 3. ), spin( 0.1, 0.2, 0.3 ); btVector3 velocity( 1., 2., 3. ), spin( 0.1, 0.2, .3 ); - btTransform interpolated; + btTransform interpolated; - // TODO: This is inaccurate for small spins. - btTransformUtil::integrateTransform( mRigidBody.getCenterOfMassTransform(), velocity, spin, 1.0f/60.f, interpolated ); + // TODO: This is inaccurate for small spins. + btTransformUtil::integrateTransform( mRigidBody->getCenterOfMassTransform(), velocity, spin, 1.0f/60.f, interpolated ); - mRigidBody.setInterpolationWorldTransform( interpolated ); - - // -- . - - mWorld->stepSimulation( 1.f/60.f, 60, 1.0f/60.f ); - - btScalar a = 0.f; + mRigidBody->setInterpolationWorldTransform( interpolated ); - btScalar f = 1.f/a; - CPPUNIT_ASSERT_DOUBLES_EQUAL( mRigidBody.getLinearVelocity().length2(), velocity.length2(), 1e-8 ); + mWorld->stepSimulation( 1.f/60.f, 60, 1.0f/60.f ); + + CPPUNIT_ASSERT_DOUBLES_EQUAL( mRigidBody->getLinearVelocity().length2(), velocity.length2(), 1e-8 ); #ifdef BT_USE_DOUBLE_PRECISION CPPUNIT_ASSERT_DOUBLES_EQUAL( mRigidBody.getAngularVelocity().length2(), spin.length2(), 1e-4 ); #else - CPPUNIT_ASSERT_DOUBLES_EQUAL( mRigidBody.getAngularVelocity().length2(), spin.length2(), 5e-3 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL( mRigidBody->getAngularVelocity().length2(), spin.length2(), 5e-3 ); #endif //CPPUNIT_ASSERT_DOUBLES_EQUAL - - delete mWorld; - delete mConstraintSolver; - delete mBroadphase; - delete mCollisionDispatch; - delete mCollisionConfig; - delete bodyShape; - } - - CPPUNIT_TEST_SUITE(TestBulletOnly); - CPPUNIT_TEST(testKinematicVelocity0); - CPPUNIT_TEST_SUITE_END(); + + } + + CPPUNIT_TEST_SUITE(TestBulletOnly); + CPPUNIT_TEST(testKinematicVelocity0); + CPPUNIT_TEST_SUITE_END(); private: - + }; #endif diff --git a/UnitTests/BulletUnitTests/TestLinearMath.h b/UnitTests/BulletUnitTests/TestLinearMath.h new file mode 100644 index 000000000..13d01d5f4 --- /dev/null +++ b/UnitTests/BulletUnitTests/TestLinearMath.h @@ -0,0 +1,65 @@ +#ifndef TEST_LINEAR_MATH_HAS_BEEN_INCLUDED +#define TEST_LINEAR_MATH_HAS_BEEN_INCLUDED + +#include "cppunit/TestFixture.h" +#include "cppunit/extensions/HelperMacros.h" + +#include "LinearMath/btScalar.h" + +// --------------------------------------------------------------------------- + +class TestLinearMath : public CppUnit::TestFixture +{ + + +public: + + void setUp() + { + } + + void tearDown() + { + } + + + void testNormalize() + { + + const btVector3 xaxis(1,0,0); + const btVector3 yaxis(0,1,0); + const btVector3 zaxis(0,0,1); + + const btVector3 negxaxis(-1,0,0); + const btVector3 negyaxis(0,-1,0); + const btVector3 negzaxis(0,0,-1); + + btVector3 vec; + + vec.setValue(1e-20,0,0); + vec.safeNormalize(); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-6 ); + + vec.setValue(1e20,0,0); + vec.safeNormalize(); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-6 ); + + vec.setValue(1e-20,0,0); + vec.normalize(); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 ); + + vec.setValue(1e19,0,0); + vec.normalize(); + CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 ); + + } + + CPPUNIT_TEST_SUITE(TestLinearMath); + CPPUNIT_TEST(testNormalize); + CPPUNIT_TEST_SUITE_END(); + +private: + +}; + +#endif //TEST_LINEAR_MATH_HAS_BEEN_INCLUDED diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index f1f0be45c..6d42c3527 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -148,6 +148,19 @@ public: * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + SIMD_FORCE_INLINE btVector3& safeNormalize() + { + btVector3 absVec = this->absolute(); + int maxIndex = absVec.maxAxis(); + if (absVec[maxIndex]>0) + { + *this /= absVec[maxIndex]; + return *this /= length(); + } + setValue(1,0,0); + return *this; + } + /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize()