2 * Copyright © 2014 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
19 package org.altusmetrum.altoslib_14;
21 public class AltosQuaternion {
22 double r; /* real bit */
23 double x, y, z; /* imaginary bits */
26 public AltosQuaternion multiply(AltosQuaternion b) {
27 return new AltosQuaternion(
28 this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
29 this.r * b.x + this.x * b.r + this.y * b.z - this.z * b.y,
30 this.r * b.y - this.x * b.z + this.y * b.r + this.z * b.x,
31 this.r * b.z + this.x * b.y - this.y * b.x + this.z * b.r);
34 public AltosQuaternion conjugate() {
35 return new AltosQuaternion(this.r,
41 public double normal() {
42 return Math.sqrt(this.r * this.r +
48 /* Scale by a real value */
49 public AltosQuaternion scale(double b) {
50 return new AltosQuaternion(this.r * b,
56 /* Divide by the length to end up with a quaternion of length 1 */
57 public AltosQuaternion normalize() {
65 public double dot(AltosQuaternion b) {
66 return (this.r * b.r +
72 /* Rotate 'this' by 'b' */
73 public AltosQuaternion rotate(AltosQuaternion b) {
74 return (b.multiply(this)).multiply(b.conjugate());
77 /* Given two vectors (this and b), compute a quaternion
78 * representing the rotation between them
80 public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
82 * The cross product will point orthogonally to the two
83 * vectors, forming our rotation axis. The length will be
84 * sin(θ), so these values are already multiplied by that.
87 double x = this.y * b.z - this.z * b.y;
88 double y = this.z * b.x - this.x * b.z;
89 double z = this.x * b.y - this.y * b.x;
91 double s_2 = x*x + y*y + z*z;
92 double s = Math.sqrt(s_2);
94 /* cos(θ) = a · b / (|a| |b|).
96 * a and b are both unit vectors, so the divisor is one
98 double c = this.x*b.x + this.y*b.y + this.z*b.z;
100 double c_half = Math.sqrt ((1 + c) / 2);
101 double s_half = Math.sqrt ((1 - c) / 2);
104 * Divide out the sine factor from the
105 * cross product, then multiply in the
106 * half sine factor needed for the quaternion
108 double s_scale = s_half / s;
110 AltosQuaternion r = new AltosQuaternion(c_half,
114 return r.normalize();
117 public AltosQuaternion(double r, double x, double y, double z) {
124 public AltosQuaternion(AltosQuaternion q) {
131 public AltosQuaternion() {
138 static public AltosQuaternion vector(double x, double y, double z) {
139 return new AltosQuaternion(0, x, y, z);
142 static public AltosQuaternion rotation(double x, double y, double z,
143 double s, double c) {
144 return new AltosQuaternion(c,
150 static public AltosQuaternion zero_rotation() {
151 return new AltosQuaternion(1, 0, 0, 0);
154 static public AltosQuaternion euler(double x, double y, double z) {
156 /* Halve the euler angles */
161 double s_x = Math.sin(x), c_x = Math.cos(x);
162 double s_y = Math.sin(y), c_y = Math.cos(y);
163 double s_z = Math.sin(z), c_z = Math.cos(z);;
165 return new AltosQuaternion(c_x * c_y * c_z + s_x * s_y * s_z,
166 s_x * c_y * c_z - c_x * s_y * s_z,
167 c_x * s_y * c_z + s_x * c_y * s_z,
168 c_x * c_y * s_z - s_x * s_y * c_z);