15 刚体变换模块(rigid.rs)

rigid.rs是一个表示三维刚体变换(Rigid Transformation)的结构体定义,用于在计算机图形学、机器人学以及物理模拟等领域中表示物体在三维空间中的旋转和平移。在这个定义中,所有长度在变换后都保持不变,这是刚体变换的一个基本特性。

一、rigid.rs源码

//! All matrix multiplication in this module is in row-vector notation,
//! i.e. a vector `v` is transformed with `v * T`, and if you want to apply `T1`
//! before `T2` you use `T1 * T2`

use crate::approxeq::ApproxEq;
use crate::trig::Trig;
use crate::{Rotation3D, Transform3D, UnknownUnit, Vector3D};

use core::{fmt, hash};

#[cfg(feature = "bytemuck")]
use bytemuck::{Pod, Zeroable};
use num_traits::real::Real;
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};

/// A rigid transformation. All lengths are preserved under such a transformation.
///
///
/// Internally, this is a rotation and a translation, with the rotation
/// applied first (i.e. `Rotation * Translation`, in row-vector notation)
///
/// This can be more efficient to use over full matrices, especially if you
/// have to deal with the decomposed quantities often.
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[repr(C)]
pub struct RigidTransform3D<T, Src, Dst> {
    pub rotation: Rotation3D<T, Src, Dst>,
    pub translation: Vector3D<T, Dst>,
}

impl<T, Src, Dst> RigidTransform3D<T, Src, Dst> {
    /// Construct a new rigid transformation, where the `rotation` applies first
    #[inline]
    pub const fn new(rotation: Rotation3D<T, Src, Dst>, translation: Vector3D<T, Dst>) -> Self {
        Self {
            rotation,
            translation,
        }
    }
}

impl<T: Copy, Src, Dst> RigidTransform3D<T, Src, Dst> {
    pub fn cast_unit<Src2, Dst2>(&self) -> RigidTransform3D<T, Src2, Dst2> {
        RigidTransform3D {
            rotation: self.rotation.cast_unit(),
            translation: self.translation.cast_unit(),
        }
    }
}

impl<T: Real + ApproxEq<T>, Src, Dst> RigidTransform3D<T, Src, Dst> {
    /// Construct an identity transform
    #[inline]
    pub fn identity() -> Self {
        Self {
            rotation: Rotation3D::identity(),
            translation: Vector3D::zero(),
        }
    }

    /// Construct a new rigid transformation, where the `translation` applies first
    #[inline]
    pub fn new_from_reversed(
        translation: Vector3D<T, Src>,
        rotation: Rotation3D<T, Src, Dst>,
    ) -> Self {
        // T * R
        //   = (R * R^-1) * T * R
        //   = R * (R^-1 * T * R)
        //   = R * T'
        //
        // T' = (R^-1 * T * R) is also a translation matrix
        // It is equivalent to the translation matrix obtained by rotating the
        // translation by R

        let translation = rotation.transform_vector3d(translation);
        Self {
            rotation,
            translation,
        }
    }

    #[inline]
    pub fn from_rotation(rotation: Rotation3D<T, Src, Dst>) -> Self {
        Self {
            rotation,
            translation: Vector3D::zero(),
        }
    }

    #[inline]
    pub fn from_translation(translation: Vector3D<T, Dst>) -> Self {
        Self {
            translation,
            rotation: Rotation3D::identity(),
        }
    }

    /// Decompose this into a translation and an rotation to be applied in the opposite order
    ///
    /// i.e., the translation is applied _first_
    #[inline]
    pub fn decompose_reversed(&self) -> (Vector3D<T, Src>, Rotation3D<T, Src, Dst>) {
        // self = R * T
        //      = R * T * (R^-1 * R)
        //      = (R * T * R^-1) * R)
        //      = T' * R
        //
        // T' = (R^ * T * R^-1) is T rotated by R^-1

        let translation = self.rotation.inverse().transform_vector3d(self.translation);
        (translation, self.rotation)
    }

    /// Returns the multiplication of the two transforms such that
    /// other's transformation applies after self's transformation.
    ///
    /// i.e., this produces `self * other` in row-vector notation
    #[inline]
    pub fn then<Dst2>(
        &self,
        other: &RigidTransform3D<T, Dst, Dst2>,
    ) -> RigidTransform3D<T, Src, Dst2> {
        // self = R1 * T1
        // other = R2 * T2
        // result = R1 * T1 * R2 * T2
        //        = R1 * (R2 * R2^-1) * T1 * R2 * T2
        //        = (R1 * R2) * (R2^-1 * T1 * R2) * T2
        //        = R' * T' * T2
        //        = R' * T''
        //
        // (R2^-1 * T2 * R2^) = T' = T2 rotated by R2
        // R1 * R2  = R'
        // T' * T2 = T'' = vector addition of translations T2 and T'

        let t_prime = other.rotation.transform_vector3d(self.translation);
        let r_prime = self.rotation.then(&other.rotation);
        let t_prime2 = t_prime + other.translation;
        RigidTransform3D {
            rotation: r_prime,
            translation: t_prime2,
        }
    }

    /// Inverts the transformation
    #[inline]
    pub fn inverse(&self) -> RigidTransform3D<T, Dst, Src> {
        // result = (self)^-1
        //        = (R * T)^-1
        //        = T^-1 * R^-1
        //        = (R^-1 * R) * T^-1 * R^-1
        //        = R^-1 * (R * T^-1 * R^-1)
        //        = R' * T'
        //
        // T' = (R * T^-1 * R^-1) = (-T) rotated by R^-1
        // R' = R^-1
        //
        // An easier way of writing this is to use new_from_reversed() with R^-1 and T^-1

        RigidTransform3D::new_from_reversed(-self.translation, self.rotation.inverse())
    }

    pub fn to_transform(&self) -> Transform3D<T, Src, Dst>
    where
        T: Trig,
    {
        self.rotation
            .to_transform()
            .then(&self.translation.to_transform())
    }

    /// Drop the units, preserving only the numeric value.
    #[inline]
    pub fn to_untyped(&self) -> RigidTransform3D<T, UnknownUnit, UnknownUnit> {
        RigidTransform3D {
            rotation: self.rotation.to_untyped(),
            translation: self.translation.to_untyped(),
        }
    }

    /// Tag a unitless value with units.
    #[inline]
    pub fn from_untyped(transform: &RigidTransform3D<T, UnknownUnit, UnknownUnit>) -> Self {
        RigidTransform3D {
            rotation: Rotation3D::from_untyped(&transform.rotation),
            translation: Vector3D::from_untyped(transform.translation),
        }
    }
}

impl<T: fmt::Debug, Src, Dst> fmt::Debug for RigidTransform3D<T, Src, Dst> {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        f.debug_struct("RigidTransform3D")
            .field("rotation", &self.rotation)
            .field("translation", &self.translation)
            .finish()
    }
}

impl<T: PartialEq, Src, Dst> PartialEq for RigidTransform3D<T, Src, Dst> {
    fn eq(&self, other: &Self) -> bool {
        self.rotation == other.rotation && self.translation == other.translation
    }
}
impl<T: Eq, Src, Dst> Eq for RigidTransform3D<T, Src, Dst> {}

impl<T: hash::Hash, Src, Dst> hash::Hash for RigidTransform3D<T, Src, Dst> {
    fn hash<H: hash::Hasher>(&self, state: &mut H) {
        self.rotation.hash(state);
        self.translation.hash(state);
    }
}

impl<T: Copy, Src, Dst> Copy for RigidTransform3D<T, Src, Dst> {}

impl<T: Clone, Src, Dst> Clone for RigidTransform3D<T, Src, Dst> {
    fn clone(&self) -> Self {
        RigidTransform3D {
            rotation: self.rotation.clone(),
            translation: self.translation.clone(),
        }
    }
}

#[cfg(feature = "arbitrary")]
impl<'a, T, Src, Dst> arbitrary::Arbitrary<'a> for RigidTransform3D<T, Src, Dst>
where
    T: arbitrary::Arbitrary<'a>,
{
    fn arbitrary(u: &mut arbitrary::Unstructured<'a>) -> arbitrary::Result<Self> {
        Ok(RigidTransform3D {
            rotation: arbitrary::Arbitrary::arbitrary(u)?,
            translation: arbitrary::Arbitrary::arbitrary(u)?,
        })
    }
}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Zeroable, Src, Dst> Zeroable for RigidTransform3D<T, Src, Dst> {}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Pod, Src: 'static, Dst: 'static> Pod for RigidTransform3D<T, Src, Dst> {}

impl<T: Real + ApproxEq<T>, Src, Dst> From<Rotation3D<T, Src, Dst>>
    for RigidTransform3D<T, Src, Dst>
{
    fn from(rot: Rotation3D<T, Src, Dst>) -> Self {
        Self::from_rotation(rot)
    }
}

impl<T: Real + ApproxEq<T>, Src, Dst> From<Vector3D<T, Dst>> for RigidTransform3D<T, Src, Dst> {
    fn from(t: Vector3D<T, Dst>) -> Self {
        Self::from_translation(t)
    }
}

#[cfg(test)]
mod test {
    use super::RigidTransform3D;
    use crate::default::{Rotation3D, Transform3D, Vector3D};

    #[test]
    fn test_rigid_construction() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        assert!(rigid
            .to_transform()
            .approx_eq(&rotation.to_transform().then(&translation.to_transform())));

        let rigid = RigidTransform3D::new_from_reversed(translation, rotation);
        assert!(rigid
            .to_transform()
            .approx_eq(&translation.to_transform().then(&rotation.to_transform())));
    }

    #[test]
    fn test_rigid_decomposition() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let (t2, r2) = rigid.decompose_reversed();
        assert!(rigid
            .to_transform()
            .approx_eq(&t2.to_transform().then(&r2.to_transform())));
    }

    #[test]
    fn test_rigid_inverse() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);

        let rigid = RigidTransform3D::new(rotation, translation);
        let inverse = rigid.inverse();
        assert!(rigid
            .then(&inverse)
            .to_transform()
            .approx_eq(&Transform3D::identity()));
        assert!(inverse
            .to_transform()
            .approx_eq(&rigid.to_transform().inverse().unwrap()));
    }

    #[test]
    fn test_rigid_multiply() {
        let translation = Vector3D::new(12.1, 17.8, -5.5);
        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
        let translation2 = Vector3D::new(9.3, -3.9, 1.1);
        let rotation2 = Rotation3D::unit_quaternion(0.1, 0.2, 0.3, -0.4);
        let rigid = RigidTransform3D::new(rotation, translation);
        let rigid2 = RigidTransform3D::new(rotation2, translation2);

        assert!(rigid
            .then(&rigid2)
            .to_transform()
            .approx_eq(&rigid.to_transform().then(&rigid2.to_transform())));
        assert!(rigid2
            .then(&rigid)
            .to_transform()
            .approx_eq(&rigid2.to_transform().then(&rigid.to_transform())));
    }
}

二、结构体字段说明:

  • rotation: 这是一个Rotation3D类型的字段,表示旋转变换。Rotation3D是另一个结构体,用于表示三维空间中的旋转。Src和Dst是泛型参数,分别代表旋转前后的坐标系。
  • translation: 这是一个Vector3D类型的字段,表示平移变换。Vector3D是一个表示三维向量的结构体,Dst表示平移后的坐标系。
  • 变换顺序:在这个结构体中,旋转是先于平移应用的,即先进行旋转(Rotation * Translation),然后进行平移。这里的乘法表示的是变换的组合,但在行向量表示法中,通常理解为矩阵乘法,其中旋转矩阵乘以平移向量(实际上是通过扩展平移向量为齐次坐标形式后进行矩阵乘法)。

三、方法

  1. 构造方法 new:
  • 这个方法用于创建一个新的 RigidTransform3D 实例。
  • 它接受一个旋转 Rotation3D<T, Src, Dst> 和一个平移 Vector3D<T, Dst> 作为参数。
  • 这里的 T 表示数值类型,Src 和 Dst 分别表示旋转和平移的单位类型。
  1. cast_unit 方法:
  • 这个方法用于将 RigidTransform3D 实例的单位从一种类型转换为另一种类型。
  • 它要求数值类型 T 实现 Copy trait,以便可以复制数值。
  • Src2 和 Dst2 是新的单位类型。
  1. identity 方法:
  • 用于构造一个表示恒等变换的 RigidTransform3D 实例。
  • 恒等变换意味着物体不会发生任何旋转或平移。
    +这个方法要求数值类型 T 实现 Real 和 ApproxEq traits。
  1. to_transform 方法:
  • 这个方法将 RigidTransform3D 转换为一个更一般的 Transform3D 实例。
  • 它要求数值类型 T 实现 Trig trait,以便可以进行三角函数计算。
  • 方法内部,首先将旋转转换为变换,然后将平移转换为变换,并将这两个变换组合起来。
  1. to_untyped 和 from_untyped 方法:
  • to_untyped 方法用于去除单位,只保留数值值,返回一个单位类型为 UnknownUnit 的 RigidTransform3D 实例。
  • from_untyped 方法则相反,它接受一个单位类型为 UnknownUnit 的 RigidTransform3D 实例,并为其添加单位,返回一个新的 RigidTransform3D 实例。
  1. fmt::Debug实现:
  • 为RigidTransform3D实现了Debug trait,允许使用{:?}格式化字符串来打印该类型的实例。
  • 在实现中,通过f.debug_struct(“RigidTransform3D”)创建一个调试结构体,并添加rotation和translation两个字段,最后调用.finish()完成结构体的构建。
  1. PartialEq和Eq实现:
  • 为RigidTransform3D实现了PartialEq trait,允许比较两个实例是否相等。
  • 通过比较rotation和translation两个成员是否相等来确定两个RigidTransform3D实例是否相等。
  • 实现了Eq trait,这通常是在实现PartialEq之后自动实现的,表示RigidTransform3D实例之间的相等性是一致和反射的。
  1. hash::Hash实现:
  • 为RigidTransform3D实现了Hash trait,允许该类型的实例被哈希。
  • 实现中,通过调用self.rotation.hash(state)和self.translation.hash(state)将rotation和translation两个成员的值合并到哈希状态中。
  1. Copy和Clone实现:
  • 如果T实现了Copy trait,则RigidTransform3D也实现了Copy trait,允许通过复制位模式来复制实例。
  • 如果T实现了Clone trait,则RigidTransform3D也实现了Clone trait,允许通过调用clone方法来创建实例的深拷贝。
  1. arbitrary::Arbitrary实现(依赖proptest库):
  • 为RigidTransform3D实现了Arbitrary trait,允许在属性测试中自动生成实例。
  • 通过调用arbitrary::Arbitrary::arbitrary(u)?分别为rotation和translation生成任意值。
  1. Zeroable和Pod实现(依赖bytemuck库):
  • 如果T实现了Zeroable trait,则RigidTransform3D也实现了Zeroable trait,允许将实例的内存区域清零。
  • 如果T实现了Pod trait(Plain Old Data,简单旧数据),且Src和Dst具有静态生命周期,则RigidTransform3D也实现了Pod trait,表示实例的内存布局是平凡的,可以安全地复制和比较内存块。
  1. 从Rotation3D和Vector3D的类型转换:
  • 如果T实现了Real trait(通常表示实数类型,如浮点数)和ApproxEq trait(允许近似相等比较),则可以从Rotation3D<T, Src, Dst>和Vector3D<T, Dst>类型转换为RigidTransform3D<T, Src, Dst>。
  • 提供了from_rotation和from_translation的便捷方法(虽然在这段代码中没有直接定义这些方法,但从上下文中可以推断出这一点)。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/963953.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

【优先算法】专题——前缀和

目录 一、【模版】前缀和 参考代码&#xff1a; 二、【模版】 二维前缀和 参考代码&#xff1a; 三、寻找数组的中心下标 参考代码&#xff1a; 四、除自身以外数组的乘积 参考代码&#xff1a; 五、和为K的子数组 参考代码&#xff1a; 六、和可被K整除的子数组 参…

刷题记录 动态规划-6: 62. 不同路径

题目&#xff1a;62. 不同路径 难度&#xff1a;中等 一个机器人位于一个 m x n 网格的左上角 &#xff08;起始点在下图中标记为 “Start” &#xff09;。 机器人每次只能向下或者向右移动一步。机器人试图达到网格的右下角&#xff08;在下图中标记为 “Finish” &#x…

梯度、梯度下降、最小二乘法

在求解机器学习算法的模型参数&#xff0c;即无约束优化问题时&#xff0c;梯度下降是最常采用的方法之一&#xff0c;另一种常用的方法是最小二乘法。 1. 梯度和梯度下降 在微积分里面&#xff0c;对多元函数的参数求∂偏导数&#xff0c;把求得的各个参数的偏导数以向量的形式…

基于STM32的智能安防监控系统

1. 引言 随着物联网技术的普及&#xff0c;智能安防系统在家庭与工业场景中的应用日益广泛。本文设计了一款基于STM32的智能安防监控系统&#xff0c;集成人体感应、环境异常检测、图像识别与云端联动功能&#xff0c;支持实时报警、远程监控与数据回溯。该系统采用边缘计算与…

优化代码性能:利用CPU缓存原理

在计算机的世界里&#xff0c;有一场如同龟兔赛跑般的速度较量&#xff0c;主角便是 CPU 和内存 。龟兔赛跑的故事大家都耳熟能详&#xff0c;兔子速度飞快&#xff0c;乌龟则慢吞吞的。在计算机中&#xff0c;CPU 就如同那敏捷的兔子&#xff0c;拥有超高的运算速度&#xff0…

Notepad++消除生成bak文件

设置(T) ⇒ 首选项... ⇒ 备份 ⇒ 勾选 "禁用" 勾选禁用 就不会再生成bak文件了 notepad怎么修改字符集编码格式为gbk 如图所示

如何创建折叠式Title

文章目录 1 概念介绍2 使用方法3 示例代码 我们在上一章回中介绍了SliverGrid组件相关的内容&#xff0c;本章回中将介绍SliverAppBar组件.闲话休提&#xff0c;让我们一起Talk Flutter吧。 1 概念介绍 我们在本章回中介绍的SliverAppBar和普通的AppBar类似&#xff0c;它们的…

K个不同子数组的数目--滑动窗口--字节--亚马逊

Stay hungry, stay foolish 题目描述 给定一个正整数数组 nums和一个整数 k&#xff0c;返回 nums 中 「好子数组」 的数目。 如果 nums 的某个子数组中不同整数的个数恰好为 k&#xff0c;则称 nums 的这个连续、不一定不同的子数组为 「好子数组 」。 例如&#xff0c;[1,2,…

Chromium132 编译指南 - Android 篇(一):编译前准备

1. 引言 欢迎来到《Chromium 132 编译指南 - Android 篇》系列的第一部分。本系列指南将引导您逐步完成在 Android 平台上编译 Chromium 132 版本的全过程。Chromium 作为一款由 Google 主导开发的开源浏览器引擎&#xff0c;为众多现代浏览器提供了核心驱动力。而 Android 作…

webpack传输性能优化

手动分包 基本原理 手动分包的总体思路是&#xff1a;先打包公共模块&#xff0c;然后再打包业务代码。 打包公共模块 公共模块会被打包成为动态链接库&#xff08;dll Dynamic Link Library&#xff09;&#xff0c;并生成资源清单。 打包业务代码 打包时&#xff0c;如果…

6 [新一代Github投毒针对网络安全人员钓鱼]

0x01 前言 在Github上APT组织“海莲花”发布存在后门的提权BOF&#xff0c;通过该项目针对网络安全从业人员进行钓鱼。不过其实早在几年前就已经有人对Visual Studio项目恶意利用进行过研究&#xff0c;所以投毒的手法也不算是新的技术。但这次国内有大量的安全从业者转发该钓…

加载数据,并切分

# Step 3 . WebBaseLoader 配置为专门从 Lilian Weng 的博客文章中抓取和加载内容。它仅针对网页的相关部分&#xff08;例如帖子内容、标题和标头&#xff09;进行处理。 加载信息 from langchain_community.document_loaders import WebBaseLoader loader WebBaseLoader(w…

【Numpy核心编程攻略:Python数据处理、分析详解与科学计算】2.5 高级索引应用:图像处理中的区域提取

2.5 高级索引应用&#xff1a;图像处理中的区域提取 目录/提纲 #mermaid-svg-BI09xc20YqcpUam7 {font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-BI09xc20YqcpUam7 .error-icon{fill:#552222;}#mermaid-svg-BI09xc20…

房屋中介管理系统的设计与实现

房屋中介管理系统的设计与实现 摘要&#xff1a;随着房地产市场的快速发展&#xff0c;房屋中介行业的信息管理需求日益增长。传统的管理方式已无法满足中介公司对房源信息、客户信息以及业务流程的高效管理需求。为此&#xff0c;本文设计并实现了一套房屋中介管理系统&#x…

Vue指令v-on

目录 一、Vue中的v-on指令是什么&#xff1f;二、v-on指令的简写三、v-on指令的使用 一、Vue中的v-on指令是什么&#xff1f; v-on指令的作用是&#xff1a;为元素绑定事件。 二、v-on指令的简写 “v-on&#xff1a;“指令可以简写为”” 三、v-on指令的使用 1、v-on指令绑…

力扣第435场周赛讲解

文章目录 题目总览题目详解3442.奇偶频次间的最大差值I3443.K次修改后的最大曼哈顿距离3444. 使数组包含目标值倍数的最少增量3445.奇偶频次间的最大差值 II 题目总览 奇偶频次间的最大差值I K次修改后的最大曼哈顿距离 使数组包含目标值倍数的最少增量 奇偶频次间的最大差值I…

编程AI深度实战:给vim装上AI

系列文章&#xff1a; 编程AI深度实战&#xff1a;私有模型deep seek r1&#xff0c;必会ollama-CSDN博客 编程AI深度实战&#xff1a;自己的AI&#xff0c;必会LangChain-CSDN博客 编程AI深度实战&#xff1a;给vim装上AI-CSDN博客 编程AI深度实战&#xff1a;火的编程AI&…

嵌入式知识点总结 操作系统 专题提升(四)-上下文

针对于嵌入式软件杂乱的知识点总结起来&#xff0c;提供给读者学习复习对下述内容的强化。 目录 1.上下文有哪些?怎么理解? 2.为什么会有上下文这种概念? 3.什么情况下进行用户态到内核态的切换? 4.中断上下文代码中有哪些注意事项&#xff1f; 5.请问线程需要保存哪些…

python算法和数据结构刷题[6]:二叉树、堆、BFS\DFS

遍历二叉树 前序遍历NLR&#xff1a;先访问根结点&#xff0c;再前序遍历左子树&#xff0c;最后前序遍历右子树。中序遍历LNR&#xff1a;先中序遍历左子树&#xff0c;再访问根结点&#xff0c;最后中序遍历右子树。后序遍历 LRN&#xff1a;先后序遍历左子树&#xff0c;再…

012-51单片机CLD1602显示万年历+闹钟+农历+整点报时

1. 硬件设计 硬件是我自己设计的一个通用的51单片机开发平台&#xff0c;可以根据需要自行焊接模块&#xff0c;这是用立创EDA画的一个双层PCB板&#xff0c;所以模块都是插针式&#xff0c;不是表贴的。电路原理图在文末的链接里&#xff0c;PCB图暂时不选择开源。 B站上传的…