From 3709bedfc90a690696eb5fc0e64fd8e17a023ce6 Mon Sep 17 00:00:00 2001 From: Andy Spencer Date: Fri, 7 Mar 2014 08:53:26 +0000 Subject: [PATCH] Add quaternion and vector code --- src/edu/ucla/iBeaconNav/Quat.java | 130 ++++++++++++++++++++++++++++++ src/edu/ucla/iBeaconNav/Vect.java | 56 +++++++++++++ 2 files changed, 186 insertions(+) create mode 100644 src/edu/ucla/iBeaconNav/Quat.java create mode 100644 src/edu/ucla/iBeaconNav/Vect.java diff --git a/src/edu/ucla/iBeaconNav/Quat.java b/src/edu/ucla/iBeaconNav/Quat.java new file mode 100644 index 0000000..7483f8d --- /dev/null +++ b/src/edu/ucla/iBeaconNav/Quat.java @@ -0,0 +1,130 @@ +class Quat +{ + private double x = 0; + private double y = 0; + private double z = 0; + private double w = 1; + + public Quat() + { + } + + public Quat(double roll, double pitch, double yaw) + { + double euler[] = { roll, pitch, yaw }; + this.set(euler); + } + + public void set(double euler[]) + { + // Calculate sin/cos of roll/pitch/yaw + double sr = Math.sin(euler[0] * 0.5f); + double cr = Math.cos(euler[0] * 0.5f); + + double sp = Math.sin(euler[1] * 0.5f); + double cp = Math.cos(euler[1] * 0.5f); + + double sy = Math.sin(euler[2] * 0.5f); + double cy = Math.cos(euler[2] * 0.5f); + + // Calculate quaternion + double tx = (cr * cp * sy) + (sr * sp * cy); + double ty = (sr * cp * cy) + (cr * sp * sy); + double tz = (cr * sp * cy) - (sr * cp * sy); + double tw = (cr * cp * cy) - (sr * sp * sy); + + // Normalize output + double dist = Math.sqrt(tx*tx + ty*ty + tz*tz + tw*tw); + + this.x = tx / dist; + this.y = ty / dist; + this.z = tz / dist; + this.w = tw / dist; + } + + public void get(double[] euler) + { + double test = x*y + z*w; + + if (test > 0.499) { + // north pole + euler[0] = 2 * Math.atan2(x, w); + euler[1] = Math.PI / 2; + euler[2] = 0; + } else if (test < -0.499) { + // south pole + euler[0] = -2 * Math.atan2(x, w); + euler[1] = Math.PI / 2; + euler[2] = 0; + } else { + // normal + euler[0] = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z)); + euler[1] = Math.asin(2*x*y + 2*z*w); + euler[2] = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z)); + } + } + + public void mul(Quat q) + { + double tw = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z); + double tx = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y); + double ty = (w * q.y) - (x * q.z) + (y * q.w) + (z * q.x); + double tz = (w * q.z) + (x * q.y) - (y * q.x) + (z * q.w); + + this.w = tw; + this.x = tx; + this.y = ty; + this.z = tz; + } + + public void print(String label) + { + double euler[] = {0, 0, 0}; + this.get(euler); + System.out.format("%8s rpy - %7.2f %7.2f %7.2f\n", + label + ":", + euler[0]*180/Math.PI, + euler[1]*180/Math.PI, + euler[2]*180/Math.PI); + } + + public static void test() { + Quat id = new Quat(0, 0, 0); + Quat r90 = new Quat(Math.PI/2, 0, 0); + Quat p90 = new Quat(0, Math.PI/2, 0); + Quat y90 = new Quat(0, 0, Math.PI/2); + + Quat sum = new Quat(); + + System.out.println("Init"); + r90.print("r90"); + p90.print("p90"); + y90.print("y90"); + + System.out.println("Roll:"); + sum.mul(r90); sum.print("sum"); + sum.mul(r90); sum.print("sum"); + sum.mul(r90); sum.print("sum"); + sum.mul(r90); sum.print("sum"); + + System.out.println("Pitch:"); + sum.mul(p90); sum.print("sum"); + sum.mul(p90); sum.print("sum"); + sum.mul(p90); sum.print("sum"); + sum.mul(p90); sum.print("sum"); + + System.out.println("Yaw:"); + sum.mul(y90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + + System.out.println("Test:"); + sum.mul(r90); sum.print("sum"); + sum.mul(p90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + sum.mul(p90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + sum.mul(y90); sum.print("sum"); + } +} diff --git a/src/edu/ucla/iBeaconNav/Vect.java b/src/edu/ucla/iBeaconNav/Vect.java new file mode 100644 index 0000000..1949da5 --- /dev/null +++ b/src/edu/ucla/iBeaconNav/Vect.java @@ -0,0 +1,56 @@ +package edu.ucla.iBeaconNav; + +class Vect +{ + public double x = 0; + public double y = 0; + public double z = 0; + + /* Constructors */ + public Vect() + { + } + + public Vect(double x, double y, double z) + { + this.x = x; + this.y = y; + this.z = z; + } + + /* Set functions */ + public void set(double x, double y, double z) + { + this.x = x; + this.y = y; + this.z = z; + } + + /* Add functions */ + public void add(Vect v) + { + this.x += v.x; + this.y += v.y; + this.z += v.z; + } + public void add(double off) + { + this.x += off; + this.y += off; + this.z += off; + } + + /* Multiply functions (scalar) */ + public void mul(Vect v) + { + this.x *= v.x; + this.y *= v.y; + this.z *= v.z; + } + public void mul(double sf) + { + this.x *= sf; + this.y *= sf; + this.z *= sf; + } +} -- 2.43.2