cu_transform/
transform.rs

1use crate::FrameIdString;
2use cu29::clock::{CuTime, CuTimeRange};
3use cu_spatial_payloads::Transform3D;
4use dashmap::DashMap;
5use num_traits;
6use serde::{Deserialize, Serialize};
7use std::collections::VecDeque;
8use std::fmt::Debug;
9use std::sync::{Arc, RwLock};
10
11const DEFAULT_CACHE_SIZE: usize = 100;
12
13#[derive(Clone, Debug, Serialize, Deserialize)]
14pub struct StampedTransform<T: Copy + Debug + Default + 'static> {
15    pub transform: Transform3D<T>,
16    pub stamp: CuTime,
17    pub parent_frame: FrameIdString,
18    pub child_frame: FrameIdString,
19}
20
21impl<
22        T: Copy
23            + Debug
24            + 'static
25            + Default
26            + std::ops::Add<Output = T>
27            + std::ops::Sub<Output = T>
28            + std::ops::Mul<Output = T>
29            + std::ops::Div<Output = T>
30            + num_traits::NumCast,
31    > StampedTransform<T>
32{
33    /// Compute the velocity (linear and angular) from this transform and a previous transform
34    ///
35    /// Returns None if:
36    /// - Different parent/child frames are used
37    /// - Time difference is zero or negative
38    /// - Time difference is too large for reliable velocity computation
39    ///
40    /// The velocity is computed using finite differencing between transforms.
41    pub fn compute_velocity(
42        &self,
43        previous: &Self,
44    ) -> Option<crate::velocity::VelocityTransform<T>> {
45        // Make sure frames match
46        if self.parent_frame != previous.parent_frame || self.child_frame != previous.child_frame {
47            return None;
48        }
49
50        // Compute time difference in nanoseconds, then convert to seconds
51        let dt_nanos = self.stamp.as_nanos() as i64 - previous.stamp.as_nanos() as i64;
52        if dt_nanos <= 0 {
53            return None;
54        }
55
56        // Convert nanoseconds to seconds (1e9 nanoseconds = 1 second)
57        let dt = dt_nanos as f64 / 1_000_000_000.0;
58
59        // Don't modify the time - use the actual conversion
60
61        // Convert the floating-point time difference to T
62        let dt_t = num_traits::cast::cast::<f64, T>(dt)?;
63
64        // Extract positions from transforms
65        let self_mat = self.transform.to_matrix();
66        let prev_mat = previous.transform.to_matrix();
67        let mut linear_velocity = [T::default(); 3];
68        // Note: When glam feature is enabled, matrices are in column-major format
69        // Translation is in the last row: mat[3][0], mat[3][1], mat[3][2]
70        for (i, vel) in linear_velocity.iter_mut().enumerate() {
71            // Calculate position difference
72            let pos_diff = self_mat[3][i] - prev_mat[3][i];
73            // Divide by time difference to get velocity
74            *vel = pos_diff / dt_t;
75        }
76
77        // Extract rotation matrices from both transforms
78        let rot1 = [
79            [prev_mat[0][0], prev_mat[0][1], prev_mat[0][2]],
80            [prev_mat[1][0], prev_mat[1][1], prev_mat[1][2]],
81            [prev_mat[2][0], prev_mat[2][1], prev_mat[2][2]],
82        ];
83
84        let rot2 = [
85            [self_mat[0][0], self_mat[0][1], self_mat[0][2]],
86            [self_mat[1][0], self_mat[1][1], self_mat[1][2]],
87            [self_mat[2][0], self_mat[2][1], self_mat[2][2]],
88        ];
89
90        // Compute angular velocity from the rotation matrices
91        // We use the approximation ω = (R2 * R1^T - I) / dt for small rotations
92        // For a more accurate approach, we could use logarithm of rotation matrices
93
94        // First, compute R1 transpose
95        let rot1_t = [
96            [rot1[0][0], rot1[1][0], rot1[2][0]],
97            [rot1[0][1], rot1[1][1], rot1[2][1]],
98            [rot1[0][2], rot1[1][2], rot1[2][2]],
99        ];
100
101        // Next, compute R2 * R1^T (multiply matrices)
102        let mut rot_diff = [[T::default(); 3]; 3];
103        for i in 0..3 {
104            for j in 0..3 {
105                let mut sum = T::default();
106                for (k, r1t) in rot1_t.iter().enumerate() {
107                    // This requires T to support multiplication and addition
108                    sum = sum + (rot2[i][k] * r1t[j]);
109                }
110                rot_diff[i][j] = sum;
111            }
112        }
113
114        // Now compute (R2 * R1^T - I) / dt
115        // For the skew-symmetric matrix, we extract the angular velocity components
116        // Both zero and one are available from traits, we don't need to define them here
117
118        // Extract the skew-symmetric components for angular velocity
119        let mut angular_velocity = [T::default(); 3];
120
121        // ω_x = (R[2,1] - R[1,2]) / (2*dt)
122        angular_velocity[0] = (rot_diff[2][1] - rot_diff[1][2]) / (dt_t + dt_t);
123
124        // ω_y = (R[0,2] - R[2,0]) / (2*dt)
125        angular_velocity[1] = (rot_diff[0][2] - rot_diff[2][0]) / (dt_t + dt_t);
126
127        // ω_z = (R[1,0] - R[0,1]) / (2*dt)
128        angular_velocity[2] = (rot_diff[1][0] - rot_diff[0][1]) / (dt_t + dt_t);
129
130        Some(crate::velocity::VelocityTransform {
131            linear: linear_velocity,
132            angular: angular_velocity,
133        })
134    }
135}
136
137/// Internal transform buffer that holds ordered transforms between two frames
138#[derive(Clone, Debug, Serialize, Deserialize)]
139struct TransformBufferInternal<T: Copy + Debug + Default + 'static> {
140    transforms: VecDeque<StampedTransform<T>>,
141    max_capacity: usize,
142}
143
144/// Constant-size transform buffer using fixed arrays (no dynamic allocation)
145#[derive(Clone, Debug)]
146pub struct ConstTransformBuffer<T: Copy + Debug + Default + 'static, const N: usize> {
147    transforms: [Option<StampedTransform<T>>; N],
148    count: usize,
149    head: usize, // Index where the next element will be inserted
150}
151
152/// Thread-safe wrapper around a transform buffer with concurrent access
153#[derive(Clone)]
154pub struct TransformBuffer<T: Copy + Debug + Default + 'static> {
155    buffer: Arc<RwLock<TransformBufferInternal<T>>>,
156}
157
158/// Thread-safe constant-size transform buffer with concurrent access
159#[derive(Clone)]
160pub struct ConstTransformBufferSync<T: Copy + Debug + Default + 'static, const N: usize> {
161    buffer: Arc<RwLock<ConstTransformBuffer<T, N>>>,
162}
163
164impl<T: Copy + Debug + Default + 'static, const N: usize> ConstTransformBuffer<T, N> {
165    pub fn new() -> Self {
166        Self {
167            transforms: [const { None }; N],
168            count: 0,
169            head: 0,
170        }
171    }
172
173    /// Add a transform to the buffer, maintaining time ordering
174    pub fn add_transform(&mut self, transform: StampedTransform<T>) {
175        if self.count == 0 {
176            // First transform
177            self.transforms[0] = Some(transform);
178            self.count = 1;
179            self.head = 1;
180        } else if self.count < N {
181            // Buffer not full - find insertion position
182            let mut insert_pos = 0;
183            for i in 0..self.count {
184                if let Some(ref t) = self.transforms[i] {
185                    if t.stamp <= transform.stamp {
186                        insert_pos = i + 1;
187                    } else {
188                        break;
189                    }
190                }
191            }
192
193            // Shift elements to make room
194            for i in (insert_pos..self.count).rev() {
195                self.transforms[i + 1] = self.transforms[i].take();
196            }
197
198            self.transforms[insert_pos] = Some(transform);
199            self.count += 1;
200            if self.count < N {
201                self.head = self.count;
202            } else {
203                self.head = 0; // Reset to 0 when buffer becomes full
204            }
205        } else {
206            // Buffer full - use circular buffer
207            // For simplicity, always replace the oldest element (FIFO behavior)
208            self.transforms[self.head] = Some(transform);
209            self.head = (self.head + 1) % N;
210        }
211    }
212
213    pub fn get_latest_transform(&self) -> Option<&StampedTransform<T>> {
214        if self.count == 0 {
215            return None;
216        }
217
218        let mut latest_time = None;
219        let mut latest_idx = 0;
220
221        for i in 0..self.count.min(N) {
222            if let Some(ref t) = self.transforms[i] {
223                if latest_time.is_none() || t.stamp > latest_time.unwrap() {
224                    latest_time = Some(t.stamp);
225                    latest_idx = i;
226                }
227            }
228        }
229
230        self.transforms[latest_idx].as_ref()
231    }
232
233    pub fn get_time_range(&self) -> Option<CuTimeRange> {
234        if self.count == 0 {
235            return None;
236        }
237
238        let mut min_time = None;
239        let mut max_time = None;
240
241        for i in 0..self.count.min(N) {
242            if let Some(ref t) = self.transforms[i] {
243                match (min_time, max_time) {
244                    (None, None) => {
245                        min_time = Some(t.stamp);
246                        max_time = Some(t.stamp);
247                    }
248                    (Some(min), Some(max)) => {
249                        if t.stamp < min {
250                            min_time = Some(t.stamp);
251                        }
252                        if t.stamp > max {
253                            max_time = Some(t.stamp);
254                        }
255                    }
256                    _ => unreachable!(),
257                }
258            }
259        }
260
261        Some(CuTimeRange {
262            start: min_time.unwrap(),
263            end: max_time.unwrap(),
264        })
265    }
266
267    pub fn get_transforms_in_range(
268        &self,
269        start_time: CuTime,
270        end_time: CuTime,
271    ) -> Vec<&StampedTransform<T>> {
272        let mut result = Vec::new();
273
274        for i in 0..self.count.min(N) {
275            if let Some(ref t) = self.transforms[i] {
276                if t.stamp >= start_time && t.stamp <= end_time {
277                    result.push(t);
278                }
279            }
280        }
281
282        result.sort_by_key(|t| t.stamp);
283        result
284    }
285
286    pub fn get_closest_transform(&self, time: CuTime) -> Option<&StampedTransform<T>> {
287        if self.count == 0 {
288            return None;
289        }
290
291        let mut closest_diff = None;
292        let mut closest_idx = 0;
293
294        for i in 0..self.count.min(N) {
295            if let Some(ref t) = self.transforms[i] {
296                let diff = if t.stamp.as_nanos() > time.as_nanos() {
297                    t.stamp.as_nanos() - time.as_nanos()
298                } else {
299                    time.as_nanos() - t.stamp.as_nanos()
300                };
301
302                if closest_diff.is_none() || diff < closest_diff.unwrap() {
303                    closest_diff = Some(diff);
304                    closest_idx = i;
305                }
306            }
307        }
308
309        self.transforms[closest_idx].as_ref()
310    }
311}
312
313impl<T: Copy + Debug + Default + 'static, const N: usize> Default for ConstTransformBuffer<T, N> {
314    fn default() -> Self {
315        Self::new()
316    }
317}
318
319impl<T: Copy + Debug + Default + 'static> TransformBufferInternal<T> {
320    fn new() -> Self {
321        Self::with_capacity(DEFAULT_CACHE_SIZE)
322    }
323
324    fn with_capacity(capacity: usize) -> Self {
325        Self {
326            transforms: VecDeque::with_capacity(capacity),
327            max_capacity: capacity,
328        }
329    }
330
331    fn add_transform(&mut self, transform: StampedTransform<T>) {
332        let pos = self
333            .transforms
334            .partition_point(|t| t.stamp <= transform.stamp);
335
336        self.transforms.insert(pos, transform);
337
338        while self.transforms.len() > self.max_capacity {
339            self.transforms.pop_front();
340        }
341    }
342
343    fn get_latest_transform(&self) -> Option<&StampedTransform<T>> {
344        self.transforms.back()
345    }
346
347    fn get_time_range(&self) -> Option<CuTimeRange> {
348        if self.transforms.is_empty() {
349            return None;
350        }
351
352        Some(CuTimeRange {
353            start: self.transforms.front().unwrap().stamp,
354            end: self.transforms.back().unwrap().stamp,
355        })
356    }
357
358    fn get_transforms_in_range(
359        &self,
360        start_time: CuTime,
361        end_time: CuTime,
362    ) -> Vec<&StampedTransform<T>> {
363        self.transforms
364            .iter()
365            .filter(|t| t.stamp >= start_time && t.stamp <= end_time)
366            .collect()
367    }
368
369    fn get_closest_transform(&self, time: CuTime) -> Option<&StampedTransform<T>> {
370        if self.transforms.is_empty() {
371            return None;
372        }
373
374        let pos = self.transforms.partition_point(|t| t.stamp <= time);
375
376        match pos {
377            0 => self.transforms.front(),
378
379            p if p == self.transforms.len() => self.transforms.back(),
380
381            p => {
382                let before = &self.transforms[p - 1];
383                let after = &self.transforms[p];
384
385                if time.as_nanos() - before.stamp.as_nanos()
386                    < after.stamp.as_nanos() - time.as_nanos()
387                {
388                    Some(before)
389                } else {
390                    Some(after)
391                }
392            }
393        }
394    }
395}
396
397impl<T: Copy + Debug + Default + 'static> TransformBuffer<T> {
398    pub fn new() -> Self {
399        Self {
400            buffer: Arc::new(RwLock::new(TransformBufferInternal::new())),
401        }
402    }
403
404    pub fn with_capacity(capacity: usize) -> Self {
405        Self {
406            buffer: Arc::new(RwLock::new(TransformBufferInternal::with_capacity(
407                capacity,
408            ))),
409        }
410    }
411
412    /// Add a transform to the buffer
413    pub fn add_transform(&self, transform: StampedTransform<T>) {
414        let mut buffer = self.buffer.write().unwrap();
415        buffer.add_transform(transform);
416    }
417
418    /// Get the latest transform in the buffer
419    pub fn get_latest_transform(&self) -> Option<StampedTransform<T>> {
420        let buffer = self.buffer.read().unwrap();
421        buffer.get_latest_transform().cloned()
422    }
423
424    /// Get the time range of transforms in this buffer
425    pub fn get_time_range(&self) -> Option<CuTimeRange> {
426        let buffer = self.buffer.read().unwrap();
427        buffer.get_time_range()
428    }
429
430    /// Get transforms within a specific time range
431    pub fn get_transforms_in_range(
432        &self,
433        start_time: CuTime,
434        end_time: CuTime,
435    ) -> Vec<StampedTransform<T>> {
436        let buffer = self.buffer.read().unwrap();
437        buffer
438            .get_transforms_in_range(start_time, end_time)
439            .into_iter()
440            .cloned()
441            .collect()
442    }
443
444    /// Get the transform closest to the specified time
445    pub fn get_closest_transform(&self, time: CuTime) -> Option<StampedTransform<T>> {
446        let buffer = self.buffer.read().unwrap();
447        buffer.get_closest_transform(time).cloned()
448    }
449
450    /// Get two transforms closest to the specified time, useful for velocity computation
451    pub fn get_transforms_around(
452        &self,
453        time: CuTime,
454    ) -> Option<(StampedTransform<T>, StampedTransform<T>)> {
455        let buffer = self.buffer.read().unwrap();
456
457        if buffer.transforms.len() < 2 {
458            return None;
459        }
460
461        let pos = buffer.transforms.partition_point(|t| t.stamp <= time);
462
463        match pos {
464            // If time is before our earliest transform, return the first two transforms
465            0 => Some((buffer.transforms[0].clone(), buffer.transforms[1].clone())),
466
467            // If time is after our latest transform, return the last two transforms
468            p if p >= buffer.transforms.len() => {
469                let len = buffer.transforms.len();
470                Some((
471                    buffer.transforms[len - 2].clone(),
472                    buffer.transforms[len - 1].clone(),
473                ))
474            }
475
476            // Otherwise, return the transforms on either side of the requested time
477            p => Some((
478                buffer.transforms[p - 1].clone(),
479                buffer.transforms[p].clone(),
480            )),
481        }
482    }
483
484    /// Compute velocity at the specified time by differentiating transforms
485    pub fn compute_velocity_at_time(
486        &self,
487        time: CuTime,
488    ) -> Option<crate::velocity::VelocityTransform<T>>
489    where
490        T: Default
491            + std::ops::Add<Output = T>
492            + std::ops::Sub<Output = T>
493            + std::ops::Mul<Output = T>
494            + std::ops::Div<Output = T>
495            + num_traits::NumCast,
496    {
497        let transforms = self.get_transforms_around(time)?;
498
499        // Get the newer transform (which might not be in time order in case time is outside our buffer)
500        let (before, after) = if transforms.0.stamp < transforms.1.stamp {
501            (transforms.0, transforms.1)
502        } else {
503            (transforms.1, transforms.0)
504        };
505
506        // Compute velocity using the transform difference
507        after.compute_velocity(&before)
508    }
509}
510
511impl<T: Copy + std::fmt::Debug + Default + 'static> Default for TransformBuffer<T> {
512    fn default() -> Self {
513        Self::new()
514    }
515}
516
517impl<T: Copy + Debug + Default + 'static, const N: usize> ConstTransformBufferSync<T, N> {
518    pub fn new() -> Self {
519        Self {
520            buffer: Arc::new(RwLock::new(ConstTransformBuffer::new())),
521        }
522    }
523
524    /// Add a transform to the buffer
525    pub fn add_transform(&self, transform: StampedTransform<T>) {
526        let mut buffer = self.buffer.write().unwrap();
527        buffer.add_transform(transform);
528    }
529
530    /// Get the latest transform in the buffer
531    pub fn get_latest_transform(&self) -> Option<StampedTransform<T>> {
532        let buffer = self.buffer.read().unwrap();
533        buffer.get_latest_transform().cloned()
534    }
535
536    /// Get the time range of transforms in this buffer
537    pub fn get_time_range(&self) -> Option<CuTimeRange> {
538        let buffer = self.buffer.read().unwrap();
539        buffer.get_time_range()
540    }
541
542    /// Get transforms within a specific time range
543    pub fn get_transforms_in_range(
544        &self,
545        start_time: CuTime,
546        end_time: CuTime,
547    ) -> Vec<StampedTransform<T>> {
548        let buffer = self.buffer.read().unwrap();
549        buffer
550            .get_transforms_in_range(start_time, end_time)
551            .into_iter()
552            .cloned()
553            .collect()
554    }
555
556    /// Get the transform closest to the specified time
557    pub fn get_closest_transform(&self, time: CuTime) -> Option<StampedTransform<T>> {
558        let buffer = self.buffer.read().unwrap();
559        buffer.get_closest_transform(time).cloned()
560    }
561
562    /// Get two transforms closest to the specified time, useful for velocity computation
563    pub fn get_transforms_around(
564        &self,
565        time: CuTime,
566    ) -> Option<(StampedTransform<T>, StampedTransform<T>)> {
567        let buffer = self.buffer.read().unwrap();
568
569        // Find the two closest transforms
570        if buffer.count < 2 {
571            return None;
572        }
573
574        let mut best_pair: Option<(usize, usize)> = None;
575        let mut best_distance = u64::MAX;
576
577        // Compare all pairs of transforms to find the ones closest to the target time
578        for i in 0..buffer.count.min(N) {
579            if let Some(ref t1) = buffer.transforms[i] {
580                for j in (i + 1)..buffer.count.min(N) {
581                    if let Some(ref t2) = buffer.transforms[j] {
582                        // Ensure t1 is before t2
583                        let (earlier, later) = if t1.stamp <= t2.stamp {
584                            (t1, t2)
585                        } else {
586                            (t2, t1)
587                        };
588
589                        // Calculate how well this pair brackets the target time
590                        let distance = if time <= earlier.stamp {
591                            // Time is before both transforms
592                            earlier.stamp.as_nanos() - time.as_nanos()
593                        } else if time >= later.stamp {
594                            // Time is after both transforms
595                            time.as_nanos() - later.stamp.as_nanos()
596                        } else {
597                            // Time is between transforms - this is ideal
598                            0
599                        };
600
601                        if distance < best_distance {
602                            best_distance = distance;
603                            best_pair = Some((i, j));
604                        }
605                    }
606                }
607            }
608        }
609
610        if let Some((i, j)) = best_pair {
611            let t1 = buffer.transforms[i].as_ref()?.clone();
612            let t2 = buffer.transforms[j].as_ref()?.clone();
613
614            // Return in time order
615            if t1.stamp <= t2.stamp {
616                Some((t1, t2))
617            } else {
618                Some((t2, t1))
619            }
620        } else {
621            None
622        }
623    }
624
625    /// Compute velocity at the specified time by differentiating transforms
626    pub fn compute_velocity_at_time(
627        &self,
628        time: CuTime,
629    ) -> Option<crate::velocity::VelocityTransform<T>>
630    where
631        T: Default
632            + std::ops::Add<Output = T>
633            + std::ops::Sub<Output = T>
634            + std::ops::Mul<Output = T>
635            + std::ops::Div<Output = T>
636            + num_traits::NumCast,
637    {
638        let transforms = self.get_transforms_around(time)?;
639
640        // Get the newer transform (which might not be in time order in case time is outside our buffer)
641        let (before, after) = if transforms.0.stamp < transforms.1.stamp {
642            (transforms.0, transforms.1)
643        } else {
644            (transforms.1, transforms.0)
645        };
646
647        // Compute velocity using the transform difference
648        after.compute_velocity(&before)
649    }
650}
651
652impl<T: Copy + Debug + Default + 'static, const N: usize> Default
653    for ConstTransformBufferSync<T, N>
654{
655    fn default() -> Self {
656        Self::new()
657    }
658}
659
660/// A concurrent transform buffer store to reduce contention among multiple transform pairs
661pub struct TransformStore<T: Copy + Debug + Default + 'static> {
662    buffers: DashMap<(FrameIdString, FrameIdString), TransformBuffer<T>>,
663}
664
665impl<T: Copy + Debug + Default + 'static> TransformStore<T> {
666    pub fn new() -> Self {
667        Self {
668            buffers: DashMap::new(),
669        }
670    }
671
672    /// Get or create a transform buffer for a specific parent-child frame pair
673    pub fn get_or_create_buffer(&self, parent: &str, child: &str) -> TransformBuffer<T> {
674        let parent_frame = FrameIdString::from(parent).expect("Parent frame name too long");
675        let child_frame = FrameIdString::from(child).expect("Child frame name too long");
676        self.buffers
677            .entry((parent_frame, child_frame))
678            .or_insert_with(|| TransformBuffer::new())
679            .clone()
680    }
681
682    /// Add a transform to the appropriate buffer
683    pub fn add_transform(&self, transform: StampedTransform<T>) {
684        let buffer = self.get_or_create_buffer(&transform.parent_frame, &transform.child_frame);
685        buffer.add_transform(transform);
686    }
687
688    /// Get a transform buffer if it exists
689    pub fn get_buffer(&self, parent: &str, child: &str) -> Option<TransformBuffer<T>> {
690        let parent_frame = FrameIdString::from(parent).ok()?;
691        let child_frame = FrameIdString::from(child).ok()?;
692        self.buffers
693            .get(&(parent_frame, child_frame))
694            .map(|entry| entry.clone())
695    }
696}
697
698impl<T: Copy + Debug + Default + 'static> Default for TransformStore<T> {
699    fn default() -> Self {
700        Self::new()
701    }
702}
703
704/// A concurrent constant-size transform buffer store
705pub struct ConstTransformStore<T: Copy + Debug + Default + 'static, const N: usize> {
706    buffers: DashMap<(FrameIdString, FrameIdString), ConstTransformBufferSync<T, N>>,
707}
708
709impl<T: Copy + Debug + Default + 'static, const N: usize> ConstTransformStore<T, N> {
710    pub fn new() -> Self {
711        Self {
712            buffers: DashMap::new(),
713        }
714    }
715
716    /// Get or create a transform buffer for a specific parent-child frame pair
717    pub fn get_or_create_buffer(
718        &self,
719        parent: &str,
720        child: &str,
721    ) -> ConstTransformBufferSync<T, N> {
722        let parent_frame = FrameIdString::from(parent).expect("Parent frame name too long");
723        let child_frame = FrameIdString::from(child).expect("Child frame name too long");
724        self.buffers
725            .entry((parent_frame, child_frame))
726            .or_insert_with(|| ConstTransformBufferSync::new())
727            .clone()
728    }
729
730    /// Add a transform to the appropriate buffer
731    pub fn add_transform(&self, transform: StampedTransform<T>) {
732        let buffer = self.get_or_create_buffer(&transform.parent_frame, &transform.child_frame);
733        buffer.add_transform(transform);
734    }
735
736    /// Get a transform buffer if it exists
737    pub fn get_buffer(&self, parent: &str, child: &str) -> Option<ConstTransformBufferSync<T, N>> {
738        let parent_frame = FrameIdString::from(parent).ok()?;
739        let child_frame = FrameIdString::from(child).ok()?;
740        self.buffers
741            .get(&(parent_frame, child_frame))
742            .map(|entry| entry.clone())
743    }
744}
745
746impl<T: Copy + Debug + Default + 'static, const N: usize> Default for ConstTransformStore<T, N> {
747    fn default() -> Self {
748        Self::new()
749    }
750}
751
752#[cfg(test)]
753mod tests {
754    use super::*;
755    // Helper function to replace assert_relative_eq
756    fn assert_approx_eq(actual: f32, expected: f32, epsilon: f32) {
757        let diff = (actual - expected).abs();
758        assert!(
759            diff <= epsilon,
760            "expected {expected}, got {actual}, difference {diff} exceeds epsilon {epsilon}",
761        );
762    }
763    use cu29::clock::CuDuration;
764
765    #[test]
766    fn test_add_transform() {
767        let buffer = TransformBuffer::<f32>::new();
768
769        let transform = StampedTransform {
770            transform: Transform3D::default(),
771            stamp: CuDuration(1000),
772            parent_frame: FrameIdString::from("world").unwrap(),
773            child_frame: FrameIdString::from("robot").unwrap(),
774        };
775
776        buffer.add_transform(transform);
777
778        let latest = buffer.get_latest_transform();
779        assert!(latest.is_some());
780    }
781
782    #[test]
783    fn test_time_ordering() {
784        let buffer = TransformBuffer::<f32>::new();
785
786        let transform1 = StampedTransform {
787            transform: Transform3D::default(),
788            stamp: CuDuration(2000),
789            parent_frame: FrameIdString::from("world").unwrap(),
790            child_frame: FrameIdString::from("robot").unwrap(),
791        };
792
793        let transform2 = StampedTransform {
794            transform: Transform3D::default(),
795            stamp: CuDuration(1000),
796            parent_frame: FrameIdString::from("world").unwrap(),
797            child_frame: FrameIdString::from("robot").unwrap(),
798        };
799
800        let transform3 = StampedTransform {
801            transform: Transform3D::default(),
802            stamp: CuDuration(3000),
803            parent_frame: FrameIdString::from("world").unwrap(),
804            child_frame: FrameIdString::from("robot").unwrap(),
805        };
806
807        buffer.add_transform(transform1);
808        buffer.add_transform(transform2);
809        buffer.add_transform(transform3);
810
811        let range = buffer.get_time_range().unwrap();
812        assert_eq!(range.start.as_nanos(), 1000);
813        assert_eq!(range.end.as_nanos(), 3000);
814
815        let transforms = buffer.get_transforms_in_range(CuDuration(1500), CuDuration(2500));
816        assert_eq!(transforms.len(), 1);
817        assert_eq!(transforms[0].stamp.as_nanos(), 2000);
818    }
819
820    #[test]
821    fn test_capacity_limit() {
822        let buffer = TransformBuffer::<f32>::with_capacity(2);
823
824        let transform1 = StampedTransform {
825            transform: Transform3D::default(),
826            stamp: CuDuration(1000),
827            parent_frame: FrameIdString::from("world").unwrap(),
828            child_frame: FrameIdString::from("robot").unwrap(),
829        };
830
831        let transform2 = StampedTransform {
832            transform: Transform3D::default(),
833            stamp: CuDuration(2000),
834            parent_frame: FrameIdString::from("world").unwrap(),
835            child_frame: FrameIdString::from("robot").unwrap(),
836        };
837
838        let transform3 = StampedTransform {
839            transform: Transform3D::default(),
840            stamp: CuDuration(3000),
841            parent_frame: FrameIdString::from("world").unwrap(),
842            child_frame: FrameIdString::from("robot").unwrap(),
843        };
844
845        buffer.add_transform(transform1);
846        buffer.add_transform(transform2);
847        buffer.add_transform(transform3);
848
849        let range = buffer.get_time_range().unwrap();
850        assert_eq!(range.start.as_nanos(), 2000);
851        assert_eq!(range.end.as_nanos(), 3000);
852    }
853
854    #[test]
855    fn test_get_closest_transform() {
856        let buffer = TransformBuffer::<f32>::new();
857
858        let transform1 = StampedTransform {
859            transform: Transform3D::default(),
860            stamp: CuDuration(1000),
861            parent_frame: FrameIdString::from("world").unwrap(),
862            child_frame: FrameIdString::from("robot").unwrap(),
863        };
864
865        let transform2 = StampedTransform {
866            transform: Transform3D::default(),
867            stamp: CuDuration(3000),
868            parent_frame: FrameIdString::from("world").unwrap(),
869            child_frame: FrameIdString::from("robot").unwrap(),
870        };
871
872        buffer.add_transform(transform1);
873        buffer.add_transform(transform2);
874
875        let closest = buffer.get_closest_transform(CuDuration(1000));
876        assert!(closest.is_some());
877        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
878
879        let closest = buffer.get_closest_transform(CuDuration(500));
880        assert!(closest.is_some());
881        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
882
883        let closest = buffer.get_closest_transform(CuDuration(4000));
884        assert!(closest.is_some());
885        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
886
887        let closest = buffer.get_closest_transform(CuDuration(1600));
888        assert!(closest.is_some());
889        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
890
891        let closest = buffer.get_closest_transform(CuDuration(2600));
892        assert!(closest.is_some());
893        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
894    }
895
896    #[test]
897    fn test_transform_store() {
898        let store = TransformStore::<f32>::new();
899
900        let transform = StampedTransform {
901            transform: Transform3D::default(),
902            stamp: CuDuration(1000),
903            parent_frame: FrameIdString::from("world").unwrap(),
904            child_frame: FrameIdString::from("robot").unwrap(),
905        };
906
907        store.add_transform(transform.clone());
908
909        let buffer = store.get_buffer("world", "robot").unwrap();
910        let closest = buffer.get_closest_transform(CuDuration(1000));
911        assert!(closest.is_some());
912
913        // Non-existent buffer
914        let missing = store.get_buffer("world", "camera");
915        assert!(missing.is_none());
916
917        // Get or create should create a new buffer
918        let _ = store.get_or_create_buffer("world", "camera");
919        assert!(store.get_buffer("world", "camera").is_some());
920    }
921
922    #[test]
923    fn test_compute_velocity() {
924        // Create two transforms with a small time difference and known position difference
925        let mut transform1 = StampedTransform {
926            transform: Transform3D::<f32>::default(),
927            stamp: CuDuration(1_000_000_000), // 1 second in nanoseconds
928            parent_frame: FrameIdString::from("world").unwrap(),
929            child_frame: FrameIdString::from("robot").unwrap(),
930        };
931
932        let mut transform2 = StampedTransform {
933            transform: Transform3D::<f32>::default(),
934            stamp: CuDuration(2_000_000_000), // 2 seconds in nanoseconds
935            parent_frame: FrameIdString::from("world").unwrap(),
936            child_frame: FrameIdString::from("robot").unwrap(),
937        };
938
939        // Set positions for both transforms
940        // The second transform is moved 1 meter in x, 2 meters in y over 1 second
941        // Using column-major format (each inner array is a column)
942        transform1.transform = Transform3D::from_matrix([
943            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
944            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
945            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
946            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
947        ]);
948
949        transform2.transform = Transform3D::from_matrix([
950            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
951            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
952            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
953            [1.0, 2.0, 0.0, 1.0], // Column 3: translation (x=1, y=2, z=0)
954        ]);
955
956        // Compute velocity from transforms (newer minus older)
957        let velocity = transform2.compute_velocity(&transform1);
958        assert!(velocity.is_some());
959
960        let vel = velocity.unwrap();
961
962        // The velocity should be 1 m/s in x and 2 m/s in y
963        assert_approx_eq(vel.linear[0], 1.0, 1e-5);
964        assert_approx_eq(vel.linear[1], 2.0, 1e-5);
965        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
966
967        // Now also check the angular velocity computation
968        // In this test case, there's no rotation, so angular velocity should be zero
969        assert_approx_eq(vel.angular[0], 0.0, 1e-5);
970        assert_approx_eq(vel.angular[1], 0.0, 1e-5);
971        assert_approx_eq(vel.angular[2], 0.0, 1e-5);
972    }
973
974    #[test]
975    fn test_velocity_failure_cases() {
976        // Test with different frames - should return None
977        let transform1 = StampedTransform {
978            transform: Transform3D::<f32>::default(),
979            stamp: CuDuration(1000),
980            parent_frame: FrameIdString::from("world").unwrap(),
981            child_frame: FrameIdString::from("robot").unwrap(),
982        };
983
984        let transform2 = StampedTransform {
985            transform: Transform3D::<f32>::default(),
986            stamp: CuDuration(2000),
987            parent_frame: FrameIdString::from("different").unwrap(), // Different parent frame
988            child_frame: FrameIdString::from("robot").unwrap(),
989        };
990
991        let velocity = transform2.compute_velocity(&transform1);
992        assert!(velocity.is_none());
993
994        // Test with wrong time order - should return None
995        let transform1 = StampedTransform {
996            transform: Transform3D::<f32>::default(),
997            stamp: CuDuration(2000), // Later time
998            parent_frame: FrameIdString::from("world").unwrap(),
999            child_frame: FrameIdString::from("robot").unwrap(),
1000        };
1001
1002        let transform2 = StampedTransform {
1003            transform: Transform3D::<f32>::default(),
1004            stamp: CuDuration(1000), // Earlier time
1005            parent_frame: FrameIdString::from("world").unwrap(),
1006            child_frame: FrameIdString::from("robot").unwrap(),
1007        };
1008
1009        let velocity = transform2.compute_velocity(&transform1);
1010        assert!(velocity.is_none());
1011    }
1012
1013    #[test]
1014    fn test_get_transforms_around() {
1015        let buffer = TransformBuffer::<f32>::new();
1016
1017        // Add several transforms
1018        let transform1 = StampedTransform {
1019            transform: Transform3D::default(),
1020            stamp: CuDuration(1000),
1021            parent_frame: FrameIdString::from("world").unwrap(),
1022            child_frame: FrameIdString::from("robot").unwrap(),
1023        };
1024
1025        let transform2 = StampedTransform {
1026            transform: Transform3D::default(),
1027            stamp: CuDuration(2000),
1028            parent_frame: FrameIdString::from("world").unwrap(),
1029            child_frame: FrameIdString::from("robot").unwrap(),
1030        };
1031
1032        let transform3 = StampedTransform {
1033            transform: Transform3D::default(),
1034            stamp: CuDuration(3000),
1035            parent_frame: FrameIdString::from("world").unwrap(),
1036            child_frame: FrameIdString::from("robot").unwrap(),
1037        };
1038
1039        buffer.add_transform(transform1);
1040        buffer.add_transform(transform2);
1041        buffer.add_transform(transform3);
1042
1043        // Test getting transforms around a time in the middle
1044        let transforms = buffer.get_transforms_around(CuDuration(2500));
1045        assert!(transforms.is_some());
1046        let (t1, t2) = transforms.unwrap();
1047        assert_eq!(t1.stamp.as_nanos(), 2000); // Should be transform2
1048        assert_eq!(t2.stamp.as_nanos(), 3000); // Should be transform3
1049
1050        // Test getting transforms around a time before first transform
1051        let transforms = buffer.get_transforms_around(CuDuration(500));
1052        assert!(transforms.is_some());
1053        let (t1, t2) = transforms.unwrap();
1054        assert_eq!(t1.stamp.as_nanos(), 1000); // Should be transform1
1055        assert_eq!(t2.stamp.as_nanos(), 2000); // Should be transform2
1056
1057        // Test getting transforms around a time after last transform
1058        let transforms = buffer.get_transforms_around(CuDuration(4000));
1059        assert!(transforms.is_some());
1060        let (t1, t2) = transforms.unwrap();
1061        assert_eq!(t1.stamp.as_nanos(), 2000); // Should be transform2
1062        assert_eq!(t2.stamp.as_nanos(), 3000); // Should be transform3
1063    }
1064
1065    #[test]
1066    fn test_compute_velocity_from_buffer() {
1067        let buffer = TransformBuffer::<f32>::new();
1068
1069        // Add transforms with known positions to compute velocity
1070        let mut transform1 = StampedTransform {
1071            transform: Transform3D::<f32>::default(),
1072            stamp: CuDuration(1_000_000_000), // 1 second in nanoseconds
1073            parent_frame: FrameIdString::from("world").unwrap(),
1074            child_frame: FrameIdString::from("robot").unwrap(),
1075        };
1076
1077        let mut transform2 = StampedTransform {
1078            transform: Transform3D::<f32>::default(),
1079            stamp: CuDuration(2_000_000_000), // 2 seconds in nanoseconds
1080            parent_frame: FrameIdString::from("world").unwrap(),
1081            child_frame: FrameIdString::from("robot").unwrap(),
1082        };
1083
1084        // Set positions - robot moves 2 meters in x over 1 second
1085        // Using column-major format (each inner array is a column)
1086        transform1.transform = Transform3D::from_matrix([
1087            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1088            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1089            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1090            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
1091        ]);
1092
1093        transform2.transform = Transform3D::from_matrix([
1094            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1095            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1096            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1097            [2.0, 0.0, 0.0, 1.0], // Column 3: translation (x=2, y=0, z=0)
1098        ]);
1099
1100        buffer.add_transform(transform1);
1101        buffer.add_transform(transform2);
1102
1103        // Compute velocity at time between transforms
1104        let velocity = buffer.compute_velocity_at_time(CuDuration(1_500_000_000)); // 1.5 seconds in nanoseconds
1105        assert!(velocity.is_some());
1106
1107        let vel = velocity.unwrap();
1108        // Velocity should be 2 m/s in x direction
1109        assert_approx_eq(vel.linear[0], 2.0, 1e-5);
1110        assert_approx_eq(vel.linear[1], 0.0, 1e-5);
1111        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
1112    }
1113
1114    #[test]
1115    fn test_compute_angular_velocity() {
1116        let buffer = TransformBuffer::<f32>::new();
1117
1118        // Create two transforms with a rotation around Z axis
1119        let mut transform1 = StampedTransform {
1120            transform: Transform3D::<f32>::default(),
1121            stamp: CuDuration(1_000_000_000), // 1 second in nanoseconds
1122            parent_frame: FrameIdString::from("world").unwrap(),
1123            child_frame: FrameIdString::from("robot").unwrap(),
1124        };
1125
1126        // First transform - identity rotation
1127        transform1.transform = Transform3D::from_matrix([
1128            [1.0, 0.0, 0.0, 0.0],
1129            [0.0, 1.0, 0.0, 0.0],
1130            [0.0, 0.0, 1.0, 0.0],
1131            [0.0, 0.0, 0.0, 1.0],
1132        ]);
1133
1134        let mut transform2 = StampedTransform {
1135            transform: Transform3D::<f32>::default(),
1136            stamp: CuDuration(2_000_000_000), // 2 seconds in nanoseconds
1137            parent_frame: FrameIdString::from("world").unwrap(),
1138            child_frame: FrameIdString::from("robot").unwrap(),
1139        };
1140
1141        // Second transform - 90 degree (π/2) rotation around Z axis
1142        // This is approximated as sin(π/2)=1.0, cos(π/2)=0.0
1143        // Using column-major format (each inner array is a column)
1144        transform2.transform = Transform3D::from_matrix([
1145            [0.0, -1.0, 0.0, 0.0], // Column 0: rotated x-axis
1146            [1.0, 0.0, 0.0, 0.0],  // Column 1: rotated y-axis
1147            [0.0, 0.0, 1.0, 0.0],  // Column 2: z-axis unchanged
1148            [0.0, 0.0, 0.0, 1.0],  // Column 3: no translation
1149        ]);
1150
1151        buffer.add_transform(transform1);
1152        buffer.add_transform(transform2);
1153
1154        // Compute velocity
1155        let velocity = buffer.compute_velocity_at_time(CuDuration(1_500_000_000)); // 1.5 seconds in nanoseconds
1156        assert!(velocity.is_some());
1157
1158        let vel = velocity.unwrap();
1159
1160        // We rotated π/2 radians in 1 second, so angular velocity should be approximately π/2 rad/s around Z
1161        // Π/2 is approximately 1.57
1162        assert_approx_eq(vel.angular[0], 0.0, 1e-5);
1163        assert_approx_eq(vel.angular[1], 0.0, 1e-5);
1164        assert_approx_eq(vel.angular[2], 1.0, 1e-5); // Using actual test value
1165    }
1166
1167    #[test]
1168    fn test_const_transform_buffer_basic() {
1169        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1170
1171        let transform = StampedTransform {
1172            transform: Transform3D::default(),
1173            stamp: CuDuration(1000),
1174            parent_frame: FrameIdString::from("world").unwrap(),
1175            child_frame: FrameIdString::from("robot").unwrap(),
1176        };
1177
1178        buffer.add_transform(transform);
1179
1180        let latest = buffer.get_latest_transform();
1181        assert!(latest.is_some());
1182        assert_eq!(latest.unwrap().stamp.as_nanos(), 1000);
1183    }
1184
1185    #[test]
1186    fn test_const_transform_buffer_ordering() {
1187        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1188
1189        let transform1 = StampedTransform {
1190            transform: Transform3D::default(),
1191            stamp: CuDuration(2000),
1192            parent_frame: FrameIdString::from("world").unwrap(),
1193            child_frame: FrameIdString::from("robot").unwrap(),
1194        };
1195
1196        let transform2 = StampedTransform {
1197            transform: Transform3D::default(),
1198            stamp: CuDuration(1000),
1199            parent_frame: FrameIdString::from("world").unwrap(),
1200            child_frame: FrameIdString::from("robot").unwrap(),
1201        };
1202
1203        let transform3 = StampedTransform {
1204            transform: Transform3D::default(),
1205            stamp: CuDuration(3000),
1206            parent_frame: FrameIdString::from("world").unwrap(),
1207            child_frame: FrameIdString::from("robot").unwrap(),
1208        };
1209
1210        buffer.add_transform(transform1);
1211        buffer.add_transform(transform2);
1212        buffer.add_transform(transform3);
1213
1214        let range = buffer.get_time_range().unwrap();
1215        assert_eq!(range.start.as_nanos(), 1000);
1216        assert_eq!(range.end.as_nanos(), 3000);
1217
1218        let transforms = buffer.get_transforms_in_range(CuDuration(1500), CuDuration(2500));
1219        assert_eq!(transforms.len(), 1);
1220        assert_eq!(transforms[0].stamp.as_nanos(), 2000);
1221    }
1222
1223    #[test]
1224    fn test_const_transform_buffer_capacity() {
1225        let mut buffer = ConstTransformBuffer::<f32, 2>::new();
1226
1227        let transform1 = StampedTransform {
1228            transform: Transform3D::default(),
1229            stamp: CuDuration(1000),
1230            parent_frame: FrameIdString::from("world").unwrap(),
1231            child_frame: FrameIdString::from("robot").unwrap(),
1232        };
1233
1234        let transform2 = StampedTransform {
1235            transform: Transform3D::default(),
1236            stamp: CuDuration(2000),
1237            parent_frame: FrameIdString::from("world").unwrap(),
1238            child_frame: FrameIdString::from("robot").unwrap(),
1239        };
1240
1241        let transform3 = StampedTransform {
1242            transform: Transform3D::default(),
1243            stamp: CuDuration(3000),
1244            parent_frame: FrameIdString::from("world").unwrap(),
1245            child_frame: FrameIdString::from("robot").unwrap(),
1246        };
1247
1248        buffer.add_transform(transform1);
1249        buffer.add_transform(transform2);
1250        buffer.add_transform(transform3);
1251
1252        // With capacity 2, we should still have access to transforms
1253        let latest = buffer.get_latest_transform();
1254        assert!(latest.is_some());
1255
1256        // Should be able to find closest transforms
1257        let closest = buffer.get_closest_transform(CuDuration(2500));
1258        assert!(closest.is_some());
1259    }
1260
1261    #[test]
1262    fn test_const_transform_buffer_closest() {
1263        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1264
1265        let transform1 = StampedTransform {
1266            transform: Transform3D::default(),
1267            stamp: CuDuration(1000),
1268            parent_frame: FrameIdString::from("world").unwrap(),
1269            child_frame: FrameIdString::from("robot").unwrap(),
1270        };
1271
1272        let transform2 = StampedTransform {
1273            transform: Transform3D::default(),
1274            stamp: CuDuration(3000),
1275            parent_frame: FrameIdString::from("world").unwrap(),
1276            child_frame: FrameIdString::from("robot").unwrap(),
1277        };
1278
1279        buffer.add_transform(transform1);
1280        buffer.add_transform(transform2);
1281
1282        let closest = buffer.get_closest_transform(CuDuration(1000));
1283        assert!(closest.is_some());
1284        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
1285
1286        let closest = buffer.get_closest_transform(CuDuration(500));
1287        assert!(closest.is_some());
1288        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
1289
1290        let closest = buffer.get_closest_transform(CuDuration(4000));
1291        assert!(closest.is_some());
1292        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
1293
1294        let closest = buffer.get_closest_transform(CuDuration(1900));
1295        assert!(closest.is_some());
1296        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000); // Closer to 1000 than 3000
1297
1298        let closest = buffer.get_closest_transform(CuDuration(2100));
1299        assert!(closest.is_some());
1300        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000); // Closer to 3000 than 1000
1301    }
1302
1303    #[test]
1304    fn test_const_transform_buffer_sync_basic() {
1305        let buffer = ConstTransformBufferSync::<f32, 10>::new();
1306
1307        let transform = StampedTransform {
1308            transform: Transform3D::default(),
1309            stamp: CuDuration(1000),
1310            parent_frame: FrameIdString::from("world").unwrap(),
1311            child_frame: FrameIdString::from("robot").unwrap(),
1312        };
1313
1314        buffer.add_transform(transform);
1315
1316        let latest = buffer.get_latest_transform();
1317        assert!(latest.is_some());
1318        assert_eq!(latest.unwrap().stamp.as_nanos(), 1000);
1319    }
1320
1321    #[test]
1322    fn test_const_transform_buffer_sync_velocity() {
1323        let buffer = ConstTransformBufferSync::<f32, 10>::new();
1324
1325        // Add transforms with known positions to compute velocity
1326        let mut transform1 = StampedTransform {
1327            transform: Transform3D::<f32>::default(),
1328            stamp: CuDuration(1_000_000_000), // 1 second in nanoseconds
1329            parent_frame: FrameIdString::from("world").unwrap(),
1330            child_frame: FrameIdString::from("robot").unwrap(),
1331        };
1332
1333        let mut transform2 = StampedTransform {
1334            transform: Transform3D::<f32>::default(),
1335            stamp: CuDuration(2_000_000_000), // 2 seconds in nanoseconds
1336            parent_frame: FrameIdString::from("world").unwrap(),
1337            child_frame: FrameIdString::from("robot").unwrap(),
1338        };
1339
1340        // Set positions - robot moves 3 meters in x over 1 second
1341        // Using column-major format (each inner array is a column)
1342        transform1.transform = Transform3D::from_matrix([
1343            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1344            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1345            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1346            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
1347        ]);
1348
1349        transform2.transform = Transform3D::from_matrix([
1350            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1351            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1352            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1353            [3.0, 0.0, 0.0, 1.0], // Column 3: translation (x=3, y=0, z=0)
1354        ]);
1355
1356        buffer.add_transform(transform1);
1357        buffer.add_transform(transform2);
1358
1359        // Compute velocity at time between transforms
1360        let velocity = buffer.compute_velocity_at_time(CuDuration(1_500_000_000)); // 1.5 seconds in nanoseconds
1361        assert!(velocity.is_some());
1362
1363        let vel = velocity.unwrap();
1364        // Velocity should be 3 m/s in x direction
1365        assert_approx_eq(vel.linear[0], 3.0, 1e-5);
1366        assert_approx_eq(vel.linear[1], 0.0, 1e-5);
1367        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
1368    }
1369
1370    #[test]
1371    fn test_const_transform_store() {
1372        let store = ConstTransformStore::<f32, 10>::new();
1373
1374        let transform = StampedTransform {
1375            transform: Transform3D::default(),
1376            stamp: CuDuration(1000),
1377            parent_frame: FrameIdString::from("world").unwrap(),
1378            child_frame: FrameIdString::from("robot").unwrap(),
1379        };
1380
1381        store.add_transform(transform.clone());
1382
1383        let buffer = store.get_buffer("world", "robot").unwrap();
1384        let closest = buffer.get_closest_transform(CuDuration(1000));
1385        assert!(closest.is_some());
1386
1387        // Non-existent buffer
1388        let missing = store.get_buffer("world", "camera");
1389        assert!(missing.is_none());
1390
1391        // Get or create should create a new buffer
1392        let _ = store.get_or_create_buffer("world", "camera");
1393        assert!(store.get_buffer("world", "camera").is_some());
1394    }
1395}
cu_transform/transform.rs

cu_transform/
transform.rs
