use std::iter::FromIterator;
use nalgebra::{storage::StorageMut, Dim, RealField, Vector};
#[derive(Debug, Clone, Copy)]
pub struct VariableBuilder<T: RealField + Copy>(Variable<T>);
impl<T: RealField + Copy> VariableBuilder<T> {
fn new() -> Self {
Self(Variable::new())
}
pub fn bounds(mut self, lower: T, upper: T) -> Self {
self.0.set_bounds(lower, upper);
self
}
pub fn magnitude(mut self, magnitude: T) -> Self {
self.0.set_magnitude(magnitude);
self
}
pub fn finalize(self) -> Variable<T> {
self.0
}
}
#[derive(Debug, Clone, Copy)]
pub struct Variable<T: RealField + Copy> {
bounds: (T, T),
magnitude: T,
}
impl<T: RealField + Copy> Variable<T> {
pub fn new() -> Self {
let inf = T::from_subset(&f64::INFINITY);
Self {
bounds: (-inf, inf),
magnitude: T::one(),
}
}
pub fn builder() -> VariableBuilder<T> {
VariableBuilder::new()
}
pub fn set_bounds(&mut self, lower: T, upper: T) -> &mut Self {
assert!(lower <= upper, "invalid bounds");
if lower.is_finite() && upper.is_finite() {
self.magnitude = estimate_magnitude(lower, upper);
}
self.bounds = (lower, upper);
self
}
pub fn set_magnitude(&mut self, magnitude: T) -> &mut Self {
assert!(magnitude > T::zero(), "magnitude must be positive");
self.magnitude = magnitude;
self
}
pub fn lower(&self) -> T {
self.bounds.0
}
pub fn upper(&self) -> T {
self.bounds.1
}
pub fn magnitude(&self) -> T {
self.magnitude
}
pub fn scale(&self) -> T {
T::one() / self.magnitude
}
pub fn is_within(&self, value: &T) -> bool {
value >= &self.lower() && value <= &self.upper()
}
pub fn clamp(&self, value: T) -> T {
if value < self.lower() {
self.lower()
} else if value > self.upper() {
self.upper()
} else {
value
}
}
}
impl<T: RealField + Copy> Default for Variable<T> {
fn default() -> Self {
Self::new()
}
}
impl<T: RealField + Copy> From<VariableBuilder<T>> for Variable<T> {
fn from(def: VariableBuilder<T>) -> Self {
def.finalize()
}
}
fn estimate_magnitude<T: RealField + Copy>(lower: T, upper: T) -> T {
let ten = T::from_subset(&10.0);
let half = T::from_subset(&0.5);
let avg = half * (lower.abs() + upper.abs());
let magnitude = ten.powf(avg.abs().log10().trunc());
if magnitude.is_finite() && magnitude > T::zero() {
magnitude
} else {
T::one()
}
}
#[macro_export]
macro_rules! var {
($lower:expr, $upper:expr; $magnitude:expr) => {
$crate::core::Variable::builder()
.bounds($lower, $upper)
.magnitude($magnitude)
.finalize()
};
($lower:expr, $upper:expr) => {
$crate::core::Variable::builder()
.bounds($lower, $upper)
.finalize()
};
($magnitude:expr) => {
$crate::core::Variable::builder()
.magnitude($magnitude)
.finalize()
};
}
pub struct Domain<T: RealField + Copy> {
vars: Vec<Variable<T>>,
}
impl<T: RealField + Copy> Domain<T> {
pub fn with_dim(n: usize) -> Self {
(0..n).map(|_| Variable::default()).collect()
}
pub fn with_vars(vars: Vec<Variable<T>>) -> Self {
assert!(!vars.is_empty(), "empty domain");
Self { vars }
}
pub fn vars(&self) -> &[Variable<T>] {
self.vars.as_slice()
}
}
impl<T: RealField + Copy> FromIterator<Variable<T>> for Domain<T> {
fn from_iter<I: IntoIterator<Item = Variable<T>>>(iter: I) -> Self {
Self::with_vars(iter.into_iter().collect())
}
}
impl<T: RealField + Copy> From<Vec<Variable<T>>> for Domain<T> {
fn from(vars: Vec<Variable<T>>) -> Self {
Self::with_vars(vars)
}
}
pub trait VectorDomainExt<T: RealField + Copy, D: Dim> {
fn project(&mut self, dom: &Domain<T>) -> bool;
}
impl<T: RealField + Copy, D: Dim, S> VectorDomainExt<T, D> for Vector<T, D, S>
where
S: StorageMut<T, D>,
{
fn project(&mut self, dom: &Domain<T>) -> bool {
let not_feasible = self
.iter()
.zip(dom.vars().iter())
.any(|(xi, vi)| !vi.is_within(xi));
if not_feasible {
self.iter_mut()
.zip(dom.vars().iter())
.for_each(|(xi, vi)| *xi = vi.clamp(*xi));
}
not_feasible
}
}
#[cfg(test)]
#[allow(clippy::float_cmp)]
mod tests {
use super::*;
macro_rules! magnitude_of {
($lower:expr, $upper:expr) => {
Variable::builder()
.bounds($lower, $upper)
.finalize()
.magnitude()
};
}
#[test]
fn magnitude() {
assert_eq!(magnitude_of!(-1e10f64, 1e10).log10(), 10.0);
assert_eq!(magnitude_of!(-1e4f64, -1e2).log10(), 3.0);
assert_eq!(magnitude_of!(-6e-6f64, 9e-6).log10().trunc(), -5.0);
assert_eq!(magnitude_of!(-6e-6f64, 9e-6) / 1e-5, 1.0);
}
#[test]
fn magnitude_when_bound_is_zero() {
assert_eq!(magnitude_of!(0f64, 1e2).log10(), 1.0);
assert_eq!(magnitude_of!(-1e2f64, 0.0).log10(), 1.0);
}
#[test]
fn edge_cases() {
assert_eq!(magnitude_of!(0.0f64, 0.0), 1.0);
}
}