git.gag.com Git - fw/altos/blob - altoslib/AltosQuaternion.java

   1 /*
   2  * Copyright © 2014 Keith Packard <keithp@keithp.com>
   3  *
   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.
   8  *
   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.
  13  *
  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.
  17  */
  18
  19 package org.altusmetrum.altoslib_13;
  20
  21 public class AltosQuaternion {
  22         double  r;              /* real bit */
  23         double  x, y, z;        /* imaginary bits */
  24
  25         /* Multiply by b */
  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);
  32         }
  33
  34         public AltosQuaternion conjugate() {
  35                 return new AltosQuaternion(this.r,
  36                                            -this.x,
  37                                            -this.y,
  38                                            -this.z);
  39         }
  40
  41         public double normal() {
  42                 return Math.sqrt(this.r * this.r +
  43                                  this.x * this.x +
  44                                  this.y * this.y +
  45                                  this.z * this.z);
  46         }
  47
  48         /* Scale by a real value */
  49         public AltosQuaternion scale(double b) {
  50                 return new AltosQuaternion(this.r * b,
  51                                            this.x * b,
  52                                            this.y * b,
  53                                            this.z * b);
  54         }
  55
  56         /* Divide by the length to end up with a quaternion of length 1 */
  57         public AltosQuaternion normalize() {
  58                 double  n = normal();
  59                 if (n <= 0)
  60                         return this;
  61                 return scale(1/n);
  62         }
  63
  64         /* dot product */
  65         public double dot(AltosQuaternion b) {
  66                 return (this.r * b.r +
  67                         this.x * b.x +
  68                         this.y * b.y +
  69                         this.z * b.z);
  70         }
  71
  72         /* Rotate 'this' by 'b' */
  73         public AltosQuaternion rotate(AltosQuaternion b) {
  74                 return (b.multiply(this)).multiply(b.conjugate());
  75         }
  76
  77         /* Given two vectors (this and b), compute a quaternion
  78          * representing the rotation between them
  79          */
  80         public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
  81                 /*
  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.
  85                  */
  86
  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;
  90
  91                 double s_2 = x*x + y*y + z*z;
  92                 double s = Math.sqrt(s_2);
  93
  94                 /* cos(θ) = a · b / (|a| |b|).
  95                  *
  96                  * a and b are both unit vectors, so the divisor is one
  97                  */
  98                 double c = this.x*b.x + this.y*b.y + this.z*b.z;
  99
 100                 double c_half = Math.sqrt ((1 + c) / 2);
 101                 double s_half = Math.sqrt ((1 - c) / 2);
 102
 103                 /*
 104                  * Divide out the sine factor from the
 105                  * cross product, then multiply in the
 106                  * half sine factor needed for the quaternion
 107                  */
 108                 double s_scale = s_half / s;
 109
 110                 AltosQuaternion r = new AltosQuaternion(c_half,
 111                                                         x * s_scale,
 112                                                         y * s_scale,
 113                                                         z * s_scale);
 114                 return r.normalize();
 115         }
 116
 117         public AltosQuaternion(double r, double x, double y, double z) {
 118                 this.r = r;
 119                 this.x = x;
 120                 this.y = y;
 121                 this.z = z;
 122         }
 123
 124         public AltosQuaternion(AltosQuaternion q) {
 125                 r = q.r;
 126                 x = q.x;
 127                 y = q.y;
 128                 z = q.z;
 129         }
 130
 131         public AltosQuaternion() {
 132                 r = 1;
 133                 x = 0;
 134                 y = 0;
 135                 z = 0;
 136         }
 137
 138         static public AltosQuaternion vector(double x, double y, double z) {
 139                 return new AltosQuaternion(0, x, y, z);
 140         }
 141
 142         static public AltosQuaternion rotation(double x, double y, double z,
 143                                                double s, double c) {
 144                 return new AltosQuaternion(c,
 145                                            s*x,
 146                                            s*y,
 147                                            s*z);
 148         }
 149
 150         static public AltosQuaternion zero_rotation() {
 151                 return new AltosQuaternion(1, 0, 0, 0);
 152         }
 153
 154         static public AltosQuaternion euler(double x, double y, double z) {
 155
 156                 /* Halve the euler angles */
 157                 x = x / 2.0;
 158                 y = y / 2.0;
 159                 z = z / 2.0;
 160
 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);;
 164
 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);
 169         }
 170 }