~xdavidwu/motion-control

7825035984194b2e723f3c40a2251d23853464e3 — xdavidwu 2 years ago 1f9ec88
math::quaternion: add from_euler
1 files changed, 15 insertions(+), 0 deletions(-)

M math/quaternion/conversion.ha
M math/quaternion/conversion.ha => math/quaternion/conversion.ha +15 -0
@@ 15,3 15,18 @@ export fn to_euler(q: quaternion) (f64, f64, f64) = {
		math::atanf64(2.0 * (q.0 * q.3 + q.1 * q.2) / (1.0 - 2.0 * (q.2 * q.2 + q.3 * q.3))),
	);
};

export fn from_euler(v: (f64, f64, f64)) quaternion = {
	const s0 = math::sinf64(v.0 / 2.0);
	const c0 = math::cosf64(v.0 / 2.0);
	const s1 = math::sinf64(v.1 / 2.0);
	const c1 = math::cosf64(v.1 / 2.0);
	const s2 = math::sinf64(v.2 / 2.0);
	const c2 = math::cosf64(v.2 / 2.0);
	return (
		c0 * c1 * c2 + s0 * s1 * s2,
		s0 * c1 * c2 - c0 * s1 * s2,
		c0 * s1 * c2 + s0 * c1 * s2,
		c0 * c1 * s2 - s0 * s1 * c2,
	);
};