diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
index cc3e5054cf0dc..aa5eecc245de6 100644
--- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
+++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
@@ -144,11 +144,10 @@ inline bool smoothPath(
       0, trajectory.size(), external_v_limit->max_velocity, trajectory);
   }
   const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
-  auto nearest_idx =
-    motion_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position);
 
   // Resample trajectory with ego-velocity based interval distances
-  auto traj_resampled = smoother->resampleTrajectory(*traj_lateral_acc_filtered, v0, nearest_idx);
+  auto traj_resampled = smoother->resampleTrajectory(
+    *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits<double>::max());
   const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4);
   if (!traj_resampled_closest) {
     return false;
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
index 77aba3fcbc294..6ea62a101e39b 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -165,8 +165,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   void updateDataForExternalVelocityLimit();
 
-  boost::optional<TrajectoryPoints> poseResampleTrajectory(const TrajectoryPoints trajectory) const;
-
   AlgorithmType getAlgorithmType(const std::string & algorithm_name) const;
 
   TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const;
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
index 7301ae49f412b..76fffd5f95901 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
@@ -19,6 +19,7 @@
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include <geometry_msgs/msg/pose.hpp>
 
 #include "boost/optional.hpp"
 
@@ -43,12 +44,14 @@ struct ResampleParam
 };
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
-  const ResampleParam & param);
+  const TrajectoryPoints & input, const double v_current,
+  const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold,
+  const ResampleParam & param, const bool use_zoh_for_v = true);
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
-  const double nominal_ds);
+  const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose,
+  const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds,
+  const bool use_zoh_for_v = true);
 }  // namespace resampling
 }  // namespace motion_velocity_smoother
 
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
index 5a02e48afb91d..32e97b5511827 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
@@ -72,7 +72,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+    [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose,
+    [[maybe_unused]] const double delta_yaw_threshold) const override;
 
   boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
index 3138716359c45..0cbf3a2cbd733 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
@@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+    const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const override;
 
   void setParam(const Param & param);
   Param getParam() const;
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
index 5ab6befb5b676..c822c2625c29a 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
@@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_yaw_threshold) const override;
 
   void setParam(const Param & smoother_param);
   Param getParam() const;
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
index 53c112ddf90aa..145336211228f 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
@@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_yaw_threshold) const override;
 
   void setParam(const Param & smoother_param);
   Param getParam() const;
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
index e34487b767b45..0d401af8e5ebf 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -57,7 +57,8 @@ class SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
 
   virtual boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_yaw_threshold) const = 0;
 
   virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0,
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
index 4d529bdc5398d..ba3bb4805ae89 100644
--- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
@@ -64,10 +64,6 @@ void applyMaximumVelocityLimit(
 
 boost::optional<size_t> searchZeroVelocityIdx(const TrajectoryPoints & trajectory);
 
-boost::optional<TrajectoryPoints> applyLinearInterpolation(
-  const std::vector<double> & base_index, const TrajectoryPoints & base_trajectory,
-  const std::vector<double> & out_index, const bool use_spline_for_pose = false);
-
 bool calcStopDistWithJerkConstraints(
   const double v0, const double a0, const double jerk_acc, const double jerk_dec,
   const double min_acc, const double target_vel, std::map<double, double> & jerk_profile,
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
index e4985d0525335..2393de92326c6 100644
--- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
+++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -376,7 +376,10 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
     return;
   }
 
-  auto output_resampled = poseResampleTrajectory(output);
+  // Note that output velocity is resampled by linear interpolation
+  auto output_resampled = resampling::resampleTrajectory(
+    output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose,
+    node_param_.delta_yaw_threshold, node_param_.post_resample_param, false);
   if (!output_resampled) {
     RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory");
     return;
@@ -495,15 +498,9 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   }
 
   // Resample trajectory with ego-velocity based interval distance
-  const auto traj_pre_resampled_closest = findNearestIndexFromEgo(*traj_lateral_acc_filtered);
-  if (!traj_pre_resampled_closest) {
-    RCLCPP_WARN(
-      get_logger(), "Cannot find closest waypoint for traj_lateral_acc_filtered trajectory");
-    return false;
-  }
   auto traj_resampled = smoother_->resampleTrajectory(
     *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
-    *traj_pre_resampled_closest);
+    current_pose_ptr_->pose, node_param_.delta_yaw_threshold);
   if (!traj_resampled) {
     RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
     return false;
@@ -606,22 +603,6 @@ void MotionVelocitySmootherNode::insertBehindVelocity(
   }
 }
 
-boost::optional<TrajectoryPoints> MotionVelocitySmootherNode::poseResampleTrajectory(
-  const TrajectoryPoints trajectory) const
-{
-  // Get the nearest point
-  const auto closest_idx = findNearestIndexFromEgo(trajectory);
-  if (!closest_idx) {
-    return {};
-  }
-
-  auto trajectory_resampled = resampling::resampleTrajectory(
-    trajectory, current_odometry_ptr_->twist.twist.linear.x, *closest_idx,
-    node_param_.post_resample_param);
-
-  return trajectory_resampled;
-}
-
 void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const
 {
   const auto closest_optional = findNearestIndexFromEgo(trajectory);
diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp
index 23245f12d44f2..f04a564bbdeda 100644
--- a/planning/motion_velocity_smoother/src/resample.cpp
+++ b/planning/motion_velocity_smoother/src/resample.cpp
@@ -14,6 +14,9 @@
 
 #include "motion_velocity_smoother/resample.hpp"
 
+#include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/tmp_conversion.hpp"
+
 #include <algorithm>
 #include <vector>
 
@@ -22,23 +25,23 @@ namespace motion_velocity_smoother
 namespace resampling
 {
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
-  const ResampleParam & param)
+  const TrajectoryPoints & input, const double v_current,
+  const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold,
+  const ResampleParam & param, const bool use_zoh_for_v)
 {
   // Arc length from the initial point to the closest point
-  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
-
-  // Get the nearest point where velocity is zero
-  auto zero_vel_id = motion_utils::searchZeroVelocityIndex(input, closest_id, input.size());
-  // Arc length from the closest point to the point where velocity is zero
-  double zero_vel_arclength_value = param.max_trajectory_length;
-  if (zero_vel_id) {
-    zero_vel_arclength_value = std::min(
-      zero_vel_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *zero_vel_id));
+  const auto negative_front_arclength_value = motion_utils::calcSignedArcLength(
+    input, current_pose, 0, std::numeric_limits<double>::max(), delta_yaw_threshold);
+  if (!negative_front_arclength_value) {
+    return {};
   }
+  const auto front_arclength_value = std::fabs(*negative_front_arclength_value);
+
+  const auto dist_to_closest_stop_point =
+    motion_utils::calcDistanceToForwardStopPoint(input, current_pose);
 
   // Get the resample size from the closest point
-  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  const double trajectory_length = motion_utils::calcArcLength(input);
   const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001);
   const double ds_nominal =
     std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance);
@@ -77,7 +80,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Check if the distance is longer than input arclength
-    if (dist_i + front_arclength_value >= in_arclength.back()) {
+    if (dist_i + front_arclength_value >= trajectory_length) {
       is_endpoint_included = true;  // distance is over input endpoint.
       break;
     }
@@ -95,20 +98,22 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Handle Close Stop Point
-    if (dist_i > zero_vel_arclength_value && !is_zero_point_included) {
-      if (std::fabs(dist_i - zero_vel_arclength_value) > 1e-3) {
+    if (
+      dist_to_closest_stop_point && dist_i > *dist_to_closest_stop_point &&
+      !is_zero_point_included) {
+      if (std::fabs(dist_i - *dist_to_closest_stop_point) > 1e-3) {
         // dist_i is much bigger than zero_vel_arclength_value
         if (
           !out_arclength.empty() &&
-          std::fabs(out_arclength.back() - (zero_vel_arclength_value + front_arclength_value)) <
+          std::fabs(out_arclength.back() - (*dist_to_closest_stop_point + front_arclength_value)) <
             1e-3) {
-          out_arclength.back() = zero_vel_arclength_value + front_arclength_value;
+          out_arclength.back() = *dist_to_closest_stop_point + front_arclength_value;
         } else {
-          out_arclength.push_back(zero_vel_arclength_value + front_arclength_value);
+          out_arclength.push_back(*dist_to_closest_stop_point + front_arclength_value);
         }
       } else {
         // dist_i is close to the zero_vel_arclength_value
-        dist_i = zero_vel_arclength_value;
+        dist_i = *dist_to_closest_stop_point;
       }
 
       is_zero_point_included = true;
@@ -117,24 +122,21 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     out_arclength.push_back(dist_i + front_arclength_value);
   }
 
-  auto output =
-    trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength, true);
-  if (!output) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
-      "fail trajectory interpolation. size : in_arclength = %lu, "
-      "input = %lu, out_arclength = %lu",
-      in_arclength.size(), input.size(), out_arclength.size());
-    return boost::none;
+  if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) {
+    return input;
   }
 
+  const auto output_traj = motion_utils::resampleTrajectory(
+    motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+
   // add end point directly to consider the endpoint velocity.
   if (is_endpoint_included) {
     constexpr double ep_dist = 1.0E-3;
-    if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
-      output->back() = input.back();
+    if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) {
+      output.back() = input.back();
     } else {
-      output->push_back(input.back());
+      output.push_back(input.back());
     }
   }
 
@@ -142,36 +144,37 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
 }
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
-  const double nominal_ds)
+  const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose,
+  const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds,
+  const bool use_zoh_for_v)
 {
   // input arclength
-  std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  const double trajectory_length = motion_utils::calcArcLength(input);
+  const auto dist_to_closest_stop_point =
+    motion_utils::calcDistanceToForwardStopPoint(input, current_pose);
 
-  // Get the nearest point where velocity is zero
-  // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1.
-  auto stop_id = motion_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size());
-  // Arc length from the closest point to the point where velocity is zero
+  // distance to stop point
   double stop_arclength_value = param.max_trajectory_length;
-  if (stop_id) {
-    stop_arclength_value = std::min(
-      stop_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *stop_id));
+  if (dist_to_closest_stop_point) {
+    stop_arclength_value = std::min(*dist_to_closest_stop_point, stop_arclength_value);
   }
-
-  // Do dense resampling before the stop line(3[m] ahead of the stop line)
   if (param.min_trajectory_length < stop_arclength_value) {
     stop_arclength_value = param.min_trajectory_length;
   }
-  if (in_arclength.back() < stop_arclength_value) {
-    stop_arclength_value = in_arclength.back();
-  }
-  double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0);
+
+  // Do dense resampling before the stop line(3[m] ahead of the stop line)
+  const double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0);
 
   std::vector<double> out_arclength;
 
   // Step1. Resample front trajectory
   // Arc length from the initial point to the closest point
-  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
+  const auto negative_front_arclength_value = motion_utils::calcSignedArcLength(
+    input, current_pose, 0, std::numeric_limits<double>::max(), delta_yaw_threshold);
+  if (!negative_front_arclength_value) {
+    return {};
+  }
+  const auto front_arclength_value = std::fabs(*negative_front_arclength_value);
   for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) {
     out_arclength.push_back(s);
   }
@@ -199,7 +202,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Check if the distance is longer than input arclength
-    if (dist_i + front_arclength_value >= in_arclength.back()) {
+    if (dist_i + front_arclength_value >= trajectory_length) {
       is_endpoint_included = true;  // distance is over input endpoint.
       break;
     }
@@ -238,23 +241,21 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     out_arclength.push_back(dist_i + front_arclength_value);
   }
 
-  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
-  if (!output) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
-      "fail trajectory interpolation. size : in_arclength = %lu, "
-      "input = %lu, out_arclength = %lu",
-      in_arclength.size(), input.size(), out_arclength.size());
-    return boost::none;
+  if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) {
+    return input;
   }
 
+  const auto output_traj = motion_utils::resampleTrajectory(
+    motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+
   // add end point directly to consider the endpoint velocity.
   if (is_endpoint_included) {
     constexpr double ep_dist = 1.0E-3;
-    if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
-      output->back() = input.back();
+    if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) {
+      output.back() = input.back();
     } else {
-      output->push_back(input.back());
+      output.push_back(input.back());
     }
   }
 
diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
index 1d6157ea67202..e32d37b403a15 100644
--- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -14,6 +14,9 @@
 
 #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
 
+#include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/tmp_conversion.hpp"
+
 #include <algorithm>
 #include <string>
 #include <tuple>
@@ -229,8 +232,9 @@ bool AnalyticalJerkConstrainedSmoother::apply(
 }
 
 boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTrajectory(
-  const TrajectoryPoints & input, [[maybe_unused]] const double v_current,
-  [[maybe_unused]] const int closest_id) const
+  const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+  [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose,
+  [[maybe_unused]] const double delta_yaw_threshold) const
 {
   TrajectoryPoints output;
   if (input.empty()) {
@@ -291,19 +295,17 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLatera
   for (double s = 0; s < in_arclength.back(); s += points_interval) {
     out_arclength.push_back(s);
   }
-  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
-  if (!output) {
-    RCLCPP_WARN(logger_, "Interpolation failed at lateral acceleration filter.");
-    return boost::none;
-  }
-  output->back() = input.back();  // keep the final speed.
+  const auto output_traj =
+    motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+  output.back() = input.back();  // keep the final speed.
 
   constexpr double curvature_calc_dist = 5.0;  // [m] calc curvature with 5m away points
   const size_t idx_dist =
     static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
 
   // Calculate curvature assuming the trajectory points interval is constant
-  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist);
+  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist);
 
   // Decrease speed according to lateral G
   const size_t before_decel_index =
@@ -313,17 +315,17 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLatera
   const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel);
 
   std::vector<int> filtered_points;
-  for (size_t i = 0; i < output->size(); ++i) {
+  for (size_t i = 0; i < output.size(); ++i) {
     double curvature = 0.0;
     const size_t start = i > before_decel_index ? i - before_decel_index : 0;
-    const size_t end = std::min(output->size(), i + after_decel_index);
+    const size_t end = std::min(output.size(), i + after_decel_index);
     for (size_t j = start; j < end; ++j) {
       curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
     }
     double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
     v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity);
-    if (output->at(i).longitudinal_velocity_mps > v_curvature_max) {
-      output->at(i).longitudinal_velocity_mps = v_curvature_max;
+    if (output.at(i).longitudinal_velocity_mps > v_curvature_max) {
+      output.at(i).longitudinal_velocity_mps = v_curvature_max;
       filtered_points.push_back(i);
     }
   }
@@ -341,29 +343,29 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLatera
     if (is_updated == false) {
       start_index = index;
       end_index = index;
-      min_latacc_velocity = output->at(index).longitudinal_velocity_mps;
+      min_latacc_velocity = output.at(index).longitudinal_velocity_mps;
       is_updated = true;
       continue;
     }
 
     if (
-      tier4_autoware_utils::calcDistance2d(output->at(end_index), output->at(index)) <
+      tier4_autoware_utils::calcDistance2d(output.at(end_index), output.at(index)) <
       dist_threshold) {
       end_index = index;
       min_latacc_velocity = std::min(
-        static_cast<double>(output->at(index).longitudinal_velocity_mps), min_latacc_velocity);
+        static_cast<double>(output.at(index).longitudinal_velocity_mps), min_latacc_velocity);
     } else {
       latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity);
       start_index = index;
       end_index = index;
-      min_latacc_velocity = output->at(index).longitudinal_velocity_mps;
+      min_latacc_velocity = output.at(index).longitudinal_velocity_mps;
     }
   }
   if (is_updated) {
     latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity);
   }
 
-  for (size_t i = 0; i < output->size(); ++i) {
+  for (size_t i = 0; i < output.size(); ++i) {
     for (const auto & lat_acc_filtered_range : latacc_filtered_ranges) {
       const size_t start_index = std::get<0>(lat_acc_filtered_range);
       const size_t end_index = std::get<1>(lat_acc_filtered_range);
@@ -372,7 +374,7 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLatera
       if (
         start_index <= i && i <= end_index &&
         smoother_param_.latacc.enable_constant_velocity_while_turning) {
-        output->at(i).longitudinal_velocity_mps = min_latacc_velocity;
+        output.at(i).longitudinal_velocity_mps = min_latacc_velocity;
         break;
       }
     }
diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
index 9332c971aa9d0..2fcfc1c77fd8a 100644
--- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -100,8 +100,10 @@ bool JerkFilteredSmoother::apply(
   debug_trajectories[2] = filtered;
 
   // Resample TrajectoryPoints for Optimization
-  auto opt_resampled_trajectory =
-    resampling::resampleTrajectory(filtered, v0, 0, base_param_.resample_param);
+  const auto initial_traj_pose = filtered.front().pose;
+  auto opt_resampled_trajectory = resampling::resampleTrajectory(
+    filtered, v0, initial_traj_pose, std::numeric_limits<double>::max(),
+    base_param_.resample_param);
 
   if (!opt_resampled_trajectory) {
     RCLCPP_WARN(logger_, "Resample failed!");
@@ -468,10 +470,12 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
 }
 
 boost::optional<TrajectoryPoints> JerkFilteredSmoother::resampleTrajectory(
-  const TrajectoryPoints & input, const double /*v_current*/, const int closest_id) const
+  const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+  const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const
 {
   return resampling::resampleTrajectory(
-    input, closest_id, base_param_.resample_param, smoother_param_.jerk_filter_ds);
+    input, current_pose, delta_yaw_threshold, base_param_.resample_param,
+    smoother_param_.jerk_filter_ds);
 }
 
 }  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
index 092ec10fe2902..da3764e307cc7 100644
--- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -222,9 +222,11 @@ bool L2PseudoJerkSmoother::apply(
 }
 
 boost::optional<TrajectoryPoints> L2PseudoJerkSmoother::resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const int closest_id) const
+  const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+  const double delta_yaw_threshold) const
 {
-  return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param);
+  return resampling::resampleTrajectory(
+    input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param);
 }
 
 }  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
index 51a572d731005..8030a0ff5c1da 100644
--- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -234,9 +234,11 @@ bool LinfPseudoJerkSmoother::apply(
 }
 
 boost::optional<TrajectoryPoints> LinfPseudoJerkSmoother::resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const int closest_id) const
+  const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+  const double delta_yaw_threshold) const
 {
-  return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param);
+  return resampling::resampleTrajectory(
+    input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param);
 }
 
 }  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
index ed20cfc4bb0aa..fa357948e24af 100644
--- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -14,6 +14,8 @@
 
 #include "motion_velocity_smoother/smoother/smoother_base.hpp"
 
+#include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/tmp_conversion.hpp"
 #include "motion_velocity_smoother/resample.hpp"
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 
@@ -73,25 +75,21 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   // Interpolate with constant interval distance for lateral acceleration calculation.
   constexpr double points_interval = 0.1;  // [m]
   std::vector<double> out_arclength;
-  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
-  for (double s = 0; s < in_arclength.back(); s += points_interval) {
+  const auto traj_length = motion_utils::calcArcLength(input);
+  for (double s = 0; s < traj_length; s += points_interval) {
     out_arclength.push_back(s);
   }
-  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
-  if (!output) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("smoother").get_child("smoother_base"),
-      "interpolation failed at lateral acceleration filter.");
-    return boost::none;
-  }
-  output->back() = input.back();  // keep the final speed.
+  const auto output_traj =
+    motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+  output.back() = input.back();  // keep the final speed.
 
   constexpr double curvature_calc_dist = 5.0;  // [m] calc curvature with 5m away points
   const size_t idx_dist =
     static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
 
   // Calculate curvature assuming the trajectory points interval is constant
-  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist);
+  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist);
 
   //  Decrease speed according to lateral G
   const size_t before_decel_index =
@@ -103,13 +101,13 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   const auto latacc_min_vel_arr =
     enable_smooth_limit
       ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit(
-          *output, v0, a0, base_param_.min_jerk, base_param_.max_accel, base_param_.min_decel)
+          output, v0, a0, base_param_.min_jerk, base_param_.max_accel, base_param_.min_decel)
       : std::vector<double>{};
 
-  for (size_t i = 0; i < output->size(); ++i) {
+  for (size_t i = 0; i < output.size(); ++i) {
     double curvature = 0.0;
     const size_t start = i > after_decel_index ? i - after_decel_index : 0;
-    const size_t end = std::min(output->size(), i + before_decel_index + 1);
+    const size_t end = std::min(output.size(), i + before_decel_index + 1);
     for (size_t j = start; j < end; ++j) {
       if (j >= curvature_v.size()) return output;
       curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
@@ -120,8 +118,8 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
       if (i >= latacc_min_vel_arr.size()) return output;
       v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i));
     }
-    if (output->at(i).longitudinal_velocity_mps > v_curvature_max) {
-      output->at(i).longitudinal_velocity_mps = v_curvature_max;
+    if (output.at(i).longitudinal_velocity_mps > v_curvature_max) {
+      output.at(i).longitudinal_velocity_mps = v_curvature_max;
     }
   }
   return output;
diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
index 5e59a2ec881a7..8cb38496a8e67 100644
--- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp
+++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
@@ -14,6 +14,7 @@
 
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 
+#include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
 #include "motion_velocity_smoother/linear_interpolation.hpp"
 
@@ -83,17 +84,14 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint(
   // calc internal proportion
   const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))};
 
-  auto interpolate = [&prop](double x1, double x2) { return prop * x1 + (1.0 - prop) * x2; };
-
   {
     const auto & seg_pt = trajectory.at(segment_idx);
     const auto & next_pt = trajectory.at(segment_idx + 1);
-    traj_p.longitudinal_velocity_mps =
-      interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps);
-    traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2);
-    traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x);
-    traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y);
-    traj_p.pose.position.z = interpolate(next_pt.pose.position.z, seg_pt.pose.position.z);
+    traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop);
+    traj_p.longitudinal_velocity_mps = interpolation::lerp(
+      seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop);
+    traj_p.acceleration_mps2 =
+      interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop);
   }
 
   return traj_p;
@@ -273,62 +271,6 @@ void applyMaximumVelocityLimit(
   }
 }
 
-boost::optional<TrajectoryPoints> applyLinearInterpolation(
-  const std::vector<double> & base_index, const TrajectoryPoints & base_trajectory,
-  const std::vector<double> & out_index, const bool use_spline_for_pose)
-{
-  std::vector<double> px, py, pz, pyaw, tlx, taz, alx;
-  for (const auto & p : base_trajectory) {
-    px.push_back(p.pose.position.x);
-    py.push_back(p.pose.position.y);
-    pz.push_back(p.pose.position.z);
-    pyaw.push_back(tf2::getYaw(p.pose.orientation));
-    tlx.push_back(p.longitudinal_velocity_mps);
-    taz.push_back(p.heading_rate_rps);
-    alx.push_back(p.acceleration_mps2);
-  }
-
-  convertEulerAngleToMonotonic(pyaw);
-
-  boost::optional<std::vector<double>> px_p, py_p, pz_p, pyaw_p;
-  if (use_spline_for_pose) {
-    px_p = interpolation::slerp(base_index, px, out_index);
-    py_p = interpolation::slerp(base_index, py, out_index);
-    pz_p = interpolation::slerp(base_index, pz, out_index);
-    pyaw_p = interpolation::slerp(base_index, pyaw, out_index);
-  } else {
-    px_p = linear_interpolation::interpolate(base_index, px, out_index);
-    py_p = linear_interpolation::interpolate(base_index, py, out_index);
-    pz_p = linear_interpolation::interpolate(base_index, pz, out_index);
-    pyaw_p = linear_interpolation::interpolate(base_index, pyaw, out_index);
-  }
-  const auto tlx_p = linear_interpolation::interpolate(base_index, tlx, out_index);
-  const auto taz_p = linear_interpolation::interpolate(base_index, taz, out_index);
-  const auto alx_p = linear_interpolation::interpolate(base_index, alx, out_index);
-
-  if (!px_p || !py_p || !pz_p || !pyaw_p || !tlx_p || !taz_p || !alx_p) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
-      "interpolation error!!");
-    return {};
-  }
-
-  TrajectoryPoints out_trajectory;
-  TrajectoryPoint point;
-  for (unsigned int i = 0; i < out_index.size(); ++i) {
-    point.pose.position.x = px_p->at(i);
-    point.pose.position.y = py_p->at(i);
-    point.pose.position.z = pz_p->at(i);
-    point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(pyaw_p->at(i));
-
-    point.longitudinal_velocity_mps = tlx_p->at(i);
-    point.heading_rate_rps = taz_p->at(i);
-    point.acceleration_mps2 = alx_p->at(i);
-    out_trajectory.push_back(point);
-  }
-  return boost::optional<TrajectoryPoints>(out_trajectory);
-}
-
 bool calcStopDistWithJerkConstraints(
   const double v0, const double a0, const double jerk_acc, const double jerk_dec,
   const double min_acc, const double target_vel, std::map<double, double> & jerk_profile,
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index d904cf595b9f4..413c06f85f7ac 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -14,6 +14,7 @@
 
 #include "obstacle_cruise_planner/node.hpp"
 
+#include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
 #include "obstacle_cruise_planner/polygon_utils.hpp"
 #include "obstacle_cruise_planner/utils.hpp"
@@ -75,57 +76,15 @@ bool isFrontObstacle(
   return true;
 }
 
-TrajectoryPoint calcLinearPoint(
-  const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const double length)
-{
-  TrajectoryPoint output;
-  const double dx = p_to.pose.position.x - p_from.pose.position.x;
-  const double dy = p_to.pose.position.y - p_from.pose.position.y;
-  const double norm = std::hypot(dx, dy);
-
-  output = p_to;
-  output.pose.position.x += length * dx / norm;
-  output.pose.position.y += length * dy / norm;
-
-  return output;
-}
-
-// TODO(murooka) replace with spline interpolation
 Trajectory decimateTrajectory(const Trajectory & input, const double step_length)
 {
   Trajectory output{};
 
-  if (input.points.empty()) {
+  if (input.points.size() < 2) {
     return output;
   }
 
-  double trajectory_length_sum = 0.0;
-  double next_length = 0.0;
-
-  constexpr double epsilon = 1e-3;
-  for (int i = 0; i < static_cast<int>(input.points.size()) - 1; ++i) {
-    const auto & p_front = input.points.at(i);
-    const auto & p_back = input.points.at(i + 1);
-
-    if (next_length <= trajectory_length_sum + epsilon) {
-      const auto p_interpolate =
-        calcLinearPoint(p_front, p_back, next_length - trajectory_length_sum);
-      output.points.push_back(p_interpolate);
-      next_length += step_length;
-      continue;
-    }
-
-    trajectory_length_sum += tier4_autoware_utils::calcDistance2d(p_front, p_back);
-  }
-
-  // avoid "Same points are given"
-  if (
-    !input.points.empty() && !output.points.empty() &&
-    epsilon < tier4_autoware_utils::calcDistance2d(input.points.back(), output.points.back())) {
-    output.points.push_back(input.points.back());
-  }
-
-  return output;
+  return motion_utils::resampleTrajectory(input, step_length);
 }
 
 PredictedPath getHighestConfidencePredictedPath(const PredictedObject & predicted_object)