15b539461dcc159bd89297443780d635ccc5e3564John Hoford/* 25b539461dcc159bd89297443780d635ccc5e3564John Hoford * Copyright (C) 2015 The Android Open Source Project 35b539461dcc159bd89297443780d635ccc5e3564John Hoford * 45b539461dcc159bd89297443780d635ccc5e3564John Hoford * Licensed under the Apache License, Version 2.0 (the "License"); 55b539461dcc159bd89297443780d635ccc5e3564John Hoford * you may not use this file except in compliance with the License. 65b539461dcc159bd89297443780d635ccc5e3564John Hoford * You may obtain a copy of the License at 75b539461dcc159bd89297443780d635ccc5e3564John Hoford * 85b539461dcc159bd89297443780d635ccc5e3564John Hoford * http://www.apache.org/licenses/LICENSE-2.0 95b539461dcc159bd89297443780d635ccc5e3564John Hoford * 105b539461dcc159bd89297443780d635ccc5e3564John Hoford * Unless required by applicable law or agreed to in writing, software 115b539461dcc159bd89297443780d635ccc5e3564John Hoford * distributed under the License is distributed on an "AS IS" BASIS, 125b539461dcc159bd89297443780d635ccc5e3564John Hoford * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 135b539461dcc159bd89297443780d635ccc5e3564John Hoford * See the License for the specific language governing permissions and 145b539461dcc159bd89297443780d635ccc5e3564John Hoford * limitations under the License. 155b539461dcc159bd89297443780d635ccc5e3564John Hoford */ 165b539461dcc159bd89297443780d635ccc5e3564John Hofordpackage com.example.android.rs.vr.engine; 175b539461dcc159bd89297443780d635ccc5e3564John Hoford 185b539461dcc159bd89297443780d635ccc5e3564John Hoford/** 195b539461dcc159bd89297443780d635ccc5e3564John Hoford * Generic Quaternion 205b539461dcc159bd89297443780d635ccc5e3564John Hoford * Written for maximum portability between desktop and Android 215b539461dcc159bd89297443780d635ccc5e3564John Hoford * Not in performance critical sections 225b539461dcc159bd89297443780d635ccc5e3564John Hoford */ 235b539461dcc159bd89297443780d635ccc5e3564John Hofordpublic class Quaternion { 245b539461dcc159bd89297443780d635ccc5e3564John Hoford private final double[] x = new double[4]; // w,x,y,z, 255b539461dcc159bd89297443780d635ccc5e3564John Hoford 265b539461dcc159bd89297443780d635ccc5e3564John Hoford public void set(double w, double x, double y, double z) { 275b539461dcc159bd89297443780d635ccc5e3564John Hoford this.x[0] = w; 285b539461dcc159bd89297443780d635ccc5e3564John Hoford this.x[1] = x; 295b539461dcc159bd89297443780d635ccc5e3564John Hoford this.x[2] = y; 305b539461dcc159bd89297443780d635ccc5e3564John Hoford this.x[3] = z; 315b539461dcc159bd89297443780d635ccc5e3564John Hoford } 325b539461dcc159bd89297443780d635ccc5e3564John Hoford 335b539461dcc159bd89297443780d635ccc5e3564John Hoford public void clone(Quaternion src) { 345b539461dcc159bd89297443780d635ccc5e3564John Hoford System.arraycopy(src.x, 0, x, 0, x.length); 355b539461dcc159bd89297443780d635ccc5e3564John Hoford } 365b539461dcc159bd89297443780d635ccc5e3564John Hoford 375b539461dcc159bd89297443780d635ccc5e3564John Hoford private static double[] cross(double[] a, double[] b) { 385b539461dcc159bd89297443780d635ccc5e3564John Hoford double out0 = a[1] * b[2] - b[1] * a[2]; 395b539461dcc159bd89297443780d635ccc5e3564John Hoford double out1 = a[2] * b[0] - b[2] * a[0]; 405b539461dcc159bd89297443780d635ccc5e3564John Hoford double out2 = a[0] * b[1] - b[0] * a[1]; 415b539461dcc159bd89297443780d635ccc5e3564John Hoford return new double[]{out0, out1, out2}; 425b539461dcc159bd89297443780d635ccc5e3564John Hoford } 435b539461dcc159bd89297443780d635ccc5e3564John Hoford 445b539461dcc159bd89297443780d635ccc5e3564John Hoford private static double dot(double[] a, double[] b) { 455b539461dcc159bd89297443780d635ccc5e3564John Hoford return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; 465b539461dcc159bd89297443780d635ccc5e3564John Hoford } 475b539461dcc159bd89297443780d635ccc5e3564John Hoford 485b539461dcc159bd89297443780d635ccc5e3564John Hoford private static double[] normal(double[] a) { 495b539461dcc159bd89297443780d635ccc5e3564John Hoford double norm = Math.sqrt(dot(a, a)); 505b539461dcc159bd89297443780d635ccc5e3564John Hoford return new double[]{a[0] / norm, a[1] / norm, a[2] / norm}; 515b539461dcc159bd89297443780d635ccc5e3564John Hoford } 525b539461dcc159bd89297443780d635ccc5e3564John Hoford 535b539461dcc159bd89297443780d635ccc5e3564John Hoford public void set(double[] v1, double[] v2) { 545b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec1 = normal(v1); 555b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec2 = normal(v2); 565b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] axis = normal(cross(vec1, vec2)); 575b539461dcc159bd89297443780d635ccc5e3564John Hoford double angle = Math.acos(dot(vec1, vec2)); 585b539461dcc159bd89297443780d635ccc5e3564John Hoford set(angle, axis); 595b539461dcc159bd89297443780d635ccc5e3564John Hoford } 605b539461dcc159bd89297443780d635ccc5e3564John Hoford 615b539461dcc159bd89297443780d635ccc5e3564John Hoford public static double calcAngle(double[] v1, double[] v2) { 625b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec1 = normal(v1); 635b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec2 = normal(v2); 645b539461dcc159bd89297443780d635ccc5e3564John Hoford return Math.acos(Math.min(dot(vec1, vec2), 1)); 655b539461dcc159bd89297443780d635ccc5e3564John Hoford } 665b539461dcc159bd89297443780d635ccc5e3564John Hoford 675b539461dcc159bd89297443780d635ccc5e3564John Hoford public static double[] calcAxis(double[] v1, double[] v2) { 685b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec1 = normal(v1); 695b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] vec2 = normal(v2); 705b539461dcc159bd89297443780d635ccc5e3564John Hoford return normal(cross(vec1, vec2)); 715b539461dcc159bd89297443780d635ccc5e3564John Hoford } 725b539461dcc159bd89297443780d635ccc5e3564John Hoford 735b539461dcc159bd89297443780d635ccc5e3564John Hoford public void set(double angle, double[] axis) { 745b539461dcc159bd89297443780d635ccc5e3564John Hoford x[0] = Math.cos(angle / 2); 755b539461dcc159bd89297443780d635ccc5e3564John Hoford double sin = Math.sin(angle / 2); 765b539461dcc159bd89297443780d635ccc5e3564John Hoford x[1] = axis[0] * sin; 775b539461dcc159bd89297443780d635ccc5e3564John Hoford x[2] = axis[1] * sin; 785b539461dcc159bd89297443780d635ccc5e3564John Hoford x[3] = axis[2] * sin; 795b539461dcc159bd89297443780d635ccc5e3564John Hoford } 805b539461dcc159bd89297443780d635ccc5e3564John Hoford 815b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion(double x0, double x1, double x2, double x3) { 825b539461dcc159bd89297443780d635ccc5e3564John Hoford x[0] = x0; 835b539461dcc159bd89297443780d635ccc5e3564John Hoford x[1] = x1; 845b539461dcc159bd89297443780d635ccc5e3564John Hoford x[2] = x2; 855b539461dcc159bd89297443780d635ccc5e3564John Hoford x[3] = x3; 865b539461dcc159bd89297443780d635ccc5e3564John Hoford } 875b539461dcc159bd89297443780d635ccc5e3564John Hoford 885b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion conjugate() { 895b539461dcc159bd89297443780d635ccc5e3564John Hoford return new Quaternion(x[0], -x[1], -x[2], -x[3]); 905b539461dcc159bd89297443780d635ccc5e3564John Hoford } 915b539461dcc159bd89297443780d635ccc5e3564John Hoford 925b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion plus(Quaternion b) { 935b539461dcc159bd89297443780d635ccc5e3564John Hoford Quaternion a = this; 945b539461dcc159bd89297443780d635ccc5e3564John Hoford 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]); 955b539461dcc159bd89297443780d635ccc5e3564John Hoford } 965b539461dcc159bd89297443780d635ccc5e3564John Hoford 975b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion times(Quaternion b) { 985b539461dcc159bd89297443780d635ccc5e3564John Hoford Quaternion a = this; 995b539461dcc159bd89297443780d635ccc5e3564John Hoford 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]; 1005b539461dcc159bd89297443780d635ccc5e3564John Hoford 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]; 1015b539461dcc159bd89297443780d635ccc5e3564John Hoford 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]; 1025b539461dcc159bd89297443780d635ccc5e3564John Hoford 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]; 1035b539461dcc159bd89297443780d635ccc5e3564John Hoford return new Quaternion(y0, y1, y2, y3); 1045b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1055b539461dcc159bd89297443780d635ccc5e3564John Hoford 1065b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion inverse() { 1075b539461dcc159bd89297443780d635ccc5e3564John Hoford double d = x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]; 1085b539461dcc159bd89297443780d635ccc5e3564John Hoford return new Quaternion(x[0] / d, -x[1] / d, -x[2] / d, -x[3] / d); 1095b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1105b539461dcc159bd89297443780d635ccc5e3564John Hoford 1115b539461dcc159bd89297443780d635ccc5e3564John Hoford public Quaternion divides(Quaternion b) { 1125b539461dcc159bd89297443780d635ccc5e3564John Hoford Quaternion a = this; 1135b539461dcc159bd89297443780d635ccc5e3564John Hoford return a.inverse().times(b); 1145b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1155b539461dcc159bd89297443780d635ccc5e3564John Hoford 1165b539461dcc159bd89297443780d635ccc5e3564John Hoford 1175b539461dcc159bd89297443780d635ccc5e3564John Hoford public double[] rotateVec(double[] v) { 1185b539461dcc159bd89297443780d635ccc5e3564John Hoford 1195b539461dcc159bd89297443780d635ccc5e3564John Hoford double v0 = v[0]; 1205b539461dcc159bd89297443780d635ccc5e3564John Hoford double v1 = v[1]; 1215b539461dcc159bd89297443780d635ccc5e3564John Hoford double v2 = v[2]; 1225b539461dcc159bd89297443780d635ccc5e3564John Hoford 1235b539461dcc159bd89297443780d635ccc5e3564John Hoford double s = x[1] * v0 + x[2] * v1 + x[3] * v2; 1245b539461dcc159bd89297443780d635ccc5e3564John Hoford 1255b539461dcc159bd89297443780d635ccc5e3564John Hoford double n0 = 2 * (x[0] * (v0 * x[0] - (x[2] * v2 - x[3] * v1)) + s * x[1]) - v0; 1265b539461dcc159bd89297443780d635ccc5e3564John Hoford double n1 = 2 * (x[0] * (v1 * x[0] - (x[3] * v0 - x[1] * v2)) + s * x[2]) - v1; 1275b539461dcc159bd89297443780d635ccc5e3564John Hoford double n2 = 2 * (x[0] * (v2 * x[0] - (x[1] * v1 - x[2] * v0)) + s * x[3]) - v2; 1285b539461dcc159bd89297443780d635ccc5e3564John Hoford 1295b539461dcc159bd89297443780d635ccc5e3564John Hoford return new double[]{n0, n1, n2}; 1305b539461dcc159bd89297443780d635ccc5e3564John Hoford 1315b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1325b539461dcc159bd89297443780d635ccc5e3564John Hoford 1335b539461dcc159bd89297443780d635ccc5e3564John Hoford void matrix() { 1345b539461dcc159bd89297443780d635ccc5e3564John Hoford double xx = x[1] * x[1]; 1355b539461dcc159bd89297443780d635ccc5e3564John Hoford double xy = x[1] * x[2]; 1365b539461dcc159bd89297443780d635ccc5e3564John Hoford double xz = x[1] * x[3]; 1375b539461dcc159bd89297443780d635ccc5e3564John Hoford double xw = x[1] * x[0]; 1385b539461dcc159bd89297443780d635ccc5e3564John Hoford 1395b539461dcc159bd89297443780d635ccc5e3564John Hoford double yy = x[2] * x[2]; 1405b539461dcc159bd89297443780d635ccc5e3564John Hoford double yz = x[2] * x[3]; 1415b539461dcc159bd89297443780d635ccc5e3564John Hoford double yw = x[2] * x[0]; 1425b539461dcc159bd89297443780d635ccc5e3564John Hoford 1435b539461dcc159bd89297443780d635ccc5e3564John Hoford double zz = x[3] * x[3]; 1445b539461dcc159bd89297443780d635ccc5e3564John Hoford double zw = x[3] * x[0]; 1455b539461dcc159bd89297443780d635ccc5e3564John Hoford double[] m = new double[16]; 1465b539461dcc159bd89297443780d635ccc5e3564John Hoford m[0] = 1 - 2 * (yy + zz); 1475b539461dcc159bd89297443780d635ccc5e3564John Hoford m[1] = 2 * (xy - zw); 1485b539461dcc159bd89297443780d635ccc5e3564John Hoford m[2] = 2 * (xz + yw); 1495b539461dcc159bd89297443780d635ccc5e3564John Hoford 1505b539461dcc159bd89297443780d635ccc5e3564John Hoford m[4] = 2 * (xy + zw); 1515b539461dcc159bd89297443780d635ccc5e3564John Hoford m[5] = 1 - 2 * (xx + zz); 1525b539461dcc159bd89297443780d635ccc5e3564John Hoford m[6] = 2 * (yz - xw); 1535b539461dcc159bd89297443780d635ccc5e3564John Hoford 1545b539461dcc159bd89297443780d635ccc5e3564John Hoford m[8] = 2 * (xz - yw); 1555b539461dcc159bd89297443780d635ccc5e3564John Hoford m[9] = 2 * (yz + xw); 1565b539461dcc159bd89297443780d635ccc5e3564John Hoford m[10] = 1 - 2 * (xx + yy); 1575b539461dcc159bd89297443780d635ccc5e3564John Hoford 1585b539461dcc159bd89297443780d635ccc5e3564John Hoford m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0; 1595b539461dcc159bd89297443780d635ccc5e3564John Hoford m[15] = 1; 1605b539461dcc159bd89297443780d635ccc5e3564John Hoford } 1615b539461dcc159bd89297443780d635ccc5e3564John Hoford} 162