From 16a7c55334fafa48d5fd84e86071263f43a3724f Mon Sep 17 00:00:00 2001 From: gingerBill Date: Wed, 1 Jan 2020 16:14:00 +0000 Subject: [PATCH] Add `x y z w` fields to quaternion types; Improve linalg quaternion mathematics --- core/math/linalg/general.odin | 4 +- core/math/linalg/specific.odin | 206 +++++++++++++++++++++++---------- src/check_expr.cpp | 42 +++++++ src/ir.cpp | 21 +++- src/types.cpp | 88 ++++++++++++++ 5 files changed, 299 insertions(+), 62 deletions(-) diff --git a/core/math/linalg/general.odin b/core/math/linalg/general.odin index f938c7631..c24d23acf 100644 --- a/core/math/linalg/general.odin +++ b/core/math/linalg/general.odin @@ -17,10 +17,10 @@ vector_dot :: proc(a, b: $T/[$N]$E) -> (c: E) where IS_NUMERIC(E) { return; } quaternion128_dot :: proc(a, b: $T/quaternion128) -> (c: f32) { - return real(a)*real(a) + imag(a)*imag(b) + jmag(a)*jmag(b) + kmag(a)*kmag(b); + return a.w*a.w + a.x*b.x + a.y*b.y + a.z*b.z; } quaternion256_dot :: proc(a, b: $T/quaternion256) -> (c: f64) { - return real(a)*real(a) + imag(a)*imag(b) + jmag(a)*jmag(b) + kmag(a)*kmag(b); + return a.w*a.w + a.x*b.x + a.y*b.y + a.z*b.z; } dot :: proc{vector_dot, quaternion128_dot, quaternion256_dot}; diff --git a/core/math/linalg/specific.odin b/core/math/linalg/specific.odin index 77cdf8ead..055c112bb 100644 --- a/core/math/linalg/specific.odin +++ b/core/math/linalg/specific.odin @@ -172,72 +172,93 @@ quaternion_angle_axis :: proc(angle_radians: Float, axis: Vector3) -> Quaternion return quaternion(w, v.x, v.y, v.z); } -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; + +quaternion_from_euler_angles :: proc(roll, pitch, yaw: Float) -> Quaternion { + x, y, z := roll, pitch, yaw; + a, b, c := x, y, z; + + ca, sa := math.cos(a*0.5), math.sin(a*0.5); + cb, sb := math.cos(b*0.5), math.sin(b*0.5); + cc, sc := math.cos(c*0.5), math.sin(c*0.5); + + q: Quaternion; + q.x = sa*cb*cc - ca*sb*sc; + q.y = ca*sb*cc + sa*cb*sc; + q.z = ca*cb*sc - sa*sb*cc; + q.w = ca*cb*cc + sa*sb*sc; + return q; } 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)); + // roll, x-axis rotation + sinr_cosp: Float = 2 * (q.w * q.x + q.y * q.z); + cosr_cosp: Float = 1 - 2 * (q.x * q.x + q.y * q.y); + roll = 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)); - } + // pitch, y-axis rotation + sinp: Float = 2 * (q.w * q.y - q.z * q.x); + if abs(sinp) >= 1 { + pitch = math.copy_sign(math.TAU * 0.25, sinp); + } else { + pitch = 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)); + // yaw, z-axis rotation + siny_cosp: Float = 2 * (q.w * q.z + q.x * q.y); + cosy_cosp: Float = 1 - 2 * (q.y * q.y + q.z * q.z); + yaw = math.atan2(siny_cosp, cosy_cosp); - return; + return; } -quaternion_look_at :: proc(eye, centre: Vector3, up: Vector3) -> Quaternion { - m := matrix3_look_at(eye, centre, up); +quaternion_from_forward_and_up :: proc(forward, up: Vector3) -> Quaternion { + f := normalize(forward); + s := normalize(cross(f, up)); + u := cross(s, f); + m := Matrix3{ + {+s.x, +u.x, -f.x}, + {+s.y, +u.y, -f.y}, + {+s.z, +u.z, -f.z}, + }; + tr := trace(m); - w, x, y, z: Float; + q: Quaternion; switch { case tr > 0: S := 2 * math.sqrt(1 + tr); - w = 0.25 * S; - x = (m[2][1] - m[1][2]) / S; - y = (m[0][2] - m[2][0]) / S; - z = (m[1][0] - m[0][1]) / S; + q.w = 0.25 * S; + q.x = (m[2][1] - m[1][2]) / S; + q.y = (m[0][2] - m[2][0]) / S; + q.z = (m[1][0] - m[0][1]) / S; case (m[0][0] > m[1][1]) && (m[0][0] > m[2][2]): S := 2 * math.sqrt(1 + m[0][0] - m[1][1] - m[2][2]); - w = (m[2][1] - m[1][2]) / S; - x = 0.25 * S; - y = (m[0][1] + m[1][0]) / S; - z = (m[0][2] + m[2][0]) / S; + q.w = (m[2][1] - m[1][2]) / S; + q.x = 0.25 * S; + q.y = (m[0][1] + m[1][0]) / S; + q.z = (m[0][2] + m[2][0]) / S; case m[1][1] > m[2][2]: S := 2 * math.sqrt(1 + m[1][1] - m[0][0] - m[2][2]); - w = (m[0][2] - m[2][0]) / S; - x = (m[0][1] + m[1][0]) / S; - y = 0.25 * S; - z = (m[1][2] + m[2][1]) / S; + q.w = (m[0][2] - m[2][0]) / S; + q.x = (m[0][1] + m[1][0]) / S; + q.y = 0.25 * S; + q.z = (m[1][2] + m[2][1]) / S; case: S := 2 * math.sqrt(1 + m[2][2] - m[0][0] - m[1][1]); - w = (m[1][0] - m[0][1]) / S; - x = (m[0][2] - m[2][0]) / S; - y = (m[1][2] + m[2][1]) / S; - z = 0.25 * S; + q.w = (m[1][0] - m[0][1]) / S; + q.x = (m[0][2] - m[2][0]) / S; + q.y = (m[1][2] + m[2][1]) / S; + q.z = 0.25 * S; } - q: Quaternion = quaternion(w, x, y, z); return normalize(q); } +quaternion_look_at :: proc(eye, centre: Vector3, up: Vector3) -> Quaternion { + return quaternion_from_forward_and_up(centre-eye, up); +} + quaternion_nlerp :: proc(a, b: Quaternion, t: Float) -> Quaternion { c := a + (b-a)*quaternion(t, 0, 0, 0); @@ -335,6 +356,73 @@ quaternion_from_matrix4 :: proc(m: Matrix4) -> Quaternion { } +quaternion_from_matrix3 :: proc(m: Matrix3) -> Quaternion { + four_x_squared_minus_1, four_y_squared_minus_1, + four_z_squared_minus_1, four_w_squared_minus_1, + four_biggest_squared_minus_1: Float; + + /* xyzw */ + /* 0123 */ + biggest_index := 3; + biggest_value, mult: Float; + + four_x_squared_minus_1 = m[0][0] - m[1][1] - m[2][2]; + four_y_squared_minus_1 = m[1][1] - m[0][0] - m[2][2]; + four_z_squared_minus_1 = m[2][2] - m[0][0] - m[1][1]; + four_w_squared_minus_1 = m[0][0] + m[1][1] + m[2][2]; + + four_biggest_squared_minus_1 = four_w_squared_minus_1; + if four_x_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_x_squared_minus_1; + biggest_index = 0; + } + if four_y_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_y_squared_minus_1; + biggest_index = 1; + } + if four_z_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_z_squared_minus_1; + biggest_index = 2; + } + + biggest_value = math.sqrt(four_biggest_squared_minus_1 + 1) * 0.5; + mult = 0.25 / biggest_value; + + + switch biggest_index { + case 0: + return quaternion( + biggest_value, + (m[0][1] + m[1][0]) * mult, + (m[2][0] + m[0][2]) * mult, + (m[1][2] - m[2][1]) * mult, + ); + case 1: + return quaternion( + (m[0][1] + m[1][0]) * mult, + biggest_value, + (m[1][2] + m[2][1]) * mult, + (m[2][0] - m[0][2]) * mult, + ); + case 2: + return quaternion( + (m[2][0] + m[0][2]) * mult, + (m[1][2] + m[2][1]) * mult, + biggest_value, + (m[0][1] - m[1][0]) * mult, + ); + case 3: + return quaternion( + (m[1][2] - m[2][1]) * mult, + (m[2][0] - m[0][2]) * mult, + (m[0][1] - m[1][0]) * mult, + biggest_value, + ); + } + + return 0; +} + quaternion_between_two_vector3 :: proc(from, to: Vector3) -> Quaternion { x := normalize(from); y := normalize(to); @@ -385,15 +473,15 @@ matrix2_adjoint :: proc(m: Matrix2) -> Matrix2 { matrix3_from_quaternion :: proc(q: Quaternion) -> Matrix3 { - xx := imag(q) * imag(q); - xy := imag(q) * jmag(q); - xz := imag(q) * kmag(q); - xw := imag(q) * real(q); - yy := jmag(q) * jmag(q); - yz := jmag(q) * kmag(q); - yw := jmag(q) * real(q); - zz := kmag(q) * kmag(q); - zw := kmag(q) * real(q); + xx := q.x * q.x; + xy := q.x * q.y; + xz := q.x * q.z; + xw := q.x * q.w; + yy := q.y * q.y; + yz := q.y * q.z; + yw := q.y * q.w; + zz := q.z * q.z; + zw := q.z * q.w; m: Matrix3; m[0][0] = 1 - 2 * (yy + zz); @@ -498,15 +586,15 @@ matrix3_look_at :: proc(eye, centre, up: Vector3) -> Matrix3 { matrix4_from_quaternion :: proc(q: Quaternion) -> Matrix4 { m := identity(Matrix4); - xx := imag(q) * imag(q); - xy := imag(q) * jmag(q); - xz := imag(q) * kmag(q); - xw := imag(q) * real(q); - yy := jmag(q) * jmag(q); - yz := jmag(q) * kmag(q); - yw := jmag(q) * real(q); - zz := kmag(q) * kmag(q); - zw := kmag(q) * real(q); + xx := q.x * q.x; + xy := q.x * q.y; + xz := q.x * q.z; + xw := q.x * q.w; + yy := q.y * q.y; + yz := q.y * q.z; + yw := q.y * q.w; + zz := q.z * q.z; + zw := q.z * q.w; m[0][0] = 1 - 2 * (yy + zz); m[1][0] = 2 * (xy - zw); diff --git a/src/check_expr.cpp b/src/check_expr.cpp index bb76638ec..f0353e25b 100644 --- a/src/check_expr.cpp +++ b/src/check_expr.cpp @@ -3313,6 +3313,48 @@ ExactValue get_constant_field(CheckerContext *c, Operand const *operand, Selecti if (success_) *success_ = true; return value; + } else if (value.kind == ExactValue_Quaternion) { + // @QuaternionLayout + Quaternion256 q = value.value_quaternion; + GB_ASSERT(sel.index.count == 1); + + switch (sel.index[0]) { + case 3: // w + if (success_) *success_ = true; + return exact_value_float(q.real); + + case 0: // x + if (success_) *success_ = true; + return exact_value_float(q.imag); + + case 1: // y + if (success_) *success_ = true; + return exact_value_float(q.jmag); + + case 2: // z + if (success_) *success_ = true; + return exact_value_float(q.kmag); + } + + if (success_) *success_ = false; + return empty_exact_value; + } else if (value.kind == ExactValue_Complex) { + // @QuaternionLayout + Complex128 c = value.value_complex; + GB_ASSERT(sel.index.count == 1); + + switch (sel.index[0]) { + case 0: // real + if (success_) *success_ = true; + return exact_value_float(c.real); + + case 1: // imag + if (success_) *success_ = true; + return exact_value_float(c.imag); + } + + if (success_) *success_ = false; + return empty_exact_value; } if (success_) *success_ = true; diff --git a/src/ir.cpp b/src/ir.cpp index 13f2add13..501332347 100644 --- a/src/ir.cpp +++ b/src/ir.cpp @@ -4783,6 +4783,14 @@ irValue *ir_emit_struct_ep(irProcedure *proc, irValue *s, i32 index) { irValue *ir_emit_struct_ev(irProcedure *proc, irValue *s, i32 index) { // NOTE(bill): For some weird legacy reason in LLVM, structure elements must be accessed as an i32 + if (s->kind == irValue_Instr) { + if (s->Instr.kind == irInstr_Load) { + irValue *addr = s->Instr.Load.address; + irValue *ptr = ir_emit_struct_ep(proc, addr, index); + return ir_emit_load(proc, ptr); + } + } + gbAllocator a = ir_allocator(); Type *t = base_type(ir_type(s)); Type *result_type = nullptr; @@ -4889,7 +4897,9 @@ irValue *ir_emit_deep_field_gep(irProcedure *proc, irValue *e, Selection sel) { } type = core_type(type); - if (is_type_raw_union(type)) { + if (is_type_quaternion(type)) { + e = ir_emit_struct_ep(proc, e, index); + } else if (is_type_raw_union(type)) { type = type->Struct.fields[index]->type; GB_ASSERT(is_type_pointer(ir_type(e))); e = ir_emit_bitcast(proc, e, alloc_type_pointer(type)); @@ -4942,6 +4952,15 @@ irValue *ir_emit_deep_field_gep(irProcedure *proc, irValue *e, Selection sel) { irValue *ir_emit_deep_field_ev(irProcedure *proc, irValue *e, Selection sel) { GB_ASSERT(sel.index.count > 0); + + if (e->kind == irValue_Instr) { + if (e->Instr.kind == irInstr_Load) { + irValue *addr = e->Instr.Load.address; + irValue *ptr = ir_emit_deep_field_gep(proc, addr, sel); + return ir_emit_load(proc, ptr); + } + } + Type *type = ir_type(e); for_array(i, sel.index) { diff --git a/src/types.cpp b/src/types.cpp index 9b286c8df..b4d42d67a 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -2333,6 +2333,94 @@ Selection lookup_field_with_selection(Type *type_, String field_name, bool is_ty } #endif } break; + + case Basic_quaternion128: { + // @QuaternionLayout + gb_local_persist String w = str_lit("w"); + gb_local_persist String x = str_lit("x"); + gb_local_persist String y = str_lit("y"); + gb_local_persist String z = str_lit("z"); + gb_local_persist Entity *entity__w = alloc_entity_field(nullptr, make_token_ident(w), t_f32, false, 3); + gb_local_persist Entity *entity__x = alloc_entity_field(nullptr, make_token_ident(x), t_f32, false, 0); + gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_f32, false, 1); + gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_f32, false, 2); + if (field_name == w) { + selection_add_index(&sel, 3); + sel.entity = entity__w; + return sel; + } else if (field_name == x) { + selection_add_index(&sel, 0); + sel.entity = entity__x; + return sel; + } else if (field_name == y) { + selection_add_index(&sel, 1); + sel.entity = entity__y; + return sel; + } else if (field_name == z) { + selection_add_index(&sel, 2); + sel.entity = entity__z; + return sel; + } + } break; + + case Basic_quaternion256: { + // @QuaternionLayout + gb_local_persist String w = str_lit("w"); + gb_local_persist String x = str_lit("x"); + gb_local_persist String y = str_lit("y"); + gb_local_persist String z = str_lit("z"); + gb_local_persist Entity *entity__w = alloc_entity_field(nullptr, make_token_ident(w), t_f64, false, 3); + gb_local_persist Entity *entity__x = alloc_entity_field(nullptr, make_token_ident(x), t_f64, false, 0); + gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_f64, false, 1); + gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_f64, false, 2); + if (field_name == w) { + selection_add_index(&sel, 3); + sel.entity = entity__w; + return sel; + } else if (field_name == x) { + selection_add_index(&sel, 0); + sel.entity = entity__x; + return sel; + } else if (field_name == y) { + selection_add_index(&sel, 1); + sel.entity = entity__y; + return sel; + } else if (field_name == z) { + selection_add_index(&sel, 2); + sel.entity = entity__z; + return sel; + } + } break; + + case Basic_UntypedQuaternion: { + // @QuaternionLayout + gb_local_persist String w = str_lit("w"); + gb_local_persist String x = str_lit("x"); + gb_local_persist String y = str_lit("y"); + gb_local_persist String z = str_lit("z"); + gb_local_persist Entity *entity__w = alloc_entity_field(nullptr, make_token_ident(w), t_untyped_float, false, 3); + gb_local_persist Entity *entity__x = alloc_entity_field(nullptr, make_token_ident(x), t_untyped_float, false, 0); + gb_local_persist Entity *entity__y = alloc_entity_field(nullptr, make_token_ident(y), t_untyped_float, false, 1); + gb_local_persist Entity *entity__z = alloc_entity_field(nullptr, make_token_ident(z), t_untyped_float, false, 2); + if (field_name == w) { + selection_add_index(&sel, 3); + sel.entity = entity__w; + return sel; + } else if (field_name == x) { + selection_add_index(&sel, 0); + sel.entity = entity__x; + return sel; + } else if (field_name == y) { + selection_add_index(&sel, 1); + sel.entity = entity__y; + return sel; + } else if (field_name == z) { + selection_add_index(&sel, 2); + sel.entity = entity__z; + return sel; + } + } break; + } return sel;