Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-26 18:55:37 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

code revamp and correction in parallel axis theorem

Browse source
This commit is contained in:
Virendra Singh 2015-03-11 03:11:17 +05:30
parent ce4226d6b9
commit a0e7125e11
4 changed files with 99 additions and 77 deletions
libraries/physics/src
MeshInfo.cppMeshInfo.h
tests/physics/src
MeshInfoTests.cppMeshInfoTests.h

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

@ -10,7 +10,7 @@
//
#include "MeshInfo.h"
#include <iostream>df
#include <iostream>
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<float> MeshInfo::computeMassProperties(){
vector<float> volumeAndInertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
Vertex origin(0.0, 0.0, 0.0);
glm::mat3 identity;
float meshVolume = 0.0f;
glm::mat3 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<float> 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<float> 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<float> 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<float> 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;
}

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

@ -26,7 +26,6 @@ namespace meshinfo{
vector<int> *_triangles;
MeshInfo(vector<Vertex> *vertices, vector<int> *triangles);
~MeshInfo();
inline Vertex getCentroid(const Vertex p1, const Vertex p2, const Vertex p3, const Vertex p4) const;
Vertex getMeshCentroid() const;
vector<float> computeMassProperties();
};

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

@ -14,7 +14,7 @@
#include <MeshInfo.h>
#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<float> voumeAndInertia = meshinfo.computeMassProperties();
glm::vec3 tetCenterOfMass = meshinfo.getMeshCentroid();
//get original center of mass
tetCenterOfMass = tetCenterOfMass + p0;
glm::vec3 diff = centroid - tetCenterOfMass;
std::cout << std::setprecision(12);
//test if centroid is correct
if (diff.x > epsilon || diff.y > epsilon || diff.z > epsilon){
@ -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<glm::vec3> vertices = { p0, p1, p2, p3 };
vector<int> triangles = { 0, 1, 2, 0, 3, 1, 1, 3, 2, 0, 2, 3 }; //clockwise direction
meshinfo::MeshInfo massProp(&vertices, &triangles);
vector<float> volumeAndInertia = massProp.computeMassProperties();
std::cout << volumeAndInertia[0] << " " << volumeAndInertia[1] << " " << volumeAndInertia[2]
<< " " << volumeAndInertia[3]
<< " " << volumeAndInertia[4]
<< " " << volumeAndInertia[5] << " " << volumeAndInertia[6] << std::endl;
//translate the tetrahedron so that the model is placed at origin i.e. com is at origin
p0 -= centroid;
p1 -= centroid;
p2 -= centroid;
p3 -= centroid;
}
void MeshInfoTests::testWithCube(){
glm::vec3 p0(1.0, -1.0, -1.0);
glm::vec3 p1(1.0, -1.0, 1.0);
@ -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<float> 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<float> volumeAndInertia = massProp.computeMassProperties();
if (abs(centerOfMass.x - massProp.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp.getMeshCentroid().y) > epsilon ||
abs(centerOfMass.z - massProp.getMeshCentroid().z) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " <<
centerOfMass.y << " " << centerOfMass.z << ", actual = " << 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<float> 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<float> volumeAndInertia = massProp.computeMassProperties();
if (abs(centerOfMass.x - massProp.getMeshCentroid().x) > epsilon || abs(centerOfMass.y - massProp.getMeshCentroid().y) >
epsilon || abs(centerOfMass.z - massProp.getMeshCentroid().z) > epsilon){
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x <<
" " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << 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();
}

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

@ -16,5 +16,6 @@ namespace MeshInfoTests{
void testWithUnitCube();
void testWithCube();
void runAllTests();
void testWithTetrahedronAsMesh();
}
#endif // hifi_MeshInfoTests_h