Mirrors/overte-thingvellir
3
0
Fork 0
mirror of https://thingvellir.net/git/overte synced 2025-03-27 23:52:03 +01:00
Code Issues Projects Releases Packages Wiki Activity Actions

rewritten MassProperties with complete unit tests

Browse source
This commit is contained in:
Andrew Meadows 2015-05-26 22:48:45 -07:00
parent 857552ae0b
commit efbb9b0238
9 changed files with 857 additions and 443 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

150
libraries/physics/src/MeshInfo.cpp
View file

@ -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 <iostream>
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<Vertex> *vertices, vector<int> *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<float> MeshInfo::computeMassProperties(){
vector<float> 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;
}

33
libraries/physics/src/MeshInfo.h
View file

@ -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 <vector>
#include <glm/glm.hpp>
#include <glm/gtx/norm.hpp>
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<float> computeVolumeAndInertia(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const;
public:
vector<Vertex> *_vertices;
Vertex _centerOfMass;
vector<int> *_triangles;
MeshInfo(vector<Vertex> *vertices, vector<int> *triangles);
~MeshInfo();
Vertex getMeshCentroid() const;
vector<float> computeMassProperties();
};
}
#endif // hifi_MeshInfo_h

321
libraries/physics/src/MeshMassProperties.cpp Normal file
View file

@ -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 <assert.h>
#include <stdint.h>
#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);
}

60
libraries/physics/src/MeshMassProperties.h Normal file
View file

@ -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 <vector>
#include <btBulletDynamicsCommon.h>
typedef std::vector<btVector3> VectorOfPoints;
typedef std::vector<uint32_t> 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

237
tests/physics/src/MeshInfoTests.cpp
View file

@ -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 <iostream>
#include <iomanip>
#include <MeshInfo.h>
#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<glm::vec3> vertices = { p11, p22, p33 };
vector<int> 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<float> 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<glm::vec3> vertices = { p0, p1, p2, p3 };
vector<int> triangles = { 0, 2, 1, 0, 3, 2, 0, 1, 3, 1, 2, 3 };
meshinfo::MeshInfo massProp(&vertices, &triangles);
vector<float> 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<glm::vec3> 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<int> 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<float> 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<glm::vec3> 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<int> 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<float> 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();
}

21
tests/physics/src/MeshInfoTests.h
View file

@ -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

452
tests/physics/src/MeshMassPropertiesTests.cpp Normal file
View file

@ -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 <iostream>
#include <MeshMassProperties.h>
#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();
}

22
tests/physics/src/MeshMassPropertiesTests.h Normal file
View file

@ -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

4
tests/physics/src/main.cpp
View file

@ -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;
}