diff --git a/examples/collisionshapes/Scene.h b/examples/collisionshapes/Scene.h index 55e6c684..afc0537c 100644 --- a/examples/collisionshapes/Scene.h +++ b/examples/collisionshapes/Scene.h @@ -40,13 +40,13 @@ #include "../common/Viewer.h" // Constants -const int NB_BOXES = 4; -const int NB_SPHERES = 2; -const int NB_CONES = 3; -const int NB_CYLINDERS = 1; +const int NB_BOXES = 2; +const int NB_SPHERES = 3; +const int NB_CONES = 1; +const int NB_CYLINDERS = 2; const int NB_CAPSULES = 1; -const int NB_MESHES = 2; -const int NB_COMPOUND_SHAPES = 3; +const int NB_MESHES = 3; +const int NB_COMPOUND_SHAPES = 1; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; const float CONE_RADIUS = 2.0f; diff --git a/examples/common/Dumbbell.cpp b/examples/common/Dumbbell.cpp index 17a6b04d..d6810d06 100644 --- a/examples/common/Dumbbell.cpp +++ b/examples/common/Dumbbell.cpp @@ -61,7 +61,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Initial position and orientation of the rigid body rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); + rp3d::decimal angleAroundX = rp3d::PI / 4; + rp3d::Quaternion initOrientation(angleAroundX, 0, 0); rp3d::Transform transformBody(initPosition, initOrientation); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) @@ -77,8 +78,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, mRigidBody = dynamicsWorld->createRigidBody(transformBody); // Add the three collision shapes to the body and specify the mass and transform of the shapes - mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1); - mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2); + mRigidBody->addCollisionShape(sphereCollisionShape, 1, transformSphereShape1); + mRigidBody->addCollisionShape(sphereCollisionShape, 4, transformSphereShape2); mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape); } diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index a764b421..9c10370a 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -158,7 +158,7 @@ void CollisionBody::removeAllCollisionShapes() { } // Reset the contact manifold lists -void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) { +void CollisionBody::resetContactManifoldsList() { // Delete the linked list of contact manifolds of that body ContactManifoldListElement* currentElement = mContactManifoldsList; @@ -167,13 +167,11 @@ void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) // Delete the current element currentElement->ContactManifoldListElement::~ContactManifoldListElement(); - memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } mContactManifoldsList = NULL; - - assert(mContactManifoldsList == NULL); } // Update the broad-phase state for this body (because it has moved for instance) @@ -190,3 +188,14 @@ void CollisionBody::updateBroadPhaseState() const { mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb); } } + +// Ask the broad-phase to test again the collision shapes of the body for collision +// (as if the body has moved). +void CollisionBody::askForBroadPhaseCollisionCheck() const { + + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + + mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); + } +} diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index e78bf04b..08b764f5 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -102,7 +102,7 @@ class CollisionBody : public Body { CollisionBody& operator=(const CollisionBody& body); /// Reset the contact manifold lists - void resetContactManifoldsList(MemoryAllocator& memoryAllocator); + void resetContactManifoldsList(); /// Remove all the collision shapes void removeAllCollisionShapes(); @@ -113,6 +113,10 @@ class CollisionBody : public Body { /// Update the broad-phase state for this body (because it has moved for instance) void updateBroadPhaseState() const; + /// Ask the broad-phase to test again the collision shapes of the body for collision + /// (as if the body has moved). + void askForBroadPhaseCollisionCheck() const; + public : // -------------------- Methods -------------------- // diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 257b8d94..c12daa54 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -82,6 +82,17 @@ void RigidBody::setType(BodyType type) { // Awake the body setIsSleeping(false); + + // Remove all the contacts with this body + resetContactManifoldsList(); + + // Ask the broad-phase to test again the collision shapes of the body for collision + // detection (as if the body has moved) + askForBroadPhaseCollisionCheck(); + + // Reset the force and torque on the body + mExternalForce.setToZero(); + mExternalTorque.setToZero(); } // Set the local inertia tensor of the body (in body coordinates) diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 0b64e5e9..bb0e1d49 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -135,6 +135,9 @@ class CollisionDetection { /// Remove a pair of bodies that cannot collide with each other void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2); + /// Ask for a collision shape to be tested again during broad-phase. + void askForBroadPhaseCollisionCheck(ProxyShape* shape); + /// Compute the collision detection void computeCollisionDetection(); @@ -183,6 +186,13 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); } +// Ask for a collision shape to be tested again during broad-phase. +/// We simply put the shape in the list of collision shape that have moved in the +/// previous frame so that it is tested for collision again in the broad-phase. +inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { + mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); +} + // Update a proxy collision shape (that has moved for instance) inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb) { mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index d3554624..3f9888aa 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -36,12 +36,6 @@ using namespace std; // TODO : Check if we really need to store the contact manifolds also in mContactManifolds. -// TODO : Check how to compute the initial inertia tensor now (especially when a body has multiple -// collision shapes. - -// TODO : Check the Body::setType() function in Box2D to be sure we are not missing any update -// of the collision shapes and broad-phase modification. - // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true), @@ -194,7 +188,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { } // Get current position and orientation of the body - const Vector3& currentPosition = bodies[b]->getTransform().getPosition(); + const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld; const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); // Update the new constrained position and orientation of the body @@ -202,6 +196,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() { mConstrainedOrientations[indexArray] = currentOrientation + Quaternion(0, newAngVelocity) * currentOrientation * decimal(0.5) * dt; + + // TODO : DELETE THIS + Vector3 newPos = mConstrainedPositions[indexArray]; + Quaternion newOrientation = mConstrainedOrientations[indexArray]; } } } @@ -221,6 +219,10 @@ void DynamicsWorld::updateBodiesState() { uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + // TODO : Delete this + Vector3 linVel = mConstrainedLinearVelocities[index]; + Vector3 angVel = mConstrainedAngularVelocities[index]; + // Update the linear and angular velocity of the body bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index]; @@ -228,6 +230,9 @@ void DynamicsWorld::updateBodiesState() { // Update the position of the center of mass of the body bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; + // TODO : DELETE THIS + Quaternion newOrient = mConstrainedOrientations[index]; + // Update the orientation of the body bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit()); @@ -450,23 +455,9 @@ void DynamicsWorld::solvePositionCorrection() { // Do not continue if there is no constraints if (mJoints.empty()) return; - // ---------- Get the position/orientation of the rigid bodies ---------- // - // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { - // For each body of the island - RigidBody** bodies = mIslands[islandIndex]->getBodies(); - for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - - uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - - // Get the position/orientation of the rigid body - const Transform& transform = bodies[b]->getTransform(); - mConstrainedPositions[index] = bodies[b]->mCenterOfMassWorld; - mConstrainedOrientations[index]= transform.getOrientation(); - } - // ---------- Solve the position error correction for the constraints ---------- // // For each iteration of the position (error correction) solver @@ -516,7 +507,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { } // Reset the contact manifold list of the body - rigidBody->resetContactManifoldsList(mMemoryAllocator); + rigidBody->resetContactManifoldsList(); // Call the destructor of the rigid body rigidBody->RigidBody::~RigidBody(); @@ -655,7 +646,7 @@ void DynamicsWorld::resetContactManifoldListsOfBodies() { for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // Reset the contact manifold list of the body - (*it)->resetContactManifoldsList(mMemoryAllocator); + (*it)->resetContactManifoldsList(); } } diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index b8c4110c..20cc494e 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -47,6 +47,16 @@ Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), } +// Constructor which convert Euler angles (in radians) to a quaternion +Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) { + initWithEulerAngles(angleX, angleY, angleZ); +} + +// Constructor which convert Euler angles (in radians) to a quaternion +Quaternion::Quaternion(const Vector3& eulerAngles) { + initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); +} + // Copy-constructor Quaternion::Quaternion(const Quaternion& quaternion) :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) { @@ -219,3 +229,32 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, // Compute and return the interpolated quaternion return quaternion1 * coeff1 + quaternion2 * coeff2; } + +// Initialize the quaternion using Euler angles +void Quaternion::initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { + + decimal angle = angleX * decimal(0.5); + const decimal sinX = std::sin(angle); + const decimal cosX = std::cos(angle); + + angle = angleY * decimal(0.5); + const decimal sinY = std::sin(angle); + const decimal cosY = std::cos(angle); + + angle = angleZ * decimal(0.5); + const decimal sinZ = std::sin(angle); + const decimal cosZ = std::cos(angle); + + const decimal cosYcosZ = cosY * cosZ; + const decimal sinYcosZ = sinY * cosZ; + const decimal cosYsinZ = cosY * sinZ; + const decimal sinYsinZ = sinY * sinZ; + + x = sinX * cosYcosZ - cosX * sinYsinZ; + y = cosX * sinYcosZ + sinX * cosYsinZ; + z = cosX * cosYsinZ - sinX * sinYcosZ; + w = cosX * cosYcosZ + sinX * sinYsinZ; + + // Normalize the quaternion + normalize(); +} diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 5bc862da..8895ceff 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -69,6 +69,12 @@ struct Quaternion { /// Constructor with the component w and the vector v=(x y z) Quaternion(decimal newW, const Vector3& v); + /// Constructor which convert Euler angles (in radians) to a quaternion + Quaternion(decimal angleX, decimal angleY, decimal angleZ); + + /// Constructor which convert Euler angles (in radians) to a quaternion + Quaternion(const Vector3& eulerAngles); + /// Copy-constructor Quaternion(const Quaternion& quaternion); @@ -153,6 +159,11 @@ struct Quaternion { /// Overloaded operator for equality condition bool operator==(const Quaternion& quaternion) const; + + private: + + /// Initialize the quaternion using Euler angles + void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ); }; /// Set all the values diff --git a/test/Test.h b/test/Test.h index 19861876..0d6106ba 100644 --- a/test/Test.h +++ b/test/Test.h @@ -90,7 +90,7 @@ class Test { Test(std::ostream* stream = &std::cout); /// Destructor - ~Test(); + virtual ~Test(); /// Return the number of passed tests long getNbPassedTests() const; diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index a7f3d324..f343ce84 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -77,7 +77,7 @@ class TestQuaternion : public Test { void testConstructors() { Quaternion quaternion1(mQuaternion1); - test(mQuaternion1== quaternion1); + test(mQuaternion1 == quaternion1); Quaternion quaternion2(4, 5, 6, 7); test(quaternion2 == Quaternion(4, 5, 6, 7)); @@ -90,6 +90,34 @@ class TestQuaternion : public Test { test(approxEqual(quaternion4.y, mQuaternion1.y)); test(approxEqual(quaternion4.z, mQuaternion1.z)); test(approxEqual(quaternion4.w, mQuaternion1.w)); + + // Test conversion from Euler angles to quaternion + + const decimal PI_OVER_2 = PI * decimal(0.5); + const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); + Quaternion quaternion5(PI_OVER_2, 0, 0); + Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); + quaternionTest5.normalize(); + test(approxEqual(quaternion5.x, quaternionTest5.x)); + test(approxEqual(quaternion5.y, quaternionTest5.y)); + test(approxEqual(quaternion5.z, quaternionTest5.z)); + test(approxEqual(quaternion5.w, quaternionTest5.w)); + + Quaternion quaternion6(0, PI_OVER_2, 0); + Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); + quaternionTest6.normalize(); + test(approxEqual(quaternion6.x, quaternionTest6.x)); + test(approxEqual(quaternion6.y, quaternionTest6.y)); + test(approxEqual(quaternion6.z, quaternionTest6.z)); + test(approxEqual(quaternion6.w, quaternionTest6.w)); + + Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); + Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); + quaternionTest7.normalize(); + test(approxEqual(quaternion7.x, quaternionTest7.x)); + test(approxEqual(quaternion7.y, quaternionTest7.y)); + test(approxEqual(quaternion7.z, quaternionTest7.z)); + test(approxEqual(quaternion7.w, quaternionTest7.w)); } /// Test unit, length, normalize methods