BLI_math_rotation.hh

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2023 Blender Authors
 *
 * SPDX-License-Identifier: GPL-2.0-or-later */
 
#pragma once
 
#include "BLI_math_rotation_types.hh"
 
#include "BLI_math_axis_angle.hh"
#include "BLI_math_euler.hh"
#include "BLI_math_matrix.hh"
#include "BLI_math_quaternion.hh"
#include "BLI_math_vector.hh"
 
namespace blender::math {
 
/* -------------------------------------------------------------------- */
template<typename T, typename RotT>
[[nodiscard]] QuaternionBase<T> rotate(const QuaternionBase<T> &a, const RotT &b);
 
template<typename T, typename RotT, typename AngleT>
[[nodiscard]] AxisAngleBase<T, AngleT> rotate(const AxisAngleBase<T, AngleT> &a, const RotT &b);
 
template<typename T, typename RotT>
[[nodiscard]] EulerXYZBase<T> rotate(const EulerXYZBase<T> &a, const RotT &b);
 
template<typename T, typename RotT>
[[nodiscard]] Euler3Base<T> rotate(const Euler3Base<T> &a, const RotT &b);
 
template<typename T>
[[nodiscard]] QuaternionBase<T> rotation_between(const QuaternionBase<T> &a,
                                                 const QuaternionBase<T> &b);
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
                                              const VecBase<T, 3> &v2,
                                              const VecBase<T, 3> &v3,
                                              const VecBase<T, 3> &normal);
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
                                              const VecBase<T, 3> &v2,
                                              const VecBase<T, 3> &v3);
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_vector(const VecBase<T, 3> &vector,
                                            const AxisSigned track_flag,
                                            const Axis up_flag);
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_tracking(AxisSigned forward_axis, Axis up_axis);
 
template<typename T> [[nodiscard]] MatBase<T, 3, 3> to_gimbal_axis(const Euler3Base<T> &rotation);
 
/* -------------------------------------------------------------------- */
template<typename T> [[nodiscard]] AngleRadianBase<T> angle_of(const QuaternionBase<T> &q)
{
  BLI_assert(is_unit_scale(q));
  return T(2) * math::safe_acos(q.w);
}
 
template<typename T> [[nodiscard]] AngleRadianBase<T> angle_of_signed(const QuaternionBase<T> &q)
{
  BLI_assert(is_unit_scale(q));
  return T(2) * ((q.w >= T(0)) ? math::safe_acos(q.w) : -math::safe_acos(-q.w));
}
 
template<typename T>
[[nodiscard]] AngleRadianBase<T> angle_between(const QuaternionBase<T> &a,
                                               const QuaternionBase<T> &b)
{
  return angle_of(rotation_between(a, b));
}
template<typename T>
[[nodiscard]] AngleRadianBase<T> angle_between(const VecBase<T, 3> &a, const VecBase<T, 3> &b)
{
  BLI_assert(is_unit_scale(a));
  BLI_assert(is_unit_scale(b));
  return math::safe_acos(dot(a, b));
}
template<typename T>
[[nodiscard]] AngleFraction<T> angle_between(const AxisSigned a, const AxisSigned b)
{
  if (a == b) {
    return AngleFraction<T>::identity();
  }
  if (abs(a) == abs(b)) {
    return AngleFraction<T>::pi();
  }
  return AngleFraction<T>::pi() / 2;
}
 
template<typename T>
[[nodiscard]] AngleRadianBase<T> angle_between_signed(const QuaternionBase<T> &a,
                                                      const QuaternionBase<T> &b)
{
  return angle_of_signed(rotation_between(a, b));
}
 
/* -------------------------------------------------------------------- */
template<typename T, typename RotT>
[[nodiscard]] QuaternionBase<T> rotate(const QuaternionBase<T> &a, const RotT &b)
{
  return a * QuaternionBase<T>(b);
}
 
template<typename T, typename RotT, typename AngleT>
[[nodiscard]] AxisAngleBase<T, AngleT> rotate(const AxisAngleBase<T, AngleT> &a, const RotT &b)
{
  return AxisAngleBase<T, AngleT>(QuaternionBase<T>(a) * QuaternionBase<T>(b));
}
 
template<typename T, typename RotT>
[[nodiscard]] EulerXYZBase<T> rotate(const EulerXYZBase<T> &a, const RotT &b)
{
  MatBase<T, 3, 3> tmp = from_rotation<MatBase<T, 3, 3>>(a) * from_rotation<MatBase<T, 3, 3>>(b);
  return to_euler(tmp);
}
 
template<typename T, typename RotT>
[[nodiscard]] Euler3Base<T> rotate(const Euler3Base<T> &a, const RotT &b)
{
  const MatBase<T, 3, 3> tmp = from_rotation<MatBase<T, 3, 3>>(a) *
                               from_rotation<MatBase<T, 3, 3>>(b);
  return to_euler(tmp, a.order());
}
 
template<typename T>
[[nodiscard]] QuaternionBase<T> rotation_between(const QuaternionBase<T> &a,
                                                 const QuaternionBase<T> &b)
{
  return invert(a) * b;
}
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
                                              const VecBase<T, 3> &v2,
                                              const VecBase<T, 3> &v3,
                                              const VecBase<T, 3> &normal)
{
  /* Force to used an unused var to avoid the same function signature as the version without
   * `normal` argument. */
  UNUSED_VARS(v3);
 
  using Vec3T = VecBase<T, 3>;
 
  /* Move z-axis to face-normal. */
  const Vec3T z_axis = normal;
  Vec3T nor = normalize(Vec3T(z_axis.y, -z_axis.x, T(0)));
  if (is_zero(nor.xy())) {
    nor.x = T(1);
  }
 
  T angle = T(-0.5) * math::safe_acos(z_axis.z);
  T si = math::sin(angle);
  QuaternionBase<T> q1(math::cos(angle), nor.x * si, nor.y * si, T(0));
 
  /* Rotate back line v1-v2. */
  Vec3T line = transform_point(conjugate(q1), (v2 - v1));
  /* What angle has this line with x-axis? */
  line = normalize(Vec3T(line.x, line.y, T(0)));
 
  angle = T(0.5) * math::atan2(line.y, line.x);
  QuaternionBase<T> q2(math::cos(angle), 0.0, 0.0, math::sin(angle));
 
  return q1 * q2;
}
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
                                              const VecBase<T, 3> &v2,
                                              const VecBase<T, 3> &v3)
{
  return from_triangle(v1, v2, v3, normal_tri(v1, v2, v3));
}
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_vector(const VecBase<T, 3> &vector,
                                            const AxisSigned track_flag,
                                            const Axis up_flag)
{
  using Vec2T = VecBase<T, 2>;
  using Vec3T = VecBase<T, 3>;
  using Vec4T = VecBase<T, 4>;
 
  const T vec_len = length(vector);
 
  if (UNLIKELY(vec_len == 0.0f)) {
    return QuaternionBase<T>::identity();
  }
 
  const Axis axis = track_flag.axis();
  const Vec3T vec = track_flag.is_negative() ? vector : -vector;
 
  Vec3T rotation_axis;
  constexpr T eps = T(1e-4);
  T axis_len;
  switch (axis) {
    case Axis::X:
      rotation_axis = normalize_and_get_length(Vec3T(T(0), -vec.z, vec.y), axis_len);
      if (axis_len < eps) {
        rotation_axis = Vec3T(0, 1, 0);
      }
      break;
    case Axis::Y:
      rotation_axis = normalize_and_get_length(Vec3T(vec.z, T(0), -vec.x), axis_len);
      if (axis_len < eps) {
        rotation_axis = Vec3T(0, 0, 1);
      }
      break;
    default:
    case Axis::Z:
      rotation_axis = normalize_and_get_length(Vec3T(-vec.y, vec.x, T(0)), axis_len);
      if (axis_len < eps) {
        rotation_axis = Vec3T(1, 0, 0);
      }
      break;
  }
  /* TODO(fclem): Can optimize here by initializing AxisAngle using the cos an sin directly.
   * Avoiding the need for safe_acos and deriving sin from cos. */
  const T rotation_angle = math::safe_acos(vec[axis.as_int()] / vec_len);
 
  const QuaternionBase<T> q1 = to_quaternion(
      AxisAngleBase<T, AngleRadianBase<T>>(rotation_axis, rotation_angle));
 
  if (axis == up_flag) {
    /* Nothing else to do. */
    return q1;
  }
 
  /* Extract rotation between the up axis of the rotated space and the up axis. */
  /* There might be an easier way to get this angle directly from the quaternion representation. */
  const Vec3T rotated_up = transform_point(q1, Vec3T(0, 0, 1));
 
  /* Project using axes index instead of arithmetic. It's much faster and more precise. */
  const AxisSigned y_axis_signed = math::cross(AxisSigned(axis), AxisSigned(up_flag));
  const Axis x_axis = up_flag;
  const Axis y_axis = y_axis_signed.axis();
 
  Vec2T projected = normalize(Vec2T(rotated_up[x_axis.as_int()], rotated_up[y_axis.as_int()]));
  /* Flip sign for flipped axis. */
  if (y_axis_signed.is_negative()) {
    projected.y = -projected.y;
  }
  /* Not sure if this was a bug or not in the previous implementation.
   * Carry over this weird behavior to avoid regressions. */
  if (axis == Axis::Z) {
    projected = -projected;
  }
 
  const AngleCartesianBase<T> angle(projected.x, projected.y);
  const AngleCartesianBase<T> half_angle = angle / T(2);
 
  const QuaternionBase<T> q2(Vec4T(half_angle.cos(), vec * (half_angle.sin() / vec_len)));
 
  return q2 * q1;
}
 
template<typename T>
[[nodiscard]] QuaternionBase<T> from_tracking(AxisSigned forward_axis, Axis up_axis)
{
  BLI_assert(forward_axis.axis() != up_axis);
 
  /* Curve have Z forward, Y up, X left. */
  return QuaternionBase<T>(
      rotation_between(from_orthonormal_axes(AxisSigned::Z_POS, AxisSigned::Y_POS),
                       from_orthonormal_axes(forward_axis, AxisSigned(up_axis))));
}
 
template<typename T> [[nodiscard]] MatBase<T, 3, 3> to_gimbal_axis(const Euler3Base<T> &rotation)
{
  using Mat3T = MatBase<T, 3, 3>;
  using Vec3T = VecBase<T, 3>;
  const int i_index = rotation.i_index();
  const int j_index = rotation.j_index();
  const int k_index = rotation.k_index();
 
  Mat3T result;
  /* First axis is local. */
  result[i_index] = from_rotation<Mat3T>(rotation)[i_index];
  /* Second axis is local minus first rotation. */
  Euler3Base<T> tmp_rot = rotation;
  tmp_rot.i() = T(0);
  result[j_index] = from_rotation<Mat3T>(tmp_rot)[j_index];
  /* Last axis is global. */
  result[k_index] = Vec3T(0);
  result[k_index][k_index] = T(1);
 
  return result;
}
 
/* -------------------------------------------------------------------- */
template<typename T> QuaternionBase<T> to_quaternion(const CartesianBasis &rotation)
{
  constexpr auto map = [](AxisSigned x, AxisSigned y, AxisSigned z) {
    return x.as_int() << 16 | y.as_int() << 8 | z.as_int();
  };
 
  switch (map(rotation.axes.x, rotation.axes.y, rotation.axes.z)) {
    default:
      return QuaternionBase<T>::identity();
    case map(AxisSigned::Z_POS, AxisSigned::X_POS, AxisSigned::Y_POS):
      return QuaternionBase<T>{T(0.5), T(-0.5), T(-0.5), T(-0.5)};
    case map(AxisSigned::Y_NEG, AxisSigned::X_POS, AxisSigned::Z_POS):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(0), T(0), T(-rcp(numbers::sqrt2))};
    case map(AxisSigned::Z_NEG, AxisSigned::X_POS, AxisSigned::Y_NEG):
      return QuaternionBase<T>{T(0.5), T(0.5), T(0.5), T(-0.5)};
    case map(AxisSigned::Y_POS, AxisSigned::X_POS, AxisSigned::Z_NEG):
      return QuaternionBase<T>{T(0), T(rcp(numbers::sqrt2)), T(rcp(numbers::sqrt2)), T(0)};
    case map(AxisSigned::Z_NEG, AxisSigned::Y_POS, AxisSigned::X_POS):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(0), T(rcp(numbers::sqrt2)), T(0)};
    case map(AxisSigned::Z_POS, AxisSigned::Y_POS, AxisSigned::X_NEG):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(0), T(-rcp(numbers::sqrt2)), T(0)};
    case map(AxisSigned::X_NEG, AxisSigned::Y_POS, AxisSigned::Z_NEG):
      return QuaternionBase<T>{T(0), T(0), T(1), T(0)};
    case map(AxisSigned::Y_POS, AxisSigned::Z_POS, AxisSigned::X_POS):
      return QuaternionBase<T>{T(0.5), T(0.5), T(0.5), T(0.5)};
    case map(AxisSigned::X_NEG, AxisSigned::Z_POS, AxisSigned::Y_POS):
      return QuaternionBase<T>{T(0), T(0), T(rcp(numbers::sqrt2)), T(rcp(numbers::sqrt2))};
    case map(AxisSigned::Y_NEG, AxisSigned::Z_POS, AxisSigned::X_NEG):
      return QuaternionBase<T>{T(0.5), T(0.5), T(-0.5), T(-0.5)};
    case map(AxisSigned::X_POS, AxisSigned::Z_POS, AxisSigned::Y_NEG):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(rcp(numbers::sqrt2)), T(0), T(0)};
    case map(AxisSigned::Z_NEG, AxisSigned::X_NEG, AxisSigned::Y_POS):
      return QuaternionBase<T>{T(0.5), T(-0.5), T(0.5), T(0.5)};
    case map(AxisSigned::Y_POS, AxisSigned::X_NEG, AxisSigned::Z_POS):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(0), T(0), T(rcp(numbers::sqrt2))};
    case map(AxisSigned::Z_POS, AxisSigned::X_NEG, AxisSigned::Y_NEG):
      return QuaternionBase<T>{T(0.5), T(0.5), T(-0.5), T(0.5)};
    case map(AxisSigned::Y_NEG, AxisSigned::X_NEG, AxisSigned::Z_NEG):
      return QuaternionBase<T>{T(0), T(-rcp(numbers::sqrt2)), T(rcp(numbers::sqrt2)), T(0)};
    case map(AxisSigned::Z_POS, AxisSigned::Y_NEG, AxisSigned::X_POS):
      return QuaternionBase<T>{T(0), T(rcp(numbers::sqrt2)), T(0), T(rcp(numbers::sqrt2))};
    case map(AxisSigned::X_NEG, AxisSigned::Y_NEG, AxisSigned::Z_POS):
      return QuaternionBase<T>{T(0), T(0), T(0), T(1)};
    case map(AxisSigned::Z_NEG, AxisSigned::Y_NEG, AxisSigned::X_NEG):
      return QuaternionBase<T>{T(0), T(-rcp(numbers::sqrt2)), T(0), T(rcp(numbers::sqrt2))};
    case map(AxisSigned::X_POS, AxisSigned::Y_NEG, AxisSigned::Z_NEG):
      return QuaternionBase<T>{T(0), T(1), T(0), T(0)};
    case map(AxisSigned::Y_NEG, AxisSigned::Z_NEG, AxisSigned::X_POS):
      return QuaternionBase<T>{T(0.5), T(-0.5), T(0.5), T(-0.5)};
    case map(AxisSigned::X_POS, AxisSigned::Z_NEG, AxisSigned::Y_POS):
      return QuaternionBase<T>{T(rcp(numbers::sqrt2)), T(-rcp(numbers::sqrt2)), T(0), T(0)};
    case map(AxisSigned::Y_POS, AxisSigned::Z_NEG, AxisSigned::X_NEG):
      return QuaternionBase<T>{T(0.5), T(-0.5), T(-0.5), T(0.5)};
    case map(AxisSigned::X_NEG, AxisSigned::Z_NEG, AxisSigned::Y_NEG):
      return QuaternionBase<T>{T(0), T(0), T(-rcp(numbers::sqrt2)), T(rcp(numbers::sqrt2))};
  }
}
 
}  // namespace blender::math
 
namespace blender::math {
 
/* -------------------------------------------------------------------- */
/* Using explicit template instantiations in order to reduce compilation time. */
extern template EulerXYZ to_euler(const AxisAngle &);
extern template EulerXYZ to_euler(const AxisAngleCartesian &);
extern template EulerXYZ to_euler(const Quaternion &);
extern template Euler3 to_euler(const AxisAngle &, EulerOrder);
extern template Euler3 to_euler(const AxisAngleCartesian &, EulerOrder);
extern template Euler3 to_euler(const Quaternion &, EulerOrder);
extern template Quaternion to_quaternion(const AxisAngle &);
extern template Quaternion to_quaternion(const AxisAngleCartesian &);
extern template Quaternion to_quaternion(const Euler3 &);
extern template Quaternion to_quaternion(const EulerXYZ &);
extern template AxisAngleCartesian to_axis_angle(const Euler3 &);
extern template AxisAngleCartesian to_axis_angle(const EulerXYZ &);
extern template AxisAngleCartesian to_axis_angle(const Quaternion &);
extern template AxisAngle to_axis_angle(const Euler3 &);
extern template AxisAngle to_axis_angle(const EulerXYZ &);
extern template AxisAngle to_axis_angle(const Quaternion &);
 
}  // namespace blender::math