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