box3d_rust/math_functions/
vector.rs

1// Vec3 arithmetic, length/normalize, and closely related helpers.
2// Part of the math_functions module.
3
4use super::*;
5
6/// Vector addition.
7pub fn add(a: Vec3, b: Vec3) -> Vec3 {
8    Vec3 {
9        x: a.x + b.x,
10        y: a.y + b.y,
11        z: a.z + b.z,
12    }
13}
14
15/// Vector subtraction.
16pub fn sub(a: Vec3, b: Vec3) -> Vec3 {
17    Vec3 {
18        x: a.x - b.x,
19        y: a.y - b.y,
20        z: a.z - b.z,
21    }
22}
23
24/// Vector component-wise multiplication.
25pub fn mul(a: Vec3, b: Vec3) -> Vec3 {
26    Vec3 {
27        x: a.x * b.x,
28        y: a.y * b.y,
29        z: a.z * b.z,
30    }
31}
32
33/// Vector negation.
34pub fn neg(a: Vec3) -> Vec3 {
35    Vec3 {
36        x: -a.x,
37        y: -a.y,
38        z: -a.z,
39    }
40}
41
42/// Vector dot product.
43pub fn dot(a: Vec3, b: Vec3) -> f32 {
44    a.x * b.x + a.y * b.y + a.z * b.z
45}
46
47/// Vector length.
48pub fn length(v: Vec3) -> f32 {
49    dot(v, v).sqrt()
50}
51
52/// Vector length squared.
53pub fn length_squared(a: Vec3) -> f32 {
54    a.x * a.x + a.y * a.y + a.z * a.z
55}
56
57/// Distance between two points.
58pub fn distance(a: Vec3, b: Vec3) -> f32 {
59    let dv = Vec3 {
60        x: b.x - a.x,
61        y: b.y - a.y,
62        z: b.z - a.z,
63    };
64    length(dv)
65}
66
67/// Squared distance between two points.
68pub fn distance_squared(a: Vec3, b: Vec3) -> f32 {
69    let dv = Vec3 {
70        x: b.x - a.x,
71        y: b.y - a.y,
72        z: b.z - a.z,
73    };
74    dv.x * dv.x + dv.y * dv.y + dv.z * dv.z
75}
76
77/// Normalize a vector. Returns a zero vector if the input vector is very small.
78pub fn normalize(a: Vec3) -> Vec3 {
79    let length_squared = a.x * a.x + a.y * a.y + a.z * a.z;
80
81    if length_squared > 1000.0 * f32::MIN_POSITIVE {
82        let s = 1.0 / length_squared.sqrt();
83        Vec3 {
84            x: s * a.x,
85            y: s * a.y,
86            z: s * a.z,
87        }
88    } else {
89        Vec3 {
90            x: 0.0,
91            y: 0.0,
92            z: 0.0,
93        }
94    }
95}
96
97/// Normalize a vector and return the length. Returns a zero vector
98/// if the input is very small.
99pub fn get_length_and_normalize(length_out: &mut f32, a: Vec3) -> Vec3 {
100    *length_out = length(a);
101    if *length_out < f32::EPSILON {
102        return VEC3_ZERO;
103    }
104
105    let inv_length = 1.0 / *length_out;
106    Vec3 {
107        x: inv_length * a.x,
108        y: inv_length * a.y,
109        z: inv_length * a.z,
110    }
111}
112
113/// Get a unit vector that is perpendicular to the supplied vector.
114pub fn perp(a: Vec3) -> Vec3 {
115    // Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
116    // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component
117    // of a unit vector must be greater or equal to 0.57735.
118    let p = if a.x < -0.5 || 0.5 < a.x {
119        Vec3 {
120            x: a.y,
121            y: -a.x,
122            z: 0.0,
123        }
124    } else {
125        Vec3 {
126            x: 0.0,
127            y: a.z,
128            z: -a.y,
129        }
130    };
131
132    normalize(p)
133}
134
135/// Is a vector normalized? In other words, does it have unit length?
136pub fn is_normalized(a: Vec3) -> bool {
137    let aa = dot(a, a);
138    abs_float(1.0 - aa) < 100.0 * f32::EPSILON
139}
140
141/// a + s * b
142pub fn mul_add(a: Vec3, s: f32, b: Vec3) -> Vec3 {
143    Vec3 {
144        x: a.x + s * b.x,
145        y: a.y + s * b.y,
146        z: a.z + s * b.z,
147    }
148}
149
150/// a - s * b
151pub fn mul_sub(a: Vec3, s: f32, b: Vec3) -> Vec3 {
152    Vec3 {
153        x: a.x - s * b.x,
154        y: a.y - s * b.y,
155        z: a.z - s * b.z,
156    }
157}
158
159/// s * a
160pub fn mul_sv(s: f32, a: Vec3) -> Vec3 {
161    Vec3 {
162        x: s * a.x,
163        y: s * a.y,
164        z: s * a.z,
165    }
166}
167
168/// <https://en.wikipedia.org/wiki/Cross_product>
169pub fn cross(a: Vec3, b: Vec3) -> Vec3 {
170    Vec3 {
171        x: a.y * b.z - a.z * b.y,
172        y: a.z * b.x - a.x * b.z,
173        z: a.x * b.y - a.y * b.x,
174    }
175}
176
177/// Linearly interpolate between two vectors.
178pub fn lerp(a: Vec3, b: Vec3, alpha: f32) -> Vec3 {
179    debug_assert!((0.0..=1.0).contains(&alpha));
180
181    Vec3 {
182        x: (1.0 - alpha) * a.x + alpha * b.x,
183        y: (1.0 - alpha) * a.y + alpha * b.y,
184        z: (1.0 - alpha) * a.z + alpha * b.z,
185    }
186}
187
188/// Blend two vectors: s * a + t * b
189pub fn blend2(s: f32, a: Vec3, t: f32, b: Vec3) -> Vec3 {
190    Vec3 {
191        x: s * a.x + t * b.x,
192        y: s * a.y + t * b.y,
193        z: s * a.z + t * b.z,
194    }
195}
196
197/// Blend three vectors: s * a + t * b + u * c. (math_internal.h: b3Blend3)
198pub fn blend3(s: f32, a: Vec3, t: f32, b: Vec3, u: f32, c: Vec3) -> Vec3 {
199    Vec3 {
200        x: s * a.x + t * b.x + u * c.x,
201        y: s * a.y + t * b.y + u * c.y,
202        z: s * a.z + t * b.z + u * c.z,
203    }
204}
205
206/// Component-wise absolute value.
207pub fn abs(a: Vec3) -> Vec3 {
208    Vec3 {
209        x: abs_float(a.x),
210        y: abs_float(a.y),
211        z: abs_float(a.z),
212    }
213}
214
215/// Component-wise -1 or 1 (1 if zero).
216pub fn sign(a: Vec3) -> Vec3 {
217    Vec3 {
218        x: if a.x >= 0.0 { 1.0 } else { -1.0 },
219        y: if a.y >= 0.0 { 1.0 } else { -1.0 },
220        z: if a.z >= 0.0 { 1.0 } else { -1.0 },
221    }
222}
223
224/// Component-wise minimum value.
225pub fn min(a: Vec3, b: Vec3) -> Vec3 {
226    Vec3 {
227        x: min_float(a.x, b.x),
228        y: min_float(a.y, b.y),
229        z: min_float(a.z, b.z),
230    }
231}
232
233/// Component-wise maximum value.
234pub fn max(a: Vec3, b: Vec3) -> Vec3 {
235    Vec3 {
236        x: max_float(a.x, b.x),
237        y: max_float(a.y, b.y),
238        z: max_float(a.z, b.z),
239    }
240}
241
242/// Component-wise clamped value.
243pub fn clamp(a: Vec3, lower: Vec3, upper: Vec3) -> Vec3 {
244    Vec3 {
245        x: clamp_float(a.x, lower.x, upper.x),
246        y: clamp_float(a.y, lower.y, upper.y),
247        z: clamp_float(a.z, lower.z, upper.z),
248    }
249}
250
251/// Create a safe scaling value for scaling collision. This allows
252/// negative scale, but keeps scale sufficiently far from zero.
253pub fn safe_scale(a: Vec3) -> Vec3 {
254    let abs_scale = abs(a);
255    let min_scale = Vec3 {
256        x: MIN_SCALE,
257        y: MIN_SCALE,
258        z: MIN_SCALE,
259    };
260    mul(sign(a), max(abs_scale, min_scale))
261}
box3d_rust/math_functions/vector.rs

box3d_rust/math_functions/
vector.rs
