1use crate::math::{FusedMultiplyAdd, FusedMultiplyNegAdd};
30use crate::mlaf::{mlaf, neg_mlaf};
31use crate::{Vector3f, Vector4f};
32use std::ops::{Add, Mul, Sub};
33
34impl FusedMultiplyAdd<f32> for f32 {
35 #[inline(always)]
36 fn mla(&self, b: f32, c: f32) -> f32 {
37 mlaf(*self, b, c)
38 }
39}
40
41impl FusedMultiplyNegAdd<f32> for f32 {
42 #[inline(always)]
43 fn neg_mla(&self, b: f32, c: f32) -> f32 {
44 neg_mlaf(*self, b, c)
45 }
46}
47
48#[inline(always)]
49pub(crate) fn lerp<
50 T: Mul<Output = T>
51 + Sub<Output = T>
52 + Add<Output = T>
53 + From<f32>
54 + Copy
55 + FusedMultiplyAdd<T>
56 + FusedMultiplyNegAdd<T>,
57>(
58 a: T,
59 b: T,
60 t: T,
61) -> T {
62 a.neg_mla(a, t).mla(b, t)
63}
64
65pub struct Hypercube<'a> {
69 array: &'a [f32],
70 x_stride: u32,
71 y_stride: u32,
72 z_stride: u32,
73 grid_size: [u8; 4],
74}
75
76trait Fetcher4<T> {
77 fn fetch(&self, x: i32, y: i32, z: i32, w: i32) -> T;
78}
79
80impl Hypercube<'_> {
81 pub fn new(array: &[f32], grid_size: usize) -> Hypercube<'_> {
82 let z_stride = grid_size as u32;
83 let y_stride = z_stride * z_stride;
84 let x_stride = z_stride * z_stride * z_stride;
85 Hypercube {
86 array,
87 x_stride,
88 y_stride,
89 z_stride,
90 grid_size: [
91 grid_size as u8,
92 grid_size as u8,
93 grid_size as u8,
94 grid_size as u8,
95 ],
96 }
97 }
98
99 pub fn new_hypercube(array: &[f32], grid_size: [u8; 4]) -> Hypercube<'_> {
100 let z_stride = grid_size[2] as u32;
101 let y_stride = z_stride * grid_size[1] as u32;
102 let x_stride = y_stride * grid_size[0] as u32;
103 Hypercube {
104 array,
105 x_stride,
106 y_stride,
107 z_stride,
108 grid_size,
109 }
110 }
111}
112
113struct Fetch4Vec3<'a> {
114 array: &'a [f32],
115 x_stride: u32,
116 y_stride: u32,
117 z_stride: u32,
118}
119
120struct Fetch4Vec4<'a> {
121 array: &'a [f32],
122 x_stride: u32,
123 y_stride: u32,
124 z_stride: u32,
125}
126
127impl Fetcher4<Vector3f> for Fetch4Vec3<'_> {
128 #[inline(always)]
129 fn fetch(&self, x: i32, y: i32, z: i32, w: i32) -> Vector3f {
130 let start = (x as u32 * self.x_stride
131 + y as u32 * self.y_stride
132 + z as u32 * self.z_stride
133 + w as u32) as usize
134 * 3;
135 let k = &self.array[start..start + 3];
136 Vector3f {
137 v: [k[0], k[1], k[2]],
138 }
139 }
140}
141
142impl Fetcher4<Vector4f> for Fetch4Vec4<'_> {
143 #[inline(always)]
144 fn fetch(&self, x: i32, y: i32, z: i32, w: i32) -> Vector4f {
145 let start = (x as u32 * self.x_stride
146 + y as u32 * self.y_stride
147 + z as u32 * self.z_stride
148 + w as u32) as usize
149 * 4;
150 let k = &self.array[start..start + 4];
151 Vector4f {
152 v: [k[0], k[1], k[2], k[3]],
153 }
154 }
155}
156
157impl Hypercube<'_> {
158 #[inline(always)]
159 fn quadlinear<
160 T: From<f32>
161 + Add<T, Output = T>
162 + Mul<T, Output = T>
163 + FusedMultiplyAdd<T>
164 + Sub<T, Output = T>
165 + Copy
166 + FusedMultiplyNegAdd<T>,
167 >(
168 &self,
169 lin_x: f32,
170 lin_y: f32,
171 lin_z: f32,
172 lin_w: f32,
173 r: impl Fetcher4<T>,
174 ) -> T {
175 let lin_x = lin_x.max(0.0).min(1.0);
176 let lin_y = lin_y.max(0.0).min(1.0);
177 let lin_z = lin_z.max(0.0).min(1.0);
178 let lin_w = lin_w.max(0.0).min(1.0);
179
180 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
181 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
182 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
183 let scale_w = (self.grid_size[3] as i32 - 1) as f32;
184
185 let x = (lin_x * scale_x).floor() as i32;
186 let y = (lin_y * scale_y).floor() as i32;
187 let z = (lin_z * scale_z).floor() as i32;
188 let w = (lin_w * scale_w).floor() as i32;
189
190 let x_n = (lin_x * scale_x).ceil() as i32;
191 let y_n = (lin_y * scale_y).ceil() as i32;
192 let z_n = (lin_z * scale_z).ceil() as i32;
193 let w_n = (lin_w * scale_w).ceil() as i32;
194
195 let x_d = T::from(lin_x * scale_x - x as f32);
196 let y_d = T::from(lin_y * scale_y - y as f32);
197 let z_d = T::from(lin_z * scale_z - z as f32);
198 let w_d = T::from(lin_w * scale_w - w as f32);
199
200 let r_x1 = lerp(r.fetch(x, y, z, w), r.fetch(x_n, y, z, w), x_d);
201 let r_x2 = lerp(r.fetch(x, y_n, z, w), r.fetch(x_n, y_n, z, w), x_d);
202 let r_y1 = lerp(r_x1, r_x2, y_d);
203 let r_x3 = lerp(r.fetch(x, y, z_n, w), r.fetch(x_n, y, z_n, w), x_d);
204 let r_x4 = lerp(r.fetch(x, y_n, z_n, w), r.fetch(x_n, y_n, z_n, w), x_d);
205 let r_y2 = lerp(r_x3, r_x4, y_d);
206 let r_z1 = lerp(r_y1, r_y2, z_d);
207
208 let r_x1 = lerp(r.fetch(x, y, z, w_n), r.fetch(x_n, y, z, w_n), x_d);
209 let r_x2 = lerp(r.fetch(x, y_n, z, w_n), r.fetch(x_n, y_n, z, w_n), x_d);
210 let r_y1 = lerp(r_x1, r_x2, y_d);
211 let r_x3 = lerp(r.fetch(x, y, z_n, w_n), r.fetch(x_n, y, z_n, w_n), x_d);
212 let r_x4 = lerp(r.fetch(x, y_n, z_n, w_n), r.fetch(x_n, y_n, z_n, w_n), x_d);
213 let r_y2 = lerp(r_x3, r_x4, y_d);
214 let r_z2 = lerp(r_y1, r_y2, z_d);
215 lerp(r_z1, r_z2, w_d)
216 }
217
218 #[inline]
219 pub fn quadlinear_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector3f {
220 self.quadlinear(
221 lin_x,
222 lin_y,
223 lin_z,
224 lin_w,
225 Fetch4Vec3 {
226 array: self.array,
227 x_stride: self.x_stride,
228 y_stride: self.y_stride,
229 z_stride: self.z_stride,
230 },
231 )
232 }
233
234 #[inline]
235 pub fn quadlinear_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector4f {
236 self.quadlinear(
237 lin_x,
238 lin_y,
239 lin_z,
240 lin_w,
241 Fetch4Vec4 {
242 array: self.array,
243 x_stride: self.x_stride,
244 y_stride: self.y_stride,
245 z_stride: self.z_stride,
246 },
247 )
248 }
249
250 #[cfg(feature = "options")]
251 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
252 #[inline(always)]
253 fn pyramid<
254 T: From<f32>
255 + Add<T, Output = T>
256 + Mul<T, Output = T>
257 + FusedMultiplyAdd<T>
258 + Sub<T, Output = T>
259 + Copy
260 + FusedMultiplyNegAdd<T>,
261 >(
262 &self,
263 lin_x: f32,
264 lin_y: f32,
265 lin_z: f32,
266 lin_w: f32,
267 r: impl Fetcher4<T>,
268 ) -> T {
269 let lin_x = lin_x.max(0.0).min(1.0);
270 let lin_y = lin_y.max(0.0).min(1.0);
271 let lin_z = lin_z.max(0.0).min(1.0);
272 let lin_w = lin_w.max(0.0).min(1.0);
273
274 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
275 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
276 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
277 let scale_w = (self.grid_size[3] as i32 - 1) as f32;
278
279 let x = (lin_x * scale_x).floor() as i32;
280 let y = (lin_y * scale_y).floor() as i32;
281 let z = (lin_z * scale_z).floor() as i32;
282 let w = (lin_w * scale_w).floor() as i32;
283
284 let x_n = (lin_x * scale_x).ceil() as i32;
285 let y_n = (lin_y * scale_y).ceil() as i32;
286 let z_n = (lin_z * scale_z).ceil() as i32;
287 let w_n = (lin_w * scale_w).ceil() as i32;
288
289 let dr = lin_x * scale_x - x as f32;
290 let dg = lin_y * scale_y - y as f32;
291 let db = lin_z * scale_z - z as f32;
292 let dw = lin_w * scale_w - w as f32;
293
294 let c0 = r.fetch(x, y, z, w);
295
296 let w0 = if dr > db && dg > db {
297 let x0 = r.fetch(x_n, y_n, z_n, w);
298 let x1 = r.fetch(x_n, y_n, z, w);
299 let x2 = r.fetch(x_n, y, z, w);
300 let x3 = r.fetch(x, y_n, z, w);
301
302 let c1 = x0 - x1;
303 let c2 = x2 - c0;
304 let c3 = x3 - c0;
305 let c4 = c0 - x3 - x2 + x1;
306
307 let s0 = c0.mla(c1, T::from(db));
308 let s1 = s0.mla(c2, T::from(dr));
309 let s2 = s1.mla(c3, T::from(dg));
310 s2.mla(c4, T::from(dr * dg))
311 } else if db > dr && dg > dr {
312 let x0 = r.fetch(x, y, z_n, w);
313 let x1 = r.fetch(x_n, y_n, z_n, w);
314 let x2 = r.fetch(x, y_n, z_n, w);
315 let x3 = r.fetch(x, y_n, z, w);
316
317 let c1 = x0 - c0;
318 let c2 = x1 - x2;
319 let c3 = x3 - c0;
320 let c4 = c0 - x3 - x0 + x2;
321
322 let s0 = c0.mla(c1, T::from(db));
323 let s1 = s0.mla(c2, T::from(dr));
324 let s2 = s1.mla(c3, T::from(dg));
325 s2.mla(c4, T::from(dg * db))
326 } else {
327 let x0 = r.fetch(x, y, z_n, w);
328 let x1 = r.fetch(x_n, y, z, w);
329 let x2 = r.fetch(x_n, y, z_n, w);
330 let x3 = r.fetch(x_n, y_n, z_n, w);
331
332 let c1 = x0 - c0;
333 let c2 = x1 - c0;
334 let c3 = x3 - x2;
335 let c4 = c0 - x1 - x0 + x2;
336
337 let s0 = c0.mla(c1, T::from(db));
338 let s1 = s0.mla(c2, T::from(dr));
339 let s2 = s1.mla(c3, T::from(dg));
340 s2.mla(c4, T::from(db * dr))
341 };
342
343 let c0 = r.fetch(x, y, z, w_n);
344
345 let w1 = if dr > db && dg > db {
346 let x0 = r.fetch(x_n, y_n, z_n, w_n);
347 let x1 = r.fetch(x_n, y_n, z, w_n);
348 let x2 = r.fetch(x_n, y, z, w_n);
349 let x3 = r.fetch(x, y_n, z, w_n);
350
351 let c1 = x0 - x1;
352 let c2 = x2 - c0;
353 let c3 = x3 - c0;
354 let c4 = c0 - x3 - x2 + x1;
355
356 let s0 = c0.mla(c1, T::from(db));
357 let s1 = s0.mla(c2, T::from(dr));
358 let s2 = s1.mla(c3, T::from(dg));
359 s2.mla(c4, T::from(dr * dg))
360 } else if db > dr && dg > dr {
361 let x0 = r.fetch(x, y, z_n, w_n);
362 let x1 = r.fetch(x_n, y_n, z_n, w_n);
363 let x2 = r.fetch(x, y_n, z_n, w_n);
364 let x3 = r.fetch(x, y_n, z, w_n);
365
366 let c1 = x0 - c0;
367 let c2 = x1 - x2;
368 let c3 = x3 - c0;
369 let c4 = c0 - x3 - x0 + x2;
370
371 let s0 = c0.mla(c1, T::from(db));
372 let s1 = s0.mla(c2, T::from(dr));
373 let s2 = s1.mla(c3, T::from(dg));
374 s2.mla(c4, T::from(dg * db))
375 } else {
376 let x0 = r.fetch(x, y, z_n, w_n);
377 let x1 = r.fetch(x_n, y, z, w_n);
378 let x2 = r.fetch(x_n, y, z_n, w_n);
379 let x3 = r.fetch(x_n, y_n, z_n, w_n);
380
381 let c1 = x0 - c0;
382 let c2 = x1 - c0;
383 let c3 = x3 - x2;
384 let c4 = c0 - x1 - x0 + x2;
385
386 let s0 = c0.mla(c1, T::from(db));
387 let s1 = s0.mla(c2, T::from(dr));
388 let s2 = s1.mla(c3, T::from(dg));
389 s2.mla(c4, T::from(db * dr))
390 };
391 w0.neg_mla(w0, T::from(dw)).mla(w1, T::from(dw))
392 }
393
394 #[cfg(feature = "options")]
395 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
396 #[inline]
397 pub fn pyramid_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector3f {
398 self.pyramid(
399 lin_x,
400 lin_y,
401 lin_z,
402 lin_w,
403 Fetch4Vec3 {
404 array: self.array,
405 x_stride: self.x_stride,
406 y_stride: self.y_stride,
407 z_stride: self.z_stride,
408 },
409 )
410 }
411
412 #[cfg(feature = "options")]
413 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
414 #[inline]
415 pub fn pyramid_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector4f {
416 self.pyramid(
417 lin_x,
418 lin_y,
419 lin_z,
420 lin_w,
421 Fetch4Vec4 {
422 array: self.array,
423 x_stride: self.x_stride,
424 y_stride: self.y_stride,
425 z_stride: self.z_stride,
426 },
427 )
428 }
429
430 #[cfg(feature = "options")]
431 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
432 #[inline(always)]
433 fn prism<
434 T: From<f32>
435 + Add<T, Output = T>
436 + Mul<T, Output = T>
437 + FusedMultiplyAdd<T>
438 + Sub<T, Output = T>
439 + Copy
440 + FusedMultiplyNegAdd<T>,
441 >(
442 &self,
443 lin_x: f32,
444 lin_y: f32,
445 lin_z: f32,
446 lin_w: f32,
447 r: impl Fetcher4<T>,
448 ) -> T {
449 let lin_x = lin_x.max(0.0).min(1.0);
450 let lin_y = lin_y.max(0.0).min(1.0);
451 let lin_z = lin_z.max(0.0).min(1.0);
452 let lin_w = lin_w.max(0.0).min(1.0);
453
454 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
455 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
456 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
457 let scale_w = (self.grid_size[3] as i32 - 1) as f32;
458
459 let x = (lin_x * scale_x).floor() as i32;
460 let y = (lin_y * scale_y).floor() as i32;
461 let z = (lin_z * scale_z).floor() as i32;
462 let w = (lin_w * scale_w).floor() as i32;
463
464 let x_n = (lin_x * scale_x).ceil() as i32;
465 let y_n = (lin_y * scale_y).ceil() as i32;
466 let z_n = (lin_z * scale_z).ceil() as i32;
467 let w_n = (lin_w * scale_w).ceil() as i32;
468
469 let dr = lin_x * scale_x - x as f32;
470 let dg = lin_y * scale_y - y as f32;
471 let db = lin_z * scale_z - z as f32;
472 let dw = lin_w * scale_w - w as f32;
473
474 let c0 = r.fetch(x, y, z, w);
475
476 let w0 = if db >= dr {
477 let x0 = r.fetch(x, y, z_n, w);
478 let x1 = r.fetch(x_n, y, z_n, w);
479 let x2 = r.fetch(x, y_n, z, w);
480 let x3 = r.fetch(x, y_n, z_n, w);
481 let x4 = r.fetch(x_n, y_n, z_n, w);
482
483 let c1 = x0 - c0;
484 let c2 = x1 - x0;
485 let c3 = x2 - c0;
486 let c4 = c0 - x2 - x0 + x3;
487 let c5 = x0 - x3 - x1 + x4;
488
489 let s0 = c0.mla(c1, T::from(db));
490 let s1 = s0.mla(c2, T::from(dr));
491 let s2 = s1.mla(c3, T::from(dg));
492 let s3 = s2.mla(c4, T::from(dg * db));
493 s3.mla(c5, T::from(dr * dg))
494 } else {
495 let x0 = r.fetch(x_n, y, z, w);
496 let x1 = r.fetch(x_n, y, z_n, w);
497 let x2 = r.fetch(x, y_n, z, w);
498 let x3 = r.fetch(x_n, y_n, z, w);
499 let x4 = r.fetch(x_n, y_n, z_n, w);
500
501 let c1 = x1 - x0;
502 let c2 = x0 - c0;
503 let c3 = x2 - c0;
504 let c4 = x0 - x3 - x1 + x4;
505 let c5 = c0 - x2 - x0 + x3;
506
507 let s0 = c0.mla(c1, T::from(db));
508 let s1 = s0.mla(c2, T::from(dr));
509 let s2 = s1.mla(c3, T::from(dg));
510 let s3 = s2.mla(c4, T::from(dg * db));
511 s3.mla(c5, T::from(dr * dg))
512 };
513
514 let c0 = r.fetch(x, y, z, w_n);
515
516 let w1 = if db >= dr {
517 let x0 = r.fetch(x, y, z_n, w_n);
518 let x1 = r.fetch(x_n, y, z_n, w_n);
519 let x2 = r.fetch(x, y_n, z, w_n);
520 let x3 = r.fetch(x, y_n, z_n, w_n);
521 let x4 = r.fetch(x_n, y_n, z_n, w_n);
522
523 let c1 = x0 - c0;
524 let c2 = x1 - x0;
525 let c3 = x2 - c0;
526 let c4 = c0 - x2 - x0 + x3;
527 let c5 = x0 - x3 - x1 + x4;
528
529 let s0 = c0.mla(c1, T::from(db));
530 let s1 = s0.mla(c2, T::from(dr));
531 let s2 = s1.mla(c3, T::from(dg));
532 let s3 = s2.mla(c4, T::from(dg * db));
533 s3.mla(c5, T::from(dr * dg))
534 } else {
535 let x0 = r.fetch(x_n, y, z, w_n);
536 let x1 = r.fetch(x_n, y, z_n, w_n);
537 let x2 = r.fetch(x, y_n, z, w_n);
538 let x3 = r.fetch(x_n, y_n, z, w_n);
539 let x4 = r.fetch(x_n, y_n, z_n, w_n);
540
541 let c1 = x1 - x0;
542 let c2 = x0 - c0;
543 let c3 = x2 - c0;
544 let c4 = x0 - x3 - x1 + x4;
545 let c5 = c0 - x2 - x0 + x3;
546
547 let s0 = c0.mla(c1, T::from(db));
548 let s1 = s0.mla(c2, T::from(dr));
549 let s2 = s1.mla(c3, T::from(dg));
550 let s3 = s2.mla(c4, T::from(dg * db));
551 s3.mla(c5, T::from(dr * dg))
552 };
553 w0.neg_mla(w0, T::from(dw)).mla(w1, T::from(dw))
554 }
555
556 #[cfg(feature = "options")]
557 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
558 #[inline]
559 pub fn prism_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector3f {
560 self.prism(
561 lin_x,
562 lin_y,
563 lin_z,
564 lin_w,
565 Fetch4Vec3 {
566 array: self.array,
567 x_stride: self.x_stride,
568 y_stride: self.y_stride,
569 z_stride: self.z_stride,
570 },
571 )
572 }
573
574 #[cfg(feature = "options")]
575 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
576 #[inline]
577 pub fn prism_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector4f {
578 self.prism(
579 lin_x,
580 lin_y,
581 lin_z,
582 lin_w,
583 Fetch4Vec4 {
584 array: self.array,
585 x_stride: self.x_stride,
586 y_stride: self.y_stride,
587 z_stride: self.z_stride,
588 },
589 )
590 }
591
592 #[cfg(feature = "options")]
593 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
594 #[inline(always)]
595 fn tetra<
596 T: From<f32>
597 + Add<T, Output = T>
598 + Mul<T, Output = T>
599 + FusedMultiplyAdd<T>
600 + Sub<T, Output = T>
601 + Copy
602 + FusedMultiplyNegAdd<T>,
603 >(
604 &self,
605 lin_x: f32,
606 lin_y: f32,
607 lin_z: f32,
608 lin_w: f32,
609 r: impl Fetcher4<T>,
610 ) -> T {
611 let lin_x = lin_x.max(0.0).min(1.0);
612 let lin_y = lin_y.max(0.0).min(1.0);
613 let lin_z = lin_z.max(0.0).min(1.0);
614 let lin_w = lin_w.max(0.0).min(1.0);
615
616 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
617 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
618 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
619 let scale_w = (self.grid_size[3] as i32 - 1) as f32;
620
621 let x = (lin_x * scale_x).floor() as i32;
622 let y = (lin_y * scale_y).floor() as i32;
623 let z = (lin_z * scale_z).floor() as i32;
624 let w = (lin_w * scale_w).floor() as i32;
625
626 let x_n = (lin_x * scale_x).ceil() as i32;
627 let y_n = (lin_y * scale_y).ceil() as i32;
628 let z_n = (lin_z * scale_z).ceil() as i32;
629 let w_n = (lin_w * scale_w).ceil() as i32;
630
631 let rx = lin_x * scale_x - x as f32;
632 let ry = lin_y * scale_y - y as f32;
633 let rz = lin_z * scale_z - z as f32;
634 let rw = lin_w * scale_w - w as f32;
635
636 let c0 = r.fetch(x, y, z, w);
637 let c2;
638 let c1;
639 let c3;
640 if rx >= ry {
641 if ry >= rz {
642 c1 = r.fetch(x_n, y, z, w) - c0;
644 c2 = r.fetch(x_n, y_n, z, w) - r.fetch(x_n, y, z, w);
645 c3 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x_n, y_n, z, w);
646 } else if rx >= rz {
647 c1 = r.fetch(x_n, y, z, w) - c0;
649 c2 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x_n, y, z_n, w);
650 c3 = r.fetch(x_n, y, z_n, w) - r.fetch(x_n, y, z, w);
651 } else {
652 c1 = r.fetch(x_n, y, z_n, w) - r.fetch(x, y, z_n, w);
654 c2 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x_n, y, z_n, w);
655 c3 = r.fetch(x, y, z_n, w) - c0;
656 }
657 } else if rx >= rz {
658 c1 = r.fetch(x_n, y_n, z, w) - r.fetch(x, y_n, z, w);
660 c2 = r.fetch(x, y_n, z, w) - c0;
661 c3 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x_n, y_n, z, w);
662 } else if ry >= rz {
663 c1 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x, y_n, z_n, w);
665 c2 = r.fetch(x, y_n, z, w) - c0;
666 c3 = r.fetch(x, y_n, z_n, w) - r.fetch(x, y_n, z, w);
667 } else {
668 c1 = r.fetch(x_n, y_n, z_n, w) - r.fetch(x, y_n, z_n, w);
670 c2 = r.fetch(x, y_n, z_n, w) - r.fetch(x, y, z_n, w);
671 c3 = r.fetch(x, y, z_n, w) - c0;
672 }
673 let s0 = c0.mla(c1, T::from(rx));
674 let s1 = s0.mla(c2, T::from(ry));
675 let w0 = s1.mla(c3, T::from(rz));
676
677 let c0 = r.fetch(x, y, z, w_n);
678 let c2;
679 let c1;
680 let c3;
681 if rx >= ry {
682 if ry >= rz {
683 c1 = r.fetch(x_n, y, z, w_n) - c0;
685 c2 = r.fetch(x_n, y_n, z, w_n) - r.fetch(x_n, y, z, w_n);
686 c3 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x_n, y_n, z, w_n);
687 } else if rx >= rz {
688 c1 = r.fetch(x_n, y, z, w_n) - c0;
690 c2 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x_n, y, z_n, w_n);
691 c3 = r.fetch(x_n, y, z_n, w_n) - r.fetch(x_n, y, z, w_n);
692 } else {
693 c1 = r.fetch(x_n, y, z_n, w_n) - r.fetch(x, y, z_n, w_n);
695 c2 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x_n, y, z_n, w_n);
696 c3 = r.fetch(x, y, z_n, w_n) - c0;
697 }
698 } else if rx >= rz {
699 c1 = r.fetch(x_n, y_n, z, w_n) - r.fetch(x, y_n, z, w_n);
701 c2 = r.fetch(x, y_n, z, w_n) - c0;
702 c3 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x_n, y_n, z, w_n);
703 } else if ry >= rz {
704 c1 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x, y_n, z_n, w_n);
706 c2 = r.fetch(x, y_n, z, w_n) - c0;
707 c3 = r.fetch(x, y_n, z_n, w_n) - r.fetch(x, y_n, z, w_n);
708 } else {
709 c1 = r.fetch(x_n, y_n, z_n, w_n) - r.fetch(x, y_n, z_n, w_n);
711 c2 = r.fetch(x, y_n, z_n, w_n) - r.fetch(x, y, z_n, w_n);
712 c3 = r.fetch(x, y, z_n, w_n) - c0;
713 }
714 let s0 = c0.mla(c1, T::from(rx));
715 let s1 = s0.mla(c2, T::from(ry));
716 let w1 = s1.mla(c3, T::from(rz));
717 w0.neg_mla(w0, T::from(rw)).mla(w1, T::from(rw))
718 }
719
720 #[cfg(feature = "options")]
721 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
722 #[inline]
723 pub fn tetra_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector3f {
724 self.tetra(
725 lin_x,
726 lin_y,
727 lin_z,
728 lin_w,
729 Fetch4Vec3 {
730 array: self.array,
731 x_stride: self.x_stride,
732 y_stride: self.y_stride,
733 z_stride: self.z_stride,
734 },
735 )
736 }
737
738 #[cfg(feature = "options")]
739 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
740 #[inline]
741 pub fn tetra_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32, lin_w: f32) -> Vector4f {
742 self.tetra(
743 lin_x,
744 lin_y,
745 lin_z,
746 lin_w,
747 Fetch4Vec4 {
748 array: self.array,
749 x_stride: self.x_stride,
750 y_stride: self.y_stride,
751 z_stride: self.z_stride,
752 },
753 )
754 }
755}
756
757pub struct Cube<'a> {
761 array: &'a [f32],
762 x_stride: u32,
763 y_stride: u32,
764 grid_size: [u8; 3],
765}
766
767pub(crate) trait ArrayFetch<T> {
768 fn fetch(&self, x: i32, y: i32, z: i32) -> T;
769}
770
771struct ArrayFetchVector3f<'a> {
772 array: &'a [f32],
773 x_stride: u32,
774 y_stride: u32,
775}
776
777impl ArrayFetch<Vector3f> for ArrayFetchVector3f<'_> {
778 #[inline(always)]
779 fn fetch(&self, x: i32, y: i32, z: i32) -> Vector3f {
780 let start = (x as u32 * self.x_stride + y as u32 * self.y_stride + z as u32) as usize * 3;
781 let k = &self.array[start..start + 3];
782 Vector3f {
783 v: [k[0], k[1], k[2]],
784 }
785 }
786}
787
788struct ArrayFetchVector4f<'a> {
789 array: &'a [f32],
790 x_stride: u32,
791 y_stride: u32,
792}
793
794impl ArrayFetch<Vector4f> for ArrayFetchVector4f<'_> {
795 #[inline(always)]
796 fn fetch(&self, x: i32, y: i32, z: i32) -> Vector4f {
797 let start = (x as u32 * self.x_stride + y as u32 * self.y_stride + z as u32) as usize * 4;
798 let k = &self.array[start..start + 4];
799 Vector4f {
800 v: [k[0], k[1], k[2], k[3]],
801 }
802 }
803}
804
805impl Cube<'_> {
806 pub fn new(array: &[f32], grid_size: usize) -> Cube<'_> {
807 let y_stride = grid_size;
808 let x_stride = y_stride * y_stride;
809 Cube {
810 array,
811 x_stride: x_stride as u32,
812 y_stride: y_stride as u32,
813 grid_size: [grid_size as u8, grid_size as u8, grid_size as u8],
814 }
815 }
816
817 pub fn new_cube(array: &[f32], grid_size: [u8; 3]) -> Cube<'_> {
818 let y_stride = grid_size[1] as u32;
819 let x_stride = y_stride * grid_size[0] as u32;
820 Cube {
821 array,
822 x_stride,
823 y_stride,
824 grid_size,
825 }
826 }
827
828 #[inline(always)]
829 fn trilinear<
830 T: Copy
831 + From<f32>
832 + Sub<T, Output = T>
833 + Mul<T, Output = T>
834 + Add<T, Output = T>
835 + FusedMultiplyNegAdd<T>
836 + FusedMultiplyAdd<T>,
837 >(
838 &self,
839 lin_x: f32,
840 lin_y: f32,
841 lin_z: f32,
842 fetch: impl ArrayFetch<T>,
843 ) -> T {
844 let lin_x = lin_x.max(0.0).min(1.0);
845 let lin_y = lin_y.max(0.0).min(1.0);
846 let lin_z = lin_z.max(0.0).min(1.0);
847
848 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
849 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
850 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
851
852 let x = (lin_x * scale_x).floor() as i32;
853 let y = (lin_y * scale_y).floor() as i32;
854 let z = (lin_z * scale_z).floor() as i32;
855
856 let x_n = (lin_x * scale_x).ceil() as i32;
857 let y_n = (lin_y * scale_y).ceil() as i32;
858 let z_n = (lin_z * scale_z).ceil() as i32;
859
860 let x_d = T::from(lin_x * scale_x - x as f32);
861 let y_d = T::from(lin_y * scale_y - y as f32);
862 let z_d = T::from(lin_z * scale_z - z as f32);
863
864 let c000 = fetch.fetch(x, y, z);
865 let c100 = fetch.fetch(x_n, y, z);
866 let c010 = fetch.fetch(x, y_n, z);
867 let c110 = fetch.fetch(x_n, y_n, z);
868 let c001 = fetch.fetch(x, y, z_n);
869 let c101 = fetch.fetch(x_n, y, z_n);
870 let c011 = fetch.fetch(x, y_n, z_n);
871 let c111 = fetch.fetch(x_n, y_n, z_n);
872
873 let c00 = c000.neg_mla(c000, x_d).mla(c100, x_d);
874 let c10 = c010.neg_mla(c010, x_d).mla(c110, x_d);
875 let c01 = c001.neg_mla(c001, x_d).mla(c101, x_d);
876 let c11 = c011.neg_mla(c011, x_d).mla(c111, x_d);
877
878 let c0 = c00.neg_mla(c00, y_d).mla(c10, y_d);
879 let c1 = c01.neg_mla(c01, y_d).mla(c11, y_d);
880
881 c0.neg_mla(c0, z_d).mla(c1, z_d)
882 }
883
884 #[cfg(feature = "options")]
885 #[inline]
886 fn pyramid<
887 T: Copy
888 + From<f32>
889 + Sub<T, Output = T>
890 + Mul<T, Output = T>
891 + Add<T, Output = T>
892 + FusedMultiplyAdd<T>,
893 >(
894 &self,
895 lin_x: f32,
896 lin_y: f32,
897 lin_z: f32,
898 fetch: impl ArrayFetch<T>,
899 ) -> T {
900 let lin_x = lin_x.max(0.0).min(1.0);
901 let lin_y = lin_y.max(0.0).min(1.0);
902 let lin_z = lin_z.max(0.0).min(1.0);
903
904 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
905 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
906 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
907
908 let x = (lin_x * scale_x).floor() as i32;
909 let y = (lin_y * scale_y).floor() as i32;
910 let z = (lin_z * scale_z).floor() as i32;
911
912 let x_n = (lin_x * scale_x).ceil() as i32;
913 let y_n = (lin_y * scale_y).ceil() as i32;
914 let z_n = (lin_z * scale_z).ceil() as i32;
915
916 let dr = lin_x * scale_x - x as f32;
917 let dg = lin_y * scale_y - y as f32;
918 let db = lin_z * scale_z - z as f32;
919
920 let c0 = fetch.fetch(x, y, z);
921
922 if dr > db && dg > db {
923 let x0 = fetch.fetch(x_n, y_n, z_n);
924 let x1 = fetch.fetch(x_n, y_n, z);
925 let x2 = fetch.fetch(x_n, y, z);
926 let x3 = fetch.fetch(x, y_n, z);
927
928 let c1 = x0 - x1;
929 let c2 = x2 - c0;
930 let c3 = x3 - c0;
931 let c4 = c0 - x3 - x2 + x1;
932
933 let s0 = c0.mla(c1, T::from(db));
934 let s1 = s0.mla(c2, T::from(dr));
935 let s2 = s1.mla(c3, T::from(dg));
936 s2.mla(c4, T::from(dr * dg))
937 } else if db > dr && dg > dr {
938 let x0 = fetch.fetch(x, y, z_n);
939 let x1 = fetch.fetch(x_n, y_n, z_n);
940 let x2 = fetch.fetch(x, y_n, z_n);
941 let x3 = fetch.fetch(x, y_n, z);
942
943 let c1 = x0 - c0;
944 let c2 = x1 - x2;
945 let c3 = x3 - c0;
946 let c4 = c0 - x3 - x0 + x2;
947
948 let s0 = c0.mla(c1, T::from(db));
949 let s1 = s0.mla(c2, T::from(dr));
950 let s2 = s1.mla(c3, T::from(dg));
951 s2.mla(c4, T::from(dg * db))
952 } else {
953 let x0 = fetch.fetch(x, y, z_n);
954 let x1 = fetch.fetch(x_n, y, z);
955 let x2 = fetch.fetch(x_n, y, z_n);
956 let x3 = fetch.fetch(x_n, y_n, z_n);
957
958 let c1 = x0 - c0;
959 let c2 = x1 - c0;
960 let c3 = x3 - x2;
961 let c4 = c0 - x1 - x0 + x2;
962
963 let s0 = c0.mla(c1, T::from(db));
964 let s1 = s0.mla(c2, T::from(dr));
965 let s2 = s1.mla(c3, T::from(dg));
966 s2.mla(c4, T::from(db * dr))
967 }
968 }
969
970 #[cfg(feature = "options")]
971 #[inline]
972 fn tetra<
973 T: Copy
974 + From<f32>
975 + Sub<T, Output = T>
976 + Mul<T, Output = T>
977 + Add<T, Output = T>
978 + FusedMultiplyAdd<T>,
979 >(
980 &self,
981 lin_x: f32,
982 lin_y: f32,
983 lin_z: f32,
984 fetch: impl ArrayFetch<T>,
985 ) -> T {
986 let lin_x = lin_x.max(0.0).min(1.0);
987 let lin_y = lin_y.max(0.0).min(1.0);
988 let lin_z = lin_z.max(0.0).min(1.0);
989
990 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
991 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
992 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
993
994 let x = (lin_x * scale_x).floor() as i32;
995 let y = (lin_y * scale_y).floor() as i32;
996 let z = (lin_z * scale_z).floor() as i32;
997
998 let x_n = (lin_x * scale_x).ceil() as i32;
999 let y_n = (lin_y * scale_y).ceil() as i32;
1000 let z_n = (lin_z * scale_z).ceil() as i32;
1001
1002 let rx = lin_x * scale_x - x as f32;
1003 let ry = lin_y * scale_y - y as f32;
1004 let rz = lin_z * scale_z - z as f32;
1005
1006 let c0 = fetch.fetch(x, y, z);
1007 let c2;
1008 let c1;
1009 let c3;
1010 if rx >= ry {
1011 if ry >= rz {
1012 c1 = fetch.fetch(x_n, y, z) - c0;
1014 c2 = fetch.fetch(x_n, y_n, z) - fetch.fetch(x_n, y, z);
1015 c3 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x_n, y_n, z);
1016 } else if rx >= rz {
1017 c1 = fetch.fetch(x_n, y, z) - c0;
1019 c2 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x_n, y, z_n);
1020 c3 = fetch.fetch(x_n, y, z_n) - fetch.fetch(x_n, y, z);
1021 } else {
1022 c1 = fetch.fetch(x_n, y, z_n) - fetch.fetch(x, y, z_n);
1024 c2 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x_n, y, z_n);
1025 c3 = fetch.fetch(x, y, z_n) - c0;
1026 }
1027 } else if rx >= rz {
1028 c1 = fetch.fetch(x_n, y_n, z) - fetch.fetch(x, y_n, z);
1030 c2 = fetch.fetch(x, y_n, z) - c0;
1031 c3 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x_n, y_n, z);
1032 } else if ry >= rz {
1033 c1 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x, y_n, z_n);
1035 c2 = fetch.fetch(x, y_n, z) - c0;
1036 c3 = fetch.fetch(x, y_n, z_n) - fetch.fetch(x, y_n, z);
1037 } else {
1038 c1 = fetch.fetch(x_n, y_n, z_n) - fetch.fetch(x, y_n, z_n);
1040 c2 = fetch.fetch(x, y_n, z_n) - fetch.fetch(x, y, z_n);
1041 c3 = fetch.fetch(x, y, z_n) - c0;
1042 }
1043 let s0 = c0.mla(c1, T::from(rx));
1044 let s1 = s0.mla(c2, T::from(ry));
1045 s1.mla(c3, T::from(rz))
1046 }
1047
1048 #[cfg(feature = "options")]
1049 #[inline]
1050 fn prism<
1051 T: Copy
1052 + From<f32>
1053 + Sub<T, Output = T>
1054 + Mul<T, Output = T>
1055 + Add<T, Output = T>
1056 + FusedMultiplyAdd<T>,
1057 >(
1058 &self,
1059 lin_x: f32,
1060 lin_y: f32,
1061 lin_z: f32,
1062 fetch: impl ArrayFetch<T>,
1063 ) -> T {
1064 let lin_x = lin_x.max(0.0).min(1.0);
1065 let lin_y = lin_y.max(0.0).min(1.0);
1066 let lin_z = lin_z.max(0.0).min(1.0);
1067
1068 let scale_x = (self.grid_size[0] as i32 - 1) as f32;
1069 let scale_y = (self.grid_size[1] as i32 - 1) as f32;
1070 let scale_z = (self.grid_size[2] as i32 - 1) as f32;
1071
1072 let x = (lin_x * scale_x).floor() as i32;
1073 let y = (lin_y * scale_y).floor() as i32;
1074 let z = (lin_z * scale_z).floor() as i32;
1075
1076 let x_n = (lin_x * scale_x).ceil() as i32;
1077 let y_n = (lin_y * scale_y).ceil() as i32;
1078 let z_n = (lin_z * scale_z).ceil() as i32;
1079
1080 let dr = lin_x * scale_x - x as f32;
1081 let dg = lin_y * scale_y - y as f32;
1082 let db = lin_z * scale_z - z as f32;
1083
1084 let c0 = fetch.fetch(x, y, z);
1085
1086 if db >= dr {
1087 let x0 = fetch.fetch(x, y, z_n);
1088 let x1 = fetch.fetch(x_n, y, z_n);
1089 let x2 = fetch.fetch(x, y_n, z);
1090 let x3 = fetch.fetch(x, y_n, z_n);
1091 let x4 = fetch.fetch(x_n, y_n, z_n);
1092
1093 let c1 = x0 - c0;
1094 let c2 = x1 - x0;
1095 let c3 = x2 - c0;
1096 let c4 = c0 - x2 - x0 + x3;
1097 let c5 = x0 - x3 - x1 + x4;
1098
1099 let s0 = c0.mla(c1, T::from(db));
1100 let s1 = s0.mla(c2, T::from(dr));
1101 let s2 = s1.mla(c3, T::from(dg));
1102 let s3 = s2.mla(c4, T::from(dg * db));
1103 s3.mla(c5, T::from(dr * dg))
1104 } else {
1105 let x0 = fetch.fetch(x_n, y, z);
1106 let x1 = fetch.fetch(x_n, y, z_n);
1107 let x2 = fetch.fetch(x, y_n, z);
1108 let x3 = fetch.fetch(x_n, y_n, z);
1109 let x4 = fetch.fetch(x_n, y_n, z_n);
1110
1111 let c1 = x1 - x0;
1112 let c2 = x0 - c0;
1113 let c3 = x2 - c0;
1114 let c4 = x0 - x3 - x1 + x4;
1115 let c5 = c0 - x2 - x0 + x3;
1116
1117 let s0 = c0.mla(c1, T::from(db));
1118 let s1 = s0.mla(c2, T::from(dr));
1119 let s2 = s1.mla(c3, T::from(dg));
1120 let s3 = s2.mla(c4, T::from(dg * db));
1121 s3.mla(c5, T::from(dr * dg))
1122 }
1123 }
1124
1125 pub fn trilinear_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector3f {
1126 self.trilinear(
1127 lin_x,
1128 lin_y,
1129 lin_z,
1130 ArrayFetchVector3f {
1131 array: self.array,
1132 x_stride: self.x_stride,
1133 y_stride: self.y_stride,
1134 },
1135 )
1136 }
1137
1138 #[cfg(feature = "options")]
1139 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
1140 pub fn prism_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector3f {
1141 self.prism(
1142 lin_x,
1143 lin_y,
1144 lin_z,
1145 ArrayFetchVector3f {
1146 array: self.array,
1147 x_stride: self.x_stride,
1148 y_stride: self.y_stride,
1149 },
1150 )
1151 }
1152
1153 #[cfg(feature = "options")]
1154 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
1155 pub fn pyramid_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector3f {
1156 self.pyramid(
1157 lin_x,
1158 lin_y,
1159 lin_z,
1160 ArrayFetchVector3f {
1161 array: self.array,
1162 x_stride: self.x_stride,
1163 y_stride: self.y_stride,
1164 },
1165 )
1166 }
1167
1168 #[cfg(feature = "options")]
1169 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
1170 pub fn tetra_vec3(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector3f {
1171 self.tetra(
1172 lin_x,
1173 lin_y,
1174 lin_z,
1175 ArrayFetchVector3f {
1176 array: self.array,
1177 x_stride: self.x_stride,
1178 y_stride: self.y_stride,
1179 },
1180 )
1181 }
1182
1183 pub fn trilinear_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector4f {
1184 self.trilinear(
1185 lin_x,
1186 lin_y,
1187 lin_z,
1188 ArrayFetchVector4f {
1189 array: self.array,
1190 x_stride: self.x_stride,
1191 y_stride: self.y_stride,
1192 },
1193 )
1194 }
1195
1196 #[cfg(feature = "options")]
1197 pub fn tetra_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector4f {
1198 self.tetra(
1199 lin_x,
1200 lin_y,
1201 lin_z,
1202 ArrayFetchVector4f {
1203 array: self.array,
1204 x_stride: self.x_stride,
1205 y_stride: self.y_stride,
1206 },
1207 )
1208 }
1209
1210 #[cfg(feature = "options")]
1211 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
1212 pub fn pyramid_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector4f {
1213 self.pyramid(
1214 lin_x,
1215 lin_y,
1216 lin_z,
1217 ArrayFetchVector4f {
1218 array: self.array,
1219 x_stride: self.x_stride,
1220 y_stride: self.y_stride,
1221 },
1222 )
1223 }
1224
1225 #[cfg(feature = "options")]
1226 #[cfg_attr(docsrs, doc(cfg(feature = "options")))]
1227 pub fn prism_vec4(&self, lin_x: f32, lin_y: f32, lin_z: f32) -> Vector4f {
1228 self.prism(
1229 lin_x,
1230 lin_y,
1231 lin_z,
1232 ArrayFetchVector4f {
1233 array: self.array,
1234 x_stride: self.x_stride,
1235 y_stride: self.y_stride,
1236 },
1237 )
1238 }
1239}