diff --git a/libraries/physics/src/MeshInfo.cpp b/libraries/physics/src/MeshInfo.cpp deleted file mode 100644 index 29ddc74a98..0000000000 --- a/libraries/physics/src/MeshInfo.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// -// MeshInfo.cpp -// libraries/physics/src -// -// Created by Virendra Singh 2015.02.28 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#include "MeshInfo.h" -#include -using namespace meshinfo; - -//class to compute volume, mass, center of mass, and inertia tensor of a mesh. -//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. - -MeshInfo::MeshInfo(vector *vertices, vector *triangles) :\ - _vertices(vertices), - _centerOfMass(Vertex(0.0, 0.0, 0.0)), - _triangles(triangles) -{ - -} - -MeshInfo::~MeshInfo(){ - - _vertices = NULL; - _triangles = NULL; - -} - -inline float MeshInfo::getVolume(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ - glm::mat4 tet = { glm::vec4(p1.x, p2.x, p3.x, p4.x), glm::vec4(p1.y, p2.y, p3.y, p4.y), glm::vec4(p1.z, p2.z, p3.z, p4.z), - glm::vec4(1.0f, 1.0f, 1.0f, 1.0f) }; - return glm::determinant(tet) / 6.0f; -} - -Vertex MeshInfo::getMeshCentroid() const{ - return _centerOfMass; -} - -vector MeshInfo::computeMassProperties(){ - vector volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - Vertex origin(0.0, 0.0, 0.0); - glm::mat3 identity; - float meshVolume = 0.0f; - glm::mat3 globalInertiaTensors(0.0); - - for (unsigned int i = 0; i < _triangles->size(); i += 3){ - Vertex p1 = _vertices->at(_triangles->at(i)); - Vertex p2 = _vertices->at(_triangles->at(i + 1)); - Vertex p3 = _vertices->at(_triangles->at(i + 2)); - float volume = getVolume(p1, p2, p3, origin); - Vertex com = 0.25f * (p1 + p2 + p3); - meshVolume += volume; - _centerOfMass += com * volume; - - //centroid is used for calculating inertia tensor relative to center of mass. - // translate the tetrahedron to its center of mass using P = P - centroid - Vertex p0 = origin - com; - p1 = p1 - com; - p2 = p2 - com; - p3 = p3 - com; - - //Calculate inertia tensor based on Tonon's Formulae given in the paper mentioned below. - //http://docsdrive.com/pdfs/sciencepublications/jmssp/2005/8-11.pdf - //Explicit exact formulas for the 3-D tetrahedron inertia tensor in terms of its vertex coordinates - F.Tonon - - float i11 = (volume * 0.1f) * ( - p0.y*p0.y + p0.y*p1.y + p0.y*p2.y + p0.y*p3.y + - p1.y*p1.y + p1.y*p2.y + p1.y*p3.y + - p2.y*p2.y + p2.y*p3.y + - p3.y*p3.y + - p0.z*p0.z + p0.z*p1.z + p0.z*p2.z + p0.z*p3.z + - p1.z*p1.z + p1.z*p2.z + p1.z*p3.z + - p2.z*p2.z + p2.z*p3.z + - p3.z*p3.z); - - float i22 = (volume * 0.1f) * ( - p0.x*p0.x + p0.x*p1.x + p0.x*p2.x + p0.x*p3.x + - p1.x*p1.x + p1.x*p2.x + p1.x*p3.x + - p2.x*p2.x + p2.x*p3.x + - p3.x*p3.x + - p0.z*p0.z + p0.z*p1.z + p0.z*p2.z + p0.z*p3.z + - p1.z*p1.z + p1.z*p2.z + p1.z*p3.z + - p2.z*p2.z + p2.z*p3.z + - p3.z*p3.z); - - float i33 = (volume * 0.1f) * ( - p0.x*p0.x + p0.x*p1.x + p0.x*p2.x + p0.x*p3.x + - p1.x*p1.x + p1.x*p2.x + p1.x*p3.x + - p2.x*p2.x + p2.x*p3.x + - p3.x*p3.x + - p0.y*p0.y + p0.y*p1.y + p0.y*p2.y + p0.y*p3.y + - p1.y*p1.y + p1.y*p2.y + p1.y*p3.y + - p2.y*p2.y + p2.y*p3.y + - p3.y*p3.y); - - float i23 = -(volume * 0.05f) * (2.0f * (p0.y*p0.z + p1.y*p1.z + p2.y*p2.z + p3.y*p3.z) + - p0.y*p1.z + p0.y*p2.z + p0.y*p3.z + - p1.y*p0.z + p1.y*p2.z + p1.y*p3.z + - p2.y*p0.z + p2.y*p1.z + p2.y*p3.z + - p3.y*p0.z + p3.y*p1.z + p3.y*p2.z); - - float i21 = -(volume * 0.05f) * (2.0f * (p0.x*p0.z + p1.x*p1.z + p2.x*p2.z + p3.x*p3.z) + - p0.x*p1.z + p0.x*p2.z + p0.x*p3.z + - p1.x*p0.z + p1.x*p2.z + p1.x*p3.z + - p2.x*p0.z + p2.x*p1.z + p2.x*p3.z + - p3.x*p0.z + p3.x*p1.z + p3.x*p2.z); - - float i31 = -(volume * 0.05f) * (2.0f * (p0.x*p0.y + p1.x*p1.y + p2.x*p2.y + p3.x*p3.y) + - p0.x*p1.y + p0.x*p2.y + p0.x*p3.y + - p1.x*p0.y + p1.x*p2.y + p1.x*p3.y + - p2.x*p0.y + p2.x*p1.y + p2.x*p3.y + - p3.x*p0.y + p3.x*p1.y + p3.x*p2.y); - - //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements - // | I11 -I12 -I13 | - // I = | -I21 I22 -I23 | - // | -I31 -I32 I33 | - glm::mat3 localInertiaTensors = { Vertex(i11, i21, i31), Vertex(i21, i22, i23), - Vertex(i31, i23, i33) }; - - //Translate the inertia tensors from center of mass to origin - //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product - globalInertiaTensors += localInertiaTensors + volume * ((glm::dot(com, com) * identity) - - glm::outerProduct(com, com)); - } - - //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem - if (meshVolume == 0){ - return volumeAndInertia; - } - _centerOfMass = (_centerOfMass / meshVolume); - - //Translate the inertia tensors from origin to mesh's center of mass. - globalInertiaTensors = globalInertiaTensors - meshVolume * ((glm::dot(_centerOfMass, _centerOfMass) * - identity) - glm::outerProduct(_centerOfMass, _centerOfMass)); - - volumeAndInertia[0] = meshVolume; - volumeAndInertia[1] = globalInertiaTensors[0][0]; //i11 - volumeAndInertia[2] = globalInertiaTensors[1][1]; //i22 - volumeAndInertia[3] = globalInertiaTensors[2][2]; //i33 - volumeAndInertia[4] = -globalInertiaTensors[2][1]; //i23 or i32 - volumeAndInertia[5] = -globalInertiaTensors[1][0]; //i21 or i12 - volumeAndInertia[6] = -globalInertiaTensors[2][0]; //i13 or i31 - return volumeAndInertia; -} \ No newline at end of file diff --git a/libraries/physics/src/MeshInfo.h b/libraries/physics/src/MeshInfo.h deleted file mode 100644 index 67dbc8b0d6..0000000000 --- a/libraries/physics/src/MeshInfo.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// MeshInfo.h -// libraries/physics/src -// -// Created by Virendra Singh 2015.02.28 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// -#ifndef hifi_MeshInfo_h -#define hifi_MeshInfo_h -#include -#include -#include -using namespace std; -namespace meshinfo{ - typedef glm::vec3 Vertex; - class MeshInfo{ - private: - inline float getVolume(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; - vector computeVolumeAndInertia(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; - public: - vector *_vertices; - Vertex _centerOfMass; - vector *_triangles; - MeshInfo(vector *vertices, vector *triangles); - ~MeshInfo(); - Vertex getMeshCentroid() const; - vector computeMassProperties(); - }; -} -#endif // hifi_MeshInfo_h \ No newline at end of file diff --git a/libraries/physics/src/MeshMassProperties.cpp b/libraries/physics/src/MeshMassProperties.cpp new file mode 100644 index 0000000000..e5d4f4e5af --- /dev/null +++ b/libraries/physics/src/MeshMassProperties.cpp @@ -0,0 +1,321 @@ +// +// MeshMassProperties.cpp +// libraries/physics/src +// +// Created by Andrew Meadows 2015.05.25 +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include +#include + +#include "MeshMassProperties.h" + +// this method is included for unit test verification +void computeBoxInertia(btScalar mass, const btVector3& diagonal, btMatrix3x3& inertia) { + // formula for box inertia tensor: + // + // | y^2 + z^2 0 0 | + // | | + // inertia = M/12 * | 0 z^2 + x^2 0 | + // | | + // | 0 0 x^2 + y^2 | + // + + mass = mass / btScalar(12.0f); + btScalar x = diagonal[0]; + x = mass * x * x; + btScalar y = diagonal[1]; + y = mass * y * y; + btScalar z = diagonal[2]; + z = mass * z * z; + inertia.setIdentity(); + inertia[0][0] = y + z; + inertia[1][1] = z + x; + inertia[2][2] = x + y; +} + +void computeTetrahedronInertia(btScalar mass, btVector3* points, btMatrix3x3& inertia) { + // Computes the inertia tensor of a tetrahedron about its center of mass. + // The tetrahedron is defined by array of four points in its center of mass frame. + // + // The analytic formulas were obtained from Tonon's paper: + // http://docsdrive.com/pdfs/sciencepublications/jmssp/2005/8-11.pdf + // http://thescipub.com/PDF/jmssp.2005.8.11.pdf + // + // The inertia tensor has the following form: + // + // | a f e | + // | | + // inertia = | f b d | + // | | + // | e d c | + + const btVector3& p0 = points[0]; + const btVector3& p1 = points[1]; + const btVector3& p2 = points[2]; + const btVector3& p3 = points[3]; + + for (uint32_t i = 0; i < 3; ++i ) { + uint32_t j = (i + 1) % 3; + uint32_t k = (j + 1) % 3; + + // compute diagonal + inertia[i][i] = mass * btScalar(0.1f) * + ( p0[j] * (p0[j] + p1[j] + p2[j] + p3[j]) + + p1[j] * (p1[j] + p2[j] + p3[j]) + + p2[j] * (p2[j] + p3[j]) + + p3[j] * p3[j] + + p0[k] * (p0[k] + p1[k] + p2[k] + p3[k]) + + p1[k] * (p1[k] + p2[k] + p3[k]) + + p2[k] * (p2[k] + p3[k]) + + p3[k] * p3[k] ); + + // compute off-diagonals + inertia[j][k] = inertia[k][j] = - mass * btScalar(0.05f) * + ( btScalar(2.0f) * ( p0[j] * p0[k] + p1[j] * p1[k] + p2[j] * p2[k] + p3[j] * p3[k] ) + + p0[j] * (p1[k] + p2[k] + p3[k]) + + p1[j] * (p0[k] + p2[k] + p3[k]) + + p2[j] * (p0[k] + p1[k] + p3[k]) + + p3[j] * (p0[k] + p1[k] + p2[k]) ); + } +} + +// helper function +void computePointInertia(const btVector3& point, btScalar mass, btMatrix3x3& inertia) { + btScalar distanceSquared = point.length2(); + if (distanceSquared > 0.0f) { + for (uint32_t i = 0; i < 3; ++i) { + btScalar pointi = point[i]; + inertia[i][i] = mass * (distanceSquared - (pointi * pointi)); + for (uint32_t j = i + 1; j < 3; ++j) { + btScalar offDiagonal = - mass * pointi * point[j]; + inertia[i][j] = offDiagonal; + inertia[j][i] = offDiagonal; + } + } + } +} + +// this method is included for unit test verification +void computeTetrahedronInertiaByBruteForce(btVector3* points, btMatrix3x3& inertia) { + // Computes the approximate inertia tensor of a tetrahedron (about frame's origin) + // by integration over the "point" masses. This is numerically expensive so it may + // take a while to complete. + + VectorOfIndices triangles = { + 0, 2, 1, + 0, 3, 2, + 0, 1, 3, + 1, 2, 3 }; + + for (int i = 0; i < 3; ++i) { + inertia[i].setZero(); + } + + // compute normals + btVector3 center = btScalar(0.25f) * (points[0] + points[1] + points[2] + points[3]); + btVector3 normals[4]; + btVector3 pointsOnPlane[4]; + for (int i = 0; i < 4; ++i) { + int t = 3 * i; + btVector3& p0 = points[triangles[t]]; + btVector3& p1 = points[triangles[t + 1]]; + btVector3& p2 = points[triangles[t + 2]]; + normals[i] = ((p1 - p0).cross(p2 - p1)).normalized(); + // make sure normal points away from center + if (normals[i].dot(p0 - center) < btScalar(0.0f)) { + normals[i] *= btScalar(-1.0f); + } + pointsOnPlane[i] = p0; + } + + // compute bounds of integration + btVector3 boxMax = points[0]; + btVector3 boxMin = points[0]; + for (int i = 1; i < 4; ++i) { + for(int j = 0; j < 3; ++j) { + if (points[i][j] > boxMax[j]) { + boxMax[j] = points[i][j]; + } + if (points[i][j] < boxMin[j]) { + boxMin[j] = points[i][j]; + } + } + } + + // compute step size + btVector3 diagonal = boxMax - boxMin; + btScalar maxDimension = diagonal[0]; + if (diagonal[1] > maxDimension) { + maxDimension = diagonal[1]; + } + if (diagonal[2] > maxDimension) { + maxDimension = diagonal[2]; + } + btScalar resolutionOfIntegration = btScalar(400.0f); + btScalar delta = maxDimension / resolutionOfIntegration; + btScalar deltaVolume = delta * delta * delta; + + // integrate over three dimensions + btMatrix3x3 deltaInertia; + btScalar XX = boxMax[0]; + btScalar YY = boxMax[1]; + btScalar ZZ = boxMax[2]; + btScalar x = boxMin[0]; + while(x < XX) { + btScalar y = boxMin[1]; + while (y < YY) { + btScalar z = boxMin[2]; + while (z < ZZ) { + btVector3 p(x, y, z); + // the point is inside the shape if it is behind all face planes + bool pointInside = true; + for (int i = 0; i < 4; ++i) { + if ((p - pointsOnPlane[i]).dot(normals[i]) > btScalar(0.0f)) { + pointInside = false; + break; + } + } + if (pointInside) { + // this point contributes to the total + computePointInertia(p, deltaVolume, deltaInertia); + inertia += deltaInertia; + } + z += delta; + } + y += delta; + } + x += delta; + } +} + +btScalar computeTetrahedronVolume(btVector3* points) { + // Assumes triangle {1, 2, 3} is wound according to the right-hand-rule. + // NOTE: volume may be negative, in which case the tetrahedron contributes negatively to totals + // volume = (face_area * face_normal).dot(face_to_far_point) / 3.0 + // (face_area * face_normal) = side0.cross(side1) / 2.0 + return ((points[2] - points[1]).cross(points[3] - points[2])).dot(points[3] - points[0]) / btScalar(6.0f); +} + +void applyParallelAxisTheorem(btMatrix3x3& inertia, const btVector3& shift, btScalar mass) { + // Parallel Axis Theorem says: + // + // Ishifted = Icm + M * [ (R*R)E - R(x)R ] + // + // where R*R = inside product + // R(x)R = outside product + // E = identity matrix + + btScalar distanceSquared = shift.length2(); + if (distanceSquared > btScalar(0.0f)) { + for (uint32_t i = 0; i < 3; ++i) { + btScalar shifti = shift[i]; + inertia[i][i] += mass * (distanceSquared - (shifti * shifti)); + for (uint32_t j = i + 1; j < 3; ++j) { + btScalar offDiagonal = mass * shifti * shift[j]; + inertia[i][j] -= offDiagonal; + inertia[j][i] -= offDiagonal; + } + } + } +} + +// helper function +void applyInverseParallelAxisTheorem(btMatrix3x3& inertia, const btVector3& shift, btScalar mass) { + // Parallel Axis Theorem says: + // + // Ishifted = Icm + M * [ (R*R)E - R(x)R ] + // + // So the inverse would be: + // + // Icm = Ishifted - M * [ (R*R)E - R(x)R ] + + btScalar distanceSquared = shift.length2(); + if (distanceSquared > btScalar(0.0f)) { + for (uint32_t i = 0; i < 3; ++i) { + btScalar shifti = shift[i]; + inertia[i][i] -= mass * (distanceSquared - (shifti * shifti)); + for (uint32_t j = i + 1; j < 3; ++j) { + btScalar offDiagonal = mass * shifti * shift[j]; + inertia[i][j] += offDiagonal; + inertia[j][i] += offDiagonal; + } + } + } +} + +MeshMassProperties::MeshMassProperties(const VectorOfPoints& points, const VectorOfIndices& triangleIndices) { + computeMassProperties(points, triangleIndices); +} + +void MeshMassProperties::computeMassProperties(const VectorOfPoints& points, const VectorOfIndices& triangleIndices) { + // We process the mesh one triangle at a time. Each triangle defines a tetrahedron + // relative to some local point p0 (which we chose to be the local origin for convenience). + // Each tetrahedron contributes to the three totals: volume, centerOfMass, and inertiaTensor. + // + // We assume the mesh triangles are wound using the right-hand-rule, such that the + // triangle's points circle counter-clockwise about its face normal. + // + + // initialize the totals + m_volume = btScalar(0.0f); + btVector3 weightedCenter; + weightedCenter.setZero(); + for (uint32_t i = 0; i < 3; ++i) { + m_inertia[i].setZero(); + } + + // create some variables to hold temporary results + uint32_t numPoints = points.size(); + const btVector3 p0(0.0f, 0.0f, 0.0f); + btMatrix3x3 tetraInertia; + btMatrix3x3 doubleDebugInertia; + btVector3 tetraPoints[4]; + btVector3 center; + + // loop over triangles + uint32_t numTriangles = triangleIndices.size() / 3; + for (uint32_t i = 0; i < numTriangles; ++i) { + uint32_t t = 3 * i; + assert(triangleIndices[t] < numPoints); + assert(triangleIndices[t + 1] < numPoints); + assert(triangleIndices[t + 2] < numPoints); + + // extract raw vertices + tetraPoints[0] = p0; + tetraPoints[1] = points[triangleIndices[t]]; + tetraPoints[2] = points[triangleIndices[t + 1]]; + tetraPoints[3] = points[triangleIndices[t + 2]]; + + // compute volume + btScalar volume = computeTetrahedronVolume(tetraPoints); + + // compute center + // NOTE: since tetraPoints[0] is the origin, we don't include it in the sum + center = btScalar(0.25f) * (tetraPoints[1] + tetraPoints[2] + tetraPoints[3]); + + // shift vertices so that center of mass is at origin + tetraPoints[0] -= center; + tetraPoints[1] -= center; + tetraPoints[2] -= center; + tetraPoints[3] -= center; + + // compute inertia tensor then shift it to origin-frame + computeTetrahedronInertia(volume, tetraPoints, tetraInertia); + applyParallelAxisTheorem(tetraInertia, center, volume); + + // tally results + weightedCenter += volume * center; + m_volume += volume; + m_inertia += tetraInertia; + } + + m_centerOfMass = weightedCenter / m_volume; + + applyInverseParallelAxisTheorem(m_inertia, m_centerOfMass, m_volume); +} + diff --git a/libraries/physics/src/MeshMassProperties.h b/libraries/physics/src/MeshMassProperties.h new file mode 100644 index 0000000000..a500cb28e8 --- /dev/null +++ b/libraries/physics/src/MeshMassProperties.h @@ -0,0 +1,60 @@ +// +// MeshMassProperties.h +// libraries/physics/src +// +// Created by Andrew Meadows 2015.05.25 +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_MeshMassProperties_h +#define hifi_MeshMassProperties_h + +#include + +#include + +typedef std::vector VectorOfPoints; +typedef std::vector VectorOfIndices; + +#define EXPOSE_HELPER_FUNCTIONS_FOR_UNIT_TEST +#ifdef EXPOSE_HELPER_FUNCTIONS_FOR_UNIT_TEST +void computeBoxInertia(btScalar mass, const btVector3& diagonal, btMatrix3x3& I); + +// mass = input mass of tetrahedron +// points = input array of four points with center of mass at origin +// I = output inertia of tetrahedron about its center of mass +void computeTetrahedronInertia(btScalar mass, btVector3* points, btMatrix3x3& I); +void computeTetrahedronInertiaByBruteForce(btVector3* points, btMatrix3x3& I); + +btScalar computeTetrahedronVolume(btVector3* points); + +void applyParallelAxisTheorem(btMatrix3x3& inertia, const btVector3& shift, btScalar mass); +#endif // EXPOSE_HELPER_FUNCTIONS_FOR_UNIT_TEST + +// Given a closed mesh with right-hand triangles a MeshMassProperties instance will compute +// its mass properties: +// +// volume +// center-of-mass +// normalized interia tensor about center of mass +// +class MeshMassProperties { +public: + + // the mass properties calculation is done in the constructor, so if the mesh is complex + // then the construction could be computationally expensiuve. + MeshMassProperties(const VectorOfPoints& points, const VectorOfIndices& triangleIndices); + + // compute the mass properties of a new mesh + void computeMassProperties(const VectorOfPoints& points, const VectorOfIndices& triangleIndices); + + // harveste the mass properties from these public data members + btScalar m_volume = 1.0f; + btVector3 m_centerOfMass = btVector3(0.0f, 0.0f, 0.0f); + btMatrix3x3 m_inertia = btMatrix3x3(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f); +}; + +#endif // _hifi_MeshMassProperties_h diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp deleted file mode 100644 index 221ffa117c..0000000000 --- a/tests/physics/src/MeshInfoTests.cpp +++ /dev/null @@ -1,237 +0,0 @@ -// -// MeshInfoTests.cpp -// tests/physics/src -// -// Created by Virendra Singh on 2015.03.02 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#include -#include -#include - -#include "MeshInfoTests.h" -const float epsilon = 0.015f; -void MeshInfoTests::testWithTetrahedron(){ - glm::vec3 p0(8.33220, -11.86875, 0.93355); - glm::vec3 p1(0.75523, 5.00000, 16.37072); - glm::vec3 p2(52.61236, 5.00000, -5.38580); - glm::vec3 p3(2.00000, 5.00000, 3.00000); - glm::vec3 centroid(15.92492, 0.782813, 3.72962); - - //translate the tetrahedron so that its apex is on origin - glm::vec3 p11 = p1 - p0; - glm::vec3 p22 = p2 - p0; - glm::vec3 p33 = p3 - p0; - vector vertices = { p11, p22, p33 }; - vector triangles = { 0, 1, 2 }; - - float volume = 1873.233236f; - float inertia_a = 43520.33257f; - //actual should be 194711.28938f. But for some reason it becomes 194711.296875 during - //runtime due to how floating points are stored. - float inertia_b = 194711.289f; - float inertia_c = 191168.76173f; - float inertia_aa = 4417.66150f; - float inertia_bb = -46343.16662f; - float inertia_cc = 11996.20119f; - - meshinfo::MeshInfo meshinfo(&vertices,&triangles); - vector voumeAndInertia = meshinfo.computeMassProperties(); - glm::vec3 tetCenterOfMass = meshinfo.getMeshCentroid(); - - //get original center of mass - tetCenterOfMass = tetCenterOfMass + p0; - glm::vec3 diff = centroid - tetCenterOfMass; - std::cout << std::setprecision(12); - //test if centroid is correct - if (diff.x > epsilon || diff.y > epsilon || diff.z > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Centroid is incorrect : Expected = " << centroid.x << " " << - centroid.y << " " << centroid.z << ", actual = " << tetCenterOfMass.x << " " << tetCenterOfMass.y << - " " << tetCenterOfMass.z << std::endl; - } - - //test if volume is correct - if (abs(volume - voumeAndInertia.at(0)) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << " " << - ", actual = " << voumeAndInertia.at(0) << std::endl; - } - - //test if moment of inertia with respect to x axis is correct - if (abs(inertia_a - (voumeAndInertia.at(1))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to x axis is incorrect : Expected = " << - inertia_a << " " << ", actual = " << voumeAndInertia.at(1) << std::endl; - } - - //test if moment of inertia with respect to y axis is correct - if (abs(inertia_b - voumeAndInertia.at(2)) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to y axis is incorrect : Expected = " << - inertia_b << " " << ", actual = " << (voumeAndInertia.at(2)) << std::endl; - } - - //test if moment of inertia with respect to z axis is correct - if (abs(inertia_c - (voumeAndInertia.at(3))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to z axis is incorrect : Expected = " << - inertia_c << " " << ", actual = " << (voumeAndInertia.at(3)) << std::endl; - } - - //test if product of inertia with respect to x axis is correct - if (abs(inertia_aa - (voumeAndInertia.at(4))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to x axis is incorrect : Expected = " << - inertia_aa << " " << ", actual = " << (voumeAndInertia.at(4)) << std::endl; - } - - //test if product of inertia with respect to y axis is correct - if (abs(inertia_bb - (voumeAndInertia.at(5))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to y axis is incorrect : Expected = " << - inertia_bb << " " << ", actual = " << (voumeAndInertia.at(5)) << std::endl; - } - - //test if product of inertia with respect to z axis is correct - if (abs(inertia_cc - (voumeAndInertia.at(6))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to z axis is incorrect : Expected = " << - inertia_cc << " " << ", actual = " << (voumeAndInertia.at(6)) << std::endl; - } - -} - -void MeshInfoTests::testWithTetrahedronAsMesh(){ - glm::vec3 p0(8.33220, -11.86875, 0.93355); - glm::vec3 p1(0.75523, 5.00000, 16.37072); - glm::vec3 p2(52.61236, 5.00000, -5.38580); - glm::vec3 p3(2.00000, 5.00000, 3.00000); - glm::vec3 centroid(15.92492, 0.782813, 3.72962); - /* TODO: actually test inertia/volume calculations here - //float volume = 1873.233236f; - //runtime due to how floating points are stored. - float inertia_a = 43520.33257f; - float inertia_b = 194711.289f; - float inertia_c = 191168.76173f; - float inertia_aa = 4417.66150f; - float inertia_bb = -46343.16662f; - float inertia_cc = 11996.20119f; - */ - std::cout << std::setprecision(12); - vector vertices = { p0, p1, p2, p3 }; - vector triangles = { 0, 2, 1, 0, 3, 2, 0, 1, 3, 1, 2, 3 }; - meshinfo::MeshInfo massProp(&vertices, &triangles); - vector volumeAndInertia = massProp.computeMassProperties(); - std::cout << volumeAndInertia[0] << " " << volumeAndInertia[1] << " " << volumeAndInertia[2] - << " " << volumeAndInertia[3] - << " " << volumeAndInertia[4] - << " " << volumeAndInertia[5] << " " << volumeAndInertia[6] << std::endl; - - //translate the tetrahedron so that the model is placed at origin i.e. com is at origin - p0 -= centroid; - p1 -= centroid; - p2 -= centroid; - p3 -= centroid; -} - -void MeshInfoTests::testWithCube(){ - glm::vec3 p0(1.0, -1.0, -1.0); - glm::vec3 p1(1.0, -1.0, 1.0); - glm::vec3 p2(-1.0, -1.0, 1.0); - glm::vec3 p3(-1.0, -1.0, -1.0); - glm::vec3 p4(1.0, 1.0, -1.0); - glm::vec3 p5(1.0, 1.0, 1.0); - glm::vec3 p6(-1.0, 1.0, 1.0); - glm::vec3 p7(-1.0, 1.0, -1.0); - vector vertices; - vertices.push_back(p0); - vertices.push_back(p1); - vertices.push_back(p2); - vertices.push_back(p3); - vertices.push_back(p4); - vertices.push_back(p5); - vertices.push_back(p6); - vertices.push_back(p7); - std::cout << std::setprecision(10); - vector triangles = { 0, 1, 2, 0, 2, 3, 4, 7, 6, 4, 6, 5, 0, 4, 5, 0, 5, 1, 1, 5, 6, 1, 6, 2, 2, 6, - 7, 2, 7, 3, 4, 0, 3, 4, 3, 7 }; - glm::vec3 centerOfMass(0.0, 0.0, 0.0); - double volume = 8.0; - double side = 2.0; - double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6 - - //test with origin as reference point - meshinfo::MeshInfo massProp(&vertices, &triangles); - vector volumeAndInertia = massProp.computeMassProperties(); - if (abs(centerOfMass.x - massProp.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp.getMeshCentroid().y) > epsilon || - abs(centerOfMass.z - massProp.getMeshCentroid().z) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << - centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp.getMeshCentroid().x << " " << - massProp.getMeshCentroid().y << " " << massProp.getMeshCentroid().z << std::endl; - } - - if (abs(volume - volumeAndInertia.at(0)) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia.at(0) << std::endl; - } - - if (abs(inertia - (volumeAndInertia.at(1))) > epsilon || abs(inertia - (volumeAndInertia.at(2))) > epsilon || - abs(inertia - (volumeAndInertia.at(3))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << - inertia << " " << inertia << ", actual = " << (volumeAndInertia.at(1)) << " " << (volumeAndInertia.at(2)) << - " " << (volumeAndInertia.at(3)) << std::endl; - } -} - -void MeshInfoTests::testWithUnitCube() -{ - glm::vec3 p0(0, 0, 1); - glm::vec3 p1(1, 0, 1); - glm::vec3 p2(0, 1, 1); - glm::vec3 p3(1, 1, 1); - glm::vec3 p4(0, 0, 0); - glm::vec3 p5(1, 0, 0); - glm::vec3 p6(0, 1, 0); - glm::vec3 p7(1, 1, 0); - vector vertices; - vertices.push_back(p0); - vertices.push_back(p1); - vertices.push_back(p2); - vertices.push_back(p3); - vertices.push_back(p4); - vertices.push_back(p5); - vertices.push_back(p6); - vertices.push_back(p7); - vector triangles = { 0, 1, 2, 1, 3, 2, 2, 3, 7, 2, 7, 6, 1, 7, 3, 1, 5, 7, 6, 7, 4, 7, 5, 4, 0, 4, 1, - 1, 4, 5, 2, 6, 4, 0, 2, 4 }; - glm::vec3 centerOfMass(0.5, 0.5, 0.5); - double volume = 1.0; - double side = 1.0; - double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6 - std::cout << std::setprecision(10); - - //test with origin as reference point - meshinfo::MeshInfo massProp(&vertices, &triangles); - vector volumeAndInertia = massProp.computeMassProperties(); - if (abs(centerOfMass.x - massProp.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp.getMeshCentroid().y) > - epsilon || abs(centerOfMass.z - massProp.getMeshCentroid().z) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << - " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp.getMeshCentroid().x << " " << - massProp.getMeshCentroid().y << " " << massProp.getMeshCentroid().z << std::endl; - } - - if (abs(volume - volumeAndInertia.at(0)) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia.at(0) << std::endl; - } - - if (abs(inertia - (volumeAndInertia.at(1))) > epsilon || abs(inertia - (volumeAndInertia.at(2))) > epsilon || - abs(inertia - (volumeAndInertia.at(3))) > epsilon){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << - inertia << " " << inertia << ", actual = " << (volumeAndInertia.at(1)) << " " << (volumeAndInertia.at(2)) << - " " << (volumeAndInertia.at(3)) << std::endl; - } -} -void MeshInfoTests::runAllTests(){ - testWithTetrahedron(); - testWithTetrahedronAsMesh(); - testWithUnitCube(); - testWithCube(); -} diff --git a/tests/physics/src/MeshInfoTests.h b/tests/physics/src/MeshInfoTests.h deleted file mode 100644 index 0ddd8d0944..0000000000 --- a/tests/physics/src/MeshInfoTests.h +++ /dev/null @@ -1,21 +0,0 @@ -// -// MeshInfoTests.h -// tests/physics/src -// -// Created by Virendra Singh on 2015.03.02 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#ifndef hifi_MeshInfoTests_h -#define hifi_MeshInfoTests_h -namespace MeshInfoTests{ - void testWithTetrahedron(); - void testWithUnitCube(); - void testWithCube(); - void runAllTests(); - void testWithTetrahedronAsMesh(); -} -#endif // hifi_MeshInfoTests_h \ No newline at end of file diff --git a/tests/physics/src/MeshMassPropertiesTests.cpp b/tests/physics/src/MeshMassPropertiesTests.cpp new file mode 100644 index 0000000000..6434bd07ff --- /dev/null +++ b/tests/physics/src/MeshMassPropertiesTests.cpp @@ -0,0 +1,452 @@ +// +// MeshMassProperties.cpp +// tests/physics/src +// +// Created by Virendra Singh on 2015.03.02 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include +#include + +#include "MeshMassPropertiesTests.h" + +//#define VERBOSE_UNIT_TESTS + +const btScalar acceptableRelativeError(1.0e-5f); +const btScalar acceptableAbsoluteError(1.0e-4f); + +void printMatrix(const std::string& name, const btMatrix3x3& matrix) { + std::cout << name << " = [" << std::endl; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + std::cout << " " << matrix[i][j]; + } + std::cout << std::endl; + } + std::cout << "]" << std::endl; +} + +void MeshMassPropertiesTests::testParallelAxisTheorem() { +#ifdef EXPOSE_HELPER_FUNCTIONS_FOR_UNIT_TEST + // verity we can compute the inertia tensor of a box in two different ways: + // (a) as one box + // (b) as a combination of two partial boxes. +#ifdef VERBOSE_UNIT_TESTS + std::cout << "\n" << __FUNCTION__ << std::endl; +#endif // VERBOSE_UNIT_TESTS + + btScalar bigBoxX = 7.0f; + btScalar bigBoxY = 9.0f; + btScalar bigBoxZ = 11.0f; + btScalar bigBoxMass = bigBoxX * bigBoxY * bigBoxZ; + btMatrix3x3 bitBoxInertia; + computeBoxInertia(bigBoxMass, btVector3(bigBoxX, bigBoxY, bigBoxZ), bitBoxInertia); + + btScalar smallBoxX = bigBoxX / 2.0f; + btScalar smallBoxY = bigBoxY; + btScalar smallBoxZ = bigBoxZ; + btScalar smallBoxMass = smallBoxX * smallBoxY * smallBoxZ; + btMatrix3x3 smallBoxI; + computeBoxInertia(smallBoxMass, btVector3(smallBoxX, smallBoxY, smallBoxZ), smallBoxI); + + btVector3 smallBoxOffset(smallBoxX / 2.0f, 0.0f, 0.0f); + + btMatrix3x3 smallBoxShiftedRight = smallBoxI; + applyParallelAxisTheorem(smallBoxShiftedRight, smallBoxOffset, smallBoxMass); + + btMatrix3x3 smallBoxShiftedLeft = smallBoxI; + applyParallelAxisTheorem(smallBoxShiftedLeft, -smallBoxOffset, smallBoxMass); + + btMatrix3x3 twoSmallBoxesInertia = smallBoxShiftedRight + smallBoxShiftedLeft; + + // verify bigBox same as twoSmallBoxes + btScalar error; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + error = bitBoxInertia[i][j] - twoSmallBoxesInertia[i][j]; + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : box inertia[" << i << "][" << j << "] off by = " + << error << std::endl; + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + printMatrix("expected inertia", bitBoxInertia); + printMatrix("computed inertia", twoSmallBoxesInertia); +#endif // VERBOSE_UNIT_TESTS +#endif // EXPOSE_HELPER_FUNCTIONS_FOR_UNIT_TEST +} + +void MeshMassPropertiesTests::testTetrahedron(){ + // given the four vertices of a tetrahedron verify the analytic formula for inertia + // agrees with expected results +#ifdef VERBOSE_UNIT_TESTS + std::cout << "\n" << __FUNCTION__ << std::endl; +#endif // VERBOSE_UNIT_TESTS + + // these numbers from the Tonon paper: + btVector3 points[4]; + points[0] = btVector3(8.33220f, -11.86875f, 0.93355f); + points[1] = btVector3(0.75523f, 5.00000f, 16.37072f); + points[2] = btVector3(52.61236f, 5.00000f, -5.38580f); + points[3] = btVector3(2.00000f, 5.00000f, 3.00000f); + + btScalar expectedVolume = 1873.233236f; + + btMatrix3x3 expectedInertia; + expectedInertia[0][0] = 43520.33257f; + expectedInertia[1][1] = 194711.28938f; + expectedInertia[2][2] = 191168.76173f; + expectedInertia[1][2] = -4417.66150f; + expectedInertia[2][1] = -4417.66150f; + expectedInertia[0][2] = 46343.16662f; + expectedInertia[2][0] = 46343.16662f; + expectedInertia[0][1] = -11996.20119f; + expectedInertia[1][0] = -11996.20119f; + + // compute volume + btScalar volume = computeTetrahedronVolume(points); + btScalar error = (volume - expectedVolume) / expectedVolume; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = " + << error << std::endl; + } + + btVector3 centerOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]); + + // compute inertia tensor + // (shift the points so that tetrahedron's local centerOfMass is at origin) + for (int i = 0; i < 4; ++i) { + points[i] -= centerOfMass; + } + btMatrix3x3 inertia; + computeTetrahedronInertia(volume, points, inertia); + + // verify + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + error = (inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << std::endl; + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + std::cout << "expected volume = " << expectedVolume << std::endl; + std::cout << "measured volume = " << volume << std::endl; + printMatrix("expected inertia", expectedInertia); + printMatrix("computed inertia", inertia); + + // when building VERBOSE you might be instrested in the results from the brute force method: + btMatrix3x3 bruteInertia; + computeTetrahedronInertiaByBruteForce(points, bruteInertia); + printMatrix("brute inertia", bruteInertia); +#endif // VERBOSE_UNIT_TESTS +} + +void MeshMassPropertiesTests::testOpenTetrahedonMesh() { + // given the simplest possible mesh (open, with one triangle) + // verify MeshMassProperties computes the right nubers +#ifdef VERBOSE_UNIT_TESTS + std::cout << "\n" << __FUNCTION__ << std::endl; +#endif // VERBOSE_UNIT_TESTS + + // these numbers from the Tonon paper: + VectorOfPoints points; + points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f)); + points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f)); + points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f)); + points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f)); + + btScalar expectedVolume = 1873.233236f; + + btMatrix3x3 expectedInertia; + expectedInertia[0][0] = 43520.33257f; + expectedInertia[1][1] = 194711.28938f; + expectedInertia[2][2] = 191168.76173f; + expectedInertia[1][2] = -4417.66150f; + expectedInertia[2][1] = -4417.66150f; + expectedInertia[0][2] = 46343.16662f; + expectedInertia[2][0] = 46343.16662f; + expectedInertia[0][1] = -11996.20119f; + expectedInertia[1][0] = -11996.20119f; + + // test as an open mesh with one triangle + VectorOfPoints shiftedPoints; + shiftedPoints.push_back(points[0] - points[0]); + shiftedPoints.push_back(points[1] - points[0]); + shiftedPoints.push_back(points[2] - points[0]); + shiftedPoints.push_back(points[3] - points[0]); + VectorOfIndices triangles = { 1, 2, 3 }; + btVector3 expectedCenterOfMass = 0.25f * (shiftedPoints[0] + shiftedPoints[1] + shiftedPoints[2] + shiftedPoints[3]); + + // compute mass properties + MeshMassProperties mesh(shiftedPoints, triangles); + + // verify + btScalar error = (mesh.m_volume - expectedVolume) / expectedVolume; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = " + << error << std::endl; + } + + error = (mesh.m_centerOfMass - expectedCenterOfMass).length(); + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : centerOfMass of tetrahedron off by = " + << error << std::endl; + } + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + error = (mesh.m_inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << std::endl; + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + std::cout << "expected volume = " << expectedVolume << std::endl; + std::cout << "measured volume = " << mesh.m_volume << std::endl; + printMatrix("expected inertia", expectedInertia); + printMatrix("computed inertia", mesh.m_inertia); +#endif // VERBOSE_UNIT_TESTS +} + +void MeshMassPropertiesTests::testClosedTetrahedronMesh() { + // given a tetrahedron as a closed mesh of four tiangles + // verify MeshMassProperties computes the right nubers +#ifdef VERBOSE_UNIT_TESTS + std::cout << "\n" << __FUNCTION__ << std::endl; +#endif // VERBOSE_UNIT_TESTS + + // these numbers from the Tonon paper: + VectorOfPoints points; + points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f)); + points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f)); + points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f)); + points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f)); + + btScalar expectedVolume = 1873.233236f; + + btMatrix3x3 expectedInertia; + expectedInertia[0][0] = 43520.33257f; + expectedInertia[1][1] = 194711.28938f; + expectedInertia[2][2] = 191168.76173f; + expectedInertia[1][2] = -4417.66150f; + expectedInertia[2][1] = -4417.66150f; + expectedInertia[0][2] = 46343.16662f; + expectedInertia[2][0] = 46343.16662f; + expectedInertia[0][1] = -11996.20119f; + expectedInertia[1][0] = -11996.20119f; + + btVector3 expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]); + + VectorOfIndices triangles = { + 0, 2, 1, + 0, 3, 2, + 0, 1, 3, + 1, 2, 3 }; + + // compute mass properties + MeshMassProperties mesh(points, triangles); + + // verify + btScalar error; + error = (mesh.m_volume - expectedVolume) / expectedVolume; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = " + << error << std::endl; + } + + error = (mesh.m_centerOfMass - expectedCenterOfMass).length(); + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : centerOfMass of tetrahedron off by = " + << error << std::endl; + } + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + error = (mesh.m_inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << std::endl; + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + std::cout << "(a) tetrahedron as mesh" << std::endl; + std::cout << "expected volume = " << expectedVolume << std::endl; + std::cout << "measured volume = " << mesh.m_volume << std::endl; + printMatrix("expected inertia", expectedInertia); + printMatrix("computed inertia", mesh.m_inertia); +#endif // VERBOSE_UNIT_TESTS + + // test again, but this time shift the points so that the origin is definitely OUTSIDE the mesh + btVector3 shift = points[0] + expectedCenterOfMass; + for (int i = 0; i < (int)points.size(); ++i) { + points[i] += shift; + } + expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]); + + // compute mass properties + mesh.computeMassProperties(points, triangles); + + // verify + error = (mesh.m_volume - expectedVolume) / expectedVolume; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = " + << error << std::endl; + } + + error = (mesh.m_centerOfMass - expectedCenterOfMass).length(); + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : centerOfMass of tetrahedron off by = " + << error << std::endl; + } + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + error = (mesh.m_inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << std::endl; + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + std::cout << "(b) shifted tetrahedron as mesh" << std::endl; + std::cout << "expected volume = " << expectedVolume << std::endl; + std::cout << "measured volume = " << mesh.m_volume << std::endl; + printMatrix("expected inertia", expectedInertia); + printMatrix("computed inertia", mesh.m_inertia); +#endif // VERBOSE_UNIT_TESTS +} + +void MeshMassPropertiesTests::testBoxAsMesh() { + // verify that a mesh box produces the same mass properties as the analytic box. +#ifdef VERBOSE_UNIT_TESTS + std::cout << "\n" << __FUNCTION__ << std::endl; +#endif // VERBOSE_UNIT_TESTS + + + // build a box: + // / + // y + // / + // 6-------------------------7 + // /| /| + // / | / | + // / 2----------------------/--3 + // / / / / + // | 4-------------------------5 / --x-- + // z | / | / + // | |/ |/ + // 0 ------------------------1 + + btScalar x(5.0f); + btScalar y(3.0f); + btScalar z(2.0f); + + VectorOfPoints points; + points.reserve(8); + + points.push_back(btVector3(0.0f, 0.0f, 0.0f)); + points.push_back(btVector3(x, 0.0f, 0.0f)); + points.push_back(btVector3(0.0f, y, 0.0f)); + points.push_back(btVector3(x, y, 0.0f)); + points.push_back(btVector3(0.0f, 0.0f, z)); + points.push_back(btVector3(x, 0.0f, z)); + points.push_back(btVector3(0.0f, y, z)); + points.push_back(btVector3(x, y, z)); + + VectorOfIndices triangles = { + 0, 1, 4, + 1, 5, 4, + 1, 3, 5, + 3, 7, 5, + 2, 0, 6, + 0, 4, 6, + 3, 2, 7, + 2, 6, 7, + 4, 5, 6, + 5, 7, 6, + 0, 2, 1, + 2, 3, 1 + }; + + // compute expected mass properties analytically + btVector3 expectedCenterOfMass = 0.5f * btVector3(x, y, z); + btScalar expectedVolume = x * y * z; + btMatrix3x3 expectedInertia; + computeBoxInertia(expectedVolume, btVector3(x, y, z), expectedInertia); + + // compute the mass properties using the mesh + MeshMassProperties mesh(points, triangles); + + // verify + btScalar error; + error = (mesh.m_volume - expectedVolume) / expectedVolume; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = " + << error << std::endl; + } + + error = (mesh.m_centerOfMass - expectedCenterOfMass).length(); + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : centerOfMass of tetrahedron off by = " + << error << std::endl; + } + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + if (expectedInertia [i][j] == btScalar(0.0f)) { + error = mesh.m_inertia[i][j] - expectedInertia[i][j]; + if (fabsf(error) > acceptableAbsoluteError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << " absolute"<< std::endl; + } + } else { + error = (mesh.m_inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; + if (fabsf(error) > acceptableRelativeError) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by " + << error << std::endl; + } + } + } + } + +#ifdef VERBOSE_UNIT_TESTS + std::cout << "expected volume = " << expectedVolume << std::endl; + std::cout << "measured volume = " << mesh.m_volume << std::endl; + std::cout << "expected center of mass = < " + << expectedCenterOfMass[0] << ", " + << expectedCenterOfMass[1] << ", " + << expectedCenterOfMass[2] << "> " << std::endl; + std::cout << "computed center of mass = < " + << mesh.m_centerOfMass[0] << ", " + << mesh.m_centerOfMass[1] << ", " + << mesh.m_centerOfMass[2] << "> " << std::endl; + printMatrix("expected inertia", expectedInertia); + printMatrix("computed inertia", mesh.m_inertia); +#endif // VERBOSE_UNIT_TESTS +} + +void MeshMassPropertiesTests::runAllTests() { + testParallelAxisTheorem(); + testTetrahedron(); + testOpenTetrahedonMesh(); + testClosedTetrahedronMesh(); + testBoxAsMesh(); + //testWithCube(); +} diff --git a/tests/physics/src/MeshMassPropertiesTests.h b/tests/physics/src/MeshMassPropertiesTests.h new file mode 100644 index 0000000000..ab352bfce2 --- /dev/null +++ b/tests/physics/src/MeshMassPropertiesTests.h @@ -0,0 +1,22 @@ +// +// MeshMassPropertiesTests.h +// tests/physics/src +// +// Created by Virendra Singh on 2015.03.02 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_MeshMassPropertiesTests_h +#define hifi_MeshMassPropertiesTests_h +namespace MeshMassPropertiesTests{ + void testParallelAxisTheorem(); + void testTetrahedron(); + void testOpenTetrahedonMesh(); + void testClosedTetrahedronMesh(); + void testBoxAsMesh(); + void runAllTests(); +} +#endif // hifi_MeshMassPropertiesTests_h diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index 0f35ed5002..f63925bb34 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -12,13 +12,13 @@ #include "ShapeInfoTests.h" #include "ShapeManagerTests.h" #include "BulletUtilTests.h" -#include "MeshInfoTests.h" +#include "MeshMassPropertiesTests.h" int main(int argc, char** argv) { ShapeColliderTests::runAllTests(); ShapeInfoTests::runAllTests(); ShapeManagerTests::runAllTests(); BulletUtilTests::runAllTests(); - MeshInfoTests::runAllTests(); + MeshMassPropertiesTests::runAllTests(); return 0; }