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::CuTime;
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: CuTime::from_nanos(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: CuTime::from_nanos(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: CuTime::from_nanos(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: CuTime::from_nanos(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 =
808            buffer.get_transforms_in_range(CuTime::from_nanos(1500), CuTime::from_nanos(2500));
809        assert_eq!(transforms.len(), 1);
810        assert_eq!(transforms[0].stamp.as_nanos(), 2000);
811    }
812
813    #[test]
814    fn test_capacity_limit() {
815        let buffer = TransformBuffer::<f32>::with_capacity(2);
816
817        let transform1 = StampedTransform {
818            transform: Transform3D::default(),
819            stamp: CuTime::from_nanos(1000),
820            parent_frame: FrameIdString::from("world").unwrap(),
821            child_frame: FrameIdString::from("robot").unwrap(),
822        };
823
824        let transform2 = StampedTransform {
825            transform: Transform3D::default(),
826            stamp: CuTime::from_nanos(2000),
827            parent_frame: FrameIdString::from("world").unwrap(),
828            child_frame: FrameIdString::from("robot").unwrap(),
829        };
830
831        let transform3 = StampedTransform {
832            transform: Transform3D::default(),
833            stamp: CuTime::from_nanos(3000),
834            parent_frame: FrameIdString::from("world").unwrap(),
835            child_frame: FrameIdString::from("robot").unwrap(),
836        };
837
838        buffer.add_transform(transform1);
839        buffer.add_transform(transform2);
840        buffer.add_transform(transform3);
841
842        let range = buffer.get_time_range().unwrap();
843        assert_eq!(range.start.as_nanos(), 2000);
844        assert_eq!(range.end.as_nanos(), 3000);
845    }
846
847    #[test]
848    fn test_get_closest_transform() {
849        let buffer = TransformBuffer::<f32>::new();
850
851        let transform1 = StampedTransform {
852            transform: Transform3D::default(),
853            stamp: CuTime::from_nanos(1000),
854            parent_frame: FrameIdString::from("world").unwrap(),
855            child_frame: FrameIdString::from("robot").unwrap(),
856        };
857
858        let transform2 = StampedTransform {
859            transform: Transform3D::default(),
860            stamp: CuTime::from_nanos(3000),
861            parent_frame: FrameIdString::from("world").unwrap(),
862            child_frame: FrameIdString::from("robot").unwrap(),
863        };
864
865        buffer.add_transform(transform1);
866        buffer.add_transform(transform2);
867
868        let closest = buffer.get_closest_transform(CuTime::from_nanos(1000));
869        assert!(closest.is_some());
870        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
871
872        let closest = buffer.get_closest_transform(CuTime::from_nanos(500));
873        assert!(closest.is_some());
874        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
875
876        let closest = buffer.get_closest_transform(CuTime::from_nanos(4000));
877        assert!(closest.is_some());
878        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
879
880        let closest = buffer.get_closest_transform(CuTime::from_nanos(1600));
881        assert!(closest.is_some());
882        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
883
884        let closest = buffer.get_closest_transform(CuTime::from_nanos(2600));
885        assert!(closest.is_some());
886        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
887    }
888
889    #[test]
890    fn test_transform_store() {
891        let store = TransformStore::<f32>::new();
892
893        let transform = StampedTransform {
894            transform: Transform3D::default(),
895            stamp: CuTime::from_nanos(1000),
896            parent_frame: FrameIdString::from("world").unwrap(),
897            child_frame: FrameIdString::from("robot").unwrap(),
898        };
899
900        store.add_transform(transform.clone());
901
902        let buffer = store.get_buffer("world", "robot").unwrap();
903        let closest = buffer.get_closest_transform(CuTime::from_nanos(1000));
904        assert!(closest.is_some());
905
906        // Non-existent buffer
907        let missing = store.get_buffer("world", "camera");
908        assert!(missing.is_none());
909
910        // Get or create should create a new buffer
911        let _ = store.get_or_create_buffer("world", "camera");
912        assert!(store.get_buffer("world", "camera").is_some());
913    }
914
915    #[test]
916    fn test_compute_velocity() {
917        // Create two transforms with a small time difference and known position difference
918        let mut transform1 = StampedTransform {
919            transform: Transform3D::<f32>::default(),
920            stamp: CuTime::from_nanos(1_000_000_000), // 1 second in nanoseconds
921            parent_frame: FrameIdString::from("world").unwrap(),
922            child_frame: FrameIdString::from("robot").unwrap(),
923        };
924
925        let mut transform2 = StampedTransform {
926            transform: Transform3D::<f32>::default(),
927            stamp: CuTime::from_nanos(2_000_000_000), // 2 seconds in nanoseconds
928            parent_frame: FrameIdString::from("world").unwrap(),
929            child_frame: FrameIdString::from("robot").unwrap(),
930        };
931
932        // Set positions for both transforms
933        // The second transform is moved 1 meter in x, 2 meters in y over 1 second
934        // Using column-major format (each inner array is a column)
935        transform1.transform = Transform3D::from_matrix([
936            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
937            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
938            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
939            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
940        ]);
941
942        transform2.transform = Transform3D::from_matrix([
943            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
944            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
945            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
946            [1.0, 2.0, 0.0, 1.0], // Column 3: translation (x=1, y=2, z=0)
947        ]);
948
949        // Compute velocity from transforms (newer minus older)
950        let velocity = transform2.compute_velocity(&transform1);
951        assert!(velocity.is_some());
952
953        let vel = velocity.unwrap();
954
955        // The velocity should be 1 m/s in x and 2 m/s in y
956        assert_approx_eq(vel.linear[0], 1.0, 1e-5);
957        assert_approx_eq(vel.linear[1], 2.0, 1e-5);
958        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
959
960        // Now also check the angular velocity computation
961        // In this test case, there's no rotation, so angular velocity should be zero
962        assert_approx_eq(vel.angular[0], 0.0, 1e-5);
963        assert_approx_eq(vel.angular[1], 0.0, 1e-5);
964        assert_approx_eq(vel.angular[2], 0.0, 1e-5);
965    }
966
967    #[test]
968    fn test_velocity_failure_cases() {
969        // Test with different frames - should return None
970        let transform1 = StampedTransform {
971            transform: Transform3D::<f32>::default(),
972            stamp: CuTime::from_nanos(1000),
973            parent_frame: FrameIdString::from("world").unwrap(),
974            child_frame: FrameIdString::from("robot").unwrap(),
975        };
976
977        let transform2 = StampedTransform {
978            transform: Transform3D::<f32>::default(),
979            stamp: CuTime::from_nanos(2000),
980            parent_frame: FrameIdString::from("different").unwrap(), // Different parent frame
981            child_frame: FrameIdString::from("robot").unwrap(),
982        };
983
984        let velocity = transform2.compute_velocity(&transform1);
985        assert!(velocity.is_none());
986
987        // Test with wrong time order - should return None
988        let transform1 = StampedTransform {
989            transform: Transform3D::<f32>::default(),
990            stamp: CuTime::from_nanos(2000), // Later time
991            parent_frame: FrameIdString::from("world").unwrap(),
992            child_frame: FrameIdString::from("robot").unwrap(),
993        };
994
995        let transform2 = StampedTransform {
996            transform: Transform3D::<f32>::default(),
997            stamp: CuTime::from_nanos(1000), // Earlier time
998            parent_frame: FrameIdString::from("world").unwrap(),
999            child_frame: FrameIdString::from("robot").unwrap(),
1000        };
1001
1002        let velocity = transform2.compute_velocity(&transform1);
1003        assert!(velocity.is_none());
1004    }
1005
1006    #[test]
1007    fn test_get_transforms_around() {
1008        let buffer = TransformBuffer::<f32>::new();
1009
1010        // Add several transforms
1011        let transform1 = StampedTransform {
1012            transform: Transform3D::default(),
1013            stamp: CuTime::from_nanos(1000),
1014            parent_frame: FrameIdString::from("world").unwrap(),
1015            child_frame: FrameIdString::from("robot").unwrap(),
1016        };
1017
1018        let transform2 = StampedTransform {
1019            transform: Transform3D::default(),
1020            stamp: CuTime::from_nanos(2000),
1021            parent_frame: FrameIdString::from("world").unwrap(),
1022            child_frame: FrameIdString::from("robot").unwrap(),
1023        };
1024
1025        let transform3 = StampedTransform {
1026            transform: Transform3D::default(),
1027            stamp: CuTime::from_nanos(3000),
1028            parent_frame: FrameIdString::from("world").unwrap(),
1029            child_frame: FrameIdString::from("robot").unwrap(),
1030        };
1031
1032        buffer.add_transform(transform1);
1033        buffer.add_transform(transform2);
1034        buffer.add_transform(transform3);
1035
1036        // Test getting transforms around a time in the middle
1037        let transforms = buffer.get_transforms_around(CuTime::from_nanos(2500));
1038        assert!(transforms.is_some());
1039        let (t1, t2) = transforms.unwrap();
1040        assert_eq!(t1.stamp.as_nanos(), 2000); // Should be transform2
1041        assert_eq!(t2.stamp.as_nanos(), 3000); // Should be transform3
1042
1043        // Test getting transforms around a time before first transform
1044        let transforms = buffer.get_transforms_around(CuTime::from_nanos(500));
1045        assert!(transforms.is_some());
1046        let (t1, t2) = transforms.unwrap();
1047        assert_eq!(t1.stamp.as_nanos(), 1000); // Should be transform1
1048        assert_eq!(t2.stamp.as_nanos(), 2000); // Should be transform2
1049
1050        // Test getting transforms around a time after last transform
1051        let transforms = buffer.get_transforms_around(CuTime::from_nanos(4000));
1052        assert!(transforms.is_some());
1053        let (t1, t2) = transforms.unwrap();
1054        assert_eq!(t1.stamp.as_nanos(), 2000); // Should be transform2
1055        assert_eq!(t2.stamp.as_nanos(), 3000); // Should be transform3
1056    }
1057
1058    #[test]
1059    fn test_compute_velocity_from_buffer() {
1060        let buffer = TransformBuffer::<f32>::new();
1061
1062        // Add transforms with known positions to compute velocity
1063        let mut transform1 = StampedTransform {
1064            transform: Transform3D::<f32>::default(),
1065            stamp: CuTime::from_nanos(1_000_000_000), // 1 second in nanoseconds
1066            parent_frame: FrameIdString::from("world").unwrap(),
1067            child_frame: FrameIdString::from("robot").unwrap(),
1068        };
1069
1070        let mut transform2 = StampedTransform {
1071            transform: Transform3D::<f32>::default(),
1072            stamp: CuTime::from_nanos(2_000_000_000), // 2 seconds in nanoseconds
1073            parent_frame: FrameIdString::from("world").unwrap(),
1074            child_frame: FrameIdString::from("robot").unwrap(),
1075        };
1076
1077        // Set positions - robot moves 2 meters in x over 1 second
1078        // Using column-major format (each inner array is a column)
1079        transform1.transform = Transform3D::from_matrix([
1080            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1081            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1082            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1083            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
1084        ]);
1085
1086        transform2.transform = Transform3D::from_matrix([
1087            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1088            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1089            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1090            [2.0, 0.0, 0.0, 1.0], // Column 3: translation (x=2, y=0, z=0)
1091        ]);
1092
1093        buffer.add_transform(transform1);
1094        buffer.add_transform(transform2);
1095
1096        // Compute velocity at time between transforms
1097        let velocity = buffer.compute_velocity_at_time(CuTime::from_nanos(1_500_000_000)); // 1.5 seconds in nanoseconds
1098        assert!(velocity.is_some());
1099
1100        let vel = velocity.unwrap();
1101        // Velocity should be 2 m/s in x direction
1102        assert_approx_eq(vel.linear[0], 2.0, 1e-5);
1103        assert_approx_eq(vel.linear[1], 0.0, 1e-5);
1104        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
1105    }
1106
1107    #[test]
1108    fn test_compute_angular_velocity() {
1109        let buffer = TransformBuffer::<f32>::new();
1110
1111        // Create two transforms with a rotation around Z axis
1112        let mut transform1 = StampedTransform {
1113            transform: Transform3D::<f32>::default(),
1114            stamp: CuTime::from_nanos(1_000_000_000), // 1 second in nanoseconds
1115            parent_frame: FrameIdString::from("world").unwrap(),
1116            child_frame: FrameIdString::from("robot").unwrap(),
1117        };
1118
1119        // First transform - identity rotation
1120        transform1.transform = Transform3D::from_matrix([
1121            [1.0, 0.0, 0.0, 0.0],
1122            [0.0, 1.0, 0.0, 0.0],
1123            [0.0, 0.0, 1.0, 0.0],
1124            [0.0, 0.0, 0.0, 1.0],
1125        ]);
1126
1127        let mut transform2 = StampedTransform {
1128            transform: Transform3D::<f32>::default(),
1129            stamp: CuTime::from_nanos(2_000_000_000), // 2 seconds in nanoseconds
1130            parent_frame: FrameIdString::from("world").unwrap(),
1131            child_frame: FrameIdString::from("robot").unwrap(),
1132        };
1133
1134        // Second transform - 90 degree (π/2) rotation around Z axis
1135        // This is approximated as sin(π/2)=1.0, cos(π/2)=0.0
1136        // Using column-major format (each inner array is a column)
1137        transform2.transform = Transform3D::from_matrix([
1138            [0.0, -1.0, 0.0, 0.0], // Column 0: rotated x-axis
1139            [1.0, 0.0, 0.0, 0.0],  // Column 1: rotated y-axis
1140            [0.0, 0.0, 1.0, 0.0],  // Column 2: z-axis unchanged
1141            [0.0, 0.0, 0.0, 1.0],  // Column 3: no translation
1142        ]);
1143
1144        buffer.add_transform(transform1);
1145        buffer.add_transform(transform2);
1146
1147        // Compute velocity
1148        let velocity = buffer.compute_velocity_at_time(CuTime::from_nanos(1_500_000_000)); // 1.5 seconds in nanoseconds
1149        assert!(velocity.is_some());
1150
1151        let vel = velocity.unwrap();
1152
1153        // We rotated π/2 radians in 1 second, so angular velocity should be approximately π/2 rad/s around Z
1154        // Π/2 is approximately 1.57
1155        assert_approx_eq(vel.angular[0], 0.0, 1e-5);
1156        assert_approx_eq(vel.angular[1], 0.0, 1e-5);
1157        assert_approx_eq(vel.angular[2], 1.0, 1e-5); // Using actual test value
1158    }
1159
1160    #[test]
1161    fn test_const_transform_buffer_basic() {
1162        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1163
1164        let transform = StampedTransform {
1165            transform: Transform3D::default(),
1166            stamp: CuTime::from_nanos(1000),
1167            parent_frame: FrameIdString::from("world").unwrap(),
1168            child_frame: FrameIdString::from("robot").unwrap(),
1169        };
1170
1171        buffer.add_transform(transform);
1172
1173        let latest = buffer.get_latest_transform();
1174        assert!(latest.is_some());
1175        assert_eq!(latest.unwrap().stamp.as_nanos(), 1000);
1176    }
1177
1178    #[test]
1179    fn test_const_transform_buffer_ordering() {
1180        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1181
1182        let transform1 = StampedTransform {
1183            transform: Transform3D::default(),
1184            stamp: CuTime::from_nanos(2000),
1185            parent_frame: FrameIdString::from("world").unwrap(),
1186            child_frame: FrameIdString::from("robot").unwrap(),
1187        };
1188
1189        let transform2 = StampedTransform {
1190            transform: Transform3D::default(),
1191            stamp: CuTime::from_nanos(1000),
1192            parent_frame: FrameIdString::from("world").unwrap(),
1193            child_frame: FrameIdString::from("robot").unwrap(),
1194        };
1195
1196        let transform3 = StampedTransform {
1197            transform: Transform3D::default(),
1198            stamp: CuTime::from_nanos(3000),
1199            parent_frame: FrameIdString::from("world").unwrap(),
1200            child_frame: FrameIdString::from("robot").unwrap(),
1201        };
1202
1203        buffer.add_transform(transform1);
1204        buffer.add_transform(transform2);
1205        buffer.add_transform(transform3);
1206
1207        let range = buffer.get_time_range().unwrap();
1208        assert_eq!(range.start.as_nanos(), 1000);
1209        assert_eq!(range.end.as_nanos(), 3000);
1210
1211        let transforms =
1212            buffer.get_transforms_in_range(CuTime::from_nanos(1500), CuTime::from_nanos(2500));
1213        assert_eq!(transforms.len(), 1);
1214        assert_eq!(transforms[0].stamp.as_nanos(), 2000);
1215    }
1216
1217    #[test]
1218    fn test_const_transform_buffer_capacity() {
1219        let mut buffer = ConstTransformBuffer::<f32, 2>::new();
1220
1221        let transform1 = StampedTransform {
1222            transform: Transform3D::default(),
1223            stamp: CuTime::from_nanos(1000),
1224            parent_frame: FrameIdString::from("world").unwrap(),
1225            child_frame: FrameIdString::from("robot").unwrap(),
1226        };
1227
1228        let transform2 = StampedTransform {
1229            transform: Transform3D::default(),
1230            stamp: CuTime::from_nanos(2000),
1231            parent_frame: FrameIdString::from("world").unwrap(),
1232            child_frame: FrameIdString::from("robot").unwrap(),
1233        };
1234
1235        let transform3 = StampedTransform {
1236            transform: Transform3D::default(),
1237            stamp: CuTime::from_nanos(3000),
1238            parent_frame: FrameIdString::from("world").unwrap(),
1239            child_frame: FrameIdString::from("robot").unwrap(),
1240        };
1241
1242        buffer.add_transform(transform1);
1243        buffer.add_transform(transform2);
1244        buffer.add_transform(transform3);
1245
1246        // With capacity 2, we should still have access to transforms
1247        let latest = buffer.get_latest_transform();
1248        assert!(latest.is_some());
1249
1250        // Should be able to find closest transforms
1251        let closest = buffer.get_closest_transform(CuTime::from_nanos(2500));
1252        assert!(closest.is_some());
1253    }
1254
1255    #[test]
1256    fn test_const_transform_buffer_closest() {
1257        let mut buffer = ConstTransformBuffer::<f32, 10>::new();
1258
1259        let transform1 = StampedTransform {
1260            transform: Transform3D::default(),
1261            stamp: CuTime::from_nanos(1000),
1262            parent_frame: FrameIdString::from("world").unwrap(),
1263            child_frame: FrameIdString::from("robot").unwrap(),
1264        };
1265
1266        let transform2 = StampedTransform {
1267            transform: Transform3D::default(),
1268            stamp: CuTime::from_nanos(3000),
1269            parent_frame: FrameIdString::from("world").unwrap(),
1270            child_frame: FrameIdString::from("robot").unwrap(),
1271        };
1272
1273        buffer.add_transform(transform1);
1274        buffer.add_transform(transform2);
1275
1276        let closest = buffer.get_closest_transform(CuTime::from_nanos(1000));
1277        assert!(closest.is_some());
1278        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
1279
1280        let closest = buffer.get_closest_transform(CuTime::from_nanos(500));
1281        assert!(closest.is_some());
1282        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000);
1283
1284        let closest = buffer.get_closest_transform(CuTime::from_nanos(4000));
1285        assert!(closest.is_some());
1286        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000);
1287
1288        let closest = buffer.get_closest_transform(CuTime::from_nanos(1900));
1289        assert!(closest.is_some());
1290        assert_eq!(closest.unwrap().stamp.as_nanos(), 1000); // Closer to 1000 than 3000
1291
1292        let closest = buffer.get_closest_transform(CuTime::from_nanos(2100));
1293        assert!(closest.is_some());
1294        assert_eq!(closest.unwrap().stamp.as_nanos(), 3000); // Closer to 3000 than 1000
1295    }
1296
1297    #[test]
1298    fn test_const_transform_buffer_sync_basic() {
1299        let buffer = ConstTransformBufferSync::<f32, 10>::new();
1300
1301        let transform = StampedTransform {
1302            transform: Transform3D::default(),
1303            stamp: CuTime::from_nanos(1000),
1304            parent_frame: FrameIdString::from("world").unwrap(),
1305            child_frame: FrameIdString::from("robot").unwrap(),
1306        };
1307
1308        buffer.add_transform(transform);
1309
1310        let latest = buffer.get_latest_transform();
1311        assert!(latest.is_some());
1312        assert_eq!(latest.unwrap().stamp.as_nanos(), 1000);
1313    }
1314
1315    #[test]
1316    fn test_const_transform_buffer_sync_velocity() {
1317        let buffer = ConstTransformBufferSync::<f32, 10>::new();
1318
1319        // Add transforms with known positions to compute velocity
1320        let mut transform1 = StampedTransform {
1321            transform: Transform3D::<f32>::default(),
1322            stamp: CuTime::from_nanos(1_000_000_000), // 1 second in nanoseconds
1323            parent_frame: FrameIdString::from("world").unwrap(),
1324            child_frame: FrameIdString::from("robot").unwrap(),
1325        };
1326
1327        let mut transform2 = StampedTransform {
1328            transform: Transform3D::<f32>::default(),
1329            stamp: CuTime::from_nanos(2_000_000_000), // 2 seconds in nanoseconds
1330            parent_frame: FrameIdString::from("world").unwrap(),
1331            child_frame: FrameIdString::from("robot").unwrap(),
1332        };
1333
1334        // Set positions - robot moves 3 meters in x over 1 second
1335        // Using column-major format (each inner array is a column)
1336        transform1.transform = Transform3D::from_matrix([
1337            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1338            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1339            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1340            [0.0, 0.0, 0.0, 1.0], // Column 3: translation (x=0, y=0, z=0)
1341        ]);
1342
1343        transform2.transform = Transform3D::from_matrix([
1344            [1.0, 0.0, 0.0, 0.0], // Column 0: x-axis
1345            [0.0, 1.0, 0.0, 0.0], // Column 1: y-axis
1346            [0.0, 0.0, 1.0, 0.0], // Column 2: z-axis
1347            [3.0, 0.0, 0.0, 1.0], // Column 3: translation (x=3, y=0, z=0)
1348        ]);
1349
1350        buffer.add_transform(transform1);
1351        buffer.add_transform(transform2);
1352
1353        // Compute velocity at time between transforms
1354        let velocity = buffer.compute_velocity_at_time(CuTime::from_nanos(1_500_000_000)); // 1.5 seconds in nanoseconds
1355        assert!(velocity.is_some());
1356
1357        let vel = velocity.unwrap();
1358        // Velocity should be 3 m/s in x direction
1359        assert_approx_eq(vel.linear[0], 3.0, 1e-5);
1360        assert_approx_eq(vel.linear[1], 0.0, 1e-5);
1361        assert_approx_eq(vel.linear[2], 0.0, 1e-5);
1362    }
1363
1364    #[test]
1365    fn test_const_transform_store() {
1366        let store = ConstTransformStore::<f32, 10>::new();
1367
1368        let transform = StampedTransform {
1369            transform: Transform3D::default(),
1370            stamp: CuTime::from_nanos(1000),
1371            parent_frame: FrameIdString::from("world").unwrap(),
1372            child_frame: FrameIdString::from("robot").unwrap(),
1373        };
1374
1375        store.add_transform(transform.clone());
1376
1377        let buffer = store.get_buffer("world", "robot").unwrap();
1378        let closest = buffer.get_closest_transform(CuTime::from_nanos(1000));
1379        assert!(closest.is_some());
1380
1381        // Non-existent buffer
1382        let missing = store.get_buffer("world", "camera");
1383        assert!(missing.is_none());
1384
1385        // Get or create should create a new buffer
1386        let _ = store.get_or_create_buffer("world", "camera");
1387        assert!(store.get_buffer("world", "camera").is_some());
1388    }
1389}
cu_transform/transform.rs

cu_transform/
transform.rs
