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
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OVector};
use simba::scalar::SupersetOf;
use crate::misc::FloatingPoint;
/// Curvature structure
#[derive(Debug, Clone, PartialEq)]
pub struct Curvature<T: FloatingPoint, D: DimName>
where
DefaultAllocator: Allocator<D>,
{
/// Unit tangent vector
t: OVector<T, D>,
/// Curvature vector
k: OVector<T, D>,
}
impl<T: FloatingPoint, D: DimName> Curvature<T, D>
where
DefaultAllocator: Allocator<D>,
{
pub fn new(t: OVector<T, D>, k: OVector<T, D>) -> Self {
Self { t, k }
}
/// Compute curvature from first and second derivatives
/// Returns an error if the first derivative is zero
pub fn derivatives(deriv1: OVector<T, D>, deriv2: OVector<T, D>) -> Result<Self, Self> {
// Evaluate unit tangent and curvature from first and second derivatives
// T = D1 / |D1|
// K = ( D2 - (D2 o T)*T )/( D1 o D1)
let n1 = deriv1.norm();
if n1.is_zero() {
// Use L'hopital's rule to show that if the unit tangent
// exists and the 1rst derivative is zero and the 2nd derivative is
// nonzero, then the unit tangent is equal to +/-the unitized
// 2nd derivative. The sign is equal to the sign of D1(s) o D2(s)
// as s approaches the evaluation parameter.
//
let n2 = deriv2.norm();
if !n2.is_zero() {
let u = deriv2 / n2;
Err(Self::new(u, OVector::zeros()))
} else {
Err(Self::new(OVector::zeros(), OVector::zeros()))
}
} else {
let tangent = deriv1.clone() / n1;
let dot = deriv2.dot(&tangent);
let d1 = T::one() / (deriv1.dot(&deriv1));
let k = (deriv2 - tangent.clone() * dot) * d1;
Ok(Self::new(tangent, k))
}
}
/// Returns the unit vector
pub fn tangent_vector(&self) -> OVector<T, D> {
self.t.clone()
}
/// Returns the curvature vector
pub fn curvature_vector(&self) -> OVector<T, D> {
self.k.clone()
}
/// Returns the curvature magnitude
pub fn kappa(&self) -> T {
self.k.norm()
}
/// Casts the curvature to another floating point type.
pub fn cast<F: FloatingPoint + SupersetOf<T>>(&self) -> Curvature<F, D> {
Curvature {
t: self.t.clone().cast(),
k: self.k.clone().cast(),
}
}
}