From 6a7ccd8c0a0e64cd04741bbd7c86da0bccfa7490 Mon Sep 17 00:00:00 2001 From: gingerBill Date: Sat, 28 Dec 2019 18:12:27 +0000 Subject: [PATCH] Add new procedures for `package math`: `atan2`, `asin`, `acos`, `atan`, `sin_bit`, `ldexp` --- core/math/linalg/linalg.odin | 101 +++++++++++++++++----- core/math/math.odin | 162 ++++++++++++++++++++++++++++++++++- 2 files changed, 240 insertions(+), 23 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.. Matrix4 { +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}; + + +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 :: 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); +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_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;