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

cu_transform/
transform.rs
