Refactor Gram-Schmidt

src/chrono/core/ChMatrix33.h

@@ -390,7 +390,7 @@ inline void ChMatrix33<Real>::SetFromAxisX(const ChVector3<Real>& x_dir, const C
     ChVector3<Real> mX;
     ChVector3<Real> mY;
     ChVector3<Real> mZ;
-    x_dir.GetDirectionAxesAsX(mX, mY, mZ, y_sugg);
+    x_dir.GetDirectionAxes(mX, mY, mZ, y_sugg);
     this->SetFromDirectionAxes(mX, mY, mZ);
 }
 
@@ -399,7 +399,7 @@ inline void ChMatrix33<Real>::SetFromAxisY(const ChVector3<Real>& y_dir, const C
     ChVector3<Real> mX;
     ChVector3<Real> mY;
     ChVector3<Real> mZ;
-    y_dir.GetDirectionAxesAsY(mX, mY, mZ, z_sugg);
+    y_dir.GetDirectionAxes(mY, mZ, mX, z_sugg);
     this->SetFromDirectionAxes(mX, mY, mZ);
 }
 
@@ -408,7 +408,7 @@ inline void ChMatrix33<Real>::SetFromAxisZ(const ChVector3<Real>& z_dir, const C
     ChVector3<Real> mX;
     ChVector3<Real> mY;
     ChVector3<Real> mZ;
-    z_dir.GetDirectionAxesAsZ(mX, mY, mZ, x_sugg);
+    z_dir.GetDirectionAxes(mZ, mX, mY, x_sugg);
     this->SetFromDirectionAxes(mX, mY, mZ);
 }
 

src/chrono/core/ChVector3.h

@@ -23,6 +23,7 @@
 #include "chrono/core/ChMatrix.h"
 #include "chrono/serialization/ChArchive.h"
 #include "chrono/serialization/ChOutputASCII.h"
+#include "chrono/utils/ChConstants.h"
 
 namespace chrono {
 
@@ -247,6 +248,15 @@ class ChVector3 {
                              ChVector3<Real>& Vz,
                              ChVector3<Real> x_sugg = ChVector3<Real>(1, 0, 0)) const;
 
+    /// Output three orthonormal vectors considering this vector defining the first axis.
+    /// Optionally, the \a V2_sugg vector can be used to suggest the second axis.
+    /// It is recommended to set \a V2_sugg to be not parallel to this vector.
+    /// Rely on Gram-Schmidt orthonormalization.
+    void GetDirectionAxes(ChVector3<Real>& V1,
+                          ChVector3<Real>& V2,
+                          ChVector3<Real>& V3,
+                          ChVector3<Real> V2_sugg = ChVector3<Real>(0, 1, 0)) const;
+
     /// Return the index of the largest component in absolute value.
     int GetMaxComponent() const;
 
@@ -931,7 +941,7 @@ inline void ChVector3<Real>::GetDirectionAxesAsY(ChVector3<Real>& Vx,
         }
     }
 
-    Vy.Cross(Vz, Vx);
+    Vz.Cross(Vx, Vy);
 }
 
 template <class Real>
@@ -960,6 +970,44 @@ inline void ChVector3<Real>::GetDirectionAxesAsZ(ChVector3<Real>& Vx,
     Vx.Cross(Vy, Vz);
 }
 
+template <class Real>
+inline void ChVector3<Real>::GetDirectionAxes(ChVector3<Real>& V1,
+                                              ChVector3<Real>& V2,
+                                              ChVector3<Real>& V3,
+                                              ChVector3<Real> V2_sugg) const {
+    V1 = *this;
+    if (!V1.Normalize()) { // JBC: If zero is passed, I would argue this should be an error! Not a warning or a change to some arbitrary vector!
+        std::cerr << "Fatal: " << __FUNCTION__ << "first vector V1="<<V1<<" has zero length."<<std::endl;
+        throw std::runtime_error("Cannot perform Gram-Schmidt");
+    }
+
+    V3.Cross(V1, V2_sugg);
+    if (V3.Normalize()) {
+        // TODO JBC: Normalize() tests length against the numeric limits.
+        // Very small numbers can pass this test and cause very large errors and loss or orthogonality.
+        // Consider testing against such number here, e.g., V3.length2() <= 1e-10 * V2_sugg.length2(), rather than against numeric limit inside Normalize()
+        // Current implementation seems slightly less prone to this issue than implementation attempt using V2 = V2_sugg - V2_sugg.Dot(V1) * V1, and checking V2.Normalize()
+        V2.Cross(V3, V1);
+    } else {
+        std::cerr << "Warning: suggested second  vector V2_sugg=" << V2_sugg << " is either zero, or collinear to first vector V1=" << V1 << std::endl;
+        // Find one (among infinitely many) vector normal to V1 = (a, b, c), e.g., (0, -c, b), (c, 0, -a), (-b, a, 0)
+        // If two components are zero, e.g., V1 = (1, 0, 0), heuristics above may give a zero-vector.
+        // V1 normalized so one component >= sqrt(1/3): find it and select above heuristic that ensures non-zero normal vector.
+        double pad = 0.95; // Padding safety for rounding errors
+        for (int i = 0; i < 3; i++) {
+            if (std::abs(V1[i]) >= pad * CH_SQRT_1_3) {
+                V2[(i + 1) % 3] = 0.0;
+                V2[(i + 2) % 3] = V1[i];
+                V2[i] = -V1[(i + 2) % 3];
+                break;
+            }
+        }
+        V2.Normalize();
+        std::cerr << "Second vector modified to V2=" << V2 << " in " << __FUNCTION__ << std::endl;
+        V3.Cross(V1, V2);
+    }
+}
+
 template <class Real>
 inline int ChVector3<Real>::GetMaxComponent() const {
     int idx = 0;

src/chrono/geometry/ChLineSegment.cpp

@@ -27,7 +27,7 @@ ChLineSegment::ChLineSegment(const ChLineSegment& source) : ChLine(source) {
 ChFrame<> ChLineSegment::GetFrame() const {
     ChVector3d dir = (pB - pA).GetNormalized();
     ChVector3d u, v, w;
-    dir.GetDirectionAxesAsX(w, u, v);
+    dir.GetDirectionAxes(w, u, v);
 
     return ChFrame<>(0.5 * (pB + pA), ChMatrix33<>(u, v, w));
 }

src/chrono/physics/ChLinkMate.cpp

@@ -1286,7 +1286,7 @@ void ChLinkMateRackPinion::Update(double time, bool update_assets) {
     ChVector3d abs_Dx;
     ChVector3d abs_Dy;
     ChVector3d abs_Dz;
-    abs_Dpin.GetDirectionAxesAsX(
+    abs_Dpin.GetDirectionAxes(
         abs_Dz, abs_Dx, abs_Dy,
         abs_rack.GetRotMat().GetAxisX());  // with z as pinion shaft and x as suggested rack X dir
 

src/chrono/utils/ChConstants.h

@@ -26,6 +26,9 @@ static constexpr double CH_RPM_TO_RAD_S = CH_2PI / 60.0;
 static constexpr double CH_RAD_S_TO_RPM = 60.0 / CH_2PI;
 
 static constexpr double CH_SQRT_2 = 1.41421356237309504880;
+static constexpr double CH_SQRT_1_2 = 0.70710678118654757274;
+static constexpr double CH_SQRT_3 = 1.73205080756887729353;
+static constexpr double CH_SQRT_1_3 = 0.57735026918962573106;
 
 static constexpr double CH_1_3 = 1.0 / 3.0;
 static constexpr double CH_1_6 = 1.0 / 6.0;

src/chrono_irrlicht/ChIrrTools.cpp

@@ -1094,7 +1094,7 @@ void drawArrow(ChVisualSystemIrrlicht* vis,
     drawSegment(vis, start, end, col, use_Zbuffer);  // main segment
     ChVector3d dir = (end - start).GetNormalized();
     ChVector3d u, v, w;
-    dir.GetDirectionAxesAsX(u, v, w, plane_normal);
+    dir.GetDirectionAxes(u, v, w, plane_normal);
     ChVector3d p1, p2;
     if (!sharp) {
         p1 = end + 0.25 * (w - dir);

src/demos/mbs/demo_MBS_link_mate.cpp

@@ -192,7 +192,7 @@ void test_pendulum() {
     ChVector3d mX;
     ChVector3d mY;
     ChVector3d mZ;
-    Xdir.GetDirectionAxesAsX(mX, mY, mZ, Ydir);
+    Xdir.GetDirectionAxes(mX, mY, mZ, Ydir);
     ChQuaternion<> mass_rot = ChMatrix33<>(mX, mY, mZ).GetQuaternion();
     my_mass->SetCoordsys(mass_pos, mass_rot);
     my_mass->SetMass(tip_mass);
@@ -402,7 +402,7 @@ void test_anchorchain() {
         ChVector3d mX;
         ChVector3d mY;
         ChVector3d mZ;
-        Xdir.GetDirectionAxesAsX(mX, mY, mZ, Ydir);
+        Xdir.GetDirectionAxes(mX, mY, mZ, Ydir);
         ChQuaternion<> knot_rot = ChMatrix33<>(mX, mY, mZ).GetQuaternion();
 
         for (int i_body = 0; i_body < mN; i_body++) {

src/tests/unit_tests/core/utest_CH_ChVector.cpp

@@ -19,6 +19,8 @@
 #include "gtest/gtest.h"
 #include "chrono/core/ChVector3.h"
 
+#include <random>
+
 using namespace chrono;
 
 const double ABS_ERR_D = 1e-15;
@@ -93,3 +95,151 @@ TEST(ChVectorTest, cross) {
     auto pf = af.GetOrthogonalVector();
     ASSERT_NEAR(af.Cross(pf).Length(), af.Length() * pf.Length(), ABS_ERR_F);
 }
+
+TEST(ChVectorTest, gram_schmidt) {
+    // Temporary verification test
+
+    // Generate Random vectors
+    long loops = 1000;
+    double tol = 1e-10; // Numerical errors of complex process needs larger tolerance than ABS_ERR_D
+    std::random_device rd;
+    std::default_random_engine re(rd());
+    std::uniform_real_distribution<double> dist(-10.0, 10.0);
+    // Ensures results from ChVector3::GetDirectionAxesAsX, ChVector3::GetDirectionAxesAsY, ChVector3::GetDirectionAxesAsZ
+    ChVector3<> Vx_old;
+    ChVector3<> Vy_old;
+    ChVector3<> Vz_old;
+    // Are the same as refactored ChVector3::GetDirectionAxes
+    ChVector3<> Vx_new;
+    ChVector3<> Vy_new;
+    ChVector3<> Vz_new;
+
+
+
+
+    //////////////////////////////
+    // 1. ---- x_dir.GetDirectionAxesAsX(Vx, Vy, Vz, y_sugg) == xdir.GetDirectionAxes(Vx, Vy, Vz, y_sugg)
+    //////////////////////////////
+    ChVector3<> x_dir;
+    ChVector3<> y_sugg;
+
+    // 1.1 -- Corner cases:
+    x_dir.Set(0.0, -1.5, 0.0);
+
+    // Second vector is zero
+    y_sugg.SetNull();
+    x_dir.GetDirectionAxesAsX(Vx_old, Vy_old, Vz_old, y_sugg);
+    x_dir.GetDirectionAxes(Vx_new, Vy_new, Vz_new, y_sugg);
+    std::cout<<"New heuristic expected to have different behavior than old one so no formal test"<<std::endl;
+    std::cout<<"This is okay: no simulation should rely on one given arbitrary heuristic rollback of the bad input to function correctly"<<std::endl;
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;
+
+    // Second vector is collinear to first vector
+    y_sugg = x_dir * 3.0;
+    x_dir.GetDirectionAxesAsX(Vx_old, Vy_old, Vz_old, y_sugg);
+    x_dir.GetDirectionAxes(Vx_new, Vy_new, Vz_new, y_sugg);
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;
+
+
+    // 1.2 -- Good (random) input
+    for (long i =0 ; i < loops ; i++) {
+        x_dir.Set(dist(re), dist(re), dist(re));
+        y_sugg.Set(dist(re), dist(re), dist(re));
+
+        x_dir.GetDirectionAxesAsX(Vx_old, Vy_old, Vz_old, y_sugg);
+        x_dir.GetDirectionAxes(Vx_new, Vy_new, Vz_new, y_sugg);
+
+        for (int i = 0; i < 3; i++) {
+            ASSERT_NEAR(Vx_old[i], Vx_new[i], tol);
+            ASSERT_NEAR(Vy_old[i], Vy_new[i], tol);
+            ASSERT_NEAR(Vz_old[i], Vz_new[i], tol);
+        }
+    }
+
+    //////////////////////////////
+    // 2. ---- ydir.GetDirectionAxesAsY(Vx, Vy, Vz, x_sugg) == ydir.GetDirectionAxes(Vy, Vz, Vx, z_sugg)
+    //////////////////////////////
+    ChVector3<> y_dir;
+    ChVector3<> z_sugg;
+
+    // 2.1 -- Corner cases:
+    y_dir.Set(1.2, -1.5, 0.0);
+
+    // Second vector is zero
+    z_sugg.SetNull();
+    y_dir.GetDirectionAxesAsY(Vx_old, Vy_old, Vz_old, z_sugg);
+    y_dir.GetDirectionAxes(Vy_new, Vz_new, Vx_new, z_sugg);
+    std::cout<<"New heuristic expected to have different behavior than old one so no formal test"<<std::endl;
+    std::cout<<"This is okay: no simulation should rely on one given arbitrary heuristic rollback of the bad input to function correctly"<<std::endl;
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;
+
+    // Second vector is collinear to first vector
+    z_sugg = y_dir * 16.78;
+    y_dir.GetDirectionAxesAsY(Vx_old, Vy_old, Vz_old, z_sugg);
+    y_dir.GetDirectionAxes(Vy_new, Vz_new, Vx_new, z_sugg);
+    std::cout<<"UTEST WARNING: THIS SHOULD TRIGGER A COLLINEAR WARNING"<<std::endl;
+    std::cout<<"THE FACT THAT IT DOES NOT SUGGESTS THE NUMERIC LIMITS IN Normalize() MIGHT BE TOO SMALL (see TODO JBC)"<<std::endl;
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;    
+
+    // 2.2 -- Good (random) input
+    for (long i =0 ; i < loops ; i++) {
+        y_dir.Set(dist(re), dist(re), dist(re));
+        z_sugg.Set(dist(re), dist(re), dist(re));
+
+        y_dir.GetDirectionAxesAsY(Vx_old, Vy_old, Vz_old, z_sugg);
+        y_dir.GetDirectionAxes(Vy_new, Vz_new, Vx_new, z_sugg);
+
+        for (int i = 0; i < 3; i++) {            
+            ASSERT_NEAR(Vx_old[i], Vx_new[i], tol);
+            ASSERT_NEAR(Vy_old[i], Vy_new[i], tol);
+            ASSERT_NEAR(Vz_old[i], Vz_new[i], tol);
+        }
+    }
+
+    //////////////////////////////
+    // 3. ---- zdir.GetDirectionAxesAsZ(Vx, Vy, Vz, x_sugg) == zdir.GetDirectionAxes(Vz, Vx, Vy, x_sugg)
+    //////////////////////////////
+    ChVector3<> z_dir;
+    ChVector3<> x_sugg;
+
+    // 3.1 -- Corner cases:
+    z_dir.Set(1.2, -1.5, 13.157);
+
+    // Second vector is zero
+    x_sugg.SetNull();
+    z_dir.GetDirectionAxesAsZ(Vx_old, Vy_old, Vz_old, x_sugg);
+    z_dir.GetDirectionAxes(Vz_new, Vx_new, Vy_new, x_sugg);
+    std::cout<<"New heuristic expected to have different behavior than old one so no formal test"<<std::endl;
+    std::cout<<"This is okay: no simulation should rely on one given arbitrary heuristic rollback of the bad input to function correctly"<<std::endl;
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;
+
+    // Second vector is collinear to first vector
+    x_sugg = z_dir * (-6.59);
+    z_dir.GetDirectionAxesAsZ(Vx_old, Vy_old, Vz_old, x_sugg);
+    z_dir.GetDirectionAxes(Vz_new, Vx_new, Vy_new, x_sugg);
+    std::cout<<"UTEST WARNING: THIS SHOULD TRIGGER A COLLINEAR WARNING"<<std::endl;
+    std::cout<<"THE FACT THAT IT DOES NOT SUGGESTS THE NUMERIC LIMITS IN Normalize() MIGHT BE TOO SMALL (see TODO JBC)"<<std::endl;
+    std::cout<<Vx_old<<" | "<<Vy_old<<" | "<<Vz_old<<std::endl;
+    std::cout<<Vx_new<<" | "<<Vy_new<<" | "<<Vz_new<<std::endl<<std::endl;
+
+
+    // 2.2 -- Good (random) input
+    for (long i =0 ; i < loops ; i++) {
+        z_dir.Set(dist(re), dist(re), dist(re));
+        x_sugg.Set(dist(re), dist(re), dist(re));
+
+        z_dir.GetDirectionAxesAsZ(Vx_old, Vy_old, Vz_old, x_sugg);
+        z_dir.GetDirectionAxes(Vz_new, Vx_new, Vy_new, x_sugg);
+
+        for (int i = 0; i < 3; i++) {
+            ASSERT_NEAR(Vx_old[i], Vx_new[i], tol);
+            ASSERT_NEAR(Vy_old[i], Vy_new[i], tol);
+            ASSERT_NEAR(Vz_old[i], Vz_new[i], tol);
+        }
+    }
+}