tmaq
/
tornavis
1
0
Fork 0
Code Issues 2 Pull Requests 29 Packages Projects 24 Releases Wiki Activity
tornavis/source/blender/blenlib/BLI_math_rotation.hh

487 lines
18 KiB
C++
Raw Blame History

/* SPDX-FileCopyrightText: 2023 Blender Authors
*
* SPDX-License-Identifier: GPL-2.0-or-later */
#pragma once
/** \file
* \ingroup bli
*/
#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 {
/* -------------------------------------------------------------------- */
/** \name Rotation helpers
* \{ */
/**
* Rotate \a a by \a b. In other word, insert the \a b rotation before \a a.
*
* \note Since \a a is a #Quaternion it will cast \a b to a #Quaternion.
* This might introduce some precision loss and have performance implication.
*/
template<typename T, typename RotT>
[[nodiscard]] QuaternionBase<T> rotate(const QuaternionBase<T> &a, const RotT &b);
/**
* Rotate \a a by \a b. In other word, insert the \a b rotation before \a a.
*
* \note Since \a a is an #AxisAngle it will cast both \a a and \a b to #Quaternion.
* This might introduce some precision loss and have performance implication.
*/
template<typename T, typename RotT, typename AngleT>
[[nodiscard]] AxisAngleBase<T, AngleT> rotate(const AxisAngleBase<T, AngleT> &a, const RotT &b);
/**
* Rotate \a a by \a b. In other word, insert the \a b rotation before \a a.
*
* \note Since \a a is an #EulerXYZ it will cast both \a a and \a b to #MatBase<T, 3, 3>.
* This might introduce some precision loss and have performance implication.
*/
template<typename T, typename RotT>
[[nodiscard]] EulerXYZBase<T> rotate(const EulerXYZBase<T> &a, const RotT &b);
/**
* Rotate \a a by \a b. In other word, insert the \a b rotation before \a a.
*
* \note Since \a a is an #Euler3 it will cast both \a a and \a b to #MatBase<T, 3, 3>.
* This might introduce some precision loss and have performance implication.
*/
template<typename T, typename RotT>
[[nodiscard]] Euler3Base<T> rotate(const Euler3Base<T> &a, const RotT &b);
/**
* Return rotation from orientation \a a to orientation \a b into another quaternion.
*/
template<typename T>
[[nodiscard]] QuaternionBase<T> rotation_between(const QuaternionBase<T> &a,
const QuaternionBase<T> &b);
/**
* Create a orientation from a triangle plane and the axis formed by the segment(v1, v2).
* Takes pre-computed \a normal from the triangle.
* Used for Ngons when their normal is known.
*/
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);
/**
* Create a orientation from a triangle plane and the axis formed by the segment(v1, v2).
*/
template<typename T>
[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
const VecBase<T, 3> &v2,
const VecBase<T, 3> &v3);
/**
* Create a rotation from a vector and a basis rotation.
* Used for tracking.
* \a track_flag is supposed to be #Object.trackflag
* \a up_flag is supposed to be #Object.upflag
*/
template<typename T>
[[nodiscard]] QuaternionBase<T> from_vector(const VecBase<T, 3> &vector,
const AxisSigned track_flag,
const Axis up_flag);
/**
* Returns a quaternion for converting local space to tracking space.
* This is slightly different than from_axis_conversion for legacy reasons.
*/
template<typename T>
[[nodiscard]] QuaternionBase<T> from_tracking(AxisSigned forward_axis, Axis up_axis);
/**
* Convert euler rotation to gimbal rotation matrix.
*/
template<typename T> [[nodiscard]] MatBase<T, 3, 3> to_gimbal_axis(const Euler3Base<T> &rotation);
/** \} */
/* -------------------------------------------------------------------- */
/** \name Angles
* \{ */
/**
* Extract rotation angle from a unit quaternion.
* Returned angle is in [0..2pi] range.
*
* Unlike the angle between vectors, this does *NOT* return the shortest angle.
* See `angle_of_signed` below for this.
*/
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);
}
/**
* Extract rotation angle from a unit quaternion. Always return the shortest angle.
* Returned angle is in [-pi..pi] range.
*
* `angle_of` with quaternion can exceed PI radians. Having signed versions of these functions
* allows to use 'abs(angle_of_signed(...))' to get the shortest angle between quaternions with
* higher precision than subtracting 2pi afterwards.
*/
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));
}
/**
* Extract angle between 2 orientations.
* For #Quaternion, the returned angle is in [0..2pi] range.
* For other types, the returned angle is in [0..pi] range.
* See `angle_of` for more detail.
*/
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;
}
/**
* Extract angle between 2 orientations.
* Returned angle is in [-pi..pi] range.
* See `angle_of_signed` for more detail.
*/
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));
}
/** \} */
/* -------------------------------------------------------------------- */
/** \name Template implementations
* \{ */
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;
}
/** \} */
/* -------------------------------------------------------------------- */
/** \name Conversion from Cartesian Basis
* \{ */
/**
* Creates a quaternion from an axis triple.
* This is faster and more precise than converting from another representation.
*/
template<typename T> QuaternionBase<T> to_quaternion(const CartesianBasis &rotation)
{
/**
* There are only 6 * 4 = 24 possible valid orthonormal orientations.
* We precompute them and store them inside this switch using a key.
* Generated using `generate_axes_to_quaternion_switch_cases()`.
*/
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(M_SQRT1_2), T(0), T(0), T(-M_SQRT1_2)};
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(M_SQRT1_2), T(M_SQRT1_2), T(0)};
case map(AxisSigned::Z_NEG, AxisSigned::Y_POS, AxisSigned::X_POS):
return QuaternionBase<T>{T(M_SQRT1_2), T(0), T(M_SQRT1_2), T(0)};
case map(AxisSigned::Z_POS, AxisSigned::Y_POS, AxisSigned::X_NEG):
return QuaternionBase<T>{T(M_SQRT1_2), T(0), T(-M_SQRT1_2), 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(M_SQRT1_2), T(M_SQRT1_2)};
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(M_SQRT1_2), T(M_SQRT1_2), 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(M_SQRT1_2), T(0), T(0), T(M_SQRT1_2)};
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(-M_SQRT1_2), T(M_SQRT1_2), T(0)};
case map(AxisSigned::Z_POS, AxisSigned::Y_NEG, AxisSigned::X_POS):
return QuaternionBase<T>{T(0), T(M_SQRT1_2), T(0), T(M_SQRT1_2)};
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(-M_SQRT1_2), T(0), T(M_SQRT1_2)};
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(M_SQRT1_2), T(-M_SQRT1_2), 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(-M_SQRT1_2), T(M_SQRT1_2)};
}
}
/** \} */
} // namespace blender::math
namespace blender::math {
/* -------------------------------------------------------------------- */
/** \name Explicit Template Instantiations
* \{ */
/* 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
Reference in New Issue View Git Blame Copy Permalink