research/argus
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

refactor~(core): use traits and structs for interpolation

Browse source 
We have to now pass the interpolation method as a generic argument to methods.
This commit is contained in:
Anand Balakrishnan 2023-06-07 09:57:56 -04:00
parent 2b16ef9c40
commit 87afc11b90
No known key found for this signature in database
8 changed files with 314 additions and 306 deletions
Download patch file Download diff file Expand all files Collapse all files

199
argus-core/src/signals.rs
View file

@ -2,6 +2,7 @@
mod bool_ops; mod bool_ops;
mod cast; mod cast;
mod cmp_ops; mod cmp_ops;
pub mod interpolation;
pub mod iter; pub mod iter;
mod num_ops; mod num_ops;
mod shift_ops; mod shift_ops;
@ -23,43 +24,6 @@ use utils::intersect_bounds;
use crate::{ArgusResult, Error}; use crate::{ArgusResult, Error};
/// Interpolation methods supported by Argus signals.
///
/// Defaults to `Linear` interpolation (which corresponds to constant interpolation for
/// `bool` signals).
#[derive(Debug, Clone, Copy, Default)]
pub enum InterpolationMethod {
/// Linear interpolation
#[default]
Linear,
/// Interpolate from the nearest sample point.
Nearest,
}
impl InterpolationMethod {
pub(crate) fn at<T>(self, time: Duration, a: &Option<Sample<T>>, b: &Option<Sample<T>>) -> Option<T>
where
T: Copy + LinearInterpolatable,
{
use InterpolationMethod::*;
match (self, a, b) {
(Nearest, Some(ref a), Some(ref b)) => {
assert!(a.time < time && time < b.time);
if (b.time - time) > (time - a.time) {
// a is closer to the required time than b
Some(a.value)
} else {
// b is closer
Some(b.value)
}
}
(Nearest, Some(nearest), None) | (Nearest, None, Some(nearest)) => Some(nearest.value),
(Linear, Some(a), Some(b)) => Some(T::interpolate_at(a, b, time)),
_ => None,
}
}
}
/// A single sample of a signal. /// A single sample of a signal.
#[derive(Copy, Clone, Debug)] #[derive(Copy, Clone, Debug)]
pub struct Sample<T> { pub struct Sample<T> {
@ -241,80 +205,6 @@ impl<T> Signal<T> {
} }
} }
/// Interpolate the value of the signal at the given time point
///
/// If there exists a sample at the given time point then `Some(value)` is returned
/// with the value of the signal at the point. Otherwise, a the
/// [`InterpolationMethod`] is used to compute the value. If the given interpolation
/// method cannot be used at the given time (for example, if we use
/// [`InterpolationMethod::Linear`] and the `time` point is outside the signal
/// domain), then a `None` is returned.
pub fn interpolate_at(&self, time: Duration, interp: InterpolationMethod) -> Option<T>
where
T: Copy + LinearInterpolatable,
{
match self {
Signal::Empty => None,
Signal::Constant { value } => Some(*value),
Signal::Sampled { values, time_points } => {
assert_eq!(
time_points.len(),
values.len(),
"invariant: number of time points must equal number of samples"
);
// if there are no sample points, then there is no sample point (nor neighboring
// sample points) to return
if time_points.is_empty() {
return None;
}
// We will use binary search to find the appropriate index
let hint_idx = match time_points.binary_search(&time) {
Ok(idx) => return values.get(idx).copied(),
Err(idx) => idx,
};
// We have an hint as to where the sample _should have been_.
// So, lets check if there is a preceding and/or following sample.
let (first, second) = if hint_idx == 0 {
// Sample appears before the start of the signal
// So, let's return just the following sample, which is the first sample
// (since we know that the signal is non-empty).
let preceding = None;
let following = Some(Sample {
time: time_points[hint_idx],
value: values[hint_idx],
});
(preceding, following)
} else if hint_idx == time_points.len() {
// Sample appears past the end of the signal
// So, let's return just the preceding sample, which is the last sample
// (since we know the signal is non-empty)
let preceding = Some(Sample {
time: time_points[hint_idx - 1],
value: values[hint_idx - 1],
});
let following = None;
(preceding, following)
} else {
// The sample should exist within the signal.
assert!(time_points.len() >= 2, "There should be at least 2 elements");
let preceding = Some(Sample {
time: time_points[hint_idx - 1],
value: values[hint_idx - 1],
});
let following = Some(Sample {
time: time_points[hint_idx],
value: values[hint_idx],
});
(preceding, following)
};
interp.at(time, &first, &second)
}
}
}
/// Return the vector of points where the signal is sampled. /// Return the vector of points where the signal is sampled.
/// ///
/// - If the signal is empty ([`Signal::Empty`]), the output is `None`. /// - If the signal is empty ([`Signal::Empty`]), the output is `None`.
@ -371,9 +261,10 @@ impl<T> Signal<T> {
} }
/// Augment synchronization points with time points where signals intersect /// Augment synchronization points with time points where signals intersect
pub fn sync_with_intersection(&self, other: &Signal<T>) -> Option<Vec<Duration>> pub fn sync_with_intersection<Interp>(&self, other: &Signal<T>) -> Option<Vec<Duration>>
where where
T: PartialOrd + Copy + LinearInterpolatable, T: PartialOrd + Copy,
Interp: FindIntersectionMethod<T>,
{ {
use core::cmp::Ordering::*; use core::cmp::Ordering::*;
let sync_points: Vec<&Duration> = self.sync_points(other)?.into_iter().collect(); let sync_points: Vec<&Duration> = self.sync_points(other)?.into_iter().collect();
@ -405,7 +296,7 @@ impl<T> Signal<T> {
first: other.at(tm1).copied().map(|value| Sample { time: tm1, value }), first: other.at(tm1).copied().map(|value| Sample { time: tm1, value }),
second: other.at(*t).copied().map(|value| Sample { time: *t, value }), second: other.at(*t).copied().map(|value| Sample { time: *t, value }),
}; };
let intersect = T::find_intersection(&a, &b); let intersect = Interp::find_intersection(&a, &b);
return_points.push(intersect.time); return_points.push(intersect.time);
} }
} }
@ -418,6 +309,70 @@ impl<T> Signal<T> {
} }
} }
impl<T: Copy> Signal<T> {
/// Interpolate the value of the signal at the given time point
///
/// If there exists a sample at the given time point then `Some(value)` is returned
/// with the value of the signal at the point. Otherwise, a the
/// [`InterpolationMethod`] is used to compute the value. If the given interpolation
/// method cannot be used at the given time (for example, if we use
/// [`interpolation::Linear`] and the `time` point is outside the signal
/// domain), then a `None` is returned.
pub fn interpolate_at<Interp>(&self, time: Duration) -> Option<T>
where
Interp: InterpolationMethod<T>,
{
match self {
Signal::Empty => None,
Signal::Constant { value } => Some(*value),
Signal::Sampled { values, time_points } => {
assert_eq!(
time_points.len(),
values.len(),
"invariant: number of time points must equal number of samples"
);
// if there are no sample points, then there is no sample point (nor neighboring
// sample points) to return
if time_points.is_empty() {
return None;
}
// We will use binary search to find the appropriate index
let hint_idx = match time_points.binary_search(&time) {
Ok(idx) => return values.get(idx).copied(),
Err(idx) => idx,
};
// We have an hint as to where the sample _should have been_.
// So, lets check if there is a preceding and/or following sample.
if hint_idx == 0 {
// Sample appears before the start of the signal
// So, let's return just the following sample, which is the first sample
// (since we know that the signal is non-empty).
Some(values[hint_idx])
} else if hint_idx == time_points.len() {
// Sample appears past the end of the signal
// So, let's return just the preceding sample, which is the last sample
// (since we know the signal is non-empty)
Some(values[hint_idx - 1])
} else {
// The sample should exist within the signal.
assert!(time_points.len() >= 2, "There should be at least 2 elements");
let first = Sample {
time: time_points[hint_idx - 1],
value: values[hint_idx - 1],
};
let second = Sample {
time: time_points[hint_idx],
value: values[hint_idx],
};
Interp::at(&first, &second, time)
}
}
}
}
}
impl<T: Num> Signal<T> { impl<T: Num> Signal<T> {
/// Create a constant `0` signal /// Create a constant `0` signal
pub fn zero() -> Self { pub fn zero() -> Self {
@ -611,10 +566,10 @@ mod tests {
($ty:ty, $op:tt sig) => { ($ty:ty, $op:tt sig) => {
proptest! { proptest! {
|(sig in arbitrary::sampled_signal::<$ty>(1..100))| { |(sig in arbitrary::sampled_signal::<$ty>(1..100))| {
use InterpolationMethod::Linear; use interpolation::Linear;
let new_sig = $op (&sig); let new_sig = $op (&sig);
for (t, v) in new_sig.iter() { for (t, v) in new_sig.iter() {
let prev = sig.interpolate_at(*t, Linear).unwrap(); let prev = sig.interpolate_at::<Linear>(*t).unwrap();
assert_eq!($op prev, *v); assert_eq!($op prev, *v);
} }
} }
@ -623,11 +578,11 @@ mod tests {
($ty:ty, lhs $op:tt rhs) => { ($ty:ty, lhs $op:tt rhs) => {
proptest! { proptest! {
|(sig1 in arbitrary::sampled_signal::<$ty>(1..100), sig2 in arbitrary::sampled_signal::<$ty>(1..100))| { |(sig1 in arbitrary::sampled_signal::<$ty>(1..100), sig2 in arbitrary::sampled_signal::<$ty>(1..100))| {
use InterpolationMethod::Linear; use interpolation::Linear;
let new_sig = &sig1 $op &sig2; let new_sig = &sig1 $op &sig2;
for (t, v) in new_sig.iter() { for (t, v) in new_sig.iter() {
let v1 = sig1.interpolate_at(*t, Linear).unwrap(); let v1 = sig1.interpolate_at::<Linear>(*t).unwrap();
let v2 = sig2.interpolate_at(*t, Linear).unwrap(); let v2 = sig2.interpolate_at::<Linear>(*t).unwrap();
assert_eq!(v1 $op v2, *v); assert_eq!(v1 $op v2, *v);
} }
} }
@ -635,11 +590,11 @@ mod tests {
proptest! { proptest! {
|(sig1 in arbitrary::sampled_signal::<$ty>(1..100), sig2 in arbitrary::constant_signal::<$ty>())| { |(sig1 in arbitrary::sampled_signal::<$ty>(1..100), sig2 in arbitrary::constant_signal::<$ty>())| {
use InterpolationMethod::Linear; use interpolation::Linear;
let new_sig = &sig1 $op &sig2; let new_sig = &sig1 $op &sig2;
for (t, v) in new_sig.iter() { for (t, v) in new_sig.iter() {
let v1 = sig1.interpolate_at(*t, Linear).unwrap(); let v1 = sig1.interpolate_at::<Linear>(*t).unwrap();
let v2 = sig2.interpolate_at(*t, Linear).unwrap(); let v2 = sig2.interpolate_at::<Linear>(*t).unwrap();
assert_eq!(v1 $op v2, *v); assert_eq!(v1 $op v2, *v);
} }
} }

5
argus-core/src/signals/bool_ops.rs
View file

@ -1,3 +1,4 @@
use super::interpolation::Linear;
use crate::signals::utils::{apply1, apply2}; use crate::signals::utils::{apply1, apply2};
use crate::signals::Signal; use crate::signals::Signal;
@ -13,7 +14,7 @@ impl core::ops::BitAnd<Self> for &Signal<bool> {
type Output = Signal<bool>; type Output = Signal<bool>;
fn bitand(self, other: Self) -> Self::Output { fn bitand(self, other: Self) -> Self::Output {
apply2(self, other, |lhs, rhs| lhs && rhs) apply2::<_, _, _, Linear>(self, other, |lhs, rhs| lhs && rhs)
} }
} }
@ -21,6 +22,6 @@ impl core::ops::BitOr<Self> for &Signal<bool> {
type Output = Signal<bool>; type Output = Signal<bool>;
fn bitor(self, other: Self) -> Self::Output { fn bitor(self, other: Self) -> Self::Output {
apply2(self, other, |lhs, rhs| lhs || rhs) apply2::<_, _, _, Linear>(self, other, |lhs, rhs| lhs || rhs)
} }
} }

32
argus-core/src/signals/cmp_ops.rs
View file

@ -1,30 +1,29 @@
use std::cmp::Ordering; use std::cmp::Ordering;
use num_traits::NumCast; use super::interpolation::Linear;
use super::traits::{SignalMinMax, SignalPartialOrd};
use super::traits::{LinearInterpolatable, SignalMinMax, SignalPartialOrd}; use super::{FindIntersectionMethod, InterpolationMethod, Signal};
use super::{InterpolationMethod, Signal};
impl<T> SignalPartialOrd<Self> for Signal<T> impl<T> SignalPartialOrd<Self> for Signal<T>
where where
T: PartialOrd + Copy + std::fmt::Debug + NumCast + LinearInterpolatable, T: PartialOrd + Copy,
Linear: InterpolationMethod<T> + FindIntersectionMethod<T>,
{ {
fn signal_cmp<F>(&self, other: &Self, op: F) -> Option<Signal<bool>> fn signal_cmp<F>(&self, other: &Self, op: F) -> Option<Signal<bool>>
where where
F: Fn(Ordering) -> bool, F: Fn(Ordering) -> bool,
{ {
use super::InterpolationMethod::Linear;
// This has to be manually implemented and cannot use the apply2 functions. // This has to be manually implemented and cannot use the apply2 functions.
// This is because if we have two signals that cross each other, then there is // This is because if we have two signals that cross each other, then there is
// an intermediate point where the two signals are equal. This point must be // an intermediate point where the two signals are equal. This point must be
// added to the signal appropriately. // added to the signal appropriately.
// the union of the sample points in self and other // the union of the sample points in self and other
let sync_points = self.sync_with_intersection(other)?; let sync_points = self.sync_with_intersection::<Linear>(other)?;
let sig: Option<Signal<bool>> = sync_points let sig: Option<Signal<bool>> = sync_points
.into_iter() .into_iter()
.map(|t| { .map(|t| {
let lhs = self.interpolate_at(t, Linear).unwrap(); let lhs = self.interpolate_at::<Linear>(t).unwrap();
let rhs = other.interpolate_at(t, Linear).unwrap(); let rhs = other.interpolate_at::<Linear>(t).unwrap();
let cmp = lhs.partial_cmp(&rhs); let cmp = lhs.partial_cmp(&rhs);
cmp.map(|v| (t, op(v))) cmp.map(|v| (t, op(v)))
}) })
@ -35,17 +34,18 @@ where
impl<T> SignalMinMax<Self> for Signal<T> impl<T> SignalMinMax<Self> for Signal<T>
where where
T: PartialOrd + Copy + LinearInterpolatable + NumCast, T: PartialOrd + Copy,
Linear: InterpolationMethod<T> + FindIntersectionMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
fn min(&self, other: &Self) -> Self::Output { fn min(&self, other: &Self) -> Self::Output {
let time_points = self.sync_with_intersection(other).unwrap(); let time_points = self.sync_with_intersection::<Linear>(other).unwrap();
time_points time_points
.into_iter() .into_iter()
.map(|t| { .map(|t| {
let lhs = self.interpolate_at(t, InterpolationMethod::Linear).unwrap(); let lhs = self.interpolate_at::<Linear>(t).unwrap();
let rhs = other.interpolate_at(t, InterpolationMethod::Linear).unwrap(); let rhs = other.interpolate_at::<Linear>(t).unwrap();
if lhs < rhs { if lhs < rhs {
(t, lhs) (t, lhs)
} else { } else {
@ -56,12 +56,12 @@ where
} }
fn max(&self, other: &Self) -> Self::Output { fn max(&self, other: &Self) -> Self::Output {
let time_points = self.sync_with_intersection(other).unwrap(); let time_points = self.sync_with_intersection::<Linear>(other).unwrap();
time_points time_points
.into_iter() .into_iter()
.map(|t| { .map(|t| {
let lhs = self.interpolate_at(t, InterpolationMethod::Linear).unwrap(); let lhs = self.interpolate_at::<Linear>(t).unwrap();
let rhs = other.interpolate_at(t, InterpolationMethod::Linear).unwrap(); let rhs = other.interpolate_at::<Linear>(t).unwrap();
if lhs > rhs { if lhs > rhs {
(t, lhs) (t, lhs)
} else { } else {

177
argus-core/src/signals/interpolation.rs Normal file
View file

@ -0,0 +1,177 @@
//! Interpolation methods
use std::time::Duration;
use super::utils::Neighborhood;
use super::{FindIntersectionMethod, InterpolationMethod, Sample};
/// Constant interpolation.
///
/// Here, the previous signal value is propagated to the requested time point.
pub struct Constant;
impl<T: Clone> InterpolationMethod<T> for Constant {
fn at(a: &Sample<T>, b: &Sample<T>, time: Duration) -> Option<T> {
if time == b.time {
Some(b.value.clone())
} else if a.time <= time && time < b.time {
Some(a.value.clone())
} else {
None
}
}
}
/// Nearest interpolation.
///
/// Here, the signal value from the nearest sample (time-wise) is propagated to the
/// requested time point.
pub struct Nearest;
impl<T: Clone> InterpolationMethod<T> for Nearest {
fn at(a: &super::Sample<T>, b: &super::Sample<T>, time: std::time::Duration) -> Option<T> {
if time < a.time || time > b.time {
// `time` is outside the segments.
None
} else if (b.time - time) > (time - a.time) {
// a is closer to the required time than b
Some(a.value.clone())
} else {
// b is closer
Some(b.value.clone())
}
}
}
/// Linear interpolation.
///
/// Here, linear interpolation is performed to estimate the sample at the time point
/// between two samples.
pub struct Linear;
impl InterpolationMethod<bool> for Linear {
fn at(a: &Sample<bool>, b: &Sample<bool>, time: Duration) -> Option<bool> {
if a.time < time && time < b.time {
// We can't linear interpolate a boolean, so we return the previous.
Some(a.value)
} else {
None
}
}
}
impl FindIntersectionMethod<bool> for Linear {
fn find_intersection(a: &Neighborhood<bool>, b: &Neighborhood<bool>) -> Sample<bool> {
let Sample { time: ta1, value: ya1 } = a.first.unwrap();
let Sample { time: ta2, value: ya2 } = a.second.unwrap();
let Sample { time: tb1, value: yb1 } = b.first.unwrap();
let Sample { time: tb2, value: yb2 } = b.second.unwrap();
let left_cmp = ya1.cmp(&yb1);
let right_cmp = ya2.cmp(&yb2);
if left_cmp.is_eq() {
// They already intersect, so we return the inner time-point
if ta1 < tb1 {
Sample { time: tb1, value: yb1 }
} else {
Sample { time: ta1, value: ya1 }
}
} else if right_cmp.is_eq() {
// They intersect at the end, so we return the outer time-point, as that is
// when they become equal.
if ta2 < tb2 {
Sample { time: tb2, value: yb2 }
} else {
Sample { time: ta2, value: ya2 }
}
} else {
// The switched, so the one that switched earlier will intersect with the
// other.
// So, we find the one that has a lower time point, i.e., the inner one.
if ta2 < tb2 {
Sample { time: ta2, value: ya2 }
} else {
Sample { time: tb2, value: yb2 }
}
}
}
}
macro_rules! interpolate_for_num {
($ty:ty) => {
impl InterpolationMethod<$ty> for Linear {
fn at(first: &Sample<$ty>, second: &Sample<$ty>, time: Duration) -> Option<$ty> {
use num_traits::cast;
// We will need to cast the samples to f64 values (along with the time
// window) to be able to interpolate correctly.
// TODO(anand): Verify this works.
let t1 = first.time.as_secs_f64();
let t2 = second.time.as_secs_f64();
let at = time.as_secs_f64();
assert!((t1..=t2).contains(&at));
// We need to do stable linear interpolation
// https://www.open-std.org/jtc1/sc22/wg21/docs/papers/2019/p0811r3.html
let a: f64 = cast(first.value).unwrap();
let b: f64 = cast(second.value).unwrap();
// Set t to a value in [0, 1]
let t = (at - t1) / (t2 - t1);
assert!((0.0..=1.0).contains(&t));
let val = if (a <= 0.0 && b >= 0.0) || (a >= 0.0 && b <= 0.0) {
t * b + (1.0 - t) * a
} else if t == 1.0 {
b
} else {
a + t * (b - a)
};
cast(val)
}
}
impl FindIntersectionMethod<$ty> for Linear {
fn find_intersection(a: &Neighborhood<$ty>, b: &Neighborhood<$ty>) -> Sample<$ty> {
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
use num_traits::cast;
let Sample { time: t1, value: y1 } = a.first.unwrap();
let Sample { time: t2, value: y2 } = a.second.unwrap();
let Sample { time: t3, value: y3 } = b.first.unwrap();
let Sample { time: t4, value: y4 } = b.second.unwrap();
let t1 = t1.as_secs_f64();
let t2 = t2.as_secs_f64();
let t3 = t3.as_secs_f64();
let t4 = t4.as_secs_f64();
let y1: f64 = cast(y1).unwrap();
let y2: f64 = cast(y2).unwrap();
let y3: f64 = cast(y3).unwrap();
let y4: f64 = cast(y4).unwrap();
let denom = ((t1 - t2) * (y3 - y4)) - ((y1 - y2) * (t3 - t4));
let t_top = (((t1 * y2) - (y1 * t2)) * (t3 - t4)) - ((t1 - t2) * (t3 * y4 - y3 * t4));
let y_top = (((t1 * y2) - (y1 * t2)) * (y3 - y4)) - ((y1 - y2) * (t3 * y4 - y3 * t4));
let t = Duration::from_secs_f64(t_top / denom);
let y: $ty = num_traits::cast(y_top / denom).unwrap();
Sample { time: t, value: y }
}
}
};
}
interpolate_for_num!(i8);
interpolate_for_num!(i16);
interpolate_for_num!(i32);
interpolate_for_num!(i64);
interpolate_for_num!(u8);
interpolate_for_num!(u16);
interpolate_for_num!(u32);
interpolate_for_num!(u64);
interpolate_for_num!(f32);
interpolate_for_num!(f64);

36
argus-core/src/signals/num_ops.rs
View file

@ -1,6 +1,8 @@
use num_traits::{NumCast, Signed}; use num_traits::Signed;
use super::traits::{LinearInterpolatable, SignalAbs}; use super::interpolation::Linear;
use super::traits::SignalAbs;
use super::{FindIntersectionMethod, InterpolationMethod};
use crate::signals::utils::{apply1, apply2}; use crate::signals::utils::{apply1, apply2};
use crate::signals::Signal; use crate::signals::Signal;
@ -18,37 +20,39 @@ where
impl<T> core::ops::Add for &Signal<T> impl<T> core::ops::Add for &Signal<T>
where where
T: core::ops::Add<T, Output = T> + Copy + LinearInterpolatable, T: core::ops::Add<T, Output = T> + Copy,
Linear: InterpolationMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
/// Add the given signal with another /// Add the given signal with another
fn add(self, rhs: Self) -> Self::Output { fn add(self, rhs: Self) -> Self::Output {
apply2(self, rhs, |lhs, rhs| lhs + rhs) apply2::<_, _, _, Linear>(self, rhs, |lhs, rhs| lhs + rhs)
} }
} }
impl<T> core::ops::Mul for &Signal<T> impl<T> core::ops::Mul for &Signal<T>
where where
T: core::ops::Mul<T, Output = T> + Copy + LinearInterpolatable, T: core::ops::Mul<T, Output = T> + Copy,
Linear: InterpolationMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
/// Multiply the given signal with another /// Multiply the given signal with another
fn mul(self, rhs: Self) -> Self::Output { fn mul(self, rhs: Self) -> Self::Output {
apply2(self, rhs, |lhs, rhs| lhs * rhs) apply2::<_, _, _, Linear>(self, rhs, |lhs, rhs| lhs * rhs)
} }
} }
impl<T> core::ops::Sub for &Signal<T> impl<T> core::ops::Sub for &Signal<T>
where where
T: core::ops::Sub<T, Output = T> + Copy + LinearInterpolatable + PartialOrd + NumCast, T: core::ops::Sub<T, Output = T> + Copy + PartialOrd,
Linear: InterpolationMethod<T> + FindIntersectionMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
/// Subtract the given signal with another /// Subtract the given signal with another
fn sub(self, rhs: Self) -> Self::Output { fn sub(self, rhs: Self) -> Self::Output {
use super::InterpolationMethod::Linear;
// This has to be manually implemented and cannot use the apply2 functions. // This has to be manually implemented and cannot use the apply2 functions.
// This is because if we have two signals that cross each other, then there is // This is because if we have two signals that cross each other, then there is
// an intermediate point where the two signals are equal. This point must be // an intermediate point where the two signals are equal. This point must be
@ -60,12 +64,12 @@ where
} }
// the union of the sample points in self and other // the union of the sample points in self and other
let sync_points = self.sync_with_intersection(rhs).unwrap(); let sync_points = self.sync_with_intersection::<Linear>(rhs).unwrap();
sync_points sync_points
.into_iter() .into_iter()
.map(|t| { .map(|t| {
let lhs = self.interpolate_at(t, Linear).unwrap(); let lhs = self.interpolate_at::<Linear>(t).unwrap();
let rhs = rhs.interpolate_at(t, Linear).unwrap(); let rhs = rhs.interpolate_at::<Linear>(t).unwrap();
(t, lhs - rhs) (t, lhs - rhs)
}) })
.collect() .collect()
@ -74,25 +78,27 @@ where
impl<T> core::ops::Div for &Signal<T> impl<T> core::ops::Div for &Signal<T>
where where
T: core::ops::Div<T, Output = T> + Copy + LinearInterpolatable, T: core::ops::Div<T, Output = T> + Copy,
Linear: InterpolationMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
/// Divide the given signal with another /// Divide the given signal with another
fn div(self, rhs: Self) -> Self::Output { fn div(self, rhs: Self) -> Self::Output {
apply2(self, rhs, |lhs, rhs| lhs / rhs) apply2::<_, _, _, Linear>(self, rhs, |lhs, rhs| lhs / rhs)
} }
} }
impl<T> num_traits::Pow<Self> for &Signal<T> impl<T> num_traits::Pow<Self> for &Signal<T>
where where
T: num_traits::Pow<T, Output = T> + Copy + LinearInterpolatable, T: num_traits::Pow<T, Output = T> + Copy,
Linear: InterpolationMethod<T>,
{ {
type Output = Signal<T>; type Output = Signal<T>;
/// Returns the values in `self` to the power of the values in `other` /// Returns the values in `self` to the power of the values in `other`
fn pow(self, other: Self) -> Self::Output { fn pow(self, other: Self) -> Self::Output {
apply2(self, other, |lhs, rhs| lhs.pow(rhs)) apply2::<_, _, _, Linear>(self, other, |lhs, rhs| lhs.pow(rhs))
} }
} }

7
argus-core/src/signals/shift_ops.rs
View file

@ -3,12 +3,13 @@ use core::time::Duration;
use itertools::Itertools; use itertools::Itertools;
use super::traits::LinearInterpolatable; use super::interpolation::Linear;
use super::{InterpolationMethod, Signal}; use super::{InterpolationMethod, Signal};
impl<T> Signal<T> impl<T> Signal<T>
where where
T: Copy + LinearInterpolatable, T: Copy,
Linear: InterpolationMethod<T>,
{ {
/// Shift all samples in the signal by `delta` amount to the left. /// Shift all samples in the signal by `delta` amount to the left.
/// ///
@ -34,7 +35,7 @@ where
if idx > 0 && first_t != &delta { if idx > 0 && first_t != &delta {
// The shifted signal will not start at 0, and we have a previous // The shifted signal will not start at 0, and we have a previous
// index to interpolate from. // index to interpolate from.
let v = self.interpolate_at(delta, InterpolationMethod::Linear).unwrap(); let v = self.interpolate_at::<Linear>(delta).unwrap();
new_samples.push((Duration::ZERO, v)); new_samples.push((Duration::ZERO, v));
} }
// Shift the rest of the samples // Shift the rest of the samples

154
argus-core/src/signals/traits.rs
View file

@ -10,155 +10,23 @@ use super::utils::Neighborhood;
use super::{Sample, Signal}; use super::{Sample, Signal};
use crate::ArgusResult; use crate::ArgusResult;
/// Trait for values that are linear interpolatable /// Trait implemented by interpolation strategies
pub trait LinearInterpolatable { pub trait InterpolationMethod<T> {
/// Compute the linear interpolation of two samples at `time` /// Compute the interpolation of two samples at `time`.
/// ///
/// This should assume that the `time` value is between the sample times of `a` and /// Returns `None` if it isn't possible to interpolate at the given time using the
/// `b`. This should be enforced as an assertion. /// given samples.
fn interpolate_at(a: &Sample<Self>, b: &Sample<Self>, time: Duration) -> Self fn at(a: &Sample<T>, b: &Sample<T>, time: Duration) -> Option<T>;
where }
Self: Sized;
/// Trait implemented by interpolation strategies that allow finding the intersection of
/// two signal segments defined by start and end samples (see [`Neighborhood`]).
pub trait FindIntersectionMethod<T>: InterpolationMethod<T> {
/// Given two signals with two sample points each, find the intersection of the two /// Given two signals with two sample points each, find the intersection of the two
/// lines. /// lines.
fn find_intersection(a: &Neighborhood<Self>, b: &Neighborhood<Self>) -> Sample<Self> fn find_intersection(a: &Neighborhood<T>, b: &Neighborhood<T>) -> Sample<T>;
where
Self: Sized;
} }
impl LinearInterpolatable for bool {
fn interpolate_at(a: &Sample<Self>, b: &Sample<Self>, time: Duration) -> Self
where
Self: Sized,
{
assert!(a.time < time && time < b.time);
// We can't linear interpolate a boolean, so we return the previous.
a.value
}
fn find_intersection(a: &Neighborhood<Self>, b: &Neighborhood<Self>) -> Sample<Self>
where
Self: Sized,
{
let Sample { time: ta1, value: ya1 } = a.first.unwrap();
let Sample { time: ta2, value: ya2 } = a.second.unwrap();
let Sample { time: tb1, value: yb1 } = b.first.unwrap();
let Sample { time: tb2, value: yb2 } = b.second.unwrap();
let left_cmp = ya1.cmp(&yb1);
let right_cmp = ya2.cmp(&yb2);
if left_cmp.is_eq() {
// They already intersect, so we return the inner time-point
if ta1 < tb1 {
Sample { time: tb1, value: yb1 }
} else {
Sample { time: ta1, value: ya1 }
}
} else if right_cmp.is_eq() {
// They intersect at the end, so we return the outer time-point, as that is
// when they become equal.
if ta2 < tb2 {
Sample { time: tb2, value: yb2 }
} else {
Sample { time: ta2, value: ya2 }
}
} else {
// The switched, so the one that switched earlier will intersect with the
// other.
// So, we find the one that has a lower time point, i.e., the inner one.
if ta2 < tb2 {
Sample { time: ta2, value: ya2 }
} else {
Sample { time: tb2, value: yb2 }
}
}
}
}
macro_rules! interpolate_for_num {
($ty:ty) => {
impl LinearInterpolatable for $ty {
fn interpolate_at(first: &Sample<Self>, second: &Sample<Self>, time: Duration) -> Self
where
Self: Sized,
{
use num_traits::cast;
// We will need to cast the samples to f64 values (along with the time
// window) to be able to interpolate correctly.
// TODO(anand): Verify this works.
let t1 = first.time.as_secs_f64();
let t2 = second.time.as_secs_f64();
let at = time.as_secs_f64();
assert!((t1..=t2).contains(&at));
// We need to do stable linear interpolation
// https://www.open-std.org/jtc1/sc22/wg21/docs/papers/2019/p0811r3.html
let a: f64 = cast(first.value).unwrap();
let b: f64 = cast(second.value).unwrap();
// Set t to a value in [0, 1]
let t = (at - t1) / (t2 - t1);
assert!((0.0..=1.0).contains(&t));
let val = if (a <= 0.0 && b >= 0.0) || (a >= 0.0 && b <= 0.0) {
t * b + (1.0 - t) * a
} else if t == 1.0 {
b
} else {
a + t * (b - a)
};
cast(val).unwrap()
}
fn find_intersection(a: &Neighborhood<Self>, b: &Neighborhood<Self>) -> Sample<Self>
where
Self: Sized,
{
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
use num_traits::cast;
let Sample { time: t1, value: y1 } = a.first.unwrap();
let Sample { time: t2, value: y2 } = a.second.unwrap();
let Sample { time: t3, value: y3 } = b.first.unwrap();
let Sample { time: t4, value: y4 } = b.second.unwrap();
let t1 = t1.as_secs_f64();
let t2 = t2.as_secs_f64();
let t3 = t3.as_secs_f64();
let t4 = t4.as_secs_f64();
let y1: f64 = cast(y1).unwrap();
let y2: f64 = cast(y2).unwrap();
let y3: f64 = cast(y3).unwrap();
let y4: f64 = cast(y4).unwrap();
let denom = ((t1 - t2) * (y3 - y4)) - ((y1 - y2) * (t3 - t4));
let t_top = (((t1 * y2) - (y1 * t2)) * (t3 - t4)) - ((t1 - t2) * (t3 * y4 - y3 * t4));
let y_top = (((t1 * y2) - (y1 * t2)) * (y3 - y4)) - ((y1 - y2) * (t3 * y4 - y3 * t4));
let t = Duration::from_secs_f64(t_top / denom);
let y: Self = cast(y_top / denom).unwrap();
Sample { time: t, value: y }
}
}
};
}
interpolate_for_num!(i8);
interpolate_for_num!(i16);
interpolate_for_num!(i32);
interpolate_for_num!(i64);
interpolate_for_num!(u8);
interpolate_for_num!(u16);
interpolate_for_num!(u32);
interpolate_for_num!(u64);
interpolate_for_num!(f32);
interpolate_for_num!(f64);
/// Simple trait to be used as a trait object for [`Signal<T>`] types. /// Simple trait to be used as a trait object for [`Signal<T>`] types.
/// ///
/// This is mainly for external libraries to use for trait objects and downcasting to /// This is mainly for external libraries to use for trait objects and downcasting to

10
argus-core/src/signals/utils.rs
View file

@ -8,7 +8,6 @@ use core::ops::{Bound, RangeBounds};
use core::time::Duration; use core::time::Duration;
use std::iter::zip; use std::iter::zip;
use super::traits::LinearInterpolatable;
use super::{InterpolationMethod, Sample, Signal}; use super::{InterpolationMethod, Sample, Signal};
/// The neighborhood around a signal such that the time `at` is between the `first` and /// The neighborhood around a signal such that the time `at` is between the `first` and
@ -42,11 +41,12 @@ where
} }
#[inline] #[inline]
pub fn apply2<'a, T, U, F>(lhs: &'a Signal<T>, rhs: &'a Signal<T>, op: F) -> Signal<U> pub fn apply2<'a, T, U, F, Interp>(lhs: &'a Signal<T>, rhs: &'a Signal<T>, op: F) -> Signal<U>
where where
T: Copy + LinearInterpolatable, T: Copy,
U: Copy, U: Copy,
F: Fn(T, T) -> U, F: Fn(T, T) -> U,
Interp: InterpolationMethod<T>,
{ {
use Signal::*; use Signal::*;
// If either of the signals are empty, we return an empty signal. // If either of the signals are empty, we return an empty signal.
@ -67,8 +67,8 @@ where
time_points time_points
.into_iter() .into_iter()
.map(|t| { .map(|t| {
let v1 = lhs.interpolate_at(*t, InterpolationMethod::Linear).unwrap(); let v1 = lhs.interpolate_at::<Interp>(*t).unwrap();
let v2 = rhs.interpolate_at(*t, InterpolationMethod::Linear).unwrap(); let v2 = rhs.interpolate_at::<Interp>(*t).unwrap();
(*t, op(v1, v2)) (*t, op(v1, v2))
}) })
.collect() .collect()