move logic to common phase_mod_2pi functionality

Signed-off-by: Martijn Govers <Martijn.Govers@Alliander.com>

Author: mgovers

power_grid_model_c/power_grid_model/include/power_grid_model/common/three_phase_tensor.hpp

@@ -176,6 +176,14 @@ inline ComplexValue<asymmetric_t> phase_shift(ComplexValue<asymmetric_t> const&
     return {phase_shift(m(0)), phase_shift(m(1)), phase_shift(m(2))};
 }
 
+// arg(e^(i * phase)) = phase (mod 2pi). By convention restrict to [-pi, pi].
+inline auto phase_mod_2pi(double phase) {
+    return RealValue<symmetric_t>{arg(ComplexValue<symmetric_t>{exp(1.0i * phase)})};
+}
+inline auto phase_mod_2pi(RealValue<asymmetric_t> const& phase) {
+    return RealValue<asymmetric_t>{arg(ComplexValue<asymmetric_t>{exp(1.0i * phase)})};
+}
+
 // calculate kron product of two vector
 inline double vector_outer_product(double x, double y) { return x * y; }
 inline DoubleComplex vector_outer_product(DoubleComplex x, DoubleComplex y) { return x * y; }

power_grid_model_c/power_grid_model/include/power_grid_model/component/current_sensor.hpp

@@ -174,9 +174,7 @@ template <symmetry_tag current_sensor_symmetry_> class CurrentSensor : public Ge
             }
         }();
         output.i_residual = (cabs(i_measured_complex) - cabs(i_output)) * base_current_;
-        // arg(e^i * u_angle) = u_angle in (-pi, pi]
-        auto const unbounded_i_angle_residual = arg(i_measured_complex) - arg(i_output);
-        output.i_angle_residual = arg(ComplexValue<sym_calc>{exp(1.0i * unbounded_i_angle_residual)});
+        output.i_angle_residual = phase_mod_2pi(arg(i_measured_complex) - arg(i_output));
         return output;
     }
 };

power_grid_model_c/power_grid_model/include/power_grid_model/component/voltage_sensor.hpp

@@ -151,7 +151,7 @@ template <symmetry_tag sym> class VoltageSensor : public GenericVoltageSensor {
         } else {
             value.u_residual = (real(u1_measured) - cabs(u)) * u_rated_;
         }
-        value.u_angle_residual = arg(ComplexValue<symmetric_t>{exp(1.0i * (arg(u1_measured) - arg(u)))});
+        value.u_angle_residual = phase_mod_2pi(arg(u1_measured) - arg(u));
         return value;
     }
 
@@ -160,7 +160,7 @@ template <symmetry_tag sym> class VoltageSensor : public GenericVoltageSensor {
         value.id = id();
         value.energized = 1;
         value.u_residual = (u_measured_ - cabs(u)) * u_rated_ / sqrt3;
-        value.u_angle_residual = arg(ComplexValue<asymmetric_t>{exp(1.0i * (u_angle_measured_ - arg(u)))});
+        value.u_angle_residual = phase_mod_2pi(u_angle_measured_ - arg(u));
         return value;
     }
 };

tests/cpp_unit_tests/test_three_phase_tensor.cpp

@@ -206,6 +206,38 @@ TEST_CASE("Three phase tensor") {
         CHECK(is_nan(vec(1)));
         CHECK(vec(2) == 6.0);
     }
+    SUBCASE("phase_mod_2pi") {
+        auto const check = [](double value) {
+            CAPTURE(value);
+            CHECK_GE(value, -pi);
+            CHECK_LE(value, pi);
+            if (value != pi && value != -pi) {
+                CHECK(phase_mod_2pi(value) == doctest::Approx(value));
+            }
+        };
+        auto const check_asym = [&check](RealValue<asymmetric_t> const& value) {
+            for (Idx i : {0, 1, 2}) {
+                CAPTURE(i);
+                check(value(i));
+            }
+        };
+        check(phase_mod_2pi(0.0));
+        check(phase_mod_2pi(2.0 * pi));
+        check(phase_mod_2pi(2.0 * pi + 1.0));
+        check(phase_mod_2pi(-1.0));
+        check(phase_mod_2pi(-pi));
+        check(phase_mod_2pi(pi));
+        check(phase_mod_2pi(-3.0 * pi));
+        check(phase_mod_2pi(3.0 * pi));
+        check(phase_mod_2pi(pi * (1.0 + std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(pi * (1.0 - std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(-pi * (1.0 + std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(-pi * (1.0 - std::numeric_limits<double>::epsilon())));
+
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+    }
 }
 
 } // namespace power_grid_model