Add new procedures for `package math`: `atan2`, `asin`, `acos`, `atan`, `sin_bit`, `ldexp`

2 files changed, 239 insertions, 22 deletions

diff --git a/core/math/linalg/linalg.odin b/core/math/linalg/linalg.odin
index 066d72b19..1220b952e 100644
--- a/core/math/linalg/linalg.odin
+++ b/core/math/linalg/linalg.odin
@@ -64,6 +64,10 @@ length :: proc(v: $T/[$N]$E) -> E {
 	return math.sqrt(dot(v, v));
 }
 
+length2 :: proc(v: $T/[$N]$E) -> E {
+	return dot(v, v);
+}
+
 
 identity :: proc($T: typeid/[$N][N]$E) -> (m: T) {
 	for i in 0..<N do m[i][i] = E(1);
@@ -176,17 +180,51 @@ Matrix4x2 :: distinct [4][2]Float;
 Matrix4x3 :: distinct [4][3]Float;
 Matrix4x4 :: distinct [4][4]Float;
 
-
 Matrix1 :: Matrix1x1;
 Matrix2 :: Matrix2x2;
 Matrix3 :: Matrix3x3;
 Matrix4 :: Matrix4x4;
 
-
 Quaternion :: distinct (size_of(Float) == size_of(f32) ? quaternion128 : quaternion256);
 
+MATRIX1_IDENTITY :: Matrix1{{1}};
+MATRIX2_IDENTITY :: Matrix2{{1, 0}, {0, 1}};
+MATRIX3_IDENTITY :: Matrix3{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+MATRIX4_IDENTITY :: Matrix4{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
+
+QUATERNION_IDENTITY :: Quaternion(1);
+
+VECTOR3_X_AXIS :: Vector3{1, 0, 0};
+VECTOR3_Y_AXIS :: Vector3{0, 1, 0};
+VECTOR3_Z_AXIS :: Vector3{0, 0, 1};
+
 
-translate_matrix4 :: proc(v: Vector3) -> Matrix4 {
+vector3_orthogonal :: proc(v: Vector3) -> Vector3 {
+	x := abs(v.x);
+	y := abs(v.y);
+	z := abs(v.z);
+
+	other: Vector3 = x < y ? (x < z ? {1, 0, 0} : {0, 0, 1}) : (y < z ? {0, 1, 0} : {0, 0, 1});
+
+	return normalize(cross3(v, other));
+}
+
+vector3_reflect :: proc(i, n: Vector3) -> Vector3 {
+	b := n * 2 * dot(n, i);
+	return i - b;
+}
+
+vector3_refract :: proc(i, n: Vector3, eta: Float) -> Vector3 {
+	dv := dot(n, i);
+	k := 1 - eta*eta - (1 - dv*dv);
+	a := i * eta;
+	b := n * eta*dv*math.sqrt(k);
+	return (a - b) * Float(int(k >= 0));
+}
+
+
+translate_matrix4 :: matrix4_translate;
+matrix4_translate :: proc(v: Vector3) -> Matrix4 {
 	m := identity(Matrix4);
 	m[3][0] = v[0];
 	m[3][1] = v[1];
@@ -195,7 +233,8 @@ translate_matrix4 :: proc(v: Vector3) -> Matrix4 {
 }
 
 
-rotate_matrix4 :: proc(v: Vector3, angle_radians: Float) -> Matrix4 {
+rotate_matrix4 :: matrix4_rotate;
+matrix4_rotate :: proc(v: Vector3, angle_radians: Float) -> Matrix4 {
 	c := math.cos(angle_radians);
 	s := math.sin(angle_radians);
 
@@ -222,7 +261,8 @@ rotate_matrix4 :: proc(v: Vector3, angle_radians: Float) -> Matrix4 {
 	return rot;
 }
 
-scale_matrix4 :: proc(m: Matrix4, v: Vector3) -> Matrix4 {
+scale_matrix4 :: matrix4_scale;
+matrix4_scale :: proc(m: Matrix4, v: Vector3) -> Matrix4 {
 	mm := m;
 	mm[0][0] *= v[0];
 	mm[1][1] *= v[1];
@@ -230,8 +270,8 @@ scale_matrix4 :: proc(m: Matrix4, v: Vector3) -> Matrix4 {
 	return mm;
 }
 
-
-look_at :: proc(eye, centre, up: Vector3) -> Matrix4 {
+look_at :: matrix4_look_at;
+matrix4_look_at :: proc(eye, centre, up: Vector3) -> Matrix4 {
 	f := normalize(centre - eye);
 	s := normalize(cross(f, up));
 	u := cross(s, f);
@@ -244,7 +284,8 @@ look_at :: proc(eye, centre, up: Vector3) -> Matrix4 {
 }
 
 
-perspective :: proc(fovy, aspect, near, far: Float) -> (m: Matrix4) {
+perspective :: matrix4_perspective;
+matrix4_perspective :: proc(fovy, aspect, near, far: Float) -> (m: Matrix4) {
 	tan_half_fovy := math.tan(0.5 * fovy);
 	m[0][0] = 1 / (aspect*tan_half_fovy);
 	m[1][1] = 1 / (tan_half_fovy);
@@ -255,7 +296,7 @@ perspective :: proc(fovy, aspect, near, far: Float) -> (m: Matrix4) {
 }
 
 
-ortho3d :: proc(left, right, bottom, top, near, far: Float) -> (m: Matrix4) {
+matrix_ortho3d :: proc(left, right, bottom, top, near, far: Float) -> (m: Matrix4) {
 	m[0][0] = +2 / (right - left);
 	m[1][1] = +2 / (top - bottom);
 	m[2][2] = -2 / (far - near);
@@ -267,23 +308,41 @@ ortho3d :: proc(left, right, bottom, top, near, far: Float) -> (m: Matrix4) {
 }
 
 
-axis_angle :: proc(axis: Vector3, angle_radians: Float) -> Quaternion {
+axis_angle :: quaternion_angle_axis;
+angle_axis :: quaternion_angle_axis;
+quaternion_angle_axis :: proc(angle_radians: Float, axis: Vector3) -> Quaternion {
 	t := angle_radians*0.5;
 	w := math.cos(t);
 	v := normalize(axis) * math.sin(t);
 	return quaternion(w, v.x, v.y, v.z);
 }
 
-angle_axis :: proc(angle_radians: Float, axis: Vector3) -> Quaternion {
-	t := angle_radians*0.5;
-	w := math.cos(t);
-	v := normalize(axis) * math.sin(t);
-	return quaternion(w, v.x, v.y, v.z);
+euler_angles :: quaternion_from_euler_angles;
+quaternion_from_euler_angles :: proc(pitch, yaw, roll: Float) -> Quaternion {
+	p := quaternion_angle_axis(pitch, {1, 0, 0});
+	y := quaternion_angle_axis(yaw,   {0, 1, 0});
+	r := quaternion_angle_axis(roll,  {0, 0, 1});
+	return (y * p) * r;
 }
 
-euler_angles :: proc(pitch, yaw, roll: Float) -> Quaternion {
-	p := axis_angle({1, 0, 0}, pitch);
-	y := axis_angle({0, 1, 0}, yaw);
-	r := axis_angle({0, 0, 1}, roll);
-	return (y * p) * r;
+euler_angles_from_quaternion :: proc(q: Quaternion) -> (roll, pitch, yaw: Float) {
+	// roll (x-axis rotation)
+	sinr_cosp: Float = 2 * (real(q)*imag(q) + jmag(q)*kmag(q));
+	cosr_cosp: Float = 1 - 2 * (imag(q)*imag(q) + jmag(q)*jmag(q));
+	roll = Float(math.atan2(sinr_cosp, cosr_cosp));
+
+	// pitch (y-axis rotation)
+	sinp: Float = 2 * (real(q)*kmag(q) - kmag(q)*imag(q));
+	if abs(sinp) >= 1 {
+		pitch = Float(math.copy_sign(math.TAU * 0.25, sinp));
+	} else {
+		pitch = Float(math.asin(sinp));
+	}
+
+	// yaw (z-axis rotation)
+	siny_cosp: Float = 2 * (real(q)*kmag(q) + imag(q)*jmag(q));
+	cosy_cosp: Float = 1 - 2 * (jmag(q)*jmag(q) + kmag(q)*kmag(q));
+	yaw = Float(math.atan2(siny_cosp, cosy_cosp));
+
+	return;
 }
diff --git a/core/math/math.odin b/core/math/math.odin
index 1acf63171..b56434428 100644
--- a/core/math/math.odin
+++ b/core/math/math.odin
@@ -71,6 +71,12 @@ foreign _ {
 	exp_f32 :: proc(x: f32) -> f32 ---;
 	@(link_name="llvm.exp.f64")
 	exp_f64 :: proc(x: f64) -> f64 ---;
+
+	@(link_name="llvm.ldexp.f32")
+	ldexp_f32 :: proc(val: f32, exp: i32) -> f32 ---;
+
+	@(link_name="llvm.ldexp.f64")
+	ldexp_f64 :: proc(val: f64, exp: i32) -> f64 ---;
 }
 
 sqrt      :: proc{sqrt_f32, sqrt_f64};
@@ -81,6 +87,8 @@ fmuladd   :: proc{fmuladd_f32, fmuladd_f64};
 ln        :: proc{ln_f32, ln_f64};
 exp       :: proc{exp_f32, exp_f64};
 
+ldexp :: proc{ldexp_f32, ldexp_f64};
+
 log_f32 :: proc(x, base: f32) -> f32 { return ln(x) / ln(base); }
 log_f64 :: proc(x, base: f64) -> f64 { return ln(x) / ln(base); }
 log     :: proc{log_f32, log_f64};
@@ -108,6 +116,14 @@ sign_f32 :: proc(x: f32) -> f32 { return f32(int(0 < x) - int(x < 0)); }
 sign_f64 :: proc(x: f64) -> f64 { return f64(int(0 < x) - int(x < 0)); }
 sign     :: proc{sign_f32, sign_f64};
 
+sign_bit_f32 :: proc(x: f32) -> bool {
+	return (transmute(u32)x) & (1<<31) != 0;
+}
+sign_bit_f64 :: proc(x: f64) -> bool {
+	return (transmute(u64)x) & (1<<63) != 0;
+}
+sign_bit :: proc{sign_bit_f32, sign_bit_f64};
+
 copy_sign_f32 :: proc(x, y: f32) -> f32 {
 	ix := transmute(u32)x;
 	iy := transmute(u32)y;
@@ -511,8 +527,31 @@ is_nan_f32 :: proc(x: f32) -> bool { return classify(x) == .NaN; }
 is_nan_f64 :: proc(x: f64) -> bool { return classify(x) == .NaN; }
 is_nan :: proc{is_nan_f32, is_nan_f64};
 
-is_inf_f32 :: proc(x: f32) -> bool { return classify(abs(x)) == .Inf; }
-is_inf_f64 :: proc(x: f64) -> bool { return classify(abs(x)) == .Inf; }
+
+// is_inf reports whether f is an infinity, according to sign.
+// If sign > 0, is_inf reports whether f is positive infinity.
+// If sign < 0, is_inf reports whether f is negative infinity.
+// If sign == 0, is_inf reports whether f is either infinity.
+is_inf_f32 :: proc(x: f32, sign: int = 0) -> bool {
+	class := classify(abs(x));
+	switch {
+	case sign > 0:
+		return class == .Inf;
+	case sign < 0:
+		return class == .Neg_Inf;
+	}
+	return class == .Inf || class == .Neg_Inf;
+}
+is_inf_f64 :: proc(x: f64, sign: int = 0) -> bool {
+	class := classify(abs(x));
+	switch {
+	case sign > 0:
+		return class == .Inf;
+	case sign < 0:
+		return class == .Neg_Inf;
+	}
+	return class == .Inf || class == .Neg_Inf;
+}
 is_inf :: proc{is_inf_f32, is_inf_f64};
 
 
@@ -572,6 +611,125 @@ cumsum :: proc(dst, src: $T/[]$E) -> T
 }
 
 
+
+atan2_f32 :: proc(y, x: f32) -> f32 {
+	// TODO(bill): Better atan2_f32
+	return f32(atan2_f64(f64(y), f64(x)));
+}
+
+atan2_f64 :: proc(y, x: f64) -> f64 {
+	// TODO(bill): Faster atan2_f64 if possible
+
+	// The original C code:
+	//   Stephen L. Moshier
+	//   moshier@na-net.ornl.gov
+
+	NAN :: 0h7fff_ffff_ffff_ffff;
+	INF :: 0h7FF0_0000_0000_0000;
+	PI  :: 0h4009_21fb_5444_2d18;
+
+	atan :: proc(x: f64) -> f64 {
+		if x == 0 {
+			return x;
+		}
+		if x > 0 {
+			return s_atan(x);
+		}
+		return -s_atan(-x);
+	}
+	// s_atan reduces its argument (known to be positive) to the range [0, 0.66] and calls x_atan.
+	s_atan :: proc(x: f64) -> f64 {
+		MORE_BITS :: 6.123233995736765886130e-17; // pi/2 = PIO2 + MORE_BITS
+		TAN3PI08  :: 2.41421356237309504880;      // tan(3*pi/8)
+		if x <= 0.66 {
+			return x_atan(x);
+		}
+		if x > TAN3PI08 {
+			return PI/2 - x_atan(1/x) + MORE_BITS;
+		}
+		return PI/4 + x_atan((x-1)/(x+1)) + 0.5*MORE_BITS;
+	}
+	// x_atan evaluates a series valid in the range [0, 0.66].
+	x_atan :: proc(x: f64) -> f64 {
+		P0 :: -8.750608600031904122785e-01;
+		P1 :: -1.615753718733365076637e+01;
+		P2 :: -7.500855792314704667340e+01;
+		P3 :: -1.228866684490136173410e+02;
+		P4 :: -6.485021904942025371773e+01;
+		Q0 :: +2.485846490142306297962e+01;
+		Q1 :: +1.650270098316988542046e+02;
+		Q2 :: +4.328810604912902668951e+02;
+		Q3 :: +4.853903996359136964868e+02;
+		Q4 :: +1.945506571482613964425e+02;
+
+		z := x * x;
+		z = z * ((((P0*z+P1)*z+P2)*z+P3)*z + P4) / (((((z+Q0)*z+Q1)*z+Q2)*z+Q3)*z + Q4);
+		z = x*z + x;
+		return z;
+	}
+
+	switch {
+	case is_nan(y) || is_nan(x):
+		return NAN;
+	case y == 0:
+		if x >= 0 && !sign_bit(x) {
+			return copy_sign(0.0, y);
+		}
+		return copy_sign(PI, y);
+	case x == 0:
+		return copy_sign(PI*0.5, y);
+	case is_inf(x, 0):
+		if is_inf(x, 1) {
+			if is_inf(y, 0) {
+				return copy_sign(PI*0.25, y);
+			}
+			return copy_sign(0, y);
+		}
+		if is_inf(y, 0) {
+			return copy_sign(PI*0.75, y);
+		}
+		return copy_sign(PI, y);
+	case is_inf(y, 0):
+		return copy_sign(PI*0.5, y);
+	}
+
+	q := atan(y / x);
+	if x < 0 {
+		if q <= 0 {
+			return q + PI;
+		}
+		return q - PI;
+	}
+	return q;
+}
+
+
+atan2 :: proc{atan2_f32, atan2_f64};
+
+atan_f32 :: proc(x: f32) -> f32 {
+	return atan2_f32(1.0, x);
+}
+atan :: proc{atan_f32};
+
+
+asin_f32 :: proc(x: f32) -> f32 {
+	return atan2_f32(x, sqrt_f32(1 - x*x));
+}
+asin_f64 :: proc(x: f64) -> f64 {
+	return atan2_f64(x, sqrt_f64(1 - x*x));
+}
+asin :: proc{asin_f32};
+
+acos_f32 :: proc(x: f32) -> f32 {
+	return atan2_f32(sqrt_f32(1 - x), sqrt_f32(1 + x));
+}
+acos_f64 :: proc(x: f64) -> f64 {
+	return atan2_f64(sqrt_f64(1 - x), sqrt_f64(1 + x));
+}
+acos :: proc{acos_f32};
+
+
+
 F32_DIG        :: 6;
 F32_EPSILON    :: 1.192092896e-07;
 F32_GUARD      :: 0;