1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456 |
- package linalg
- import "core:math"
- @(require_results)
- euler_angles_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) {
- switch order {
- case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix3(m)
- case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix3(m)
- case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix3(m)
- case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix3(m)
- case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix3(m)
- case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix3(m)
- case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix3(m)
- case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix3(m)
- case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix3(m)
- case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix3(m)
- case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix3(m)
- case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix3(m)
- }
- return
- }
- @(require_results)
- euler_angles_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) {
- switch order {
- case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix4(m)
- case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix4(m)
- case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix4(m)
- case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix4(m)
- case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix4(m)
- case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix4(m)
- case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix4(m)
- case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix4(m)
- case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix4(m)
- case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix4(m)
- case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix4(m)
- case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix4(m)
- }
- return
- }
- @(require_results)
- euler_angles_from_quaternion_f64 :: proc "contextless" (m: Quaternionf64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) {
- switch order {
- case .XYZ: t1, t2, t3 = euler_angles_xyz_from_quaternion(m)
- case .XZY: t1, t2, t3 = euler_angles_xzy_from_quaternion(m)
- case .YXZ: t1, t2, t3 = euler_angles_yxz_from_quaternion(m)
- case .YZX: t1, t2, t3 = euler_angles_yzx_from_quaternion(m)
- case .ZXY: t1, t2, t3 = euler_angles_zxy_from_quaternion(m)
- case .ZYX: t1, t2, t3 = euler_angles_zyx_from_quaternion(m)
- case .XYX: t1, t2, t3 = euler_angles_xyx_from_quaternion(m)
- case .XZX: t1, t2, t3 = euler_angles_xzx_from_quaternion(m)
- case .YXY: t1, t2, t3 = euler_angles_yxy_from_quaternion(m)
- case .YZY: t1, t2, t3 = euler_angles_yzy_from_quaternion(m)
- case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_quaternion(m)
- case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_quaternion(m)
- }
- return
- }
- @(require_results)
- matrix3_from_euler_angles_f64 :: proc "contextless" (t1, t2, t3: f64, order: Euler_Angle_Order) -> (m: Matrix3f64) {
- switch order {
- case .XYZ: return matrix3_from_euler_angles_xyz(t1, t2, t3) // m1, m2, m3 = X(t1), Y(t2), Z(t3);
- case .XZY: return matrix3_from_euler_angles_xzy(t1, t2, t3) // m1, m2, m3 = X(t1), Z(t2), Y(t3);
- case .YXZ: return matrix3_from_euler_angles_yxz(t1, t2, t3) // m1, m2, m3 = Y(t1), X(t2), Z(t3);
- case .YZX: return matrix3_from_euler_angles_yzx(t1, t2, t3) // m1, m2, m3 = Y(t1), Z(t2), X(t3);
- case .ZXY: return matrix3_from_euler_angles_zxy(t1, t2, t3) // m1, m2, m3 = Z(t1), X(t2), Y(t3);
- case .ZYX: return matrix3_from_euler_angles_zyx(t1, t2, t3) // m1, m2, m3 = Z(t1), Y(t2), X(t3);
- case .XYX: return matrix3_from_euler_angles_xyx(t1, t2, t3) // m1, m2, m3 = X(t1), Y(t2), X(t3);
- case .XZX: return matrix3_from_euler_angles_xzx(t1, t2, t3) // m1, m2, m3 = X(t1), Z(t2), X(t3);
- case .YXY: return matrix3_from_euler_angles_yxy(t1, t2, t3) // m1, m2, m3 = Y(t1), X(t2), Y(t3);
- case .YZY: return matrix3_from_euler_angles_yzy(t1, t2, t3) // m1, m2, m3 = Y(t1), Z(t2), Y(t3);
- case .ZXZ: return matrix3_from_euler_angles_zxz(t1, t2, t3) // m1, m2, m3 = Z(t1), X(t2), Z(t3);
- case .ZYZ: return matrix3_from_euler_angles_zyz(t1, t2, t3) // m1, m2, m3 = Z(t1), Y(t2), Z(t3);
- }
- return
- }
- @(require_results)
- matrix4_from_euler_angles_f64 :: proc "contextless" (t1, t2, t3: f64, order: Euler_Angle_Order) -> (m: Matrix4f64) {
- switch order {
- case .XYZ: return matrix4_from_euler_angles_xyz(t1, t2, t3) // m1, m2, m3 = X(t1), Y(t2), Z(t3);
- case .XZY: return matrix4_from_euler_angles_xzy(t1, t2, t3) // m1, m2, m3 = X(t1), Z(t2), Y(t3);
- case .YXZ: return matrix4_from_euler_angles_yxz(t1, t2, t3) // m1, m2, m3 = Y(t1), X(t2), Z(t3);
- case .YZX: return matrix4_from_euler_angles_yzx(t1, t2, t3) // m1, m2, m3 = Y(t1), Z(t2), X(t3);
- case .ZXY: return matrix4_from_euler_angles_zxy(t1, t2, t3) // m1, m2, m3 = Z(t1), X(t2), Y(t3);
- case .ZYX: return matrix4_from_euler_angles_zyx(t1, t2, t3) // m1, m2, m3 = Z(t1), Y(t2), X(t3);
- case .XYX: return matrix4_from_euler_angles_xyx(t1, t2, t3) // m1, m2, m3 = X(t1), Y(t2), X(t3);
- case .XZX: return matrix4_from_euler_angles_xzx(t1, t2, t3) // m1, m2, m3 = X(t1), Z(t2), X(t3);
- case .YXY: return matrix4_from_euler_angles_yxy(t1, t2, t3) // m1, m2, m3 = Y(t1), X(t2), Y(t3);
- case .YZY: return matrix4_from_euler_angles_yzy(t1, t2, t3) // m1, m2, m3 = Y(t1), Z(t2), Y(t3);
- case .ZXZ: return matrix4_from_euler_angles_zxz(t1, t2, t3) // m1, m2, m3 = Z(t1), X(t2), Z(t3);
- case .ZYZ: return matrix4_from_euler_angles_zyz(t1, t2, t3) // m1, m2, m3 = Z(t1), Y(t2), Z(t3);
- }
- return
- }
- @(require_results)
- quaternion_from_euler_angles_f64 :: proc "contextless" (t1, t2, t3: f64, order: Euler_Angle_Order) -> Quaternionf64 {
- X :: quaternion_from_euler_angle_x
- Y :: quaternion_from_euler_angle_y
- Z :: quaternion_from_euler_angle_z
- q1, q2, q3: Quaternionf64
- switch order {
- case .XYZ: q1, q2, q3 = X(t1), Y(t2), Z(t3)
- case .XZY: q1, q2, q3 = X(t1), Z(t2), Y(t3)
- case .YXZ: q1, q2, q3 = Y(t1), X(t2), Z(t3)
- case .YZX: q1, q2, q3 = Y(t1), Z(t2), X(t3)
- case .ZXY: q1, q2, q3 = Z(t1), X(t2), Y(t3)
- case .ZYX: q1, q2, q3 = Z(t1), Y(t2), X(t3)
- case .XYX: q1, q2, q3 = X(t1), Y(t2), X(t3)
- case .XZX: q1, q2, q3 = X(t1), Z(t2), X(t3)
- case .YXY: q1, q2, q3 = Y(t1), X(t2), Y(t3)
- case .YZY: q1, q2, q3 = Y(t1), Z(t2), Y(t3)
- case .ZXZ: q1, q2, q3 = Z(t1), X(t2), Z(t3)
- case .ZYZ: q1, q2, q3 = Z(t1), Y(t2), Z(t3)
- }
- return q1 * (q2 * q3)
- }
- // Quaternionf64s
- @(require_results)
- quaternion_from_euler_angle_x_f64 :: proc "contextless" (angle_x: f64) -> (q: Quaternionf64) {
- return quaternion_angle_axis_f64(angle_x, {1, 0, 0})
- }
- @(require_results)
- quaternion_from_euler_angle_y_f64 :: proc "contextless" (angle_y: f64) -> (q: Quaternionf64) {
- return quaternion_angle_axis_f64(angle_y, {0, 1, 0})
- }
- @(require_results)
- quaternion_from_euler_angle_z_f64 :: proc "contextless" (angle_z: f64) -> (q: Quaternionf64) {
- return quaternion_angle_axis_f64(angle_z, {0, 0, 1})
- }
- @(require_results)
- quaternion_from_pitch_yaw_roll_f64 :: proc "contextless" (pitch, yaw, roll: f64) -> Quaternionf64 {
- a, b, c := pitch, yaw, roll
- 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: Quaternionf64
- 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
- }
- @(require_results)
- roll_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> f64 {
- return math.atan2(2 * q.x*q.y + q.w*q.z, q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z)
- }
- @(require_results)
- pitch_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> f64 {
- y := 2 * (q.y*q.z + q.w*q.w)
- x := q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z
- if abs(x) <= F64_EPSILON && abs(y) <= F64_EPSILON {
- return 2 * math.atan2(q.x, q.w)
- }
- return math.atan2(y, x)
- }
- @(require_results)
- yaw_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> f64 {
- return math.asin(clamp(-2 * (q.x*q.z - q.w*q.y), -1, 1))
- }
- @(require_results)
- pitch_yaw_roll_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (pitch, yaw, roll: f64) {
- pitch = pitch_from_quaternion(q)
- yaw = yaw_from_quaternion(q)
- roll = roll_from_quaternion(q)
- return
- }
- @(require_results)
- euler_angles_xyz_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_xyz_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_yxz_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_yxz_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_xzx_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_xzx_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_xyx_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_xyx_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_yxy_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_yxy_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_yzy_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_yzy_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_zyz_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_zyz_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_zxz_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_zxz_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_xzy_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_xzy_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_yzx_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_yzx_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_zyx_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_zyx_from_matrix4(matrix4_from_quaternion(q))
- }
- @(require_results)
- euler_angles_zxy_from_quaternion_f64 :: proc "contextless" (q: Quaternionf64) -> (t1, t2, t3: f64) {
- return euler_angles_zxy_from_matrix4(matrix4_from_quaternion(q))
- }
- // Matrix3
- @(require_results)
- matrix3_from_euler_angle_x_f64 :: proc "contextless" (angle_x: f64) -> (m: Matrix3f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- m[0, 0] = 1
- m[1, 1] = +cos_x
- m[1, 2] = +sin_x
- m[2, 1] = -sin_x
- m[2, 2] = +cos_x
- return
- }
- @(require_results)
- matrix3_from_euler_angle_y_f64 :: proc "contextless" (angle_y: f64) -> (m: Matrix3f64) {
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = +cos_y
- m[0, 2] = -sin_y
- m[1, 1] = 1
- m[2, 0] = +sin_y
- m[2, 2] = +cos_y
- return
- }
- @(require_results)
- matrix3_from_euler_angle_z_f64 :: proc "contextless" (angle_z: f64) -> (m: Matrix3f64) {
- cos_z, sin_z := math.cos(angle_z), math.sin(angle_z)
- m[0, 0] = +cos_z
- m[0, 1] = +sin_z
- m[1, 1] = +cos_z
- m[1, 0] = -sin_z
- m[2, 2] = 1
- return
- }
- @(require_results)
- matrix3_from_derived_euler_angle_x_f64 :: proc "contextless" (angle_x: f64, angular_velocity_x: f64) -> (m: Matrix3f64) {
- cos_x := math.cos(angle_x) * angular_velocity_x
- sin_x := math.sin(angle_x) * angular_velocity_x
- m[0, 0] = 1
- m[1, 1] = +cos_x
- m[1, 2] = +sin_x
- m[2, 1] = -sin_x
- m[2, 2] = +cos_x
- return
- }
- @(require_results)
- matrix3_from_derived_euler_angle_y_f64 :: proc "contextless" (angle_y: f64, angular_velocity_y: f64) -> (m: Matrix3f64) {
- cos_y := math.cos(angle_y) * angular_velocity_y
- sin_y := math.sin(angle_y) * angular_velocity_y
- m[0, 0] = +cos_y
- m[0, 2] = -sin_y
- m[1, 1] = 1
- m[2, 0] = +sin_y
- m[2, 2] = +cos_y
- return
- }
- @(require_results)
- matrix3_from_derived_euler_angle_z_f64 :: proc "contextless" (angle_z: f64, angular_velocity_z: f64) -> (m: Matrix3f64) {
- cos_z := math.cos(angle_z) * angular_velocity_z
- sin_z := math.sin(angle_z) * angular_velocity_z
- m[0, 0] = +cos_z
- m[0, 1] = +sin_z
- m[1, 1] = +cos_z
- m[1, 0] = -sin_z
- m[2, 2] = 1
- return
- }
- @(require_results)
- matrix3_from_euler_angles_xy_f64 :: proc "contextless" (angle_x, angle_y: f64) -> (m: Matrix3f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = cos_y
- m[0, 1] = -sin_x * - sin_y
- m[0, 2] = -cos_x * - sin_y
- m[1, 1] = cos_x
- m[1, 2] = sin_x
- m[2, 0] = sin_y
- m[2, 1] = -sin_x * cos_y
- m[2, 2] = cos_x * cos_y
- return
- }
- @(require_results)
- matrix3_from_euler_angles_yx_f64 :: proc "contextless" (angle_y, angle_x: f64) -> (m: Matrix3f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = cos_y
- m[0, 2] = -sin_y
- m[1, 0] = sin_y*sin_x
- m[1, 1] = cos_x
- m[1, 2] = cos_y*sin_x
- m[2, 0] = sin_y*cos_x
- m[2, 1] = -sin_x
- m[2, 2] = cos_y*cos_x
- return
- }
- @(require_results)
- matrix3_from_euler_angles_xz_f64 :: proc "contextless" (angle_x, angle_z: f64) -> (m: Matrix3f64) {
- return mul(matrix3_from_euler_angle_x(angle_x), matrix3_from_euler_angle_z(angle_z))
- }
- @(require_results)
- matrix3_from_euler_angles_zx_f64 :: proc "contextless" (angle_z, angle_x: f64) -> (m: Matrix3f64) {
- return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_x(angle_x))
- }
- @(require_results)
- matrix3_from_euler_angles_yz_f64 :: proc "contextless" (angle_y, angle_z: f64) -> (m: Matrix3f64) {
- return mul(matrix3_from_euler_angle_y(angle_y), matrix3_from_euler_angle_z(angle_z))
- }
- @(require_results)
- matrix3_from_euler_angles_zy_f64 :: proc "contextless" (angle_z, angle_y: f64) -> (m: Matrix3f64) {
- return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_y(angle_y))
- }
- @(require_results)
- matrix3_from_euler_angles_xyz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(-t1)
- c2 := math.cos(-t2)
- c3 := math.cos(-t3)
- s1 := math.sin(-t1)
- s2 := math.sin(-t2)
- s3 := math.sin(-t3)
- m[0, 0] = c2 * c3
- m[1, 0] =-c1 * s3 + s1 * s2 * c3
- m[2, 0] = s1 * s3 + c1 * s2 * c3
- m[0, 1] = c2 * s3
- m[1, 1] = c1 * c3 + s1 * s2 * s3
- m[2, 1] =-s1 * c3 + c1 * s2 * s3
- m[0, 2] =-s2
- m[1, 2] = s1 * c2
- m[2, 2] = c1 * c2
- return
- }
- @(require_results)
- matrix3_from_euler_angles_yxz_f64 :: proc "contextless" (yaw, pitch, roll: f64) -> (m: Matrix3f64) {
- ch := math.cos(yaw)
- sh := math.sin(yaw)
- cp := math.cos(pitch)
- sp := math.sin(pitch)
- cb := math.cos(roll)
- sb := math.sin(roll)
- m[0, 0] = ch * cb + sh * sp * sb
- m[1, 0] = sb * cp
- m[2, 0] = -sh * cb + ch * sp * sb
- m[0, 1] = -ch * sb + sh * sp * cb
- m[1, 1] = cb * cp
- m[2, 1] = sb * sh + ch * sp * cb
- m[0, 2] = sh * cp
- m[1, 2] = -sp
- m[2, 2] = ch * cp
- return
- }
- @(require_results)
- matrix3_from_euler_angles_xzx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2
- m[1, 0] = c1 * s2
- m[2, 0] = s1 * s2
- m[0, 1] =-c3 * s2
- m[1, 1] = c1 * c2 * c3 - s1 * s3
- m[2, 1] = c1 * s3 + c2 * c3 * s1
- m[0, 2] = s2 * s3
- m[1, 2] =-c3 * s1 - c1 * c2 * s3
- m[2, 2] = c1 * c3 - c2 * s1 * s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_xyx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2
- m[1, 0] = s1 * s2
- m[2, 0] =-c1 * s2
- m[0, 1] = s2 * s3
- m[1, 1] = c1 * c3 - c2 * s1 * s3
- m[2, 1] = c3 * s1 + c1 * c2 * s3
- m[0, 2] = c3 * s2
- m[1, 2] =-c1 * s3 - c2 * c3 * s1
- m[2, 2] = c1 * c2 * c3 - s1 * s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_yxy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - c2 * s1 * s3
- m[1, 0] = s2* s3
- m[2, 0] =-c3 * s1 - c1 * c2 * s3
- m[0, 1] = s1 * s2
- m[1, 1] = c2
- m[2, 1] = c1 * s2
- m[0, 2] = c1 * s3 + c2 * c3 * s1
- m[1, 2] =-c3 * s2
- m[2, 2] = c1 * c2 * c3 - s1 * s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_yzy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2 * c3 - s1 * s3
- m[1, 0] = c3 * s2
- m[2, 0] =-c1 * s3 - c2 * c3 * s1
- m[0, 1] =-c1 * s2
- m[1, 1] = c2
- m[2, 1] = s1 * s2
- m[0, 2] = c3 * s1 + c1 * c2 * s3
- m[1, 2] = s2 * s3
- m[2, 2] = c1 * c3 - c2 * s1 * s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_zyz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2 * c3 - s1 * s3
- m[1, 0] = c1 * s3 + c2 * c3 * s1
- m[2, 0] =-c3 * s2
- m[0, 1] =-c3 * s1 - c1 * c2 * s3
- m[1, 1] = c1 * c3 - c2 * s1 * s3
- m[2, 1] = s2 * s3
- m[0, 2] = c1 * s2
- m[1, 2] = s1 * s2
- m[2, 2] = c2
- return
- }
- @(require_results)
- matrix3_from_euler_angles_zxz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - c2 * s1 * s3
- m[1, 0] = c3 * s1 + c1 * c2 * s3
- m[2, 0] = s2 *s3
- m[0, 1] =-c1 * s3 - c2 * c3 * s1
- m[1, 1] = c1 * c2 * c3 - s1 * s3
- m[2, 1] = c3 * s2
- m[0, 2] = s1 * s2
- m[1, 2] =-c1 * s2
- m[2, 2] = c2
- return
- }
- @(require_results)
- matrix3_from_euler_angles_xzy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2 * c3
- m[1, 0] = s1 * s3 + c1 * c3 * s2
- m[2, 0] = c3 * s1 * s2 - c1 * s3
- m[0, 1] =-s2
- m[1, 1] = c1 * c2
- m[2, 1] = c2 * s1
- m[0, 2] = c2 * s3
- m[1, 2] = c1 * s2 * s3 - c3 * s1
- m[2, 2] = c1 * c3 + s1 * s2 *s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_yzx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2
- m[1, 0] = s2
- m[2, 0] =-c2 * s1
- m[0, 1] = s1 * s3 - c1 * c3 * s2
- m[1, 1] = c2 * c3
- m[2, 1] = c1 * s3 + c3 * s1 * s2
- m[0, 2] = c3 * s1 + c1 * s2 * s3
- m[1, 2] =-c2 * s3
- m[2, 2] = c1 * c3 - s1 * s2 * s3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_zyx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2
- m[1, 0] = c2 * s1
- m[2, 0] =-s2
- m[0, 1] = c1 * s2 * s3 - c3 * s1
- m[1, 1] = c1 * c3 + s1 * s2 * s3
- m[2, 1] = c2 * s3
- m[0, 2] = s1 * s3 + c1 * c3 * s2
- m[1, 2] = c3 * s1 * s2 - c1 * s3
- m[2, 2] = c2 * c3
- return
- }
- @(require_results)
- matrix3_from_euler_angles_zxy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix3f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - s1 * s2 * s3
- m[1, 0] = c3 * s1 + c1 * s2 * s3
- m[2, 0] =-c2 * s3
- m[0, 1] =-c2 * s1
- m[1, 1] = c1 * c2
- m[2, 1] = s2
- m[0, 2] = c1 * s3 + c3 * s1 * s2
- m[1, 2] = s1 * s3 - c1 * c3 * s2
- m[2, 2] = c2 * c3
- return
- }
- @(require_results)
- matrix3_from_yaw_pitch_roll_f64 :: proc "contextless" (yaw, pitch, roll: f64) -> (m: Matrix3f64) {
- ch := math.cos(yaw)
- sh := math.sin(yaw)
- cp := math.cos(pitch)
- sp := math.sin(pitch)
- cb := math.cos(roll)
- sb := math.sin(roll)
- m[0, 0] = ch * cb + sh * sp * sb
- m[1, 0] = sb * cp
- m[2, 0] = -sh * cb + ch * sp * sb
- m[0, 1] = -ch * sb + sh * sp * cb
- m[1, 1] = cb * cp
- m[2, 1] = sb * sh + ch * sp * cb
- m[0, 2] = sh * cp
- m[1, 2] = -sp
- m[2, 2] = ch * cp
- return m
- }
- @(require_results)
- euler_angles_xyz_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 2], m[2, 2])
- C2 := math.sqrt(m[0, 0]*m[0, 0] + m[0, 1]*m[0, 1])
- T2 := math.atan2(-m[0, 2], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[2, 0] - C1*m[1, 0], C1*m[1, 1] - S1*m[2, 1])
- t1 = -T1
- t2 = -T2
- t3 = -T3
- return
- }
- @(require_results)
- euler_angles_yxz_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 2], m[2, 2])
- C2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 1]*m[1, 1])
- T2 := math.atan2(-m[1, 2], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[2, 1] - C1*m[0, 1], C1*m[0, 0] - S1*m[2, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xzx_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 0], m[1, 0])
- S2 := math.sqrt(m[0, 1]*m[0, 1] + m[0, 2]*m[0, 2])
- T2 := math.atan2(S2, m[0, 0])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[2, 1] - S1*m[1, 1], C1*m[2, 2] - S1*m[1, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xyx_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 0], -m[2, 0])
- S2 := math.sqrt(m[0, 1]*m[0, 1] + m[0, 2]*m[0, 2])
- T2 := math.atan2(S2, m[0, 0])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-C1*m[1, 2] - S1*m[2, 2], C1*m[1, 1] + S1*m[2, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yxy_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 1], m[2, 1])
- S2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 2]*m[1, 2])
- T2 := math.atan2(S2, m[1, 1])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[0, 2] - S1*m[2, 2], C1*m[0, 0] - S1*m[2, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yzy_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 1], -m[0, 1])
- S2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 2]*m[1, 2])
- T2 := math.atan2(S2, m[1, 1])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-S1*m[0, 0] - C1*m[2, 0], S1*m[0, 2] + C1*m[2, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zyz_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 2], m[0, 2])
- S2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 1]*m[2, 1])
- T2 := math.atan2(S2, m[2, 2])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[1, 0] - S1*m[0, 0], C1*m[1, 1] - S1*m[0, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zxz_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 2], -m[1, 2])
- S2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 1]*m[2, 1])
- T2 := math.atan2(S2, m[2, 2])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-C1*m[0, 1] - S1*m[1, 1], C1*m[0, 0] + S1*m[1, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xzy_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 1], m[1, 1])
- C2 := math.sqrt(m[0, 0]*m[0, 0] + m[0, 2]*m[0, 2])
- T2 := math.atan2(-m[0, 1], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[1, 0] - C1*m[2, 0], C1*m[2, 2] - S1*m[1, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yzx_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(-m[2, 0], m[0, 0])
- C2 := math.sqrt(m[1, 1]*m[1, 1] + m[1, 2]*m[1, 2])
- T2 := math.atan2(m[1, 0], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[0, 1] + C1*m[2, 1], S1*m[0, 2] + C1*m[2, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zyx_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 0], m[0, 0])
- C2 := math.sqrt(m[2, 1]*m[2, 1] + m[2, 2]*m[2, 2])
- T2 := math.atan2(-m[2, 0], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[0, 2] - C1*m[1, 2], C1*m[1, 1] - S1*m[0, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zxy_from_matrix3_f64 :: proc "contextless" (m: Matrix3f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(-m[0, 1], m[1, 1])
- C2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 2]*m[2, 2])
- T2 := math.atan2(m[2, 1], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[0, 2] + S1*m[1, 2], C1*m[0, 0] + S1*m[1, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- // Matrix4
- @(require_results)
- matrix4_from_euler_angle_x_f64 :: proc "contextless" (angle_x: f64) -> (m: Matrix4f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- m[0, 0] = 1
- m[1, 1] = +cos_x
- m[1, 2] = +sin_x
- m[2, 1] = -sin_x
- m[2, 2] = +cos_x
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angle_y_f64 :: proc "contextless" (angle_y: f64) -> (m: Matrix4f64) {
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = +cos_y
- m[0, 2] = -sin_y
- m[1, 1] = 1
- m[2, 0] = +sin_y
- m[2, 2] = +cos_y
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angle_z_f64 :: proc "contextless" (angle_z: f64) -> (m: Matrix4f64) {
- cos_z, sin_z := math.cos(angle_z), math.sin(angle_z)
- m[0, 0] = +cos_z
- m[0, 1] = +sin_z
- m[1, 1] = +cos_z
- m[1, 0] = -sin_z
- m[2, 2] = 1
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_derived_euler_angle_x_f64 :: proc "contextless" (angle_x: f64, angular_velocity_x: f64) -> (m: Matrix4f64) {
- cos_x := math.cos(angle_x) * angular_velocity_x
- sin_x := math.sin(angle_x) * angular_velocity_x
- m[0, 0] = 1
- m[1, 1] = +cos_x
- m[1, 2] = +sin_x
- m[2, 1] = -sin_x
- m[2, 2] = +cos_x
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_derived_euler_angle_y_f64 :: proc "contextless" (angle_y: f64, angular_velocity_y: f64) -> (m: Matrix4f64) {
- cos_y := math.cos(angle_y) * angular_velocity_y
- sin_y := math.sin(angle_y) * angular_velocity_y
- m[0, 0] = +cos_y
- m[0, 2] = -sin_y
- m[1, 1] = 1
- m[2, 0] = +sin_y
- m[2, 2] = +cos_y
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_derived_euler_angle_z_f64 :: proc "contextless" (angle_z: f64, angular_velocity_z: f64) -> (m: Matrix4f64) {
- cos_z := math.cos(angle_z) * angular_velocity_z
- sin_z := math.sin(angle_z) * angular_velocity_z
- m[0, 0] = +cos_z
- m[0, 1] = +sin_z
- m[1, 1] = +cos_z
- m[1, 0] = -sin_z
- m[2, 2] = 1
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_xy_f64 :: proc "contextless" (angle_x, angle_y: f64) -> (m: Matrix4f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = cos_y
- m[0, 1] = -sin_x * - sin_y
- m[0, 2] = -cos_x * - sin_y
- m[1, 1] = cos_x
- m[1, 2] = sin_x
- m[2, 0] = sin_y
- m[2, 1] = -sin_x * cos_y
- m[2, 2] = cos_x * cos_y
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_yx_f64 :: proc "contextless" (angle_y, angle_x: f64) -> (m: Matrix4f64) {
- cos_x, sin_x := math.cos(angle_x), math.sin(angle_x)
- cos_y, sin_y := math.cos(angle_y), math.sin(angle_y)
- m[0, 0] = cos_y
- m[0, 2] = -sin_y
- m[1, 0] = sin_y*sin_x
- m[1, 1] = cos_x
- m[1, 2] = cos_y*sin_x
- m[2, 0] = sin_y*cos_x
- m[2, 1] = -sin_x
- m[2, 2] = cos_y*cos_x
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_xz_f64 :: proc "contextless" (angle_x, angle_z: f64) -> (m: Matrix4f64) {
- return mul(matrix4_from_euler_angle_x(angle_x), matrix4_from_euler_angle_z(angle_z))
- }
- @(require_results)
- matrix4_from_euler_angles_zx_f64 :: proc "contextless" (angle_z, angle_x: f64) -> (m: Matrix4f64) {
- return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_x(angle_x))
- }
- @(require_results)
- matrix4_from_euler_angles_yz_f64 :: proc "contextless" (angle_y, angle_z: f64) -> (m: Matrix4f64) {
- return mul(matrix4_from_euler_angle_y(angle_y), matrix4_from_euler_angle_z(angle_z))
- }
- @(require_results)
- matrix4_from_euler_angles_zy_f64 :: proc "contextless" (angle_z, angle_y: f64) -> (m: Matrix4f64) {
- return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_y(angle_y))
- }
- @(require_results)
- matrix4_from_euler_angles_xyz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(-t1)
- c2 := math.cos(-t2)
- c3 := math.cos(-t3)
- s1 := math.sin(-t1)
- s2 := math.sin(-t2)
- s3 := math.sin(-t3)
- m[0, 0] = c2 * c3
- m[1, 0] =-c1 * s3 + s1 * s2 * c3
- m[2, 0] = s1 * s3 + c1 * s2 * c3
- m[3, 0] = 0
- m[0, 1] = c2 * s3
- m[1, 1] = c1 * c3 + s1 * s2 * s3
- m[2, 1] =-s1 * c3 + c1 * s2 * s3
- m[3, 1] = 0
- m[0, 2] =-s2
- m[1, 2] = s1 * c2
- m[2, 2] = c1 * c2
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_yxz_f64 :: proc "contextless" (yaw, pitch, roll: f64) -> (m: Matrix4f64) {
- ch := math.cos(yaw)
- sh := math.sin(yaw)
- cp := math.cos(pitch)
- sp := math.sin(pitch)
- cb := math.cos(roll)
- sb := math.sin(roll)
- m[0, 0] = ch * cb + sh * sp * sb
- m[1, 0] = sb * cp
- m[2, 0] = -sh * cb + ch * sp * sb
- m[3, 0] = 0
- m[0, 1] = -ch * sb + sh * sp * cb
- m[1, 1] = cb * cp
- m[2, 1] = sb * sh + ch * sp * cb
- m[3, 1] = 0
- m[0, 2] = sh * cp
- m[1, 2] = -sp
- m[2, 2] = ch * cp
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_xzx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2
- m[1, 0] = c1 * s2
- m[2, 0] = s1 * s2
- m[3, 0] = 0
- m[0, 1] =-c3 * s2
- m[1, 1] = c1 * c2 * c3 - s1 * s3
- m[2, 1] = c1 * s3 + c2 * c3 * s1
- m[3, 1] = 0
- m[0, 2] = s2 * s3
- m[1, 2] =-c3 * s1 - c1 * c2 * s3
- m[2, 2] = c1 * c3 - c2 * s1 * s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_xyx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2
- m[1, 0] = s1 * s2
- m[2, 0] =-c1 * s2
- m[3, 0] = 0
- m[0, 1] = s2 * s3
- m[1, 1] = c1 * c3 - c2 * s1 * s3
- m[2, 1] = c3 * s1 + c1 * c2 * s3
- m[3, 1] = 0
- m[0, 2] = c3 * s2
- m[1, 2] =-c1 * s3 - c2 * c3 * s1
- m[2, 2] = c1 * c2 * c3 - s1 * s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_yxy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - c2 * s1 * s3
- m[1, 0] = s2* s3
- m[2, 0] =-c3 * s1 - c1 * c2 * s3
- m[3, 0] = 0
- m[0, 1] = s1 * s2
- m[1, 1] = c2
- m[2, 1] = c1 * s2
- m[3, 1] = 0
- m[0, 2] = c1 * s3 + c2 * c3 * s1
- m[1, 2] =-c3 * s2
- m[2, 2] = c1 * c2 * c3 - s1 * s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_yzy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2 * c3 - s1 * s3
- m[1, 0] = c3 * s2
- m[2, 0] =-c1 * s3 - c2 * c3 * s1
- m[3, 0] = 0
- m[0, 1] =-c1 * s2
- m[1, 1] = c2
- m[2, 1] = s1 * s2
- m[3, 1] = 0
- m[0, 2] = c3 * s1 + c1 * c2 * s3
- m[1, 2] = s2 * s3
- m[2, 2] = c1 * c3 - c2 * s1 * s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_zyz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2 * c3 - s1 * s3
- m[1, 0] = c1 * s3 + c2 * c3 * s1
- m[2, 0] =-c3 * s2
- m[3, 0] = 0
- m[0, 1] =-c3 * s1 - c1 * c2 * s3
- m[1, 1] = c1 * c3 - c2 * s1 * s3
- m[2, 1] = s2 * s3
- m[3, 1] = 0
- m[0, 2] = c1 * s2
- m[1, 2] = s1 * s2
- m[2, 2] = c2
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_zxz_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - c2 * s1 * s3
- m[1, 0] = c3 * s1 + c1 * c2 * s3
- m[2, 0] = s2 *s3
- m[3, 0] = 0
- m[0, 1] =-c1 * s3 - c2 * c3 * s1
- m[1, 1] = c1 * c2 * c3 - s1 * s3
- m[2, 1] = c3 * s2
- m[3, 1] = 0
- m[0, 2] = s1 * s2
- m[1, 2] =-c1 * s2
- m[2, 2] = c2
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_xzy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c2 * c3
- m[1, 0] = s1 * s3 + c1 * c3 * s2
- m[2, 0] = c3 * s1 * s2 - c1 * s3
- m[3, 0] = 0
- m[0, 1] =-s2
- m[1, 1] = c1 * c2
- m[2, 1] = c2 * s1
- m[3, 1] = 0
- m[0, 2] = c2 * s3
- m[1, 2] = c1 * s2 * s3 - c3 * s1
- m[2, 2] = c1 * c3 + s1 * s2 *s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_yzx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2
- m[1, 0] = s2
- m[2, 0] =-c2 * s1
- m[3, 0] = 0
- m[0, 1] = s1 * s3 - c1 * c3 * s2
- m[1, 1] = c2 * c3
- m[2, 1] = c1 * s3 + c3 * s1 * s2
- m[3, 1] = 0
- m[0, 2] = c3 * s1 + c1 * s2 * s3
- m[1, 2] =-c2 * s3
- m[2, 2] = c1 * c3 - s1 * s2 * s3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_zyx_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c2
- m[1, 0] = c2 * s1
- m[2, 0] =-s2
- m[3, 0] = 0
- m[0, 1] = c1 * s2 * s3 - c3 * s1
- m[1, 1] = c1 * c3 + s1 * s2 * s3
- m[2, 1] = c2 * s3
- m[3, 1] = 0
- m[0, 2] = s1 * s3 + c1 * c3 * s2
- m[1, 2] = c3 * s1 * s2 - c1 * s3
- m[2, 2] = c2 * c3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_euler_angles_zxy_f64 :: proc "contextless" (t1, t2, t3: f64) -> (m: Matrix4f64) {
- c1 := math.cos(t1)
- s1 := math.sin(t1)
- c2 := math.cos(t2)
- s2 := math.sin(t2)
- c3 := math.cos(t3)
- s3 := math.sin(t3)
- m[0, 0] = c1 * c3 - s1 * s2 * s3
- m[1, 0] = c3 * s1 + c1 * s2 * s3
- m[2, 0] =-c2 * s3
- m[3, 0] = 0
- m[0, 1] =-c2 * s1
- m[1, 1] = c1 * c2
- m[2, 1] = s2
- m[3, 1] = 0
- m[0, 2] = c1 * s3 + c3 * s1 * s2
- m[1, 2] = s1 * s3 - c1 * c3 * s2
- m[2, 2] = c2 * c3
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return
- }
- @(require_results)
- matrix4_from_yaw_pitch_roll_f64 :: proc "contextless" (yaw, pitch, roll: f64) -> (m: Matrix4f64) {
- ch := math.cos(yaw)
- sh := math.sin(yaw)
- cp := math.cos(pitch)
- sp := math.sin(pitch)
- cb := math.cos(roll)
- sb := math.sin(roll)
- m[0, 0] = ch * cb + sh * sp * sb
- m[1, 0] = sb * cp
- m[2, 0] = -sh * cb + ch * sp * sb
- m[3, 0] = 0
- m[0, 1] = -ch * sb + sh * sp * cb
- m[1, 1] = cb * cp
- m[2, 1] = sb * sh + ch * sp * cb
- m[3, 1] = 0
- m[0, 2] = sh * cp
- m[1, 2] = -sp
- m[2, 2] = ch * cp
- m[3, 2] = 0
- m[0, 3] = 0
- m[1, 3] = 0
- m[2, 3] = 0
- m[3, 3] = 1
- return m
- }
- @(require_results)
- euler_angles_xyz_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 2], m[2, 2])
- C2 := math.sqrt(m[0, 0]*m[0, 0] + m[0, 1]*m[0, 1])
- T2 := math.atan2(-m[0, 2], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[2, 0] - C1*m[1, 0], C1*m[1, 1] - S1*m[2, 1])
- t1 = -T1
- t2 = -T2
- t3 = -T3
- return
- }
- @(require_results)
- euler_angles_yxz_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 2], m[2, 2])
- C2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 1]*m[1, 1])
- T2 := math.atan2(-m[1, 2], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[2, 1] - C1*m[0, 1], C1*m[0, 0] - S1*m[2, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xzx_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 0], m[1, 0])
- S2 := math.sqrt(m[0, 1]*m[0, 1] + m[0, 2]*m[0, 2])
- T2 := math.atan2(S2, m[0, 0])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[2, 1] - S1*m[1, 1], C1*m[2, 2] - S1*m[1, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xyx_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 0], -m[2, 0])
- S2 := math.sqrt(m[0, 1]*m[0, 1] + m[0, 2]*m[0, 2])
- T2 := math.atan2(S2, m[0, 0])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-C1*m[1, 2] - S1*m[2, 2], C1*m[1, 1] + S1*m[2, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yxy_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 1], m[2, 1])
- S2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 2]*m[1, 2])
- T2 := math.atan2(S2, m[1, 1])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[0, 2] - S1*m[2, 2], C1*m[0, 0] - S1*m[2, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yzy_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 1], -m[0, 1])
- S2 := math.sqrt(m[1, 0]*m[1, 0] + m[1, 2]*m[1, 2])
- T2 := math.atan2(S2, m[1, 1])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-S1*m[0, 0] - C1*m[2, 0], S1*m[0, 2] + C1*m[2, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zyz_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 2], m[0, 2])
- S2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 1]*m[2, 1])
- T2 := math.atan2(S2, m[2, 2])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[1, 0] - S1*m[0, 0], C1*m[1, 1] - S1*m[0, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zxz_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[0, 2], -m[1, 2])
- S2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 1]*m[2, 1])
- T2 := math.atan2(S2, m[2, 2])
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(-C1*m[0, 1] - S1*m[1, 1], C1*m[0, 0] + S1*m[1, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_xzy_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[2, 1], m[1, 1])
- C2 := math.sqrt(m[0, 0]*m[0, 0] + m[0, 2]*m[0, 2])
- T2 := math.atan2(-m[0, 1], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[1, 0] - C1*m[2, 0], C1*m[2, 2] - S1*m[1, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_yzx_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(-m[2, 0], m[0, 0])
- C2 := math.sqrt(m[1, 1]*m[1, 1] + m[1, 2]*m[1, 2])
- T2 := math.atan2(m[1, 0], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[0, 1] + C1*m[2, 1], S1*m[0, 2] + C1*m[2, 2])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zyx_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(m[1, 0], m[0, 0])
- C2 := math.sqrt(m[2, 1]*m[2, 1] + m[2, 2]*m[2, 2])
- T2 := math.atan2(-m[2, 0], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(S1*m[0, 2] - C1*m[1, 2], C1*m[1, 1] - S1*m[0, 1])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
- @(require_results)
- euler_angles_zxy_from_matrix4_f64 :: proc "contextless" (m: Matrix4f64) -> (t1, t2, t3: f64) {
- T1 := math.atan2(-m[0, 1], m[1, 1])
- C2 := math.sqrt(m[2, 0]*m[2, 0] + m[2, 2]*m[2, 2])
- T2 := math.atan2(m[2, 1], C2)
- S1 := math.sin(T1)
- C1 := math.cos(T1)
- T3 := math.atan2(C1*m[0, 2] + S1*m[1, 2], C1*m[0, 0] + S1*m[1, 0])
- t1 = T1
- t2 = T2
- t3 = T3
- return
- }
|