use core::ops::{Add, Mul, Sub};
use rand::prelude::*;
use serde::{Deserialize, Serialize};
use crate::vector::Vector;
cfg_if::cfg_if! {
if #[cfg(feature = "f32")] {
type FloatType = f32;
} else {
type FloatType = f64;
}
}
#[derive(Copy, Clone, Debug, Serialize, Deserialize)]
pub struct HeightVector<const N: usize> {
#[serde(flatten)]
position: Vector<FloatType, N>,
height: FloatType,
}
impl<const N: usize> HeightVector<N> {
pub(crate) fn random() -> Self {
let mut rng = thread_rng();
let mut vec = [0.0; N];
for i in vec.iter_mut().take(N) {
*i = rng.gen::<FloatType>() - 0.5;
}
let height = rng.gen::<FloatType>().abs();
Self {
position: Vector::<FloatType, N>::from(vec),
height,
}
.normalized()
}
pub(crate) fn len(&self) -> FloatType {
self.position.len() + self.height
}
pub(crate) fn normalized(&self) -> Self {
let len = self.len();
let ret = Self {
position: self.position / len,
height: self.height / len,
};
if ret.is_valid() {
ret
} else {
Self::random()
}
}
pub(crate) fn is_valid(&self) -> bool {
!self.is_invalid()
}
pub(crate) fn is_invalid(&self) -> bool {
self.position.is_invalid()
|| self.height.is_nan()
|| self.height.is_infinite()
|| self.height < 0.0
}
}
impl<const N: usize> Default for HeightVector<N> {
fn default() -> Self {
Self {
position: Vector::default(),
height: Default::default(),
}
}
}
impl<const N: usize> From<([FloatType; N], FloatType)> for HeightVector<N> {
fn from(value: ([FloatType; N], FloatType)) -> Self {
let ret = Self {
position: Vector::<FloatType, N>::from(value.0),
height: value.1,
};
if ret.is_valid() {
ret
} else {
Self::random()
}
}
}
impl<const N: usize> Add for HeightVector<N> {
type Output = Self;
fn add(self, rhs: Self) -> Self::Output {
let ret = Self {
position: self.position + rhs.position,
height: self.height + rhs.height,
};
if ret.is_valid() {
ret
} else {
Self::random()
}
}
}
impl<const N: usize> Sub for HeightVector<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self::Output {
let ret = Self {
position: self.position - rhs.position,
height: self.height + rhs.height,
};
if ret.is_valid() {
ret
} else {
Self::random()
}
}
}
impl<const N: usize> Mul<FloatType> for HeightVector<N> {
type Output = Self;
fn mul(self, rhs: FloatType) -> Self::Output {
let ret = Self {
position: self.position * rhs,
height: self.height * rhs,
};
if ret.is_valid() {
ret
} else {
Self::random()
}
}
}
#[allow(clippy::ignored_unit_patterns)]
#[cfg(test)]
mod tests {
use super::*;
use assert_approx_eq::assert_approx_eq;
use proptest::prelude::*;
fn approx_eq(a: FloatType, b: FloatType, margin: FloatType) -> bool {
(a - b).abs() <= margin
}
proptest! {
#[test]
fn proptest_len(x: FloatType, y: FloatType, h: FloatType) {
let len = x.hypot(y) + h.abs();
let a = HeightVector::<2>::from(([x,y],h));
if x.is_nan() || x.is_infinite() || y.is_nan() || y.is_infinite() || h.is_nan() || h.is_infinite() || h < 0.0 {
prop_assert!(approx_eq(a.len(), 1.0, 0.0001));
} else {
if len.is_nan() || len.is_infinite() {
prop_assert!(a.len().is_nan() || a.len().is_infinite());
} else {
prop_assert!(approx_eq(a.len(), len, 0.0001));
}
}
}
#[test]
#[allow(clippy::cast_lossless, clippy::cast_precision_loss)]
fn proptest_add(x0 in -1_000_000_000..1_000_000_000i32, y0 in -1_000_000_000..1_000_000_000i32, h0 in 0..1_000_000_000i32, x1 in -1_000_000_000..1_000_000_000i32, y1 in -1_000_000_000..1_000_000_000i32, h1 in 0..1_000_000i32) {
let (fx0,fy0,fh0) = (x0 as FloatType / 1_000.0, y0 as FloatType / 1_000.0, h0 as FloatType / 1_000.0);
let (fx1,fy1,fh1) = (x1 as FloatType / 1_000.0, y1 as FloatType / 1_000.0, h1 as FloatType / 1_000.0);
let a = HeightVector::<2>::from(([fx0 ,fy0 ],fh0 ));
let b = HeightVector::<2>::from(([fx1 ,fy1 ],fh1 ));
let c = a+b;
prop_assert!(approx_eq(c.position[0], fx0 + fx1, 0.0001));
prop_assert!(approx_eq(c.position[1], fy0 + fy1, 0.0001));
prop_assert!(approx_eq(c.height, fh0 + fh1, 0.0001));
}
#[test]
#[allow(clippy::cast_lossless, clippy::cast_precision_loss)]
fn proptest_sub(x0 in -1_000_000_000..1_000_000_000i32, y0 in -1_000_000_000..1_000_000_000i32, h0 in 0..1_000_000_000i32, x1 in -1_000_000_000..1_000_000_000i32, y1 in -1_000_000_000..1_000_000_000i32, h1 in 0..1_000_000i32) {
let (fx0,fy0,fh0) = (x0 as FloatType / 1_000.0, y0 as FloatType / 1_000.0, h0 as FloatType / 1_000.0);
let (fx1,fy1,fh1) = (x1 as FloatType / 1_000.0, y1 as FloatType / 1_000.0, h1 as FloatType / 1_000.0);
let a = HeightVector::<2>::from(([fx0 ,fy0 ],fh0 ));
let b = HeightVector::<2>::from(([fx1 ,fy1 ],fh1 ));
let c = a-b;
prop_assert!(approx_eq(c.position[0], fx0 - fx1, 0.0001));
prop_assert!(approx_eq(c.position[1], fy0 - fy1, 0.0001));
prop_assert!(approx_eq(c.height, fh0 + fh1, 0.0001));
}
#[test]
#[allow(clippy::cast_lossless, clippy::cast_precision_loss)]
fn proptest_mul(x in -1_000_000_000..1_000_000_000i32, y in -1_000_000_000..1_000_000_000i32, h in 0..1_000_000_000i32, m in -1_000_000_000..1_000_000_000i32) {
let (fx,fy,fh,fm) = (x as FloatType / 1_000.0, y as FloatType / 1_000.0, h as FloatType / 1_000.0, m as FloatType / 1_000.0);
let a = HeightVector::<2>::from(([fx, fy], fh));
let b = a * fm;
if fm < 0.0 {
prop_assert!(approx_eq(b.len(), 1.0, 0.0001));
} else {
prop_assert!(approx_eq(b.position[0], fx * fm, 0.0001));
prop_assert!(approx_eq(b.position[1], fy * fm, 0.0001));
prop_assert!(approx_eq(b.height, fh * fm, 0.0001));
}
}
}
#[test]
fn test_len() {
let a = HeightVector::<3>::from(([1.0, 2.0, 3.0], 4.0));
assert_approx_eq!(a.len(), 7.741_657, 0.00001);
}
#[test]
fn test_new() {
let a = HeightVector::<3>::random();
assert_approx_eq!(a.len(), 1.0);
}
#[test]
fn test_default() {
let a = HeightVector::<3>::default();
assert_approx_eq!(a.len(), 0.0);
}
#[test]
fn test_from() {
let a = HeightVector::<2>::from(([1.0, 2.0], 3.0));
assert_approx_eq!(a.position[0], 1.0);
assert_approx_eq!(a.position[1], 2.0);
assert_approx_eq!(a.height, 3.0);
}
#[test]
fn test_add() {
let a = HeightVector::<2>::from(([1.0, 2.0], 3.0));
let b = HeightVector::<2>::from(([3.0, 2.0], 1.0));
let c = a + b;
assert_approx_eq!(c.position[0], 4.0);
assert_approx_eq!(c.position[1], 4.0);
assert_approx_eq!(c.height, 4.0);
}
#[test]
fn test_sub() {
let a = HeightVector::<2>::from(([1.0, 2.0], 3.0));
let b = HeightVector::<2>::from(([3.0, 2.0], 1.0));
let c = a - b;
assert_approx_eq!(c.position[0], -2.0);
assert_approx_eq!(c.position[1], 0.0);
assert_approx_eq!(c.height, 4.0);
}
#[test]
fn test_mul_scalar() {
let a = HeightVector::<2>::from(([1.0, 2.0], 3.0)) * 10.0;
assert_approx_eq!(a.position[0], 10.0);
assert_approx_eq!(a.position[1], 20.0);
assert_approx_eq!(a.height, 30.0);
}
#[test]
fn test_serde() {
let s = "{\"position\":[1.0,2.0,3.0],\"height\":4.0}";
let a: HeightVector<3> =
serde_json::from_str(s).expect("deserialization failed during test");
assert_approx_eq!(a.len(), 7.741_657, 0.001);
let t = serde_json::to_string(&a);
assert_eq!(t.as_ref().expect("serialization failed during test"), s);
}
#[test]
fn test_sub_invalid() {
let valid = HeightVector::<2>::from(([1.0, 2.0], 3.0));
let mut invalid = HeightVector::<2>::from(([1.0, 2.0], 3.0));
invalid.height = 1.0 / 0.0;
let result = valid - invalid;
assert_approx_eq!(result.len(), 1.0);
}
#[test]
fn test_add_invalid() {
let valid = HeightVector::<2>::from(([1.0, 2.0], 3.0));
let mut invalid = HeightVector::<2>::from(([1.0 / 0.0, 2.0], 3.0));
assert_approx_eq!(invalid.len(), 1.0);
invalid.height = 1.0 / 0.0;
let result = valid + invalid;
assert_approx_eq!(result.len(), 1.0);
}
#[test]
fn test_zero_norm() {
let a = HeightVector::<2>::from(([0.0, 0.0], 0.0));
let b = a.normalized();
let len = b.len();
assert_approx_eq!(len, 1.0);
}
}