/*
* Copyright (C) 2015 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.example.android.rs.vr.engine;
/**
* Generic Quaternion
* Written for maximum portability between desktop and Android
* Not in performance critical sections
*/
public class Quaternion {
private final double[] x = new double[4]; // w,x,y,z,
public void set(double w, double x, double y, double z) {
this.x[0] = w;
this.x[1] = x;
this.x[2] = y;
this.x[3] = z;
}
public void clone(Quaternion src) {
System.arraycopy(src.x, 0, x, 0, x.length);
}
private static double[] cross(double[] a, double[] b) {
double out0 = a[1] * b[2] - b[1] * a[2];
double out1 = a[2] * b[0] - b[2] * a[0];
double out2 = a[0] * b[1] - b[0] * a[1];
return new double[]{out0, out1, out2};
}
private static double dot(double[] a, double[] b) {
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
private static double[] normal(double[] a) {
double norm = Math.sqrt(dot(a, a));
return new double[]{a[0] / norm, a[1] / norm, a[2] / norm};
}
public void set(double[] v1, double[] v2) {
double[] vec1 = normal(v1);
double[] vec2 = normal(v2);
double[] axis = normal(cross(vec1, vec2));
double angle = Math.acos(dot(vec1, vec2));
set(angle, axis);
}
public static double calcAngle(double[] v1, double[] v2) {
double[] vec1 = normal(v1);
double[] vec2 = normal(v2);
return Math.acos(Math.min(dot(vec1, vec2), 1));
}
public static double[] calcAxis(double[] v1, double[] v2) {
double[] vec1 = normal(v1);
double[] vec2 = normal(v2);
return normal(cross(vec1, vec2));
}
public void set(double angle, double[] axis) {
x[0] = Math.cos(angle / 2);
double sin = Math.sin(angle / 2);
x[1] = axis[0] * sin;
x[2] = axis[1] * sin;
x[3] = axis[2] * sin;
}
public Quaternion(double x0, double x1, double x2, double x3) {
x[0] = x0;
x[1] = x1;
x[2] = x2;
x[3] = x3;
}
public Quaternion conjugate() {
return new Quaternion(x[0], -x[1], -x[2], -x[3]);
}
public Quaternion plus(Quaternion b) {
Quaternion a = this;
return new Quaternion(a.x[0] + b.x[0], a.x[1] + b.x[1], a.x[2] + b.x[2], a.x[3] + b.x[3]);
}
public Quaternion times(Quaternion b) {
Quaternion a = this;
double y0 = a.x[0] * b.x[0] - a.x[1] * b.x[1] - a.x[2] * b.x[2] - a.x[3] * b.x[3];
double y1 = a.x[0] * b.x[1] + a.x[1] * b.x[0] + a.x[2] * b.x[3] - a.x[3] * b.x[2];
double y2 = a.x[0] * b.x[2] - a.x[1] * b.x[3] + a.x[2] * b.x[0] + a.x[3] * b.x[1];
double y3 = a.x[0] * b.x[3] + a.x[1] * b.x[2] - a.x[2] * b.x[1] + a.x[3] * b.x[0];
return new Quaternion(y0, y1, y2, y3);
}
public Quaternion inverse() {
double d = x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3];
return new Quaternion(x[0] / d, -x[1] / d, -x[2] / d, -x[3] / d);
}
public Quaternion divides(Quaternion b) {
Quaternion a = this;
return a.inverse().times(b);
}
public double[] rotateVec(double[] v) {
double v0 = v[0];
double v1 = v[1];
double v2 = v[2];
double s = x[1] * v0 + x[2] * v1 + x[3] * v2;
double n0 = 2 * (x[0] * (v0 * x[0] - (x[2] * v2 - x[3] * v1)) + s * x[1]) - v0;
double n1 = 2 * (x[0] * (v1 * x[0] - (x[3] * v0 - x[1] * v2)) + s * x[2]) - v1;
double n2 = 2 * (x[0] * (v2 * x[0] - (x[1] * v1 - x[2] * v0)) + s * x[3]) - v2;
return new double[]{n0, n1, n2};
}
void matrix() {
double xx = x[1] * x[1];
double xy = x[1] * x[2];
double xz = x[1] * x[3];
double xw = x[1] * x[0];
double yy = x[2] * x[2];
double yz = x[2] * x[3];
double yw = x[2] * x[0];
double zz = x[3] * x[3];
double zw = x[3] * x[0];
double[] m = new double[16];
m[0] = 1 - 2 * (yy + zz);
m[1] = 2 * (xy - zw);
m[2] = 2 * (xz + yw);
m[4] = 2 * (xy + zw);
m[5] = 1 - 2 * (xx + zz);
m[6] = 2 * (yz - xw);
m[8] = 2 * (xz - yw);
m[9] = 2 * (yz + xw);
m[10] = 1 - 2 * (xx + yy);
m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0;
m[15] = 1;
}
}