From 7a129235c26af553f0fe5991e493b32e5516f6e4 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 4 Mar 2015 01:12:20 +0530 Subject: [PATCH] Mass properties unit tests --- libraries/physics/src/MassProperties.cpp | 279 ++++++++++++---------- libraries/physics/src/MassProperties.h | 89 +++---- tests/physics/src/MassPropertiesTests.cpp | 133 +++++++++++ tests/physics/src/MassPropertiesTests.h | 20 ++ tests/physics/src/main.cpp | 14 +- 5 files changed, 369 insertions(+), 166 deletions(-) create mode 100644 tests/physics/src/MassPropertiesTests.cpp create mode 100644 tests/physics/src/MassPropertiesTests.h diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp index ec4066ce9f..4d2e668f95 100644 --- a/libraries/physics/src/MassProperties.cpp +++ b/libraries/physics/src/MassProperties.cpp @@ -12,123 +12,119 @@ #include "MassProperties.h" using namespace massproperties; -Tetrahedron::Tetrahedron(Vertex p1, Vertex p2, Vertex p3, Vertex p4) :\ +Tetrahedron::Tetrahedron(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) :\ _w(p1), _x(p2), _y(p3), _z(p4){ - - computeVolumeAndInertia(); + computeVolume(); + computeInertia(); } Tetrahedron::~Tetrahedron(){ } -Vertex Tetrahedron::getX(){ - return _x; +Vertex Tetrahedron::getX() const{ + return _x; } -Vertex Tetrahedron::getY(){ - return _y; +Vertex Tetrahedron::getY() const{ + return _y; } -Vertex Tetrahedron::getZ(){ - return _z; +Vertex Tetrahedron::getZ() const{ + return _z; } -Vertex Tetrahedron::getw(){ - return _w; +Vertex Tetrahedron::getw() const{ + return _w; } -Vertex Tetrahedron::getCentroid(){ - Vertex com; - com.x = (_x.x + _y.x + _z.x + _w.x) / 4.0f; - com.y = (_x.y + _y.y + _z.y + _w.y) / 4.0f; - com.z = (_x.z + _y.z + _z.z + _w.z) / 4.0f; - return com; +Vertex Tetrahedron::getCentroid() const{ + Vertex com; + com.x = (_x.x + _y.x + _z.x + _w.x) / 4.0f; + com.y = (_x.y + _y.y + _z.y + _w.y) / 4.0f; + com.z = (_x.z + _y.z + _z.z + _w.z) / 4.0f; + return com; } -vector Tetrahedron::getVolumeAndInertia(){ - return _volumeAndInertia; +vector Tetrahedron::getVolumeAndInertia() const{ + return _volumeAndInertia; } -void Tetrahedron::computeVolumeAndInertia(){ - double A = glm::distance2(_w, _x); - double B = glm::distance2(_w, _y); - double C = glm::distance2(_x, _y); - double a = glm::distance2(_y, _z); - double b = glm::distance2(_x, _z); - double c = glm::distance2(_w, _z); - double squaredVol = (4 * a * b * c) - (a*glm::pow((b + c - A), 2.0)) - (b*glm::pow((c + a - B), 2.0)) - - (c*glm::pow((a + b - C), 2.0)) + ((a + b - C)*(a + c - B)*(b + c - A)); +void Tetrahedron::computeVolume(){ + glm::mat4 tet = { glm::vec4(_x.x, _y.x, _z.x, _w.x), glm::vec4(_x.y, _y.y, _z.y, _w.y), glm::vec4(_x.z, _y.z, _z.z, _w.z), glm::vec4(1.0f, 1.0f, 1.0f, 1.0f) }; + _volume = glm::determinant(tet) / 6.0f; + _volumeAndInertia.push_back(_volume); + std::cout << "volume : " << _volume << std::endl; +} - double volume = glm::sqrt(squaredVol);// volume of tetrahedron - _volumeAndInertia.push_back(volume); +void Tetrahedron::computeInertia(){ + + //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 com = getCentroid(); + Vertex p0 = _w - com; + Vertex p1 = _x - com; + Vertex p2 = _y - com; + Vertex p3 = _z - com; - //centroid is used for calculating inertia tensor relative to center of mass. - // translatw the tetrahedron to its center of mass using parallel axis theorem - Vertex com = getCentroid(); - Vertex p0 = _w - com; - Vertex p1 = _x - com; - Vertex p2 = _y - com; - Vertex p3 = _z - 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 - //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 + double inertia_a = (_volume * 6.0 / 60.0) * ( + 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); + _volumeAndInertia.push_back(inertia_a); - double inertia_a = (volume * 6.0 / 60.0) * ( - 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); - _volumeAndInertia.push_back(inertia_a); + double inertia_b = (_volume * 6.0 / 60.0) * ( + 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); + _volumeAndInertia.push_back(inertia_b); - double inertia_b = (volume * 6.0 / 60.0) * ( - 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); - _volumeAndInertia.push_back(inertia_b); + double inertia_c = (_volume * 6.0 / 60.0) * ( + 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); + _volumeAndInertia.push_back(inertia_c); - double inertia_c = (volume * 6.0 / 60.0) * ( - 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); - _volumeAndInertia.push_back(inertia_c); + double inertia_aa = (_volume * 6.0 / 120.0) * (2.0 * (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); + _volumeAndInertia.push_back(inertia_aa); - double inertia_aa = (volume * 6.0 / 60.0) * (2.0 * (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); - _volumeAndInertia.push_back(inertia_aa); + double inertia_bb = (_volume * 6.0 / 120.0) * (2.0 * (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); + _volumeAndInertia.push_back(inertia_bb); - double inertia_bb = (volume * 6.0 / 60.0) * (2.0 * (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); - _volumeAndInertia.push_back(inertia_bb); - - double inertia_cc = (volume * 6.0 / 60.0) * (2.0 * (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); - _volumeAndInertia.push_back(inertia_cc); + double inertia_cc = (_volume * 6.0 / 120.0) * (2.0 * (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); + _volumeAndInertia.push_back(inertia_cc); } //class to compute volume, mass, center of mass, and inertia tensor of a mesh. @@ -141,55 +137,98 @@ _triangles(triangles), _referencePoint(referencepoint), _trianglesCount(0), _tetrahedraCount(0), -_verticesCount(0){ +_verticesCount(0), +_centerOfMass(glm::vec3(0.0, 0.0, 0.0)){ - if (_triangles){ - _trianglesCount = _triangles->size() / 3; - } + if (_triangles){ + _trianglesCount = _triangles->size() / 3; + } - if (_vertices){ - _verticesCount = _vertices->size(); - } - generateTetrahedra(); + if (_vertices){ + _verticesCount = _vertices->size(); + } + generateTetrahedra(); } MassProperties::~MassProperties(){ - if (_vertices){ - _vertices->clear(); - } - if (_triangles){ - _triangles->clear(); - } - delete _vertices; - delete _triangles; + if (_vertices){ + _vertices->clear(); + } + if (_triangles){ + _triangles->clear(); + } } -void MassProperties::generateTetrahedra(){ - for (int i = 0; i < _trianglesCount * 3; 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)); - Tetrahedron t(_referencePoint, p1, p2, p3); - _tetrahedra.push_back(t); - } +void MassProperties::generateTetrahedra() { + std::cout << "apex : " << _referencePoint.x << " " << _referencePoint.y << " " << _referencePoint.z << std::endl; + for (int i = 0; i < _trianglesCount * 3; 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)); + Tetrahedron t(_referencePoint, p1, p2, p3); + _tetrahedra.push_back(t); + } } int MassProperties::getTriangleCount() const{ - return _trianglesCount; + return _trianglesCount; } int MassProperties::getVerticesCount() const{ - return _verticesCount; + return _verticesCount; +} + +Vertex MassProperties::getCenterOfMass() const{ + return _centerOfMass; } int MassProperties::getTetrahedraCount() const{ - return _tetrahedra.size(); + return _tetrahedra.size(); } vector MassProperties::getTetrahedra() const{ - return _tetrahedra; + return _tetrahedra; } -vector MassProperties::getVolumeAndInertia(){ - vector volumeAndInertia; - return volumeAndInertia; + +vector MassProperties::getMassProperties(){ + vector volumeAndInertia; + double volume = 0.0; + double inertia_a = 0.0; + double inertia_b = 0.0; + double inertia_c = 0.0; + double inertia_aa = 0.0; + double inertia_bb = 0.0; + double inertia_cc = 0.0; + glm::vec3 centerOfMass; + + //Translate accumulated center of mass from each tetrahedron to mesh center of mass using parallel axis theorem + for each (Tetrahedron tet in _tetrahedra){ + vector tetMassProperties = tet.getVolumeAndInertia(); + volume += tetMassProperties.at(0); //volume + centerOfMass += tet.getCentroid() * (float)tetMassProperties.at(0); + } + + if (volume != 0){ + _centerOfMass = (centerOfMass / (float)volume); + } + + //Translate the moment of inertia from each tetrahedron to mesh center of mass using parallel axis theorem + for each (Tetrahedron tet in _tetrahedra){ + vector tetMassProperties = tet.getVolumeAndInertia(); + const double dist = glm::distance(_centerOfMass, tet.getCentroid()); + inertia_a += tetMassProperties.at(1) + (dist * dist * tetMassProperties.at(0)); + inertia_b += tetMassProperties.at(2) + (dist * dist * tetMassProperties.at(0)); + inertia_c += tetMassProperties.at(3) + (dist * dist * tetMassProperties.at(0)); + inertia_aa += tetMassProperties.at(4) + (dist * dist * tetMassProperties.at(0)); + inertia_bb += tetMassProperties.at(5) + (dist * dist * tetMassProperties.at(0)); + inertia_cc += tetMassProperties.at(6) + (dist * dist * tetMassProperties.at(0)); + } + volumeAndInertia.push_back(volume); + volumeAndInertia.push_back(inertia_a); + volumeAndInertia.push_back(inertia_b); + volumeAndInertia.push_back(inertia_c); + volumeAndInertia.push_back(inertia_aa); + volumeAndInertia.push_back(inertia_bb); + volumeAndInertia.push_back(inertia_cc); + return volumeAndInertia; } \ No newline at end of file diff --git a/libraries/physics/src/MassProperties.h b/libraries/physics/src/MassProperties.h index f5a2f0a0c7..240e0e53f7 100644 --- a/libraries/physics/src/MassProperties.h +++ b/libraries/physics/src/MassProperties.h @@ -8,6 +8,8 @@ // 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_MassProperties_h +#define hifi_MassProperties_h #include #include @@ -15,46 +17,51 @@ #include using namespace std; namespace massproperties{ - typedef glm::vec3 Vertex; - typedef vector Triangle; + typedef glm::vec3 Vertex; + typedef vector Triangle; - //Tetrahedron class containing the base triangle and the apex. - class Tetrahedron{ - private: - Vertex _w; //apex - Vertex _x; - Vertex _y; - Vertex _z; - vector _volumeAndInertia; - public: - Tetrahedron(Vertex p1, Vertex p2, Vertex p3, Vertex p4); - ~Tetrahedron(); - Vertex getX(); - Vertex getY(); - Vertex getZ(); - Vertex getw(); - Vertex getCentroid(); - void computeVolumeAndInertia(); - vector getVolumeAndInertia(); - }; + //Tetrahedron class containing the base triangle and the apex. + class Tetrahedron{ + private: + Vertex _w; //apex + Vertex _x; + Vertex _y; + Vertex _z; + double _volume; + vector _volumeAndInertia; + void computeInertia(); + void computeVolume(); + public: + Tetrahedron(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4); + ~Tetrahedron(); + Vertex getX() const; + Vertex getY() const; + Vertex getZ() const; + Vertex getw() const; + Vertex getCentroid() const; + vector getVolumeAndInertia() const; + }; - class MassProperties{ - private: - int _trianglesCount; - int _tetrahedraCount; - int _verticesCount; - vector *_vertices; - Vertex _referencePoint; - Triangle *_triangles; - vector _tetrahedra; - void generateTetrahedra(); - public: - MassProperties(vector *vertices, Triangle *triangles, Vertex refewrencepoint); - ~MassProperties(); - int getTriangleCount() const; - int getVerticesCount() const; - int getTetrahedraCount() const; - vector getTetrahedra() const; - vector getVolumeAndInertia(); - }; -} \ No newline at end of file + class MassProperties{ + private: + int _trianglesCount; + int _tetrahedraCount; + int _verticesCount; + vector *_vertices; + Vertex _referencePoint; + Vertex _centerOfMass; + Triangle *_triangles; + vector _tetrahedra; + void generateTetrahedra(); + public: + MassProperties(vector *vertices, Triangle *triangles, Vertex refewrencepoint); + ~MassProperties(); + int getTriangleCount() const; + int getVerticesCount() const; + int getTetrahedraCount() const; + Vertex getCenterOfMass() const; + vector getTetrahedra() const; + vector getMassProperties(); + }; +} +#endif // hifi_MassProperties_h \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.cpp b/tests/physics/src/MassPropertiesTests.cpp new file mode 100644 index 0000000000..83a8443cf9 --- /dev/null +++ b/tests/physics/src/MassPropertiesTests.cpp @@ -0,0 +1,133 @@ +// +// MassPropertiesTests.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 "MassPropertiesTests.h" + +void MassPropertiesTests::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); + double volume = 1873.233236; + double inertia_a = 43520.33257; + double inertia_b = 194711.28938; + double inertia_c = 191168.76173; + double inertia_aa = 4417.66150; + double inertia_bb = -46343.16662; + double inertia_cc = 11996.20119; + massproperties::Tetrahedron tet(p0, p1, p2, p3); + glm::vec3 diff = centroid - tet.getCentroid(); + vector voumeAndInertia = tet.getVolumeAndInertia(); + std::cout << std::setprecision(12); + //test if centroid is correct + if (diff.x > epsilion || diff.y > epsilion || diff.z > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Centroid is incorrect : Expected = " << centroid.x << " " << + centroid.y << " " << centroid.z << ", actual = " << tet.getCentroid().x << " " << tet.getCentroid().y << + " " << tet.getCentroid().z << std::endl; + } + + //test if volume is correct + if (abs(volume - voumeAndInertia.at(0)) > epsilion){ + 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))) > epsilion){ + 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))) > epsilion){ + 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))) > epsilion){ + 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))) > epsilion){ + 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))) > epsilion){ + 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))) > epsilion){ + 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 MassPropertiesTests::testWithUnitCube(){ + massproperties::Vertex p0(1.0, -1.0, -1.0); + massproperties::Vertex p1(1.0, -1.0, 1.0); + massproperties::Vertex p2(-1.0, -1.0, 1.0); + massproperties::Vertex p3(-1.0, -1.0, -1.0); + massproperties::Vertex p4(1.0, 1.0, -1.0); + massproperties::Vertex p5(1.0, 1.0, 1.0); + massproperties::Vertex p6(-1.0, 1.0, 1.0); + massproperties::Vertex 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(5); + vector triangles = { 1 - 1, 2 - 1, 3 - 1, 1 - 1, 3 - 1, 4 - 1, 5 - 1, 8 - 1, 7 - 1, 5 - 1, 7 - 1, 6 - 1, 1 - 1, 5 - 1, 6 - 1, 1 - 1, + 6 - 1, 2 - 1, 2 - 1, 6 - 1, 7 - 1, 2 - 1, 7 - 1, 3 - 1, 3 - 1, 7 - 1, 8 - 1, 3 - 1, 8 - 1, 4 - 1, 5 - 1, 1 - 1, 4 - 1, 5 - 1, 4 - 1, 8 - 1 }; + 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 + massproperties::MassProperties massProp1(&vertices, &triangles, {}); + vector volumeAndInertia1 = massProp1.getMassProperties(); + if (abs(centerOfMass.x - massProp1.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp1.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp1.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getCenterOfMass().x << " " << massProp1.getCenterOfMass().y << + " " << massProp1.getCenterOfMass().z << std::endl; + } + + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilion || abs(inertia - (volumeAndInertia1.at(2))) > epsilion || + abs(inertia - (volumeAndInertia1.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << + " " << (volumeAndInertia1.at(3)) << std::endl; + } +} + +void MassPropertiesTests::runAllTests(){ + testWithTetrahedron(); + testWithUnitCube(); +} \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.h b/tests/physics/src/MassPropertiesTests.h new file mode 100644 index 0000000000..583ac25126 --- /dev/null +++ b/tests/physics/src/MassPropertiesTests.h @@ -0,0 +1,20 @@ +// +// MassPropertiesTests.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_MassPropertiesTests_h +#define hifi_MassPropertiesTests_h +#define epsilion 0.02 +namespace MassPropertiesTests{ + void testWithTetrahedron(); + void testWithUnitCube(); + void runAllTests(); +} +#endif // hifi_MassPropertiesTests_h \ No newline at end of file diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index bcf26f4115..eed58dbbdb 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -8,17 +8,21 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include #include "ShapeColliderTests.h" #include "VerletShapeTests.h" #include "ShapeInfoTests.h" #include "ShapeManagerTests.h" #include "BulletUtilTests.h" +#include "MassPropertiesTests.h" int main(int argc, char** argv) { - ShapeColliderTests::runAllTests(); - VerletShapeTests::runAllTests(); - ShapeInfoTests::runAllTests(); - ShapeManagerTests::runAllTests(); - BulletUtilTests::runAllTests(); + //ShapeColliderTests::runAllTests(); + //VerletShapeTests::runAllTests(); + //ShapeInfoTests::runAllTests(); + //ShapeManagerTests::runAllTests(); + // BulletUtilTests::runAllTests(); + MassPropertiesTests::runAllTests(); + getch(); return 0; }