| blob | b34d7d1991a72863720aa80e94ae7649406deb60 |
1 /*!
2 An algorithm library based on C/C++
4 ## Installation
6 Add this to your `Cargo.toml`:
8 ```toml
9 [dependencies]
10 liba = "0.1"
11 ```
12 */
14 #![warn(missing_docs)]
15 #![allow(non_camel_case_types)]
16 #![cfg_attr(not(features = "std"), no_std)]
18 /// Equivalent to C's int type.
19 pub type int = core::ffi::c_int;
20 /// Equivalent to C's unsigned int type.
21 pub type uint = core::ffi::c_uint;
22 /// floating-point number stored using `f64`
23 #[cfg(not(feature = "float"))]
24 pub type float = f64;
25 /// floating-point number stored using `f32`
26 #[cfg(feature = "float")]
27 pub type float = f32;
29 extern "C" {
30 fn a_u32_sqrt(x: u32) -> u16;
31 fn a_u64_sqrt(x: u64) -> u32;
32 }
34 /// square root of an unsigned integer
35 #[inline(always)]
36 pub fn u32_sqrt(x: u32) -> u16 {
37 unsafe { a_u32_sqrt(x) }
38 }
39 /// square root of an unsigned integer
40 #[inline(always)]
41 pub fn u64_sqrt(x: u64) -> u32 {
42 unsafe { a_u64_sqrt(x) }
43 }
45 extern "C" {
46 fn a_f32_rsqrt(x: f32) -> f32;
47 fn a_f64_rsqrt(x: f64) -> f64;
48 }
50 /// reciprocal of square-root, `\frac{1}{\sqrt{x}}`
51 #[inline(always)]
52 pub fn f32_rsqrt(x: f32) -> f32 {
53 unsafe { a_f32_rsqrt(x) }
54 }
55 /// reciprocal of square-root, `\frac{1}{\sqrt{x}}`
56 #[inline(always)]
57 pub fn f64_rsqrt(x: f64) -> f64 {
58 unsafe { a_f64_rsqrt(x) }
59 }
61 extern "C" {
62 fn a_hash_bkdr_(pdata: *const u8, nbyte: usize, value: u32) -> u32;
63 fn a_hash_sdbm_(pdata: *const u8, nbyte: usize, value: u32) -> u32;
64 }
66 /// a hash function whose prime number is 131
67 #[inline(always)]
68 pub fn hash_bkdr(block: &[u8], value: u32) -> u32 {
69 unsafe { a_hash_bkdr_(block.as_ptr(), block.len(), value) }
70 }
71 /// a hash function whose prime number is 65599
72 #[inline(always)]
73 pub fn hash_sdbm(block: &[u8], value: u32) -> u32 {
74 unsafe { a_hash_sdbm_(block.as_ptr(), block.len(), value) }
75 }
77 mod test {
78 extern crate std;
79 #[test]
80 fn usqrt() {
81 std::println!("{}", crate::u32_sqrt(u32::MAX));
82 std::println!("{}", crate::u64_sqrt(u64::MAX));
83 }
84 #[test]
85 fn rsqrt() {
86 std::println!("1/sqrt({})={}", 4, crate::f32_rsqrt(4.0));
87 std::println!("1/sqrt({})={}", 4, crate::f64_rsqrt(4.0));
88 }
89 #[test]
90 fn hash() {
91 let text: [u8; 10] = *b"0123456789";
92 std::println!("{}", crate::hash_bkdr(&text, 0));
93 std::println!("{}", crate::hash_sdbm(&text, 0));
94 }
95 }
97 /// Cyclic Redundancy Check for 8 bits
98 #[repr(C)]
99 pub struct crc8 {
100 /// Cyclic Redundancy Check comparison table
101 pub table: [u8; 0x100],
102 }
104 extern "C" {
105 fn a_crc8m_init(table: *mut u8, poly: u8);
106 fn a_crc8l_init(table: *mut u8, poly: u8);
107 fn a_crc8(table: *const u8, pdate: *const u8, nbyte: usize, value: u8) -> u8;
108 }
110 impl crc8 {
111 /// initialize for MSB CRC-8
112 #[inline(always)]
113 pub fn new_msb(poly: u8) -> Self {
114 let mut ctx: Self = Self { table: [0; 0x100] };
115 unsafe { a_crc8m_init(ctx.table.as_mut_ptr(), poly) };
116 ctx
117 }
118 /// initialize for LSB CRC-8
119 #[inline(always)]
120 pub fn new_lsb(poly: u8) -> Self {
121 let mut ctx: Self = Self { table: [0; 0x100] };
122 unsafe { a_crc8l_init(ctx.table.as_mut_ptr(), poly) };
123 ctx
124 }
125 /// generate for MSB CRC-8
126 #[inline(always)]
127 pub fn gen_msb(&mut self, poly: u8) -> &mut Self {
128 unsafe { a_crc8m_init(self.table.as_mut_ptr(), poly) };
129 self
130 }
131 /// generate for LSB CRC-8
132 #[inline(always)]
133 pub fn gen_lsb(&mut self, poly: u8) -> &mut Self {
134 unsafe { a_crc8l_init(self.table.as_mut_ptr(), poly) };
135 self
136 }
137 /// calculate for CRC-8
138 #[inline(always)]
139 pub fn eval(self, block: &[u8], value: u8) -> u8 {
140 unsafe { a_crc8(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
141 }
142 }
144 #[test]
145 fn crc8() {
146 extern crate std;
147 {
148 let ctx = crate::crc8::new_msb(0x07);
149 assert_eq!(ctx.eval(b"0123456789", 0), 0x45);
150 }
151 {
152 let ctx = crate::crc8::new_lsb(0x31);
153 assert_eq!(ctx.eval(b"0123456789", 0), 0x75);
154 }
155 }
157 /// Cyclic Redundancy Check for 16 bits
158 #[repr(C)]
159 pub struct crc16 {
160 /// Cyclic Redundancy Check comparison table
161 pub table: [u16; 0x100],
162 eval: unsafe extern "C" fn(*const u16, *const u8, usize, u16) -> u16,
163 }
165 extern "C" {
166 fn a_crc16m_init(table: *mut u16, poly: u16);
167 fn a_crc16l_init(table: *mut u16, poly: u16);
168 fn a_crc16m(table: *const u16, pdate: *const u8, nbyte: usize, value: u16) -> u16;
169 fn a_crc16l(table: *const u16, pdate: *const u8, nbyte: usize, value: u16) -> u16;
170 }
172 impl crc16 {
173 /// initialize for MSB CRC-16
174 #[inline(always)]
175 pub fn new_msb(poly: u16) -> Self {
176 let mut ctx: Self = Self {
177 table: [0; 0x100],
178 eval: a_crc16m,
179 };
180 unsafe { a_crc16m_init(ctx.table.as_mut_ptr(), poly) };
181 ctx
182 }
183 /// initialize for LSB CRC-16
184 #[inline(always)]
185 pub fn new_lsb(poly: u16) -> Self {
186 let mut ctx: Self = Self {
187 table: [0; 0x100],
188 eval: a_crc16l,
189 };
190 unsafe { a_crc16l_init(ctx.table.as_mut_ptr(), poly) };
191 ctx
192 }
193 /// generate for MSB CRC-16
194 #[inline(always)]
195 pub fn gen_msb(&mut self, poly: u16) -> &mut Self {
196 unsafe { a_crc16m_init(self.table.as_mut_ptr(), poly) };
197 self.eval = a_crc16m;
198 self
199 }
200 /// generate for LSB CRC-16
201 #[inline(always)]
202 pub fn gen_lsb(&mut self, poly: u16) -> &mut Self {
203 unsafe { a_crc16l_init(self.table.as_mut_ptr(), poly) };
204 self.eval = a_crc16l;
205 self
206 }
207 /// calculate for CRC-16
208 #[inline(always)]
209 pub fn eval(self, block: &[u8], value: u16) -> u16 {
210 unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
211 }
212 }
214 #[test]
215 fn crc16() {
216 extern crate std;
217 {
218 let ctx = crate::crc16::new_msb(0x1021);
219 assert_eq!(ctx.eval(b"0123456789", 0), 0x9C58);
220 }
221 {
222 let ctx = crate::crc16::new_lsb(0x8005);
223 assert_eq!(ctx.eval(b"0123456789", 0), 0x443D);
224 }
225 }
227 /// Cyclic Redundancy Check for 32 bits
228 #[repr(C)]
229 pub struct crc32 {
230 /// Cyclic Redundancy Check comparison table
231 pub table: [u32; 0x100],
232 eval: unsafe extern "C" fn(*const u32, *const u8, usize, u32) -> u32,
233 }
235 extern "C" {
236 fn a_crc32m_init(table: *mut u32, poly: u32);
237 fn a_crc32l_init(table: *mut u32, poly: u32);
238 fn a_crc32m(table: *const u32, pdate: *const u8, nbyte: usize, value: u32) -> u32;
239 fn a_crc32l(table: *const u32, pdate: *const u8, nbyte: usize, value: u32) -> u32;
240 }
242 impl crc32 {
243 /// initialize for MSB CRC-32
244 #[inline(always)]
245 pub fn new_msb(poly: u32) -> Self {
246 let mut ctx: Self = Self {
247 table: [0; 0x100],
248 eval: a_crc32m,
249 };
250 unsafe { a_crc32m_init(ctx.table.as_mut_ptr(), poly) };
251 ctx
252 }
253 /// initialize for LSB CRC-32
254 #[inline(always)]
255 pub fn new_lsb(poly: u32) -> Self {
256 let mut ctx: Self = Self {
257 table: [0; 0x100],
258 eval: a_crc32l,
259 };
260 unsafe { a_crc32l_init(ctx.table.as_mut_ptr(), poly) };
261 ctx
262 }
263 /// generate for MSB CRC-32
264 #[inline(always)]
265 pub fn gen_msb(&mut self, poly: u32) -> &mut Self {
266 unsafe { a_crc32m_init(self.table.as_mut_ptr(), poly) };
267 self.eval = a_crc32m;
268 self
269 }
270 /// generate for LSB CRC-32
271 #[inline(always)]
272 pub fn gen_lsb(&mut self, poly: u32) -> &mut Self {
273 unsafe { a_crc32l_init(self.table.as_mut_ptr(), poly) };
274 self.eval = a_crc32l;
275 self
276 }
277 /// calculate for CRC-32
278 #[inline(always)]
279 pub fn eval(self, block: &[u8], value: u32) -> u32 {
280 unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
281 }
282 }
284 #[test]
285 fn crc32() {
286 extern crate std;
287 {
288 let ctx = crate::crc32::new_msb(0x1EDC6F41);
289 assert_eq!(ctx.eval(b"0123456789", 0), 0x512B456E);
290 }
291 {
292 let ctx = crate::crc32::new_lsb(0x04C11DB7);
293 assert_eq!(ctx.eval(b"0123456789", 0), 0x450EAFB0);
294 }
295 }
297 /// Cyclic Redundancy Check for 64 bits
298 #[repr(C)]
299 pub struct crc64 {
300 /// Cyclic Redundancy Check comparison table
301 pub table: [u64; 0x100],
302 eval: unsafe extern "C" fn(*const u64, *const u8, usize, u64) -> u64,
303 }
305 extern "C" {
306 fn a_crc64m_init(table: *mut u64, poly: u64);
307 fn a_crc64l_init(table: *mut u64, poly: u64);
308 fn a_crc64m(table: *const u64, pdate: *const u8, nbyte: usize, value: u64) -> u64;
309 fn a_crc64l(table: *const u64, pdate: *const u8, nbyte: usize, value: u64) -> u64;
310 }
312 impl crc64 {
313 /// initialize for MSB CRC-64
314 #[inline(always)]
315 pub fn new_msb(poly: u64) -> Self {
316 let mut ctx: Self = Self {
317 table: [0; 0x100],
318 eval: a_crc64m,
319 };
320 unsafe { a_crc64m_init(ctx.table.as_mut_ptr(), poly) };
321 ctx
322 }
323 /// initialize for LSB CRC-64
324 #[inline(always)]
325 pub fn new_lsb(poly: u64) -> Self {
326 let mut ctx: Self = Self {
327 table: [0; 0x100],
328 eval: a_crc64l,
329 };
330 unsafe { a_crc64l_init(ctx.table.as_mut_ptr(), poly) };
331 ctx
332 }
333 /// generate for MSB CRC-64
334 #[inline(always)]
335 pub fn gen_msb(&mut self, poly: u64) -> &mut Self {
336 unsafe { a_crc64m_init(self.table.as_mut_ptr(), poly) };
337 self.eval = a_crc64m;
338 self
339 }
340 /// generate for LSB CRC-64
341 #[inline(always)]
342 pub fn gen_lsb(&mut self, poly: u64) -> &mut Self {
343 unsafe { a_crc64l_init(self.table.as_mut_ptr(), poly) };
344 self.eval = a_crc64l;
345 self
346 }
347 /// calculate for CRC-64
348 #[inline(always)]
349 pub fn eval(self, block: &[u8], value: u64) -> u64 {
350 unsafe { (self.eval)(self.table.as_ptr(), block.as_ptr(), block.len(), value) }
351 }
352 }
354 #[test]
355 fn crc64() {
356 extern crate std;
357 {
358 let ctx = crate::crc64::new_msb(0x000000000000001B);
359 assert_eq!(ctx.eval(b"0123456789", 0), 0xE4FFBEA588AFC790);
360 }
361 {
362 let ctx = crate::crc64::new_lsb(0x42F0E1EBA9EA3693);
363 assert_eq!(ctx.eval(b"0123456789", 0), 0xDA60676A5CDE0008);
364 }
365 }
367 #[allow(clippy::excessive_precision)]
368 const TAU: float = 6.28318530717958647692528676655900577;
370 /// High Pass Filter
371 #[repr(C)]
372 pub struct hpf {
373 /// filter coefficient
374 pub alpha: float,
375 /// filter output
376 pub output: float,
377 /// filter input
378 pub input: float,
379 }
381 impl hpf {
382 /// initialize for High Pass Filter
383 #[inline(always)]
384 pub fn new(fc: float, ts: float) -> Self {
385 Self {
386 alpha: 1.0 / (TAU * fc * ts + 1.0),
387 output: 0.0,
388 input: 0.0,
389 }
390 }
391 /// generate for High Pass Filter
392 #[inline(always)]
393 pub fn gen(&mut self, fc: float, ts: float) -> &mut Self {
394 self.alpha = 1.0 / (TAU * fc * ts + 1.0);
395 self
396 }
397 /// calculate for High Pass Filter
398 #[inline(always)]
399 pub fn iter(&mut self, x: float) -> float {
400 self.output = self.alpha * (self.output + x - self.input);
401 self.input = x;
402 self.output
403 }
404 /// zeroing for High Pass Filter
405 #[inline(always)]
406 pub fn zero(&mut self) -> &mut Self {
407 self.output = 0.0;
408 self.input = 0.0;
409 self
410 }
411 }
413 #[test]
414 fn hpf() {
415 extern crate std;
416 let mut a = crate::hpf::new(10.0, 0.01);
417 a.gen(10.0, 0.01).zero().iter(1.0);
418 }
420 #[allow(clippy::excessive_precision)]
421 const _1_TAU: float = 0.159154943091895335768883763372514362;
423 /// Low Pass Filter
424 #[repr(C)]
425 pub struct lpf {
426 /// filter coefficient
427 pub alpha: float,
428 /// filter output
429 pub output: float,
430 }
432 impl lpf {
433 /// initialize for Low Pass Filter
434 #[inline(always)]
435 pub fn new(fc: float, ts: float) -> Self {
436 Self {
437 alpha: ts / (_1_TAU / fc + ts),
438 output: 0.0,
439 }
440 }
441 /// generate for Low Pass Filter
442 #[inline(always)]
443 pub fn gen(&mut self, fc: float, ts: float) -> &mut Self {
444 self.alpha = ts / (_1_TAU / fc + ts);
445 self
446 }
447 /// calculate for Low Pass Filter
448 #[inline(always)]
449 pub fn iter(&mut self, x: float) -> float {
450 self.output *= 1.0 - self.alpha;
451 self.output += x * self.alpha;
452 self.output
453 }
454 /// zeroing for Low Pass Filter
455 #[inline(always)]
456 pub fn zero(&mut self) -> &mut Self {
457 self.output = 0.0;
458 self
459 }
460 }
462 #[test]
463 fn lpf() {
464 extern crate std;
465 let mut a = crate::lpf::new(10.0, 0.01);
466 a.gen(10.0, 0.01).zero().iter(1.0);
467 }
469 /// membership function
470 pub mod mf {
471 use crate::int;
472 /// none
473 pub const NUL: int = 0;
474 /// gaussian membership function
475 pub const GAUSS: int = 1;
476 /// gaussian combination membership function
477 pub const GAUSS2: int = 2;
478 /// generalized bell-shaped membership function
479 pub const GBELL: int = 3;
480 /// sigmoidal membership function
481 pub const SIG: int = 4;
482 /// difference between two sigmoidal membership functions
483 pub const DSIG: int = 5;
484 /// product of two sigmoidal membership functions
485 pub const PSIG: int = 6;
486 /// trapezoidal membership function
487 pub const TRAP: int = 7;
488 /// triangular membership function
489 pub const TRI: int = 8;
490 /// linear s-shaped saturation membership function
491 pub const LINS: int = 9;
492 /// linear z-shaped saturation membership function
493 pub const LINZ: int = 10;
494 /// s-shaped membership function
495 pub const S: int = 11;
496 /// z-shaped membership function
497 pub const Z: int = 12;
498 /// pi-shaped membership function
499 pub const PI: int = 13;
501 use crate::float;
502 extern "C" {
503 fn a_mf_gauss(x: float, sigma: float, c: float) -> float;
504 fn a_mf_gauss2(x: float, sigma1: float, c1: float, sigma2: float, c2: float) -> float;
505 fn a_mf_gbell(x: float, a: float, b: float, c: float) -> float;
506 fn a_mf_sig(x: float, a: float, c: float) -> float;
507 fn a_mf_dsig(x: float, a1: float, c1: float, a2: float, c2: float) -> float;
508 fn a_mf_psig(x: float, a1: float, c1: float, a2: float, c2: float) -> float;
509 fn a_mf_trap(x: float, a: float, b: float, c: float, d: float) -> float;
510 fn a_mf_tri(x: float, a: float, b: float, c: float) -> float;
511 fn a_mf_lins(x: float, a: float, b: float) -> float;
512 fn a_mf_linz(x: float, a: float, b: float) -> float;
513 fn a_mf_s(x: float, a: float, b: float) -> float;
514 fn a_mf_z(x: float, a: float, b: float) -> float;
515 fn a_mf_pi(x: float, a: float, b: float, c: float, d: float) -> float;
516 }
518 /// gaussian membership function
519 #[inline(always)]
520 pub fn gauss(x: float, sigma: float, c: float) -> float {
521 unsafe { a_mf_gauss(x, sigma, c) }
522 }
523 /// gaussian combination membership function
524 #[inline(always)]
525 pub fn gauss2(x: float, sigma1: float, c1: float, sigma2: float, c2: float) -> float {
526 unsafe { a_mf_gauss2(x, sigma1, c1, sigma2, c2) }
527 }
528 /// generalized bell-shaped membership function
529 #[inline(always)]
530 pub fn gbell(x: float, a: float, b: float, c: float) -> float {
531 unsafe { a_mf_gbell(x, a, b, c) }
532 }
533 /// sigmoidal membership function
534 #[inline(always)]
535 pub fn sig(x: float, a: float, c: float) -> float {
536 unsafe { a_mf_sig(x, a, c) }
537 }
538 /// difference between two sigmoidal membership functions
539 #[inline(always)]
540 pub fn dsig(x: float, a1: float, c1: float, a2: float, c2: float) -> float {
541 unsafe { a_mf_dsig(x, a1, c1, a2, c2) }
542 }
543 /// product of two sigmoidal membership functions
544 #[inline(always)]
545 pub fn psig(x: float, a1: float, c1: float, a2: float, c2: float) -> float {
546 unsafe { a_mf_psig(x, a1, c1, a2, c2) }
547 }
548 /// trapezoidal membership function
549 #[inline(always)]
550 pub fn trap(x: float, a: float, b: float, c: float, d: float) -> float {
551 unsafe { a_mf_trap(x, a, b, c, d) }
552 }
553 /// triangular membership function
554 #[inline(always)]
555 pub fn tri(x: float, a: float, b: float, c: float) -> float {
556 unsafe { a_mf_tri(x, a, b, c) }
557 }
558 /// linear s-shaped saturation membership function
559 #[inline(always)]
560 pub fn lins(x: float, a: float, b: float) -> float {
561 unsafe { a_mf_lins(x, a, b) }
562 }
563 /// linear z-shaped saturation membership function
564 #[inline(always)]
565 pub fn linz(x: float, a: float, b: float) -> float {
566 unsafe { a_mf_linz(x, a, b) }
567 }
568 /// s-shaped membership function
569 #[inline(always)]
570 pub fn s(x: float, a: float, b: float) -> float {
571 unsafe { a_mf_s(x, a, b) }
572 }
573 /// z-shaped membership function
574 #[inline(always)]
575 pub fn z(x: float, a: float, b: float) -> float {
576 unsafe { a_mf_z(x, a, b) }
577 }
578 /// pi-shaped membership function
579 #[inline(always)]
580 pub fn pi(x: float, a: float, b: float, c: float, d: float) -> float {
581 unsafe { a_mf_pi(x, a, b, c, d) }
582 }
584 #[cfg(test)]
585 mod test {
586 extern crate std;
587 #[test]
588 fn gauss2() {
589 for i in -4..=4 {
590 let x = i as crate::float;
591 std::println!("{:+} {}", x, crate::mf::gauss2(x, 1.0, -1.0, 1.0, 1.0));
592 }
593 }
594 #[test]
595 fn gbell() {
596 for i in -4..=4 {
597 let x = i as crate::float;
598 std::println!("{:+} {}", x, crate::mf::gbell(x, 2.0, 4.0, 0.0));
599 }
600 }
601 #[test]
602 fn sig() {
603 for i in -4..=4 {
604 let x = i as crate::float;
605 std::println!("{:+} {}", x, crate::mf::sig(x, 2.0, 0.0));
606 }
607 }
608 #[test]
609 fn dsig() {
610 for i in -4..=4 {
611 let x = i as crate::float;
612 std::println!("{:+} {}", x, crate::mf::dsig(x, 5.0, -2.0, 5.0, 2.0));
613 }
614 }
615 #[test]
616 fn psig() {
617 for i in -4..=4 {
618 let x = i as crate::float;
619 std::println!("{:+} {}", x, crate::mf::psig(x, 5.0, -2.0, -5.0, 2.0));
620 }
621 }
622 #[test]
623 fn trap() {
624 for i in -3..=3 {
625 let x = i as crate::float;
626 std::println!("{:+} {}", x, crate::mf::trap(x, -2.0, -1.0, 1.0, 2.0));
627 }
628 }
629 #[test]
630 fn tri() {
631 for i in -2..=2 {
632 let x = i as crate::float;
633 std::println!("{:+} {}", x, crate::mf::tri(x, -1.0, 0.0, 1.0));
634 }
635 }
636 #[test]
637 fn lins() {
638 for i in -2..=2 {
639 let x = i as crate::float;
640 std::println!("{:+} {}", x, crate::mf::lins(x, -1.0, 1.0));
641 }
642 }
643 #[test]
644 fn linz() {
645 for i in -2..=2 {
646 let x = i as crate::float;
647 std::println!("{:+} {}", x, crate::mf::linz(x, -1.0, 1.0));
648 }
649 }
650 #[test]
651 fn s() {
652 for i in -2..=2 {
653 let x = i as crate::float;
654 std::println!("{:+} {}", x, crate::mf::s(x, -1.0, 1.0));
655 }
656 }
657 #[test]
658 fn z() {
659 for i in -2..=2 {
660 let x = i as crate::float;
661 std::println!("{:+} {}", x, crate::mf::z(x, -1.0, 1.0));
662 }
663 }
664 #[test]
665 fn pi() {
666 for i in -3..=3 {
667 let x = i as crate::float;
668 std::println!("{:+} {}", x, crate::mf::pi(x, -2.0, -1.0, 1.0, 2.0));
669 }
670 }
671 }
672 }
674 /// proportional integral derivative controller
675 #[repr(C)]
676 pub struct pid {
677 /// proportional constant
678 pub kp: float,
679 /// integral constant
680 pub ki: float,
681 /// derivative constant
682 pub kd: float,
683 /// maximum integral output
684 pub summax: float,
685 /// minimum integral output
686 pub summin: float,
687 /// controller integral output
688 pub sum: float,
689 /// maximum final output
690 pub outmax: float,
691 /// minimum final output
692 pub outmin: float,
693 /// controller final output
694 pub out: float,
695 /// cache variable
696 pub(crate) var: float,
697 /// cache feedback
698 pub fdb: float,
699 /// cache error
700 pub err: float,
701 }
703 impl Default for pid {
704 #[inline(always)]
705 fn default() -> Self {
706 Self {
707 kp: 0.0,
708 ki: 0.0,
709 kd: 0.0,
710 summax: float::INFINITY,
711 summin: -float::INFINITY,
712 sum: 0.0,
713 outmax: float::INFINITY,
714 outmin: -float::INFINITY,
715 out: 0.0,
716 var: 0.0,
717 fdb: 0.0,
718 err: 0.0,
719 }
720 }
721 }
723 extern "C" {
724 fn a_pid_kpid(ctx: *mut pid, kp: float, ki: float, kd: float);
725 fn a_pid_run(ctx: *mut pid, set: float, fdb: float) -> float;
726 fn a_pid_pos(ctx: *mut pid, set: float, fdb: float) -> float;
727 fn a_pid_inc(ctx: *mut pid, set: float, fdb: float) -> float;
728 fn a_pid_zero(ctx: *mut pid);
729 }
731 impl pid {
732 /// initialize for PID controller
733 #[inline(always)]
734 pub fn new() -> Self {
735 Self::default()
736 }
737 /// set proportional integral derivative constant for PID controller
738 #[inline(always)]
739 pub fn kpid(&mut self, kp: float, ki: float, kd: float) -> &mut Self {
740 unsafe { a_pid_kpid(self, kp, ki, kd) };
741 self
742 }
743 /// calculate for PID controller
744 #[inline(always)]
745 pub fn run(&mut self, set: float, fdb: float) -> float {
746 unsafe { a_pid_run(self, set, fdb) }
747 }
748 /// calculate for positional PID controller
749 #[inline(always)]
750 pub fn pos(&mut self, set: float, fdb: float) -> float {
751 unsafe { a_pid_pos(self, set, fdb) }
752 }
753 /// calculate for incremental PID controller
754 #[inline(always)]
755 pub fn inc(&mut self, set: float, fdb: float) -> float {
756 unsafe { a_pid_inc(self, set, fdb) }
757 }
758 /// zeroing for PID controller
759 #[inline(always)]
760 pub fn zero(&mut self) -> &mut Self {
761 unsafe { a_pid_zero(self) };
762 self
763 }
764 }
766 #[test]
767 fn pid() {
768 extern crate std;
769 let mut a = crate::pid::new();
770 a.kpid(10.0, 0.1, 1.0);
771 std::println!("{}", a.zero().pos(1.0, 0.0));
772 std::println!("{}", a.zero().inc(1.0, 0.0));
773 }
775 /// fuzzy PID controller operator
776 pub mod fuzzy {
777 use crate::uint;
778 /// min(a,b)
779 pub const CAP: uint = 1;
780 /// a*b
781 pub const CAP_ALGEBRA: uint = 2;
782 /// max(a+b-1,0)
783 pub const CAP_BOUNDED: uint = 3;
784 /// max(a,b)
785 pub const CUP: uint = 4;
786 /// a+b-a*b
787 pub const CUP_ALGEBRA: uint = 5;
788 /// min(a+b,1)
789 pub const CUP_BOUNDED: uint = 6;
790 /// sqrt(a,b)*sqrt(1-(1-a)*(1-b))
791 pub const EQU: uint = 0;
792 }
794 #[macro_export]
795 /// compute size of memory block for fuzzy PID controller
796 macro_rules! PID_FUZZY_BLOCK {
797 ($n:expr) => {{
798 core::mem::size_of::<uint>() * $n * 2 + core::mem::size_of::<float>() * $n * (2 + $n)
799 }};
800 }
802 /// fuzzy proportional integral derivative controller
803 #[repr(C)]
804 pub struct pid_fuzzy {
805 /// proportional integral derivative controller
806 pub pid: pid,
807 pub(crate) me: *const float,
808 pub(crate) mec: *const float,
809 pub(crate) mkp: *const float,
810 pub(crate) mki: *const float,
811 pub(crate) mkd: *const float,
812 pub(crate) idx: *mut uint,
813 pub(crate) val: *mut float,
814 /// fuzzy relational operator
815 pub(crate) op: extern "C" fn(float, float) -> float,
816 /// base proportional constant
817 pub kp: float,
818 /// base integral constant
819 pub ki: float,
820 /// base derivative constant
821 pub kd: float,
822 /// number of order in the square matrix
823 pub(crate) order: uint,
824 /// maximum number triggered by the rule
825 pub(crate) block: uint,
826 }
828 impl Default for pid_fuzzy {
829 #[inline(always)]
830 fn default() -> Self {
831 Self {
832 pid: pid::default(),
833 me: core::ptr::null(),
834 mec: core::ptr::null(),
835 mkp: core::ptr::null(),
836 mki: core::ptr::null(),
837 mkd: core::ptr::null(),
838 idx: core::ptr::null_mut(),
839 val: core::ptr::null_mut(),
840 op: unsafe { a_pid_fuzzy_op(fuzzy::EQU) },
841 kp: 0.0,
842 ki: 0.0,
843 kd: 0.0,
844 order: 0,
845 block: 0,
846 }
847 }
848 }
850 extern "C" {
851 fn a_pid_fuzzy_op(op: uint) -> extern "C" fn(float, float) -> float;
852 fn a_pid_fuzzy_set_op(ctx: *mut pid_fuzzy, op: uint);
853 fn a_pid_fuzzy_rule(
854 ctx: *mut pid_fuzzy,
855 num: uint,
856 me: *const float,
857 mec: *const float,
858 mkp: *const float,
859 mki: *const float,
860 mkd: *const float,
861 );
862 fn a_pid_fuzzy_block(ctx: *mut pid_fuzzy) -> *mut u8;
863 fn a_pid_fuzzy_set_block(ctx: *mut pid_fuzzy, ptr: *mut u8, num: usize);
864 fn a_pid_fuzzy_kpid(ctx: *mut pid_fuzzy, kp: float, ki: float, kd: float);
865 fn a_pid_fuzzy_run(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
866 fn a_pid_fuzzy_pos(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
867 fn a_pid_fuzzy_inc(ctx: *mut pid_fuzzy, set: float, fdb: float) -> float;
868 fn a_pid_fuzzy_zero(ctx: *mut pid_fuzzy);
869 }
871 impl pid_fuzzy {
872 /// initialize for fuzzy PID controller
873 #[inline(always)]
874 pub fn new() -> Self {
875 Self::default()
876 }
877 /// set rule base for fuzzy PID controller
878 #[inline(always)]
879 pub fn rule(
880 &mut self,
881 col: usize,
882 me: &[float],
883 mec: &[float],
884 mkp: &[float],
885 mki: &[float],
886 mkd: &[float],
887 ) -> &mut Self {
888 unsafe {
889 a_pid_fuzzy_rule(
890 self,
891 col as uint,
892 me.as_ptr(),
893 mec.as_ptr(),
894 mkp.as_ptr(),
895 mki.as_ptr(),
896 mkd.as_ptr(),
897 )
898 };
899 self
900 }
901 /// set proportional integral derivative constant for fuzzy PID controller
902 #[inline(always)]
903 pub fn kpid(&mut self, kp: float, ki: float, kd: float) -> &mut Self {
904 unsafe { a_pid_fuzzy_kpid(self, kp, ki, kd) };
905 self
906 }
907 /// set memory block for fuzzy PID controller
908 #[inline(always)]
909 pub fn set_block(&mut self, ptr: &mut [u8], num: usize) -> &mut Self {
910 unsafe { a_pid_fuzzy_set_block(self, ptr.as_mut_ptr(), num) };
911 self
912 }
913 /// get memory block for fuzzy PID controller
914 #[inline(always)]
915 pub fn block(&mut self) -> &mut [u8] {
916 unsafe {
917 core::slice::from_raw_parts_mut(
918 a_pid_fuzzy_block(self),
919 PID_FUZZY_BLOCK!(self.block as usize),
920 )
921 }
922 }
923 /// set fuzzy relational operator for fuzzy PID controller
924 #[inline(always)]
925 pub fn op(&mut self, op: uint) -> &mut Self {
926 unsafe { a_pid_fuzzy_set_op(self, op) };
927 self
928 }
929 /// calculate for fuzzy PID controller
930 #[inline(always)]
931 pub fn run(&mut self, set: float, fdb: float) -> float {
932 unsafe { a_pid_fuzzy_run(self, set, fdb) }
933 }
934 /// calculate for positional fuzzy PID controller
935 #[inline(always)]
936 pub fn pos(&mut self, set: float, fdb: float) -> float {
937 unsafe { a_pid_fuzzy_pos(self, set, fdb) }
938 }
939 /// calculate for incremental fuzzy PID controller
940 #[inline(always)]
941 pub fn inc(&mut self, set: float, fdb: float) -> float {
942 unsafe { a_pid_fuzzy_inc(self, set, fdb) }
943 }
944 /// zeroing for fuzzy PID controller
945 #[inline(always)]
946 pub fn zero(&mut self) -> &mut Self {
947 unsafe { a_pid_fuzzy_zero(self) };
948 self
949 }
950 }
952 #[test]
953 #[allow(non_snake_case)]
954 fn pid_fuzzy() {
955 extern crate std;
956 let NL: float = -3.0;
957 let NM: float = -2.0;
958 let NS: float = -1.0;
959 let ZO: float = 0.0;
960 let PS: float = 1.0;
961 let PM: float = 2.0;
962 let PL: float = 3.0;
963 let me = [
964 [crate::mf::TRI as float, NL, NL, NM],
965 [crate::mf::TRI as float, NL, NM, NS],
966 [crate::mf::TRI as float, NM, NS, ZO],
967 [crate::mf::TRI as float, NS, ZO, PS],
968 [crate::mf::TRI as float, ZO, PS, PM],
969 [crate::mf::TRI as float, PS, PM, PL],
970 [crate::mf::TRI as float, PM, PL, PL],
971 ];
972 let NL: float = -3.0;
973 let NM: float = -2.0;
974 let NS: float = -1.0;
975 let ZO: float = 0.0;
976 let PS: float = 1.0;
977 let PM: float = 2.0;
978 let PL: float = 3.0;
979 let mec = [
980 [crate::mf::TRI as float, NL, NL, NM],
981 [crate::mf::TRI as float, NL, NM, NS],
982 [crate::mf::TRI as float, NM, NS, ZO],
983 [crate::mf::TRI as float, NS, ZO, PS],
984 [crate::mf::TRI as float, ZO, PS, PM],
985 [crate::mf::TRI as float, PS, PM, PL],
986 [crate::mf::TRI as float, PM, PL, PL],
987 ];
988 let NL: float = -15.0;
989 let NM: float = -10.0;
990 let NS: float = -5.0;
991 let ZO: float = 0.0;
992 let PS: float = 5.0;
993 let PM: float = 10.0;
994 let PL: float = 15.0;
995 let mkp = [
996 [NL, NL, NM, NM, NS, ZO, ZO],
997 [NL, NL, NM, NS, NS, ZO, PS],
998 [NM, NM, NM, NS, ZO, PS, PS],
999 [NM, NM, NS, ZO, PS, PM, PM],
1000 [NS, NS, ZO, PS, PS, PM, PM],
1001 [NS, ZO, PS, PM, PM, PM, PL],
1002 [ZO, ZO, PM, PM, PM, PL, PL],
1003 ];
1004 let NL: float = -3.0;
1005 let NM: float = -2.0;
1006 let NS: float = -1.0;
1007 let ZO: float = 0.0;
1008 let PS: float = 1.0;
1009 let PM: float = 2.0;
1010 let PL: float = 3.0;
1011 let mki = [
1012 [PL, PL, PM, PM, PS, ZO, ZO],
1013 [PL, PL, PM, PS, PS, ZO, ZO],
1014 [PL, PM, PS, PS, ZO, NS, NS],
1015 [PM, PM, PS, ZO, NS, NM, NM],
1016 [PM, PS, ZO, NS, NS, NM, NL],
1017 [ZO, ZO, NS, NS, NM, NL, NL],
1018 [ZO, ZO, NS, NM, NM, NL, NL],
1019 ];
1020 let mkd = [
1021 [NS, PS, PL, PL, PL, PM, NS],
1022 [NS, PS, PL, PM, PM, PS, ZO],
1023 [ZO, PS, PM, PM, PS, PS, ZO],
1024 [ZO, PS, PS, PS, PS, PS, ZO],
1025 [ZO, ZO, ZO, ZO, ZO, ZO, ZO],
1026 [NL, NS, NS, NS, NS, NS, NL],
1027 [NL, NM, NM, NM, NS, NS, NL],
1028 ];
1029 let mut block = [0u8; crate::PID_FUZZY_BLOCK!(2)];
1030 let mut a = pid_fuzzy::new();
1031 a.rule(
1032 me.len(),
1033 &me.concat(),
1034 &mec.concat(),
1035 &mkp.concat(),
1036 &mki.concat(),
1037 &mkd.concat(),
1038 )
1039 .kpid(10.0, 0.1, 1.0)
1040 .set_block(&mut block, 2);
1041 a.op(crate::fuzzy::EQU).zero();
1042 std::println!("{} {}", a.pos(1.0, 0.0), a.pos(1.0, 0.0));
1043 a.op(crate::fuzzy::EQU).zero();
1044 std::println!("{} {}", a.inc(1.0, 0.0), a.inc(1.0, 0.0));
1045 }
1047 /// single neuron proportional integral derivative controller
1048 #[repr(C)]
1049 pub struct pid_neuro {
1050 /// proportional integral derivative controller
1051 pub pid: pid,
1052 /// proportional coefficient
1053 pub k: float,
1054 /// proportional weight
1055 pub wp: float,
1056 /// integral weight
1057 pub wi: float,
1058 /// derivative weight
1059 pub wd: float,
1060 /// error change
1061 pub ec: float,
1062 }
1064 impl Default for pid_neuro {
1065 #[inline(always)]
1066 fn default() -> Self {
1067 Self {
1068 pid: pid::default(),
1069 k: 0.0,
1070 wp: 0.0,
1071 wi: 0.0,
1072 wd: 0.0,
1073 ec: 0.0,
1074 }
1075 }
1076 }
1078 extern "C" {
1079 fn a_pid_neuro_kpid(ctx: *mut pid_neuro, k: float, kp: float, ki: float, kd: float);
1080 fn a_pid_neuro_wpid(ctx: *mut pid_neuro, wp: float, wi: float, wd: float);
1081 fn a_pid_neuro_run(ctx: *mut pid_neuro, set: float, fdb: float) -> float;
1082 fn a_pid_neuro_inc(ctx: *mut pid_neuro, set: float, fdb: float) -> float;
1083 fn a_pid_neuro_zero(ctx: *mut pid_neuro);
1084 }
1086 impl pid_neuro {
1087 /// initialize for single neuron PID controller
1088 #[inline(always)]
1089 pub fn new() -> Self {
1090 Self::default()
1091 }
1092 /// set proportional integral derivative constant for single neuron PID controller
1093 #[inline(always)]
1094 pub fn kpid(&mut self, k: float, kp: float, ki: float, kd: float) -> &mut Self {
1095 unsafe { a_pid_neuro_kpid(self, k, kp, ki, kd) };
1096 self
1097 }
1098 /// set proportional integral derivative weight for single neuron PID controller
1099 #[inline(always)]
1100 pub fn wpid(&mut self, wp: float, wi: float, wd: float) -> &mut Self {
1101 unsafe { a_pid_neuro_wpid(self, wp, wi, wd) };
1102 self
1103 }
1104 /// calculate for single neuron PID controller
1105 #[inline(always)]
1106 pub fn run(&mut self, set: float, fdb: float) -> float {
1107 unsafe { a_pid_neuro_run(self, set, fdb) }
1108 }
1109 /// calculate for incremental single neuron PID controller
1110 #[inline(always)]
1111 pub fn inc(&mut self, set: float, fdb: float) -> float {
1112 unsafe { a_pid_neuro_inc(self, set, fdb) }
1113 }
1114 /// zeroing for single neuron PID controller
1115 #[inline(always)]
1116 pub fn zero(&mut self) -> &mut Self {
1117 unsafe { a_pid_neuro_zero(self) };
1118 self
1119 }
1120 }
1122 #[test]
1123 fn pid_neuro() {
1124 extern crate std;
1125 let mut a = crate::pid_neuro::new();
1126 a.kpid(10.0, 1.0, 0.1, 1.0).wpid(1.0, 0.0, 0.0);
1127 std::println!("{}", a.inc(1.0, 0.0));
1128 a.zero();
1129 }
1131 /// transfer function
1132 #[repr(C)]
1133 pub struct tf {
1134 /// input
1135 input: *mut float,
1136 /// output
1137 output: *mut float,
1138 /// numerator
1139 num_p: *const float,
1140 /// denominator
1141 den_p: *const float,
1142 /// numerator number
1143 num_n: uint,
1144 /// denominator number
1145 den_n: uint,
1146 }
1148 extern "C" {
1149 fn a_tf_set_num(ctx: *mut tf, num_n: uint, num_p: *const float, input: *mut float);
1150 fn a_tf_set_den(ctx: *mut tf, den_n: uint, den_p: *const float, output: *mut float);
1151 fn a_tf_init(
1152 ctx: *mut tf,
1153 num_n: uint,
1154 num_p: *const float,
1155 input: *mut float,
1156 den_n: uint,
1157 den_p: *const float,
1158 output: *mut float,
1159 );
1160 fn a_tf_iter(ctx: *const tf, x: float) -> float;
1161 fn a_tf_zero(ctx: *const tf);
1162 }
1164 impl tf {
1165 /// initialize for transfer function
1166 #[inline(always)]
1167 pub fn new(num: &[float], input: &mut [float], den: &[float], output: &mut [float]) -> Self {
1168 let mut ctx: Self = Self {
1169 input: core::ptr::null_mut(),
1170 output: core::ptr::null_mut(),
1171 num_p: core::ptr::null(),
1172 den_p: core::ptr::null(),
1173 num_n: 0,
1174 den_n: 0,
1175 };
1176 unsafe {
1177 a_tf_init(
1178 &mut ctx,
1179 num.len() as uint,
1180 num.as_ptr(),
1181 input.as_mut_ptr(),
1182 den.len() as uint,
1183 den.as_ptr(),
1184 output.as_mut_ptr(),
1185 )
1186 };
1187 ctx
1188 }
1189 /// calculate for transfer function
1190 #[inline(always)]
1191 pub fn iter(&mut self, x: float) -> float {
1192 unsafe { a_tf_iter(self, x) }
1193 }
1194 /// zeroing for transfer function
1195 #[inline(always)]
1196 pub fn zero(&mut self) -> &mut Self {
1197 unsafe { a_tf_zero(self) };
1198 self
1199 }
1200 /// get input for transfer function
1201 #[inline(always)]
1202 pub fn input(&self) -> &[float] {
1203 unsafe { core::slice::from_raw_parts(self.input, self.num_n as usize) }
1204 }
1205 /// get numerator for transfer function
1206 #[inline(always)]
1207 pub fn num(&self) -> &[float] {
1208 unsafe { core::slice::from_raw_parts(self.num_p, self.num_n as usize) }
1209 }
1210 /// set numerator for transfer function
1211 #[inline(always)]
1212 pub fn set_num(&mut self, num: &[float], input: &mut [float]) -> &mut Self {
1213 unsafe { a_tf_set_num(self, num.len() as uint, num.as_ptr(), input.as_mut_ptr()) };
1214 self
1215 }
1216 /// get output for transfer function
1217 #[inline(always)]
1218 pub fn output(&self) -> &[float] {
1219 unsafe { core::slice::from_raw_parts(self.output, self.den_n as usize) }
1220 }
1221 /// get denominator for transfer function
1222 #[inline(always)]
1223 pub fn den(&self) -> &[float] {
1224 unsafe { core::slice::from_raw_parts(self.den_p, self.den_n as usize) }
1225 }
1226 /// set denominator for transfer function
1227 #[inline(always)]
1228 pub fn set_den(&mut self, den: &[float], output: &mut [float]) -> &mut Self {
1229 unsafe { a_tf_set_den(self, den.len() as uint, den.as_ptr(), output.as_mut_ptr()) };
1230 self
1231 }
1232 }
1234 #[test]
1235 fn tf() {
1236 extern crate std;
1237 let num = [6.59492796e-05, 6.54019884e-05];
1238 let den = [-1.97530991, 0.97530991];
1239 let mut input = [0.0; 2];
1240 let mut output = [0.0; 2];
1241 let mut a = crate::tf::new(&num, &mut input, &den, &mut output);
1242 a.set_num(&num, &mut input).set_den(&den, &mut output);
1243 std::println!("{} {}", a.iter(10.0), a.iter(10.0));
1244 std::println!("{:?} {:?}", a.num(), a.input());
1245 std::println!("{:?} {:?}", a.den(), a.output());
1246 a.zero();
1247 }
1249 /// bell-shaped velocity trajectory
1250 #[repr(C)]
1251 pub struct trajbell {
1252 /// total duration
1253 pub t: float,
1254 /// constant velocity phase
1255 pub tv: float,
1256 /// acceleration phase
1257 pub ta: float,
1258 /// deceleration phase
1259 pub td: float,
1260 /// time-interval in which the jerk is constant (j max or j min ) during the acceleration phase
1261 pub taj: float,
1262 /// time-interval in which the jerk is constant (j max or j min ) during the deceleration phase
1263 pub tdj: float,
1264 /// initial position
1265 pub p0: float,
1266 /// final position
1267 pub p1: float,
1268 /// initial velocity
1269 pub v0: float,
1270 /// final velocity
1271 pub v1: float,
1272 /// maximum velocity
1273 pub vm: float,
1274 /// maximum jerk
1275 pub jm: float,
1276 /// maximum acceleration
1277 pub am: float,
1278 /// maximum deceleration
1279 pub dm: float,
1280 }
1282 impl Default for trajbell {
1283 #[inline(always)]
1284 fn default() -> Self {
1285 Self {
1286 t: 0.0,
1287 tv: 0.0,
1288 ta: 0.0,
1289 td: 0.0,
1290 taj: 0.0,
1291 tdj: 0.0,
1292 p0: 0.0,
1293 p1: 0.0,
1294 v0: 0.0,
1295 v1: 0.0,
1296 vm: 0.0,
1297 jm: 0.0,
1298 am: 0.0,
1299 dm: 0.0,
1300 }
1301 }
1302 }
1304 extern "C" {
1305 fn a_trajbell_gen(
1306 ctx: *mut trajbell,
1307 jm: float,
1308 am: float,
1309 vm: float,
1310 p0: float,
1311 p1: float,
1312 v0: float,
1313 v1: float,
1314 ) -> float;
1315 fn a_trajbell_pos(ctx: *const trajbell, dt: float) -> float;
1316 fn a_trajbell_vel(ctx: *const trajbell, dt: float) -> float;
1317 fn a_trajbell_acc(ctx: *const trajbell, dt: float) -> float;
1318 fn a_trajbell_jer(ctx: *const trajbell, dt: float) -> float;
1319 }
1321 impl trajbell {
1322 /// initialize for bell-shaped velocity trajectory
1323 #[inline(always)]
1324 pub fn new() -> Self {
1325 Self::default()
1326 }
1327 /// generate for bell-shaped velocity trajectory
1328 #[allow(clippy::too_many_arguments)]
1329 #[inline(always)]
1330 pub fn gen(
1331 &mut self,
1332 jm: float,
1333 am: float,
1334 vm: float,
1335 p0: float,
1336 p1: float,
1337 v0: float,
1338 v1: float,
1339 ) -> float {
1340 unsafe { a_trajbell_gen(self, jm, am, vm, p0, p1, v0, v1) }
1341 }
1342 /// calculate position for bell-shaped velocity trajectory
1343 #[inline(always)]
1344 pub fn pos(&mut self, dt: float) -> float {
1345 unsafe { a_trajbell_pos(self, dt) }
1346 }
1347 /// calculate velocity for bell-shaped velocity trajectory
1348 #[inline(always)]
1349 pub fn vel(&mut self, dt: float) -> float {
1350 unsafe { a_trajbell_vel(self, dt) }
1351 }
1352 /// calculate acceleration for bell-shaped velocity trajectory
1353 #[inline(always)]
1354 pub fn acc(&mut self, dt: float) -> float {
1355 unsafe { a_trajbell_acc(self, dt) }
1356 }
1357 /// calculate jerk for bell-shaped velocity trajectory
1358 #[inline(always)]
1359 pub fn jer(&mut self, dt: float) -> float {
1360 unsafe { a_trajbell_jer(self, dt) }
1361 }
1362 }
1364 #[test]
1365 fn trajbell() {
1366 extern crate std;
1367 let dt = 0.5;
1368 {
1369 let mut a = crate::trajbell::new();
1370 std::print!("{} ", a.gen(3.0, 2.0, 3.0, 0.0, 10.0, 0.0, 0.0));
1371 std::println!(
1372 "[{}, {}, {}, {}]",
1373 a.pos(dt),
1374 a.vel(dt),
1375 a.acc(dt),
1376 a.jer(dt)
1377 );
1378 }
1379 }
1381 /// cubic polynomial trajectory
1382 #[repr(C)]
1383 pub struct trajpoly3 {
1384 /// coefficients of position
1385 pub p: [float; 4],
1386 /// coefficients of velocity
1387 pub v: [float; 3],
1388 /// coefficients of acceleration
1389 pub a: [float; 2],
1390 }
1392 extern "C" {
1393 fn a_trajpoly3_gen(ctx: *mut trajpoly3, ts: float, p0: float, p1: float, v0: float, v1: float);
1394 fn a_trajpoly3_pos(ctx: *const trajpoly3, dt: float) -> float;
1395 fn a_trajpoly3_vel(ctx: *const trajpoly3, dt: float) -> float;
1396 fn a_trajpoly3_acc(ctx: *const trajpoly3, dt: float) -> float;
1397 }
1399 impl trajpoly3 {
1400 /// initialize for cubic polynomial trajectory
1401 #[inline(always)]
1402 pub fn new(ts: float, p0: float, p1: float, v0: float, v1: float) -> Self {
1403 let mut ctx: Self = Self {
1404 p: [0.0; 4],
1405 v: [0.0; 3],
1406 a: [0.0; 2],
1407 };
1408 unsafe { a_trajpoly3_gen(&mut ctx, ts, p0, p1, v0, v1) };
1409 ctx
1410 }
1411 /// generate for cubic polynomial trajectory
1412 #[inline(always)]
1413 pub fn gen(&mut self, ts: float, p0: float, p1: float, v0: float, v1: float) -> &mut Self {
1414 unsafe { a_trajpoly3_gen(self, ts, p0, p1, v0, v1) };
1415 self
1416 }
1417 /// calculate position for cubic polynomial trajectory
1418 #[inline(always)]
1419 pub fn pos(&mut self, dt: float) -> float {
1420 unsafe { a_trajpoly3_pos(self, dt) }
1421 }
1422 /// calculate velocity for cubic polynomial trajectory
1423 #[inline(always)]
1424 pub fn vel(&mut self, dt: float) -> float {
1425 unsafe { a_trajpoly3_vel(self, dt) }
1426 }
1427 /// calculate acceleration for cubic polynomial trajectory
1428 #[inline(always)]
1429 pub fn acc(&mut self, dt: float) -> float {
1430 unsafe { a_trajpoly3_acc(self, dt) }
1431 }
1432 }
1434 #[test]
1435 fn trajpoly3() {
1436 extern crate std;
1437 let dt = 0.5;
1438 {
1439 let mut a = crate::trajpoly3::new(1.0, 0.0, 1.0, 0.0, 1.0);
1440 std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1441 }
1442 }
1444 /// quintic polynomial trajectory
1445 #[repr(C)]
1446 pub struct trajpoly5 {
1447 /// coefficients of position
1448 pub p: [float; 6],
1449 /// coefficients of velocity
1450 pub v: [float; 5],
1451 /// coefficients of acceleration
1452 pub a: [float; 4],
1453 }
1455 extern "C" {
1456 fn a_trajpoly5_gen(
1457 ctx: *mut trajpoly5,
1458 ts: float,
1459 p0: float,
1460 p1: float,
1461 v0: float,
1462 v1: float,
1463 a0: float,
1464 a1: float,
1465 );
1466 fn a_trajpoly5_pos(ctx: *const trajpoly5, dt: float) -> float;
1467 fn a_trajpoly5_vel(ctx: *const trajpoly5, dt: float) -> float;
1468 fn a_trajpoly5_acc(ctx: *const trajpoly5, dt: float) -> float;
1469 }
1471 impl trajpoly5 {
1472 /// initialize for quintic polynomial trajectory
1473 #[allow(clippy::too_many_arguments)]
1474 #[inline(always)]
1475 pub fn new(
1476 ts: float,
1477 p0: float,
1478 p1: float,
1479 v0: float,
1480 v1: float,
1481 a0: float,
1482 a1: float,
1483 ) -> Self {
1484 let mut ctx: Self = Self {
1485 p: [0.0; 6],
1486 v: [0.0; 5],
1487 a: [0.0; 4],
1488 };
1489 unsafe { a_trajpoly5_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1) };
1490 ctx
1491 }
1492 /// generate for quintic polynomial trajectory
1493 #[allow(clippy::too_many_arguments)]
1494 #[inline(always)]
1495 pub fn gen(
1496 &mut self,
1497 ts: float,
1498 p0: float,
1499 p1: float,
1500 v0: float,
1501 v1: float,
1502 a0: float,
1503 a1: float,
1504 ) -> &mut Self {
1505 unsafe { a_trajpoly5_gen(self, ts, p0, p1, v0, v1, a0, a1) };
1506 self
1507 }
1508 /// calculate position for quintic polynomial trajectory
1509 #[inline(always)]
1510 pub fn pos(&mut self, dt: float) -> float {
1511 unsafe { a_trajpoly5_pos(self, dt) }
1512 }
1513 /// calculate velocity for quintic polynomial trajectory
1514 #[inline(always)]
1515 pub fn vel(&mut self, dt: float) -> float {
1516 unsafe { a_trajpoly5_vel(self, dt) }
1517 }
1518 /// calculate acceleration for quintic polynomial trajectory
1519 #[inline(always)]
1520 pub fn acc(&mut self, dt: float) -> float {
1521 unsafe { a_trajpoly5_acc(self, dt) }
1522 }
1523 }
1525 #[test]
1526 fn trajpoly5() {
1527 extern crate std;
1528 let dt = 0.5;
1529 {
1530 let mut a = crate::trajpoly5::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1531 std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1532 }
1533 }
1535 /// hepta polynomial trajectory
1536 #[repr(C)]
1537 pub struct trajpoly7 {
1538 /// coefficients of position
1539 pub p: [float; 8],
1540 /// coefficients of velocity
1541 pub v: [float; 7],
1542 /// coefficients of acceleration
1543 pub a: [float; 6],
1544 /// coefficients of jerk
1545 pub j: [float; 5],
1546 }
1548 extern "C" {
1549 fn a_trajpoly7_gen(
1550 ctx: *mut trajpoly7,
1551 ts: float,
1552 p0: float,
1553 p1: float,
1554 v0: float,
1555 v1: float,
1556 a0: float,
1557 a1: float,
1558 j0: float,
1559 j1: float,
1560 );
1561 fn a_trajpoly7_pos(ctx: *const trajpoly7, dt: float) -> float;
1562 fn a_trajpoly7_vel(ctx: *const trajpoly7, dt: float) -> float;
1563 fn a_trajpoly7_acc(ctx: *const trajpoly7, dt: float) -> float;
1564 fn a_trajpoly7_jer(ctx: *const trajpoly7, dt: float) -> float;
1565 }
1567 impl trajpoly7 {
1568 /// initialize for hepta polynomial trajectory
1569 #[allow(clippy::too_many_arguments)]
1570 #[inline(always)]
1571 pub fn new(
1572 ts: float,
1573 p0: float,
1574 p1: float,
1575 v0: float,
1576 v1: float,
1577 a0: float,
1578 a1: float,
1579 j0: float,
1580 j1: float,
1581 ) -> Self {
1582 let mut ctx: Self = Self {
1583 p: [0.0; 8],
1584 v: [0.0; 7],
1585 a: [0.0; 6],
1586 j: [0.0; 5],
1587 };
1588 unsafe { a_trajpoly7_gen(&mut ctx, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1589 ctx
1590 }
1591 /// generate for hepta polynomial trajectory
1592 #[allow(clippy::too_many_arguments)]
1593 #[inline(always)]
1594 pub fn gen(
1595 &mut self,
1596 ts: float,
1597 p0: float,
1598 p1: float,
1599 v0: float,
1600 v1: float,
1601 a0: float,
1602 a1: float,
1603 j0: float,
1604 j1: float,
1605 ) -> &mut Self {
1606 unsafe { a_trajpoly7_gen(self, ts, p0, p1, v0, v1, a0, a1, j0, j1) };
1607 self
1608 }
1609 /// calculate position for hepta polynomial trajectory
1610 #[inline(always)]
1611 pub fn pos(&mut self, dt: float) -> float {
1612 unsafe { a_trajpoly7_pos(self, dt) }
1613 }
1614 /// calculate velocity for hepta polynomial trajectory
1615 #[inline(always)]
1616 pub fn vel(&mut self, dt: float) -> float {
1617 unsafe { a_trajpoly7_vel(self, dt) }
1618 }
1619 /// calculate acceleration for hepta polynomial trajectory
1620 #[inline(always)]
1621 pub fn acc(&mut self, dt: float) -> float {
1622 unsafe { a_trajpoly7_acc(self, dt) }
1623 }
1624 /// calculate jerk for hepta polynomial trajectory
1625 #[inline(always)]
1626 pub fn jer(&mut self, dt: float) -> float {
1627 unsafe { a_trajpoly7_jer(self, dt) }
1628 }
1629 }
1631 #[test]
1632 fn trajpoly7() {
1633 extern crate std;
1634 let dt = 0.5;
1635 {
1636 let mut a = crate::trajpoly7::new(1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
1637 std::println!(
1638 "[{}, {}, {}, {}]",
1639 a.pos(dt),
1640 a.vel(dt),
1641 a.acc(dt),
1642 a.jer(dt)
1643 );
1644 }
1645 }
1647 /// trapezoidal velocity trajectory
1648 #[repr(C)]
1649 pub struct trajtrap {
1650 /// total duration
1651 pub t: float,
1652 /// initial position
1653 pub p0: float,
1654 /// final position
1655 pub p1: float,
1656 /// initial velocity
1657 pub v0: float,
1658 /// final velocity
1659 pub v1: float,
1660 /// constant velocity
1661 pub vc: float,
1662 /// time before constant velocity
1663 pub ta: float,
1664 /// time after constant velocity
1665 pub td: float,
1666 /// position before constant velocity
1667 pub pa: float,
1668 /// position after constant velocity
1669 pub pd: float,
1670 /// acceleration before constant velocity
1671 pub ac: float,
1672 /// acceleration after constant velocity
1673 pub de: float,
1674 }
1676 impl Default for trajtrap {
1677 #[inline(always)]
1678 fn default() -> Self {
1679 Self {
1680 t: 0.0,
1681 p0: 0.0,
1682 p1: 0.0,
1683 v0: 0.0,
1684 v1: 0.0,
1685 vc: 0.0,
1686 ta: 0.0,
1687 td: 0.0,
1688 pa: 0.0,
1689 pd: 0.0,
1690 ac: 0.0,
1691 de: 0.0,
1692 }
1693 }
1694 }
1696 extern "C" {
1697 fn a_trajtrap_gen(
1698 ctx: *mut trajtrap,
1699 vm: float,
1700 ac: float,
1701 de: float,
1702 p0: float,
1703 p1: float,
1704 v0: float,
1705 v1: float,
1706 ) -> float;
1707 fn a_trajtrap_pos(ctx: *const trajtrap, dt: float) -> float;
1708 fn a_trajtrap_vel(ctx: *const trajtrap, dt: float) -> float;
1709 fn a_trajtrap_acc(ctx: *const trajtrap, dt: float) -> float;
1710 }
1712 impl trajtrap {
1713 /// initialize for trapezoidal velocity trajectory
1714 #[inline(always)]
1715 pub fn new() -> Self {
1716 Self::default()
1717 }
1718 /// generate for trapezoidal velocity trajectory
1719 #[allow(clippy::too_many_arguments)]
1720 #[inline(always)]
1721 pub fn gen(
1722 &mut self,
1723 vm: float,
1724 ac: float,
1725 de: float,
1726 p0: float,
1727 p1: float,
1728 v0: float,
1729 v1: float,
1730 ) -> float {
1731 unsafe { a_trajtrap_gen(self, vm, ac, de, p0, p1, v0, v1) }
1732 }
1733 /// calculate position for trapezoidal velocity trajectory
1734 #[inline(always)]
1735 pub fn pos(&mut self, dt: float) -> float {
1736 unsafe { a_trajtrap_pos(self, dt) }
1737 }
1738 /// calculate velocity for trapezoidal velocity trajectory
1739 #[inline(always)]
1740 pub fn vel(&mut self, dt: float) -> float {
1741 unsafe { a_trajtrap_vel(self, dt) }
1742 }
1743 /// calculate acceleration for trapezoidal velocity trajectory
1744 #[inline(always)]
1745 pub fn acc(&mut self, dt: float) -> float {
1746 unsafe { a_trajtrap_acc(self, dt) }
1747 }
1748 }
1750 #[test]
1751 fn trajtrap() {
1752 extern crate std;
1753 let dt = 0.5;
1754 {
1755 let mut a = crate::trajtrap::new();
1756 std::print!("{} ", a.gen(2.0, 2.0, -2.0, 0.0, 2.0, 0.0, 0.0));
1757 std::println!("[{}, {}, {}]", a.pos(dt), a.vel(dt), a.acc(dt));
1758 }
1759 }
1761 /// version
1762 ///
1763 /// ## Examples
1764 ///
1765 /// ```
1766 /// println!(
1767 /// "version {}.{}.{}+{}",
1768 /// liba::version::major(),
1769 /// liba::version::minor(),
1770 /// liba::version::patch(),
1771 /// liba::version::tweak()
1772 /// );
1773 /// ```
1774 #[repr(C)]
1775 pub struct version {
1776 /// major number
1777 pub major: uint,
1778 /// minor number
1779 pub minor: uint,
1780 /// third number
1781 pub third: uint,
1782 /// extra number
1783 pub extra: uint,
1784 /// alphabet
1785 pub alpha: [u8; 4],
1786 }
1788 extern "C" {
1789 static a_version_major: uint;
1790 static a_version_minor: uint;
1791 static a_version_patch: uint;
1792 static a_version_tweak: u32;
1793 fn a_version_set_alpha(ctx: *mut version, alpha: *const u8);
1794 fn a_version_alpha(ctx: *const version, alpha: &mut [u8; 5]);
1795 fn a_version_parse(ctx: *mut version, ver: *const u8) -> uint;
1796 fn a_version_tostr(ctx: *const version, p: *mut u8, n: usize) -> int;
1797 fn a_version_cmp(ctx: *const version, rhs: *const version) -> int;
1798 fn a_version_lt(ctx: *const version, rhs: *const version) -> bool;
1799 fn a_version_gt(ctx: *const version, rhs: *const version) -> bool;
1800 fn a_version_le(ctx: *const version, rhs: *const version) -> bool;
1801 fn a_version_ge(ctx: *const version, rhs: *const version) -> bool;
1802 fn a_version_eq(ctx: *const version, rhs: *const version) -> bool;
1803 fn a_version_check(major: uint, minor: uint, patch: uint) -> int;
1804 }
1806 impl version {
1807 /// initialize for version
1808 #[inline(always)]
1809 pub fn new(major: uint, minor: uint, third: uint) -> Self {
1810 Self {
1811 major,
1812 minor,
1813 third,
1814 extra: 0,
1815 alpha: [b'.', 0, 0, 0],
1816 }
1817 }
1818 /// set alphabet for version
1819 #[inline(always)]
1820 pub fn set_alpha(&mut self, alpha: &[u8]) {
1821 unsafe { a_version_set_alpha(self, alpha.as_ptr()) }
1822 }
1823 /// get alphabet for version
1824 #[inline(always)]
1825 pub fn alpha(&self, alpha: &mut [u8; 5]) {
1826 unsafe { a_version_alpha(self, alpha) }
1827 }
1828 /// parse version string to version
1829 #[inline(always)]
1830 pub fn parse(&mut self, ver: &str) -> uint {
1831 unsafe { a_version_parse(self, ver.as_ptr()) }
1832 }
1833 /// convert version to string
1834 #[inline(always)]
1835 pub fn tostr(&mut self, ver: &mut [u8]) -> int {
1836 unsafe { a_version_tostr(self, ver.as_mut_ptr(), ver.len()) }
1837 }
1838 /// algorithm library version check
1839 #[inline(always)]
1840 pub fn check(major: uint, minor: uint, patch: uint) -> int {
1841 unsafe { a_version_check(major, minor, patch) }
1842 }
1843 /// algorithm library version major
1844 #[inline(always)]
1845 pub fn major() -> uint {
1846 unsafe { a_version_major }
1847 }
1848 /// algorithm library version minor
1849 #[inline(always)]
1850 pub fn minor() -> uint {
1851 unsafe { a_version_minor }
1852 }
1853 /// algorithm library version patch
1854 #[inline(always)]
1855 pub fn patch() -> uint {
1856 unsafe { a_version_patch }
1857 }
1858 /// algorithm library version tweak
1859 #[inline(always)]
1860 pub fn tweak() -> u32 {
1861 unsafe { a_version_tweak }
1862 }
1863 }
1865 impl PartialOrd for version {
1866 #[inline(always)]
1867 fn partial_cmp(&self, that: &version) -> Option<core::cmp::Ordering> {
1868 let ok: int = unsafe { a_version_cmp(self, that) };
1869 if ok > 0 {
1870 return Some(core::cmp::Ordering::Greater);
1871 }
1872 if ok < 0 {
1873 return Some(core::cmp::Ordering::Less);
1874 }
1875 Some(core::cmp::Ordering::Equal)
1876 }
1877 #[inline(always)]
1878 fn lt(&self, that: &version) -> bool {
1879 unsafe { a_version_lt(self, that) }
1880 }
1881 #[inline(always)]
1882 fn gt(&self, that: &version) -> bool {
1883 unsafe { a_version_gt(self, that) }
1884 }
1885 #[inline(always)]
1886 fn le(&self, that: &version) -> bool {
1887 unsafe { a_version_le(self, that) }
1888 }
1889 #[inline(always)]
1890 fn ge(&self, that: &version) -> bool {
1891 unsafe { a_version_ge(self, that) }
1892 }
1893 }
1895 impl PartialEq for version {
1896 #[inline(always)]
1897 fn eq(&self, that: &version) -> bool {
1898 unsafe { a_version_eq(self, that) }
1899 }
1900 }
1902 #[test]
1903 fn version() {
1904 extern crate std;
1905 impl core::fmt::Debug for version {
1906 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1907 let mut alpha = [0u8; 5];
1908 self.alpha(&mut alpha);
1909 f.debug_struct("version")
1910 .field("major", &self.major)
1911 .field("minor", &self.minor)
1912 .field("third", &self.third)
1913 .field("extra", &self.extra)
1914 .field("alpha", &std::str::from_utf8(&alpha).unwrap())
1915 .finish()
1916 }
1917 }
1918 std::println!("{}", crate::version::check(0, 0, 0));
1919 std::println!("major {}", crate::version::major());
1920 std::println!("minor {}", crate::version::minor());
1921 std::println!("patch {}", crate::version::patch());
1922 std::println!("tweak {}", crate::version::tweak());
1923 let v000 = version::new(0, 0, 0);
1924 let mut v001 = version::new(0, 0, 0);
1925 let mut ver = [0u8; 48];
1926 v001.parse("0.0.1-a.1");
1927 v001.tostr(&mut ver);
1928 std::println!("{:?}", v001);
1929 std::println!("{}", std::str::from_utf8(&ver).unwrap());
1930 assert!(v001 > v000);
1931 assert!(v000 < v001);
1932 assert!(v000 >= v000);
1933 assert!(v000 <= v000);
1934 assert!(v000 == v000);
1935 assert!(v000 != v001);
1936 }