001 package com.croftsoft.core.math.quat; 002 003 import com.croftsoft.core.math.axis.AxisAngleImp; 004 import com.croftsoft.core.math.axis.AxisAngleMut; 005 import com.croftsoft.core.math.matrix.Matrix3x3Imp; 006 import com.croftsoft.core.math.matrix.Matrix3x3Mut; 007 008 /*********************************************************************** 009 * A library of static methods to manipulate Quat objects. 010 * 011 * @version 012 * $Id: QuatLib.java,v 1.10 2008/09/20 02:51:51 croft Exp $ 013 * @since 014 * 2008-05-02 015 * @author 016 * <a href="https://www.croftsoft.com/">David Wallace Croft</a> 017 ***********************************************************************/ 018 019 public final class QuatLib 020 //////////////////////////////////////////////////////////////////////// 021 //////////////////////////////////////////////////////////////////////// 022 { 023 024 public static double dotProduct ( 025 final Quat quat0, 026 final Quat quat1 ) 027 //////////////////////////////////////////////////////////////////////// 028 { 029 return quat0.getW ( ) * quat1.getW ( ) 030 * quat0.getX ( ) * quat1.getX ( ) 031 * quat0.getY ( ) * quat1.getY ( ) 032 * quat0.getZ ( ) * quat1.getZ ( ); 033 } 034 035 public static QuatMut fromEulerAngles ( 036 final double degreesX, 037 final double degreesY, 038 final double degreesZ ) 039 //////////////////////////////////////////////////////////////////////// 040 { 041 final Quat quatX = new AxisAngleImp ( degreesX, 0, 0, 1 ).toQuat ( ); 042 043 final Quat quatY = new AxisAngleImp ( degreesY, 0, 1, 0 ).toQuat ( ); 044 045 final Quat quatZ = new AxisAngleImp ( degreesZ, 0, 0, 1 ).toQuat ( ); 046 047 return quatZ.multiply ( quatY ).multiply ( quatX ); 048 } 049 050 public static boolean matches ( 051 final Quat quat0, 052 final Quat quat1 ) 053 //////////////////////////////////////////////////////////////////////// 054 { 055 return quat0.getW ( ) == quat1.getW ( ) 056 && quat0.getX ( ) == quat1.getX ( ) 057 && quat0.getY ( ) == quat1.getY ( ) 058 && quat0.getZ ( ) == quat1.getZ ( ); 059 } 060 061 public static boolean matches ( 062 final Quat quat0, 063 final Quat quat1, 064 final double tolerance ) 065 //////////////////////////////////////////////////////////////////////// 066 { 067 if ( tolerance < 0 ) 068 { 069 throw new IllegalArgumentException ( "tolerance < 0" ); 070 } 071 072 return Math.abs ( quat0.getW ( ) - quat1.getW ( ) ) <= tolerance 073 && Math.abs ( quat0.getX ( ) - quat1.getX ( ) ) <= tolerance 074 && Math.abs ( quat0.getY ( ) - quat1.getY ( ) ) <= tolerance 075 && Math.abs ( quat0.getZ ( ) - quat1.getZ ( ) ) <= tolerance; 076 } 077 078 public static QuatMut multiply ( 079 final Quat quat0, 080 final Quat quat1 ) 081 //////////////////////////////////////////////////////////////////////// 082 { 083 final double w0 = quat0.getW ( ); 084 085 final double x0 = quat0.getX ( ); 086 087 final double y0 = quat0.getY ( ); 088 089 final double z0 = quat0.getZ ( ); 090 091 final double w1 = quat1.getW ( ); 092 093 final double x1 = quat1.getX ( ); 094 095 final double y1 = quat1.getY ( ); 096 097 final double z1 = quat1.getZ ( ); 098 099 return new QuatImp ( 100 w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1, 101 y0 * z1 - z0 * y1 + w0 * x1 + x0 * w1, 102 z0 * x1 - x0 * z1 + w0 * y1 + y0 * w1, 103 x0 * y1 - y0 * x1 + w0 * z1 + z0 * w1 ); 104 } 105 106 public static AxisAngleMut toAxisAngle ( final Quat quat ) 107 //////////////////////////////////////////////////////////////////////// 108 { 109 final double w = quat.getW ( ); 110 111 final double sinThetaOver2Sq = 1 - w * w; 112 113 if ( sinThetaOver2Sq <= 0 ) 114 { 115 return new AxisAngleImp ( ); 116 } 117 118 final double oneOverSinThetaOver2 119 = 1 / Math.sqrt ( sinThetaOver2Sq ); 120 121 return new AxisAngleImp ( 122 Math.toDegrees ( 2 * Math.acos ( w ) ), 123 quat.getX ( ) * oneOverSinThetaOver2, 124 quat.getY ( ) * oneOverSinThetaOver2, 125 quat.getZ ( ) * oneOverSinThetaOver2 ); 126 } 127 128 public static Matrix3x3Mut toRotationMatrix ( final Quat quat ) 129 //////////////////////////////////////////////////////////////////////// 130 { 131 final double w = quat.getW ( ); 132 133 final double x = quat.getX ( ); 134 135 final double y = quat.getY ( ); 136 137 final double z = quat.getZ ( ); 138 139 final double wx = w * x; 140 141 final double wy = w * y; 142 143 final double wz = w * z; 144 145 final double xx = x * x; 146 147 final double xy = x * y; 148 149 final double xz = x * z; 150 151 final double yy = y * y; 152 153 final double yz = y * z; 154 155 final double zz = z * z; 156 157 return new Matrix3x3Imp ( 158 1 - 2 * ( yy + zz ), 159 2 * ( xy - wz ), 160 2 * ( wy + xz ), 161 2 * ( xy + wz ), 162 1 - 2 * ( xx + zz ), 163 2 * ( yz - wx ), 164 2 * ( xz - wy ), 165 2 * ( yz + wx ), 166 1 - 2 * ( xx + yy ) ); 167 } 168 169 public static String toString ( final Quat quat ) 170 //////////////////////////////////////////////////////////////////////// 171 { 172 return String.format ( 173 "%1$1.3f; %2$1.3f, %3$1.3f, %4$1.3f", 174 new Double ( quat.getW ( ) ), 175 new Double ( quat.getX ( ) ), 176 new Double ( quat.getY ( ) ), 177 new Double ( quat.getZ ( ) ) ); 178 } 179 180 //////////////////////////////////////////////////////////////////////// 181 // private methods 182 //////////////////////////////////////////////////////////////////////// 183 184 private QuatLib ( ) 185 //////////////////////////////////////////////////////////////////////// 186 { 187 // empty 188 } 189 190 //////////////////////////////////////////////////////////////////////// 191 //////////////////////////////////////////////////////////////////////// 192 }