1#[cfg(test)]
7mod tests;
8use crate::{nalgebra, prelude::*};
9use approx::{AbsDiffEq, UlpsEq};
10use std::borrow::Borrow;
11
12#[derive(Debug, Copy, Clone)]
13pub struct Aabb2<T> {
14 min: nalgebra::Vector2<T>,
15 max: nalgebra::Vector2<T>,
16}
17
18macro_rules! impl_aabb2 {
19 ($vec_type:ty, $scalar_type:ty) => {
20 impl Default for Aabb2<$scalar_type> {
21 fn default() -> Self {
22 Self {
23 min: <$vec_type>::splat(<$scalar_type>::INFINITY),
24 max: <$vec_type>::splat(<$scalar_type>::NEG_INFINITY),
25 }
26 }
27 }
28
29 impl crate::prelude::Aabb2 for Aabb2<$scalar_type> {
30 type Vector = $vec_type;
31
32 fn from_point(point: Self::Vector) -> Self {
33 Self {
34 min: point,
35 max: point,
36 }
37 }
38
39 #[inline(always)]
40 fn from_corners(min: Self::Vector, max: Self::Vector) -> Self {
41 let mut rv = Self { min: max, max };
42 rv.add_point(min);
43 rv
44 }
45
46 #[inline(always)]
47 fn from_center_and_half_extents(
48 center: Self::Vector,
49 half_extents: Self::Vector,
50 ) -> Self {
51 Self {
52 min: center - half_extents,
53 max: center + half_extents,
54 }
55 }
56
57 #[inline(always)]
58 fn from_center_and_size(center: Self::Vector, size: Self::Vector) -> Self {
59 let half_extents = size / 2.0;
60 Self::from_center_and_half_extents(center, half_extents)
61 }
62
63 fn from_points<I>(points: I) -> Self
64 where
65 I: IntoIterator,
66 I::Item: Borrow<Self::Vector>,
67 {
68 let mut aabb = Self::default();
69 for point in points {
70 let point = *point.borrow();
71 aabb.min = aabb.min.min(point);
72 aabb.max = aabb.max.max(point);
73 }
74 aabb
75 }
76
77 #[inline(always)]
78 fn max(&self) -> Self::Vector {
79 self.max
80 }
81 #[inline(always)]
82 fn min(&self) -> Self::Vector {
83 self.min
84 }
85 #[inline(always)]
86 fn add_point(&mut self, pos: Self::Vector) {
87 self.max = self.max.max(pos);
88 self.min = self.min.min(pos);
89 }
90 #[inline(always)]
91 fn is_empty(&self) -> bool {
92 self.max.x < self.min.x
93 }
94 #[inline(always)]
95 fn add_aabb(&mut self, other: &Self) {
96 self.max = self.max.max(other.max);
97 self.min = self.min.min(other.min);
98 }
99 #[inline(always)]
100 fn fast_pad(&mut self, delta: Self::Vector) {
101 self.max += delta;
102 self.min -= delta;
103 }
104 #[inline(always)]
105 fn pad(&mut self, delta: Self::Vector) {
106 let center = self.center();
107 let half_extent = (self.max - self.min) * 0.5;
108 let new_half_extent = (half_extent + delta).max(Self::Vector::ZERO);
109 self.max = center + new_half_extent;
110 self.min = center - new_half_extent;
111 }
112 #[inline(always)]
113 fn contains_point_inclusive(&self, point: Self::Vector) -> bool {
114 point.x >= self.min.x
115 && point.x <= self.max.x
116 && point.y >= self.min.y
117 && point.y <= self.max.y
118 }
119 #[inline(always)]
120 fn center(&self) -> Self::Vector {
121 (self.max + self.min) * 0.5
122 }
123 #[inline(always)]
124 fn extents(&self) -> (Self::Vector, Self::Vector, Self::Vector) {
125 (self.min, self.max, self.max - self.min)
126 }
127 #[inline(always)]
128 fn contains_aabb_inclusive(&self, other: &Self) -> bool {
129 self.min.x <= other.min.x
130 && other.max.x <= self.max.x
131 && self.min.y <= other.min.y
132 && other.max.y <= self.max.y
133 }
134
135 #[inline(always)]
136 fn convex_hull(&self) -> Vec<Self::Vector> {
137 crate::trait_impl::aabb_to_vec::<Aabb2<$scalar_type>>(*self)
138 }
139
140 #[inline(always)]
141 fn apply<F>(&mut self, f: F)
142 where
143 F: Fn(Self::Vector) -> Self::Vector,
144 {
145 if !self.is_empty() {
146 let new_min = f(self.min);
147 let new_max = f(self.max);
148 self.min = new_min.min(new_max);
149 self.max = new_max.max(new_min);
150 }
151 }
152 }
153
154 impl From<Aabb2<$scalar_type>> for Vec<$vec_type> {
155 #[inline(always)]
156 fn from(other: Aabb2<$scalar_type>) -> Self {
157 crate::trait_impl::aabb_to_vec(other)
158 }
159 }
160
161 impl PartialEq for Aabb2<$scalar_type>
162 where
163 Aabb2<$scalar_type>: crate::prelude::Aabb2,
164 {
165 #[inline(always)]
166 fn eq(&self, other: &Self) -> bool {
167 crate::trait_impl::aabb2_partial_eq(self, other)
168 }
169 }
170 };
171}
172
173impl_aabb2!(nalgebra::Vector2<f64>, f64);
174impl_aabb2!(nalgebra::Vector2<f32>, f32);
175
176#[derive(Debug, Copy, Clone)]
177pub struct Aabb3<T> {
178 min: nalgebra::Vector3<T>,
179 max: nalgebra::Vector3<T>,
180}
181
182macro_rules! impl_aabb3 {
183 ($vec_type:ty, $scalar_type:ty) => {
184 impl Default for Aabb3<$scalar_type> {
185 fn default() -> Self {
186 Self {
187 min: <$vec_type>::splat(<$scalar_type>::INFINITY),
188 max: <$vec_type>::splat(<$scalar_type>::NEG_INFINITY),
189 }
190 }
191 }
192
193 impl crate::prelude::Aabb3 for Aabb3<$scalar_type> {
194 type Vector = $vec_type;
195
196 fn from_point(point: Self::Vector) -> Self {
197 Self {
198 min: point,
199 max: point,
200 }
201 }
202
203 #[inline(always)]
204 fn from_corners(min: Self::Vector, max: Self::Vector) -> Self {
205 let mut rv = Self { min: max, max };
206 rv.add_point(min);
207 rv
208 }
209
210 #[inline(always)]
211 fn from_center_and_half_extents(
212 center: Self::Vector,
213 half_extents: Self::Vector,
214 ) -> Self {
215 Self {
216 min: center - half_extents,
217 max: center + half_extents,
218 }
219 }
220
221 #[inline(always)]
222 fn from_center_and_size(center: Self::Vector, size: Self::Vector) -> Self {
223 let half_extents = size / 2.0;
224 Self::from_center_and_half_extents(center, half_extents)
225 }
226
227 fn from_points<I>(points: I) -> Self
228 where
229 I: IntoIterator,
230 I::Item: Borrow<Self::Vector>,
231 {
232 let mut aabb = Self::default();
233 for point in points {
234 let point = *point.borrow();
235 aabb.min = aabb.min.min(point);
236 aabb.max = aabb.max.max(point);
237 }
238 aabb
239 }
240
241 #[inline(always)]
242 fn max(&self) -> Self::Vector {
243 self.max
244 }
245 #[inline(always)]
246 fn min(&self) -> Self::Vector {
247 self.min
248 }
249 #[inline(always)]
250 fn add_point(&mut self, pos: Self::Vector) {
251 self.max = self.max.max(pos);
252 self.min = self.min.min(pos);
253 }
254 #[inline(always)]
255 fn is_empty(&self) -> bool {
256 self.max.x < self.min.x
257 }
258 #[inline(always)]
259 fn add_aabb(&mut self, other: &Self) {
260 self.max = self.max.max(other.max);
261 self.min = self.min.min(other.min);
262 }
263 #[inline(always)]
264 fn fast_pad(&mut self, delta: Self::Vector) {
265 self.max += delta;
266 self.min -= delta;
267 }
268 #[inline(always)]
269 fn pad(&mut self, delta: Self::Vector) {
270 let center = self.center();
271 let half_extent = (self.max - self.min) * 0.5;
272 let new_half_extent = (half_extent + delta).max(Self::Vector::ZERO);
273 self.max = center + new_half_extent;
274 self.min = center - new_half_extent;
275 }
276 #[inline(always)]
277 fn contains_point_inclusive(&self, point: Self::Vector) -> bool {
278 point.x >= self.min.x
279 && point.x <= self.max.x
280 && point.y >= self.min.y
281 && point.y <= self.max.y
282 && point.z >= self.min.z
283 && point.z <= self.max.z
284 }
285 #[inline(always)]
286 fn center(&self) -> Self::Vector {
287 (self.max + self.min) * 0.5
288 }
289 #[inline(always)]
290 fn extents(&self) -> (Self::Vector, Self::Vector, Self::Vector) {
291 (self.min, self.max, self.max - self.min)
292 }
293 #[inline(always)]
294 fn contains_aabb_inclusive(&self, other: &Self) -> bool {
295 self.min.x <= other.min.x
296 && other.max.x <= self.max.x
297 && self.min.y <= other.min.y
298 && other.max.y <= self.max.y
299 && self.min.z <= other.min.z
300 && other.max.z <= self.max.z
301 }
302 #[inline(always)]
303 fn get_plane(&self) -> Option<Plane> {
304 Plane::get_plane::<Self>(self)
305 }
306 #[inline(always)]
307 fn get_plane_relaxed(&self, epsilon: $scalar_type, max_ulps: u32) -> Option<Plane> {
308 Plane::get_plane_relaxed::<$vec_type>(self, epsilon, max_ulps)
309 }
310 #[inline(always)]
311 fn apply<F>(&mut self, f: F)
312 where
313 F: Fn(Self::Vector) -> Self::Vector,
314 {
315 if !self.is_empty() {
316 let new_min = f(self.min);
317 let new_max = f(self.max);
318 self.min = new_min.min(new_max);
319 self.max = new_max.max(new_min);
320 }
321 }
322 }
323
324 impl PartialEq for Aabb3<$scalar_type>
325 where
326 Aabb3<$scalar_type>: crate::prelude::Aabb3,
327 {
328 #[inline(always)]
329 fn eq(&self, other: &Self) -> bool {
330 crate::trait_impl::aabb3_partial_eq(self, other)
331 }
332 }
333 };
334}
335
336impl_aabb3!(nalgebra::Vector3<f64>, f64);
337impl_aabb3!(nalgebra::Vector3<f32>, f32);
338
339macro_rules! impl_nalgebra_point2 {
340 ($vec2_type:ty, $scalar_type:ty) => {
341 impl HasXY for $vec2_type {
342 type Scalar = $scalar_type;
343 #[inline(always)]
344 fn new_2d(x: Self::Scalar, y: Self::Scalar) -> Self {
345 <$vec2_type>::new(x, y)
346 }
347 #[inline(always)]
348 fn x(self) -> Self::Scalar {
349 self.x
350 }
351 #[inline(always)]
352 fn set_x(&mut self, val: Self::Scalar) {
353 self.x = val
354 }
355 #[inline(always)]
356 fn x_mut(&mut self) -> &mut Self::Scalar {
357 &mut self.x
358 }
359 #[inline(always)]
360 fn y(self) -> Self::Scalar {
361 self.y
362 }
363 #[inline(always)]
364 fn set_y(&mut self, val: Self::Scalar) {
365 self.y = val
366 }
367 #[inline(always)]
368 fn y_mut(&mut self) -> &mut Self::Scalar {
369 &mut self.y
370 }
371 }
372
373 impl Approx for $vec2_type {
374 #[inline(always)]
375 fn is_ulps_eq(
376 self,
377 other: Self,
378 epsilon: <Self::Scalar as AbsDiffEq>::Epsilon,
379 max_ulps: u32,
380 ) -> bool {
381 self.x.ulps_eq(&other.x, epsilon, max_ulps)
382 && self.y.ulps_eq(&other.y, epsilon, max_ulps)
383 }
384 #[inline(always)]
385 fn is_abs_diff_eq(
386 self,
387 other: Self,
388 epsilon: <Self::Scalar as AbsDiffEq>::Epsilon,
389 ) -> bool {
390 self.x.abs_diff_eq(&other.x, epsilon) && self.y.abs_diff_eq(&other.y, epsilon)
391 }
392 }
393 };
394}
395
396macro_rules! impl_nalgebra_vector2 {
397 ($vec2_type:ty, $vec3_type:ty, $scalar_type:ty, $affine_type:ty) => {
398 impl GenericVector2 for $vec2_type {
399 const ZERO: Self = Self::new(0.0, 0.0);
400 const ONE: Self = Self::new(1.0, 1.0);
401
402 type Vector3 = $vec3_type;
403 type Affine = $affine_type;
404 type Aabb = Aabb2<$scalar_type>;
405
406 #[inline(always)]
407 fn new(x: Self::Scalar, y: Self::Scalar) -> Self {
408 <$vec2_type>::new(x, y)
409 }
410 #[inline(always)]
411 fn splat(value: Self::Scalar) -> Self {
412 Self::new(value, value)
413 }
414 #[inline(always)]
415 fn to_3d(self, z: Self::Scalar) -> Self::Vector3 {
416 <$vec3_type>::new(self.x, self.y, z)
417 }
418 #[inline(always)]
419 fn magnitude(self) -> Self::Scalar {
420 self.norm()
421 }
422 #[inline(always)]
423 fn magnitude_sq(self) -> Self::Scalar {
424 self.norm_squared()
425 }
426 #[inline(always)]
427 fn dot(self, rhs: Self) -> Self::Scalar {
428 Self::dot(&self, &rhs)
429 }
430 #[inline(always)]
431 fn normalize(self) -> Self {
432 Self::normalize(&self)
433 }
434 #[inline(always)]
435 fn perp_dot(self, other: Self) -> Self::Scalar {
436 self.x * other.y - self.y * other.x
437 }
438 #[inline(always)]
439 fn try_normalize(self, epsilon: Self::Scalar) -> Option<Self> {
440 <$vec2_type>::try_normalize(&self, epsilon)
441 }
442 #[inline(always)]
443 fn distance(self, rhs: Self) -> Self::Scalar {
444 (self - rhs).magnitude()
445 }
446 #[inline(always)]
447 fn distance_sq(self, rhs: Self) -> Self::Scalar {
448 (self - rhs).magnitude_sq()
449 }
450 #[inline(always)]
451 fn min(self, rhs: Self) -> Self {
452 self.inf(&rhs)
453 }
454 #[inline(always)]
455 fn max(self, rhs: Self) -> Self {
456 self.sup(&rhs)
457 }
458 #[inline(always)]
460 fn clamp(self, min: Self, max: Self) -> Self {
461 self.min(max).max(min)
462 }
463 #[inline(always)]
464 fn is_finite(self) -> bool {
465 self.x.is_finite() && self.y.is_finite()
466 }
467 }
468 };
469}
470
471impl_nalgebra_point2!(nalgebra::Point2<f32>, f32);
472impl_nalgebra_point2!(nalgebra::Point2<f64>, f64);
473impl_nalgebra_point2!(nalgebra::Vector2<f32>, f32);
474impl_nalgebra_point2!(nalgebra::Vector2<f64>, f64);
475impl_nalgebra_vector2!(
476 nalgebra::Vector2<f32>,
477 nalgebra::Vector3<f32>,
478 f32,
479 nalgebra::Matrix3<f32>
480);
481impl_nalgebra_vector2!(
482 nalgebra::Vector2<f64>,
483 nalgebra::Vector3<f64>,
484 f64,
485 nalgebra::Matrix3<f64>
486);
487
488macro_rules! impl_nalgebra_hasxyz {
489 ($vec3_type:ty, $scalar_type:ty) => {
490 impl HasXY for $vec3_type {
491 type Scalar = $scalar_type;
492 #[inline(always)]
493 fn new_2d(x: Self::Scalar, y: Self::Scalar) -> Self {
494 <$vec3_type>::new(x, y, Self::Scalar::ZERO)
495 }
496 #[inline(always)]
497 fn x(self) -> Self::Scalar {
498 self.x
499 }
500 #[inline(always)]
501 fn set_x(&mut self, val: Self::Scalar) {
502 self.x = val
503 }
504 #[inline(always)]
505 fn x_mut(&mut self) -> &mut Self::Scalar {
506 &mut self.x
507 }
508 #[inline(always)]
509 fn y(self) -> Self::Scalar {
510 self.y
511 }
512 #[inline(always)]
513 fn set_y(&mut self, val: Self::Scalar) {
514 self.y = val
515 }
516 #[inline(always)]
517 fn y_mut(&mut self) -> &mut Self::Scalar {
518 &mut self.y
519 }
520 }
521
522 impl HasXYZ for $vec3_type {
523 #[inline(always)]
524 fn new_3d(x: Self::Scalar, y: Self::Scalar, z: Self::Scalar) -> Self {
525 <$vec3_type>::new(x, y, z)
526 }
527 #[inline(always)]
528 fn z(self) -> Self::Scalar {
529 self.z
530 }
531 #[inline(always)]
532 fn set_z(&mut self, val: Self::Scalar) {
533 self.z = val
534 }
535 #[inline(always)]
536 fn z_mut(&mut self) -> &mut Self::Scalar {
537 &mut self.z
538 }
539 }
540
541 impl Approx for $vec3_type {
542 #[inline(always)]
543 fn is_ulps_eq(
544 self,
545 other: Self,
546 epsilon: <Self::Scalar as AbsDiffEq>::Epsilon,
547 max_ulps: u32,
548 ) -> bool {
549 self.x.ulps_eq(&other.x, epsilon, max_ulps)
550 && self.y.ulps_eq(&other.y, epsilon, max_ulps)
551 && self.z.ulps_eq(&other.z, epsilon, max_ulps)
552 }
553 #[inline(always)]
554 fn is_abs_diff_eq(
555 self,
556 other: Self,
557 epsilon: <Self::Scalar as AbsDiffEq>::Epsilon,
558 ) -> bool {
559 self.x.abs_diff_eq(&other.x, epsilon)
560 && self.y.abs_diff_eq(&other.y, epsilon)
561 && self.z.abs_diff_eq(&other.z, epsilon)
562 }
563 }
564 };
565}
566
567macro_rules! impl_nalgebra_vector3 {
568 ($vec3_type:ty, $vec2_type:ty, $scalar_type:ty) => {
569 impl GenericVector3 for $vec3_type {
570 const ZERO: Self = Self::new(0.0, 0.0, 0.0);
571 const ONE: Self = Self::new(1.0, 1.0, 1.0);
572
573 type Vector2 = $vec2_type;
574 type Affine = nalgebra::Matrix4<$scalar_type>;
575 type Aabb = Aabb3<$scalar_type>;
576
577 #[inline(always)]
578 fn new(x: Self::Scalar, y: Self::Scalar, z: Self::Scalar) -> Self {
579 <$vec3_type>::new(x, y, z)
580 }
581 #[inline(always)]
582 fn splat(value: Self::Scalar) -> Self {
583 Self::new(value, value, value)
584 }
585 #[inline(always)]
586 fn to_2d(self) -> Self::Vector2 {
587 Self::Vector2::new(self.x, self.y)
588 }
589 #[inline(always)]
590 fn magnitude(self) -> Self::Scalar {
591 self.norm()
592 }
593 #[inline(always)]
594 fn magnitude_sq(self) -> Self::Scalar {
595 self.norm_squared()
596 }
597 #[inline(always)]
598 fn normalize(self) -> Self {
599 <$vec3_type>::normalize(&self)
600 }
601 #[inline(always)]
602 fn try_normalize(self, epsilon: Self::Scalar) -> Option<Self> {
603 <$vec3_type>::try_normalize(&self, epsilon)
604 }
605 #[inline(always)]
606 fn dot(self, rhs: Self) -> Self::Scalar {
607 <$vec3_type>::dot(&self, &rhs)
608 }
609 #[inline(always)]
610 fn cross(self, rhs: Self) -> Self {
611 <$vec3_type>::cross(&self, &rhs)
612 }
613 #[inline(always)]
614 fn distance(self, rhs: Self) -> Self::Scalar {
615 (self - rhs).magnitude()
616 }
617 #[inline(always)]
618 fn distance_sq(self, rhs: Self) -> Self::Scalar {
619 (self - rhs).magnitude_squared()
620 }
621 #[inline(always)]
622 fn min(self, rhs: Self) -> Self {
623 self.inf(&rhs)
624 }
625 #[inline(always)]
626 fn max(self, rhs: Self) -> Self {
627 self.sup(&rhs)
628 }
629 #[inline(always)]
631 fn clamp(self, min: Self, max: Self) -> Self {
632 self.min(max).max(min)
633 }
634 #[inline(always)]
635 fn is_finite(self) -> bool {
636 self.x.is_finite() && self.y.is_finite() && self.z.is_finite()
637 }
638 }
639 };
640}
641
642impl_nalgebra_hasxyz!(nalgebra::Point3<f32>, f32);
643impl_nalgebra_hasxyz!(nalgebra::Point3<f64>, f64);
644impl_nalgebra_hasxyz!(nalgebra::Vector3<f32>, f32);
645impl_nalgebra_hasxyz!(nalgebra::Vector3<f64>, f64);
646impl_nalgebra_vector3!(nalgebra::Vector3<f32>, nalgebra::Vector2<f32>, f32);
647impl_nalgebra_vector3!(nalgebra::Vector3<f64>, nalgebra::Vector2<f64>, f64);
648
649macro_rules! impl_matrix3 {
650 ($mat_type:ty, $vec_type:ty, $scalar_type:ty) => {
651 impl Affine2D for $mat_type
652 where
653 $vec_type: GenericVector2<Scalar = $scalar_type> + HasXY,
654 {
655 type Vector2 = $vec_type;
656
657 #[inline(always)]
658 fn from_cols_array(array: &[$scalar_type; 9]) -> Self {
659 <$mat_type>::from_column_slice(array)
660 }
661
662 #[inline(always)]
663 fn identity() -> Self {
664 <$mat_type>::identity()
665 }
666
667 #[inline(always)]
668 fn transform_vector2(&self, vec: $vec_type) -> $vec_type {
669 <$mat_type>::transform_vector(self, &vec)
670 }
671
672 #[inline(always)]
673 fn transform_point2(&self, point: $vec_type) -> $vec_type {
674 let point2 = nalgebra::Point2::new(point.x, point.y);
675 <$mat_type>::transform_point(self, &point2).coords
676 }
677
678 #[inline(always)]
679 fn try_inverse(&self) -> Option<Self> {
680 <$mat_type>::try_inverse(*self)
681 }
682 }
683 };
684}
685
686impl_matrix3!(nalgebra::Matrix3<f32>, nalgebra::Vector2<f32>, f32);
687impl_matrix3!(nalgebra::Matrix3<f64>, nalgebra::Vector2<f64>, f64);
688
689macro_rules! impl_matrix4 {
690 ($mat_type:ty, $vec_type:ty, $scalar_type:ty) => {
691 impl Affine3D for $mat_type
692 where
693 $vec_type: GenericVector3<Scalar = $scalar_type> + HasXY,
694 {
695 type Vector3 = $vec_type;
696
697 #[inline(always)]
698 fn from_cols_array(array: &[$scalar_type; 16]) -> Self {
699 <$mat_type>::from_column_slice(array)
700 }
701
702 #[inline(always)]
703 fn identity() -> Self {
704 <$mat_type>::identity()
705 }
706
707 #[inline(always)]
708 fn transform_vector3(&self, vec: $vec_type) -> $vec_type {
709 <$mat_type>::transform_vector(self, &vec)
710 }
711
712 #[inline(always)]
713 fn transform_point3(&self, point: $vec_type) -> $vec_type {
714 let point3 = nalgebra::Point3::new(point.x, point.y, point.z);
715 <$mat_type>::transform_point(self, &point3).coords
716 }
717
718 #[inline(always)]
719 fn try_inverse(&self) -> Option<Self> {
720 <$mat_type>::try_inverse(*self)
721 }
722
723 #[inline(always)]
724 fn from_plane_to_xy(source_plane: Plane) -> Self {
725 crate::trait_impl::from_plane_to_xy::<nalgebra::Vector3<$scalar_type>>(source_plane)
726 }
727
728 #[inline(always)]
729 fn from_translation(translation: Self::Vector3) -> Self {
730 crate::trait_impl::from_translation::<nalgebra::Vector3<$scalar_type>>(translation)
731 }
732
733 #[inline(always)]
734 fn from_scale(scale: Self::Vector3) -> Self {
735 <$mat_type>::new_nonuniform_scaling(&scale)
736 }
737 }
738 };
739}
740
741impl_matrix4!(nalgebra::Matrix4<f32>, nalgebra::Vector3<f32>, f32);
742impl_matrix4!(nalgebra::Matrix4<f64>, nalgebra::Vector3<f64>, f64);