/********************************************************************************
* 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/reactphysics3d.h>
/// 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