1 /* 2 * Copyright (C) 2015 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 package com.example.android.rs.vr.engine; 17 18 /** 19 * Generic Quaternion 20 * Written for maximum portability between desktop and Android 21 * Not in performance critical sections 22 */ 23 public class Quaternion { 24 private final double[] x = new double[4]; // w,x,y,z, 25 set(double w, double x, double y, double z)26 public void set(double w, double x, double y, double z) { 27 this.x[0] = w; 28 this.x[1] = x; 29 this.x[2] = y; 30 this.x[3] = z; 31 } 32 clone(Quaternion src)33 public void clone(Quaternion src) { 34 System.arraycopy(src.x, 0, x, 0, x.length); 35 } 36 cross(double[] a, double[] b)37 private static double[] cross(double[] a, double[] b) { 38 double out0 = a[1] * b[2] - b[1] * a[2]; 39 double out1 = a[2] * b[0] - b[2] * a[0]; 40 double out2 = a[0] * b[1] - b[0] * a[1]; 41 return new double[]{out0, out1, out2}; 42 } 43 dot(double[] a, double[] b)44 private static double dot(double[] a, double[] b) { 45 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; 46 } 47 normal(double[] a)48 private static double[] normal(double[] a) { 49 double norm = Math.sqrt(dot(a, a)); 50 return new double[]{a[0] / norm, a[1] / norm, a[2] / norm}; 51 } 52 set(double[] v1, double[] v2)53 public void set(double[] v1, double[] v2) { 54 double[] vec1 = normal(v1); 55 double[] vec2 = normal(v2); 56 double[] axis = normal(cross(vec1, vec2)); 57 double angle = Math.acos(dot(vec1, vec2)); 58 set(angle, axis); 59 } 60 calcAngle(double[] v1, double[] v2)61 public static double calcAngle(double[] v1, double[] v2) { 62 double[] vec1 = normal(v1); 63 double[] vec2 = normal(v2); 64 return Math.acos(Math.min(dot(vec1, vec2), 1)); 65 } 66 calcAxis(double[] v1, double[] v2)67 public static double[] calcAxis(double[] v1, double[] v2) { 68 double[] vec1 = normal(v1); 69 double[] vec2 = normal(v2); 70 return normal(cross(vec1, vec2)); 71 } 72 set(double angle, double[] axis)73 public void set(double angle, double[] axis) { 74 x[0] = Math.cos(angle / 2); 75 double sin = Math.sin(angle / 2); 76 x[1] = axis[0] * sin; 77 x[2] = axis[1] * sin; 78 x[3] = axis[2] * sin; 79 } 80 Quaternion(double x0, double x1, double x2, double x3)81 public Quaternion(double x0, double x1, double x2, double x3) { 82 x[0] = x0; 83 x[1] = x1; 84 x[2] = x2; 85 x[3] = x3; 86 } 87 conjugate()88 public Quaternion conjugate() { 89 return new Quaternion(x[0], -x[1], -x[2], -x[3]); 90 } 91 plus(Quaternion b)92 public Quaternion plus(Quaternion b) { 93 Quaternion a = this; 94 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]); 95 } 96 times(Quaternion b)97 public Quaternion times(Quaternion b) { 98 Quaternion a = this; 99 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]; 100 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]; 101 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]; 102 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]; 103 return new Quaternion(y0, y1, y2, y3); 104 } 105 inverse()106 public Quaternion inverse() { 107 double d = x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]; 108 return new Quaternion(x[0] / d, -x[1] / d, -x[2] / d, -x[3] / d); 109 } 110 divides(Quaternion b)111 public Quaternion divides(Quaternion b) { 112 Quaternion a = this; 113 return a.inverse().times(b); 114 } 115 116 rotateVec(double[] v)117 public double[] rotateVec(double[] v) { 118 119 double v0 = v[0]; 120 double v1 = v[1]; 121 double v2 = v[2]; 122 123 double s = x[1] * v0 + x[2] * v1 + x[3] * v2; 124 125 double n0 = 2 * (x[0] * (v0 * x[0] - (x[2] * v2 - x[3] * v1)) + s * x[1]) - v0; 126 double n1 = 2 * (x[0] * (v1 * x[0] - (x[3] * v0 - x[1] * v2)) + s * x[2]) - v1; 127 double n2 = 2 * (x[0] * (v2 * x[0] - (x[1] * v1 - x[2] * v0)) + s * x[3]) - v2; 128 129 return new double[]{n0, n1, n2}; 130 131 } 132 matrix()133 void matrix() { 134 double xx = x[1] * x[1]; 135 double xy = x[1] * x[2]; 136 double xz = x[1] * x[3]; 137 double xw = x[1] * x[0]; 138 139 double yy = x[2] * x[2]; 140 double yz = x[2] * x[3]; 141 double yw = x[2] * x[0]; 142 143 double zz = x[3] * x[3]; 144 double zw = x[3] * x[0]; 145 double[] m = new double[16]; 146 m[0] = 1 - 2 * (yy + zz); 147 m[1] = 2 * (xy - zw); 148 m[2] = 2 * (xz + yw); 149 150 m[4] = 2 * (xy + zw); 151 m[5] = 1 - 2 * (xx + zz); 152 m[6] = 2 * (yz - xw); 153 154 m[8] = 2 * (xz - yw); 155 m[9] = 2 * (yz + xw); 156 m[10] = 1 - 2 * (xx + yy); 157 158 m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0; 159 m[15] = 1; 160 } 161 } 162