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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 package org.altusmetrum.altoslib_5;
  19
  20 public class AltosQuaternion {
  21         double  r;              /* real bit */
  22         double  x, y, z;        /* imaginary bits */
  23
  24         public AltosQuaternion multiply(AltosQuaternion b) {
  25                 return new AltosQuaternion(
  26                         this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
  27                         this.r * b.x + this.x * b.r + this.y * b.z - this.z * b.y,
  28                         this.r * b.y - this.x * b.z + this.y * b.r + this.z * b.x,
  29                         this.r * b.z + this.x * b.y - this.y * b.x + this.z * b.r);
  30         }
  31
  32         public AltosQuaternion conjugate() {
  33                 return new AltosQuaternion(
  34                         this.r,
  35                         -this.x,
  36                         -this.y,
  37                         -this.z);
  38         }
  39
  40         public double normal() {
  41                 return (this.r * this.r +
  42                         this.x * this.x +
  43                         this.y * this.y +
  44                         this.z * this.z);
  45         }
  46
  47         public AltosQuaternion scale(double b) {
  48                 return new AltosQuaternion(
  49                         this.r * b,
  50                         this.x * b,
  51                         this.y * b,
  52                         this.z * b);
  53         }
  54
  55         public AltosQuaternion normalize() {
  56                 double  n = normal();
  57                 if (n <= 0)
  58                         return this;
  59                 return scale(1/Math.sqrt(n));
  60         }
  61
  62         public double dot(AltosQuaternion b) {
  63                 return (this.r * b.r +
  64                         this.x * b.x +
  65                         this.y * b.y +
  66                         this.z * b.z);
  67         }
  68
  69         public AltosQuaternion rotate(AltosQuaternion b) {
  70                 return (b.multiply(this)).multiply(b.conjugate());
  71         }
  72
  73         public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
  74                 /*
  75                  * The cross product will point orthogonally to the two
  76                  * vectors, forming our rotation axis. The length will be
  77                  * sin(θ), so these values are already multiplied by that.
  78                  */
  79
  80                 double x = this.y * b.z - this.z * b.y;
  81                 double y = this.z * b.x - this.x * b.z;
  82                 double z = this.x * b.y - this.y * b.x;
  83
  84                 double s_2 = x*x + y*y + z*z;
  85                 double s = Math.sqrt(s_2);
  86
  87                 /* cos(θ) = a · b / (|a| |b|).
  88                  *
  89                  * a and b are both unit vectors, so the divisor is one
  90                  */
  91                 double c = this.x*b.x + this.y*b.y + this.z*b.z;
  92
  93                 double c_half = Math.sqrt ((1 + c) / 2);
  94                 double s_half = Math.sqrt ((1 - c) / 2);
  95
  96                 /*
  97                  * Divide out the sine factor from the
  98                  * cross product, then multiply in the
  99                  * half sine factor needed for the quaternion
 100                  */
 101                 double s_scale = s_half / s;
 102
 103                 AltosQuaternion r = new AltosQuaternion(c_half,
 104                                                         x * s_scale,
 105                                                         y * s_scale,
 106                                                         z * s_scale);
 107                 return r.normalize();
 108         }
 109
 110         public AltosQuaternion(double r, double x, double y, double z) {
 111                 this.r = r;
 112                 this.x = x;
 113                 this.y = y;
 114                 this.z = z;
 115         }
 116
 117         public AltosQuaternion(AltosQuaternion q) {
 118                 this.r = q.r;
 119                 this.x = q.x;
 120                 this.y = q.y;
 121                 this.z = q.z;
 122         }
 123
 124         static public AltosQuaternion vector(double x, double y, double z) {
 125                 return new AltosQuaternion(0, x, y, z);
 126         }
 127
 128         static public AltosQuaternion rotation(double x, double y, double z,
 129                                                double s, double c) {
 130                 return new AltosQuaternion(c,
 131                                            s*x,
 132                                            s*y,
 133                                            s*z);
 134         }
 135
 136         static public AltosQuaternion zero_rotation() {
 137                 return new AltosQuaternion(1, 0, 0, 0);
 138         }
 139
 140         static public AltosQuaternion half_euler(double x, double y, double z) {
 141                 double  s_x = Math.sin(x), c_x = Math.cos(x);
 142                 double  s_y = Math.sin(y), c_y = Math.cos(y);
 143                 double  s_z = Math.sin(z), c_z = Math.cos(z);;
 144
 145                 return new AltosQuaternion(c_x * c_y * c_z + s_x * s_y * s_z,
 146                                            s_x * c_y * c_z - c_x * s_y * s_z,
 147                                            c_x * s_y * c_z + s_x * c_y * s_z,
 148                                            c_x * c_y * s_z - s_x * s_y * c_z);
 149         }
 150 }