b_spline/
traits.rs

1use tiny_solver::manifold::so3::SO3;
2use tiny_solver::na;
3
4pub trait ToSO3<T: na::RealField> {
5    fn to_rotation3(&self) -> na::Rotation3<T>;
6    fn to_so3(&self) -> tiny_solver::manifold::so3::SO3<T>;
7}
8
9impl<T: na::RealField> ToSO3<T> for na::DVector<T> {
10    fn to_rotation3(&self) -> na::Rotation3<T> {
11        na::Rotation3::from_scaled_axis(na::Vector3::new(
12            self[0].clone(),
13            self[1].clone(),
14            self[2].clone(),
15        ))
16    }
17    fn to_so3(&self) -> tiny_solver::manifold::so3::SO3<T> {
18        SO3::exp(self.as_view())
19    }
20}
21
22impl<T: na::RealField> ToSO3<T> for [T; 3] {
23    fn to_rotation3(&self) -> na::Rotation3<T> {
24        na::Rotation3::from_scaled_axis(na::Vector3::new(
25            self[0].clone(),
26            self[1].clone(),
27            self[2].clone(),
28        ))
29    }
30    fn to_so3(&self) -> tiny_solver::manifold::so3::SO3<T> {
31        SO3::exp(self.to_dvec().as_view())
32    }
33}
34impl<T: na::RealField> ToSO3<T> for na::Vector3<T> {
35    fn to_rotation3(&self) -> na::Rotation3<T> {
36        na::Rotation3::from_scaled_axis(na::Vector3::new(
37            self.x.clone(),
38            self.y.clone(),
39            self.z.clone(),
40        ))
41    }
42    fn to_so3(&self) -> tiny_solver::manifold::so3::SO3<T> {
43        SO3::exp(self.to_dvec().as_view())
44    }
45}
46
47impl<T: na::RealField> ToDVec<T> for [T; 3] {
48    fn to_dvec(&self) -> na::DVector<T> {
49        na::DVector::<_>::from_vec_storage(na::VecStorage::new(
50            na::Dyn(3usize),
51            na::Const::<1>,
52            vec![self[0].clone(), self[1].clone(), self[2].clone()],
53        ))
54    }
55}
56
57impl<T: na::RealField> ToDVec<T> for na::Vector3<T> {
58    fn to_dvec(&self) -> na::DVector<T> {
59        na::DVector::<_>::from_vec_storage(na::VecStorage::new(
60            na::Dyn(3usize),
61            na::Const::<1>,
62            vec![self[0].clone(), self[1].clone(), self[2].clone()],
63        ))
64    }
65}
66
67pub trait ToDVec<T> {
68    fn to_dvec(&self) -> na::DVector<T>;
69}
b_spline/traits.rs

b_spline/
traits.rs
