Program Listing for File interpolation.hpp

            

            
/*
* SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef HOLOSCAN_POSE_TREE_MATH_INTERPOLATION_HPP
#define HOLOSCAN_POSE_TREE_MATH_INTERPOLATION_HPP

#include "holoscan/pose_tree/math/pose2.hpp"
#include "holoscan/pose_tree/math/pose3.hpp"
#include "holoscan/pose_tree/math/so2.hpp"
#include "holoscan/pose_tree/math/so3.hpp"

namespace holoscan {
namespace pose_tree_math {

template <typename K, typename T>
T interpolate(K q, T a, T b) {
  return a + q * (b - a);
}

}  // namespace pose_tree_math

template <typename K>
SO2<K> interpolate(K p, const SO2<K>& a, const SO2<K>& b) {
  const K a0 = a.angle();
  const K a1 = b.angle();
  return SO2<K>::from_angle(a0 + p * WrapPi(a1 - a0));
}

template <typename K>
SO3<K> interpolate(K p, const SO3<K>& a, const SO3<K>& b) {
  return SO3<K>::from_quaternion(a.quaternion().slerp(p, b.quaternion()));
}

template <typename K>
Pose2<K> interpolate(K p, const Pose2<K>& a, const Pose2<K>& b) {
  return Pose2<K>{interpolate(p, a.rotation, b.rotation),
                  pose_tree_math::interpolate(p, a.translation, b.translation)};
}

template <typename K>
Pose3<K> interpolate(K p, const Pose3<K>& a, const Pose3<K>& b) {
  return Pose3<K>{interpolate(p, a.rotation, b.rotation),
                  pose_tree_math::interpolate(p, a.translation, b.translation)};
}

template <typename K>
Pose2<K> slerp_interpolate(K p, const Pose2<K>& a, const Pose2<K>& b) {
  const Pose2<K> delta = a.inverse() * b;
  return a * delta.pow(p);
}

template <typename K>
Pose3<K> slerp_interpolate(K p, const Pose3<K>& a, const Pose3<K>& b) {
  const Pose3<K> delta = a.inverse() * b;
  return a * delta.pow(p);
}

}  // namespace holoscan

#endif/* HOLOSCAN_POSE_TREE_MATH_INTERPOLATION_HPP */