src/polytraj.rs

   1 //! polynomial trajectory
   2
   3 use crate::float;
   4
   5 /// cubic polynomial trajectory
   6 #[repr(C)]
   7 pub struct polytraj3 {
   8     /// coefficients of position
   9     pub q: [float; 4],
  10     /// coefficients of velocity
  11     pub v: [float; 3],
  12     /// coefficients of acceleration
  13     pub a: [float; 2],
  14 }
  15
  16 extern "C" {
  17     fn a_polytraj3_gen(
  18         ctx: *mut polytraj3,
  19         t0: float,
  20         t1: float,
  21         q0: float,
  22         q1: float,
  23         v0: float,
  24         v1: float,
  25     );
  26     fn a_polytraj3_pos(ctx: *const polytraj3, dt: float) -> float;
  27     fn a_polytraj3_vel(ctx: *const polytraj3, dt: float) -> float;
  28     fn a_polytraj3_acc(ctx: *const polytraj3, dt: float) -> float;
  29 }
  30
  31 impl polytraj3 {
  32     /// initialize for cubic polynomial trajectory
  33     pub fn new(t0: float, t1: float, q0: float, q1: float, v0: float, v1: float) -> Self {
  34         let mut ctx: Self = Self {
  35             q: [0.0; 4],
  36             v: [0.0; 3],
  37             a: [0.0; 2],
  38         };
  39         unsafe { a_polytraj3_gen(&mut ctx, t0, t1, q0, q1, v0, v1) };
  40         ctx
  41     }
  42
  43     /// generate for cubic polynomial trajectory
  44     pub fn gen(
  45         &mut self,
  46         t0: float,
  47         t1: float,
  48         q0: float,
  49         q1: float,
  50         v0: float,
  51         v1: float,
  52     ) -> &mut Self {
  53         unsafe { a_polytraj3_gen(self, t0, t1, q0, q1, v0, v1) };
  54         self
  55     }
  56
  57     /// calculate for cubic polynomial trajectory position
  58     pub fn pos(&mut self, dt: float) -> float {
  59         unsafe { a_polytraj3_pos(self, dt) }
  60     }
  61
  62     /// calculate for cubic polynomial trajectory velocity
  63     pub fn vel(&mut self, dt: float) -> float {
  64         unsafe { a_polytraj3_vel(self, dt) }
  65     }
  66
  67     /// calculate for cubic polynomial trajectory acceleration
  68     pub fn acc(&mut self, dt: float) -> float {
  69         unsafe { a_polytraj3_acc(self, dt) }
  70     }
  71 }
  72
  73 #[test]
  74 fn polytraj3() {
  75     extern crate std;
  76     let dt = 0.5;
  77     {
  78         let mut a = crate::polytraj::polytraj3::new(0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
  79         let pos = a.pos(dt);
  80         let vel = a.vel(dt);
  81         let acc = a.acc(dt);
  82         std::println!("[{}, {}, {}]", pos, vel, acc);
  83     }
  84 }
  85
  86 /// quintic polynomial trajectory
  87 #[repr(C)]
  88 pub struct polytraj5 {
  89     /// coefficients of position
  90     pub q: [float; 6],
  91     /// coefficients of velocity
  92     pub v: [float; 5],
  93     /// coefficients of acceleration
  94     pub a: [float; 4],
  95 }
  96
  97 extern "C" {
  98     fn a_polytraj5_gen(
  99         ctx: *mut polytraj5,
 100         t0: float,
 101         t1: float,
 102         q0: float,
 103         q1: float,
 104         v0: float,
 105         v1: float,
 106         a0: float,
 107         a1: float,
 108     );
 109     fn a_polytraj5_pos(ctx: *const polytraj5, dt: float) -> float;
 110     fn a_polytraj5_vel(ctx: *const polytraj5, dt: float) -> float;
 111     fn a_polytraj5_acc(ctx: *const polytraj5, dt: float) -> float;
 112 }
 113
 114 impl polytraj5 {
 115     /// initialize for quintic polynomial trajectory
 116     #[allow(clippy::too_many_arguments)]
 117     pub fn new(
 118         t0: float,
 119         t1: float,
 120         q0: float,
 121         q1: float,
 122         v0: float,
 123         v1: float,
 124         a0: float,
 125         a1: float,
 126     ) -> Self {
 127         let mut ctx: Self = Self {
 128             q: [0.0; 6],
 129             v: [0.0; 5],
 130             a: [0.0; 4],
 131         };
 132         unsafe { a_polytraj5_gen(&mut ctx, t0, t1, q0, q1, v0, v1, a0, a1) };
 133         ctx
 134     }
 135
 136     /// generate for quintic polynomial trajectory
 137     #[allow(clippy::too_many_arguments)]
 138     pub fn gen(
 139         &mut self,
 140         t0: float,
 141         t1: float,
 142         q0: float,
 143         q1: float,
 144         v0: float,
 145         v1: float,
 146         a0: float,
 147         a1: float,
 148     ) -> &mut Self {
 149         unsafe { a_polytraj5_gen(self, t0, t1, q0, q1, v0, v1, a0, a1) };
 150         self
 151     }
 152
 153     /// calculate for quintic polynomial trajectory position
 154     pub fn pos(&mut self, dt: float) -> float {
 155         unsafe { a_polytraj5_pos(self, dt) }
 156     }
 157
 158     /// calculate for quintic polynomial trajectory velocity
 159     pub fn vel(&mut self, dt: float) -> float {
 160         unsafe { a_polytraj5_vel(self, dt) }
 161     }
 162
 163     /// calculate for quintic polynomial trajectory acceleration
 164     pub fn acc(&mut self, dt: float) -> float {
 165         unsafe { a_polytraj5_acc(self, dt) }
 166     }
 167 }
 168
 169 #[test]
 170 fn polytraj5() {
 171     extern crate std;
 172     let dt = 0.5;
 173     {
 174         let mut a = crate::polytraj::polytraj5::new(0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
 175         let pos = a.pos(dt);
 176         let vel = a.vel(dt);
 177         let acc = a.acc(dt);
 178         std::println!("[{}, {}, {}]", pos, vel, acc);
 179     }
 180 }
 181
 182 /// hepta polynomial trajectory
 183 #[repr(C)]
 184 pub struct polytraj7 {
 185     /// coefficients of position
 186     pub q: [float; 8],
 187     /// coefficients of velocity
 188     pub v: [float; 7],
 189     /// coefficients of acceleration
 190     pub a: [float; 6],
 191     /// coefficients of jerk
 192     pub j: [float; 5],
 193 }
 194
 195 extern "C" {
 196     fn a_polytraj7_gen(
 197         ctx: *mut polytraj7,
 198         t0: float,
 199         t1: float,
 200         q0: float,
 201         q1: float,
 202         v0: float,
 203         v1: float,
 204         a0: float,
 205         a1: float,
 206         j0: float,
 207         j1: float,
 208     );
 209     fn a_polytraj7_pos(ctx: *const polytraj7, dt: float) -> float;
 210     fn a_polytraj7_vel(ctx: *const polytraj7, dt: float) -> float;
 211     fn a_polytraj7_acc(ctx: *const polytraj7, dt: float) -> float;
 212     fn a_polytraj7_jer(ctx: *const polytraj7, dt: float) -> float;
 213 }
 214
 215 impl polytraj7 {
 216     /// initialize for hepta polynomial trajectory
 217     #[allow(clippy::too_many_arguments)]
 218     pub fn new(
 219         t0: float,
 220         t1: float,
 221         q0: float,
 222         q1: float,
 223         v0: float,
 224         v1: float,
 225         a0: float,
 226         a1: float,
 227         j0: float,
 228         j1: float,
 229     ) -> Self {
 230         let mut ctx: Self = Self {
 231             q: [0.0; 8],
 232             v: [0.0; 7],
 233             a: [0.0; 6],
 234             j: [0.0; 5],
 235         };
 236         unsafe { a_polytraj7_gen(&mut ctx, t0, t1, q0, q1, v0, v1, a0, a1, j0, j1) };
 237         ctx
 238     }
 239
 240     /// generate for hepta polynomial trajectory
 241     #[allow(clippy::too_many_arguments)]
 242     pub fn gen(
 243         &mut self,
 244         t0: float,
 245         t1: float,
 246         q0: float,
 247         q1: float,
 248         v0: float,
 249         v1: float,
 250         a0: float,
 251         a1: float,
 252         j0: float,
 253         j1: float,
 254     ) -> &mut Self {
 255         unsafe { a_polytraj7_gen(self, t0, t1, q0, q1, v0, v1, a0, a1, j0, j1) };
 256         self
 257     }
 258
 259     /// calculate for hepta polynomial trajectory position
 260     pub fn pos(&mut self, dt: float) -> float {
 261         unsafe { a_polytraj7_pos(self, dt) }
 262     }
 263
 264     /// calculate for hepta polynomial trajectory velocity
 265     pub fn vel(&mut self, dt: float) -> float {
 266         unsafe { a_polytraj7_vel(self, dt) }
 267     }
 268
 269     /// calculate for hepta polynomial trajectory acceleration
 270     pub fn acc(&mut self, dt: float) -> float {
 271         unsafe { a_polytraj7_acc(self, dt) }
 272     }
 273
 274     /// calculate for hepta polynomial trajectory jerk
 275     pub fn jer(&mut self, dt: float) -> float {
 276         unsafe { a_polytraj7_jer(self, dt) }
 277     }
 278 }
 279
 280 #[test]
 281 fn polytraj7() {
 282     extern crate std;
 283     let dt = 0.5;
 284     {
 285         let mut a =
 286             crate::polytraj::polytraj7::new(0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
 287         let pos = a.pos(dt);
 288         let vel = a.vel(dt);
 289         let acc = a.acc(dt);
 290         let jer = a.jer(dt);
 291         std::println!("[{}, {}, {}, {}]", pos, vel, acc, jer);
 292     }
 293 }