Quaternion math improvements (#2524)

* Add radians to deg function

* Quaternion math fixes

* Formatting, use splat/swizzling, divide into multiple tests.

---------

Co-authored-by: tonis2 <tanton@paysure.solutions>
Co-authored-by: Christoffer Lerno <christoffer@aegik.com>

lib/std/math/math.c3

@@ -73,6 +73,11 @@ faultdef OVERFLOW, MATRIX_INVERSE_DOESNT_EXIST;
 *>
 macro deg_to_rad(x) => x * PI / 180;
 
+<*
+ @require types::is_numerical($typeof(x)) : `The input must be a numerical value or numerical vector`
+*>
+macro rad_to_deg(x) => x * 180 / PI;
+
 <*
  @require types::is_numerical($typeof(x)) : `The input must be a numerical value or numerical vector`
 *>

lib/std/math/quaternion.c3

@@ -32,12 +32,15 @@ macro QuaternionNumber QuaternionNumber.sub(self, QuaternionNumber b) @operator(
 macro QuaternionNumber QuaternionNumber.negate(self) @operator(-) => { .v = -self.v };
 macro QuaternionNumber QuaternionNumber.sub_each(self, Real b) => { .v = self.v - b };
 macro QuaternionNumber QuaternionNumber.scale(self, Real s) @operator_s(*) => { .v = self.v * s };
+macro QuaternionNumber.to_angle(self) => 2 * math::acos(self.v.w);
 macro QuaternionNumber QuaternionNumber.normalize(self) => { .v = self.v.normalize() };
 macro Real QuaternionNumber.length(self) => self.v.length();
 macro QuaternionNumber QuaternionNumber.lerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount) };
+fn QuaternionNumber QuaternionNumber.nlerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount).normalize() };
+
 macro Matrix4f QuaternionNumber.to_matrixf(&self) => into_matrix(self, Matrix4f);
 macro Matrix4 QuaternionNumber.to_matrix(&self) => into_matrix(self, Matrix4);
-fn QuaternionNumber QuaternionNumber.nlerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount).normalize() };
+
 
 fn QuaternionNumber QuaternionNumber.invert(self)
 {
@@ -47,6 +50,8 @@ fn QuaternionNumber QuaternionNumber.invert(self)
 	return { self.v[0] * -inv_length, self.v[1] * -inv_length, self.v[2] * -inv_length, self.v[3] * inv_length };
 }
 
+fn QuaternionNumber QuaternionNumber.conjugate(&self) => { -self.v.x, -self.v.y, -self.v.z, self.v.w };
+
 fn QuaternionNumber QuaternionNumber.slerp(self, QuaternionNumber q2, Real amount)
 {
 	QuaternionNumber result = {};
@@ -77,13 +82,35 @@ fn QuaternionNumber QuaternionNumber.slerp(self, QuaternionNumber q2, Real amoun
 }
 
 fn QuaternionNumber QuaternionNumber.mul(self, QuaternionNumber b) @operator(*)
-{
-	return { self.i * b.l + self.l * b.i + self.j * b.k - self.k * b.j,
-			 self.j * b.l + self.l * b.j + self.k * b.i - self.i * b.k,
-			 self.k * b.l + self.l * b.k + self.i * b.j - self.j * b.i,
-			 self.l * b.l - self.i * b.i - self.j * self.j - self.k * self.k };
+{  
+	Real[<3>] q1_axis = { self.v.x, self.v.y, self.v.z };
+	Real[<3>] q2_axis = { b.v.x, b.v.y, b.v.z };
+
+	Real scalar = (self.v.w * b.v.w - q1_axis.dot(q2_axis));
+	Real[<3>] axis = self.v.w * q2_axis + b.v.w * q1_axis + q1_axis.cross(q2_axis);
+
+	return { ...axis, scalar };
 }
 
+
+fn QuaternionNumber from_axis_angle(Real[<3>] axis, Real angle)
+{
+	Real[<3>] normal_axis = axis.normalize();
+	Real half_angle = angle * 0.5;
+	Real sin_half = math::sin(half_angle);
+
+	return { ...(normal_axis * sin_half), math::cos(half_angle) };
+}
+
+fn Real[<3>] QuaternionNumber.rotate_vec3(self, Real[<3>] vector) @operator(*)
+{
+	QuaternionNumber p = { ...vector, 0 };
+	QuaternionNumber result = self * p * self.conjugate();
+	return result.v.xyz;
+}
+
+
+
 macro into_matrix(QuaternionNumber* q, $Type) @private
 {
     QuaternionNumber rotation = q.normalize();
@@ -93,9 +120,9 @@ macro into_matrix(QuaternionNumber* q, $Type) @private
     var w = rotation.l;
 
     return ($Type) {
-        1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w, 0,
-        2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w, 0,
-        2*x*z - 2*y*w, 2*y*z + 2*x*w , 1 - 2*x*x - 2*y*y, 0,
-        0.0, 0.0, 0.0, 1.0,
+        1 - 2*y*y - 2*z*z,     2*x*y - 2*z*w,     2*x*z + 2*y*w,   0,
+            2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z,     2*y*z - 2*x*w,   0,
+            2*x*z - 2*y*w,     2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y,   0,
+                      0.0,               0.0,               0.0, 1.0,
     };
 }

lib/std/math/vector.c3

@@ -41,8 +41,8 @@ fn double double[<3>].angle(self, double[<3>] v2) => angle3(self, v2);
 fn float[<3>] float[<3>].refract(self, float[<3>] n, float r) => refract3(self, n, r);
 fn double[<3>] double[<3>].refract(self, double[<3>] n, double r) => refract3(self, n, r);
 
-fn float[<3>] float[<3>].rotate_quat(self, Quaternionf q) => rotate_by_quat3(self, q);
-fn double[<3>] double[<3>].rotate_quat(self, Quaternion q) => rotate_by_quat3(self, q);
+fn float[<3>] float[<3>].rotate_quat(self, Quaternionf q) => q * self;
+fn double[<3>] double[<3>].rotate_quat(self, Quaternion q) => q * self;
 
 fn float[<3>] float[<3>].rotate_axis(self, float[<3>] axis, float angle) => rotate_axis_angle(self, axis, angle);
 fn double[<3>] double[<3>].rotate_axis(self, double[<3>] axis, double angle) => rotate_axis_angle(self, axis, angle);
@@ -144,21 +144,6 @@ macro void ortho_normalize3(v1, v2) @private
 	*v2 = v1n.cross(vn1);
 }
 
-macro rotate_by_quat3(v, q) @private
-{
-	return ($typeof(v)){
-		v[0] * (q.i * q.i + q.l * q.l - q.j * q.j - q.k * q.k)
-			+ v[1] * (2 * q.i * q.j - 2 * q.l * q.k)
-			+ v[2] * (2 * q.i * q.k - 2 * q.l * q.j),
-		v[0] * (2 * q.l * q.k + 2 * q.i * q.j)
-			+ v[1] * (q.l * q.l - q.i * q.i + q.j * q.j - q.k * q.k)
-			+ v[2] * (-2 * q.l * q.i + 2 * q.j * q.k),
-		v[0] * (-2 * q.l * q.j + 2 * q.i * q.k)
-			+ v[1] * (2 * q.l * q.i + 2 * q.j * q.k)
-			+ v[2] * (q.l * q.l - q.i * q.i - q.j * q.j + q.k * q.k)
-	};
-}
-
 macro rotate_axis_angle(v, axis, angle) @private
 {
 	axis = axis.normalize();