mirror of
https://github.com/overte-org/overte.git
synced 2025-07-23 13:04:07 +02:00
Parallel axis theorem correction
This commit is contained in:
Virendra Singh
parent
1914661bbf
commit
4627d2e7b5
4 changed files with 141 additions and 35 deletions
|
|
@ -52,10 +52,10 @@ vector<double> 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<Vertex> *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<double> 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<double> 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<double> 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;
|
||||
}
|
||||
|
|
@ -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<int> 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<int> 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,8 +115,13 @@ 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(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 ||
|
||||
|
|
@ -125,9 +130,103 @@ void MassPropertiesTests::testWithUnitCube(){
|
|||
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<double> 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<massproperties::Vertex> 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<int> 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<double> 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<double> 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();
|
||||
}
|
||||
|
|
@ -15,6 +15,7 @@
|
|||
namespace MassPropertiesTests{
|
||||
void testWithTetrahedron();
|
||||
void testWithUnitCube();
|
||||
void testWithCube();
|
||||
void runAllTests();
|
||||
}
|
||||
#endif // hifi_MassPropertiesTests_h
|
||||
|
|
@ -8,7 +8,6 @@
|
|||
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
|
||||
//
|
||||
|
||||
#include <conio.h>
|
||||
#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();
|
||||
ShapeColliderTests::runAllTests();
|
||||
VerletShapeTests::runAllTests();
|
||||
ShapeInfoTests::runAllTests();
|
||||
ShapeManagerTests::runAllTests();
|
||||
BulletUtilTests::runAllTests();
|
||||
MassPropertiesTests::runAllTests();
|
||||
getch();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue