Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-09 02:16:51 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Accuracy improved to 0.01

Browse source
This commit is contained in:
Virendra Singh 2015-03-06 22:07:35 +05:30
parent 42867bf98d
commit 84e05e361e
3 changed files with 184 additions and 181 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -24,137 +24,138 @@ _centerOfMass(Vertex(0.0, 0.0, 0.0)){
MeshInfo::~MeshInfo(){ MeshInfo::~MeshInfo(){
_vertices = NULL; _vertices = NULL;
_triangles = NULL; _triangles = NULL;
} }
inline Vertex MeshInfo::getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ inline Vertex MeshInfo::getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{
Vertex com; Vertex com;
com.x = (p1.x + p2.x + p3.x + p4.x) / 4.0f; 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.y = (p1.y + p2.y + p3.y + p4.y) / 4.0f;
com.z = (p2.z + p2.z + p3.z + p4.z) / 4.0f; com.z = (p1.z + p2.z + p3.z + p4.z) / 4.0f;
return com; return com;
} }
inline float MeshInfo::getVolume(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const{ 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::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) }; glm::vec4(1.0f, 1.0f, 1.0f, 1.0f) };
return glm::determinant(tet) / 6.0f; return glm::determinant(tet) / 6.0f;
} }
Vertex MeshInfo::getMeshCentroid() const{ Vertex MeshInfo::getMeshCentroid() const{
return _centerOfMass; return _centerOfMass;
} }
vector<float> MeshInfo::computeMassProperties(){ vector<float> MeshInfo::computeMassProperties(){
vector<float> volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; vector<float> volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
Vertex p0(0.0, 0.0, 0.0); Vertex origin(0.0, 0.0, 0.0);
float meshVolume = 0.0f; float meshVolume = 0.0f;
glm::mat3 globalMomentOfInertia(0.0); glm::mat3 globalMomentOfInertia(0.0);
glm::mat3 globalProductOfInertia(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. //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){ for (unsigned int i = 0; i < _triangles->size(); i += 3){
Vertex p1 = _vertices->at(_triangles->at(i)); Vertex p1 = _vertices->at(_triangles->at(i));
Vertex p2 = _vertices->at(_triangles->at(i + 1)); Vertex p2 = _vertices->at(_triangles->at(i + 1));
Vertex p3 = _vertices->at(_triangles->at(i + 2)); Vertex p3 = _vertices->at(_triangles->at(i + 2));
float volume = getVolume(p1, p2, p3, p0); float volume = getVolume(p1, p2, p3, origin);
Vertex com = getCentroid(p0, p1, p2, p3); 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 //Translate accumulated center of mass from each tetrahedron to mesh's center of mass using parallel axis theorem
meshVolume += volume; meshVolume += volume;
_centerOfMass += com * volume; _centerOfMass += com * volume;
} }
if (meshVolume == 0){ if (meshVolume == 0){
return volumeAndInertia; return volumeAndInertia;
} }
_centerOfMass = (_centerOfMass / (float)meshVolume); _centerOfMass = (_centerOfMass / (float)meshVolume);
//Translate the moment of inertia from each tetrahedron to mesh's center of mass using parallel axis theorem //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){ for (unsigned int i = 0; i < _triangles->size(); i += 3){
Vertex p1 = _vertices->at(_triangles->at(i)); Vertex p1 = _vertices->at(_triangles->at(i));
Vertex p2 = _vertices->at(_triangles->at(i + 1)); Vertex p2 = _vertices->at(_triangles->at(i + 1));
Vertex p3 = _vertices->at(_triangles->at(i + 2)); Vertex p3 = _vertices->at(_triangles->at(i + 2));
float volume = getVolume(p1, p2, p3, p0); float volume = getVolume(p1, p2, p3, origin);
Vertex com = getCentroid(p0, p1, p2, p3); Vertex com = getCentroid(origin, p1, p2, p3);
glm::mat3 identity; glm::mat3 identity;
Vertex diff = _centerOfMass - com; Vertex diff = _centerOfMass - com;
float diffDot = glm::dot(diff, diff); float diffDot = glm::dot(diff, diff);
glm::mat3 outerDiff = glm::outerProduct(diff, diff); glm::mat3 outerDiff = glm::outerProduct(diff, diff);
//centroid is used for calculating inertia tensor relative to center of mass. //centroid is used for calculating inertia tensor relative to center of mass.
// translate the tetrahedron to its center of mass using P = P - centroid // translate the tetrahedron to its center of mass using P = P - centroid
p0 = p0 - com; Vertex p0 = origin - com;
p1 = p1 - com; p1 = p1 - com;
p2 = p2 - com; p2 = p2 - com;
p3 = p3 - com; p3 = p3 - com;
//Calculate inertia tensor based on Tonon's Formulae given in the paper mentioned below. //Calculate inertia tensor based on Tonon's Formulae given in the paper mentioned below.
//http://docsdrive.com/pdfs/sciencepublications/jmssp/2005/8-11.pdf //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 //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) * ( float inertia_a = (volume * 6.0f / 60.0f) * (
p0.y*p0.y + p0.y*p1.y + p0.y*p2.y + p0.y*p3.y + 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 + p1.y*p1.y + p1.y*p2.y + p1.y*p3.y +
p2.y*p2.y + p2.y*p3.y + p2.y*p2.y + p2.y*p3.y +
p3.y*p3.y + p3.y*p3.y +
p0.z*p0.z + p0.z*p1.z + p0.z*p2.z + p0.z*p3.z + 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 + p1.z*p1.z + p1.z*p2.z + p1.z*p3.z +
p2.z*p2.z + p2.z*p3.z + p2.z*p2.z + p2.z*p3.z +
p3.z*p3.z); p3.z*p3.z);
float inertia_b = (volume * 6.0 / 60.0) * ( float inertia_b = (volume * 6.0f / 60.0f) * (
p0.x*p0.x + p0.x*p1.x + p0.x*p2.x + p0.x*p3.x + 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 + p1.x*p1.x + p1.x*p2.x + p1.x*p3.x +
p2.x*p2.x + p2.x*p3.x + p2.x*p2.x + p2.x*p3.x +
p3.x*p3.x + p3.x*p3.x +
p0.z*p0.z + p0.z*p1.z + p0.z*p2.z + p0.z*p3.z + 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 + p1.z*p1.z + p1.z*p2.z + p1.z*p3.z +
p2.z*p2.z + p2.z*p3.z + p2.z*p2.z + p2.z*p3.z +
p3.z*p3.z); p3.z*p3.z);
float inertia_c = (volume * 6.0 / 60.0) * ( float inertia_c = (volume * 6.0f / 60.0f) * (
p0.x*p0.x + p0.x*p1.x + p0.x*p2.x + p0.x*p3.x + 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 + p1.x*p1.x + p1.x*p2.x + p1.x*p3.x +
p2.x*p2.x + p2.x*p3.x + p2.x*p2.x + p2.x*p3.x +
p3.x*p3.x + p3.x*p3.x +
p0.y*p0.y + p0.y*p1.y + p0.y*p2.y + p0.y*p3.y + 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 + p1.y*p1.y + p1.y*p2.y + p1.y*p3.y +
p2.y*p2.y + p2.y*p3.y + p2.y*p2.y + p2.y*p3.y +
p3.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) + 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 + p0.y*p1.z + p0.y*p2.z + p0.y*p3.z +
p1.y*p0.z + p1.y*p2.z + p1.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 + p2.y*p0.z + p2.y*p1.z + p2.y*p3.z +
p3.y*p0.z + p3.y*p1.z + p3.y*p2.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) + 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 + p0.x*p1.z + p0.x*p2.z + p0.x*p3.z +
p1.x*p0.z + p1.x*p2.z + p1.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 + p2.x*p0.z + p2.x*p1.z + p2.x*p3.z +
p3.x*p0.z + p3.x*p1.z + p3.x*p2.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) + 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 + p0.x*p1.y + p0.x*p2.y + p0.x*p3.y +
p1.x*p0.y + p1.x*p2.y + p1.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 + p2.x*p0.y + p2.x*p1.y + p2.x*p3.y +
p3.x*p0.y + p3.x*p1.y + p3.x*p2.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 //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements
globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff); glm::mat3 localMomentInertia = { Vertex(inertia_a, 0.0f, 0.0f), Vertex(0.0f, inertia_b, 0.0f),
globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff); Vertex(0.0f, 0.0f, inertia_c) };
} glm::mat3 localProductInertia = { Vertex(inertia_aa, 0.0f, 0.0f), Vertex(0.0f, inertia_bb, 0.0f),
volumeAndInertia.push_back(meshVolume); Vertex(0.0f, 0.0f, inertia_cc) };
volumeAndInertia.push_back(globalMomentOfInertia[0][0]);
volumeAndInertia.push_back(globalMomentOfInertia[1][1]); //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product
volumeAndInertia.push_back(globalMomentOfInertia[2][2]); globalMomentOfInertia += localMomentInertia + volume * ((diffDot*identity) - outerDiff);
volumeAndInertia.push_back(globalProductOfInertia[0][0]); globalProductOfInertia += localProductInertia + volume * ((diffDot * identity) - outerDiff);
volumeAndInertia.push_back(globalProductOfInertia[1][1]); }
volumeAndInertia.push_back(globalProductOfInertia[2][2]); volumeAndInertia[0] = meshVolume;
return volumeAndInertia; 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;
} }

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

@ -16,18 +16,18 @@
using namespace std; using namespace std;
namespace meshinfo{ namespace meshinfo{
typedef glm::vec3 Vertex; typedef glm::vec3 Vertex;
class MeshInfo{ class MeshInfo{
private: private:
inline float getVolume(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; inline float getVolume(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const;
vector<float> computeVolumeAndInertia(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; vector<float> computeVolumeAndInertia(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const;
public: public:
vector<Vertex> *_vertices; vector<Vertex> *_vertices;
Vertex _centerOfMass; Vertex _centerOfMass;
vector<int> *_triangles; vector<int> *_triangles;
MeshInfo(vector<Vertex> *vertices, vector<int> *triangles); MeshInfo(vector<Vertex> *vertices, vector<int> *triangles);
~MeshInfo(); ~MeshInfo();
inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const; inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const;
Vertex getMeshCentroid() const; Vertex getMeshCentroid() const;
vector<float> computeMassProperties(); vector<float> computeMassProperties();
}; };
} }

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

@ -14,7 +14,7 @@
#include <MeshInfo.h> #include <MeshInfo.h>
#include "MeshInfoTests.h" #include "MeshInfoTests.h"
const double epsilon = 0.02; const float epsilon = 0.01f;
void MeshInfoTests::testWithTetrahedron(){ void MeshInfoTests::testWithTetrahedron(){
glm::vec3 p0(8.33220, -11.86875, 0.93355); glm::vec3 p0(8.33220, -11.86875, 0.93355);
glm::vec3 p1(0.75523, 5.00000, 16.37072); 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 p3(2.00000, 5.00000, 3.00000);
glm::vec3 centroid(15.92492, 0.782813, 3.72962); glm::vec3 centroid(15.92492, 0.782813, 3.72962);
//translate the tetrahedron so that its apex is on origin //translate the tetrahedron so that its apex is on origin
glm::vec3 p11 = p1 - p0; glm::vec3 p11 = p1 - p0;
glm::vec3 p22 = p2 - p0; glm::vec3 p22 = p2 - p0;
glm::vec3 p33 = p3 - p0; glm::vec3 p33 = p3 - p0;
vector<glm::vec3> vertices = { p11, p22, p33 }; vector<glm::vec3> vertices = { p11, p22, p33 };
vector<int> triangles = { 0, 1, 2 }; vector<int> triangles = { 0, 1, 2 };
float volume = 1873.233236; float volume = 1873.233236f;
float inertia_a = 43520.33257; float inertia_a = 43520.33257f;
float inertia_b = 194711.28938; //actual should be 194711.28938f. But for some reason it becomes 194711.296875 during
float inertia_c = 191168.76173; //runtime due to how floating points are stored.
float inertia_aa = 4417.66150; float inertia_b = 194711.289f;
float inertia_bb = -46343.16662; float inertia_c = 191168.76173f;
float inertia_cc = 11996.20119; float inertia_aa = 4417.66150f;
float inertia_bb = -46343.16662f;
float inertia_cc = 11996.20119f;
meshinfo::MeshInfo meshinfo(&vertices,&triangles); meshinfo::MeshInfo meshinfo(&vertices,&triangles);
glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3); glm::vec3 tetCenterOfMass = meshinfo.getCentroid(p0, p1, p2, p3);
glm::vec3 diff = centroid - tetCenterOfMass; glm::vec3 diff = centroid - tetCenterOfMass;
vector<float> voumeAndInertia = meshinfo.computeMassProperties(); vector<float> voumeAndInertia = meshinfo.computeMassProperties();
std::cout << std::setprecision(12); std::cout << std::setprecision(12);
//test if centroid is correct //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 << " " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Centroid is incorrect : Expected = " << centroid.x << " " <<
centroid.y << " " << centroid.z << ", actual = " << tetCenterOfMass.x << " " << tetCenterOfMass.y << centroid.y << " " << centroid.z << ", actual = " << tetCenterOfMass.x << " " << tetCenterOfMass.y <<
" " << tetCenterOfMass.z << std::endl; " " << tetCenterOfMass.z << std::endl;
} }
//test if volume is correct //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 << " " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << " " <<
", actual = " << voumeAndInertia.at(0) << std::endl; ", actual = " << voumeAndInertia.at(0) << std::endl;
} }
//test if moment of inertia with respect to x axis is correct //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 = " << 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 //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 = " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to y axis is incorrect : Expected = " <<
inertia_b << " " << ", actual = " << (voumeAndInertia.at(2)) << std::endl; inertia_b << " " << ", actual = " << (voumeAndInertia.at(2)) << std::endl;
} }
//test if moment of inertia with respect to z axis is correct //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 = " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to z axis is incorrect : Expected = " <<
inertia_c << " " << ", actual = " << (voumeAndInertia.at(3)) << std::endl; inertia_c << " " << ", actual = " << (voumeAndInertia.at(3)) << std::endl;
} }
//test if product of inertia with respect to x axis is correct //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 = " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to x axis is incorrect : Expected = " <<
inertia_aa << " " << ", actual = " << (voumeAndInertia.at(4)) << std::endl; inertia_aa << " " << ", actual = " << (voumeAndInertia.at(4)) << std::endl;
} }
//test if product of inertia with respect to y axis is correct //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 = " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to y axis is incorrect : Expected = " <<
inertia_bb << " " << ", actual = " << (voumeAndInertia.at(5)) << std::endl; inertia_bb << " " << ", actual = " << (voumeAndInertia.at(5)) << std::endl;
} }
//test if product of inertia with respect to z axis is correct //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 = " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Product of inertia with respect to z axis is incorrect : Expected = " <<
inertia_cc << " " << ", actual = " << (voumeAndInertia.at(6)) << std::endl; inertia_cc << " " << ", actual = " << (voumeAndInertia.at(6)) << std::endl;
} }
@ -94,15 +96,15 @@ void MeshInfoTests::testWithTetrahedron(){
} }
void MeshInfoTests::testWithCube(){ void MeshInfoTests::testWithCube(){
glm::vec3 p0(1.0, -1.0, -1.0); glm::vec3 p0(1.0, -1.0, -1.0);
glm::vec3 p1(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 p2(-1.0, -1.0, 1.0);
glm::vec3 p3(-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 p4(1.0, 1.0, -1.0);
glm::vec3 p5(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 p6(-1.0, 1.0, 1.0);
glm::vec3 p7(-1.0, 1.0, -1.0); glm::vec3 p7(-1.0, 1.0, -1.0);
vector<glm::vec3> vertices; vector<glm::vec3> vertices;
vertices.push_back(p0); vertices.push_back(p0);
vertices.push_back(p1); vertices.push_back(p1);
vertices.push_back(p2); 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 double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6
//test with origin as reference point //test with origin as reference point
meshinfo::MeshInfo massProp1(&vertices, &triangles); meshinfo::MeshInfo massProp1(&vertices, &triangles);
vector<float> volumeAndInertia1 = massProp1.computeMassProperties(); vector<float> volumeAndInertia1 = massProp1.computeMassProperties();
if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon ||
abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " <<
centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " <<
massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; 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 << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume <<
", actual = " << volumeAndInertia1.at(0) << std::endl; ", actual = " << volumeAndInertia1.at(0) << std::endl;
} }
if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon ||
abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ abs(inertia - (volumeAndInertia1.at(3))) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " <<
inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) <<
" " << (volumeAndInertia1.at(3)) << std::endl; " " << (volumeAndInertia1.at(3)) << std::endl;
@ -144,15 +146,15 @@ void MeshInfoTests::testWithCube(){
void MeshInfoTests::testWithUnitCube() void MeshInfoTests::testWithUnitCube()
{ {
glm::vec3 p0(0, 0, 1); glm::vec3 p0(0, 0, 1);
glm::vec3 p1(1, 0, 1); glm::vec3 p1(1, 0, 1);
glm::vec3 p2(0, 1, 1); glm::vec3 p2(0, 1, 1);
glm::vec3 p3(1, 1, 1); glm::vec3 p3(1, 1, 1);
glm::vec3 p4(0, 0, 0); glm::vec3 p4(0, 0, 0);
glm::vec3 p5(1, 0, 0); glm::vec3 p5(1, 0, 0);
glm::vec3 p6(0, 1, 0); glm::vec3 p6(0, 1, 0);
glm::vec3 p7(1, 1, 0); glm::vec3 p7(1, 1, 0);
vector<glm::vec3> vertices; vector<glm::vec3> vertices;
vertices.push_back(p0); vertices.push_back(p0);
vertices.push_back(p1); vertices.push_back(p1);
vertices.push_back(p2); vertices.push_back(p2);
@ -172,20 +174,20 @@ void MeshInfoTests::testWithUnitCube()
//test with origin as reference point //test with origin as reference point
meshinfo::MeshInfo massProp1(&vertices, &triangles); meshinfo::MeshInfo massProp1(&vertices, &triangles);
vector<float> volumeAndInertia1 = massProp1.computeMassProperties(); vector<float> volumeAndInertia1 = massProp1.computeMassProperties();
if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) > epsilon || if (abs(centerOfMass.x - massProp1.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp1.getMeshCentroid().y) >
abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){ epsilon || abs(centerOfMass.z - massProp1.getMeshCentroid().z) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x <<
" " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " << " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getMeshCentroid().x << " " <<
massProp1.getMeshCentroid().y << " " << massProp1.getMeshCentroid().z << std::endl; 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 << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume <<
", actual = " << volumeAndInertia1.at(0) << std::endl; ", actual = " << volumeAndInertia1.at(0) << std::endl;
} }
if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon || if (abs(inertia - (volumeAndInertia1.at(1))) > epsilon || abs(inertia - (volumeAndInertia1.at(2))) > epsilon ||
abs(inertia - (volumeAndInertia1.at(3))) > epsilon){ abs(inertia - (volumeAndInertia1.at(3))) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " << std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia is incorrect : Expected = " << inertia << " " <<
inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) <<
" " << (volumeAndInertia1.at(3)) << std::endl; " " << (volumeAndInertia1.at(3)) << std::endl;