Replaced math functions with cmath equivalents

navground_core/include/navground/core/common.h

@@ -233,7 +233,7 @@ struct NAVGROUND_CORE_EXPORT Twist2 {
   bool is_almost_zero(ng_float_t epsilon_speed = 1e-6,
                       ng_float_t epsilon_angular_speed = 1e-6) const {
     return velocity.norm() < epsilon_speed &&
-           abs(angular_speed) < epsilon_angular_speed;
+           std::abs(angular_speed) < epsilon_angular_speed;
   }
 
   /**
@@ -243,13 +243,13 @@ struct NAVGROUND_CORE_EXPORT Twist2 {
    * @param[in]  epsilon  The tolerance
    */
   void snap_to_zero(ng_float_t epsilon = 1e-6) {
-    if (abs(velocity[0]) < epsilon) {
+    if (std::abs(velocity[0]) < epsilon) {
       velocity[0] = 0;
     }
-    if (abs(velocity[1]) < epsilon) {
+    if (std::abs(velocity[1]) < epsilon) {
       velocity[1] = 0;
     }
-    if (abs(angular_speed) < epsilon) {
+    if (std::abs(angular_speed) < epsilon) {
       angular_speed = 0;
     }
   }
@@ -278,7 +278,7 @@ struct NAVGROUND_CORE_EXPORT Twist2 {
     if (acc.norm() > max_acceleration) {
       acc = acc.normalized() * max_acceleration;
     }
-    if (abs(ang_acc) > max_angular_acceleration) {
+    if (std::abs(ang_acc) > max_angular_acceleration) {
       ang_acc = std::clamp(ang_acc, -max_angular_acceleration,
                            max_angular_acceleration);
     }

navground_core/include/navground/core/controller_3d.h

@@ -480,15 +480,15 @@ class NAVGROUND_CORE_EXPORT Controller3 : public Controller {
       return t;
     }
     return std::max(
-        t, abs(altitude.target - altitude.value) / altitude.optimal_speed);
+        t, std::abs(altitude.target - altitude.value) / altitude.optimal_speed);
   }
 
   /**
    * @private
    */
   bool is_still() const override {
     return Controller::is_still() &&
-           (limit_to_2d || abs(altitude.speed) < speed_tolerance);
+           (limit_to_2d || std::abs(altitude.speed) < speed_tolerance);
   }
 
   /**

navground_core/include/navground/core/states/geometric.h

@@ -258,7 +258,7 @@ struct LineSegment {
     const ng_float_t x = delta.dot(e1);
     if (x < 0) return delta.norm();
     if (x > length) return (point - p2).norm();
-    return abs(delta.dot(e2));
+    return std::abs(delta.dot(e2));
   }
 
   // negative <=> penetration

navground_core/include/navground/core/target.h

@@ -80,7 +80,7 @@ struct Target {
 
   bool satisfied(Radians value) const {
     return !orientation ||
-           abs(normalize(*orientation - value)) < orientation_tolerance;
+           std::abs(normalize(*orientation - value)) < orientation_tolerance;
   }
 
   bool satisfied(const Vector2& value) const {

navground_core/src/behaviors/HL.cpp

@@ -7,6 +7,7 @@
 #include <assert.h>
 
 #include <algorithm>
+#include <cmath>
 #include <iostream>
 
 #include "navground/core/collision_computation.h"
@@ -87,11 +88,11 @@ std::valarray<ng_float_t> HLBehavior::get_collision_angles() const {
 static inline ng_float_t distance_from_target(Radians angle,
                                               ng_float_t free_distance,
                                               ng_float_t horizon) {
-  if (cos(angle) * horizon < free_distance) {
-    return fabs(sin(angle) * horizon);
+  if (std::cos(angle) * horizon < free_distance) {
+    return std::abs(std::sin(angle) * horizon);
   }
-  return sqrt(horizon * horizon + free_distance * free_distance -
-              2 * free_distance * horizon * cos(angle));
+  return std::sqrt(horizon * horizon + free_distance * free_distance -
+                   2 * free_distance * horizon * std::cos(angle));
 }
 
 // TODO(J:revision2023): check why we need effective_horizon
@@ -108,7 +109,7 @@ Vector2 HLBehavior::desired_velocity_towards_point(
   // effective_horizon = horizon;
   // effective_horizon=fmin(horizon,D);
   // Vector2 effectiveTarget = agentToTarget / D * effective_horizon;
-  const ng_float_t max_distance = effective_horizon;  // - radius;
+  const ng_float_t max_distance = effective_horizon;    // - radius;
   const Radians max_angle = static_cast<Radians>(1.6);  // Radians::PI_OVER_TW0;
   Radians angle = 0;
   // relative to target direction
@@ -118,15 +119,15 @@ Vector2 HLBehavior::desired_velocity_towards_point(
   bool found = false;
   while (angle < max_angle && !(out[0] == 2 && out[1] == 2)) {
     ng_float_t distance_from_target_lower_bound =
-        fabs(sin(angle) * max_distance);
+        std::abs(std::sin(angle) * max_distance);
     // distance_from_target_lower_bound=2*D*Sin(0.5*optimal_angle);
     if (optimal_distance_from_target <= distance_from_target_lower_bound) {
       // break;
     }
     for (int i = 0; i < 2; ++i) {
       const ng_float_t s_angle = i == 0 ? angle : -angle;
       const bool inside =
-          abs(normalize(relative_start_angle + s_angle)) < aperture;
+          std::abs(normalize(relative_start_angle + s_angle)) < aperture;
       if (out[i] == 1 && !inside) out[i] = 2;
       if (out[i] == 0 && inside) out[i] = 1;
       if (inside) {
@@ -149,7 +150,8 @@ Vector2 HLBehavior::desired_velocity_towards_point(
   if (!found) return Vector2::Zero();
   const ng_float_t static_distance = collision_computation.static_free_distance(
       start_angle + optimal_angle, max_distance);
-  const ng_float_t desired_speed = fmin(target_speed, static_distance / eta);
+  const ng_float_t desired_speed =
+      std::min(target_speed, static_distance / eta);
   return desired_speed * unit(start_angle + optimal_angle);
 }
 
@@ -217,8 +219,7 @@ Twist2 HLBehavior::relax(const Twist2 &current_value, const Twist2 &value,
 Twist2 HLBehavior::compute_cmd_internal(ng_float_t dt, Frame frame) {
   Twist2 cmd_twist = Behavior::compute_cmd_internal(dt, frame);
   if (tau > 0) {
-    cmd_twist =
-        to_frame(relax(actuated_twist, cmd_twist, dt), cmd_twist.frame);
+    cmd_twist = to_frame(relax(actuated_twist, cmd_twist, dt), cmd_twist.frame);
   }
   return cmd_twist;
 }

navground_core/src/kinematics.cpp

@@ -42,7 +42,7 @@ Twist2 TwoWheelsDifferentialDriveKinematics::feasible(
   assert(value.frame == Frame::relative);
   const ng_float_t w_max = get_max_angular_speed();
   const ng_float_t w = std::clamp(value.angular_speed, -w_max, w_max);
-  const ng_float_t v_max = get_max_speed() - abs(w) * get_axis() / 2;
+  const ng_float_t v_max = get_max_speed() - std::abs(w) * get_axis() / 2;
   const ng_float_t v = std::clamp<ng_float_t>(value.velocity[0], 0, v_max);
   // const ng_float_t v = std::clamp(value.velocity[0], -v_max, v_max);
   return Twist2{{v, 0}, w, Frame::relative};
@@ -78,10 +78,10 @@ WheelSpeeds TwoWheelsDifferentialDriveKinematics::feasible_wheel_speeds(
       std::clamp<ng_float_t>(twist.velocity[0], 0, max_speed);
   ng_float_t left = linear - rotation;
   ng_float_t right = linear + rotation;
-  if (abs(left) > max_speed) {
+  if (std::abs(left) > max_speed) {
     left = std::clamp(left, -max_speed, max_speed);
     right = left + 2 * rotation;
-  } else if (abs(right) > max_speed) {
+  } else if (std::abs(right) > max_speed) {
     right = std::clamp(right, -max_speed, max_speed);
     left = right - 2 * rotation;
   }
@@ -128,22 +128,22 @@ WheelSpeeds FourWheelsOmniDriveKinematics::feasible_wheel_speeds(
   ng_float_t front_right = longitudinal + lateral + rotation;
   ng_float_t rear_left = longitudinal + lateral - rotation;
   ng_float_t rear_right = longitudinal - lateral + rotation;
-  if (abs(front_left) > max_speed) {
+  if (std::abs(front_left) > max_speed) {
     front_left = std::clamp(front_left, -max_speed, max_speed);
     front_right = front_left + 2 * lateral + 2 * rotation;
     rear_left = front_left + 2 * lateral;
     rear_right = front_left + 2 * rotation;
-  } else if (abs(front_right) > max_speed) {
+  } else if (std::abs(front_right) > max_speed) {
     front_right = std::clamp(front_right, -max_speed, max_speed);
     front_left = front_right - 2 * lateral - 2 * rotation;
     rear_left = front_right - 2 * rotation;
     rear_right = front_right - 2 * lateral;
-  } else if (abs(rear_left) > max_speed) {
+  } else if (std::abs(rear_left) > max_speed) {
     rear_left = std::clamp(rear_left, -max_speed, max_speed);
     front_left = rear_left - 2 * lateral;
     front_right = rear_left + 2 * rotation;
     rear_right = rear_left - 2 * rotation + 2 * rotation;
-  } else if (abs(rear_right) > max_speed) {
+  } else if (std::abs(rear_right) > max_speed) {
     rear_right = std::clamp(rear_right, -max_speed, max_speed);
     front_left = rear_right - 2 * rotation;
     front_right = rear_right + 2 * lateral;
@@ -185,7 +185,7 @@ Twist2 DynamicTwoWheelsDifferentialDriveKinematics::feasible(
   const ng_float_t w =
       std::clamp(feasible_target.angular_speed, w0 - dw_max, w0 + dw_max);
   const ng_float_t dv_max = get_max_acceleration() * time_step -
-                            abs(w - w0) * get_axis() * get_moi() / 4;
+                            std::abs(w - w0) * get_axis() * get_moi() / 4;
   const ng_float_t v0 = current.velocity[0];
   const ng_float_t v =
       std::clamp(feasible_target.velocity[0], v0 - dv_max, v0 + dv_max);

navground_ros/src/ROSControllerNode.cpp

@@ -145,7 +145,7 @@ static float yaw_from(const geometry_msgs::msg::Quaternion &q) {
 
 static float yaw_from(const Transform &t) {
   const auto rpy = t.linear().eulerAngles(2, 1, 0);
-  if (abs(rpy[1]) > M_PI_2 && abs(rpy[2]) > M_PI_2) {
+  if (std::abs(rpy[1]) > M_PI_2 && std::abs(rpy[2]) > M_PI_2) {
     return rpy[0] + M_PI;
   }
   return rpy[0];

navground_sim/src/world.cpp

@@ -40,11 +40,11 @@ std::optional<Vector2> penetration_vector_inside_line(const LineSegment &line,
                                                       const Vector2 &center,
                                                       ng_float_t radius) {
   ng_float_t y = (center - line.p1).dot(line.e2);
-  if (abs(y) < radius) {
+  if (std::abs(y) < radius) {
     ng_float_t x = (center - line.p1).dot(line.e1);
     if (x < radius + 1e-3 || x > line.length - radius - 1e-3)
       return std::nullopt;
-    ng_float_t p = radius - abs(y);
+    ng_float_t p = radius - std::abs(y);
     if (y < 0) p *= -1;
     return p * line.e2;
   }
@@ -54,10 +54,10 @@ std::optional<Vector2> penetration_vector_inside_line(const LineSegment &line,
 ng_float_t penetration_inside_line(const LineSegment &line,
                                    const Vector2 &center, ng_float_t radius) {
   ng_float_t y = (center - line.p1).dot(line.e2);
-  if (abs(y) < radius) {
+  if (std::abs(y) < radius) {
     ng_float_t x = (center - line.p1).dot(line.e1);
     if (x < radius + 1e-3 || x > line.length - radius - 1e-3) return 0.0;
-    return radius - abs(y);
+    return radius - std::abs(y);
   }
   return 0;
 }