23 | * Thanks to Riven on JavaGaming.org for the basis of sin/cos/atan2/floor/ceil. 24 | * @author Nathan Sweet */ 25 | public class MathUtils { 26 | static public final float nanoToSec = 1 / 1000000000f; 27 | 28 | // --- 29 | 30 | static public final float PI = 3.1415927f; 31 | 32 | static private final int SIN_BITS = 13; // Adjust for accuracy. 33 | static private final int SIN_MASK = ~(-1 << SIN_BITS); 34 | static private final int SIN_COUNT = SIN_MASK + 1; 35 | 36 | static private final float radFull = PI * 2; 37 | static private final float degFull = 360; 38 | static private final float radToIndex = SIN_COUNT / radFull; 39 | static private final float degToIndex = SIN_COUNT / degFull; 40 | 41 | static public final float radiansToDegrees = 180f / PI; 42 | static public final float radDeg = radiansToDegrees; 43 | static public final float degreesToRadians = PI / 180; 44 | static public final float degRad = degreesToRadians; 45 | 46 | static private class Sin { 47 | static final float[] table = new float[SIN_COUNT]; 48 | static { 49 | for (int i = 0; i < SIN_COUNT; i++) 50 | table[i] = (float)Math.sin((i + 0.5f) / SIN_COUNT * radFull); 51 | for (int i = 0; i < 360; i += 90) 52 | table[(int)(i * degToIndex) & SIN_MASK] = (float)Math.sin(i * degreesToRadians); 53 | } 54 | } 55 | 56 | static private class Cos { 57 | static final float[] table = new float[SIN_COUNT]; 58 | static { 59 | for (int i = 0; i < SIN_COUNT; i++) 60 | table[i] = (float)Math.cos((i + 0.5f) / SIN_COUNT * radFull); 61 | for (int i = 0; i < 360; i += 90) 62 | table[(int)(i * degToIndex) & SIN_MASK] = (float)Math.cos(i * degreesToRadians); 63 | } 64 | } 65 | 66 | /** Returns the sine in radians. */ 67 | static public final float sin (float radians) { 68 | return Sin.table[(int)(radians * radToIndex) & SIN_MASK]; 69 | } 70 | 71 | /** Returns the cosine in radians. */ 72 | static public final float cos (float radians) { 73 | return Cos.table[(int)(radians * radToIndex) & SIN_MASK]; 74 | } 75 | 76 | /** Returns the sine in radians. */ 77 | static public final float sinDeg (float degrees) { 78 | return Sin.table[(int)(degrees * degToIndex) & SIN_MASK]; 79 | } 80 | 81 | /** Returns the cosine in radians. */ 82 | static public final float cosDeg (float degrees) { 83 | return Cos.table[(int)(degrees * degToIndex) & SIN_MASK]; 84 | } 85 | 86 | // --- 87 | 88 | static private final int ATAN2_BITS = 7; // Adjust for accuracy. 89 | static private final int ATAN2_BITS2 = ATAN2_BITS << 1; 90 | static private final int ATAN2_MASK = ~(-1 << ATAN2_BITS2); 91 | static private final int ATAN2_COUNT = ATAN2_MASK + 1; 92 | static final int ATAN2_DIM = (int)Math.sqrt(ATAN2_COUNT); 93 | static private final float INV_ATAN2_DIM_MINUS_1 = 1.0f / (ATAN2_DIM - 1); 94 | 95 | static private class Atan2 { 96 | static final float[] table = new float[ATAN2_COUNT]; 97 | static { 98 | for (int i = 0; i < ATAN2_DIM; i++) { 99 | for (int j = 0; j < ATAN2_DIM; j++) { 100 | float x0 = (float)i / ATAN2_DIM; 101 | float y0 = (float)j / ATAN2_DIM; 102 | table[j * ATAN2_DIM + i] = (float)Math.atan2(y0, x0); 103 | } 104 | } 105 | } 106 | } 107 | 108 | /** Returns atan2 in radians from a lookup table. */ 109 | static public final float atan2 (float y, float x) { 110 | float add, mul; 111 | if (x < 0) { 112 | if (y < 0) { 113 | y = -y; 114 | mul = 1; 115 | } else 116 | mul = -1; 117 | x = -x; 118 | add = -PI; 119 | } else { 120 | if (y < 0) { 121 | y = -y; 122 | mul = -1; 123 | } else 124 | mul = 1; 125 | add = 0; 126 | } 127 | float invDiv = 1 / ((x < y ? y : x) * INV_ATAN2_DIM_MINUS_1); 128 | int xi = (int)(x * invDiv); 129 | int yi = (int)(y * invDiv); 130 | return (Atan2.table[yi * ATAN2_DIM + xi] + add) * mul; 131 | } 132 | 133 | // --- 134 | 135 | static public Random random = new Random(); 136 | 137 | /** Returns a random number between 0 (inclusive) and the specified value (inclusive). */ 138 | static public final int random (int range) { 139 | return random.nextInt(range + 1); 140 | } 141 | 142 | /** Returns a random number between start (inclusive) and end (inclusive). */ 143 | static public final int random (int start, int end) { 144 | return start + random.nextInt(end - start + 1); 145 | } 146 | 147 | static public final boolean randomBoolean () { 148 | return random.nextBoolean(); 149 | } 150 | 151 | static public final float random () { 152 | return random.nextFloat(); 153 | } 154 | 155 | /** Returns a random number between 0 (inclusive) and the specified value (inclusive). */ 156 | static public final float random (float range) { 157 | return random.nextFloat() * range; 158 | } 159 | 160 | /** Returns a random number between start (inclusive) and end (inclusive). */ 161 | static public final float random (float start, float end) { 162 | return start + random.nextFloat() * (end - start); 163 | } 164 | 165 | // --- 166 | 167 | /** Returns the next power of two. Returns the specified value if the value is already a power of two. */ 168 | static public int nextPowerOfTwo (int value) { 169 | if (value == 0) return 1; 170 | value--; 171 | value |= value >> 1; 172 | value |= value >> 2; 173 | value |= value >> 4; 174 | value |= value >> 8; 175 | value |= value >> 16; 176 | return value + 1; 177 | } 178 | 179 | static public boolean isPowerOfTwo (int value) { 180 | return value != 0 && (value & value - 1) == 0; 181 | } 182 | 183 | // --- 184 | 185 | static public int clamp (int value, int min, int max) { 186 | if (value < min) return min; 187 | if (value > max) return max; 188 | return value; 189 | } 190 | 191 | static public short clamp (short value, short min, short max) { 192 | if (value < min) return min; 193 | if (value > max) return max; 194 | return value; 195 | } 196 | 197 | static public float clamp (float value, float min, float max) { 198 | if (value < min) return min; 199 | if (value > max) return max; 200 | return value; 201 | } 202 | 203 | // --- 204 | 205 | static private final int BIG_ENOUGH_INT = 16 * 1024; 206 | static private final double BIG_ENOUGH_FLOOR = BIG_ENOUGH_INT; 207 | static private final double CEIL = 0.9999999; 208 | static private final double BIG_ENOUGH_CEIL = NumberUtils 209 | .longBitsToDouble(NumberUtils.doubleToLongBits(BIG_ENOUGH_INT + 1) - 1); 210 | static private final double BIG_ENOUGH_ROUND = BIG_ENOUGH_INT + 0.5f; 211 | 212 | /** Returns the largest integer less than or equal to the specified float. This method will only properly floor floats from 213 | * -(2^14) to (Float.MAX_VALUE - 2^14). */ 214 | static public int floor (float x) { 215 | return (int)(x + BIG_ENOUGH_FLOOR) - BIG_ENOUGH_INT; 216 | } 217 | 218 | /** Returns the largest integer less than or equal to the specified float. This method will only properly floor floats that are 219 | * positive. Note this method simply casts the float to int. */ 220 | static public int floorPositive (float x) { 221 | return (int)x; 222 | } 223 | 224 | /** Returns the smallest integer greater than or equal to the specified float. This method will only properly ceil floats from 225 | * -(2^14) to (Float.MAX_VALUE - 2^14). */ 226 | static public int ceil (float x) { 227 | return (int)(x + BIG_ENOUGH_CEIL) - BIG_ENOUGH_INT; 228 | } 229 | 230 | /** Returns the smallest integer greater than or equal to the specified float. This method will only properly ceil floats that 231 | * are positive. */ 232 | static public int ceilPositive (float x) { 233 | return (int)(x + CEIL); 234 | } 235 | 236 | /** Returns the closest integer to the specified float. This method will only properly round floats from -(2^14) to 237 | * (Float.MAX_VALUE - 2^14). */ 238 | static public int round (float x) { 239 | return (int)(x + BIG_ENOUGH_ROUND) - BIG_ENOUGH_INT; 240 | } 241 | 242 | /** Returns the closest integer to the specified float. This method will only properly round floats that are positive. */ 243 | static public int roundPositive (float x) { 244 | return (int)(x + 0.5f); 245 | } 246 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Matrix3.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | /******************************************************************************* 4 | * Copyright 2011 See AUTHORS file. 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | ******************************************************************************/ 18 | /** from libgdx: https://github.com/libgdx/libgdx */ 19 | import java.io.Serializable; 20 | 21 | /** A 3x3 column major matrix; useful for 2D transforms. 22 | * 23 | * @author mzechner */ 24 | public class Matrix3 implements Serializable { 25 | private static final long serialVersionUID = 7907569533774959788L; 26 | private final static float DEGREE_TO_RAD = (float)Math.PI / 180; 27 | public static final int M00 = 0; 28 | public static final int M01 = 3; 29 | public static final int M02 = 6; 30 | public static final int M10 = 1; 31 | public static final int M11 = 4; 32 | public static final int M12 = 7; 33 | public static final int M20 = 2; 34 | public static final int M21 = 5; 35 | public static final int M22 = 8; 36 | public float[] val = new float[9]; 37 | private float[] tmp = new float[9]; 38 | 39 | public Matrix3 () { 40 | idt(); 41 | } 42 | 43 | public Matrix3 (Matrix3 matrix) { 44 | set(matrix); 45 | } 46 | 47 | /** Sets this matrix to the identity matrix 48 | * @return This matrix for the purpose of chaining operations. */ 49 | public Matrix3 idt () { 50 | val[M00] = 1; 51 | val[M10] = 0; 52 | val[M20] = 0; 53 | val[M01] = 0; 54 | val[M11] = 1; 55 | val[M21] = 0; 56 | val[M02] = 0; 57 | val[M12] = 0; 58 | val[M22] = 1; 59 | return this; 60 | } 61 | 62 | /** Multiplies this matrix with the provided matrix and stores the result in this matrix. For example: 63 | * 64 | *
65 | * A.mul(B) results in A := AB 66 | *67 | * @param m Matrix to multiply by. 68 | * @return This matrix for the purpose of chaining operations together. */ 69 | public Matrix3 mul (Matrix3 m) { 70 | float v00 = val[M00] * m.val[M00] + val[M01] * m.val[M10] + val[M02] * m.val[M20]; 71 | float v01 = val[M00] * m.val[M01] + val[M01] * m.val[M11] + val[M02] * m.val[M21]; 72 | float v02 = val[M00] * m.val[M02] + val[M01] * m.val[M12] + val[M02] * m.val[M22]; 73 | 74 | float v10 = val[M10] * m.val[M00] + val[M11] * m.val[M10] + val[M12] * m.val[M20]; 75 | float v11 = val[M10] * m.val[M01] + val[M11] * m.val[M11] + val[M12] * m.val[M21]; 76 | float v12 = val[M10] * m.val[M02] + val[M11] * m.val[M12] + val[M12] * m.val[M22]; 77 | 78 | float v20 = val[M20] * m.val[M00] + val[M21] * m.val[M10] + val[M22] * m.val[M20]; 79 | float v21 = val[M20] * m.val[M01] + val[M21] * m.val[M11] + val[M22] * m.val[M21]; 80 | float v22 = val[M20] * m.val[M02] + val[M21] * m.val[M12] + val[M22] * m.val[M22]; 81 | 82 | val[M00] = v00; 83 | val[M10] = v10; 84 | val[M20] = v20; 85 | val[M01] = v01; 86 | val[M11] = v11; 87 | val[M21] = v21; 88 | val[M02] = v02; 89 | val[M12] = v12; 90 | val[M22] = v22; 91 | 92 | return this; 93 | } 94 | 95 | /** Sets this matrix to a rotation matrix that will rotate any vector in counter-clockwise order around the z-axis. 96 | * @param degrees the angle in degrees. 97 | * @return This matrix for the purpose of chaining operations. */ 98 | public Matrix3 setToRotation (float degrees) { 99 | float angle = DEGREE_TO_RAD * degrees; 100 | float cos = (float)Math.cos(angle); 101 | float sin = (float)Math.sin(angle); 102 | 103 | this.val[M00] = cos; 104 | this.val[M10] = sin; 105 | this.val[M20] = 0; 106 | 107 | this.val[M01] = -sin; 108 | this.val[M11] = cos; 109 | this.val[M21] = 0; 110 | 111 | this.val[M02] = 0; 112 | this.val[M12] = 0; 113 | this.val[M22] = 1; 114 | 115 | return this; 116 | } 117 | 118 | /** Sets this matrix to a translation matrix. 119 | * @param x the translation in x 120 | * @param y the translation in y 121 | * @return This matrix for the purpose of chaining operations. */ 122 | public Matrix3 setToTranslation (float x, float y) { 123 | this.val[M00] = 1; 124 | this.val[M10] = 0; 125 | this.val[M20] = 0; 126 | 127 | this.val[M01] = 0; 128 | this.val[M11] = 1; 129 | this.val[M21] = 0; 130 | 131 | this.val[M02] = x; 132 | this.val[M12] = y; 133 | this.val[M22] = 1; 134 | 135 | return this; 136 | } 137 | 138 | /** Sets this matrix to a translation matrix. 139 | * @param translation The translation vector. 140 | * @return This matrix for the purpose of chaining operations. */ 141 | public Matrix3 setToTranslation (Vector2 translation) { 142 | this.val[M00] = 1; 143 | this.val[M10] = 0; 144 | this.val[M20] = 0; 145 | 146 | this.val[M01] = 0; 147 | this.val[M11] = 1; 148 | this.val[M21] = 0; 149 | 150 | this.val[M02] = translation.x; 151 | this.val[M12] = translation.y; 152 | this.val[M22] = 1; 153 | 154 | return this; 155 | } 156 | 157 | /** Sets this matrix to a scaling matrix. 158 | * 159 | * @param scaleX the scale in x 160 | * @param scaleY the scale in y 161 | * @return This matrix for the purpose of chaining operations. */ 162 | public Matrix3 setToScaling (float scaleX, float scaleY) { 163 | val[M00] = scaleX; 164 | val[M10] = 0; 165 | val[M20] = 0; 166 | val[M01] = 0; 167 | val[M11] = scaleY; 168 | val[M21] = 0; 169 | val[M02] = 0; 170 | val[M12] = 0; 171 | val[M22] = 1; 172 | return this; 173 | } 174 | 175 | public String toString () { 176 | return "[" + val[0] + "|" + val[3] + "|" + val[6] + "]\n" + "[" + val[1] + "|" + val[4] + "|" + val[7] + "]\n" + "[" 177 | + val[2] + "|" + val[5] + "|" + val[8] + "]"; 178 | } 179 | 180 | /** @return The determinant of this matrix */ 181 | public float det () { 182 | return val[M00] * val[M11] * val[M22] + val[M01] * val[M12] * val[M20] + val[M02] * val[M10] * val[M21] - val[M00] 183 | * val[M12] * val[M21] - val[M01] * val[M10] * val[M22] - val[M02] * val[M11] * val[M20]; 184 | } 185 | 186 | /** Inverts this matrix given that the determinant is != 0. 187 | * @return This matrix for the purpose of chaining operations. */ 188 | public Matrix3 inv () { 189 | float det = det(); 190 | if (det == 0) throw new Error("Can't invert a singular matrix"); // changed to error from GdxRuntimeException 191 | 192 | float inv_det = 1.0f / det; 193 | 194 | tmp[M00] = val[M11] * val[M22] - val[M21] * val[M12]; 195 | tmp[M10] = val[M20] * val[M12] - val[M10] * val[M22]; 196 | tmp[M20] = val[M10] * val[M21] - val[M20] * val[M11]; 197 | tmp[M01] = val[M21] * val[M02] - val[M01] * val[M22]; 198 | tmp[M11] = val[M00] * val[M22] - val[M20] * val[M02]; 199 | tmp[M21] = val[M20] * val[M01] - val[M00] * val[M21]; 200 | tmp[M02] = val[M01] * val[M12] - val[M11] * val[M02]; 201 | tmp[M12] = val[M10] * val[M02] - val[M00] * val[M12]; 202 | tmp[M22] = val[M00] * val[M11] - val[M10] * val[M01]; 203 | 204 | val[M00] = inv_det * tmp[M00]; 205 | val[M10] = inv_det * tmp[M10]; 206 | val[M20] = inv_det * tmp[M20]; 207 | val[M01] = inv_det * tmp[M01]; 208 | val[M11] = inv_det * tmp[M11]; 209 | val[M21] = inv_det * tmp[M21]; 210 | val[M02] = inv_det * tmp[M02]; 211 | val[M12] = inv_det * tmp[M12]; 212 | val[M22] = inv_det * tmp[M22]; 213 | 214 | return this; 215 | } 216 | 217 | /** Copies the values from the provided matrix to this matrix. 218 | * @param mat The matrix to copy. 219 | * @return This matrix for the purposes of chaining. */ 220 | public Matrix3 set (Matrix3 mat) { 221 | System.arraycopy(mat.val, 0, val, 0, val.length); 222 | return this; 223 | } 224 | 225 | /** Sets this 3x3 matrix to the top left 3x3 corner of the provided 4x4 matrix. 226 | * @param mat The matrix whose top left corner will be copied. This matrix will not be modified. 227 | * @return This matrix for the purpose of chaining operations. */ 228 | public Matrix3 set (Matrix4 mat) { 229 | val[M00] = mat.val[Matrix4.M00]; 230 | val[M10] = mat.val[Matrix4.M10]; 231 | val[M20] = mat.val[Matrix4.M20]; 232 | val[M01] = mat.val[Matrix4.M01]; 233 | val[M11] = mat.val[Matrix4.M11]; 234 | val[M21] = mat.val[Matrix4.M21]; 235 | val[M02] = mat.val[Matrix4.M02]; 236 | val[M12] = mat.val[Matrix4.M12]; 237 | val[M22] = mat.val[Matrix4.M22]; 238 | return this; 239 | } 240 | 241 | /** Adds a translational component to the matrix in the 3rd column. The other columns are untouched. 242 | * @param vector The translation vector. 243 | * @return This matrix for the purpose of chaining. */ 244 | public Matrix3 trn (Vector2 vector) { 245 | val[M02] += vector.x; 246 | val[M12] += vector.y; 247 | return this; 248 | } 249 | 250 | /** Adds a translational component to the matrix in the 3rd column. The other columns are untouched. 251 | * @param x The x-component of the translation vector. 252 | * @param y The y-component of the translation vector. 253 | * @return This matrix for the purpose of chaining. */ 254 | public Matrix3 trn (float x, float y) { 255 | val[M02] += x; 256 | val[M12] += y; 257 | return this; 258 | } 259 | 260 | /** Adds a translational component to the matrix in the 3rd column. The other columns are untouched. 261 | * @param vector The translation vector. (The z-component of the vector is ignored because this is a 3x3 matrix) 262 | * @return This matrix for the purpose of chaining. */ 263 | public Matrix3 trn (Vector3 vector) { 264 | val[M02] += vector.x; 265 | val[M12] += vector.y; 266 | return this; 267 | } 268 | 269 | /** Postmultiplies this matrix by a translation matrix. Postmultiplication is also used by OpenGL ES' 1.x 270 | * glTranslate/glRotate/glScale. 271 | * @param x The x-component of the translation vector. 272 | * @param y The y-component of the translation vector. 273 | * @return This matrix for the purpose of chaining. */ 274 | public Matrix3 translate (float x, float y) { 275 | tmp[M00] = 1; 276 | tmp[M10] = 0; 277 | tmp[M20] = 0; 278 | 279 | tmp[M01] = 0; 280 | tmp[M11] = 1; 281 | tmp[M21] = 0; 282 | 283 | tmp[M02] = x; 284 | tmp[M12] = y; 285 | tmp[M22] = 1; 286 | mul(val, tmp); 287 | return this; 288 | } 289 | 290 | /** Postmultiplies this matrix by a translation matrix. Postmultiplication is also used by OpenGL ES' 1.x 291 | * glTranslate/glRotate/glScale. 292 | * @param translation The translation vector. 293 | * @return This matrix for the purpose of chaining. */ 294 | public Matrix3 translate (Vector2 translation) { 295 | tmp[M00] = 1; 296 | tmp[M10] = 0; 297 | tmp[M20] = 0; 298 | 299 | tmp[M01] = 0; 300 | tmp[M11] = 1; 301 | tmp[M21] = 0; 302 | 303 | tmp[M02] = translation.x; 304 | tmp[M12] = translation.y; 305 | tmp[M22] = 1; 306 | mul(val, tmp); 307 | return this; 308 | } 309 | 310 | /** Postmultiplies this matrix with a (counter-clockwise) rotation matrix. Postmultiplication is also used by OpenGL ES' 1.x 311 | * glTranslate/glRotate/glScale. 312 | * @param angle The angle in degrees 313 | * @return This matrix for the purpose of chaining. */ 314 | public Matrix3 rotate (float angle) { 315 | if (angle == 0) return this; 316 | angle = DEGREE_TO_RAD * angle; 317 | float cos = (float)Math.cos(angle); 318 | float sin = (float)Math.sin(angle); 319 | 320 | tmp[M00] = cos; 321 | tmp[M10] = sin; 322 | tmp[M20] = 0; 323 | 324 | tmp[M01] = -sin; 325 | tmp[M11] = cos; 326 | tmp[M21] = 0; 327 | 328 | tmp[M02] = 0; 329 | tmp[M12] = 0; 330 | tmp[M22] = 1; 331 | mul(val, tmp); 332 | return this; 333 | } 334 | 335 | /** Postmultiplies this matrix with a scale matrix. Postmultiplication is also used by OpenGL ES' 1.x 336 | * glTranslate/glRotate/glScale. 337 | * @param scaleX The scale in the x-axis. 338 | * @param scaleY The scale in the y-axis. 339 | * @return This matrix for the purpose of chaining. */ 340 | public Matrix3 scale (float scaleX, float scaleY) { 341 | tmp[M00] = scaleX; 342 | tmp[M10] = 0; 343 | tmp[M20] = 0; 344 | tmp[M01] = 0; 345 | tmp[M11] = scaleY; 346 | tmp[M21] = 0; 347 | tmp[M02] = 0; 348 | tmp[M12] = 0; 349 | tmp[M22] = 1; 350 | mul(val, tmp); 351 | return this; 352 | } 353 | 354 | /** Postmultiplies this matrix with a scale matrix. Postmultiplication is also used by OpenGL ES' 1.x 355 | * glTranslate/glRotate/glScale. 356 | * @param scale The vector to scale the matrix by. 357 | * @return This matrix for the purpose of chaining. */ 358 | public Matrix3 scale (Vector2 scale) { 359 | tmp[M00] = scale.x; 360 | tmp[M10] = 0; 361 | tmp[M20] = 0; 362 | tmp[M01] = 0; 363 | tmp[M11] = scale.y; 364 | tmp[M21] = 0; 365 | tmp[M02] = 0; 366 | tmp[M12] = 0; 367 | tmp[M22] = 1; 368 | mul(val, tmp); 369 | return this; 370 | } 371 | 372 | /** Get the values in this matrix. 373 | * @return The float values that make up this matrix in column-major order. */ 374 | public float[] getValues () { 375 | return val; 376 | } 377 | 378 | /** Scale the matrix in the both the x and y components by the scalar value. 379 | * @param scale The single value that will be used to scale both the x and y components. 380 | * @return This matrix for the purpose of chaining methods together. */ 381 | public Matrix3 scl (float scale) { 382 | val[M00] *= scale; 383 | val[M11] *= scale; 384 | return this; 385 | } 386 | 387 | /** Scale this matrix using the x and y components of the vector but leave the rest of the matrix alone. 388 | * @param scale The {@link Vector3} to use to scale this matrix. 389 | * @return This matrix for the purpose of chaining methods together. */ 390 | public Matrix3 scl (Vector2 scale) { 391 | val[M00] *= scale.x; 392 | val[M11] *= scale.y; 393 | return this; 394 | } 395 | 396 | /** Scale this matrix using the x and y components of the vector but leave the rest of the matrix alone. 397 | * @param scale The {@link Vector3} to use to scale this matrix. The z component will be ignored. 398 | * @return This matrix for the purpose of chaining methods together. */ 399 | public Matrix3 scl (Vector3 scale) { 400 | val[M00] *= scale.x; 401 | val[M11] *= scale.y; 402 | return this; 403 | } 404 | 405 | /** Transposes the current matrix. 406 | * @return This matrix for the purpose of chaining methods together. */ 407 | public Matrix3 transpose () { 408 | // Where MXY you do not have to change MXX 409 | float v01 = val[M10]; 410 | float v02 = val[M20]; 411 | float v10 = val[M01]; 412 | float v12 = val[M21]; 413 | float v20 = val[M02]; 414 | float v21 = val[M12]; 415 | val[M01] = v01; 416 | val[M02] = v02; 417 | val[M10] = v10; 418 | val[M12] = v12; 419 | val[M20] = v20; 420 | val[M21] = v21; 421 | return this; 422 | } 423 | 424 | /** Multiplies matrix a with matrix b in the following manner: 425 | * 426 | *
427 | * mul(A, B) => A := AB 428 | *429 | * @param mata The float array representing the first matrix. Must have at least 9 elements. 430 | * @param matb The float array representing the second matrix. Must have at least 9 elements. */ 431 | private static void mul (float[] mata, float[] matb) { 432 | float v00 = mata[M00] * matb[M00] + mata[M01] * matb[M10] + mata[M02] * matb[M20]; 433 | float v01 = mata[M00] * matb[M01] + mata[M01] * matb[M11] + mata[M02] * matb[M21]; 434 | float v02 = mata[M00] * matb[M02] + mata[M01] * matb[M12] + mata[M02] * matb[M22]; 435 | 436 | float v10 = mata[M10] * matb[M00] + mata[M11] * matb[M10] + mata[M12] * matb[M20]; 437 | float v11 = mata[M10] * matb[M01] + mata[M11] * matb[M11] + mata[M12] * matb[M21]; 438 | float v12 = mata[M10] * matb[M02] + mata[M11] * matb[M12] + mata[M12] * matb[M22]; 439 | 440 | float v20 = mata[M20] * matb[M00] + mata[M21] * matb[M10] + mata[M22] * matb[M20]; 441 | float v21 = mata[M20] * matb[M01] + mata[M21] * matb[M11] + mata[M22] * matb[M21]; 442 | float v22 = mata[M20] * matb[M02] + mata[M21] * matb[M12] + mata[M22] * matb[M22]; 443 | 444 | mata[M00] = v00; 445 | mata[M10] = v10; 446 | mata[M20] = v20; 447 | mata[M01] = v01; 448 | mata[M11] = v11; 449 | mata[M21] = v21; 450 | mata[M02] = v02; 451 | mata[M12] = v12; 452 | mata[M22] = v22; 453 | } 454 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/NumberUtils.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | /******************************************************************************* 4 | * Copyright 2011 See AUTHORS file. 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | ******************************************************************************/ 18 | /** from libgdx: https://github.com/libgdx/libgdx */ 19 | 20 | public class NumberUtils { 21 | public static int floatToIntBits (float value) { 22 | return Float.floatToIntBits(value); 23 | } 24 | 25 | public static int floatToRawIntBits (float value) { 26 | return Float.floatToRawIntBits(value); 27 | } 28 | 29 | public static int floatToIntColor (float value) { 30 | return Float.floatToRawIntBits(value); 31 | } 32 | 33 | /** Encodes the ABGR int color as a float. The high bits are masked to avoid using floats in the NaN range, which unfortunately 34 | * means the full range of alpha cannot be used. See {@link Float#intBitsToFloat(int)} javadocs. */ 35 | public static float intToFloatColor (int value) { 36 | return Float.intBitsToFloat(value & 0xfeffffff); 37 | } 38 | 39 | public static float intBitsToFloat (int value) { 40 | return Float.intBitsToFloat(value); 41 | } 42 | 43 | public static long doubleToLongBits (double value) { 44 | return Double.doubleToLongBits(value); 45 | } 46 | 47 | public static double longBitsToDouble (long value) { 48 | return Double.longBitsToDouble(value); 49 | } 50 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Quaternion.java: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of Libgdx by Mario Zechner (badlogicgames@gmail.com) 3 | * 4 | * Libgdx 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 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * Libgdx is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with libgdx. If not, see
310 | * Sets the Quaternion from the given x-, y- and z-axis which have to be orthonormal. 311 | *
312 | * 313 | *314 | * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ which in turn took it from Graphics Gem code at 315 | * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z. 316 | *
317 | * 318 | * @param xx x-axis x-coordinate 319 | * @param xy x-axis y-coordinate 320 | * @param xz x-axis z-coordinate 321 | * @param yx y-axis x-coordinate 322 | * @param yy y-axis y-coordinate 323 | * @param yz y-axis z-coordinate 324 | * @param zx z-axis x-coordinate 325 | * @param zy z-axis y-coordinate 326 | * @param zz z-axis z-coordinate */ 327 | public Quaternion setFromAxes (float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { 328 | // the trace is the sum of the diagonal elements; see 329 | // http://mathworld.wolfram.com/MatrixTrace.html 330 | final float m00 = xx, m01 = xy, m02 = xz; 331 | final float m10 = yx, m11 = yy, m12 = yz; 332 | final float m20 = zx, m21 = zy, m22 = zz; 333 | final float t = m00 + m11 + m22; 334 | 335 | // we protect the division by s by ensuring that s>=1 336 | double x, y, z, w; 337 | if (t >= 0) { // |w| >= .5 338 | double s = Math.sqrt(t + 1); // |s|>=1 ... 339 | w = 0.5 * s; 340 | s = 0.5 / s; // so this division isn't bad 341 | x = (m21 - m12) * s; 342 | y = (m02 - m20) * s; 343 | z = (m10 - m01) * s; 344 | } else if ((m00 > m11) && (m00 > m22)) { 345 | double s = Math.sqrt(1.0 + m00 - m11 - m22); // |s|>=1 346 | x = s * 0.5; // |x| >= .5 347 | s = 0.5 / s; 348 | y = (m10 + m01) * s; 349 | z = (m02 + m20) * s; 350 | w = (m21 - m12) * s; 351 | } else if (m11 > m22) { 352 | double s = Math.sqrt(1.0 + m11 - m00 - m22); // |s|>=1 353 | y = s * 0.5; // |y| >= .5 354 | s = 0.5 / s; 355 | x = (m10 + m01) * s; 356 | z = (m21 + m12) * s; 357 | w = (m02 - m20) * s; 358 | } else { 359 | double s = Math.sqrt(1.0 + m22 - m00 - m11); // |s|>=1 360 | z = s * 0.5; // |z| >= .5 361 | s = 0.5 / s; 362 | x = (m02 + m20) * s; 363 | y = (m21 + m12) * s; 364 | w = (m10 - m01) * s; 365 | } 366 | 367 | return set((float)x, (float)y, (float)z, (float)w); 368 | } 369 | 370 | /** Set this quaternion to the rotation between two vectors. 371 | * @param v1 The base vector 372 | * @param v2 The target vector 373 | * @return This quaternion for chaining */ 374 | public Quaternion setFromCross (final Vector3 v1, final Vector3 v2) { 375 | final float dot = MathUtils.clamp(Vector3.tmp.set(v1).nor().dot(Vector3.tmp2.set(v2).nor()), -1f, 1f); 376 | return setFromAxis(Vector3.tmp.crs(Vector3.tmp2), (float)Math.acos(dot)); 377 | } 378 | 379 | /** Set this quaternion to the rotation between two vectors. 380 | * @param x1 The base vectors x value 381 | * @param y1 The base vectors y value 382 | * @param z1 The base vectors z value 383 | * @param x2 The target vector x value 384 | * @param y2 The target vector y value 385 | * @param z2 The target vector z value 386 | * @return This quaternion for chaining */ 387 | public Quaternion setFromCross (final float x1, final float y1, final float z1, final float x2, final float y2, final float z2) { 388 | final float dot = MathUtils.clamp(Vector3.tmp.set(x1, y1, z1).nor().dot(Vector3.tmp2.set(x2, y2, z2).nor()), -1f, 1f); 389 | return setFromAxis(Vector3.tmp.crs(Vector3.tmp2), (float)Math.acos(dot)); 390 | } 391 | 392 | /** Spherical linear interpolation between this quaternion and the other quaternion, based on the alpha value in the range 393 | * [0,1]. Taken from. Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ 394 | * @param end the end quaternion 395 | * @param alpha alpha in the range [0,1] 396 | * @return this quaternion for chaining */ 397 | public Quaternion slerp (Quaternion end, float alpha) { 398 | if (this.equals(end)) { 399 | return this; 400 | } 401 | 402 | float result = dot(end); 403 | 404 | if (result < 0.0) { 405 | // Negate the second quaternion and the result of the dot product 406 | end.mul(-1); 407 | result = -result; 408 | } 409 | 410 | // Set the first and second scale for the interpolation 411 | float scale0 = 1 - alpha; 412 | float scale1 = alpha; 413 | 414 | // Check if the angle between the 2 quaternions was big enough to 415 | // warrant such calculations 416 | if ((1 - result) > 0.1) {// Get the angle between the 2 quaternions, 417 | // and then store the sin() of that angle 418 | final double theta = Math.acos(result); 419 | final double invSinTheta = 1f / Math.sin(theta); 420 | 421 | // Calculate the scale for q1 and q2, according to the angle and 422 | // it's sine value 423 | scale0 = (float)(Math.sin((1 - alpha) * theta) * invSinTheta); 424 | scale1 = (float)(Math.sin((alpha * theta)) * invSinTheta); 425 | } 426 | 427 | // Calculate the x, y, z and w values for the quaternion by using a 428 | // special form of linear interpolation for quaternions. 429 | final float x = (scale0 * this.x) + (scale1 * end.x); 430 | final float y = (scale0 * this.y) + (scale1 * end.y); 431 | final float z = (scale0 * this.z) + (scale1 * end.z); 432 | final float w = (scale0 * this.w) + (scale1 * end.w); 433 | set(x, y, z, w); 434 | 435 | // Return the interpolated quaternion 436 | return this; 437 | } 438 | 439 | public boolean equals (final Object o) { 440 | if (this == o) { 441 | return true; 442 | } 443 | if (!(o instanceof Quaternion)) { 444 | return false; 445 | } 446 | final Quaternion comp = (Quaternion)o; 447 | return this.x == comp.x && this.y == comp.y && this.z == comp.z && this.w == comp.w; 448 | 449 | } 450 | 451 | /** Dot product between this and the other quaternion. 452 | * @param other the other quaternion. 453 | * @return this quaternion for chaining. */ 454 | public float dot (Quaternion other) { 455 | return x * other.x + y * other.y + z * other.z + w * other.w; 456 | } 457 | 458 | /** Multiplies the components of this quaternion with the given scalar. 459 | * @param scalar the scalar. 460 | * @return this quaternion for chaining. */ 461 | public Quaternion mul (float scalar) { 462 | this.x *= scalar; 463 | this.y *= scalar; 464 | this.z *= scalar; 465 | this.w *= scalar; 466 | return this; 467 | } 468 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Util.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | import android.util.FloatMath; 4 | 5 | public class Util { 6 | /** 7 | * Converts a double to a value between 0 and 360 8 | * 9 | * @param x 10 | * @return double in 0-360 range 11 | */ 12 | public static float floatrev(double x) { 13 | return (float) (x - Math.floor(x / 360.0f) * 360.0f); 14 | } 15 | 16 | /** 17 | * Derive distance between 3d vector a,b 18 | * 19 | * @param a 20 | * @param b 21 | * @return 22 | */ 23 | public static float calcDistance(Vector3 a, Vector3 b) { 24 | return FloatMath.sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)); 25 | } 26 | 27 | /** 28 | * calculates the encompassing circle radius for sides a,b,c of a triangle 29 | * 30 | * @param a 31 | * @param b 32 | * @param c 33 | * @return 34 | */ 35 | public static float calcRadius(float a, float b, float c) { 36 | return (a * b * c) / FloatMath.sqrt((a + b + c) * (a - b + c) * (a + b - c) * (b + c - a)); 37 | } 38 | 39 | /** 40 | * Calculates the angle A given length a and circle radius r, according to 41 | * the law of sines ([a/sin(A) = 2R], thus [A = arcsin(a/2r)]) 42 | * 43 | * @param a 44 | * @param r 45 | * @return angle A in radians 46 | */ 47 | public static float calcAngle(float a, float r) { 48 | return (float) Math.asin(a / (2 * r)); 49 | } 50 | 51 | /** 52 | * Calculates the angle A given length a and circle radius r, according to 53 | * the law of sines ([a/sin(A) = 2R], thus [A = arcsin(a/2r)]) 54 | * 55 | * @param a 56 | * @param r 57 | * @return angle A in radians 58 | */ 59 | public static float calcAngleClamp(float a, float r) { 60 | return (float) Math.asin(Math.min(1, Math.max(-1, a / (2 * r)))); 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Vector2.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | /******************************************************************************* 4 | * Copyright 2011 See AUTHORS file. 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | ******************************************************************************/ 18 | 19 | import java.io.Serializable; 20 | 21 | /** Encapsulates a 2D vector. Allows chaining methods by returning a reference to itself 22 | * @author badlogicgames@gmail.com */ 23 | public class Vector2 implements Serializable { 24 | private static final long serialVersionUID = 913902788239530931L; 25 | 26 | /** Static temporary vector. Use with care! Use only when sure other code will not also use this. 27 | * @see #tmp() **/ 28 | public final static Vector2 tmp = new Vector2(), tmp2 = new Vector2(), tmp3 = new Vector2(); 29 | 30 | public final static Vector2 X = new Vector2(1, 0); 31 | public final static Vector2 Y = new Vector2(0, 1); 32 | public final static Vector2 Zero = new Vector2(0, 0); 33 | 34 | /** the x-component of this vector **/ 35 | public float x; 36 | /** the y-component of this vector **/ 37 | public float y; 38 | 39 | /** Constructs a new vector at (0,0) */ 40 | public Vector2 () { 41 | } 42 | 43 | /** Constructs a vector with the given components 44 | * @param x The x-component 45 | * @param y The y-component */ 46 | public Vector2 (float x, float y) { 47 | this.x = x; 48 | this.y = y; 49 | } 50 | 51 | /** Constructs a vector from the given vector 52 | * @param v The vector */ 53 | public Vector2 (Vector2 v) { 54 | set(v); 55 | } 56 | 57 | /** @return a copy of this vector */ 58 | public Vector2 cpy () { 59 | return new Vector2(this); 60 | } 61 | 62 | /** @return The euclidian length */ 63 | public float len () { 64 | return (float)Math.sqrt(x * x + y * y); 65 | } 66 | 67 | /** @return The squared euclidian length */ 68 | public float len2 () { 69 | return x * x + y * y; 70 | } 71 | 72 | /** Sets this vector from the given vector 73 | * @param v The vector 74 | * @return This vector for chaining */ 75 | public Vector2 set (Vector2 v) { 76 | x = v.x; 77 | y = v.y; 78 | return this; 79 | } 80 | 81 | /** Sets the components of this vector 82 | * @param x The x-component 83 | * @param y The y-component 84 | * @return This vector for chaining */ 85 | public Vector2 set (float x, float y) { 86 | this.x = x; 87 | this.y = y; 88 | return this; 89 | } 90 | 91 | /** Substracts the given vector from this vector. 92 | * @param v The vector 93 | * @return This vector for chaining */ 94 | public Vector2 sub (Vector2 v) { 95 | x -= v.x; 96 | y -= v.y; 97 | return this; 98 | } 99 | 100 | /** Normalizes this vector 101 | * @return This vector for chaining */ 102 | public Vector2 nor () { 103 | float len = len(); 104 | if (len != 0) { 105 | x /= len; 106 | y /= len; 107 | } 108 | return this; 109 | } 110 | 111 | /** Adds the given vector to this vector 112 | * @param v The vector 113 | * @return This vector for chaining */ 114 | public Vector2 add (Vector2 v) { 115 | x += v.x; 116 | y += v.y; 117 | return this; 118 | } 119 | 120 | /** Adds the given components to this vector 121 | * @param x The x-component 122 | * @param y The y-component 123 | * @return This vector for chaining */ 124 | public Vector2 add (float x, float y) { 125 | this.x += x; 126 | this.y += y; 127 | return this; 128 | } 129 | 130 | /** @param v The other vector 131 | * @return The dot product between this and the other vector */ 132 | public float dot (Vector2 v) { 133 | return x * v.x + y * v.y; 134 | } 135 | 136 | /** Multiplies this vector by a scalar 137 | * @param scalar The scalar 138 | * @return This vector for chaining */ 139 | public Vector2 mul (float scalar) { 140 | x *= scalar; 141 | y *= scalar; 142 | return this; 143 | } 144 | 145 | /** Multiplies this vector by a scalar 146 | * @return This vector for chaining */ 147 | public Vector2 mul (float x, float y) { 148 | this.x *= x; 149 | this.y *= y; 150 | return this; 151 | } 152 | 153 | public Vector2 div (float value) { 154 | return this.mul(1/value); 155 | } 156 | 157 | public Vector2 div (float vx, float vy) { 158 | return this.mul(1/vx, 1/vy); 159 | } 160 | 161 | public Vector2 div (Vector2 other) { 162 | return this.mul(1/other.x, 1/other.y); 163 | } 164 | 165 | /** @param v The other vector 166 | * @return the distance between this and the other vector */ 167 | public float dst (Vector2 v) { 168 | final float x_d = v.x - x; 169 | final float y_d = v.y - y; 170 | return (float)Math.sqrt(x_d * x_d + y_d * y_d); 171 | } 172 | 173 | /** @param x The x-component of the other vector 174 | * @param y The y-component of the other vector 175 | * @return the distance between this and the other vector */ 176 | public float dst (float x, float y) { 177 | final float x_d = x - this.x; 178 | final float y_d = y - this.y; 179 | return (float)Math.sqrt(x_d * x_d + y_d * y_d); 180 | } 181 | 182 | /** @param v The other vector 183 | * @return the squared distance between this and the other vector */ 184 | public float dst2 (Vector2 v) { 185 | final float x_d = v.x - x; 186 | final float y_d = v.y - y; 187 | return x_d * x_d + y_d * y_d; 188 | } 189 | 190 | /** @param x The x-component of the other vector 191 | * @param y The y-component of the other vector 192 | * @return the squared distance between this and the other vector */ 193 | public float dst2 (float x, float y) { 194 | final float x_d = x - this.x; 195 | final float y_d = y - this.y; 196 | return x_d * x_d + y_d * y_d; 197 | } 198 | 199 | public String toString () { 200 | return "[" + x + ":" + y + "]"; 201 | } 202 | 203 | /** Substracts the other vector from this vector. 204 | * @param x The x-component of the other vector 205 | * @param y The y-component of the other vector 206 | * @return This vector for chaining */ 207 | public Vector2 sub (float x, float y) { 208 | this.x -= x; 209 | this.y -= y; 210 | return this; 211 | } 212 | 213 | /** NEVER EVER SAVE THIS REFERENCE! Do not use this unless you are aware of the side-effects, e.g. other methods might call this 214 | * as well. 215 | * 216 | * @return a temporary copy of this vector. Use with care as this is backed by a single static Vector2 instance. v1.tmp().add( 217 | * v2.tmp() ) will not work! */ 218 | public Vector2 tmp () { 219 | return tmp.set(this); 220 | } 221 | 222 | /** Multiplies this vector by the given matrix 223 | * @param mat the matrix 224 | * @return this vector */ 225 | public Vector2 mul (Matrix3 mat) { 226 | float x = this.x * mat.val[0] + this.y * mat.val[3] + mat.val[6]; 227 | float y = this.x * mat.val[1] + this.y * mat.val[4] + mat.val[7]; 228 | this.x = x; 229 | this.y = y; 230 | return this; 231 | } 232 | 233 | /** Calculates the 2D cross product between this and the given vector. 234 | * @param v the other vector 235 | * @return the cross product */ 236 | public float crs (Vector2 v) { 237 | return this.x * v.y - this.y * v.x; 238 | } 239 | 240 | /** Calculates the 2D cross product between this and the given vector. 241 | * @param x the x-coordinate of the other vector 242 | * @param y the y-coordinate of the other vector 243 | * @return the cross product */ 244 | public float crs (float x, float y) { 245 | return this.x * y - this.y * x; 246 | } 247 | 248 | /** @return the angle in degrees of this vector (point) relative to the x-axis. Angles are counter-clockwise and between 0 and 249 | * 360. */ 250 | public float angle () { 251 | float angle = (float)Math.atan2(y, x) * MathUtils.radiansToDegrees; 252 | if (angle < 0) angle += 360; 253 | return angle; 254 | } 255 | 256 | /** Sets the angle of the vector. 257 | * @param angle The angle to set. */ 258 | public void setAngle (float angle) { 259 | this.set(len(), 0f); 260 | this.rotate(angle); 261 | } 262 | 263 | /** Rotates the Vector2 by the given angle, counter-clockwise. 264 | * @param degrees the angle in degrees */ 265 | public Vector2 rotate (float degrees) { 266 | float rad = degrees * MathUtils.degreesToRadians; 267 | float cos = (float)Math.cos(rad); 268 | float sin = (float)Math.sin(rad); 269 | 270 | float newX = this.x * cos - this.y * sin; 271 | float newY = this.x * sin + this.y * cos; 272 | 273 | this.x = newX; 274 | this.y = newY; 275 | 276 | return this; 277 | } 278 | 279 | /** Linearly interpolates between this vector and the target vector by alpha which is in the range [0,1]. The result is stored 280 | * in this vector. 281 | * 282 | * @param target The target vector 283 | * @param alpha The interpolation coefficient 284 | * @return This vector for chaining. */ 285 | public Vector2 lerp (Vector2 target, float alpha) { 286 | Vector2 r = this.mul(1.0f - alpha); 287 | r.add(target.tmp().mul(alpha)); 288 | return r; 289 | } 290 | 291 | @Override 292 | public int hashCode () { 293 | final int prime = 31; 294 | int result = 1; 295 | result = prime * result + NumberUtils.floatToIntBits(x); 296 | result = prime * result + NumberUtils.floatToIntBits(y); 297 | return result; 298 | } 299 | 300 | @Override 301 | public boolean equals (Object obj) { 302 | if (this == obj) return true; 303 | if (obj == null) return false; 304 | if (getClass() != obj.getClass()) return false; 305 | Vector2 other = (Vector2)obj; 306 | if (NumberUtils.floatToIntBits(x) != NumberUtils.floatToIntBits(other.x)) return false; 307 | if (NumberUtils.floatToIntBits(y) != NumberUtils.floatToIntBits(other.y)) return false; 308 | return true; 309 | } 310 | 311 | /** Compares this vector with the other vector, using the supplied epsilon for fuzzy equality testing. 312 | * @param obj 313 | * @param epsilon 314 | * @return whether the vectors are the same. */ 315 | public boolean epsilonEquals (Vector2 obj, float epsilon) { 316 | if (obj == null) return false; 317 | if (Math.abs(obj.x - x) > epsilon) return false; 318 | if (Math.abs(obj.y - y) > epsilon) return false; 319 | return true; 320 | } 321 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Vector3.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | import java.io.Serializable; 4 | 5 | /******************************************************************************* 6 | * Copyright 2011 See AUTHORS file. 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | ******************************************************************************/ 20 | 21 | /** Encapsulates a 3D vector. Allows chaining operations by returning a reference to itself in all modification methods. 22 | * @author badlogicgames@gmail.com */ 23 | public class Vector3 implements Serializable { 24 | private static final long serialVersionUID = 3840054589595372522L; 25 | 26 | /** the x-component of this vector **/ 27 | public float x; 28 | /** the x-component of this vector **/ 29 | public float y; 30 | /** the x-component of this vector **/ 31 | public float z; 32 | 33 | /** Static temporary vector. Use with care! Use only when sure other code will not also use this. 34 | * @see #tmp() **/ 35 | public final static Vector3 tmp = new Vector3(); 36 | /** Static temporary vector. Use with care! Use only when sure other code will not also use this. 37 | * @see #tmp() **/ 38 | public final static Vector3 tmp2 = new Vector3(); 39 | /** Static temporary vector. Use with care! Use only when sure other code will not also use this. 40 | * @see #tmp() **/ 41 | public final static Vector3 tmp3 = new Vector3(); 42 | 43 | public final static Vector3 X = new Vector3(1, 0, 0); 44 | public final static Vector3 Y = new Vector3(0, 1, 0); 45 | public final static Vector3 Z = new Vector3(0, 0, 1); 46 | public final static Vector3 Zero = new Vector3(0, 0, 0); 47 | 48 | private final static Matrix4 tmpMat = new Matrix4(); 49 | 50 | /** Constructs a vector at (0,0,0) */ 51 | public Vector3 () { 52 | } 53 | 54 | /** Creates a vector with the given components 55 | * @param x The x-component 56 | * @param y The y-component 57 | * @param z The z-component */ 58 | public Vector3 (float x, float y, float z) { 59 | this.set(x, y, z); 60 | } 61 | 62 | /** Creates a vector from the given vector 63 | * @param vector The vector */ 64 | public Vector3 (Vector3 vector) { 65 | this.set(vector); 66 | } 67 | 68 | /** Creates a vector from the given array. The array must have at least 3 elements. 69 | * 70 | * @param values The array */ 71 | public Vector3 (float[] values) { 72 | this.set(values[0], values[1], values[2]); 73 | } 74 | 75 | /** Sets the vector to the given components 76 | * 77 | * @param x The x-component 78 | * @param y The y-component 79 | * @param z The z-component 80 | * @return this vector for chaining */ 81 | public Vector3 set (float x, float y, float z) { 82 | this.x = x; 83 | this.y = y; 84 | this.z = z; 85 | return this; 86 | } 87 | 88 | /** Sets the components of the given vector 89 | * 90 | * @param vector The vector 91 | * @return This vector for chaining */ 92 | public Vector3 set (Vector3 vector) { 93 | return this.set(vector.x, vector.y, vector.z); 94 | } 95 | 96 | /** Sets the components from the array. The array must have at least 3 elements 97 | * 98 | * @param values The array 99 | * @return this vector for chaining */ 100 | public Vector3 set (float[] values) { 101 | return this.set(values[0], values[1], values[2]); 102 | } 103 | 104 | /** @return a copy of this vector */ 105 | public Vector3 cpy () { 106 | return new Vector3(this); 107 | } 108 | 109 | /** NEVER EVER SAVE THIS REFERENCE! Do not use this unless you are aware of the side-effects, e.g. other methods might call this 110 | * as well. 111 | * 112 | * @return a temporary copy of this vector */ 113 | public Vector3 tmp () { 114 | return tmp.set(this); 115 | } 116 | 117 | /** NEVER EVER SAVE THIS REFERENCE! Do not use this unless you are aware of the side-effects, e.g. other methods might call this 118 | * as well. 119 | * 120 | * @return a temporary copy of this vector */ 121 | public Vector3 tmp2 () { 122 | return tmp2.set(this); 123 | } 124 | 125 | /** NEVER EVER SAVE THIS REFERENCE! Do not use this unless you are aware of the side-effects, e.g. other methods might call this 126 | * as well. 127 | * 128 | * @return a temporary copy of this vector */ 129 | Vector3 tmp3 () { 130 | return tmp3.set(this); 131 | } 132 | 133 | /** Adds the given vector to this vector 134 | * 135 | * @param vector The other vector 136 | * @return This vector for chaining */ 137 | public Vector3 add (Vector3 vector) { 138 | return this.add(vector.x, vector.y, vector.z); 139 | } 140 | 141 | /** Adds the given vector to this component 142 | * @param x The x-component of the other vector 143 | * @param y The y-component of the other vector 144 | * @param z The z-component of the other vector 145 | * @return This vector for chaining. */ 146 | public Vector3 add (float x, float y, float z) { 147 | return this.set(this.x + x, this.y + y, this.z + z); 148 | } 149 | 150 | /** Adds the given value to all three components of the vector. 151 | * 152 | * @param values The value 153 | * @return This vector for chaining */ 154 | public Vector3 add (float values) { 155 | return this.set(this.x + values, this.y + values, this.z + values); 156 | } 157 | 158 | /** Subtracts the given vector from this vector 159 | * @param a_vec The other vector 160 | * @return This vector for chaining */ 161 | public Vector3 sub (Vector3 a_vec) { 162 | return this.sub(a_vec.x, a_vec.y, a_vec.z); 163 | } 164 | 165 | /** Subtracts the other vector from this vector. 166 | * 167 | * @param x The x-component of the other vector 168 | * @param y The y-component of the other vector 169 | * @param z The z-component of the other vector 170 | * @return This vector for chaining */ 171 | public Vector3 sub (float x, float y, float z) { 172 | return this.set(this.x - x, this.y - y, this.z - z); 173 | } 174 | 175 | /** Subtracts the given value from all components of this vector 176 | * 177 | * @param value The value 178 | * @return This vector for chaining */ 179 | public Vector3 sub (float value) { 180 | return this.set(this.x - value, this.y - value, this.z - value); 181 | } 182 | 183 | /** Multiplies all components of this vector by the given value 184 | * 185 | * @param value The value 186 | * @return This vector for chaining */ 187 | public Vector3 mul (float value) { 188 | return this.set(this.x * value, this.y * value, this.z * value); 189 | } 190 | 191 | /** Multiplies all components of this vector by the given vector3's values 192 | * 193 | * @param other The vector3 to multiply by 194 | * @return This vector for chaining */ 195 | public Vector3 mul (Vector3 other) { 196 | return this.mul(other.x, other.y, other.z); 197 | } 198 | 199 | /** Multiplies all components of this vector by the given values 200 | * 201 | * @param vx X value 202 | * @param vy Y value 203 | * @param vz Z value 204 | * @return This vector for chaining */ 205 | public Vector3 mul (float vx, float vy, float vz) { 206 | return this.set(this.x * vx, this.y * vy, this.z * vz); 207 | } 208 | 209 | /** Divides all components of this vector by the given value 210 | * 211 | * @param value The value 212 | * @return This vector for chaining */ 213 | public Vector3 div (float value) { 214 | return this.mul(1/value); 215 | } 216 | 217 | public Vector3 div (float vx, float vy, float vz) { 218 | return this.mul(1/vx, 1/vy, 1/vz); 219 | } 220 | 221 | public Vector3 div (Vector3 other) { 222 | return this.mul(1/other.x, 1/other.y, 1/other.z); 223 | } 224 | 225 | /** @return The euclidian length */ 226 | public float len () { 227 | return (float)Math.sqrt(x * x + y * y + z * z); 228 | } 229 | 230 | /** @return The squared euclidian length */ 231 | public float len2 () { 232 | return x * x + y * y + z * z; 233 | } 234 | 235 | /** @param vector The other vector 236 | * @return Wether this and the other vector are equal */ 237 | public boolean idt (Vector3 vector) { 238 | return x == vector.x && y == vector.y && z == vector.z; 239 | } 240 | 241 | /** @param vector The other vector 242 | * @return The euclidian distance between this and the other vector */ 243 | public float dst (Vector3 vector) { 244 | float a = vector.x - x; 245 | float b = vector.y - y; 246 | float c = vector.z - z; 247 | 248 | a *= a; 249 | b *= b; 250 | c *= c; 251 | 252 | return (float)Math.sqrt(a + b + c); 253 | } 254 | 255 | /** Normalizes this vector to unit length 256 | * 257 | * @return This vector for chaining */ 258 | public Vector3 nor () { 259 | float len = this.len(); 260 | if (len == 0) { 261 | return this; 262 | } else { 263 | return this.div(len); 264 | } 265 | } 266 | 267 | /** @param vector The other vector 268 | * @return The dot product between this and the other vector */ 269 | public float dot (Vector3 vector) { 270 | return x * vector.x + y * vector.y + z * vector.z; 271 | } 272 | 273 | /** Sets this vector to the cross product between it and the other vector. 274 | * @param vector The other vector 275 | * @return This vector for chaining */ 276 | public Vector3 crs (Vector3 vector) { 277 | return this.set(y * vector.z - z * vector.y, z * vector.x - x * vector.z, x * vector.y - y * vector.x); 278 | } 279 | 280 | /** Sets this vector to the cross product between it and the other vector. 281 | * @param x The x-component of the other vector 282 | * @param y The y-component of the other vector 283 | * @param z The z-component of the other vector 284 | * @return This vector for chaining */ 285 | public Vector3 crs (float x, float y, float z) { 286 | return this.set(this.y * z - this.z * y, this.z * x - this.x * z, this.x * y - this.y * x); 287 | } 288 | 289 | /** Multiplies the vector by the given matrix. 290 | * @param matrix The matrix 291 | * @return This vector for chaining */ 292 | public Vector3 mul (Matrix4 matrix) { 293 | float l_mat[] = matrix.val; 294 | return this.set(x * l_mat[Matrix4.M00] + y * l_mat[Matrix4.M01] + z * l_mat[Matrix4.M02] + l_mat[Matrix4.M03], x 295 | * l_mat[Matrix4.M10] + y * l_mat[Matrix4.M11] + z * l_mat[Matrix4.M12] + l_mat[Matrix4.M13], x * l_mat[Matrix4.M20] + y 296 | * l_mat[Matrix4.M21] + z * l_mat[Matrix4.M22] + l_mat[Matrix4.M23]); 297 | } 298 | 299 | /** Multiplies this vector by the given matrix dividing by w. This is mostly used to project/unproject vectors via a perspective 300 | * projection matrix. 301 | * 302 | * @param matrix The matrix. 303 | * @return This vector for chaining */ 304 | public Vector3 prj (Matrix4 matrix) { 305 | float l_mat[] = matrix.val; 306 | float l_w = x * l_mat[Matrix4.M30] + y * l_mat[Matrix4.M31] + z * l_mat[Matrix4.M32] + l_mat[Matrix4.M33]; 307 | return this.set((x * l_mat[Matrix4.M00] + y * l_mat[Matrix4.M01] + z * l_mat[Matrix4.M02] + l_mat[Matrix4.M03]) / l_w, (x 308 | * l_mat[Matrix4.M10] + y * l_mat[Matrix4.M11] + z * l_mat[Matrix4.M12] + l_mat[Matrix4.M13]) 309 | / l_w, (x * l_mat[Matrix4.M20] + y * l_mat[Matrix4.M21] + z * l_mat[Matrix4.M22] + l_mat[Matrix4.M23]) / l_w); 310 | } 311 | 312 | /** Multiplies this vector by the first three columns of the matrix, essentially only applying rotation and scaling. 313 | * 314 | * @param matrix The matrix 315 | * @return This vector for chaining */ 316 | public Vector3 rot (Matrix4 matrix) { 317 | float l_mat[] = matrix.val; 318 | return this.set(x * l_mat[Matrix4.M00] + y * l_mat[Matrix4.M01] + z * l_mat[Matrix4.M02], x * l_mat[Matrix4.M10] + y 319 | * l_mat[Matrix4.M11] + z * l_mat[Matrix4.M12], x * l_mat[Matrix4.M20] + y * l_mat[Matrix4.M21] + z * l_mat[Matrix4.M22]); 320 | } 321 | 322 | /** Rotates this vector by the given angle around the given axis. 323 | * 324 | * @param axisX the x-component of the axis 325 | * @param axisY the y-component of the axis 326 | * @param axisZ the z-component of the axis 327 | * @return This vector for chaining */ 328 | public Vector3 rotate (float angle, float axisX, float axisY, float axisZ) { 329 | return rotate(tmp.set(axisX, axisY, axisZ), angle); 330 | } 331 | 332 | /** Rotates this vector by the given angle around the given axis. 333 | * 334 | * @param axis 335 | * @param angle the angle 336 | * @return This vector for chaining */ 337 | public Vector3 rotate (Vector3 axis, float angle) { 338 | tmpMat.setToRotation(axis, angle); 339 | return this.mul(tmpMat); 340 | } 341 | 342 | /** @return Whether this vector is a unit length vector */ 343 | public boolean isUnit () { 344 | return this.len() == 1; 345 | } 346 | 347 | /** @return Whether this vector is a zero vector */ 348 | public boolean isZero () { 349 | return x == 0 && y == 0 && z == 0; 350 | } 351 | 352 | /** Linearly interpolates between this vector and the target vector by alpha which is in the range [0,1]. The result is stored 353 | * in this vector. 354 | * 355 | * @param target The target vector 356 | * @param alpha The interpolation coefficient 357 | * @return This vector for chaining. */ 358 | public Vector3 lerp (Vector3 target, float alpha) { 359 | Vector3 r = this.mul(1.0f - alpha); 360 | r.add(target.tmp().mul(alpha)); 361 | return r; 362 | } 363 | 364 | /** Spherically interpolates between this vector and the target vector by alpha which is in the range [0,1]. The result is 365 | * stored in this vector. 366 | * 367 | * @param target The target vector 368 | * @param alpha The interpolation coefficient 369 | * @return This vector for chaining. */ 370 | public Vector3 slerp (Vector3 target, float alpha) { 371 | float dot = dot(target); 372 | if (dot > 0.99995 || dot < 0.9995) { 373 | this.add(target.tmp().sub(this).mul(alpha)); 374 | this.nor(); 375 | return this; 376 | } 377 | 378 | if (dot > 1) dot = 1; 379 | if (dot < -1) dot = -1; 380 | 381 | float theta0 = (float)Math.acos(dot); 382 | float theta = theta0 * alpha; 383 | Vector3 v2 = target.tmp().sub(x * dot, y * dot, z * dot); 384 | v2.nor(); 385 | return this.mul((float)Math.cos(theta)).add(v2.mul((float)Math.sin(theta))).nor(); 386 | } 387 | 388 | /** {@inheritDoc} */ 389 | public String toString () { 390 | return x + "," + y + "," + z; 391 | } 392 | 393 | /** Returns the dot product between this and the given vector. 394 | * 395 | * @param x The x-component of the other vector 396 | * @param y The y-component of the other vector 397 | * @param z The z-component of the other vector 398 | * @return The dot product */ 399 | public float dot (float x, float y, float z) { 400 | return this.x * x + this.y * y + this.z * z; 401 | } 402 | 403 | /** Returns the squared distance between this point and the given point 404 | * 405 | * @param point The other point 406 | * @return The squared distance */ 407 | public float dst2 (Vector3 point) { 408 | 409 | float a = point.x - x; 410 | float b = point.y - y; 411 | float c = point.z - z; 412 | 413 | a *= a; 414 | b *= b; 415 | c *= c; 416 | 417 | return a + b + c; 418 | } 419 | 420 | /** Returns the squared distance between this point and the given point 421 | * 422 | * @param x The x-component of the other point 423 | * @param y The y-component of the other point 424 | * @param z The z-component of the other point 425 | * @return The squared distance */ 426 | public float dst2 (float x, float y, float z) { 427 | float a = x - this.x; 428 | float b = y - this.y; 429 | float c = z - this.z; 430 | 431 | a *= a; 432 | b *= b; 433 | c *= c; 434 | 435 | return a + b + c; 436 | } 437 | 438 | public float dst (float x, float y, float z) { 439 | return (float)Math.sqrt(dst2(x, y, z)); 440 | } 441 | 442 | /** {@inheritDoc} */ 443 | @Override 444 | public int hashCode () { 445 | final int prime = 31; 446 | int result = 1; 447 | result = prime * result + NumberUtils.floatToIntBits(x); 448 | result = prime * result + NumberUtils.floatToIntBits(y); 449 | result = prime * result + NumberUtils.floatToIntBits(z); 450 | return result; 451 | } 452 | 453 | /** {@inheritDoc} */ 454 | @Override 455 | public boolean equals (Object obj) { 456 | if (this == obj) return true; 457 | if (obj == null) return false; 458 | if (getClass() != obj.getClass()) return false; 459 | Vector3 other = (Vector3)obj; 460 | if (NumberUtils.floatToIntBits(x) != NumberUtils.floatToIntBits(other.x)) return false; 461 | if (NumberUtils.floatToIntBits(y) != NumberUtils.floatToIntBits(other.y)) return false; 462 | if (NumberUtils.floatToIntBits(z) != NumberUtils.floatToIntBits(other.z)) return false; 463 | return true; 464 | } 465 | 466 | /** Compares this vector with the other vector, using the supplied epsilon for fuzzy equality testing. 467 | * @param obj 468 | * @param epsilon 469 | * @return whether the vectors are the same. */ 470 | public boolean epsilonEquals (Vector3 obj, float epsilon) { 471 | if (obj == null) return false; 472 | if (Math.abs(obj.x - x) > epsilon) return false; 473 | if (Math.abs(obj.y - y) > epsilon) return false; 474 | if (Math.abs(obj.z - z) > epsilon) return false; 475 | return true; 476 | } 477 | 478 | /** Compares this vector with the other vector, using the supplied epsilon for fuzzy equality testing. 479 | * @return whether the vectors are the same. */ 480 | public boolean epsilonEquals (float x, float y, float z, float epsilon) { 481 | if (Math.abs(x - this.x) > epsilon) return false; 482 | if (Math.abs(y - this.y) > epsilon) return false; 483 | if (Math.abs(z - this.z) > epsilon) return false; 484 | return true; 485 | } 486 | 487 | /** Scales the vector components by the given scalars. 488 | * 489 | * @param scalarX 490 | * @param scalarY 491 | * @param scalarZ */ 492 | public Vector3 scale (float scalarX, float scalarY, float scalarZ) { 493 | x *= scalarX; 494 | y *= scalarY; 495 | z *= scalarZ; 496 | return this; 497 | } 498 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/math/Vector4.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.math; 2 | 3 | /** 4 | * Derived from Vector3 from libgdx - encapsulates a 4 component vector for 5 | * representing Homogeneous Coordinates 6 | */ 7 | public final class Vector4 { 8 | /** the x-component of this vector **/ 9 | public float x; 10 | /** the x-component of this vector **/ 11 | public float y; 12 | /** the x-component of this vector **/ 13 | public float z; 14 | 15 | public float w; 16 | 17 | private static Vector4 tmp = new Vector4(); 18 | private static Vector4 tmp2 = new Vector4(); 19 | private static Vector4 tmp3 = new Vector4(); 20 | 21 | /** 22 | * Multiplies the vector by the given matrix. 23 | * 24 | * @param matrix 25 | * The matrix 26 | * @return This vector for chaining 27 | */ 28 | public Vector4 mul(Matrix4 matrix) { 29 | float l_mat[] = matrix.val; 30 | return this.set(x * l_mat[Matrix4.M00] + y * l_mat[Matrix4.M01] + z * l_mat[Matrix4.M02] + w * l_mat[Matrix4.M03], x * l_mat[Matrix4.M10] + y * l_mat[Matrix4.M11] + z * l_mat[Matrix4.M12] + w * l_mat[Matrix4.M13], x * l_mat[Matrix4.M20] + y 31 | * l_mat[Matrix4.M21] + z * l_mat[Matrix4.M22] + w * l_mat[Matrix4.M23], x * l_mat[Matrix4.M30] + y * l_mat[Matrix4.M31] + z * l_mat[Matrix4.M32] + w * l_mat[Matrix4.M33]); 32 | } 33 | 34 | /** 35 | * Constructs a vector at (0,0,0) 36 | */ 37 | public Vector4() { 38 | } 39 | 40 | /** 41 | * Creates a vector with the given components 42 | * 43 | * @param x 44 | * The x-component 45 | * @param y 46 | * The y-component 47 | * @param z 48 | * The z-component 49 | */ 50 | public Vector4(float x, float y, float z, float w) { 51 | this.set(x, y, z, w); 52 | } 53 | 54 | /** 55 | * Creates a vector from the given vector 56 | * 57 | * @param vector 58 | * The vector 59 | */ 60 | public Vector4(Vector4 vector) { 61 | this.set(vector); 62 | } 63 | 64 | /** 65 | * Creates a vector from the given array. The array must have at least 3 66 | * elements. 67 | * 68 | * @param values 69 | * The array 70 | */ 71 | public Vector4(float[] values) { 72 | this.set(values[0], values[1], values[2], values[3]); 73 | } 74 | 75 | /** 76 | * Sets the vector to the given components 77 | * 78 | * @param x 79 | * The x-component 80 | * @param y 81 | * The y-component 82 | * @param z 83 | * The z-component 84 | * @return this vector for chaining 85 | */ 86 | public Vector4 set(float x, float y, float z, float w) { 87 | this.x = x; 88 | this.y = y; 89 | this.z = z; 90 | this.w = w; 91 | return this; 92 | } 93 | 94 | /** 95 | * Sets the components of the given vector 96 | * 97 | * @param vector 98 | * The vector 99 | * @return This vector for chaining 100 | */ 101 | public Vector4 set(Vector4 vector) { 102 | return this.set(vector.x, vector.y, vector.z, vector.w); 103 | } 104 | 105 | /** 106 | * Sets the components from the array. The array must have at least 3 107 | * elements 108 | * 109 | * @param values 110 | * The array 111 | * @return this vector for chaining 112 | */ 113 | public Vector4 set(float[] values) { 114 | return this.set(values[0], values[1], values[2], values[3]); 115 | } 116 | 117 | /** 118 | * @return a copy of this vector 119 | */ 120 | public Vector4 cpy() { 121 | return new Vector4(this); 122 | } 123 | 124 | /** 125 | * NEVER EVER SAVE THIS REFERENCE! 126 | * 127 | * @return 128 | */ 129 | public Vector4 tmp() { 130 | return tmp.set(this); 131 | } 132 | 133 | /** 134 | * NEVER EVER SAVE THIS REFERENCE! 135 | * 136 | * @return 137 | */ 138 | public Vector4 tmp2() { 139 | return tmp2.set(this); 140 | } 141 | 142 | /** 143 | * NEVER EVER SAVE THIS REFERENCE! 144 | * 145 | * @return 146 | */ 147 | Vector4 tmp3() { 148 | return tmp3.set(this); 149 | } 150 | 151 | /** 152 | * Adds the given vector to this vector 153 | * 154 | * @param vector 155 | * The other vector 156 | * @return This vector for chaining 157 | */ 158 | public Vector4 add(Vector4 vector) { 159 | return this.add(vector.x, vector.y, vector.z, vector.w); 160 | } 161 | 162 | /** 163 | * Adds the given vector to this component 164 | * 165 | * @param x 166 | * The x-component of the other vector 167 | * @param y 168 | * The y-component of the other vector 169 | * @param z 170 | * The z-component of the other vector 171 | * @return This vector for chaining. 172 | */ 173 | public Vector4 add(float x, float y, float z, float w) { 174 | return this.set(this.x + x, this.y + y, this.z + z, this.w + w); 175 | } 176 | 177 | /** 178 | * Adds the given value to all three components of the vector. 179 | * 180 | * @param values 181 | * The value 182 | * @return This vector for chaining 183 | */ 184 | public Vector4 add(float values) { 185 | return this.set(this.x + values, this.y + values, this.z + values, this.w + values); 186 | } 187 | 188 | /** 189 | * Subtracts the given vector from this vector 190 | * 191 | * @param a_vec 192 | * The other vector 193 | * @return This vector for chaining 194 | */ 195 | public Vector4 sub(Vector4 a_vec) { 196 | return this.sub(a_vec.x, a_vec.y, a_vec.z, a_vec.w); 197 | } 198 | 199 | /** 200 | * Subtracts the other vector from this vector. 201 | * 202 | * @param x 203 | * The x-component of the other vector 204 | * @param y 205 | * The y-component of the other vector 206 | * @param z 207 | * The z-component of the other vector 208 | * @return This vector for chaining 209 | */ 210 | public Vector4 sub(float x, float y, float z, float w) { 211 | return this.set(this.x - x, this.y - y, this.z - z, this.w - w); 212 | } 213 | 214 | /** 215 | * Subtracts the given value from all components of this vector 216 | * 217 | * @param value 218 | * The value 219 | * @return This vector for chaining 220 | */ 221 | public Vector4 sub(float value) { 222 | return this.set(this.x - value, this.y - value, this.z - value, this.w - value); 223 | } 224 | 225 | /** 226 | * Multiplies all components of this vector by the given value 227 | * 228 | * @param value 229 | * The value 230 | * @return This vector for chaining 231 | */ 232 | public Vector4 mul(float value) { 233 | return this.set(this.x * value, this.y * value, this.z * value, this.w * value); 234 | } 235 | 236 | /** 237 | * Divides all components of this vector by the given value 238 | * 239 | * @param value 240 | * The value 241 | * @return This vector for chaining 242 | */ 243 | public Vector4 div(float value) { 244 | float d = 1 / value; 245 | return this.set(this.x * d, this.y * d, this.z * d, this.w * d); 246 | } 247 | 248 | /** 249 | * @return The euclidian length 250 | */ 251 | public float len() { 252 | return (float) Math.sqrt(x * x + y * y + z * z + w * w); 253 | } 254 | 255 | /** 256 | * @return The squared euclidian length 257 | */ 258 | public float len2() { 259 | return x * x + y * y + z * z + w * w; 260 | } 261 | 262 | /** 263 | * @param vector 264 | * The other vector 265 | * @return Wether this and the other vector are equal 266 | */ 267 | public boolean idt(Vector4 vector) { 268 | return x == vector.x && y == vector.y && z == vector.z && w == vector.w; 269 | } 270 | 271 | /** 272 | * @param vector 273 | * The other vector 274 | * @return The euclidian distance between this and the other vector 275 | */ 276 | public float dst(Vector4 vector) { 277 | float a = vector.x - x; 278 | float b = vector.y - y; 279 | float c = vector.z - z; 280 | float d = vector.w - w; 281 | a *= a; 282 | b *= b; 283 | c *= c; 284 | d *= d; 285 | 286 | return (float) Math.sqrt(a + b + c + d); 287 | } 288 | 289 | /** 290 | * @param vector 291 | * The other vector 292 | * @return The squared euclidian distance between this and the other vector 293 | */ 294 | public float dist2(Vector4 vector) { 295 | float a = vector.x - x; 296 | float b = vector.y - y; 297 | float c = vector.z - z; 298 | float d = vector.w - w; 299 | return a * a + b * b + c * c + d * d; 300 | } 301 | 302 | /** 303 | * Normalizes this vector to unit length 304 | * 305 | * @return This vector for chaining 306 | */ 307 | public Vector4 nor() { 308 | if (x == 0 && y == 0 && z == 0 && w == 0) 309 | return this; 310 | else 311 | return this.div(this.len()); 312 | } 313 | 314 | /** 315 | * {@inheritDoc} 316 | */ 317 | @Override 318 | public int hashCode() { 319 | final int prime = 31; 320 | int result = 1; 321 | result = prime * result + Float.floatToIntBits(x); 322 | result = prime * result + Float.floatToIntBits(y); 323 | result = prime * result + Float.floatToIntBits(z); 324 | result = prime * result + Float.floatToIntBits(w); 325 | return result; 326 | } 327 | 328 | /** 329 | * {@inheritDoc} 330 | */ 331 | @Override 332 | public boolean equals(Object obj) { 333 | if (this == obj) 334 | return true; 335 | if (obj == null) 336 | return false; 337 | if (getClass() != obj.getClass()) 338 | return false; 339 | Vector4 other = (Vector4) obj; 340 | if (Float.floatToIntBits(x) != Float.floatToIntBits(other.x)) 341 | return false; 342 | if (Float.floatToIntBits(y) != Float.floatToIntBits(other.y)) 343 | return false; 344 | if (Float.floatToIntBits(z) != Float.floatToIntBits(other.z)) 345 | return false; 346 | if (Float.floatToIntBits(w) != Float.floatToIntBits(other.w)) 347 | return false; 348 | return true; 349 | } 350 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/rotation/MagAccelListener.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.rotation; 2 | 3 | import android.hardware.Sensor; 4 | import android.hardware.SensorEvent; 5 | import android.hardware.SensorEventListener; 6 | import android.hardware.SensorManager; 7 | 8 | /** 9 | * Magnetometer / Accelerometer sensor fusion Smoothed by means of simple high 10 | * pass filter 11 | * 12 | * When it receives an event it will notify the constructor-specified delegate. 13 | * 14 | * @author Adam 15 | * 16 | */ 17 | public class MagAccelListener implements SensorEventListener { 18 | private float[] mRotationM = new float[16]; 19 | // smoothing factor - tune to taste 20 | private final float mFilterFactor = 0.1f; 21 | private final float mFilterFactorInv = 1.0f - mFilterFactor; 22 | private boolean mIsReady = false; 23 | // smoothed accelerometer values 24 | private float[] mAccelVals = new float[] { 0f, 0f, 9.8f }; 25 | // smoothed magnetometer values 26 | private float[] mMagVals = new float[] { 0.5f, 0f, 0f }; 27 | private RotationUpdateDelegate mRotationUpdateDelegate; 28 | 29 | public MagAccelListener(RotationUpdateDelegate rotationUpdateDelegate) { 30 | mRotationUpdateDelegate = rotationUpdateDelegate; 31 | } 32 | 33 | @Override 34 | public void onSensorChanged(SensorEvent event) { 35 | switch (event.sensor.getType()) { 36 | case Sensor.TYPE_ACCELEROMETER: 37 | smooth(event.values, mAccelVals, mAccelVals); 38 | break; 39 | case Sensor.TYPE_MAGNETIC_FIELD: 40 | smooth(event.values, mMagVals, mMagVals); 41 | mIsReady = true; 42 | break; 43 | default: 44 | break; 45 | } 46 | // wait until we have both a new accelerometer and magnetometer sample 47 | if (mIsReady) { 48 | mIsReady = false; 49 | fuseValues(); 50 | } 51 | } 52 | 53 | private void fuseValues() { 54 | SensorManager.getRotationMatrix(mRotationM, null, mAccelVals, mMagVals); 55 | mRotationUpdateDelegate.onRotationUpdate(mRotationM); 56 | } 57 | 58 | private void smooth(float[] inv, float prevv[], float outv[]) { 59 | outv[0] = inv[0] * mFilterFactor + prevv[0] * (mFilterFactorInv); 60 | outv[1] = inv[1] * mFilterFactor + prevv[1] * (mFilterFactorInv); 61 | outv[2] = inv[2] * mFilterFactor + prevv[2] * (mFilterFactorInv); 62 | } 63 | 64 | @Override 65 | public void onAccuracyChanged(Sensor sensor, int accuracy) { 66 | } 67 | } -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/rotation/RotationUpdateDelegate.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.rotation; 2 | 3 | /** 4 | * Delegate to receive updates when rotation of device changes 5 | * 6 | * @author Adam 7 | * 8 | */ 9 | public interface RotationUpdateDelegate { 10 | /** 11 | * 12 | * @param newMatrix 13 | * - 4x4 matrix 14 | */ 15 | public void onRotationUpdate(float newMatrix[]); 16 | } 17 | -------------------------------------------------------------------------------- /RotationVectorCompass/src/com/adamratana/rotationvectorcompass/rotation/RotationVectorListener.java: -------------------------------------------------------------------------------- 1 | package com.adamratana.rotationvectorcompass.rotation; 2 | 3 | import android.annotation.SuppressLint; 4 | import android.hardware.Sensor; 5 | import android.hardware.SensorEvent; 6 | import android.hardware.SensorEventListener; 7 | import android.hardware.SensorManager; 8 | 9 | /** 10 | * Uses the new (Android 2.3+) Rotation Vector virtual sensor, which is a sensor 11 | * fusion of the magnetometer, accelerometer, and gyroscope (if present) Note: 12 | * this does not seem to function correctly for Samsung Galaxy Tab 10.1, in that 13 | * North does not point North 14 | * 15 | * When it receives an event it will notify the constructor-specified delegate. 16 | * 17 | * @author Adam 18 | * 19 | */ 20 | public class RotationVectorListener implements SensorEventListener { 21 | private float[] mRotationM = new float[16]; 22 | private RotationUpdateDelegate mRotationUpdateDelegate; 23 | 24 | public RotationVectorListener(RotationUpdateDelegate rotationUpdateDelegate) { 25 | mRotationUpdateDelegate = rotationUpdateDelegate; 26 | } 27 | 28 | @SuppressLint("NewApi") 29 | @Override 30 | public void onSensorChanged(SensorEvent event) { 31 | SensorManager.getRotationMatrixFromVector(mRotationM, event.values); 32 | mRotationUpdateDelegate.onRotationUpdate(mRotationM); 33 | } 34 | 35 | @Override 36 | public void onAccuracyChanged(Sensor sensor, int accuracy) { 37 | } 38 | } --------------------------------------------------------------------------------