Parallel axis theorem correction

libraries/physics/src/MassProperties.cpp

@@ -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
+//origin is the default reference point for generating the tetrahedron from each triangle of the mesh. We can provide 
-//point by passing it as 3rd parameter to the constructor
+//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());
+        glm::mat3 identity;
-        inertia_a += tetMassProperties.at(1) + (dist * dist * tetMassProperties.at(0));
+        glm::vec3 diff = _centerOfMass - tet.getCentroid();
-        inertia_b += tetMassProperties.at(2) + (dist * dist * tetMassProperties.at(0));
+        float diffDot = glm::dot(diff, diff);
-        inertia_c += tetMassProperties.at(3) + (dist * dist * tetMassProperties.at(0));
+        glm::mat3 outerDiff = glm::outerProduct(diff, diff);
-        inertia_aa += tetMassProperties.at(4) + (dist * dist * tetMassProperties.at(0));
+
-        inertia_bb += tetMassProperties.at(5) + (dist * dist * tetMassProperties.at(0));
+        //3x3 of local inertia tensors of each tetrahedron. Inertia tensors are the diagonal elements
-        inertia_cc += tetMassProperties.at(6) + (dist * dist * tetMassProperties.at(0));
+        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(globalInertia[0][0]);
-    volumeAndInertia.push_back(inertia_b);
+    volumeAndInertia.push_back(globalInertia[1][1]);
-    volumeAndInertia.push_back(inertia_c);
+    volumeAndInertia.push_back(globalInertia[2][2]);
-    volumeAndInertia.push_back(inertia_aa);
+    volumeAndInertia.push_back(globalProductInertia[0][0]);
-    volumeAndInertia.push_back(inertia_bb);
+    volumeAndInertia.push_back(globalProductInertia[1][1]);
-    volumeAndInertia.push_back(inertia_cc);
+    volumeAndInertia.push_back(globalProductInertia[2][2]);
     return volumeAndInertia;
 }

tests/physics/src/MassPropertiesTests.cpp

@@ -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,9 +101,9 @@ void MassPropertiesTests::testWithUnitCube(){
     vertices.push_back(p5);
     vertices.push_back(p6);
     vertices.push_back(p7);
-    std::cout << std::setprecision(5);
+    std::cout << std::setprecision(10);
-    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,
+    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, 
-        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 };
+        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;
@@ -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 <<
+            centerOfMass.y << " " << centerOfMass.z << ", actual = " << massProp1.getCenterOfMass().x << " " << 
-            " " << massProp1.getCenterOfMass().z << std::endl;
+            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();
 }

tests/physics/src/MassPropertiesTests.h

@@ -15,6 +15,7 @@
 namespace MassPropertiesTests{
     void testWithTetrahedron();
     void testWithUnitCube();
+    void testWithCube();
     void runAllTests();
 }
 #endif // hifi_MassPropertiesTests_h

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 <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();
+    ShapeColliderTests::runAllTests();
-    //VerletShapeTests::runAllTests();
+    VerletShapeTests::runAllTests();
-    //ShapeInfoTests::runAllTests();
+    ShapeInfoTests::runAllTests();
-    //ShapeManagerTests::runAllTests();
+    ShapeManagerTests::runAllTests();
-   // BulletUtilTests::runAllTests();
+    BulletUtilTests::runAllTests();
     MassPropertiesTests::runAllTests();
-    getch();
     return 0;
 }