cu_transform/
transform.rs

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

cu_transform/
transform.rs
