Redesign Vec to have correct constraints and typecasting

ComputationalBiomechanicsLab · Jul 18, 2024 · 5cd658d · 5cd658d
1 parent 9f3f382
commit 5cd658d
Show file tree

Hide file tree

Showing 21 changed files with 443 additions and 279 deletions.
diff --git a/src/OpenSimCreator/UI/Experimental/TPS2DTab.cpp b/src/OpenSimCreator/UI/Experimental/TPS2DTab.cpp
@@ -292,7 +292,7 @@ namespace
     Mesh ApplyThinPlateWarpToMesh(const ThinPlateWarper2D& t, const Mesh& mesh)
     {
         Mesh rv = mesh;
-        rv.transform_vertices([&t](Vec3 v) { return Vec3{t.transform(v), v.z}; });
+        rv.transform_vertices([&t](Vec3 v) { return Vec3{t.transform(Vec2{v}), v.z}; });
         return rv;
     }
 }

diff --git a/src/OpenSimCreator/UI/MeshImporter/MeshImporterTab.cpp b/src/OpenSimCreator/UI/MeshImporter/MeshImporterTab.cpp
@@ -2128,7 +2128,7 @@ class osc::mi::MeshImporterTab::Impl final : public IMeshImporterUILayerHost {
             MIObject& el = m_Shared->updModelGraph().updByID(id);
             switch (m_Gizmo.operation()) {
             case ui::GizmoOperation::Rotate:
-                el.applyRotation(m_Shared->getModelGraph(), userManipulation->rotation, m_GizmoModelMatrix[3]);
+                el.applyRotation(m_Shared->getModelGraph(), userManipulation->rotation, Vec3{m_GizmoModelMatrix[3]});
                 break;
             case ui::GizmoOperation::Translate: {
                 el.applyTranslation(m_Shared->getModelGraph(), userManipulation->position);

diff --git a/src/OpenSimCreator/UI/Shared/ModelSelectionGizmo.cpp b/src/OpenSimCreator/UI/Shared/ModelSelectionGizmo.cpp
@@ -348,7 +348,7 @@ namespace
                     getUndoableModel(),
                     pof,
                     ToVec3(X.p()),
-                    ToVec3(X.R().convertRotationToBodyFixedXYZ())
+                    ToEulerAngles(X.R())
                 );
             }
             else {
@@ -371,7 +371,7 @@ namespace
                     getUndoableModel(),
                     pof,
                     ToVec3(X.p()),
-                    ToVec3(X.R().convertRotationToBodyFixedXYZ())
+                    ToEulerAngles(X.R())
                 );
             }
         }
@@ -425,7 +425,7 @@ namespace
                 getUndoableModel(),
                 wrapObj,
                 ToVec3(X.p() - M_w.p()),
-                ToVec3(X.R().convertRotationToBodyFixedXYZ())
+                ToEulerAngles(X.R())
             );
         }
     };
@@ -476,7 +476,7 @@ namespace
                 getUndoableModel(),
                 contactGeom,
                 ToVec3(X.p() - M_w.p()),
-                ToVec3(X.R().convertRotationToBodyFixedXYZ())
+                ToEulerAngles(X.R())
             );
         }
     };
@@ -577,7 +577,7 @@ namespace
                     getUndoableModel(),
                     parentPOF,
                     ToVec3(X.p()),
-                    ToVec3(X.R().convertRotationToBodyFixedXYZ())
+                    ToEulerAngles(X.R())
                 );
             }
 
@@ -595,7 +595,7 @@ namespace
                 getUndoableModel(),
                 childPOF,
                 ToVec3(M_cpof2.p()),
-                ToVec3(M_cpof2.R().convertRotationToBodyFixedXYZ())
+                ToEulerAngles(M_cpof2.R())
             );
         }
     };

diff --git a/src/OpenSimCreator/Utils/SimTKHelpers.cpp b/src/OpenSimCreator/Utils/SimTKHelpers.cpp
@@ -165,6 +165,11 @@ Quat osc::ToQuat(const SimTK::Rotation& r)
     };
 }
 
+EulerAngles osc::ToEulerAngles(const SimTK::Rotation& r)
+{
+    return EulerAngles{ToVec3(r.convertRotationToBodyFixedXYZ())};
+}
+
 std::array<float, 6> osc::ToArray(const SimTK::Vec6& v)
 {
     return {

diff --git a/src/OpenSimCreator/Utils/SimTKHelpers.h b/src/OpenSimCreator/Utils/SimTKHelpers.h
@@ -37,6 +37,7 @@ namespace osc
     Mat3 ToMat3(const SimTK::Mat33&);
     Mat4 mat4_cast(const SimTK::Rotation&);
     Quat ToQuat(const SimTK::Rotation&);
+    EulerAngles ToEulerAngles(const SimTK::Rotation&);
     std::array<float, 6> ToArray(const SimTK::Vec6&);
     Transform decompose_to_transform(const SimTK::Transform&);
 }
diff --git a/src/oscar/CMakeLists.txt b/src/oscar/CMakeLists.txt
@@ -226,6 +226,7 @@ add_library(oscar STATIC
     Maths/RayCollision.h
     Maths/Rect.h
     Maths/RectFunctions.h
+    Maths/Scalar.h
     Maths/Sphere.h
     Maths/Tetrahedron.h
     Maths/Transform.h

diff --git a/src/oscar/Graphics/Unorm8.h b/src/oscar/Graphics/Unorm8.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <oscar/Maths/Scalar.h>
+
 #include <cstddef>
 #include <cstdint>
 #include <compare>
@@ -72,4 +74,11 @@ namespace osc
 
         uint8_t value_;
     };
+
+    // tag `Unorm8` as scalar-like, so that other parts of the codebase (e.g.
+    // vectors, matrices) accept it
+    template<>
+    struct IsScalar<Unorm8> final {
+        static constexpr bool value = true;
+    };
 }
diff --git a/src/oscar/Maths.h b/src/oscar/Maths.h
@@ -42,6 +42,7 @@
 #include <oscar/Maths/RayCollision.h>
 #include <oscar/Maths/Rect.h>
 #include <oscar/Maths/RectFunctions.h>
+#include <oscar/Maths/Scalar.h>
 #include <oscar/Maths/Sphere.h>
 #include <oscar/Maths/Tetrahedron.h>
 #include <oscar/Maths/Transform.h>

diff --git a/src/oscar/Maths/Angle.h b/src/oscar/Maths/Angle.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <oscar/Maths/Scalar.h>
+
 #include <algorithm>
 #include <cmath>
 #include <concepts>
@@ -32,13 +34,13 @@ namespace osc
             value_{static_cast<Rep>(value_)}
         {}
 
-        // implicitly constructs from an angle expressed in other units
+        // implicitly constructs from an angle with the same representation, but expressed in other units
         template<AngularUnitTraits Units2>
         constexpr Angle(const Angle<Rep, Units2>& other) :
             value_{static_cast<Rep>(other.count() * (Units2::radians_per_rep/Units::radians_per_rep))}
         {}
 
-        // explicitly constructs from an angle expressed of type `Rep2` expressed in other units
+        // explicitly constructs from an angle with representation `Rep2`, expressed in other units
         template<std::convertible_to<Rep> Rep2, AngularUnitTraits Units2>
         explicit constexpr Angle(const Angle<Rep2, Units2>& other) :
             value_{static_cast<Rep>(other.count() * (Units2::radians_per_rep/Units::radians_per_rep))}
@@ -149,6 +151,13 @@ namespace osc
     {
         return o << angle.count() << ' ' << Units::unit_label;
     }
+
+    // tag `Unorm8` as scalar-like, so that other parts of the codebase (e.g.
+    // vectors, matrices) accept it
+    template<std::floating_point Rep, AngularUnitTraits Units>
+    struct IsScalar<Angle<Rep, Units>> final {
+        static constexpr bool value = true;
+    };
 }
 
 // a specialization of `std::common_type` for `osc::Angle`s
@@ -203,7 +212,7 @@ namespace osc
     namespace literals
     {
         constexpr Degrees operator""_deg(long double degrees) { return Degrees{degrees}; }
-        constexpr Degrees operator""_deg(unsigned long long int degrees) { return Degrees{degrees};}
+        constexpr Degrees operator""_deg(unsigned long long int degrees) { return Degrees{degrees}; }
     }
 
     // turns

diff --git a/src/oscar/Maths/EulerAngles.h b/src/oscar/Maths/EulerAngles.h
@@ -5,6 +5,11 @@
 
 namespace osc
 {
-    // a sequence of X -> Y -> Z euler angles
-    using EulerAngles = Vec<3, Radians>;
+    // in OSC, Euler angles represent an intrinsic 3D rotation about
+    // the X, then Y, then Z axes
+
+    template<typename Units>
+    using EulerAnglesIn = Vec<3, Units>;  // useful for writing `EulerAnglesIn<Degrees>(vec)`
+
+    using EulerAngles = EulerAnglesIn<Radians>;
 }
diff --git a/src/oscar/Maths/MathsImplementation.cpp b/src/oscar/Maths/MathsImplementation.cpp
@@ -1158,8 +1158,8 @@ AABB osc::bounding_aabb_of(const Sphere& sphere)
 Line osc::transform_line(const Line& line, const Mat4& mat)
 {
     Line rv{};
-    rv.direction = mat * Vec4{line.direction, 0.0f};
-    rv.origin = mat * Vec4{line.origin, 1.0f};
+    rv.direction = Vec3{mat * Vec4{line.direction, 0.0f}};
+    rv.origin = Vec3{mat * Vec4{line.origin, 1.0f}};
     return rv;
 }
 
@@ -1369,8 +1369,7 @@ Vec3 osc::transform_point(const Mat4& mat, const Vec3& point)
 
 Quat osc::to_worldspace_rotation_quat(const EulerAngles& eulers)
 {
-    static_assert(std::is_same_v<EulerAngles::value_type, Radians>);
-    return normalize(Quat{Vec3{eulers.x.count(), eulers.y.count(), eulers.z.count()}});
+    return normalize(Quat{eulers});
 }
 
 void osc::apply_worldspace_rotation(

diff --git a/src/oscar/Maths/Qua.h b/src/oscar/Maths/Qua.h
@@ -83,8 +83,9 @@ namespace osc
             *this = normalize(Qua<T>::wxyz(real_part, t.x, t.y, t.z));
         }
 
-        // constructs a `Qua` from euler angles (pitch, yaw, roll), in radians.
-        explicit Qua(const Vec<3, T>& euler_angles)
+        // constructs a `Qua` from Euler angles that are assumed to represent an
+        // intrinsic, step-by-step, rotation about X, Y, and then Z
+        explicit Qua(const EulerAngles& euler_angles)
         {
             Vec<3, T> c = cos(euler_angles * T(0.5));
             Vec<3, T> s = sin(euler_angles * T(0.5));
@@ -95,11 +96,6 @@ namespace osc
             this->z = c.x * c.y * s.z - s.x * s.y * c.z;
         }
 
-        // constructs a `Qua` from euler angles
-        explicit Qua(const EulerAngles& euler_angles) :
-            Qua(Vec3{euler_angles.x.count(), euler_angles.y.count(), euler_angles.z.count()})
-        {}
-
         // constructs a `Qua` by decomposing an orthogonal matrix
         explicit Qua(const Mat<3, 3, T>& m)
         {

diff --git a/src/oscar/Maths/Scalar.h b/src/oscar/Maths/Scalar.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <concepts>
+
+namespace osc
+{
+    // metaclass that has a `value` member equal to `true` if its type argument
+    // behaves like a scalar (which is checked when resolving overloads of matrices
+    // and vectors)
+    template<typename>
+    struct IsScalar final {
+        static constexpr bool value = false;
+    };
+
+    template<std::floating_point T>
+    struct IsScalar<T> final {
+        static constexpr bool value = true;
+    };
+
+    template<std::integral T>
+    struct IsScalar<T> final {
+        static constexpr bool value = true;
+    };
+
+    template<typename T>
+    inline constexpr bool IsScalarV = IsScalar<T>::value;
+
+    template<typename T>
+    concept Scalar = IsScalarV<T>;
+
+    template<typename T>
+    concept ScalarOrBoolean = Scalar<T> || std::same_as<bool, T>;
+}
diff --git a/src/oscar/Maths/TrigonometricFunctions.h b/src/oscar/Maths/TrigonometricFunctions.h
@@ -27,6 +27,12 @@ namespace osc
         return map(v, sin<T>);
     }
 
+    template<size_t L, std::floating_point Rep, AngularUnitTraits Units>
+    Vec<L, Rep> sin(const Vec<L, Angle<Rep, Units>>& v)
+    {
+        return map(v, sin<Rep, Units>);
+    }
+
     template<std::floating_point GenType>
     GenType cos(GenType v)
     {
@@ -45,6 +51,12 @@ namespace osc
         return map(v, cos<T>);
     }
 
+    template<size_t L, std::floating_point Rep, AngularUnitTraits Units>
+    Vec<L, Rep> cos(const Vec<L, Angle<Rep, Units>>& v)
+    {
+        return map(v, cos<Rep, Units>);
+    }
+
     template<std::floating_point GenType>
     GenType tan(GenType v)
     {
@@ -60,7 +72,13 @@ namespace osc
     template<size_t L, std::floating_point T>
     Vec<L, T> tan(const Vec<L, T>& v)
     {
-        return elementwise_map(v, tan);
+        return map(v, tan<T>);
+    }
+
+    template<size_t L, std::floating_point Rep, AngularUnitTraits Units>
+    Vec<L, Rep> tan(const Vec<L, Angle<Rep, Units>>& v)
+    {
+        return map(v, tan<Rep, Units>);
     }
 
     template<std::floating_point Rep>

diff --git a/src/oscar/Maths/Vec.h b/src/oscar/Maths/Vec.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <oscar/Maths/Scalar.h>
 #include <oscar/Utils/HashHelpers.h>
 
 #include <cstddef>
@@ -13,10 +14,10 @@
 namespace osc
 {
     // specialized by `Vec2`, `Vec3`, etc.
-    template<size_t L, typename T>
+    template<size_t L, Scalar T>
     struct Vec;
 
-    template<size_t L, typename T>
+    template<size_t L, Scalar T>
     std::ostream& operator<<(std::ostream& out, const Vec<L, T>& vec)
     {
         out << "Vec" << L << '(';
@@ -29,7 +30,7 @@ namespace osc
         return out;
     }
 
-    template<size_t L, typename T>
+    template<size_t L, Scalar T>
     std::string to_string(const Vec<L, T>& vec)
     {
         std::stringstream ss;
@@ -39,30 +40,30 @@ namespace osc
 
     // when handled as a tuple-like object, a `Vec` decomposes into its elements
 
-    template<size_t I, size_t L, typename T>
+    template<size_t I, size_t L, Scalar T>
     constexpr const T& get(const Vec<L, T>& vec) { return vec[I]; }
 
-    template<size_t I, size_t L, typename T>
+    template<size_t I, size_t L, Scalar T>
     constexpr T& get(Vec<L, T>& vec) { return vec[I]; }
 
-    template<size_t I, size_t L, typename T>
+    template<size_t I, size_t L, Scalar T>
     constexpr T&& get(Vec<L, T>&& vec) { return std::move(vec[I]); }
 
-    template<size_t I, size_t L, typename T>
+    template<size_t I, size_t L, Scalar T>
     constexpr const T&& get(const Vec<L, T>&& vec) { return std::move(vec[I]); }
 }
 
-template<size_t L, typename T>
+template<size_t L, osc::Scalar T>
 struct std::tuple_size<osc::Vec<L, T>> {
     static inline constexpr size_t value = L;
 };
 
-template<size_t I, size_t L, typename T>
+template<size_t I, size_t L, osc::Scalar T>
 struct std::tuple_element<I, osc::Vec<L, T>> {
     using type = T;
 };
 
-template<size_t L, typename T>
+template<size_t L, osc::Scalar T>
 struct std::hash<osc::Vec<L, T>> final {
     size_t operator()(const osc::Vec<L, T>& vec) const
     {