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

use na::allocator::Allocator;
use na::{DefaultAllocator, DimName, OVector, SVector, Scalar};

use {Float, Hyperdual, OHyperdual, Zero};

/// Computes the norm of a vector of Hyperdual.
pub fn norm<T: Scalar + Float, const M: usize, const N: usize>(v: &SVector<Hyperdual<T, N>, M>) -> Hyperdual<T, N>
where
    Hyperdual<T, N>: Float,
{
    let mut val = Hyperdual::<T, N>::zero();

    for i in 0..M {
        val += v[i].powi(2);
    }

    val.sqrt()
}

/// Computes the norm of a vector of Hyperdual.
pub fn norm_owned<T: Scalar + Float, M: DimName, N: DimName>(v: &OVector<OHyperdual<T, N>, M>) -> OHyperdual<T, N>
where
    OHyperdual<T, N>: Float,
    DefaultAllocator: Allocator<OHyperdual<T, N>, M> + Allocator<T, N>,
    <DefaultAllocator as Allocator<T, N>>::Buffer: Copy,
{
    let mut val = OHyperdual::<T, N>::zero();

    for i in 0..M::dim() {
        val += v[i].powi(2);
    }

    val.sqrt()
}