1extern crate nalgebra as na;
44extern crate num_traits;
45
46use std::cmp::Ordering;
47use std::fmt::{Debug, Display, Formatter, LowerExp, Result as FmtResult};
48use std::iter::{Product, Sum};
49use std::num::FpCategory;
50use std::ops::{Add, AddAssign, Deref, DerefMut, Div, DivAssign, Mul, MulAssign, Neg, Rem, RemAssign, Sub, SubAssign};
51
52pub use num_traits::{Float, FloatConst, Num, One, Zero};
53
54mod differentials;
55
56pub use differentials::*;
58
59pub mod linalg;
60
61use num_traits::{FromPrimitive, Inv, MulAdd, MulAddAssign, NumCast, Pow, Signed, ToPrimitive, Unsigned};
62
63use na::{OVector, SVector, Scalar};
64
65pub use na::allocator::Allocator;
67pub use na::dimension::*;
68pub use na::storage::Owned;
69pub use na::{DefaultAllocator, Dim, DimName};
70
71#[derive(Clone, Copy)]
80pub struct OHyperdual<T: Copy + Scalar, N: Dim + DimName>(OVector<T, N>)
81where
82 DefaultAllocator: Allocator<N>,
83 Owned<T, N>: Copy;
84
85pub type Hyperdual<T, const N: usize> = OHyperdual<T, Const<N>>;
86
87impl<T: Copy + Scalar, N: Dim + DimName> OHyperdual<T, N>
88where
89 DefaultAllocator: Allocator<N>,
90 Owned<T, N>: Copy,
91{
92 #[inline]
94 pub fn from_slice(v: &[T]) -> Self {
95 Self(OVector::<T, N>::from_row_slice(v))
96 }
97
98 #[inline]
102 pub fn from_real(real: T) -> Self
103 where
104 T: Zero,
105 {
106 let mut dual = OVector::<T, N>::zeros();
107 dual[0] = real;
108 Self(dual)
109 }
110
111 #[inline]
113 pub fn real(&self) -> T {
114 self[0]
115 }
116
117 #[inline]
119 pub fn real_ref(&self) -> &T {
120 &self[0]
121 }
122
123 pub fn real_mut(&mut self) -> &mut T {
125 &mut self[0]
126 }
127
128 #[inline]
129 pub fn map_dual<F>(&self, r: T, f: F) -> Self
130 where
131 F: Fn(&T) -> T,
132 {
133 let mut v = self.map(|x| f(&x));
135 v[0] = r;
136 Self(v)
137 }
138
139 #[inline]
141 pub fn from_fn<F>(mut f: F) -> Self
142 where
143 F: FnMut(usize) -> T,
144 {
145 Self(OVector::<T, N>::from_fn(|i, _| f(i)))
146 }
147}
148
149impl<T: Copy + Scalar, N: Dim + DimName> Debug for OHyperdual<T, N>
150where
151 DefaultAllocator: Allocator<N>,
152 Owned<T, N>: Copy,
153{
154 fn fmt(&self, f: &mut Formatter) -> FmtResult {
155 let mut a = f.debug_tuple("Dual");
156 for x in self.iter() {
157 a.field(x);
158 }
159 a.finish()
160 }
161}
162
163impl<T: Copy + Scalar + Num + Zero, N: Dim + DimName> Default for OHyperdual<T, N>
164where
165 DefaultAllocator: Allocator<N>,
166 Owned<T, N>: Copy,
167{
168 #[inline]
169 fn default() -> Self {
170 Self::zero()
171 }
172}
173
174impl<T: Copy + Scalar + Zero, N: Dim + DimName> From<T> for OHyperdual<T, N>
175where
176 DefaultAllocator: Allocator<N>,
177 Owned<T, N>: Copy,
178{
179 #[inline]
180 fn from(real: T) -> Self {
181 Self::from_real(real)
182 }
183}
184
185impl<T: Copy + Scalar, N: Dim + DimName> Deref for OHyperdual<T, N>
186where
187 DefaultAllocator: Allocator<N>,
188 Owned<T, N>: Copy,
189{
190 type Target = OVector<T, N>;
191
192 #[inline]
193 fn deref(&self) -> &OVector<T, N> {
194 &self.0
195 }
196}
197
198impl<T: Copy + Scalar, N: Dim + DimName> DerefMut for OHyperdual<T, N>
199where
200 DefaultAllocator: Allocator<N>,
201 Owned<T, N>: Copy,
202{
203 #[inline]
204 fn deref_mut(&mut self) -> &mut OVector<T, N> {
205 &mut self.0
206 }
207}
208
209impl<T: Copy + Scalar, N: Dim + DimName> AsRef<OVector<T, N>> for OHyperdual<T, N>
210where
211 DefaultAllocator: Allocator<N>,
212 Owned<T, N>: Copy,
213{
214 #[inline]
215 fn as_ref(&self) -> &OVector<T, N> {
216 &self.0
217 }
218}
219
220impl<T: Copy + Scalar, N: Dim + DimName> AsMut<OVector<T, N>> for OHyperdual<T, N>
221where
222 DefaultAllocator: Allocator<N>,
223 Owned<T, N>: Copy,
224{
225 #[inline]
226 fn as_mut(&mut self) -> &mut OVector<T, N> {
227 &mut self.0
228 }
229}
230
231impl<T: Copy + Scalar + Neg<Output = T>, N: Dim + DimName> OHyperdual<T, N>
232where
233 DefaultAllocator: Allocator<N>,
234 Owned<T, N>: Copy,
235{
236 #[inline]
238 pub fn conjugate(self) -> Self {
239 self.map_dual(self.real(), |x| x.neg())
240 }
241}
242
243impl<T: Copy + Scalar + Display, N: Dim + DimName> Display for OHyperdual<T, N>
244where
245 DefaultAllocator: Allocator<N>,
246 Owned<T, N>: Copy,
247{
248 fn fmt(&self, f: &mut Formatter) -> FmtResult {
249 let precision = f.precision().unwrap_or(4);
250
251 write!(f, "{:.p$}", self.real(), p = precision)?;
252 for (i, x) in self.iter().skip(1).enumerate() {
253 write!(f, " + {:.p$}\u{03B5}{}", x, i + 1, p = precision)?;
254 }
255
256 Ok(())
257 }
258}
259
260impl<T: Copy + Scalar + LowerExp, N: Dim + DimName> LowerExp for OHyperdual<T, N>
261where
262 DefaultAllocator: Allocator<N>,
263 Owned<T, N>: Copy,
264{
265 fn fmt(&self, f: &mut Formatter) -> FmtResult {
266 let precision = f.precision().unwrap_or(4);
267
268 write!(f, "{:.p$e}", self.real(), p = precision)?;
269 for (i, x) in self.iter().skip(1).enumerate() {
270 write!(f, " + {:.p$e}\u{03B5}{}", x, i + 1, p = precision)?;
271 }
272
273 Ok(())
274 }
275}
276
277impl<T: Copy + Scalar + PartialEq, N: Dim + DimName> PartialEq<Self> for OHyperdual<T, N>
278where
279 DefaultAllocator: Allocator<N>,
280 Owned<T, N>: Copy,
281{
282 #[inline]
283 fn eq(&self, rhs: &Self) -> bool {
284 self.0 == rhs.0
285 }
286}
287
288impl<T: Copy + Scalar + PartialOrd, N: Dim + DimName> PartialOrd<Self> for OHyperdual<T, N>
289where
290 DefaultAllocator: Allocator<N>,
291 Owned<T, N>: Copy,
292{
293 #[inline]
294 fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
295 PartialOrd::partial_cmp(self.real_ref(), rhs.real_ref())
296 }
297
298 #[inline]
299 fn lt(&self, rhs: &Self) -> bool {
300 self.real() < rhs.real()
301 }
302
303 #[inline]
304 fn gt(&self, rhs: &Self) -> bool {
305 self.real() > rhs.real()
306 }
307}
308
309impl<T: Copy + Scalar + PartialEq, N: Dim + DimName> PartialEq<T> for OHyperdual<T, N>
310where
311 DefaultAllocator: Allocator<N>,
312 Owned<T, N>: Copy,
313{
314 #[inline]
315 fn eq(&self, rhs: &T) -> bool {
316 *self.real_ref() == *rhs
317 }
318}
319
320impl<T: Copy + Scalar + PartialOrd, N: Dim + DimName> PartialOrd<T> for OHyperdual<T, N>
321where
322 DefaultAllocator: Allocator<N>,
323 Owned<T, N>: Copy,
324{
325 #[inline]
326 fn partial_cmp(&self, rhs: &T) -> Option<Ordering> {
327 PartialOrd::partial_cmp(self.real_ref(), rhs)
328 }
329
330 #[inline]
331 fn lt(&self, rhs: &T) -> bool {
332 self.real() < *rhs
333 }
334
335 #[inline]
336 fn gt(&self, rhs: &T) -> bool {
337 self.real() > *rhs
338 }
339}
340
341macro_rules! impl_to_primitive {
342 ($($name:ident, $ty:ty),*) => {
343 impl<T: Copy + Scalar + ToPrimitive, N: Dim + DimName> ToPrimitive for OHyperdual<T, N>
344 where
345 DefaultAllocator: Allocator<N>,
346 Owned<T, N>: Copy,
347
348{
349 $(
350 #[inline]
351 fn $name(&self) -> Option<$ty> {
352 self.real_ref().$name()
353 }
354 )*
355 }
356 }
357}
358
359macro_rules! impl_from_primitive {
360 ($($name:ident, $ty:ty),*) => {
361 impl<T: Copy + Scalar + FromPrimitive, N: Dim + DimName> FromPrimitive for OHyperdual<T, N>
362 where
363 T: Zero,
364 DefaultAllocator: Allocator<N>,
365 Owned<T, N>: Copy,
366 {
367 $(
368 #[inline]
369 fn $name(n: $ty) -> Option<OHyperdual<T,N>> {
370 T::$name(n).map(Self::from_real)
371 }
372 )*
373 }
374 }
375}
376
377macro_rules! impl_primitive_cast {
378 ($($to:ident, $from:ident - $ty:ty),*) => {
379 impl_to_primitive!($($to, $ty),*);
380 impl_from_primitive!($($from, $ty),*);
381 }
382}
383
384impl_primitive_cast! {
385 to_isize, from_isize - isize,
386 to_i8, from_i8 - i8,
387 to_i16, from_i16 - i16,
388 to_i32, from_i32 - i32,
389 to_i64, from_i64 - i64,
390 to_usize, from_usize - usize,
391 to_u8, from_u8 - u8,
392 to_u16, from_u16 - u16,
393 to_u32, from_u32 - u32,
394 to_u64, from_u64 - u64,
395 to_f32, from_f32 - f32,
396 to_f64, from_f64 - f64
397}
398
399impl<T: Copy + Scalar + Num, N: Dim + DimName> Add<T> for OHyperdual<T, N>
400where
401 DefaultAllocator: Allocator<N>,
402 Owned<T, N>: Copy,
403{
404 type Output = OHyperdual<T, N>;
405
406 #[inline]
407 fn add(self, rhs: T) -> Self {
408 let mut d = self;
409 d[0] = d[0] + rhs;
410 d
411 }
412}
413
414impl<T: Copy + Scalar + Num, N: Dim + DimName> AddAssign<T> for OHyperdual<T, N>
415where
416 DefaultAllocator: Allocator<N>,
417 Owned<T, N>: Copy,
418{
419 #[inline]
420 fn add_assign(&mut self, rhs: T) {
421 *self = (*self) + Self::from_real(rhs)
422 }
423}
424
425impl<T: Copy + Scalar + Num, N: Dim + DimName> Sub<T> for OHyperdual<T, N>
426where
427 DefaultAllocator: Allocator<N>,
428 Owned<T, N>: Copy,
429{
430 type Output = OHyperdual<T, N>;
431
432 #[inline]
433 fn sub(self, rhs: T) -> Self {
434 let mut d = self;
435 d[0] = d[0] - rhs;
436 d
437 }
438}
439
440impl<T: Copy + Scalar + Num, N: Dim + DimName> SubAssign<T> for OHyperdual<T, N>
441where
442 DefaultAllocator: Allocator<N>,
443 Owned<T, N>: Copy,
444{
445 #[inline]
446 fn sub_assign(&mut self, rhs: T) {
447 *self = (*self) - Self::from_real(rhs)
448 }
449}
450
451impl<T: Copy + Scalar + Num, N: Dim + DimName> Mul<T> for OHyperdual<T, N>
452where
453 DefaultAllocator: Allocator<N>,
454 Owned<T, N>: Copy,
455{
456 type Output = OHyperdual<T, N>;
457
458 #[inline]
459 fn mul(self, rhs: T) -> Self {
460 self * Self::from_real(rhs)
461 }
462}
463
464impl<T: Copy + Scalar + Num, N: Dim + DimName> MulAssign<T> for OHyperdual<T, N>
465where
466 DefaultAllocator: Allocator<N>,
467 Owned<T, N>: Copy,
468{
469 #[inline]
470 fn mul_assign(&mut self, rhs: T) {
471 *self = (*self) * Self::from_real(rhs)
472 }
473}
474
475impl<T: Copy + Scalar + Num, N: Dim + DimName> Div<T> for OHyperdual<T, N>
476where
477 DefaultAllocator: Allocator<N>,
478 Owned<T, N>: Copy,
479{
480 type Output = OHyperdual<T, N>;
481
482 #[inline]
483 fn div(self, rhs: T) -> Self {
484 self / Self::from_real(rhs)
485 }
486}
487
488impl<T: Copy + Scalar + Num, N: Dim + DimName> DivAssign<T> for OHyperdual<T, N>
489where
490 DefaultAllocator: Allocator<N>,
491 Owned<T, N>: Copy,
492{
493 #[inline]
494 fn div_assign(&mut self, rhs: T) {
495 *self = (*self) / Self::from_real(rhs)
496 }
497}
498
499impl<T: Copy + Scalar + Signed, N: Dim + DimName> Neg for OHyperdual<T, N>
500where
501 DefaultAllocator: Allocator<N>,
502 Owned<T, N>: Copy,
503{
504 type Output = Self;
505
506 #[inline]
507 fn neg(self) -> Self {
508 Self(self.map(|x| x.neg()))
509 }
510}
511
512impl<T: Copy + Scalar + Num, N: Dim + DimName> Add<Self> for OHyperdual<T, N>
513where
514 DefaultAllocator: Allocator<N>,
515 Owned<T, N>: Copy,
516{
517 type Output = Self;
518
519 #[inline]
520 fn add(self, rhs: Self) -> Self {
521 Self(self.zip_map(&rhs, |x, y| x + y))
522 }
523}
524
525impl<T: Copy + Scalar + Num, N: Dim + DimName> Add<&Self> for OHyperdual<T, N>
526where
527 DefaultAllocator: Allocator<N>,
528 Owned<T, N>: Copy,
529{
530 type Output = Self;
531
532 #[inline]
533 fn add(self, rhs: &Self) -> Self {
534 Self(self.zip_map(rhs, |x, y| x + y))
535 }
536}
537
538impl<T: Copy + Scalar + Num, N: Dim + DimName> AddAssign<Self> for OHyperdual<T, N>
539where
540 DefaultAllocator: Allocator<N>,
541 Owned<T, N>: Copy,
542{
543 #[inline]
544 fn add_assign(&mut self, rhs: Self) {
545 *self = (*self) + rhs
546 }
547}
548
549impl<T: Copy + Scalar + Num, N: Dim + DimName> Sub<Self> for OHyperdual<T, N>
550where
551 DefaultAllocator: Allocator<N>,
552 Owned<T, N>: Copy,
553{
554 type Output = Self;
555
556 #[inline]
557 fn sub(self, rhs: Self) -> Self {
558 Self(self.zip_map(&rhs, |x, y| x - y))
559 }
560}
561
562impl<T: Copy + Scalar + Num, N: Dim + DimName> Sub<&Self> for OHyperdual<T, N>
563where
564 DefaultAllocator: Allocator<N>,
565 Owned<T, N>: Copy,
566{
567 type Output = Self;
568
569 #[inline]
570 fn sub(self, rhs: &Self) -> Self {
571 Self(self.zip_map(rhs, |x, y| x - y))
572 }
573}
574
575impl<T: Copy + Scalar + Num, N: Dim + DimName> SubAssign<Self> for OHyperdual<T, N>
576where
577 DefaultAllocator: Allocator<N>,
578 Owned<T, N>: Copy,
579{
580 #[inline]
581 fn sub_assign(&mut self, rhs: Self) {
582 *self = (*self) - rhs
583 }
584}
585
586impl<T: Copy + Scalar + Num, N: Dim + DimName> Mul<Self> for OHyperdual<T, N>
587where
588 DefaultAllocator: Allocator<N>,
589 Owned<T, N>: Copy,
590{
591 type Output = Self;
592
593 #[allow(clippy::suspicious_arithmetic_impl)]
594 #[inline]
595 fn mul(self, rhs: Self) -> Self {
596 let mut v = self.zip_map(&rhs, |x, y| rhs.real() * x + self.real() * y);
597 v[0] = self.real() * rhs.real();
598 Self(v)
599 }
600}
601
602impl<T: Copy + Scalar + Num, N: Dim + DimName> Mul<&Self> for OHyperdual<T, N>
603where
604 DefaultAllocator: Allocator<N>,
605 Owned<T, N>: Copy,
606{
607 type Output = Self;
608
609 #[allow(clippy::suspicious_arithmetic_impl)]
610 #[inline]
611 fn mul(self, rhs: &Self) -> Self {
612 let mut v = self.zip_map(rhs, |x, y| rhs.real() * x + self.real() * y);
613 v[0] = self.real() * rhs.real();
614 Self(v)
615 }
616}
617
618impl<T: Copy + Scalar + Num, N: Dim + DimName> MulAssign<Self> for OHyperdual<T, N>
619where
620 DefaultAllocator: Allocator<N>,
621 Owned<T, N>: Copy,
622{
623 #[inline]
624 fn mul_assign(&mut self, rhs: Self) {
625 *self = (*self) * rhs
626 }
627}
628
629macro_rules! impl_mul_add {
630 ($(<$a:ident, $b:ident>),*) => {
631 $(
632 impl<T: Copy + Scalar + Num + Mul + Add, N: Dim + DimName> MulAdd<$a, $b> for OHyperdual<T,N>
633 where
634 DefaultAllocator: Allocator<N>,
635 Owned<T, N>: Copy,
636 {
637 type Output = OHyperdual<T,N>;
638
639 #[inline]
640 fn mul_add(self, a: $a, b: $b) -> Self {
641 (self * a) + b
642 }
643 }
644
645 impl<T: Copy + Scalar + Num + Mul + Add, N: Dim + DimName> MulAddAssign<$a, $b> for OHyperdual<T,N>
646 where
647 DefaultAllocator: Allocator<N>,
648 Owned<T, N>: Copy,
649 {
650 #[inline]
651 fn mul_add_assign(&mut self, a: $a, b: $b) {
652 *self = (*self * a) + b;
653 }
654 }
655 )*
656 }
657}
658
659impl_mul_add! {
660 <Self, Self>,
661 <T, Self>,
662 <Self, T>,
663 <T, T>
664}
665
666impl<T: Copy + Scalar + Num, N: Dim + DimName> Div<Self> for OHyperdual<T, N>
667where
668 DefaultAllocator: Allocator<N>,
669 Owned<T, N>: Copy,
670{
671 type Output = Self;
672
673 #[inline]
674 #[allow(clippy::suspicious_arithmetic_impl)]
675 fn div(self, rhs: Self) -> Self {
676 let d = rhs.real() * rhs.real();
677
678 let mut v = self.zip_map(&rhs, |x, y| (rhs.real() * x - self.real() * y) / d);
679 v[0] = self.real() / rhs.real();
680 Self(v)
681 }
682}
683
684impl<T: Copy + Scalar + Num, N: Dim + DimName> Div<&Self> for OHyperdual<T, N>
685where
686 DefaultAllocator: Allocator<N>,
687 Owned<T, N>: Copy,
688{
689 type Output = Self;
690
691 #[inline]
692 #[allow(clippy::suspicious_arithmetic_impl)]
693 fn div(self, rhs: &Self) -> Self {
694 let d = rhs.real() * rhs.real();
695
696 let mut v = self.zip_map(rhs, |x, y| (rhs.real() * x - self.real() * y) / d);
697 v[0] = self.real() / rhs.real();
698 Self(v)
699 }
700}
701
702impl<T: Copy + Scalar + Num, N: Dim + DimName> DivAssign<Self> for OHyperdual<T, N>
703where
704 DefaultAllocator: Allocator<N>,
705 Owned<T, N>: Copy,
706{
707 #[inline]
708 fn div_assign(&mut self, rhs: Self) {
709 *self = (*self) / rhs
710 }
711}
712
713impl<T: Copy + Scalar + Num, N: Dim + DimName> Rem<Self> for OHyperdual<T, N>
714where
715 DefaultAllocator: Allocator<N>,
716 Owned<T, N>: Copy,
717{
718 type Output = Self;
719
720 fn rem(self, _: Self) -> Self {
725 unimplemented!()
726 }
727}
728
729impl<T: Copy + Scalar + Num, N: Dim + DimName> Rem<&Self> for OHyperdual<T, N>
730where
731 DefaultAllocator: Allocator<N>,
732 Owned<T, N>: Copy,
733{
734 type Output = Self;
735
736 fn rem(self, _: &Self) -> Self {
737 unimplemented!()
738 }
739}
740
741impl<T: Copy + Scalar + Num, N: Dim + DimName> RemAssign<Self> for OHyperdual<T, N>
742where
743 DefaultAllocator: Allocator<N>,
744 Owned<T, N>: Copy,
745{
746 fn rem_assign(&mut self, _: Self) {
747 unimplemented!()
748 }
749}
750
751impl<T: Copy + Scalar, P: Into<OHyperdual<T, N>>, N: Dim + DimName> Pow<P> for OHyperdual<T, N>
752where
753 OHyperdual<T, N>: Float,
754 DefaultAllocator: Allocator<N>,
755 Owned<T, N>: Copy,
756{
757 type Output = Self;
758
759 #[inline]
760 fn pow(self, rhs: P) -> Self {
761 self.powf(rhs.into())
762 }
763}
764
765impl<T: Copy + Scalar, N: Dim + DimName> Inv for OHyperdual<T, N>
766where
767 Self: One + Div<Output = Self>,
768 DefaultAllocator: Allocator<N>,
769 Owned<T, N>: Copy,
770{
771 type Output = Self;
772
773 #[inline]
774 fn inv(self) -> Self {
775 Self::one() / self
776 }
777}
778
779impl<T: Copy + Scalar, N: Dim + DimName> Signed for OHyperdual<T, N>
780where
781 T: Signed + PartialOrd,
782 DefaultAllocator: Allocator<N>,
783 Owned<T, N>: Copy,
784{
785 #[inline]
786 fn abs(&self) -> Self {
787 let s = self.real().signum();
788 Self(self.map(|x| x * s))
789 }
790
791 #[inline]
792 fn abs_sub(&self, rhs: &Self) -> Self {
793 if self.real() > rhs.real() {
794 self.sub(*rhs)
795 } else {
796 Self::zero()
797 }
798 }
799
800 #[inline]
801 fn signum(&self) -> Self {
802 Self::from_real(self.real().signum())
803 }
804
805 #[inline]
806 fn is_positive(&self) -> bool {
807 self.real().is_positive()
808 }
809
810 #[inline]
811 fn is_negative(&self) -> bool {
812 self.real().is_negative()
813 }
814}
815
816impl<T: Copy + Scalar + Unsigned, N: Dim + DimName> Unsigned for OHyperdual<T, N>
817where
818 Self: Num,
819 DefaultAllocator: Allocator<N>,
820 Owned<T, N>: Copy,
821{
822}
823
824impl<T: Copy + Scalar + Num + Zero, N: Dim + DimName> Zero for OHyperdual<T, N>
825where
826 DefaultAllocator: Allocator<N>,
827 Owned<T, N>: Copy,
828{
829 #[inline]
830 fn zero() -> Self {
831 Self::from_real(T::zero())
832 }
833
834 #[inline]
835 fn is_zero(&self) -> bool {
836 self.iter().all(|x| x.is_zero())
837 }
838}
839
840impl<T: Copy + Scalar + Num + One, N: Dim + DimName> One for OHyperdual<T, N>
841where
842 DefaultAllocator: Allocator<N>,
843 Owned<T, N>: Copy,
844{
845 #[inline]
846 fn one() -> Self {
847 Self::from_real(T::one())
848 }
849
850 #[inline]
851 fn is_one(&self) -> bool
852 where
853 Self: PartialEq,
854 {
855 self.real().is_one()
856 }
857}
858
859impl<T: Copy + Scalar + Num, N: Dim + DimName> Num for OHyperdual<T, N>
860where
861 DefaultAllocator: Allocator<N>,
862 Owned<T, N>: Copy,
863{
864 type FromStrRadixErr = <T as Num>::FromStrRadixErr;
865
866 #[inline]
867 fn from_str_radix(str: &str, radix: u32) -> Result<OHyperdual<T, N>, Self::FromStrRadixErr> {
868 <T as Num>::from_str_radix(str, radix).map(Self::from_real)
869 }
870}
871
872impl<T: Copy + Scalar + Float, N: Dim + DimName> NumCast for OHyperdual<T, N>
873where
874 DefaultAllocator: Allocator<N>,
875 Owned<T, N>: Copy,
876{
877 #[inline]
878 fn from<P: ToPrimitive>(n: P) -> Option<OHyperdual<T, N>> {
879 <T as NumCast>::from(n).map(Self::from_real)
880 }
881}
882
883macro_rules! impl_float_const {
884 ($($c:ident),*) => {
885 $(
886 fn $c()-> Self { Self::from_real(T::$c()) }
887 )*
888 }
889}
890
891impl<T: Copy + Scalar + FloatConst + Zero, N: Dim + DimName> FloatConst for OHyperdual<T, N>
892where
893 DefaultAllocator: Allocator<N>,
894 Owned<T, N>: Copy,
895{
896 impl_float_const!(
897 E,
898 FRAC_1_PI,
899 FRAC_1_SQRT_2,
900 FRAC_2_PI,
901 FRAC_2_SQRT_PI,
902 FRAC_PI_2,
903 FRAC_PI_3,
904 FRAC_PI_4,
905 FRAC_PI_6,
906 FRAC_PI_8,
907 LN_10,
908 LN_2,
909 LOG10_E,
910 LOG2_E,
911 PI,
912 SQRT_2
913 );
914}
915
916macro_rules! impl_real_constant {
917 ($($prop:ident),*) => {
918 $(
919 fn $prop() -> Self { Self::from_real(<T as Float>::$prop()) }
920 )*
921 }
922}
923
924macro_rules! impl_single_boolean_op {
925 ($op:ident REAL) => {
926 fn $op(self) -> bool {
927 self.real().$op()
928 }
929 };
930 ($op:ident OR) => {
931 fn $op(self) -> bool {
932 let mut b = self.real().$op();
933 for x in self.iter().skip(1) {
934 b |= x.$op();
935 }
936 b
937 }
938 };
939 ($op:ident AND) => {
940 fn $op(self) -> bool {
941 let mut b = self.real().$op();
942 for x in self.iter().skip(1) {
943 b &= x.$op();
944 }
945 b
946 }
947 };
948}
949
950macro_rules! impl_boolean_op {
951 ($($op:ident $t:tt),*) => {
952 $(impl_single_boolean_op!($op $t);)*
953 };
954}
955
956macro_rules! impl_real_op {
957 ($($op:ident),*) => {
958 $(
959 fn $op(self) -> Self { Self::from_real(self.real().$op()) }
960 )*
961 }
962}
963
964impl<T: Copy + Scalar + Num + Zero, N: Dim + DimName> Sum for OHyperdual<T, N>
965where
966 DefaultAllocator: Allocator<N>,
967 Owned<T, N>: Copy,
968{
969 fn sum<I: Iterator<Item = OHyperdual<T, N>>>(iter: I) -> Self {
970 iter.fold(Self::zero(), |a, b| a + b)
971 }
972}
973
974impl<'a, T: Copy + Scalar + Num + Zero, N: Dim + DimName> Sum<&'a OHyperdual<T, N>> for OHyperdual<T, N>
975where
976 DefaultAllocator: Allocator<N>,
977 Owned<T, N>: Copy,
978{
979 fn sum<I: Iterator<Item = &'a OHyperdual<T, N>>>(iter: I) -> Self {
980 iter.fold(Self::zero(), |a, b| a + *b)
981 }
982}
983
984impl<T: Copy + Scalar + Num + One, N: Dim + DimName> Product for OHyperdual<T, N>
985where
986 DefaultAllocator: Allocator<N>,
987 Owned<T, N>: Copy,
988{
989 fn product<I: Iterator<Item = OHyperdual<T, N>>>(iter: I) -> Self {
990 iter.fold(Self::one(), |a, b| a * b)
991 }
992}
993
994impl<'a, T: Copy + Scalar + Num + One, N: Dim + DimName> Product<&'a OHyperdual<T, N>> for OHyperdual<T, N>
995where
996 DefaultAllocator: Allocator<N>,
997 Owned<T, N>: Copy,
998{
999 fn product<I: Iterator<Item = &'a OHyperdual<T, N>>>(iter: I) -> Self {
1000 iter.fold(Self::one(), |a, b| a * *b)
1001 }
1002}
1003
1004impl<T: Copy + Scalar, N: Dim + DimName> Float for OHyperdual<T, N>
1005where
1006 T: Float + Signed + FloatConst,
1007 DefaultAllocator: Allocator<N>,
1008 Owned<T, N>: Copy,
1009{
1010 impl_real_constant!(nan, infinity, neg_infinity, neg_zero, min_positive_value, epsilon, min_value, max_value);
1011
1012 impl_boolean_op!(
1013 is_nan OR,
1014 is_infinite OR,
1015 is_finite AND,
1016 is_normal AND,
1017 is_sign_positive REAL,
1018 is_sign_negative REAL
1019 );
1020
1021 #[inline]
1022 fn classify(self) -> FpCategory {
1023 self.real().classify()
1024 }
1025
1026 impl_real_op!(floor, ceil, round, trunc);
1027
1028 #[inline]
1029 fn fract(self) -> Self {
1030 let mut v = self;
1031 v[0] = self.real().fract();
1032 v
1033 }
1034
1035 #[inline]
1036 fn signum(self) -> Self {
1037 Self::from_real(self.real().signum())
1038 }
1039
1040 #[inline]
1041 fn abs(self) -> Self {
1042 let s = self.real().signum();
1043 Self(self.map(|x| x * s))
1044 }
1045
1046 #[inline]
1047 fn max(self, other: Self) -> Self {
1048 if self.real() > other.real() {
1049 self
1050 } else {
1051 other
1052 }
1053 }
1054
1055 #[inline]
1056 fn min(self, other: Self) -> Self {
1057 if self.real() < other.real() {
1058 self
1059 } else {
1060 other
1061 }
1062 }
1063
1064 #[inline]
1065 fn abs_sub(self, rhs: Self) -> Self {
1066 if self.real() > rhs.real() {
1067 self.sub(rhs)
1068 } else {
1069 Self::zero()
1070 }
1071 }
1072
1073 #[inline]
1074 fn mul_add(self, a: Self, b: Self) -> Self {
1075 let mut dual = Self::from_real(self.real().mul_add(a.real(), b.real()));
1076
1077 for x in 1..self.len() {
1078 dual[x] = self[x] * a.real() + self.real() * a[x] + b[x];
1079 }
1080
1081 dual
1082 }
1083
1084 #[inline]
1085 fn recip(self) -> Self {
1086 Self::one() / self
1087 }
1088
1089 #[inline]
1090 fn powi(self, n: i32) -> Self {
1091 let r = self.real().powi(n - 1);
1092 let nf = <T as NumCast>::from(n).expect("Invalid value") * r;
1093
1094 self.map_dual(self.real() * r, |x| nf * *x)
1095 }
1096
1097 #[inline]
1098 fn powf(self, n: Self) -> Self {
1099 let real_part = self.real().powf(n.real());
1100 let a = n.real() * self.real().powf(n.real() - T::one());
1101 let b = real_part * self.real().ln();
1102
1103 let mut v = self.zip_map(&n, |x, y| a * x + b * y);
1104 v[0] = real_part;
1105 Self(v)
1106 }
1107
1108 #[inline]
1109 fn exp(self) -> Self {
1110 let real = self.real().exp();
1111 self.map_dual(real, |x| real * *x)
1112 }
1113
1114 #[inline]
1115 fn exp2(self) -> Self {
1116 let real = self.real().exp2();
1117 self.map_dual(real, |x| *x * T::LN_2() * real)
1118 }
1119
1120 #[inline]
1121 fn ln(self) -> Self {
1122 self.map_dual(self.real().ln(), |x| *x / self.real())
1123 }
1124
1125 #[inline]
1126 fn log(self, base: Self) -> Self {
1127 self.ln() / base.ln()
1128 }
1129
1130 #[inline]
1131 fn log2(self) -> Self {
1132 self.map_dual(self.real().log2(), |x| *x / (self.real() * T::LN_2()))
1133 }
1134
1135 #[inline]
1136 fn log10(self) -> Self {
1137 self.map_dual(self.real().log10(), |x| *x / (self.real() * T::LN_10()))
1138 }
1139
1140 #[inline]
1141 fn sqrt(self) -> Self {
1142 let real = self.real().sqrt();
1143 let d = T::from(1).unwrap() / (T::from(2).unwrap() * real);
1144 self.map_dual(real, |x| *x * d)
1145 }
1146
1147 #[inline]
1148 fn cbrt(self) -> Self {
1149 let real = self.real().cbrt();
1150 self.map_dual(real, |x| *x / (T::from(3).unwrap() * real))
1151 }
1152
1153 #[inline]
1154 fn hypot(self, other: Self) -> Self {
1155 let c = self.real().hypot(other.real());
1156 let mut v = self.zip_map(&other, |x, y| (self.real() * x + other.real() * y) / c);
1157 v[0] = c;
1158 Self(v)
1159 }
1160
1161 #[inline]
1162 fn sin(self) -> Self {
1163 let c = self.real().cos();
1164 self.map_dual(self.real().sin(), |x| *x * c)
1165 }
1166
1167 #[inline]
1168 fn cos(self) -> Self {
1169 let c = self.real().sin();
1170 self.map_dual(self.real().cos(), |x| x.neg() * c)
1171 }
1172
1173 #[inline]
1174 fn tan(self) -> Self {
1175 let t = self.real().tan();
1176 let c = t * t + T::one();
1177 self.map_dual(t, |x| *x * c)
1178 }
1179
1180 #[inline]
1181 fn asin(self) -> Self {
1182 let c = (T::one() - self.real().powi(2)).sqrt();
1184 self.map_dual(self.real().asin(), |x| *x / c)
1185 }
1186
1187 #[inline]
1188 fn acos(self) -> Self {
1189 let c = (T::one() - self.real().powi(2)).sqrt();
1191 self.map_dual(self.real().acos(), |x| x.neg() / c)
1192 }
1193
1194 #[inline]
1195 fn atan(self) -> Self {
1196 let c = T::one() + self.real().powi(2);
1198 self.map_dual(self.real().atan(), |x| *x / c)
1199 }
1200
1201 #[inline]
1202 fn atan2(self, other: Self) -> Self {
1203 let c = self.real().powi(2) + other.real().powi(2);
1204 let mut v = self.zip_map(&other, |x, y| (other.real() * x - self.real() * y) / c);
1205 v[0] = self.real().atan2(other.real());
1206 Self(v)
1207 }
1208
1209 #[inline]
1210 fn sin_cos(self) -> (Self, Self) {
1211 let (s, c) = self.real().sin_cos();
1212 let sn = self.map_dual(s, |x| *x * c);
1213 let cn = self.map_dual(c, |x| x.neg() * s);
1214 (sn, cn)
1215 }
1216
1217 #[inline]
1218 fn exp_m1(self) -> Self {
1219 let c = self.real().exp();
1220 self.map_dual(self.real().exp_m1(), |x| *x * c)
1221 }
1222
1223 #[inline]
1224 fn ln_1p(self) -> Self {
1225 let c = self.real() + T::one();
1226 self.map_dual(self.real().ln_1p(), |x| *x / c)
1227 }
1228
1229 #[inline]
1230 fn sinh(self) -> Self {
1231 let c = self.real().cosh();
1232 self.map_dual(self.real().sinh(), |x| *x * c)
1233 }
1234
1235 #[inline]
1236 fn cosh(self) -> Self {
1237 let c = self.real().sinh();
1238 self.map_dual(self.real().cosh(), |x| *x * c)
1239 }
1240
1241 #[inline]
1242 fn tanh(self) -> Self {
1243 let real = self.real().tanh();
1244 let c = T::one() - real.powi(2);
1245 self.map_dual(real, |x| *x * c)
1246 }
1247
1248 #[inline]
1249 fn asinh(self) -> Self {
1250 let c = (self.real().powi(2) + T::one()).sqrt();
1251 self.map_dual(self.real().asinh(), |x| *x / c)
1252 }
1253
1254 #[inline]
1255 fn acosh(self) -> Self {
1256 let c = (self.real() + T::one()).sqrt() * (self.real() - T::one()).sqrt();
1257 self.map_dual(self.real().acosh(), |x| *x / c)
1258 }
1259
1260 #[inline]
1261 fn atanh(self) -> Self {
1262 let c = T::one() - self.real().powi(2);
1263 self.map_dual(self.real().atanh(), |x| *x / c)
1264 }
1265
1266 #[inline]
1267 fn integer_decode(self) -> (u64, i16, i8) {
1268 self.real().integer_decode()
1269 }
1270
1271 #[inline]
1272 fn to_degrees(self) -> Self {
1273 self.map_dual(self.real().to_degrees(), |x| x.to_degrees())
1274 }
1275
1276 #[inline]
1277 fn to_radians(self) -> Self {
1278 self.map_dual(self.real().to_radians(), |x| x.to_radians())
1279 }
1280}
1281
1282pub type Dual<T> = Hyperdual<T, 2>;
1283
1284impl<T: Copy + Scalar> Dual<T> {
1285 #[inline]
1286 pub fn new(real: T, dual: T) -> Dual<T> {
1287 Dual::from_slice(&[real, dual])
1288 }
1289
1290 #[inline]
1291 pub fn dual(&self) -> T {
1292 self[1]
1293 }
1294
1295 #[inline]
1297 pub fn dual_ref(&self) -> &T {
1298 &self[1]
1299 }
1300
1301 #[inline]
1303 pub fn dual_mut(&mut self) -> &mut T {
1304 &mut self[1]
1305 }
1306}
1307
1308pub type DualN<T, const N: usize> = Hyperdual<T, N>;
1309
1310pub fn hyperspace_from_vector<T: Copy + Scalar + Num + Zero, const D: usize, const N: usize>(v: &SVector<T, N>) -> SVector<Hyperdual<T, D>, N> {
1311 let mut space_slice = vec![Hyperdual::<T, D>::zero(); N];
1312 for i in 0..N {
1313 space_slice[i] = Hyperdual::<T, D>::from_fn(|j| {
1314 if j == 0 {
1315 v[i]
1316 } else if i + 1 == j {
1317 T::one()
1318 } else {
1319 T::zero()
1320 }
1321 });
1322 }
1323 SVector::<Hyperdual<T, D>, N>::from_row_slice(&space_slice)
1324}
1325
1326pub fn vector_from_hyperspace<T: Scalar + Zero + Float, const DIM_VECTOR: usize, const DIM_HYPER: usize>(
1327 x_dual: &SVector<DualN<T, DIM_HYPER>, { DIM_VECTOR }>,
1328) -> SVector<T, DIM_VECTOR> {
1329 x_dual.map(|x| x.real())
1330}
1331
1332pub fn hyperspace_from_ovector<T: Copy + Scalar + Num + Zero, D: Dim + DimName, N: Dim + DimName>(v: &OVector<T, N>) -> OVector<OHyperdual<T, D>, N>
1333where
1334 DefaultAllocator: Allocator<D> + Allocator<N>,
1335 Owned<T, D>: Copy,
1336{
1337 let mut space_slice = vec![OHyperdual::<T, D>::zero(); N::dim()];
1338 for i in 0..N::dim() {
1339 space_slice[i] = OHyperdual::<T, D>::from_fn(|j| {
1340 if j == 0 {
1341 v[i]
1342 } else if i + 1 == j {
1343 T::one()
1344 } else {
1345 T::zero()
1346 }
1347 });
1348 }
1349 OVector::<OHyperdual<T, D>, N>::from_row_slice(&space_slice)
1350}
1351
1352pub fn ovector_from_hyperspace<T: Scalar + Zero + Float, DimVector: Dim + DimName, DimHyper: Dim + DimName>(
1353 x_dual: &OVector<OHyperdual<T, DimHyper>, DimVector>,
1354) -> OVector<T, DimVector>
1355where
1356 DefaultAllocator: Allocator<DimVector> + Allocator<DimVector> + Allocator<DimHyper>,
1357 <DefaultAllocator as Allocator<DimHyper>>::Buffer<T>: Copy,
1358{
1359 x_dual.map(|x| x.real())
1360}