diff --git a/src/chrono/core/ChMatrix33.h b/src/chrono/core/ChMatrix33.h index 6b9ccc547f..a1f18861de 100644 --- a/src/chrono/core/ChMatrix33.h +++ b/src/chrono/core/ChMatrix33.h @@ -390,7 +390,7 @@ inline void ChMatrix33::SetFromAxisX(const ChVector3& x_dir, const C ChVector3 mX; ChVector3 mY; ChVector3 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::SetFromAxisY(const ChVector3& y_dir, const C ChVector3 mX; ChVector3 mY; ChVector3 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::SetFromAxisZ(const ChVector3& z_dir, const C ChVector3 mX; ChVector3 mY; ChVector3 mZ; - z_dir.GetDirectionAxesAsZ(mX, mY, mZ, x_sugg); + z_dir.GetDirectionAxes(mZ, mX, mY, x_sugg); this->SetFromDirectionAxes(mX, mY, mZ); } diff --git a/src/chrono/core/ChVector3.h b/src/chrono/core/ChVector3.h index 80791145d4..90a3736530 100644 --- a/src/chrono/core/ChVector3.h +++ b/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& Vz, ChVector3 x_sugg = ChVector3(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& V1, + ChVector3& V2, + ChVector3& V3, + ChVector3 V2_sugg = ChVector3(0, 1, 0)) const; + /// Return the index of the largest component in absolute value. int GetMaxComponent() const; @@ -931,7 +941,7 @@ inline void ChVector3::GetDirectionAxesAsY(ChVector3& Vx, } } - Vy.Cross(Vz, Vx); + Vz.Cross(Vx, Vy); } template @@ -960,6 +970,44 @@ inline void ChVector3::GetDirectionAxesAsZ(ChVector3& Vx, Vx.Cross(Vy, Vz); } +template +inline void ChVector3::GetDirectionAxes(ChVector3& V1, + ChVector3& V2, + ChVector3& V3, + ChVector3 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="< inline int ChVector3::GetMaxComponent() const { int idx = 0; diff --git a/src/chrono/geometry/ChLineSegment.cpp b/src/chrono/geometry/ChLineSegment.cpp index 6c64f3a185..0250202855 100644 --- a/src/chrono/geometry/ChLineSegment.cpp +++ b/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)); } diff --git a/src/chrono/physics/ChLinkMate.cpp b/src/chrono/physics/ChLinkMate.cpp index 4650123c72..f8f4b5ff48 100644 --- a/src/chrono/physics/ChLinkMate.cpp +++ b/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 diff --git a/src/chrono/utils/ChConstants.h b/src/chrono/utils/ChConstants.h index 390d0fad5e..ddea82ab0d 100644 --- a/src/chrono/utils/ChConstants.h +++ b/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; diff --git a/src/chrono_irrlicht/ChIrrTools.cpp b/src/chrono_irrlicht/ChIrrTools.cpp index 14b54347b6..dec9d17eda 100644 --- a/src/chrono_irrlicht/ChIrrTools.cpp +++ b/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); diff --git a/src/demos/mbs/demo_MBS_link_mate.cpp b/src/demos/mbs/demo_MBS_link_mate.cpp index 207182c017..1db84c32e2 100644 --- a/src/demos/mbs/demo_MBS_link_mate.cpp +++ b/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++) { diff --git a/src/tests/unit_tests/core/utest_CH_ChVector.cpp b/src/tests/unit_tests/core/utest_CH_ChVector.cpp index 8c312a6f6d..3d984d475b 100644 --- a/src/tests/unit_tests/core/utest_CH_ChVector.cpp +++ b/src/tests/unit_tests/core/utest_CH_ChVector.cpp @@ -19,6 +19,8 @@ #include "gtest/gtest.h" #include "chrono/core/ChVector3.h" +#include + 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 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"< 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"< 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"<