summaryrefslogtreecommitdiffhomepage
path: root/opentrack/quat.hpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-25 11:52:51 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-25 11:52:51 +0200
commit10268faa526ea4eddc702af4ecd463c87bfe2b53 (patch)
tree93db1a85b270b768a30dfc931adf47dc6d0e3a71 /opentrack/quat.hpp
parent775f55b6b15f1854238eceec42865a840e496211 (diff)
some quaternion stuff was probably wrong
Diffstat (limited to 'opentrack/quat.hpp')
-rw-r--r--opentrack/quat.hpp18
1 files changed, 8 insertions, 10 deletions
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);
}
};