diff --git a/libraries/physics/src/MassProperties.cpp b/libraries/physics/src/MassProperties.cpp index 4d2e668f95..c6264ee220 100644 --- a/libraries/physics/src/MassProperties.cpp +++ b/libraries/physics/src/MassProperties.cpp @@ -52,10 +52,10 @@ vector Tetrahedron::getVolumeAndInertia() const{ } void Tetrahedron::computeVolume(){ - glm::mat4 tet = { glm::vec4(_x.x, _y.x, _z.x, _w.x), glm::vec4(_x.y, _y.y, _z.y, _w.y), glm::vec4(_x.z, _y.z, _z.z, _w.z), glm::vec4(1.0f, 1.0f, 1.0f, 1.0f) }; + glm::mat4 tet = { glm::vec4(_x.x, _y.x, _z.x, _w.x), glm::vec4(_x.y, _y.y, _z.y, _w.y), glm::vec4(_x.z, _y.z, _z.z, _w.z), + glm::vec4(1.0f, 1.0f, 1.0f, 1.0f) }; _volume = glm::determinant(tet) / 6.0f; _volumeAndInertia.push_back(_volume); - std::cout << "volume : " << _volume << std::endl; } void Tetrahedron::computeInertia(){ @@ -128,8 +128,8 @@ void Tetrahedron::computeInertia(){ } //class to compute volume, mass, center of mass, and inertia tensor of a mesh. -//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. We can provide another reference -//point by passing it as 3rd parameter to the constructor +//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. We can provide +//another reference point by passing it as 3rd parameter to the constructor MassProperties::MassProperties(vector *vertices, Triangle *triangles, Vertex referencepoint = glm::vec3(0.0,0.0,0.0)):\ _vertices(vertices), @@ -160,7 +160,6 @@ MassProperties::~MassProperties(){ } void MassProperties::generateTetrahedra() { - std::cout << "apex : " << _referencePoint.x << " " << _referencePoint.y << " " << _referencePoint.z << std::endl; for (int i = 0; i < _trianglesCount * 3; i += 3){ Vertex p1 = _vertices->at(_triangles->at(i)); Vertex p2 = _vertices->at(_triangles->at(i + 1)); @@ -200,6 +199,8 @@ vector MassProperties::getMassProperties(){ double inertia_bb = 0.0; double inertia_cc = 0.0; glm::vec3 centerOfMass; + glm::mat3 globalInertia(0.0); + glm::mat3 globalProductInertia(0.0); //Translate accumulated center of mass from each tetrahedron to mesh center of mass using parallel axis theorem for each (Tetrahedron tet in _tetrahedra){ @@ -215,20 +216,27 @@ vector MassProperties::getMassProperties(){ //Translate the moment of inertia from each tetrahedron to mesh center of mass using parallel axis theorem for each (Tetrahedron tet in _tetrahedra){ vector tetMassProperties = tet.getVolumeAndInertia(); - const double dist = glm::distance(_centerOfMass, tet.getCentroid()); - inertia_a += tetMassProperties.at(1) + (dist * dist * tetMassProperties.at(0)); - inertia_b += tetMassProperties.at(2) + (dist * dist * tetMassProperties.at(0)); - inertia_c += tetMassProperties.at(3) + (dist * dist * tetMassProperties.at(0)); - inertia_aa += tetMassProperties.at(4) + (dist * dist * tetMassProperties.at(0)); - inertia_bb += tetMassProperties.at(5) + (dist * dist * tetMassProperties.at(0)); - inertia_cc += tetMassProperties.at(6) + (dist * dist * tetMassProperties.at(0)); + glm::mat3 identity; + glm::vec3 diff = _centerOfMass - tet.getCentroid(); + float diffDot = glm::dot(diff, diff); + glm::mat3 outerDiff = glm::outerProduct(diff, diff); + + //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements + glm::mat3 localMomentInertia = { Vertex(tetMassProperties.at(1), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(2), 0.0f), + Vertex(0.0f, 0.0f, tetMassProperties.at(3)) }; + glm::mat3 localProductInertia = { Vertex(tetMassProperties.at(4), 0.0f, 0.0f), Vertex(0.0f, tetMassProperties.at(5), 0.0f), + Vertex(0.0f, 0.0f, tetMassProperties.at(6)) }; + + //Parallel axis theorem J = I * m[(R.R)*Identity - RxR] where x is outer cross product + globalInertia += localMomentInertia + (float)tetMassProperties.at(0) * ((diffDot*identity) - outerDiff); + globalProductInertia += localProductInertia + (float)tetMassProperties.at(0) * ((diffDot * identity) - outerDiff); } volumeAndInertia.push_back(volume); - volumeAndInertia.push_back(inertia_a); - volumeAndInertia.push_back(inertia_b); - volumeAndInertia.push_back(inertia_c); - volumeAndInertia.push_back(inertia_aa); - volumeAndInertia.push_back(inertia_bb); - volumeAndInertia.push_back(inertia_cc); + volumeAndInertia.push_back(globalInertia[0][0]); + volumeAndInertia.push_back(globalInertia[1][1]); + volumeAndInertia.push_back(globalInertia[2][2]); + volumeAndInertia.push_back(globalProductInertia[0][0]); + volumeAndInertia.push_back(globalProductInertia[1][1]); + volumeAndInertia.push_back(globalProductInertia[2][2]); return volumeAndInertia; } \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.cpp b/tests/physics/src/MassPropertiesTests.cpp index 83a8443cf9..f36a494b35 100644 --- a/tests/physics/src/MassPropertiesTests.cpp +++ b/tests/physics/src/MassPropertiesTests.cpp @@ -47,7 +47,7 @@ void MassPropertiesTests::testWithTetrahedron(){ //test if moment of inertia with respect to x axis is correct if (abs(inertia_a - (voumeAndInertia.at(1))) > epsilion){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to x axis is incorrect : Expected = " << + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment of inertia with respect to x axis is incorrect : Expected = " << inertia_a << " " << ", actual = " << (voumeAndInertia.at(1)) << std::endl; } @@ -83,7 +83,7 @@ void MassPropertiesTests::testWithTetrahedron(){ } -void MassPropertiesTests::testWithUnitCube(){ +void MassPropertiesTests::testWithCube(){ massproperties::Vertex p0(1.0, -1.0, -1.0); massproperties::Vertex p1(1.0, -1.0, 1.0); massproperties::Vertex p2(-1.0, -1.0, 1.0); @@ -101,11 +101,11 @@ void MassPropertiesTests::testWithUnitCube(){ vertices.push_back(p5); vertices.push_back(p6); vertices.push_back(p7); - std::cout << std::setprecision(5); - vector triangles = { 1 - 1, 2 - 1, 3 - 1, 1 - 1, 3 - 1, 4 - 1, 5 - 1, 8 - 1, 7 - 1, 5 - 1, 7 - 1, 6 - 1, 1 - 1, 5 - 1, 6 - 1, 1 - 1, - 6 - 1, 2 - 1, 2 - 1, 6 - 1, 7 - 1, 2 - 1, 7 - 1, 3 - 1, 3 - 1, 7 - 1, 8 - 1, 3 - 1, 8 - 1, 4 - 1, 5 - 1, 1 - 1, 4 - 1, 5 - 1, 4 - 1, 8 - 1 }; + std::cout << std::setprecision(10); + vector triangles = { 0, 1, 2, 0, 2, 3, 4, 7, 6, 4, 6, 5, 0, 4, 5, 0, 5, 1, 1, 5, 6, 1, 6, 2, 2, 6, + 7, 2, 7, 3, 4, 0, 3, 4, 3, 7 }; glm::vec3 centerOfMass(0.0, 0.0, 0.0); - double volume =8.0; + double volume = 8.0; double side = 2.0; double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6 @@ -115,19 +115,118 @@ void MassPropertiesTests::testWithUnitCube(){ if (abs(centerOfMass.x - massProp1.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp1.getCenterOfMass().y) > epsilion || abs(centerOfMass.z - massProp1.getCenterOfMass().z) > epsilion){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << - centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getCenterOfMass().x << " " << massProp1.getCenterOfMass().y << - " " << massProp1.getCenterOfMass().z << std::endl; + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getCenterOfMass().x << " " << + massProp1.getCenterOfMass().y << " " << massProp1.getCenterOfMass().z << std::endl; } - if (abs(inertia - (volumeAndInertia1.at(1))) > epsilion || abs(inertia - (volumeAndInertia1.at(2))) > epsilion || + if (abs(volume - volumeAndInertia1.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilion || abs(inertia - (volumeAndInertia1.at(2))) > epsilion || abs(inertia - (volumeAndInertia1.at(3))) > epsilion){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << " " << (volumeAndInertia1.at(3)) << std::endl; } + + //test with {2,2,2} as reference point + massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 2, 2 }); + vector volumeAndInertia2 = massProp2.getMassProperties(); + if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << + massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; + } + + if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia2.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || + abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << + " " << (volumeAndInertia2.at(3)) << std::endl; + } } +void MassPropertiesTests::testWithUnitCube() +{ + massproperties::Vertex p0(0, 0, 1); + massproperties::Vertex p1(1, 0, 1); + massproperties::Vertex p2(0, 1, 1); + massproperties::Vertex p3(1, 1, 1); + massproperties::Vertex p4(0, 0, 0); + massproperties::Vertex p5(1, 0, 0); + massproperties::Vertex p6(0, 1, 0); + massproperties::Vertex p7(1, 1, 0); + vector vertices; + vertices.push_back(p0); + vertices.push_back(p1); + vertices.push_back(p2); + vertices.push_back(p3); + vertices.push_back(p4); + vertices.push_back(p5); + vertices.push_back(p6); + vertices.push_back(p7); + vector triangles = { 0, 1, 2, 1, 3, 2, 2, 3, 7, 2, 7, 6, 1, 7, 3, 1, 5, 7, 6, 7, 4, 7, 5, 4, 0, 4, 1, + 1, 4, 5, 2, 6, 4, 0, 2, 4 }; + glm::vec3 centerOfMass(0.5, 0.5, 0.5); + double volume = 1.0; + double side = 1.0; + double inertia = (volume * side * side) / 6.0; //inertia of a unit cube is (mass * side * side) /6 + std::cout << std::setprecision(10); + + //test with origin as reference point + massproperties::MassProperties massProp1(&vertices, &triangles, {}); + vector volumeAndInertia1 = massProp1.getMassProperties(); + if (abs(centerOfMass.x - massProp1.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp1.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp1.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << + " " << centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getCenterOfMass().x << " " << + massProp1.getCenterOfMass().y << " " << massProp1.getCenterOfMass().z << std::endl; + } + + if (abs(volume - volumeAndInertia1.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia1.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia1.at(1))) > epsilion || abs(inertia - (volumeAndInertia1.at(2))) > epsilion || + abs(inertia - (volumeAndInertia1.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia1.at(1)) << " " << (volumeAndInertia1.at(2)) << + " " << (volumeAndInertia1.at(3)) << std::endl; + } + + //test with {2,1,2} as reference point + massproperties::MassProperties massProp2(&vertices, &triangles, { 2, 1, 2 }); + vector volumeAndInertia2 = massProp2.getMassProperties(); + if (abs(centerOfMass.x - massProp2.getCenterOfMass().x) > epsilion || abs(centerOfMass.y - massProp2.getCenterOfMass().y) > epsilion || + abs(centerOfMass.z - massProp2.getCenterOfMass().z) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Center of mass is incorrect : Expected = " << centerOfMass.x << " " << + centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp2.getCenterOfMass().x << " " << + massProp2.getCenterOfMass().y << " " << massProp2.getCenterOfMass().z << std::endl; + } + + if (abs(volume - volumeAndInertia2.at(0)) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Volume is incorrect : Expected = " << volume << + ", actual = " << volumeAndInertia2.at(0) << std::endl; + } + + if (abs(inertia - (volumeAndInertia2.at(1))) > epsilion || abs(inertia - (volumeAndInertia2.at(2))) > epsilion || + abs(inertia - (volumeAndInertia2.at(3))) > epsilion){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR : Moment is incorrect : Expected = " << inertia << " " << + inertia << " " << inertia << ", actual = " << (volumeAndInertia2.at(1)) << " " << (volumeAndInertia2.at(2)) << + " " << (volumeAndInertia2.at(3)) << std::endl; + } +} void MassPropertiesTests::runAllTests(){ testWithTetrahedron(); testWithUnitCube(); + testWithCube(); } \ No newline at end of file diff --git a/tests/physics/src/MassPropertiesTests.h b/tests/physics/src/MassPropertiesTests.h index 583ac25126..21ab3ccb0a 100644 --- a/tests/physics/src/MassPropertiesTests.h +++ b/tests/physics/src/MassPropertiesTests.h @@ -15,6 +15,7 @@ namespace MassPropertiesTests{ void testWithTetrahedron(); void testWithUnitCube(); + void testWithCube(); void runAllTests(); } #endif // hifi_MassPropertiesTests_h \ No newline at end of file diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index eed58dbbdb..d42b5d1241 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -8,7 +8,6 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#include #include "ShapeColliderTests.h" #include "VerletShapeTests.h" #include "ShapeInfoTests.h" @@ -17,12 +16,11 @@ #include "MassPropertiesTests.h" int main(int argc, char** argv) { - //ShapeColliderTests::runAllTests(); - //VerletShapeTests::runAllTests(); - //ShapeInfoTests::runAllTests(); - //ShapeManagerTests::runAllTests(); - // BulletUtilTests::runAllTests(); - MassPropertiesTests::runAllTests(); - getch(); + ShapeColliderTests::runAllTests(); + VerletShapeTests::runAllTests(); + ShapeInfoTests::runAllTests(); + ShapeManagerTests::runAllTests(); + BulletUtilTests::runAllTests(); + MassPropertiesTests::runAllTests(); return 0; }