posture 0.1.0

positional posture of spatial robot
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

use super::RotationMatrix;
use nalgebra::UnitQuaternion;
use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Quaternion([f64; 4]);

impl Quaternion {
    pub fn new(val: [f64; 4]) -> Self {
        Self(val)
    }
    pub fn into_matrix(self) -> RotationMatrix {
        RotationMatrix::from(self)
    }
    pub fn from_matrix(matrix: RotationMatrix) -> Self {
        Self::from(matrix)
    }
}

impl From<Quaternion> for RotationMatrix {
    fn from(item: Quaternion) -> Self {
        let quaternion = nalgebra::Quaternion::new(item.0[0], item.0[1], item.0[2], item.0[3]);
        UnitQuaternion::from_quaternion(quaternion).to_rotation_matrix()
    }
}

impl From<RotationMatrix> for Quaternion {
    fn from(item: RotationMatrix) -> Self {
        let q = UnitQuaternion::from_rotation_matrix(&item);
        let q = q.quaternion();
        Self([q.w, q.i, q.j, q.k])
    }
}