diff --git a/gdnative-core/src/geom/basis.rs b/gdnative-core/src/geom/basis.rs index e693bf6b2..b7562baa9 100644 --- a/gdnative-core/src/geom/basis.rs +++ b/gdnative-core/src/geom/basis.rs @@ -29,6 +29,67 @@ impl Basis { } } + pub fn invert(&mut self) { + let co = vec![ + self.elements[1].y * self.elements[2].z - self.elements[1].z * self.elements[2].y, + self.elements[1].z * self.elements[2].x - self.elements[1].x * self.elements[2].z, + self.elements[1].x * self.elements[2].y - self.elements[1].y * self.elements[2].x, + ]; + + let det = + self.elements[0].x * co[0] + self.elements[0].y * co[1] + self.elements[0].z * co[2]; + + let s = 1.0 / det; + + self.set_x(Vector3::new( + co[0] * s, + (self.elements[0].z * self.elements[2].y - self.elements[0].y * self.elements[2].z) * s, + (self.elements[0].y * self.elements[1].z - self.elements[0].z * self.elements[1].y) * s, + )); + + self.set_y(Vector3::new( + co[1] * s, + (self.elements[0].x * self.elements[2].z - self.elements[0].z * self.elements[2].x) * s, + (self.elements[0].z * self.elements[1].x - self.elements[0].x * self.elements[1].z) * s, + )); + + self.set_z(Vector3::new( + co[2] * s, + (self.elements[0].y * self.elements[2].x - self.elements[0].x * self.elements[2].y) * s, + (self.elements[0].x * self.elements[1].y - self.elements[0].y * self.elements[1].x) * s, + )); + } + + pub fn inverse(&self) -> Basis { + let mut inv = *self; + inv.invert(); + inv + } + + pub fn transpose(&mut self) { + std::mem::swap(&mut self.elements[0].y, &mut self.elements[1].x); + std::mem::swap(&mut self.elements[0].z, &mut self.elements[2].x); + std::mem::swap(&mut self.elements[1].z, &mut self.elements[2].y); + } + + pub fn transposed(&self) -> Basis { + let mut tr = *self; + tr.transpose(); + tr + } + + pub fn flip_x() -> Basis { + Basis::from_diagonal(Vector3::new(-1.0, 1.0, 1.0)) + } + + pub fn flip_y() -> Basis { + Basis::from_diagonal(Vector3::new(1.0, -1.0, 1.0)) + } + + pub fn flip_z() -> Basis { + Basis::from_diagonal(Vector3::new(1.0, 1.0, -1.0)) + } + pub fn from_diagonal(p_diag: Vector3) -> Basis { Basis { elements: [