diff --git a/CHANGELOG.md b/CHANGELOG.md index caa0352f..4f8d3198 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,9 +15,12 @@ do not hesitate to take a look at the user manual. - Method Joint::getReactionTorque() has been added to retrieve the current reaction torque of a joint - Method RigidBody::setLinearLockAxisFactor() to lock the translational movement of a body along the world-space x, y and z axes - Method RigidBody::setAngularLockAxisFactor() to lock the rotational movement of a body around the world-space x, y and z axes - - Method RigidBody::applyLocalForceAtWorldPosition() - - Method RigidBody::applyLocalForceAtLocalPosition() - - Method RigidBody::applyLocalForceToCenterOfMass() + - Method RigidBody::applyLocalForceAtWorldPosition() to manually apply a force to a rigid body + - Method RigidBody::applyLocalForceAtLocalPosition() to manually apply a force to a rigid body + - Method RigidBody::applyLocalForceToCenterOfMass() to manually apply a force to a rigid body + - Method RigidBody::applyLocalTorque() to apply a local-space torque to a rigid body + - Method RigidBody::getForce() to get the total manually applied force on a rigid body + - Method RigidBody::getTorque() to get the total manually applied torque on a rigid body - A cone limit can now be set to the ball-and-socket joint (this is useful for ragdolls) - Bridge scene has been added to the testbed application - Ragdoll scene has been added to the testbed application @@ -32,6 +35,7 @@ do not hesitate to take a look at the user manual. - Rename method RigidBody::applyForceAtWorldPosition() into RigidBody::applyWorldForceAtWorldPosition() - Rename method RigidBody::applyForceAtLocalPosition() into RigidBody::applyWorldForceAtLocalPosition() - Rename method RigidBody::applyForceToCenterOfMass() into RigidBody::applyWorldForceAtCenterOfMass() + - Rename method RigidBody::applyTorque() into RigidBody::applyWorldTorque() - The raycasting broad-phase performance has been improved - The raycasting performance against HeighFieldShape has been improved (better middle-phase algorithm) - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability) diff --git a/include/reactphysics3d/body/RigidBody.h b/include/reactphysics3d/body/RigidBody.h index 2172bacf..742cc2af 100644 --- a/include/reactphysics3d/body/RigidBody.h +++ b/include/reactphysics3d/body/RigidBody.h @@ -165,33 +165,42 @@ class RigidBody : public CollisionBody { /// Set the lock rotation factor void setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) const; - /// Apply an external force (in local-space) to the body at its center of mass. + /// Manually apply an external force (in local-space) to the body at its center of mass. void applyLocalForceAtCenterOfMass(const Vector3& force); - /// Apply an external force (in world-space) to the body at its center of mass. + /// Manually apply an external force (in world-space) to the body at its center of mass. void applyWorldForceAtCenterOfMass(const Vector3& force); - /// Apply an external force (in local-space) to the body at a given point (in local-space). + /// Manually apply an external force (in local-space) to the body at a given point (in local-space). void applyLocalForceAtLocalPosition(const Vector3& force, const Vector3& point); - /// Apply an external force (in world-space) to the body at a given point (in local-space). + /// Manually apply an external force (in world-space) to the body at a given point (in local-space). void applyWorldForceAtLocalPosition(const Vector3& force, const Vector3& point); - /// Apply an external force (in local-space) to the body at a given point (in world-space). + /// Manually apply an external force (in local-space) to the body at a given point (in world-space). void applyLocalForceAtWorldPosition(const Vector3& force, const Vector3& point); - /// Apply an external force (in world-space) to the body at a given point (in world-space). + /// Manually apply an external force (in world-space) to the body at a given point (in world-space). void applyWorldForceAtWorldPosition(const Vector3& force, const Vector3& point); - /// Apply an external torque to the body. - void applyTorque(const Vector3& torque); + /// Manually apply an external torque to the body (in world-space). + void applyWorldTorque(const Vector3& torque); - /// Reset the accumulated force to zero + /// Manually apply an external torque to the body (in local-space). + void applyLocalTorque(const Vector3& torque); + + /// Reset the manually applied force to zero void resetForce(); - /// Reset the accumulated torque to zero + /// Reset the manually applied torque to zero void resetTorque(); + /// Return the total manually applied force on the body (in world-space) + const Vector3& getForce() const; + + /// Return the total manually applied torque on the body (in world-space) + const Vector3& getTorque() const; + /// Return whether or not the body is allowed to sleep bool isAllowedToSleep() const; diff --git a/include/reactphysics3d/mathematics/Vector2.h b/include/reactphysics3d/mathematics/Vector2.h index a19598a6..255daee7 100644 --- a/include/reactphysics3d/mathematics/Vector2.h +++ b/include/reactphysics3d/mathematics/Vector2.h @@ -148,9 +148,6 @@ struct Vector2 { /// Return the zero vector static Vector2 zero(); - /// Function to test if two vectors are (almost) equal - static bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); - // -------------------- Friends -------------------- // friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2); @@ -375,7 +372,7 @@ RP3D_FORCE_INLINE Vector2 Vector2::zero() { } // Function to test if two vectors are (almost) equal -RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { +RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON) { return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); } diff --git a/include/reactphysics3d/mathematics/Vector3.h b/include/reactphysics3d/mathematics/Vector3.h index 79b47df9..eb2f7b0a 100644 --- a/include/reactphysics3d/mathematics/Vector3.h +++ b/include/reactphysics3d/mathematics/Vector3.h @@ -163,9 +163,6 @@ struct Vector3 { /// Return the zero vector static Vector3 zero(); - /// Function to test if two vectors are (almost) equal - static bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); - // -------------------- Friends -------------------- // friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2); @@ -417,7 +414,7 @@ RP3D_FORCE_INLINE Vector3 Vector3::zero() { } // Function to test if two vectors are (almost) equal -RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { +RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON) { return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && approxEqual(vec1.z, vec2.z, epsilon); } diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 6a66b463..290788c9 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -127,7 +127,7 @@ decimal RigidBody::getMass() const { return mWorld.mRigidBodyComponents.getMass(mEntity); } -// Apply an external force (in local-space) to the body at a given point (in local-space). +// Manually apply an external force (in local-space) to the body at a given point (in local-space). /// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. /// If the body is sleeping, calling this method will wake it up. Note that the @@ -146,7 +146,7 @@ void RigidBody::applyLocalForceAtLocalPosition(const Vector3& force, const Vecto applyWorldForceAtLocalPosition(worldForce, point); } -// Apply an external force (in world-space) to the body at a given point (in local-space). +// Manually apply an external force (in world-space) to the body at a given point (in local-space). /// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. /// If the body is sleeping, calling this method will wake it up. Note that the @@ -178,7 +178,7 @@ void RigidBody::applyWorldForceAtLocalPosition(const Vector3& force, const Vecto mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + (worldPoint - centerOfMassWorld).cross(force)); } -// Apply an external force (in local-space) to the body at a given point (in world-space). +// Manually apply an external force (in local-space) to the body at a given point (in world-space). /// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. /// If the body is sleeping, calling this method will wake it up. Note that the @@ -197,7 +197,7 @@ void RigidBody::applyLocalForceAtWorldPosition(const Vector3& force, const Vecto applyWorldForceAtWorldPosition(worldForce, point); } -// Apply an external force (in world-space) to the body at a given point (in world-space). +// Manually apply an external force (in world-space) to the body at a given point (in world-space). /// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. /// If the body is sleeping, calling this method will wake it up. Note that the @@ -261,7 +261,7 @@ void RigidBody::setLocalInertiaTensor(const Vector3& inertiaTensorLocal) { "Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string(), __FILE__, __LINE__); } -// Apply an external force (in local-space) to the body at its center of mass. +// Manually apply an external force (in local-space) to the body at its center of mass. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied forces and that this sum will be /// reset to zero at the end of each call of the PhyscisWorld::update() method. @@ -277,7 +277,7 @@ void RigidBody::applyLocalForceAtCenterOfMass(const Vector3& force) { applyWorldForceAtCenterOfMass(worldForce); } -// Apply an external force (in world-space) to the body at its center of mass. +// Manually apply an external force (in world-space) to the body at its center of mass. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied forces and that this sum will be /// reset to zero at the end of each call of the PhyscisWorld::update() method. @@ -885,15 +885,15 @@ void RigidBody::setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) c mWorld.mRigidBodyComponents.setAngularLockAxisFactor(mEntity, angularLockAxisFactor); } -// Apply an external torque to the body. +// Manually apply an external torque to the body (in world-space). /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied torques and that this sum will be /// reset to zero at the end of each call of the PhyscisWorld::update() method. /// You can only apply a force to a dynamic body otherwise, this method will do nothing. /** - * @param torque The external torque to apply on the body + * @param torque The external torque to apply on the body (in world-space) */ -void RigidBody::applyTorque(const Vector3& torque) { +void RigidBody::applyWorldTorque(const Vector3& torque) { // If it is not a dynamic body, we do nothing if (mWorld.mRigidBodyComponents.getBodyType(mEntity) != BodyType::DYNAMIC) return; @@ -908,6 +908,22 @@ void RigidBody::applyTorque(const Vector3& torque) { mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + torque); } +// Manually apply an external torque to the body (in local-space). +/// If the body is sleeping, calling this method will wake it up. Note that the +/// force will we added to the sum of the applied torques and that this sum will be +/// reset to zero at the end of each call of the PhyscisWorld::update() method. +/// You can only apply a force to a dynamic body otherwise, this method will do nothing. +/** + * @param torque The external torque to apply on the body (in local-space) + */ +void RigidBody::applyLocalTorque(const Vector3& torque) { + + // Convert the local-space torque to world-space + const Vector3 worldTorque = mWorld.mTransformComponents.getTransform(mEntity).getOrientation() * torque; + + applyWorldTorque(worldTorque); +} + // Reset the accumulated force to zero void RigidBody::resetForce() { @@ -928,6 +944,16 @@ void RigidBody::resetTorque() { mWorld.mRigidBodyComponents.setExternalTorque(mEntity, Vector3(0, 0, 0)); } +// Return the total manually applied force on the body (in world-space) +const Vector3& RigidBody::getForce() const { + return mWorld.mRigidBodyComponents.getExternalForce(mEntity); +} + +// Return the total manually applied torque on the body (in world-space) +const Vector3& RigidBody::getTorque() const { + return mWorld.mRigidBodyComponents.getExternalTorque(mEntity); +} + // Set the variable to know whether or not the body is sleeping void RigidBody::setIsSleeping(bool isSleeping) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c125f2ac..3bfb5f8e 100755 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -27,6 +27,7 @@ set (RP3D_TESTS_HEADERS "tests/mathematics/TestTransform.h" "tests/mathematics/TestVector2.h" "tests/mathematics/TestVector3.h" + "tests/engine/TestRigidBody.h" ) # Source files diff --git a/test/main.cpp b/test/main.cpp index 0346ee57..311547ba 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -44,6 +44,7 @@ #include "tests/containers/TestSet.h" #include "tests/containers/TestDeque.h" #include "tests/containers/TestStack.h" +#include "tests/engine/TestRigidBody.h" using namespace reactphysics3d; @@ -79,6 +80,11 @@ int main() { testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); testSuite.addTest(new TestHalfEdgeStructure("HalfEdgeStructure")); + + // ---------- Engine tests ---------- // + + testSuite.addTest(new TestRigidBody("RigidBody")); + // Run the tests testSuite.run(); diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h index 3e846dee..ef37ce66 100644 --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -2432,7 +2432,7 @@ class TestCollisionWorld : public Test { Transform transform1(Vector3(10, 22, 50), Quaternion::identity()); Transform transform2(Vector3(10, 20, 50), Quaternion::identity()); - // Move spheres to collide with each other + // Move bodies to collide with each other mBoxBody1->setTransform(transform1); mConcaveMeshBody->setTransform(transform2); @@ -2454,7 +2454,10 @@ class TestCollisionWorld : public Test { // Get collision data const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxCollider1, mConcaveMeshCollider); rp3d_test(collisionData != nullptr); + auto test = collisionData->getNbContactPairs(); rp3d_test(collisionData->getNbContactPairs() == 1); + auto test1 = collisionData->getTotalNbContactPoints(); + rp3d_test(collisionData->getTotalNbContactPoints() == 4); for (size_t i=0; icontactPairs[0].contactPoints.size(); i++) { diff --git a/test/tests/engine/TestRigidBody.h b/test/tests/engine/TestRigidBody.h new file mode 100644 index 00000000..4d6b24c2 --- /dev/null +++ b/test/tests/engine/TestRigidBody.h @@ -0,0 +1,316 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef TEST_RIGIDBODY_H +#define TEST_RIGIDBODY_H + +// Libraries +#include + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestRigidBody +/** + * Unit test for the RigidBody class. + */ +class TestRigidBody : public Test { + + private : + + // ---------- Atributes ---------- // + + PhysicsCommon mPhysicsCommon; + PhysicsWorld* mWorld; + + RigidBody* mRigidBody1; + RigidBody* mRigidBody2Box; + RigidBody* mRigidBody2Sphere; + RigidBody* mRigidBody2Convex; + RigidBody* mRigidBody3; + + Collider* mBoxCollider; + Collider* mSphereCollider; + Collider* mConvexMeshCollider; + + PolygonVertexArray* mConvexMeshPolygonVertexArray; + PolyhedronMesh* mConvexMeshPolyhedronMesh; + PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces; + TriangleVertexArray* mConcaveMeshTriangleVertexArray; + Vector3 mConvexMeshCubeVertices[8]; + int mConvexMeshCubeIndices[24]; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestRigidBody(const std::string& name) : Test(name) { + + mWorld = mPhysicsCommon.createPhysicsWorld(); + + const Transform transform1(Vector3(1, 2, 3), Quaternion::identity()); + mRigidBody1 = mWorld->createRigidBody(transform1); + + // Box + + const Transform transform2(Vector3(0, 0, 0), Quaternion::identity()); + mRigidBody2Box = mWorld->createRigidBody(transform2); + BoxShape* boxShape = mPhysicsCommon.createBoxShape(Vector3(2, 2, 2)); + mBoxCollider = mRigidBody2Box->addCollider(boxShape, Transform::identity()); + + // Sphere + + mRigidBody2Sphere = mWorld->createRigidBody(transform2); + SphereShape* sphereShape = mPhysicsCommon.createSphereShape(4); + mSphereCollider = mRigidBody2Sphere->addCollider(sphereShape, Transform::identity()); + + // Convex Meshes (in the shape of a box) + + mConvexMeshCubeVertices[0] = Vector3(-3, -3, 3); + mConvexMeshCubeVertices[1] = Vector3(3, -3, 3); + mConvexMeshCubeVertices[2] = Vector3(3, -3, -3); + mConvexMeshCubeVertices[3] = Vector3(-3, -3, -3); + mConvexMeshCubeVertices[4] = Vector3(-3, 3, 3); + mConvexMeshCubeVertices[5] = Vector3(3, 3, 3); + mConvexMeshCubeVertices[6] = Vector3(3, 3, -3); + mConvexMeshCubeVertices[7] = Vector3(-3, 3, -3); + + mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1; + mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7; + mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4; + mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5; + mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6; + mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3; + + mConvexMeshPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[6]; + rp3d::PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces; + for (int f = 0; f < 6; f++) { + face->indexBase = f * 4; + face->nbVertices = 4; + face++; + } + mConvexMeshPolygonVertexArray = new rp3d::PolygonVertexArray(8, &(mConvexMeshCubeVertices[0]), sizeof(Vector3), + &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + mConvexMeshPolyhedronMesh = mPhysicsCommon.createPolyhedronMesh(mConvexMeshPolygonVertexArray); + ConvexMeshShape* convexMeshShape = mPhysicsCommon.createConvexMeshShape(mConvexMeshPolyhedronMesh); + Transform transform3(Vector3(10, 0, 0), Quaternion::identity()); + mRigidBody2Convex = mWorld->createRigidBody(transform3); + mConvexMeshCollider = mRigidBody2Convex->addCollider(convexMeshShape, Transform::identity()); + + // Rigidbody 3 + + const decimal angleRad = 20 * PI_RP3D / 180.0; + const Transform transform4(Vector3(1, 2, 3), Quaternion::fromEulerAngles(angleRad, angleRad, angleRad)); + mRigidBody3 = mWorld->createRigidBody(transform4); + BoxShape* boxShape3 = mPhysicsCommon.createBoxShape(Vector3(2, 2, 2)); + mRigidBody3->addCollider(boxShape3, Transform::identity()); + } + + /// Destructor + virtual ~TestRigidBody() { + + mWorld->destroyRigidBody(mRigidBody1); + mWorld->destroyRigidBody(mRigidBody2Box); + mWorld->destroyRigidBody(mRigidBody2Sphere); + } + + /// Run the tests + void run() { + testGettersSetters(); + testMassPropertiesMethods(); + testApplyForcesAndTorques(); + } + + void testGettersSetters() { + + mRigidBody1->setMass(34); + rp3d_test(mRigidBody1->getMass() == 34); + + mRigidBody1->setLinearDamping(0.6); + rp3d_test(approxEqual(mRigidBody1->getLinearDamping(), 0.6)); + + mRigidBody1->setAngularDamping(0.6); + rp3d_test(approxEqual(mRigidBody1->getAngularDamping(), 0.6)); + + mRigidBody1->setLinearLockAxisFactor(Vector3(0.2, 0.3, 0.4)); + rp3d_test(approxEqual(mRigidBody1->getLinearLockAxisFactor(), Vector3(0.2, 0.3, 0.4))); + + mRigidBody1->setAngularLockAxisFactor(Vector3(0.2, 0.3, 0.4)); + rp3d_test(approxEqual(mRigidBody1->getAngularLockAxisFactor(), Vector3(0.2, 0.3, 0.4))); + + mRigidBody1->setLinearVelocity(Vector3(2, 3, 4)); + rp3d_test(approxEqual(mRigidBody1->getLinearVelocity(), Vector3(2, 3, 4))); + + mRigidBody1->setAngularVelocity(Vector3(2, 3, 4)); + rp3d_test(approxEqual(mRigidBody1->getAngularVelocity(), Vector3(2, 3, 4))); + + mRigidBody1->setTransform(Transform(Vector3(5, 4, 3), Quaternion::fromEulerAngles(1.7, 1.8, 1.9))); + rp3d_test(approxEqual(mRigidBody1->getTransform().getPosition(), Vector3(5, 4, 3))); + rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().x, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).x)); + rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().y, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).y)); + rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().z, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).z)); + rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().w, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).w)); + + mRigidBody1->setLocalCenterOfMass(Vector3(10, 20, 30)); + rp3d_test(approxEqual(mRigidBody1->getLocalCenterOfMass(), Vector3(10, 20, 30))); + + mRigidBody1->setType(BodyType::KINEMATIC); + rp3d_test(mRigidBody1->getType() == BodyType::KINEMATIC); + + mRigidBody1->setLocalInertiaTensor(Vector3(2, 4, 6)); + rp3d_test(approxEqual(mRigidBody1->getLocalInertiaTensor(), Vector3(2, 4, 6))); + } + + void testMassPropertiesMethods() { + + // Box collider + mBoxCollider->getMaterial().setMassDensity(3); + mRigidBody2Box->updateMassFromColliders(); + rp3d_test(approxEqual(mRigidBody2Box->getMass(), 64 * 3)); + + mRigidBody2Box->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Box->setMass(1); + mRigidBody2Box->updateMassPropertiesFromColliders(); + rp3d_test(approxEqual(mRigidBody2Box->getMass(), 64 * 3)); + rp3d_test(approxEqual(mRigidBody2Box->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Box->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Box->updateLocalCenterOfMassFromColliders(); + rp3d_test(approxEqual(mRigidBody2Box->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Box->setLocalInertiaTensor(Vector3(1, 2, 3)); + mRigidBody2Box->updateLocalInertiaTensorFromColliders(); + decimal tensorBox = 1.0 / 6.0 * 64 * 3 * 4 * 4; + rp3d_test(approxEqual(mRigidBody2Box->getLocalInertiaTensor(), Vector3(tensorBox, tensorBox, tensorBox))); + + // Sphere collider + mSphereCollider->getMaterial().setMassDensity(3); + mRigidBody2Sphere->updateMassFromColliders(); + const decimal sphereMass = 4.0 / 3.0 * PI_RP3D * 64 * 3; + rp3d_test(approxEqual(mRigidBody2Sphere->getMass(), sphereMass)); + + mRigidBody2Sphere->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Sphere->setMass(1); + mRigidBody2Sphere->updateMassPropertiesFromColliders(); + rp3d_test(approxEqual(mRigidBody2Sphere->getMass(), sphereMass)); + rp3d_test(approxEqual(mRigidBody2Sphere->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Sphere->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Sphere->updateLocalCenterOfMassFromColliders(); + rp3d_test(approxEqual(mRigidBody2Sphere->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Sphere->setLocalInertiaTensor(Vector3(1, 2, 3)); + mRigidBody2Sphere->updateLocalInertiaTensorFromColliders(); + const decimal tensorSphere = 2.0 / 5.0 * sphereMass * 4 * 4; + rp3d_test(approxEqual(mRigidBody2Sphere->getLocalInertiaTensor(), Vector3(tensorSphere, tensorSphere, tensorSphere))); + + // Convex mesh collider + mConvexMeshCollider->getMaterial().setMassDensity(3); + mRigidBody2Convex->updateMassFromColliders(); + rp3d_test(approxEqual(mRigidBody2Convex->getMass(), 648)); + + mRigidBody2Convex->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Convex->setMass(1); + mConvexMeshCollider->getMaterial().setMassDensity(2); + mRigidBody2Convex->updateMassPropertiesFromColliders(); + rp3d_test(approxEqual(mRigidBody2Convex->getMass(), 432)); + rp3d_test(approxEqual(mRigidBody2Convex->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Convex->setLocalCenterOfMass(Vector3(1, 2, 3)); + mRigidBody2Convex->updateLocalCenterOfMassFromColliders(); + rp3d_test(approxEqual(mRigidBody2Convex->getLocalCenterOfMass(), Vector3::zero())); + + mRigidBody2Convex->setLocalInertiaTensor(Vector3(1, 2, 3)); + mRigidBody2Convex->updateLocalInertiaTensorFromColliders(); + tensorBox = 1.0 / 6.0 * 432 * 6 * 6; + rp3d_test(approxEqual(mRigidBody2Convex->getLocalInertiaTensor(), Vector3(tensorBox, tensorBox, tensorBox))); + } + + void testApplyForcesAndTorques() { + + const Transform& worldTransform = mRigidBody3->getTransform(); + const Quaternion orientation = worldTransform.getOrientation(); + + mRigidBody3->applyLocalForceAtCenterOfMass(Vector3(4, 5, 6)); + rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(4, 5, 6))); + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3::zero())); + + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyWorldForceAtCenterOfMass(Vector3(4, 5, 6)); + rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3(4, 5, 6))); + rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3::zero())); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyLocalForceAtLocalPosition(Vector3(0, 0, 3), Vector3(2, 0, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3))); + + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero())); + rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3::zero())); + + mRigidBody3->applyLocalForceAtWorldPosition(Vector3(0, 0, 3), worldTransform * Vector3(2, 0, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3))); + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyWorldForceAtLocalPosition(orientation * Vector3(0, 0, 3), Vector3(2, 0, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3))); + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyWorldForceAtWorldPosition(orientation * Vector3(0, 0, 3), worldTransform * Vector3(2, 0, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3))); + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyWorldTorque(Vector3(0, 4, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero())); + rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3(0, 4, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + + mRigidBody3->applyLocalTorque(Vector3(0, 4, 0)); + rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero())); + rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, 4, 0), decimal(0.0001))); + mRigidBody3->resetForce(); + mRigidBody3->resetTorque(); + } + }; + +} + +#endif