Construct n-dimensional rotation matrix.
This commit is contained in:
Rouven Spreckels
parent
8c6f14fcd0
commit
5e10563612
|
|
@ -69,6 +69,66 @@ where
|
|||
}
|
||||
}
|
||||
|
||||
/// # Rotation in any dimension
|
||||
impl<T, D> OMatrix<T, D, D>
|
||||
where
|
||||
T: RealField,
|
||||
D: DimName,
|
||||
DefaultAllocator: Allocator<T, D, D> + Allocator<T, D, U1> + Allocator<T, U1, D>,
|
||||
{
|
||||
/// The n-dimensional rotation matrix described by an oriented minor arc.
|
||||
///
|
||||
/// See [`Rotation::from_arc`](`crate::Rotation::from_arc`) for details.
|
||||
#[must_use]
|
||||
pub fn from_arc<SB, SC>(
|
||||
from: &Unit<Vector<T, D, SB>>,
|
||||
to: &Unit<Vector<T, D, SC>>,
|
||||
) -> Option<Self>
|
||||
where
|
||||
SB: Storage<T, D> + Clone,
|
||||
SC: Storage<T, D> + Clone,
|
||||
{
|
||||
let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to));
|
||||
let d = T::one() + ab;
|
||||
(d > T::default_epsilon().sqrt()).then(|| {
|
||||
let k = &(b * a.transpose() - a * b.transpose());
|
||||
// Codesido's Rotation Formula
|
||||
// <https://doi.org/10.14232/ejqtde.2018.1.13>
|
||||
//
|
||||
// Equals `Self::identity() + k + k * k / d`:
|
||||
let mut r = Self::identity() + k;
|
||||
r.gemm(d.recip(), k, k, T::one());
|
||||
r
|
||||
})
|
||||
}
|
||||
/// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle.
|
||||
///
|
||||
/// See [`Rotation::from_arc_angle`](`crate::Rotation::from_arc_angle`) for details.
|
||||
#[must_use]
|
||||
pub fn from_arc_angle<SB, SC>(
|
||||
from: &Unit<Vector<T, D, SB>>,
|
||||
to: &Unit<Vector<T, D, SC>>,
|
||||
angle: T,
|
||||
) -> Option<Self>
|
||||
where
|
||||
SB: Storage<T, D> + Clone,
|
||||
SC: Storage<T, D> + Clone,
|
||||
{
|
||||
let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to));
|
||||
// Tries making `b` orthonormal to `a` via Gram-Schmidt process.
|
||||
(b - a * ab)
|
||||
.try_normalize(T::default_epsilon().sqrt())
|
||||
.map(|ref b| {
|
||||
let (sin, cos) = angle.sin_cos();
|
||||
let [at, bt] = [&a.transpose(), &b.transpose()];
|
||||
let [k1, k2] = [b * at - a * bt, -(a * at + b * bt)];
|
||||
// Simple rotations / Rotation in a two–plane
|
||||
// <https://doi.org/10.48550/arXiv.1103.5263>
|
||||
Self::identity() + k1 * sin + k2 * (T::one() - cos)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// # 2D transformations as a Matrix3
|
||||
impl<T: RealField> Matrix3<T> {
|
||||
/// Builds a 2 dimensional homogeneous rotation matrix from an angle in radian.
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ pub type MatrixCross<T, R1, C1, R2, C2> =
|
|||
/// - [2D transformations as a Matrix3 <span style="float:right;">`new_rotation`…</span>](#2d-transformations-as-a-matrix3)
|
||||
/// - [3D transformations as a Matrix4 <span style="float:right;">`new_rotation`, `new_perspective`, `look_at_rh`…</span>](#3d-transformations-as-a-matrix4)
|
||||
/// - [Translation and scaling in any dimension <span style="float:right;">`new_scaling`, `new_translation`…</span>](#translation-and-scaling-in-any-dimension)
|
||||
/// - [Rotation in any dimension <span style="float:right;">`from_arc`, `from_arc_angle`</span>](#rotation-in-any-dimension)
|
||||
/// - [Append/prepend translation and scaling <span style="float:right;">`append_scaling`, `prepend_translation_mut`…</span>](#appendprepend-translation-and-scaling)
|
||||
/// - [Transformation of vectors and points <span style="float:right;">`transform_vector`, `transform_point`…</span>](#transformation-of-vectors-and-points)
|
||||
///
|
||||
|
|
|
|||
|
|
@ -22,14 +22,14 @@ use rkyv::bytecheck;
|
|||
|
||||
/// A rotation matrix.
|
||||
///
|
||||
/// This is also known as an element of a Special Orthogonal (SO) group.
|
||||
/// The `Rotation` type can either represent a 2D or 3D rotation, represented as a matrix.
|
||||
/// For a rotation based on quaternions, see [`UnitQuaternion`](crate::UnitQuaternion) instead.
|
||||
/// This is also known as an element of a Special Orthogonal (SO) group. The [`Rotation`] type can
|
||||
/// represent an n-dimensional rotation as a matrix. For a rotation based on quaternions, see
|
||||
/// [`UnitQuaternion`](crate::UnitQuaternion) instead.
|
||||
///
|
||||
/// Note that instead of using the [`Rotation`](crate::Rotation) type in your code directly, you should use one
|
||||
/// of its aliases: [`Rotation2`](crate::Rotation2), or [`Rotation3`](crate::Rotation3). Though
|
||||
/// keep in mind that all the documentation of all the methods of these aliases will also appears on
|
||||
/// this page.
|
||||
/// For fixed dimensions, you should use its aliases (e.g., [`Rotation2`](crate::Rotation2) or
|
||||
/// [`Rotation3`](crate::Rotation3)) instead of the dimension-generic [`Rotation`](crate::Rotation)
|
||||
/// type. Though keep in mind that all the documentation of all the methods of these aliases will
|
||||
/// also appears on this page.
|
||||
///
|
||||
/// # Construction
|
||||
/// * [Identity <span style="float:right;">`identity`</span>](#identity)
|
||||
|
|
@ -38,6 +38,7 @@ use rkyv::bytecheck;
|
|||
/// * [From a 3D axis and/or angles <span style="float:right;">`new`, `from_euler_angles`, `from_axis_angle`…</span>](#construction-from-a-3d-axis-andor-angles)
|
||||
/// * [From a 3D eye position and target point <span style="float:right;">`look_at`, `look_at_lh`, `rotation_between`…</span>](#construction-from-a-3d-eye-position-and-target-point)
|
||||
/// * [From an existing 3D matrix or rotations <span style="float:right;">`from_matrix`, `rotation_between`, `powf`…</span>](#construction-from-an-existing-3d-matrix-or-rotations)
|
||||
/// * [From an arc in any dimension <span style="float:right;">`from_arc`, `from_arc_angle`…</span>](#construction-in-any-dimension)
|
||||
///
|
||||
/// # Transformation and composition
|
||||
/// Note that transforming vectors and points can be done by multiplication, e.g., `rotation * point`.
|
||||
|
|
|
|||
|
|
@ -2,10 +2,25 @@ use crate::geometry::Rotation;
|
|||
|
||||
/// A 2-dimensional rotation matrix.
|
||||
///
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type too.**
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.**
|
||||
pub type Rotation2<T> = Rotation<T, 2>;
|
||||
|
||||
/// A 3-dimensional rotation matrix.
|
||||
///
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type too.**
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.**
|
||||
pub type Rotation3<T> = Rotation<T, 3>;
|
||||
|
||||
/// A 4-dimensional rotation matrix.
|
||||
///
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.**
|
||||
pub type Rotation4<T> = Rotation<T, 4>;
|
||||
|
||||
/// A 5-dimensional rotation matrix.
|
||||
///
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.**
|
||||
pub type Rotation5<T> = Rotation<T, 5>;
|
||||
|
||||
/// A 6-dimensional rotation matrix.
|
||||
///
|
||||
/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.**
|
||||
pub type Rotation6<T> = Rotation<T, 6>;
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
use num::{One, Zero};
|
||||
|
||||
use simba::scalar::{ClosedAdd, ClosedMul, SupersetOf};
|
||||
use simba::scalar::{ClosedAdd, ClosedMul, RealField, SupersetOf};
|
||||
|
||||
use crate::base::{SMatrix, Scalar};
|
||||
use crate::base::{storage::Storage, Const, SMatrix, Scalar, Unit, Vector};
|
||||
|
||||
use crate::geometry::Rotation;
|
||||
|
||||
|
|
@ -44,6 +44,99 @@ where
|
|||
}
|
||||
}
|
||||
|
||||
/// # Construction in any dimension
|
||||
impl<T: RealField, const D: usize> Rotation<T, D> {
|
||||
/// The n-dimensional rotation matrix described by an oriented minor arc.
|
||||
///
|
||||
/// This is the rotation `rot` aligning `from` with `to` over their minor angle such that
|
||||
/// `(rot * from).angle(to) == 0` and `(rot * from).dot(to).is_positive()`.
|
||||
///
|
||||
/// Returns `None` with `from` and `to` being anti-parallel. In contrast to
|
||||
/// [`Self::from_arc_angle`], this method is robust for approximately parallel vectors
|
||||
/// continuously approaching identity.
|
||||
///
|
||||
/// See also [`OMatrix::from_arc`](`crate::OMatrix::from_arc`) for owned matrices generic over
|
||||
/// storage.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```
|
||||
/// # #[macro_use] extern crate approx;
|
||||
/// # use nalgebra::{Rotation, Unit, Vector6};
|
||||
/// let from = Unit::new_normalize(Vector6::new(-4.0, -2.4, 0.0, -3.3, -1.0, -9.0));
|
||||
/// let to = Unit::new_normalize(Vector6::new(3.0, 1.0, 2.0, 2.0, 9.0, 6.0));
|
||||
///
|
||||
/// // Aligns `from` with `to`.
|
||||
/// let rot = Rotation::from_arc(&from, &to).unwrap();
|
||||
/// assert_relative_eq!(rot * from, to, epsilon = 1.0e-6);
|
||||
/// assert_relative_eq!(rot.inverse() * to, from, epsilon = 1.0e-6);
|
||||
///
|
||||
/// // Returns identity with `from` and `to` being parallel.
|
||||
/// let rot = Rotation::from_arc(&from, &from).unwrap();
|
||||
/// assert_relative_eq!(rot, Rotation::identity(), epsilon = 1.0e-6);
|
||||
///
|
||||
/// // Returns `None` with `from` and `to` being anti-parallel.
|
||||
/// assert!(Rotation::from_arc(&from, &-from).is_none());
|
||||
/// ```
|
||||
#[must_use]
|
||||
#[inline]
|
||||
pub fn from_arc<SB, SC>(
|
||||
from: &Unit<Vector<T, Const<D>, SB>>,
|
||||
to: &Unit<Vector<T, Const<D>, SC>>,
|
||||
) -> Option<Self>
|
||||
where
|
||||
T: RealField,
|
||||
SB: Storage<T, Const<D>> + Clone,
|
||||
SC: Storage<T, Const<D>> + Clone,
|
||||
{
|
||||
SMatrix::from_arc(from, to).map(Self::from_matrix_unchecked)
|
||||
}
|
||||
/// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle.
|
||||
///
|
||||
/// Returns `None` with `from` and `to` being collinear. This method is more robust, the less
|
||||
/// `from` and `to` are collinear, regardless of `angle`.
|
||||
///
|
||||
/// See also [`Self::from_arc`] aligning `from` with `to` over their minor angle.
|
||||
///
|
||||
/// See also [`OMatrix::from_arc_angle`](`crate::OMatrix::from_arc_angle`) for owned matrices
|
||||
/// generic over storage.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```
|
||||
/// # #[macro_use] extern crate approx;
|
||||
/// # use nalgebra::{Rotation, Unit, Vector6};
|
||||
/// let from = Unit::new_normalize(Vector6::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0));
|
||||
/// let to = Unit::new_normalize(Vector6::new(3.0, 1.0, 2.0, 5.0, 9.0, 4.0));
|
||||
///
|
||||
/// // Rotates by signed angle where `from` and `to` define its orientation.
|
||||
/// let angle = 70f64.to_radians();
|
||||
/// let rot = Rotation::from_arc_angle(&from, &to, angle).unwrap();
|
||||
/// assert_relative_eq!((rot * from).angle(&from), angle, epsilon = 1.0e-6);
|
||||
/// assert_relative_eq!((rot.inverse() * to).angle(&to), angle, epsilon = 1.0e-6);
|
||||
/// let inv = Rotation::from_arc_angle(&from, &to, -angle).unwrap();
|
||||
/// assert_relative_eq!(rot.inverse(), inv, epsilon = 1.0e-6);
|
||||
///
|
||||
/// // Returns `None` with `from` and `to` being collinear.
|
||||
/// assert!(Rotation::from_arc_angle(&from, &from, angle).is_none());
|
||||
/// assert!(Rotation::from_arc_angle(&from, &-from, angle).is_none());
|
||||
/// ```
|
||||
#[must_use]
|
||||
#[inline]
|
||||
pub fn from_arc_angle<SB, SC>(
|
||||
from: &Unit<Vector<T, Const<D>, SB>>,
|
||||
to: &Unit<Vector<T, Const<D>, SC>>,
|
||||
angle: T,
|
||||
) -> Option<Self>
|
||||
where
|
||||
T: RealField,
|
||||
SB: Storage<T, Const<D>> + Clone,
|
||||
SC: Storage<T, Const<D>> + Clone,
|
||||
{
|
||||
SMatrix::from_arc_angle(from, to, angle).map(Self::from_matrix_unchecked)
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: Scalar, const D: usize> Rotation<T, D> {
|
||||
/// Cast the components of `self` to another type.
|
||||
///
|
||||
|
|
|
|||
Loading…
Reference in New Issue