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