From 3d558dae6435c0749bd84c51571bd0d94c627f54 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Tue, 3 Mar 2015 01:51:35 +0530 Subject: [PATCH 1/9] MassProperties of a 3D mesh --- libraries/physics/src/MassProperties.cpp | 195 +++++++++++++++++++++++ libraries/physics/src/MassProperties.h | 60 +++++++ 2 files changed, 255 insertions(+) create mode 100644 libraries/physics/src/MassProperties.cpp create mode 100644 libraries/physics/src/MassProperties.h diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp new file mode 100644 index 0000000000..ec4066ce9f --- /dev/null +++ b/libraries/physics/src/MassProperties.cpp @@ -0,0 +1,195 @@ +// +// MassProperties.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 "MassProperties.h" +using namespace massproperties; + +Tetrahedron::Tetrahedron(Vertex p1, Vertex p2, Vertex p3, Vertex p4) :\ +_w(p1), +_x(p2), +_y(p3), +_z(p4){ + + computeVolumeAndInertia(); +} +Tetrahedron::~Tetrahedron(){ + +} + +Vertex Tetrahedron::getX(){ + return _x; +} + +Vertex Tetrahedron::getY(){ + return _y; +} +Vertex Tetrahedron::getZ(){ + return _z; +} + +Vertex Tetrahedron::getw(){ + 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; +} + +vector Tetrahedron::getVolumeAndInertia(){ + 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)); + + double volume = glm::sqrt(squaredVol);// volume of tetrahedron + _volumeAndInertia.push_back(volume); + + //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 + + 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_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 / 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 / 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); +} + +//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. We can provide another reference +//point by passing it as 3rd parameter to the constructor + +MassProperties::MassProperties(vector *vertices, Triangle *triangles, Vertex referencepoint = glm::vec3(0.0,0.0,0.0)):\ +_vertices(vertices), +_triangles(triangles), +_referencePoint(referencepoint), +_trianglesCount(0), +_tetrahedraCount(0), +_verticesCount(0){ + + if (_triangles){ + _trianglesCount = _triangles->size() / 3; + } + + if (_vertices){ + _verticesCount = _vertices->size(); + } + generateTetrahedra(); +} + +MassProperties::~MassProperties(){ + if (_vertices){ + _vertices->clear(); + } + if (_triangles){ + _triangles->clear(); + } + delete _vertices; + delete _triangles; +} + +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); + } +} + +int MassProperties::getTriangleCount() const{ + return _trianglesCount; +} + +int MassProperties::getVerticesCount() const{ + return _verticesCount; +} + +int MassProperties::getTetrahedraCount() const{ + return _tetrahedra.size(); +} + +vector MassProperties::getTetrahedra() const{ + return _tetrahedra; +} +vector MassProperties::getVolumeAndInertia(){ + vector volumeAndInertia; + return volumeAndInertia; +} \ No newline at end of file diff --git a/libraries/physics/src/MassProperties.h b/libraries/physics/src/MassProperties.h new file mode 100644 index 0000000000..f5a2f0a0c7 --- /dev/null +++ b/libraries/physics/src/MassProperties.h @@ -0,0 +1,60 @@ +// +// MassProperties.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 +// + +#include +#include +#include +#include +using namespace std; +namespace massproperties{ + 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(); + }; + + 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 From 7a129235c26af553f0fe5991e493b32e5516f6e4 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 4 Mar 2015 01:12:20 +0530 Subject: [PATCH 2/9] 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; } From 4627d2e7b5112984edd80a250a30a9cbcb131d5e Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 4 Mar 2015 03:52:59 +0530 Subject: [PATCH 3/9] Parallel axis theorem correction --- libraries/physics/src/MassProperties.cpp | 44 ++++---- tests/physics/src/MassPropertiesTests.cpp | 117 ++++++++++++++++++++-- tests/physics/src/MassPropertiesTests.h | 1 + tests/physics/src/main.cpp | 14 ++- 4 files changed, 141 insertions(+), 35 deletions(-) diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp index 4d2e668f95..c6264ee220 100644 --- a/libraries/physics/src/MassProperties.cpp +++ b/libraries/physics/src/MassProperties.cpp @@ -52,10 +52,10 @@ vector Tetrahedron::getVolumeAndInertia() const{ } 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) }; + 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; } void Tetrahedron::computeInertia(){ @@ -128,8 +128,8 @@ void Tetrahedron::computeInertia(){ } //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. We can provide another reference -//point by passing it as 3rd parameter to the constructor +//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. We can provide +//another reference point by passing it as 3rd parameter to the constructor MassProperties::MassProperties(vector *vertices, Triangle *triangles, Vertex referencepoint = glm::vec3(0.0,0.0,0.0)):\ _vertices(vertices), @@ -160,7 +160,6 @@ MassProperties::~MassProperties(){ } 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)); @@ -200,6 +199,8 @@ vector MassProperties::getMassProperties(){ double inertia_bb = 0.0; double inertia_cc = 0.0; glm::vec3 centerOfMass; + glm::mat3 globalInertia(0.0); + glm::mat3 globalProductInertia(0.0); //Translate accumulated center of mass from each tetrahedron to mesh center of mass using parallel axis theorem for each (Tetrahedron tet in _tetrahedra){ @@ -215,20 +216,27 @@ vector MassProperties::getMassProperties(){ //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)); + glm::mat3 identity; + glm::vec3 diff = _centerOfMass - tet.getCentroid(); + float diffDot = glm::dot(diff, diff); + glm::mat3 outerDiff = glm::outerProduct(diff, diff); + + //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements + glm::mat3 localMomentInertia = { Vertex(tetMassProperties.at(1), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(2), 0.0f), + Vertex(0.0f, 0.0f, tetMassProperties.at(3)) }; + glm::mat3 localProductInertia = { Vertex(tetMassProperties.at(4), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(5), 0.0f), + Vertex(0.0f, 0.0f, tetMassProperties.at(6)) }; + + //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product + globalInertia += localMomentInertia + (float)tetMassProperties.at(0) * ((diffDot*identity) - outerDiff); + globalProductInertia += localProductInertia + (float)tetMassProperties.at(0) * ((diffDot * identity) - outerDiff); } 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); + volumeAndInertia.push_back(globalInertia[0][0]); + volumeAndInertia.push_back(globalInertia[1][1]); + volumeAndInertia.push_back(globalInertia[2][2]); + volumeAndInertia.push_back(globalProductInertia[0][0]); + volumeAndInertia.push_back(globalProductInertia[1][1]); + volumeAndInertia.push_back(globalProductInertia[2][2]); return volumeAndInertia; } \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.cpp b/tests/physics/src/MassPropertiesTests.cpp index 83a8443cf9..f36a494b35 100644 --- a/tests/physics/src/MassPropertiesTests.cpp +++ b/tests/physics/src/MassPropertiesTests.cpp @@ -47,7 +47,7 @@ void MassPropertiesTests::testWithTetrahedron(){ //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 = " << + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to x axis is incorrect : Expected = " << inertia_a << " " << ", actual = " << (voumeAndInertia.at(1)) << std::endl; } @@ -83,7 +83,7 @@ void MassPropertiesTests::testWithTetrahedron(){ } -void MassPropertiesTests::testWithUnitCube(){ +void MassPropertiesTests::testWithCube(){ 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); @@ -101,11 +101,11 @@ void MassPropertiesTests::testWithUnitCube(){ 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 }; + 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 volume = 8.0; double side = 2.0; double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6 @@ -115,19 +115,118 @@ void MassPropertiesTests::testWithUnitCube(){ 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; + 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 || + if (abs(volume - volumeAndInertia1.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << 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; } + + //test with {2,2,2} as reference point + massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 2, 2 }); + vector volumeAndInertia2 = massProp2.getMassProperties(); + if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << + massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; + } + + if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia2.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || + abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << + " " << (volumeAndInertia2.at(3)) << std::endl; + } } +void MassPropertiesTests::testWithUnitCube() +{ + massproperties::Vertex p0(0, 0, 1); + massproperties::Vertex p1(1, 0, 1); + massproperties::Vertex p2(0, 1, 1); + massproperties::Vertex p3(1, 1, 1); + massproperties::Vertex p4(0, 0, 0); + massproperties::Vertex p5(1, 0, 0); + massproperties::Vertex p6(0, 1, 0); + massproperties::Vertex 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 + 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(volume - volumeAndInertia1.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << 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; + } + + //test with {2,1,2} as reference point + massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 1, 2 }); + vector volumeAndInertia2 = massProp2.getMassProperties(); + if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << + massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; + } + + if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia2.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || + abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << + " " << (volumeAndInertia2.at(3)) << std::endl; + } +} void MassPropertiesTests::runAllTests(){ testWithTetrahedron(); testWithUnitCube(); + testWithCube(); } \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.h b/tests/physics/src/MassPropertiesTests.h index 583ac25126..21ab3ccb0a 100644 --- a/tests/physics/src/MassPropertiesTests.h +++ b/tests/physics/src/MassPropertiesTests.h @@ -15,6 +15,7 @@ namespace MassPropertiesTests{ void testWithTetrahedron(); void testWithUnitCube(); + void testWithCube(); 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 eed58dbbdb..d42b5d1241 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -8,7 +8,6 @@ // 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" @@ -17,12 +16,11 @@ #include "MassPropertiesTests.h" int main(int argc, char** argv) { - //ShapeColliderTests::runAllTests(); - //VerletShapeTests::runAllTests(); - //ShapeInfoTests::runAllTests(); - //ShapeManagerTests::runAllTests(); - // BulletUtilTests::runAllTests(); - MassPropertiesTests::runAllTests(); - getch(); + ShapeColliderTests::runAllTests(); + VerletShapeTests::runAllTests(); + ShapeInfoTests::runAllTests(); + ShapeManagerTests::runAllTests(); + BulletUtilTests::runAllTests(); + MassPropertiesTests::runAllTests(); return 0; } From 1facbbb844c8941883fb1ce26e9f7980dccbfad4 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 4 Mar 2015 08:21:39 +0530 Subject: [PATCH 4/9] Removed MSVC specific for each blocks --- libraries/physics/src/MassProperties.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp index c6264ee220..deaa37587d 100644 --- a/libraries/physics/src/MassProperties.cpp +++ b/libraries/physics/src/MassProperties.cpp @@ -203,7 +203,7 @@ vector MassProperties::getMassProperties(){ glm::mat3 globalProductInertia(0.0); //Translate accumulated center of mass from each tetrahedron to mesh center of mass using parallel axis theorem - for each (Tetrahedron tet in _tetrahedra){ + for(Tetrahedron tet : _tetrahedra){ vector tetMassProperties = tet.getVolumeAndInertia(); volume += tetMassProperties.at(0); //volume centerOfMass += tet.getCentroid() * (float)tetMassProperties.at(0); @@ -214,7 +214,7 @@ vector MassProperties::getMassProperties(){ } //Translate the moment of inertia from each tetrahedron to mesh center of mass using parallel axis theorem - for each (Tetrahedron tet in _tetrahedra){ + for(Tetrahedron tet : _tetrahedra){ vector tetMassProperties = tet.getVolumeAndInertia(); glm::mat3 identity; glm::vec3 diff = _centerOfMass - tet.getCentroid(); From 4366525da46ae7e81df17d5ddf45a1a9604926ae Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 4 Mar 2015 08:51:44 +0530 Subject: [PATCH 5/9] Removed unused variables --- libraries/physics/src/MassProperties.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp index deaa37587d..e579f2d358 100644 --- a/libraries/physics/src/MassProperties.cpp +++ b/libraries/physics/src/MassProperties.cpp @@ -192,12 +192,6 @@ vector MassProperties::getTetrahedra() const{ 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; glm::mat3 globalInertia(0.0); glm::mat3 globalProductInertia(0.0); From 42867bf98d089cdeb6e5c4a4f663a8c5fd5d7159 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Fri, 6 Mar 2015 11:10:38 +0530 Subject: [PATCH 6/9] code revamp --- libraries/physics/src/MassProperties.cpp | 236 ------------------ libraries/physics/src/MassProperties.h | 67 ----- libraries/physics/src/MeshInfo.cpp | 160 ++++++++++++ libraries/physics/src/MeshInfo.h | 34 +++ tests/physics/src/MassPropertiesTests.cpp | 232 ----------------- tests/physics/src/MeshInfoTests.cpp | 198 +++++++++++++++ ...{MassPropertiesTests.h => MeshInfoTests.h} | 11 +- tests/physics/src/main.cpp | 4 +- 8 files changed, 399 insertions(+), 543 deletions(-) delete mode 100644 libraries/physics/src/MassProperties.cpp delete mode 100644 libraries/physics/src/MassProperties.h create mode 100644 libraries/physics/src/MeshInfo.cpp create mode 100644 libraries/physics/src/MeshInfo.h delete mode 100644 tests/physics/src/MassPropertiesTests.cpp create mode 100644 tests/physics/src/MeshInfoTests.cpp rename tests/physics/src/{MassPropertiesTests.h => MeshInfoTests.h} (66%) diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp deleted file mode 100644 index e579f2d358..0000000000 --- a/libraries/physics/src/MassProperties.cpp +++ /dev/null @@ -1,236 +0,0 @@ -// -// MassProperties.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 "MassProperties.h" -using namespace massproperties; - -Tetrahedron::Tetrahedron(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) :\ -_w(p1), -_x(p2), -_y(p3), -_z(p4){ - computeVolume(); - computeInertia(); -} -Tetrahedron::~Tetrahedron(){ - -} - -Vertex Tetrahedron::getX() const{ - return _x; -} - -Vertex Tetrahedron::getY() const{ - return _y; -} -Vertex Tetrahedron::getZ() const{ - return _z; -} - -Vertex Tetrahedron::getw() const{ - return _w; -} - -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() const{ - return _volumeAndInertia; -} - -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); -} - -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; - - //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_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_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_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_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. -//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. We can provide -//another reference point by passing it as 3rd parameter to the constructor - -MassProperties::MassProperties(vector *vertices, Triangle *triangles, Vertex referencepoint = glm::vec3(0.0,0.0,0.0)):\ -_vertices(vertices), -_triangles(triangles), -_referencePoint(referencepoint), -_trianglesCount(0), -_tetrahedraCount(0), -_verticesCount(0), -_centerOfMass(glm::vec3(0.0, 0.0, 0.0)){ - - if (_triangles){ - _trianglesCount = _triangles->size() / 3; - } - - if (_vertices){ - _verticesCount = _vertices->size(); - } - generateTetrahedra(); -} - -MassProperties::~MassProperties(){ - 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); - } -} - -int MassProperties::getTriangleCount() const{ - return _trianglesCount; -} - -int MassProperties::getVerticesCount() const{ - return _verticesCount; -} - -Vertex MassProperties::getCenterOfMass() const{ - return _centerOfMass; -} - -int MassProperties::getTetrahedraCount() const{ - return _tetrahedra.size(); -} - -vector MassProperties::getTetrahedra() const{ - return _tetrahedra; -} - -vector MassProperties::getMassProperties(){ - vector volumeAndInertia; - double volume = 0.0; - glm::vec3 centerOfMass; - glm::mat3 globalInertia(0.0); - glm::mat3 globalProductInertia(0.0); - - //Translate accumulated center of mass from each tetrahedron to mesh center of mass using parallel axis theorem - for(Tetrahedron tet : _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(Tetrahedron tet : _tetrahedra){ - vector tetMassProperties = tet.getVolumeAndInertia(); - glm::mat3 identity; - glm::vec3 diff = _centerOfMass - tet.getCentroid(); - float diffDot = glm::dot(diff, diff); - glm::mat3 outerDiff = glm::outerProduct(diff, diff); - - //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements - glm::mat3 localMomentInertia = { Vertex(tetMassProperties.at(1), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(2), 0.0f), - Vertex(0.0f, 0.0f, tetMassProperties.at(3)) }; - glm::mat3 localProductInertia = { Vertex(tetMassProperties.at(4), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(5), 0.0f), - Vertex(0.0f, 0.0f, tetMassProperties.at(6)) }; - - //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product - globalInertia += localMomentInertia + (float)tetMassProperties.at(0) * ((diffDot*identity) - outerDiff); - globalProductInertia += localProductInertia + (float)tetMassProperties.at(0) * ((diffDot * identity) - outerDiff); - } - volumeAndInertia.push_back(volume); - volumeAndInertia.push_back(globalInertia[0][0]); - volumeAndInertia.push_back(globalInertia[1][1]); - volumeAndInertia.push_back(globalInertia[2][2]); - volumeAndInertia.push_back(globalProductInertia[0][0]); - volumeAndInertia.push_back(globalProductInertia[1][1]); - volumeAndInertia.push_back(globalProductInertia[2][2]); - return volumeAndInertia; -} \ No newline at end of file diff --git a/libraries/physics/src/MassProperties.h b/libraries/physics/src/MassProperties.h deleted file mode 100644 index 240e0e53f7..0000000000 --- a/libraries/physics/src/MassProperties.h +++ /dev/null @@ -1,67 +0,0 @@ -// -// MassProperties.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_MassProperties_h -#define hifi_MassProperties_h - -#include -#include -#include -#include -using namespace std; -namespace massproperties{ - 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; - 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; - 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/libraries/physics/src/MeshInfo.cpp b/libraries/physics/src/MeshInfo.cpp new file mode 100644 index 0000000000..b406cff269 --- /dev/null +++ b/libraries/physics/src/MeshInfo.cpp @@ -0,0 +1,160 @@ +// +// 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 df +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), +_triangles(triangles), +_centerOfMass(Vertex(0.0, 0.0, 0.0)){ +} + +MeshInfo::~MeshInfo(){ + + _vertices = NULL; + _triangles = NULL; + +} + +inline Vertex MeshInfo::getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ + Vertex com; + com.x = (p1.x + p2.x + p3.x + p4.x) / 4.0f; + com.y = (p2.y + p2.y + p3.y + p4.y) / 4.0f; + com.z = (p2.z + p2.z + p3.z + p4.z) / 4.0f; + return com; +} + +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 p0(0.0, 0.0, 0.0); + float meshVolume = 0.0f; + glm::mat3 globalMomentOfInertia(0.0); + glm::mat3 globalProductOfInertia(0.0); + + //First we need need the center of mass of the mesh in order to translate the tetrahedron inertia to center of mass of the mesh. + for (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, p0); + Vertex com = getCentroid(p0, p1, p2, p3); + //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem + meshVolume += volume; + _centerOfMass += com * volume; + } + if (meshVolume == 0){ + return volumeAndInertia; + } + _centerOfMass = (_centerOfMass / (float)meshVolume); + + //Translate the moment of inertia from each tetrahedron to mesh's center of mass using parallel axis theorem + for (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, p0); + Vertex com = getCentroid(p0, p1, p2, p3); + glm::mat3 identity; + Vertex diff = _centerOfMass - com; + float diffDot = glm::dot(diff, diff); + glm::mat3 outerDiff = glm::outerProduct(diff, diff); + //centroid is used for calculating inertia tensor relative to center of mass. + // translate the tetrahedron to its center of mass using P = P - centroid + p0 = p0 - 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 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); + + float 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); + + float 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); + + float 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); + + float 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); + + float 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); + //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements + glm::mat3 localMomentInertia = { Vertex(inertia_a, 0.0f, 0.0f), Vertex(0.0f, inertia_b, 0.0f), + Vertex(0.0f, 0.0f, inertia_c) }; + glm::mat3 localProductInertia = { Vertex(inertia_aa, 0.0f, 0.0f), Vertex(0.0f, inertia_bb, 0.0f), + Vertex(0.0f, 0.0f, inertia_cc) }; + + //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product + globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff); + globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff); + } + volumeAndInertia.push_back(meshVolume); + volumeAndInertia.push_back(globalMomentOfInertia[0][0]); + volumeAndInertia.push_back(globalMomentOfInertia[1][1]); + volumeAndInertia.push_back(globalMomentOfInertia[2][2]); + volumeAndInertia.push_back(globalProductOfInertia[0][0]); + volumeAndInertia.push_back(globalProductOfInertia[1][1]); + volumeAndInertia.push_back(globalProductOfInertia[2][2]); + return volumeAndInertia; +} \ No newline at end of file diff --git a/libraries/physics/src/MeshInfo.h b/libraries/physics/src/MeshInfo.h new file mode 100644 index 0000000000..b400ef9cf0 --- /dev/null +++ b/libraries/physics/src/MeshInfo.h @@ -0,0 +1,34 @@ +// +// 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(); + inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; + Vertex getMeshCentroid() const; + vector computeMassProperties(); + }; +} +#endif // hifi_MeshInfo_h \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.cpp b/tests/physics/src/MassPropertiesTests.cpp deleted file mode 100644 index f36a494b35..0000000000 --- a/tests/physics/src/MassPropertiesTests.cpp +++ /dev/null @@ -1,232 +0,0 @@ -// -// 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::testWithCube(){ - 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(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 - 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(volume - volumeAndInertia1.at(0)) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia1.at(0) << 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; - } - - //test with {2,2,2} as reference point - massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 2, 2 }); - vector volumeAndInertia2 = massProp2.getMassProperties(); - if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || - abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << - " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << - massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; - } - - if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia2.at(0) << std::endl; - } - - if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || - abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << - inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << - " " << (volumeAndInertia2.at(3)) << std::endl; - } -} - -void MassPropertiesTests::testWithUnitCube() -{ - massproperties::Vertex p0(0, 0, 1); - massproperties::Vertex p1(1, 0, 1); - massproperties::Vertex p2(0, 1, 1); - massproperties::Vertex p3(1, 1, 1); - massproperties::Vertex p4(0, 0, 0); - massproperties::Vertex p5(1, 0, 0); - massproperties::Vertex p6(0, 1, 0); - massproperties::Vertex 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 - 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(volume - volumeAndInertia1.at(0)) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia1.at(0) << 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; - } - - //test with {2,1,2} as reference point - massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 1, 2 }); - vector volumeAndInertia2 = massProp2.getMassProperties(); - if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || - abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << - centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << - massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; - } - - if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia2.at(0) << std::endl; - } - - if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || - abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << - inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << - " " << (volumeAndInertia2.at(3)) << std::endl; - } -} -void MassPropertiesTests::runAllTests(){ - testWithTetrahedron(); - testWithUnitCube(); - testWithCube(); -} \ No newline at end of file diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp new file mode 100644 index 0000000000..69a6964128 --- /dev/null +++ b/tests/physics/src/MeshInfoTests.cpp @@ -0,0 +1,198 @@ +// +// 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 double epsilon = 0.02; +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.233236; + float inertia_a = 43520.33257; + float inertia_b = 194711.28938; + float inertia_c = 191168.76173; + float inertia_aa = 4417.66150; + float inertia_bb = -46343.16662; + float inertia_cc = 11996.20119; + + meshinfo::MeshInfo meshinfo(&vertices,&triangles); + glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3); + glm::vec3 diff = centroid - tetCenterOfMass; + vector voumeAndInertia = meshinfo.computeMassProperties(); + 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::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 massProp1(&vertices, &triangles); + vector volumeAndInertia1 = massProp1.computeMassProperties(); + if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || + abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << + massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + } + + if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || + abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << + " " << (volumeAndInertia1.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 massProp1(&vertices, &triangles); + vector volumeAndInertia1 = massProp1.computeMassProperties(); + if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || + abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << + massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + } + + if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || + abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << + " " << (volumeAndInertia1.at(3)) << std::endl; + } +} +void MeshInfoTests::runAllTests(){ + testWithTetrahedron(); + testWithUnitCube(); + testWithCube(); +} \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.h b/tests/physics/src/MeshInfoTests.h similarity index 66% rename from tests/physics/src/MassPropertiesTests.h rename to tests/physics/src/MeshInfoTests.h index 21ab3ccb0a..d4c954b802 100644 --- a/tests/physics/src/MassPropertiesTests.h +++ b/tests/physics/src/MeshInfoTests.h @@ -1,5 +1,5 @@ // -// MassPropertiesTests.h +// MeshInfoTests.h // tests/physics/src // // Created by Virendra Singh on 2015.03.02 @@ -9,13 +9,12 @@ // 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{ +#ifndef hifi_MeshInfoTests_h +#define hifi_MeshInfoTests_h +namespace MeshInfoTests{ void testWithTetrahedron(); void testWithUnitCube(); void testWithCube(); void runAllTests(); } -#endif // hifi_MassPropertiesTests_h \ No newline at end of file +#endif // hifi_MeshInfoTests_h \ No newline at end of file diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index d42b5d1241..e7dae7aca5 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -13,7 +13,7 @@ #include "ShapeInfoTests.h" #include "ShapeManagerTests.h" #include "BulletUtilTests.h" -#include "MassPropertiesTests.h" +#include "MeshInfoTests.h" int main(int argc, char** argv) { ShapeColliderTests::runAllTests(); @@ -21,6 +21,6 @@ int main(int argc, char** argv) { ShapeInfoTests::runAllTests(); ShapeManagerTests::runAllTests(); BulletUtilTests::runAllTests(); - MassPropertiesTests::runAllTests(); + MeshInfoTests::runAllTests(); return 0; } From 84e05e361eed107117b6dd0b290282425f85d17b Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Fri, 6 Mar 2015 22:07:35 +0530 Subject: [PATCH 7/9] Accuracy improved to 0.01 --- libraries/physics/src/MeshInfo.cpp | 221 ++++++++++++++-------------- libraries/physics/src/MeshInfo.h | 18 +-- tests/physics/src/MeshInfoTests.cpp | 126 ++++++++-------- 3 files changed, 184 insertions(+), 181 deletions(-) diff --git a/libraries/physics/src/MeshInfo.cpp b/libraries/physics/src/MeshInfo.cpp index b406cff269..0d9149cb44 100644 --- a/libraries/physics/src/MeshInfo.cpp +++ b/libraries/physics/src/MeshInfo.cpp @@ -24,137 +24,138 @@ _centerOfMass(Vertex(0.0, 0.0, 0.0)){ MeshInfo::~MeshInfo(){ - _vertices = NULL; - _triangles = NULL; + _vertices = NULL; + _triangles = NULL; } inline Vertex MeshInfo::getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ - Vertex com; - com.x = (p1.x + p2.x + p3.x + p4.x) / 4.0f; - com.y = (p2.y + p2.y + p3.y + p4.y) / 4.0f; - com.z = (p2.z + p2.z + p3.z + p4.z) / 4.0f; - return com; + Vertex com; + com.x = (p1.x + p2.x + p3.x + p4.x) / 4.0f; + com.y = (p1.y + p2.y + p3.y + p4.y) / 4.0f; + com.z = (p1.z + p2.z + p3.z + p4.z) / 4.0f; + return com; } 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; + 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; + return _centerOfMass; } vector MeshInfo::computeMassProperties(){ - vector volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - Vertex p0(0.0, 0.0, 0.0); - float meshVolume = 0.0f; - glm::mat3 globalMomentOfInertia(0.0); - glm::mat3 globalProductOfInertia(0.0); + vector volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + Vertex origin(0.0, 0.0, 0.0); + float meshVolume = 0.0f; + glm::mat3 globalMomentOfInertia(0.0); + glm::mat3 globalProductOfInertia(0.0); - //First we need need the center of mass of the mesh in order to translate the tetrahedron inertia to center of mass of the mesh. - for (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, p0); - Vertex com = getCentroid(p0, p1, p2, p3); - //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem - meshVolume += volume; - _centerOfMass += com * volume; - } - if (meshVolume == 0){ - return volumeAndInertia; - } - _centerOfMass = (_centerOfMass / (float)meshVolume); + //First we need need the center of mass of the mesh in order to translate the tetrahedron inertia to center of mass of the mesh. + 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 = getCentroid(origin, p1, p2, p3); + //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem + meshVolume += volume; + _centerOfMass += com * volume; + } + if (meshVolume == 0){ + return volumeAndInertia; + } + _centerOfMass = (_centerOfMass / (float)meshVolume); - //Translate the moment of inertia from each tetrahedron to mesh's center of mass using parallel axis theorem - for (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, p0); - Vertex com = getCentroid(p0, p1, p2, p3); - glm::mat3 identity; - Vertex diff = _centerOfMass - com; - float diffDot = glm::dot(diff, diff); - glm::mat3 outerDiff = glm::outerProduct(diff, diff); - //centroid is used for calculating inertia tensor relative to center of mass. - // translate the tetrahedron to its center of mass using P = P - centroid - p0 = p0 - com; - p1 = p1 - com; - p2 = p2 - com; - p3 = p3 - com; + //Translate the moment of inertia from each tetrahedron to mesh's center of mass using parallel axis theorem + 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 = getCentroid(origin, p1, p2, p3); + glm::mat3 identity; + Vertex diff = _centerOfMass - com; + float diffDot = glm::dot(diff, diff); + glm::mat3 outerDiff = glm::outerProduct(diff, diff); + //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 + //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 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); + float inertia_a = (volume * 6.0f / 60.0f) * ( + 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 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); + float inertia_b = (volume * 6.0f / 60.0f) * ( + 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 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); + float inertia_c = (volume * 6.0f / 60.0f) * ( + 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 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); + float inertia_aa = (volume * 6.0f / 120.0f) * (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 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); + float inertia_bb = (volume * 6.0f / 120.0f) * (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 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); - //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements - glm::mat3 localMomentInertia = { Vertex(inertia_a, 0.0f, 0.0f), Vertex(0.0f, inertia_b, 0.0f), - Vertex(0.0f, 0.0f, inertia_c) }; - glm::mat3 localProductInertia = { Vertex(inertia_aa, 0.0f, 0.0f), Vertex(0.0f, inertia_bb, 0.0f), - Vertex(0.0f, 0.0f, inertia_cc) }; + float inertia_cc = (volume * 6.0f / 120.0f) * (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); - //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product - globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff); - globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff); - } - volumeAndInertia.push_back(meshVolume); - volumeAndInertia.push_back(globalMomentOfInertia[0][0]); - volumeAndInertia.push_back(globalMomentOfInertia[1][1]); - volumeAndInertia.push_back(globalMomentOfInertia[2][2]); - volumeAndInertia.push_back(globalProductOfInertia[0][0]); - volumeAndInertia.push_back(globalProductOfInertia[1][1]); - volumeAndInertia.push_back(globalProductOfInertia[2][2]); - return volumeAndInertia; + //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements + glm::mat3 localMomentInertia = { Vertex(inertia_a, 0.0f, 0.0f), Vertex(0.0f, inertia_b, 0.0f), + Vertex(0.0f, 0.0f, inertia_c) }; + glm::mat3 localProductInertia = { Vertex(inertia_aa, 0.0f, 0.0f), Vertex(0.0f, inertia_bb, 0.0f), + Vertex(0.0f, 0.0f, inertia_cc) }; + + //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product + globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff); + globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff); + } + volumeAndInertia[0] = meshVolume; + volumeAndInertia[1] = globalMomentOfInertia[0][0]; + volumeAndInertia[2] = globalMomentOfInertia[1][1]; + volumeAndInertia[3] = globalMomentOfInertia[2][2]; + volumeAndInertia[4] = globalProductOfInertia[0][0]; + volumeAndInertia[5] = globalProductOfInertia[1][1]; + volumeAndInertia[6] = globalProductOfInertia[2][2]; + return volumeAndInertia; } \ No newline at end of file diff --git a/libraries/physics/src/MeshInfo.h b/libraries/physics/src/MeshInfo.h index b400ef9cf0..d267c75ae6 100644 --- a/libraries/physics/src/MeshInfo.h +++ b/libraries/physics/src/MeshInfo.h @@ -16,18 +16,18 @@ 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; + 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; + vector *_vertices; + Vertex _centerOfMass; + vector *_triangles; MeshInfo(vector *vertices, vector *triangles); ~MeshInfo(); - inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; - Vertex getMeshCentroid() const; + inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; + Vertex getMeshCentroid() const; vector computeMassProperties(); }; } diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp index 69a6964128..73364b207e 100644 --- a/tests/physics/src/MeshInfoTests.cpp +++ b/tests/physics/src/MeshInfoTests.cpp @@ -14,7 +14,7 @@ #include #include "MeshInfoTests.h" -const double epsilon = 0.02; +const float epsilon = 0.01f; void MeshInfoTests::testWithTetrahedron(){ glm::vec3 p0(8.33220, -11.86875, 0.93355); glm::vec3 p1(0.75523, 5.00000, 16.37072); @@ -22,71 +22,73 @@ void MeshInfoTests::testWithTetrahedron(){ 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 }; + //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.233236; - float inertia_a = 43520.33257; - float inertia_b = 194711.28938; - float inertia_c = 191168.76173; - float inertia_aa = 4417.66150; - float inertia_bb = -46343.16662; - float inertia_cc = 11996.20119; + 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); - glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3); - glm::vec3 diff = centroid - tetCenterOfMass; - vector voumeAndInertia = meshinfo.computeMassProperties(); + meshinfo::MeshInfo meshinfo(&vertices,&triangles); + glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3); + glm::vec3 diff = centroid - tetCenterOfMass; + vector voumeAndInertia = meshinfo.computeMassProperties(); std::cout << std::setprecision(12); //test if centroid is correct - if (diff.x > epsilon || diff.y > epsilon || diff.z > epsilon){ + 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; + 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){ + 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){ + 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; + 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){ + 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){ + 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){ + 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){ + 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){ + 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; } @@ -94,15 +96,15 @@ void MeshInfoTests::testWithTetrahedron(){ } 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; + 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); @@ -120,22 +122,22 @@ void MeshInfoTests::testWithCube(){ 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 massProp1(&vertices, &triangles); + meshinfo::MeshInfo massProp1(&vertices, &triangles); vector volumeAndInertia1 = massProp1.computeMassProperties(); - if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || - abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || + abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << - centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << - massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << + massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; } - if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << ", actual = " << volumeAndInertia1.at(0) << std::endl; } - if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || - abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || + abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << " " << (volumeAndInertia1.at(3)) << std::endl; @@ -144,15 +146,15 @@ void MeshInfoTests::testWithCube(){ 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; + 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); @@ -172,20 +174,20 @@ void MeshInfoTests::testWithUnitCube() //test with origin as reference point meshinfo::MeshInfo massProp1(&vertices, &triangles); vector volumeAndInertia1 = massProp1.computeMassProperties(); - if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || - abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > + epsilon || abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << - " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << - massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << + massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; } - if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << ", actual = " << volumeAndInertia1.at(0) << std::endl; } - if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || - abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || + abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << " " << (volumeAndInertia1.at(3)) << std::endl; From a0e7125e119ce7afa6b01d6ef808bf063fe54494 Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 11 Mar 2015 03:11:17 +0530 Subject: [PATCH 8/9] code revamp and correction in parallel axis theorem --- libraries/physics/src/MeshInfo.cpp | 85 ++++++++++++--------------- libraries/physics/src/MeshInfo.h | 1 - tests/physics/src/MeshInfoTests.cpp | 89 ++++++++++++++++++++--------- tests/physics/src/MeshInfoTests.h | 1 + 4 files changed, 99 insertions(+), 77 deletions(-) diff --git a/libraries/physics/src/MeshInfo.cpp b/libraries/physics/src/MeshInfo.cpp index 0d9149cb44..8df5ff914d 100644 --- a/libraries/physics/src/MeshInfo.cpp +++ b/libraries/physics/src/MeshInfo.cpp @@ -10,7 +10,7 @@ // #include "MeshInfo.h" -#include df +#include using namespace meshinfo; //class to compute volume, mass, center of mass, and inertia tensor of a mesh. @@ -29,14 +29,6 @@ MeshInfo::~MeshInfo(){ } -inline Vertex MeshInfo::getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ - Vertex com; - com.x = (p1.x + p2.x + p3.x + p4.x) / 4.0f; - com.y = (p1.y + p2.y + p3.y + p4.y) / 4.0f; - com.z = (p1.z + p2.z + p3.z + p4.z) / 4.0f; - return com; -} - 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) }; @@ -50,37 +42,19 @@ Vertex MeshInfo::getMeshCentroid() const{ 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 globalMomentOfInertia(0.0); - glm::mat3 globalProductOfInertia(0.0); + glm::mat3 globalInertiaTensors(0.0); - //First we need need the center of mass of the mesh in order to translate the tetrahedron inertia to center of mass of the mesh. 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 = getCentroid(origin, p1, p2, p3); - //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem + float volume = getVolume(p1, p2, p3, origin); + Vertex com = 0.25f * (p1 + p2 + p3); meshVolume += volume; _centerOfMass += com * volume; - } - if (meshVolume == 0){ - return volumeAndInertia; - } - _centerOfMass = (_centerOfMass / (float)meshVolume); - //Translate the moment of inertia from each tetrahedron to mesh's center of mass using parallel axis theorem - 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 = getCentroid(origin, p1, p2, p3); - glm::mat3 identity; - Vertex diff = _centerOfMass - com; - float diffDot = glm::dot(diff, diff); - glm::mat3 outerDiff = glm::outerProduct(diff, diff); //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; @@ -92,7 +66,7 @@ vector MeshInfo::computeMassProperties(){ //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 inertia_a = (volume * 6.0f / 60.0f) * ( + 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 + @@ -102,7 +76,7 @@ vector MeshInfo::computeMassProperties(){ p2.z*p2.z + p2.z*p3.z + p3.z*p3.z); - float inertia_b = (volume * 6.0f / 60.0f) * ( + 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 + @@ -112,7 +86,7 @@ vector MeshInfo::computeMassProperties(){ p2.z*p2.z + p2.z*p3.z + p3.z*p3.z); - float inertia_c = (volume * 6.0f / 60.0f) * ( + 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 + @@ -122,40 +96,53 @@ vector MeshInfo::computeMassProperties(){ p2.y*p2.y + p2.y*p3.y + p3.y*p3.y); - float inertia_aa = (volume * 6.0f / 120.0f) * (2.0f * (p0.y*p0.z + p1.y*p1.z + p2.y*p2.z + p3.y*p3.z) + + 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 inertia_bb = (volume * 6.0f / 120.0f) * (2.0f * (p0.x*p0.z + p1.x*p1.z + p2.x*p2.z + p3.x*p3.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 inertia_cc = (volume * 6.0f / 120.0f) * (2.0f * (p0.x*p0.y + p1.x*p1.y + p2.x*p2.y + p3.x*p3.y) + + 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 - glm::mat3 localMomentInertia = { Vertex(inertia_a, 0.0f, 0.0f), Vertex(0.0f, inertia_b, 0.0f), - Vertex(0.0f, 0.0f, inertia_c) }; - glm::mat3 localProductInertia = { Vertex(inertia_aa, 0.0f, 0.0f), Vertex(0.0f, inertia_bb, 0.0f), - Vertex(0.0f, 0.0f, inertia_cc) }; + // | 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 - globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff); - globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff); + 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] = globalMomentOfInertia[0][0]; - volumeAndInertia[2] = globalMomentOfInertia[1][1]; - volumeAndInertia[3] = globalMomentOfInertia[2][2]; - volumeAndInertia[4] = globalProductOfInertia[0][0]; - volumeAndInertia[5] = globalProductOfInertia[1][1]; - volumeAndInertia[6] = globalProductOfInertia[2][2]; + 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 index d267c75ae6..67dbc8b0d6 100644 --- a/libraries/physics/src/MeshInfo.h +++ b/libraries/physics/src/MeshInfo.h @@ -26,7 +26,6 @@ namespace meshinfo{ vector *_triangles; MeshInfo(vector *vertices, vector *triangles); ~MeshInfo(); - inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; Vertex getMeshCentroid() const; vector computeMassProperties(); }; diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp index 73364b207e..58c38017a6 100644 --- a/tests/physics/src/MeshInfoTests.cpp +++ b/tests/physics/src/MeshInfoTests.cpp @@ -14,7 +14,7 @@ #include #include "MeshInfoTests.h" -const float epsilon = 0.01f; +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); @@ -40,9 +40,12 @@ void MeshInfoTests::testWithTetrahedron(){ float inertia_cc = 11996.20119f; meshinfo::MeshInfo meshinfo(&vertices,&triangles); - glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3); - glm::vec3 diff = centroid - tetCenterOfMass; 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){ @@ -95,6 +98,37 @@ void MeshInfoTests::testWithTetrahedron(){ } +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); + 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; + std::cout << std::setprecision(12); + vector vertices = { p0, p1, p2, p3 }; + vector triangles = { 0, 1, 2, 0, 3, 1, 1, 3, 2, 0, 2, 3 }; //clockwise direction + 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); @@ -122,25 +156,25 @@ void MeshInfoTests::testWithCube(){ 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 massProp1(&vertices, &triangles); - vector volumeAndInertia1 = massProp1.computeMassProperties(); - if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || - abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + 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 = " << massProp1.getMeshCentroid().x << " " << - massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp.getMeshCentroid().x << " " << + massProp.getMeshCentroid().y << " " << massProp.getMeshCentroid().z << std::endl; } - if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + if (abs(volume - volumeAndInertia.at(0)) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia1.at(0) << std::endl; + ", actual = " << volumeAndInertia.at(0) << std::endl; } - if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || - abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + 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 = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << - " " << (volumeAndInertia1.at(3)) << std::endl; + inertia << " " << inertia << ", actual = " << (volumeAndInertia.at(1)) << " " << (volumeAndInertia.at(2)) << + " " << (volumeAndInertia.at(3)) << std::endl; } } @@ -172,29 +206,30 @@ void MeshInfoTests::testWithUnitCube() std::cout << std::setprecision(10); //test with origin as reference point - meshinfo::MeshInfo massProp1(&vertices, &triangles); - vector volumeAndInertia1 = massProp1.computeMassProperties(); - if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > - epsilon || abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ + 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 = " << massProp1.getMeshCentroid().x << " " << - massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp.getMeshCentroid().x << " " << + massProp.getMeshCentroid().y << " " << massProp.getMeshCentroid().z << std::endl; } - if (abs(volume - volumeAndInertia1.at(0)) > epsilon){ + if (abs(volume - volumeAndInertia.at(0)) > epsilon){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << - ", actual = " << volumeAndInertia1.at(0) << std::endl; + ", actual = " << volumeAndInertia.at(0) << std::endl; } - if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || - abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ + 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 = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << - " " << (volumeAndInertia1.at(3)) << std::endl; + inertia << " " << inertia << ", actual = " << (volumeAndInertia.at(1)) << " " << (volumeAndInertia.at(2)) << + " " << (volumeAndInertia.at(3)) << std::endl; } } void MeshInfoTests::runAllTests(){ testWithTetrahedron(); + testWithTetrahedronAsMesh(); testWithUnitCube(); testWithCube(); } \ No newline at end of file diff --git a/tests/physics/src/MeshInfoTests.h b/tests/physics/src/MeshInfoTests.h index d4c954b802..0ddd8d0944 100644 --- a/tests/physics/src/MeshInfoTests.h +++ b/tests/physics/src/MeshInfoTests.h @@ -16,5 +16,6 @@ namespace MeshInfoTests{ void testWithUnitCube(); void testWithCube(); void runAllTests(); + void testWithTetrahedronAsMesh(); } #endif // hifi_MeshInfoTests_h \ No newline at end of file From 7954dd4d67b4a73825bbd61ffacc0c32cccc0c5d Mon Sep 17 00:00:00 2001 From: Virendra Singh Date: Wed, 11 Mar 2015 03:27:26 +0530 Subject: [PATCH 9/9] anticlockwise triangle order --- tests/physics/src/MeshInfoTests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp index 58c38017a6..fa841f3930 100644 --- a/tests/physics/src/MeshInfoTests.cpp +++ b/tests/physics/src/MeshInfoTests.cpp @@ -115,7 +115,7 @@ void MeshInfoTests::testWithTetrahedronAsMesh(){ float inertia_cc = 11996.20119f; std::cout << std::setprecision(12); vector vertices = { p0, p1, p2, p3 }; - vector triangles = { 0, 1, 2, 0, 3, 1, 1, 3, 2, 0, 2, 3 }; //clockwise direction + 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]