From 10268faa526ea4eddc702af4ecd463c87bfe2b53 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 25 Oct 2014 11:52:51 +0200 Subject: some quaternion stuff was probably wrong --- opentrack/quat.hpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) (limited to 'opentrack') diff --git a/opentrack/quat.hpp b/opentrack/quat.hpp index aec87f4c..9fb8f781 100644 --- a/opentrack/quat.hpp +++ b/opentrack/quat.hpp @@ -62,15 +62,13 @@ public: const Quat operator*(const Quat& B) const { const Quat& A = *this; - const Quat ret(A._0*B._0 - A._1*B._1 - A._2*B._2 - A._3*B._3, - A._0*B._1 + A._1*B._0 + A._2*B._3 - A._3*B._2, - A._0*B._2 - A._1*B._3 + A._2*B._0 + A._3*B._1, - A._0*B._3 + A._1*B._2 - A._2*B._1 + A._3*B._0); - const double mag_2 = ret._0 * ret._0 + - ret._1 * ret._1 + - ret._2 * ret._2 + - ret._3 * ret._3; - const double inv_mag = 1./sqrt(mag_2); - return Quat(1., ret._1 * inv_mag, ret._2 * inv_mag, ret._3 * inv_mag); + const double x = A._2 * B._0 + A._2 * B._3 - A._3 * B._2 + A._0 * B._2; + const double y = -A._2 * B._3 + A._2 * B._0 + A._3 * B._2 + A._0 * B._2; + const double z = A._2 * B._2 - A._2 * B._2 + A._3 * B._0 + A._0 * B._3; + const double w = -A._2 * B._2 - A._2 * B._2 - A._3 * B._3 + A._0 * B._0; + const double mag_2 = x*x + y*y + z*z + w*w; + const double mag = sqrt(mag_2); + const double inv_mag = 1./mag; + return Quat(mag, x*inv_mag, y*inv_mag, z*inv_mag); } }; -- cgit v1.2.3