box3d_rust/math_functions/
transform.rs

1// Rigid transforms and world-position (large-world) operations.
2// Part of the math_functions module.
3
4use super::*;
5
6#[cfg(feature = "double-precision")]
7type PosScalar = f64;
8#[cfg(not(feature = "double-precision"))]
9type PosScalar = f32;
10
11/// Multiply two transforms. If the result is applied to a point p local to frame B,
12/// the transform would first convert p to a point local to frame A, then into a point
13/// in the world frame. This is useful if frame B is a child of frame A.
14pub fn mul_transforms(a: Transform, b: Transform) -> Transform {
15    Transform {
16        p: add(rotate_vector(a.q, b.p), a.p),
17        q: mul_quat(a.q, b.q),
18    }
19}
20
21/// Creates a transform that converts a local point in frame B to a local point in frame A.
22/// This is useful for transforming points between the local spaces of two frames that are
23/// in world space.
24pub fn inv_mul_transforms(a: Transform, b: Transform) -> Transform {
25    Transform {
26        p: inv_rotate_vector(a.q, sub(b.p, a.p)),
27        q: inv_mul_quat(a.q, b.q),
28    }
29}
30
31/// Get the inverse of a transform.
32pub fn invert_transform(t: Transform) -> Transform {
33    Transform {
34        p: inv_rotate_vector(t.q, neg(t.p)),
35        q: conjugate(t.q),
36    }
37}
38
39/// Transform a point.
40pub fn transform_point(t: Transform, v: Vec3) -> Vec3 {
41    let rv = rotate_vector(t.q, v);
42    add(rv, t.p)
43}
44
45/// Inverse transform a point.
46pub fn inv_transform_point(t: Transform, v: Vec3) -> Vec3 {
47    inv_rotate_vector(t.q, sub(v, t.p))
48}
49
50// World position boundary. These cross between the double precision world space at the public
51// boundary and the float interior. One set of bodies serves both modes: the typedefs collapse
52// the types in float mode and the explicit float casts become no-ops.
53
54/// Convert a vector to a world position.
55pub fn to_pos(v: Vec3) -> Pos {
56    Pos {
57        x: v.x as PosScalar,
58        y: v.y as PosScalar,
59        z: v.z as PosScalar,
60    }
61}
62
63/// Lossy conversion of a world position to a float vector.
64pub fn to_vec3(p: Pos) -> Vec3 {
65    Vec3 {
66        x: p.x as f32,
67        y: p.y as f32,
68        z: p.z as f32,
69    }
70}
71
72/// Narrow a world coordinate to float, rounding toward negative infinity. Use with
73/// [`round_up_float`] to build a conservative float box that always contains the double bounds,
74/// where plain rounding far from the origin could clip. nextafterf is an exact IEEE operation,
75/// so this is cross-platform deterministic. With large world mode off this is a plain conversion.
76#[cfg(feature = "double-precision")]
77pub fn round_down_float(x: f64) -> f32 {
78    let f = x as f32;
79    if f as f64 > x {
80        next_after_f32(f, f32::MIN)
81    } else {
82        f
83    }
84}
85
86/// Narrow a world coordinate to float, rounding toward positive infinity.
87#[cfg(feature = "double-precision")]
88pub fn round_up_float(x: f64) -> f32 {
89    let f = x as f32;
90    if (f as f64) < x {
91        next_after_f32(f, f32::MAX)
92    } else {
93        f
94    }
95}
96
97#[cfg(not(feature = "double-precision"))]
98pub fn round_down_float(x: f64) -> f32 {
99    x as f32
100}
101
102#[cfg(not(feature = "double-precision"))]
103pub fn round_up_float(x: f64) -> f32 {
104    x as f32
105}
106
107/// C nextafterf: the next representable f32 after `from` in the direction of `to`.
108#[cfg(feature = "double-precision")]
109fn next_after_f32(from: f32, to: f32) -> f32 {
110    if from.is_nan() || to.is_nan() {
111        return f32::NAN;
112    }
113    if from == to {
114        return to;
115    }
116    if from == 0.0 {
117        return if to > 0.0 {
118            f32::from_bits(1)
119        } else {
120            -f32::from_bits(1)
121        };
122    }
123    let bits = from.to_bits();
124    let next = if (from < to) == (from > 0.0) {
125        bits + 1
126    } else {
127        bits - 1
128    };
129    f32::from_bits(next)
130}
131
132/// a - b, demoted to float. The primary precision boundary operation.
133pub fn sub_pos(a: Pos, b: Pos) -> Vec3 {
134    Vec3 {
135        x: (a.x - b.x) as f32,
136        y: (a.y - b.y) as f32,
137        z: (a.z - b.z) as f32,
138    }
139}
140
141/// p + d
142pub fn offset_pos(p: Pos, d: Vec3) -> Pos {
143    Pos {
144        x: p.x + d.x as PosScalar,
145        y: p.y + d.y as PosScalar,
146        z: p.z + d.z as PosScalar,
147    }
148}
149
150/// World position interpolation for sweeps and sampling.
151pub fn lerp_position(a: Pos, b: Pos, t: f32) -> Pos {
152    Pos {
153        x: (1.0 - t) as PosScalar * a.x + t as PosScalar * b.x,
154        y: (1.0 - t) as PosScalar * a.y + t as PosScalar * b.y,
155        z: (1.0 - t) as PosScalar * a.z + t as PosScalar * b.z,
156    }
157}
158
159/// Transform a local point to a world position. Rotation in float, translation in double.
160pub fn transform_world_point(t: WorldTransform, p: Vec3) -> Pos {
161    let r = rotate_vector(t.q, p);
162    Pos {
163        x: t.p.x + r.x as PosScalar,
164        y: t.p.y + r.y as PosScalar,
165        z: t.p.z + r.z as PosScalar,
166    }
167}
168
169/// Transform a world position to a local point. One double subtraction, then float.
170pub fn inv_transform_world_point(t: WorldTransform, p: Pos) -> Vec3 {
171    let d = Vec3 {
172        x: (p.x - t.p.x) as f32,
173        y: (p.y - t.p.y) as f32,
174        z: (p.z - t.p.z) as f32,
175    };
176    inv_rotate_vector(t.q, d)
177}
178
179/// Relative transform of frame B in frame A. The narrow phase boundary.
180pub fn inv_mul_world_transforms(a: WorldTransform, b: WorldTransform) -> Transform {
181    let d = Vec3 {
182        x: (b.p.x - a.p.x) as f32,
183        y: (b.p.y - a.p.y) as f32,
184        z: (b.p.z - a.p.z) as f32,
185    };
186    Transform {
187        q: inv_mul_quat(a.q, b.q),
188        p: inv_rotate_vector(a.q, d),
189    }
190}
191
192/// Compose a world transform with a local transform.
193pub fn mul_world_transforms(a: WorldTransform, b: Transform) -> WorldTransform {
194    let r = rotate_vector(a.q, b.p);
195    WorldTransform {
196        q: mul_quat(a.q, b.q),
197        p: Pos {
198            x: a.p.x + r.x as PosScalar,
199            y: a.p.y + r.y as PosScalar,
200            z: a.p.z + r.z as PosScalar,
201        },
202    }
203}
204
205/// Shift a world transform into the frame of a base position.
206pub fn to_relative_transform(t: WorldTransform, base: Pos) -> Transform {
207    Transform {
208        q: t.q,
209        p: Vec3 {
210            x: (t.p.x - base.x) as f32,
211            y: (t.p.y - base.y) as f32,
212            z: (t.p.z - base.z) as f32,
213        },
214    }
215}
216
217/// Promote a float transform to a world transform. Lossless.
218pub fn make_world_transform(t: Transform) -> WorldTransform {
219    WorldTransform {
220        p: to_pos(t.p),
221        q: t.q,
222    }
223}
224
225/// Translate a local AABB by a world origin, rounding outward so the float box always contains
226/// the double box. Far from the origin a plain conversion could clip a shape out of its own box.
227/// In float mode the origin is float and the rounding is a no-op.
228pub fn offset_aabb(local_box: Aabb, origin: Pos) -> Aabb {
229    Aabb {
230        lower_bound: Vec3 {
231            x: round_down_float(origin.x as f64 + local_box.lower_bound.x as f64),
232            y: round_down_float(origin.y as f64 + local_box.lower_bound.y as f64),
233            z: round_down_float(origin.z as f64 + local_box.lower_bound.z as f64),
234        },
235        upper_bound: Vec3 {
236            x: round_up_float(origin.x as f64 + local_box.upper_bound.x as f64),
237            y: round_up_float(origin.y as f64 + local_box.upper_bound.y as f64),
238            z: round_up_float(origin.z as f64 + local_box.upper_bound.z as f64),
239        },
240    }
241}
box3d_rust/math_functions/transform.rs

box3d_rust/math_functions/
transform.rs
