fix(crane_physics): FLYINGボールの着地時間計算でpos_zを考慮するよう修正

utility/crane_physics/include/crane_physics/ball_physics_model.hpp

@@ -61,8 +61,8 @@ class BallPhysicsModel
     const Point & position, const Point & velocity, Ball::State state, double pos_z, double vel_z,
     double time_ahead) const -> Point;
 
-  [[nodiscard]] auto getStopTime(const Point & velocity, Ball::State state, double vel_z) const
-    -> double;
+  [[nodiscard]] auto getStopTime(
+    const Point & velocity, Ball::State state, double pos_z, double vel_z) const -> double;
 
   [[nodiscard]] auto getMaxDistance(
     const Point & position, const Point & velocity, Ball::State state, double pos_z,
@@ -89,6 +89,14 @@ class BallPhysicsModel
   Config config_;
 
   // ヘルパー関数
+  // FLYING状態のボールが着地するまでの時間 [s] を返す。predictPosition / predictVelocity /
+  // getStopTime / getMaxDistance はすべてこの時刻を空中と転がりの境目に使う。
+  // pos_z は接地面を 0 とする高さ [m]（上向き正、checkStateTransition も z <= 0 を着地とみなす）、
+  // vel_z はその時間微分 [m/s]、config_.gravity は負値 [m/s²]。
+  // z(t) = 0 の大きい方の根（下降して地面に達する時刻）を返し、それが正でない、
+  // または判別式が負（地面まで上がらない）なら 0 を返す。
+  [[nodiscard]] auto getFlyingLandingTime(double pos_z, double vel_z) const -> double;
+
   [[nodiscard]] auto getRollingStopTime(const Point & velocity) const -> double;
 
   [[nodiscard]] auto getRollingMaxDistance(const Point & velocity) const -> double;

utility/crane_physics/src/ball_info.cpp

@@ -42,7 +42,10 @@ auto Ball::getPredictedVelocity(double time_ahead) const -> Point
   return physics_model_->predictVelocity(pos, vel, state, pos_z, vel_z, time_ahead);
 }
 
-auto Ball::getStopTime() const -> double { return physics_model_->getStopTime(vel, state, vel_z); }
+auto Ball::getStopTime() const -> double
+{
+  return physics_model_->getStopTime(vel, state, pos_z, vel_z);
+}
 
 auto Ball::getMaxDistance() const -> double
 {
@@ -336,14 +339,10 @@ auto Ball::ParabolicPhysics::getGroundIntersection() const -> std::pair<Point, d
   double t1 = (-b + sqrt_discriminant) / (2 * a);
   double t2 = (-b - sqrt_discriminant) / (2 * a);
 
-  double landing_time;
-  if (t1 > 1e-6 && t2 > 1e-6) {
-    landing_time = std::min(t1, t2);
-  } else if (t1 > 1e-6) {
-    landing_time = t1;
-  } else if (t2 > 1e-6) {
-    landing_time = t2;
-  } else {
+  // 大きい方の根が下降して地面に達する時刻。小さい方は z0 < 0 から上昇して地面を通過する時刻
+  // （BallPhysicsModel::getFlyingLandingTime と同じ選び方）
+  const double landing_time = std::max(t1, t2);
+  if (landing_time <= 1e-6) {
     return {Point(initial_position_.x(), initial_position_.y()), 0.0};
   }
 
@@ -484,15 +483,9 @@ Ball::ParabolicPhysics::Point3DStamped Ball::ParabolicPhysics::getGroundPoint()
   double t1 = (-b + sqrt_discriminant) / (2 * a);
   double t2 = (-b - sqrt_discriminant) / (2 * a);
 
-  // 正の時間で最初に地面に到達する時間を選択
-  double landing_time;
-  if (t1 > 1e-6 && t2 > 1e-6) {
-    landing_time = std::min(t1, t2);
-  } else if (t1 > 1e-6) {
-    landing_time = t1;
-  } else if (t2 > 1e-6) {
-    landing_time = t2;
-  } else {
+  // 大きい方の根が下降して地面に達する時刻（getGroundIntersection と同じ選び方）
+  const double landing_time = std::max(t1, t2);
+  if (landing_time <= 1e-6) {
     // 両方とも負または零の場合、既に地面より下にいる
     return {initial_position_, 0.0};
   }

utility/crane_physics/src/ball_physics_model.cpp

@@ -140,27 +140,7 @@ auto BallPhysicsModel::predictPosition(
       Point3D initial_pos(position.x(), position.y(), pos_z);
       Point3D initial_vel(velocity.x(), velocity.y(), vel_z);
 
-      // 着地時間計算
-      double a = 0.5 * config_.gravity;
-      double b = vel_z;
-      double c = pos_z;
-
-      double discriminant = b * b - 4 * a * c;
-      double landing_time = 0.0;
-
-      if (discriminant >= 0) {
-        double sqrt_discriminant = std::sqrt(discriminant);
-        double t1 = (-b + sqrt_discriminant) / (2 * a);
-        double t2 = (-b - sqrt_discriminant) / (2 * a);
-
-        if (t1 > 1e-6 && t2 > 1e-6) {
-          landing_time = std::min(t1, t2);
-        } else if (t1 > 1e-6) {
-          landing_time = t1;
-        } else if (t2 > 1e-6) {
-          landing_time = t2;
-        }
-      }
+      const double landing_time = getFlyingLandingTime(pos_z, vel_z);
 
       if (time_ahead <= landing_time) {
         // まだ空中
@@ -198,27 +178,7 @@ auto BallPhysicsModel::predictVelocity(
       return getRollingPredictedVelocity(velocity, time_ahead);
 
     case Ball::State::FLYING: {
-      // 独立した放物運動計算
-      double a = 0.5 * config_.gravity;
-      double b = vel_z;
-      double c = pos_z;
-
-      double discriminant = b * b - 4 * a * c;
-      double landing_time = 0.0;
-
-      if (discriminant >= 0) {
-        double sqrt_discriminant = std::sqrt(discriminant);
-        double t1 = (-b + sqrt_discriminant) / (2 * a);
-        double t2 = (-b - sqrt_discriminant) / (2 * a);
-
-        if (t1 > 1e-6 && t2 > 1e-6) {
-          landing_time = std::min(t1, t2);
-        } else if (t1 > 1e-6) {
-          landing_time = t1;
-        } else if (t2 > 1e-6) {
-          landing_time = t2;
-        }
-      }
+      const double landing_time = getFlyingLandingTime(pos_z, vel_z);
 
       if (time_ahead <= landing_time) {
         // まだ空中（XY速度は一定）
@@ -233,8 +193,8 @@ auto BallPhysicsModel::predictVelocity(
   return {0, 0};
 }
 
-auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, double vel_z) const
-  -> double
+auto BallPhysicsModel::getStopTime(
+  const Point & velocity, Ball::State state, double pos_z, double vel_z) const -> double
 {
   switch (state) {
     case Ball::State::STOPPED:
@@ -244,11 +204,8 @@ auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, do
       return getRollingStopTime(velocity);
 
     case Ball::State::FLYING: {
-      // 簡単な着地時間計算（pos_z=0と仮定）
-      double landing_time = 0.0;
-      if (vel_z != 0) {
-        landing_time = std::max(0.0, -2.0 * vel_z / config_.gravity);
-      }
+      // 現在高度 pos_z を考慮した着地時間を計算する。
+      double landing_time = getFlyingLandingTime(pos_z, vel_z);
 
       double rolling_stop_time = getRollingStopTime(velocity);
       return landing_time + rolling_stop_time;
@@ -258,7 +215,7 @@ auto BallPhysicsModel::getStopTime(const Point & velocity, Ball::State state, do
 }
 
 auto BallPhysicsModel::getMaxDistance(
-  const Point & position, const Point & velocity, Ball::State state, [[maybe_unused]] double pos_z,
+  const Point & position, const Point & velocity, Ball::State state, double pos_z,
   double vel_z) const -> double
 {
   switch (state) {
@@ -269,11 +226,8 @@ auto BallPhysicsModel::getMaxDistance(
       return getRollingMaxDistance(velocity);
 
     case Ball::State::FLYING: {
-      // 着地位置計算
-      double landing_time = 0.0;
-      if (vel_z != 0) {
-        landing_time = std::max(0.0, -2.0 * vel_z / config_.gravity);
-      }
+      // 現在高度 pos_z を考慮した着地時間から着地位置を計算する。
+      double landing_time = getFlyingLandingTime(pos_z, vel_z);
 
       Point landing_pos;
       landing_pos.x() = position.x() + velocity.x() * landing_time;
@@ -288,6 +242,23 @@ auto BallPhysicsModel::getMaxDistance(
   return 0.0;
 }
 
+auto BallPhysicsModel::getFlyingLandingTime(double pos_z, double vel_z) const -> double
+{
+  // z(t) = pos_z + vel_z * t + 0.5 * gravity * t^2 = 0 を解く。
+  // gravity < 0 で z(t) は上に凸なので、大きい方の根が下降して地面に達する時刻になる。
+  // 小さい方の根は pos_z < 0 から上昇して地面を通過する時刻なので着地には使わない。
+  const double a = 0.5 * config_.gravity;
+  const double discriminant = vel_z * vel_z - 4 * a * pos_z;
+  if (discriminant < 0) {
+    // pos_z < 0 のまま地面まで上がらない
+    return 0.0;
+  }
+  const double sqrt_discriminant = std::sqrt(discriminant);
+  const double landing_time =
+    std::max((-vel_z + sqrt_discriminant) / (2 * a), (-vel_z - sqrt_discriminant) / (2 * a));
+  return landing_time > 1e-6 ? landing_time : 0.0;
+}
+
 auto BallPhysicsModel::getRollingStopTime(const Point & velocity) const -> double
 {
   double speed = velocity.norm();

utility/crane_physics/test/test_ball_model.cpp

@@ -6,7 +6,10 @@
 
 #include <gtest/gtest.h>
 
+#include <cmath>
 #include <crane_physics/ball_info.hpp>
+#include <crane_physics/ball_physics_model.hpp>
+#include <utility>
 
 namespace crane
 {
@@ -167,6 +170,22 @@ TEST_F(BallParabolicPhysicsTest, GetGroundPointNoGroundImpact)
   EXPECT_NEAR(ground_point.position.z(), expected_z, 0.1);
 }
 
+TEST_F(BallParabolicPhysicsTest, GroundPointRisingFromBelowGroundUsesDescendingRoot)
+{
+  // 根は約 0.005 秒（上昇中の地面通過）と約 0.403 秒（下降して着地）。着地は後者
+  auto ball = createBall(Point3D(0.0, 0.0, -0.01), Point3D(1.0, 0.0, 2.0));
+  auto physics = ball.getParabolicPhysics();
+  const double expected_time = (2.0 + std::sqrt(4.0 - 2.0 * 9.81 * 0.01)) / 9.81;
+
+  auto [landing_pos, landing_time] = physics.getGroundIntersection();
+  EXPECT_NEAR(landing_time, expected_time, 1e-9);
+  EXPECT_NEAR(landing_pos.x(), expected_time, 1e-9);
+
+  auto ground_point = physics.getGroundPoint();
+  EXPECT_NEAR(ground_point.time, expected_time, 1e-9);
+  EXPECT_NEAR(ground_point.position.x(), expected_time, 1e-9);
+}
+
 TEST_F(BallParabolicPhysicsTest, EstimateInitialVelocityInsufficientData)
 {
   // データが不十分な場合のテスト
@@ -200,4 +219,67 @@ TEST_F(BallParabolicPhysicsTest, EstimateInitialVelocityNumericalStability)
   EXPECT_DOUBLE_EQ(pos_at_0.y(), 0.0);
   EXPECT_DOUBLE_EQ(pos_at_0.z(), 1.0);
 }
+
+// BallPhysicsModel の FLYING 着地時刻。
+// xy 速度 0 なら転がりが無く、getStopTime が着地時刻そのものになる
+class BallPhysicsModelFlyingLandingTest : public ::testing::Test
+{
+protected:
+  BallPhysicsModel model_ = BallPhysicsModel::createDefault();  // gravity = -9.81
+
+  [[nodiscard]] auto landingTime(double pos_z, double vel_z) const -> double
+  {
+    return model_.getStopTime(Point(0.0, 0.0), Ball::State::FLYING, pos_z, vel_z);
+  }
+};
+
+TEST_F(BallPhysicsModelFlyingLandingTest, DescendingFromHeight)
+{
+  // z(t) = 1 - t - 4.905 t^2 = 0 の正の根。pos_z を無視する旧式では 0 になっていた
+  EXPECT_NEAR(landingTime(1.0, -1.0), (-1.0 + std::sqrt(1.0 + 2.0 * 9.81 * 1.0)) / 9.81, 1e-9);
+}
+
+TEST_F(BallPhysicsModelFlyingLandingTest, RisingFromGround)
+{
+  EXPECT_NEAR(landingTime(0.0, 2.0), 2.0 * 2.0 / 9.81, 1e-9);
+}
+
+TEST_F(BallPhysicsModelFlyingLandingTest, RisingFromBelowGroundUsesDescendingRoot)
+{
+  // 根は約 0.005 秒（上昇中の地面通過）と約 0.403 秒（下降して着地）。着地は後者
+  EXPECT_NEAR(landingTime(-0.01, 2.0), (2.0 + std::sqrt(4.0 - 2.0 * 9.81 * 0.01)) / 9.81, 1e-9);
+}
+
+TEST_F(BallPhysicsModelFlyingLandingTest, NeverReachesGround)
+{
+  // 判別式が負: pos_z < 0 のまま地面まで上がらない
+  EXPECT_EQ(landingTime(-1.0, 1.0), 0.0);
+}
+
+TEST_F(BallPhysicsModelFlyingLandingTest, PredictionsSwitchToRollingAtLandingTime)
+{
+  const Point position(0.0, 0.0);
+  const Point velocity(1.0, 0.0);
+  for (const auto [pos_z, vel_z] : {std::pair{1.0, -1.0}, std::pair{-0.01, 2.0}}) {
+    const double landing_time = landingTime(pos_z, vel_z);
+    const auto state = Ball::State::FLYING;
+
+    // 着地までは xy 等速
+    EXPECT_NEAR(
+      model_.predictPosition(position, velocity, state, pos_z, vel_z, landing_time).x(),
+      landing_time, 1e-9);
+    EXPECT_EQ(
+      model_.predictVelocity(position, velocity, state, pos_z, vel_z, landing_time - 1e-3),
+      velocity);
+    // 着地後は転がりで減速する（deceleration = 0.36）
+    EXPECT_NEAR(
+      model_.predictVelocity(position, velocity, state, pos_z, vel_z, landing_time + 0.1).x(),
+      1.0 - 0.36 * 0.1, 1e-9);
+    // 停止時刻の予測位置が最大到達距離と一致する
+    const double stop_time = model_.getStopTime(velocity, state, pos_z, vel_z);
+    EXPECT_NEAR(
+      model_.predictPosition(position, velocity, state, pos_z, vel_z, stop_time).x(),
+      model_.getMaxDistance(position, velocity, state, pos_z, vel_z), 1e-9);
+  }
+}
 }  // namespace crane