[go: nahoru, domu]
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 */
16package 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 */
23public class Quaternion {
24    private final double[] x = new double[4]; // w,x,y,z,
25
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
33    public void clone(Quaternion src) {
34        System.arraycopy(src.x, 0, x, 0, x.length);
35    }
36
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
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
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
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
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
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
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
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
88    public Quaternion conjugate() {
89        return new Quaternion(x[0], -x[1], -x[2], -x[3]);
90    }
91
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
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
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
111    public Quaternion divides(Quaternion b) {
112        Quaternion a = this;
113        return a.inverse().times(b);
114    }
115
116
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
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