Program Listing for File so2.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_SO2_HPP
#define HOLOSCAN_POSE_TREE_MATH_SO2_HPP

#include <cmath>

#include "holoscan/pose_tree/math/types.hpp"

namespace holoscan {

template <typename K>
class SO2 {
 public:
  using Scalar = K;
  static constexpr int kDimension = 2;

  SO2() {}

  static SO2 identity() {
    SO2 q;
    q.cos_sin_ = Vector2<K>(K(1), K(0));
    return q;
  }

  static SO2 from_angle(K angle) {
    SO2 q;
    q.cos_sin_ = Vector2<K>(std::cos(angle), std::sin(angle));
    return q;
  }

  static SO2 from_direction(const Vector2<K>& direction) {
    SO2 q;
    const K norm = direction.norm();
    q.cos_sin_ = direction / norm;
    return q;
  }

  static SO2 from_direction(K dx, K dy) { return from_direction(Vector2<K>(dx, dy)); }

  static SO2 from_normalized(const Vector2<K>& cos_sin) {
    SO2 q;
    q.cos_sin_ = cos_sin;
    return q;
  }

  static SO2 from_normalized(K cos_angle, K sin_angle) {
    return from_normalized(Vector2<K>(cos_angle, sin_angle));
  }

  K cos() const { return cos_sin_[0]; }

  K sin() const { return cos_sin_[1]; }

  const Vector2<K>& as_direction() const { return cos_sin_; }

  K angle() const { return std::atan2(sin(), cos()); }

  Matrix2<K> matrix() const {
    Matrix2<K> m;
    m(0, 0) = cos();
    m(0, 1) = -sin();
    m(1, 0) = sin();
    m(1, 1) = cos();
    return m;
  }

  SO2 inverse() const { return SO2::from_normalized(cos(), -sin()); }

  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>
  SO2<S> cast() const {
    // We need to re-normalize in the new type
    return SO2<S>::from_direction(as_direction().template cast<S>());
  }

  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>
  const SO2& cast() const {
    // Nothing to do as the type does not change
    return *this;
  }

  friend SO2 operator*(const SO2& lhs, const SO2& rhs) {
    return from_direction(lhs.cos() * rhs.cos() - lhs.sin() * rhs.sin(),
                          lhs.sin() * rhs.cos() + lhs.cos() * rhs.sin());
  }

  friend Vector2<K> operator*(const SO2& lhs, const Vector2<K>& vec) {
    return Vector2<K>(lhs.cos() * vec[0] - lhs.sin() * vec[1],
                      lhs.sin() * vec[0] + lhs.cos() * vec[1]);
  }

 private:
  Vector2<K> cos_sin_;
};

using SO2d = SO2<double>;
using SO2f = SO2<float>;

}  // namespace holoscan

#endif/* HOLOSCAN_POSE_TREE_MATH_SO2_HPP */