mapf 0.3.0

Base traits and utilities for multi-agent planning
Documentation 
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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

/*
 * Copyright (C) 2023 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

use float_ord::FloatOrd;
use num::traits::Zero;
use std::{
    cmp::{Eq, Ord, Ordering, PartialEq, PartialOrd},
    hash::{Hash, Hasher},
    ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Sub, SubAssign},
};

/// Use Cost(f64) or Cost(f32) to use floating point values as planner costs,
/// giving them the traits of total ordering, full equivalence, and hashability.
#[derive(Clone, Copy, Debug)]
pub struct Cost<Num>(pub Num);

macro_rules! cost_impl {
    ($f:ident) => {
        impl Cost<$f> {
            fn convert(self) -> FloatOrd<$f> {
                FloatOrd(self.0)
            }
        }

        impl PartialEq for Cost<$f> {
            fn eq(&self, other: &Self) -> bool {
                self.convert() == other.convert()
            }
        }
        impl Eq for Cost<$f> {}
        impl PartialOrd for Cost<$f> {
            fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
                self.convert().partial_cmp(&other.convert())
            }
        }
        impl Ord for Cost<$f> {
            fn cmp(&self, other: &Self) -> Ordering {
                self.convert().cmp(&other.convert())
            }
        }
        impl Hash for Cost<$f> {
            fn hash<H: Hasher>(&self, state: &mut H) {
                self.convert().hash(state);
            }
        }
        impl Add for Cost<$f> {
            type Output = Self;
            fn add(self, rhs: Self) -> Self {
                Cost(self.0 + rhs.0)
            }
        }
        impl AddAssign for Cost<$f> {
            fn add_assign(&mut self, rhs: Self) {
                self.0 += rhs.0;
            }
        }
        impl Sub for Cost<$f> {
            type Output = Self;
            fn sub(self, rhs: Self) -> Self {
                Cost(self.0 - rhs.0)
            }
        }
        impl SubAssign for Cost<$f> {
            fn sub_assign(&mut self, rhs: Self) {
                self.0 -= rhs.0;
            }
        }
        impl Mul for Cost<$f> {
            type Output = Self;
            fn mul(self, rhs: Self) -> Self {
                Cost(self.0 * rhs.0)
            }
        }
        impl MulAssign for Cost<$f> {
            fn mul_assign(&mut self, rhs: Self) {
                self.0 *= rhs.0;
            }
        }
        impl Div for Cost<$f> {
            type Output = Self;
            fn div(self, rhs: Self) -> Self {
                Cost(self.0 / rhs.0)
            }
        }
        impl DivAssign for Cost<$f> {
            fn div_assign(&mut self, rhs: Self) {
                self.0 /= rhs.0;
            }
        }
        impl Zero for Cost<$f> {
            fn zero() -> Self {
                Cost(0.0)
            }
            fn set_zero(&mut self) {
                self.0 = 0.0
            }
            fn is_zero(&self) -> bool {
                self.0 == 0.0
            }
        }
    };
}

cost_impl!(f32);
cost_impl!(f64);