spatialrust_transform/
ops.rs

1//! Cloud transform and utility operations.
2
3use spatialrust_core::{
4    FieldSemantic, HasNormals3, HasPositions3, PointBuffer, PointBufferSet, PointCloud,
5    SpatialError, SpatialResult,
6};
7use spatialrust_math::{symmetric_eigen3, Mat3, Mat4, Vec3};
8
9use crate::bounds::{Aabb, Obb};
10
11/// Applies a 4×4 affine transform to a cloud's positions (and normals, if
12/// present — normals are rotated by the linear part and renormalized).
13pub fn apply_transform(input: &PointCloud, transform: Mat4<f32>) -> SpatialResult<PointCloud> {
14    if input.is_empty() {
15        return Ok(input.clone());
16    }
17    let (x, y, z) = input.positions3()?;
18    let len = input.len();
19
20    let mut tx = Vec::with_capacity(len);
21    let mut ty = Vec::with_capacity(len);
22    let mut tz = Vec::with_capacity(len);
23    for i in 0..len {
24        let p = transform.transform_point(Vec3::new(x[i], y[i], z[i]));
25        tx.push(p.x);
26        ty.push(p.y);
27        tz.push(p.z);
28    }
29
30    let has_normals = input.schema().find_semantic(FieldSemantic::NormalX).is_some();
31    let normals = if has_normals {
32        let (nx, ny, nz) = input.normals3()?;
33        let (mut rx, mut ry, mut rz) = (Vec::new(), Vec::new(), Vec::new());
34        for i in 0..len {
35            let n = transform.transform_vector(Vec3::new(nx[i], ny[i], nz[i])).normalize();
36            rx.push(n.x);
37            ry.push(n.y);
38            rz.push(n.z);
39        }
40        Some((rx, ry, rz))
41    } else {
42        None
43    };
44
45    let mut buffers = PointBufferSet::new();
46    for field in input.schema().fields() {
47        let buffer = match field.semantic {
48            FieldSemantic::PositionX => PointBuffer::from_f32(tx.clone()),
49            FieldSemantic::PositionY => PointBuffer::from_f32(ty.clone()),
50            FieldSemantic::PositionZ => PointBuffer::from_f32(tz.clone()),
51            FieldSemantic::NormalX => PointBuffer::from_f32(normals.as_ref().unwrap().0.clone()),
52            FieldSemantic::NormalY => PointBuffer::from_f32(normals.as_ref().unwrap().1.clone()),
53            FieldSemantic::NormalZ => PointBuffer::from_f32(normals.as_ref().unwrap().2.clone()),
54            _ => clone_buffer(input.field(&field.name)?),
55        };
56        buffers.insert(field.name.clone(), buffer);
57    }
58    PointCloud::try_from_parts(input.schema().clone(), buffers, input.metadata().clone())
59}
60
61/// Centroid (mean position) of a cloud.
62pub fn centroid(input: &PointCloud) -> SpatialResult<Vec3<f32>> {
63    if input.is_empty() {
64        return Err(SpatialError::InvalidArgument(
65            "cannot take centroid of empty cloud".to_owned(),
66        ));
67    }
68    let (x, y, z) = input.positions3()?;
69    let n = input.len() as f64;
70    let (mut sx, mut sy, mut sz) = (0.0_f64, 0.0_f64, 0.0_f64);
71    for i in 0..input.len() {
72        sx += f64::from(x[i]);
73        sy += f64::from(y[i]);
74        sz += f64::from(z[i]);
75    }
76    Ok(Vec3::new((sx / n) as f32, (sy / n) as f32, (sz / n) as f32))
77}
78
79/// Axis-aligned bounding box of a cloud.
80pub fn bounding_box(input: &PointCloud) -> SpatialResult<Aabb> {
81    if input.is_empty() {
82        return Err(SpatialError::InvalidArgument("cannot bound an empty cloud".to_owned()));
83    }
84    let (x, y, z) = input.positions3()?;
85    let mut min = Vec3::new(f32::INFINITY, f32::INFINITY, f32::INFINITY);
86    let mut max = Vec3::new(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY);
87    for i in 0..input.len() {
88        min.x = min.x.min(x[i]);
89        min.y = min.y.min(y[i]);
90        min.z = min.z.min(z[i]);
91        max.x = max.x.max(x[i]);
92        max.y = max.y.max(y[i]);
93        max.z = max.z.max(z[i]);
94    }
95    Ok(Aabb::new(min, max))
96}
97
98/// Translates a cloud so its centroid sits at the origin.
99pub fn recenter(input: &PointCloud) -> SpatialResult<PointCloud> {
100    let c = centroid(input)?;
101    apply_transform(input, translation(Vec3::new(-c.x, -c.y, -c.z)))
102}
103
104/// Uniformly scales a cloud about the origin by `factor`.
105pub fn scale_cloud(input: &PointCloud, factor: f32) -> SpatialResult<PointCloud> {
106    if !(factor.is_finite()) || factor == 0.0 {
107        return Err(SpatialError::InvalidArgument(
108            "scale factor must be finite and non-zero".to_owned(),
109        ));
110    }
111    let m = Mat4::from_rows(
112        [factor, 0.0, 0.0, 0.0],
113        [0.0, factor, 0.0, 0.0],
114        [0.0, 0.0, factor, 0.0],
115        [0.0, 0.0, 0.0, 1.0],
116    );
117    apply_transform(input, m)
118}
119
120/// Recenters a cloud and scales it so its farthest point is at unit distance —
121/// the canonical normalization for learned point-cloud models.
122pub fn normalize_unit_sphere(input: &PointCloud) -> SpatialResult<PointCloud> {
123    let centered = recenter(input)?;
124    let (x, y, z) = centered.positions3()?;
125    let mut max_r = 0.0_f32;
126    for i in 0..centered.len() {
127        let r = (x[i] * x[i] + y[i] * y[i] + z[i] * z[i]).sqrt();
128        if r > max_r {
129            max_r = r;
130        }
131    }
132    if max_r <= f32::EPSILON {
133        return Ok(centered);
134    }
135    scale_cloud(&centered, 1.0 / max_r)
136}
137
138/// Concatenates clouds that share an identical schema into one cloud.
139pub fn merge_clouds(clouds: &[&PointCloud]) -> SpatialResult<PointCloud> {
140    let first = clouds.first().ok_or_else(|| {
141        SpatialError::InvalidArgument("merge needs at least one cloud".to_owned())
142    })?;
143    let schema = first.schema();
144    for cloud in &clouds[1..] {
145        if cloud.schema() != schema {
146            return Err(SpatialError::InvalidArgument(
147                "all clouds must share the same schema to merge".to_owned(),
148            ));
149        }
150    }
151
152    let mut buffers = PointBufferSet::new();
153    for field in schema.fields() {
154        let sources: Vec<&PointBuffer> =
155            clouds.iter().map(|c| c.field(&field.name)).collect::<SpatialResult<_>>()?;
156        buffers.insert(field.name.clone(), concat_buffers(&sources));
157    }
158    PointCloud::try_from_parts(schema.clone(), buffers, first.metadata().clone())
159}
160
161/// Oriented bounding box via principal component analysis of the positions.
162pub fn oriented_bounding_box(input: &PointCloud) -> SpatialResult<Obb> {
163    let c = centroid(input)?;
164    let (x, y, z) = input.positions3()?;
165
166    let mut cov = [[0.0_f64; 3]; 3];
167    for i in 0..input.len() {
168        let d = [f64::from(x[i] - c.x), f64::from(y[i] - c.y), f64::from(z[i] - c.z)];
169        for r in 0..3 {
170            for col in 0..3 {
171                cov[r][col] += d[r] * d[col];
172            }
173        }
174    }
175    let eigen = symmetric_eigen3(Mat3::<f64>::from_rows(cov[0], cov[1], cov[2]));
176    let axis = |col: usize| {
177        Vec3::new(
178            eigen.eigenvectors.m[0][col] as f32,
179            eigen.eigenvectors.m[1][col] as f32,
180            eigen.eigenvectors.m[2][col] as f32,
181        )
182        .normalize()
183    };
184    // Eigenvalues are ascending, so column 2 is the principal (largest-variance)
185    // axis. Order the box axes principal-first.
186    let (a0, a1, a2) = (axis(2), axis(1), axis(0));
187
188    // Project each point onto the axes to find the extents.
189    let mut min = [f32::INFINITY; 3];
190    let mut max = [f32::NEG_INFINITY; 3];
191    for i in 0..input.len() {
192        let d = Vec3::new(x[i] - c.x, y[i] - c.y, z[i] - c.z);
193        for (k, ax) in [a0, a1, a2].iter().enumerate() {
194            let p = d.dot(*ax);
195            min[k] = min[k].min(p);
196            max[k] = max[k].max(p);
197        }
198    }
199
200    let axes = Mat3::from_rows([a0.x, a1.x, a2.x], [a0.y, a1.y, a2.y], [a0.z, a1.z, a2.z]);
201    let mid = |k: usize| 0.5 * (min[k] + max[k]);
202    let center = c + scale(a0, mid(0)) + scale(a1, mid(1)) + scale(a2, mid(2));
203    let half = Vec3::new(0.5 * (max[0] - min[0]), 0.5 * (max[1] - min[1]), 0.5 * (max[2] - min[2]));
204    Ok(Obb { center, axes, half_extents: half })
205}
206
207fn translation(t: Vec3<f32>) -> Mat4<f32> {
208    Mat4::from_rows(
209        [1.0, 0.0, 0.0, t.x],
210        [0.0, 1.0, 0.0, t.y],
211        [0.0, 0.0, 1.0, t.z],
212        [0.0, 0.0, 0.0, 1.0],
213    )
214}
215
216fn scale(v: Vec3<f32>, s: f32) -> Vec3<f32> {
217    Vec3::new(v.x * s, v.y * s, v.z * s)
218}
219
220fn clone_buffer(buffer: &PointBuffer) -> PointBuffer {
221    match buffer {
222        PointBuffer::F32(v) => PointBuffer::from_f32(v.clone()),
223        PointBuffer::F64(v) => PointBuffer::F64(v.clone()),
224        PointBuffer::U8(v) => PointBuffer::U8(v.clone()),
225        PointBuffer::U16(v) => PointBuffer::U16(v.clone()),
226        PointBuffer::U32(v) => PointBuffer::U32(v.clone()),
227        PointBuffer::I32(v) => PointBuffer::I32(v.clone()),
228    }
229}
230
231fn concat_buffers(sources: &[&PointBuffer]) -> PointBuffer {
232    match sources[0] {
233        PointBuffer::F32(_) => {
234            let mut out = Vec::new();
235            for s in sources {
236                if let PointBuffer::F32(v) = s {
237                    out.extend_from_slice(v);
238                }
239            }
240            PointBuffer::from_f32(out)
241        }
242        PointBuffer::F64(_) => {
243            let mut out = Vec::new();
244            for s in sources {
245                if let PointBuffer::F64(v) = s {
246                    out.extend_from_slice(v);
247                }
248            }
249            PointBuffer::F64(out)
250        }
251        PointBuffer::U8(_) => concat_into(sources, |b| match b {
252            PointBuffer::U8(v) => Some(v.as_slice()),
253            _ => None,
254        })
255        .map(PointBuffer::U8)
256        .unwrap(),
257        PointBuffer::U16(_) => concat_into(sources, |b| match b {
258            PointBuffer::U16(v) => Some(v.as_slice()),
259            _ => None,
260        })
261        .map(PointBuffer::U16)
262        .unwrap(),
263        PointBuffer::U32(_) => concat_into(sources, |b| match b {
264            PointBuffer::U32(v) => Some(v.as_slice()),
265            _ => None,
266        })
267        .map(PointBuffer::U32)
268        .unwrap(),
269        PointBuffer::I32(_) => concat_into(sources, |b| match b {
270            PointBuffer::I32(v) => Some(v.as_slice()),
271            _ => None,
272        })
273        .map(PointBuffer::I32)
274        .unwrap(),
275    }
276}
277
278fn concat_into<T: Clone>(
279    sources: &[&PointBuffer],
280    extract: impl Fn(&PointBuffer) -> Option<&[T]>,
281) -> Option<Vec<T>> {
282    let mut out = Vec::new();
283    for s in sources {
284        out.extend_from_slice(extract(s)?);
285    }
286    Some(out)
287}
288
289#[cfg(test)]
290mod tests {
291    use super::*;
292    use spatialrust_core::{PointCloudBuilder, StandardSchemas};
293
294    fn cloud(points: &[[f32; 3]]) -> PointCloud {
295        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
296        for p in points {
297            builder.push_point(*p).unwrap();
298        }
299        builder.build().unwrap()
300    }
301
302    #[test]
303    fn apply_transform_translates() {
304        let c = cloud(&[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]);
305        let out = apply_transform(&c, translation(Vec3::new(2.0, 3.0, 4.0))).unwrap();
306        let (x, y, z) = out.positions3().unwrap();
307        assert!(
308            (x[0] - 2.0).abs() < 1e-6 && (y[0] - 3.0).abs() < 1e-6 && (z[0] - 4.0).abs() < 1e-6
309        );
310    }
311
312    #[test]
313    fn recenter_moves_centroid_to_origin() {
314        let c = cloud(&[[1.0, 1.0, 1.0], [3.0, 3.0, 3.0]]);
315        let out = recenter(&c).unwrap();
316        let centroid = centroid(&out).unwrap();
317        assert!(centroid.x.abs() < 1e-6 && centroid.y.abs() < 1e-6 && centroid.z.abs() < 1e-6);
318    }
319
320    #[test]
321    fn normalize_unit_sphere_bounds_radius() {
322        let c = cloud(&[[10.0, 0.0, 0.0], [12.0, 0.0, 0.0], [11.0, 5.0, 0.0]]);
323        let out = normalize_unit_sphere(&c).unwrap();
324        let (x, y, z) = out.positions3().unwrap();
325        let max_r = (0..out.len())
326            .map(|i| (x[i] * x[i] + y[i] * y[i] + z[i] * z[i]).sqrt())
327            .fold(0.0_f32, f32::max);
328        assert!((max_r - 1.0).abs() < 1e-5, "max radius {max_r}");
329    }
330
331    #[test]
332    fn bounding_box_spans_extent() {
333        let c = cloud(&[[0.0, -1.0, 2.0], [4.0, 3.0, 2.0]]);
334        let aabb = bounding_box(&c).unwrap();
335        assert_eq!(aabb.min, Vec3::new(0.0, -1.0, 2.0));
336        assert_eq!(aabb.max, Vec3::new(4.0, 3.0, 2.0));
337        assert_eq!(aabb.extent(), Vec3::new(4.0, 4.0, 0.0));
338    }
339
340    #[test]
341    fn merge_concatenates() {
342        let a = cloud(&[[0.0, 0.0, 0.0]]);
343        let b = cloud(&[[1.0, 1.0, 1.0], [2.0, 2.0, 2.0]]);
344        let merged = merge_clouds(&[&a, &b]).unwrap();
345        assert_eq!(merged.len(), 3);
346    }
347
348    #[test]
349    fn obb_aligns_with_elongated_axis() {
350        // Points spread mainly along x; the first OBB axis should be ~x and the
351        // first half-extent the largest.
352        let mut pts = Vec::new();
353        for i in 0..20 {
354            pts.push([i as f32, 0.05 * (i % 2) as f32, 0.0]);
355        }
356        let c = cloud(&pts);
357        let obb = oriented_bounding_box(&c).unwrap();
358        let axis0 = Vec3::new(obb.axes.m[0][0], obb.axes.m[1][0], obb.axes.m[2][0]);
359        assert!(axis0.x.abs() > 0.99, "principal axis not along x: {axis0:?}");
360        assert!(obb.half_extents.x > obb.half_extents.y);
361        assert!(obb.half_extents.x > obb.half_extents.z);
362    }
363}
spatialrust_transform/ops.rs

spatialrust_transform/
ops.rs
