yingtongwang/sensor-fusion-demo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #1 from renaudcerrato/feature/optimize_allocations

Browse Source 
Decreased the amount of allocations
This commit is contained in:
Alexander Pacha
2016-09-18 21:08:08 +02:00
committed by GitHub
parent dfbfcbde62 7c726e1bab
commit 4c9c7cc214
17 changed files with 584 additions and 714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
app/src/main/java/org/hitlabnz/sensor_fusion_demo/CubeRenderer.java
View File

@@ -25,6 +25,7 @@ public class CubeRenderer implements GLSurfaceView.Renderer {
* The current provider of the device orientation.
*/
private OrientationProvider orientationProvider = null;
private Quaternion quaternion = new Quaternion();
/**
* Initialises a new CubeRenderer
@@ -69,8 +70,8 @@ public class CubeRenderer implements GLSurfaceView.Renderer {
//gl.glMultMatrixf(orientationProvider.getRotationMatrix().getMatrix(), 0);
// Get the rotation from the current orientationProvider as quaternion
Quaternion q = orientationProvider.getQuaternion();
gl.glRotatef((float) (2.0f * Math.acos(q.getW()) * 180.0f / Math.PI), q.getX(), q.getY(), q.getZ());
orientationProvider.getQuaternion(quaternion);
gl.glRotatef((float) (2.0f * Math.acos(quaternion.getW()) * 180.0f / Math.PI), quaternion.getX(), quaternion.getY(), quaternion.getZ());
}
// draw our object
@@ -88,8 +89,8 @@ public class CubeRenderer implements GLSurfaceView.Renderer {
//gl.glMultMatrixf(orientationProvider.getRotationMatrix().getMatrix(), 0);
// Get the rotation from the current orientationProvider as quaternion
Quaternion q = orientationProvider.getQuaternion();
gl.glRotatef((float) (2.0f * Math.acos(q.getW()) * 180.0f / Math.PI), q.getX(), q.getY(), q.getZ());
orientationProvider.getQuaternion(quaternion);
gl.glRotatef((float) (2.0f * Math.acos(quaternion.getW()) * 180.0f / Math.PI), quaternion.getX(), quaternion.getY(), quaternion.getZ());
}
float dist = 3;

17
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/AccelerometerCompassProvider.java
View File

@@ -16,12 +16,17 @@ public class AccelerometerCompassProvider extends OrientationProvider {
/**
* Compass values
*/
private float[] magnitudeValues = new float[3];
final private float[] magnitudeValues = new float[3];
/**
* Accelerometer values
*/
private float[] accelerometerValues = new float[3];
final private float[] accelerometerValues = new float[3];
/**
* Temporary variable to save allocations
*/
final float[] tmpFloat = new float[16];
/**
* Initialises a new AccelerometerCompassProvider
@@ -42,16 +47,14 @@ public class AccelerometerCompassProvider extends OrientationProvider {
// we received a sensor event. it is a good practice to check
// that we received the proper event
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
magnitudeValues = event.values.clone();
System.arraycopy(event.values, 0, magnitudeValues, 0, magnitudeValues.length);
} else if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
accelerometerValues = event.values.clone();
System.arraycopy(event.values, 0, accelerometerValues, 0, accelerometerValues.length);
}
if (magnitudeValues != null && accelerometerValues != null) {
float[] i = new float[16];
// Fuse accelerometer with compass
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, accelerometerValues,
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, tmpFloat, accelerometerValues,
magnitudeValues);
// Transform rotation matrix to quaternion
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);

9
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/CalibratedGyroscopeProvider.java
View File

@@ -54,6 +54,11 @@ public class CalibratedGyroscopeProvider extends OrientationProvider {
*/
private double gyroscopeRotationVelocity = 0;
/**
* Temporary variable to save allocations.
*/
private Quaternion correctedQuat = new Quaternion();
/**
* Initialises a new CalibratedGyroscopeProvider
*
@@ -110,7 +115,7 @@ public class CalibratedGyroscopeProvider extends OrientationProvider {
deltaQuaternion.multiplyByQuat(currentOrientationQuaternion, currentOrientationQuaternion);
}
Quaternion correctedQuat = currentOrientationQuaternion.clone();
correctedQuat.set(currentOrientationQuaternion);
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
@@ -118,7 +123,7 @@ public class CalibratedGyroscopeProvider extends OrientationProvider {
synchronized (syncToken) {
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix,
correctedQuat.ToArray());
correctedQuat.array());
}
}
timestamp = event.timestamp;

17
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/GravityCompassProvider.java
View File

@@ -16,12 +16,17 @@ public class GravityCompassProvider extends OrientationProvider {
/**
* Compass values
*/
private float[] magnitudeValues = new float[3];
final private float[] magnitudeValues = new float[3];
/**
* Gravity values
*/
private float[] gravityValues = new float[3];
final private float[] gravityValues = new float[3];
/**
* Temporary variable to save some allocations
*/
float[] tmpFloat = new float[16];
/**
* Initialises a new GravityCompassProvider
@@ -42,16 +47,14 @@ public class GravityCompassProvider extends OrientationProvider {
// we received a sensor event. it is a good practice to check
// that we received the proper event
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
magnitudeValues = event.values.clone();
System.arraycopy(event.values, 0, magnitudeValues, 0, magnitudeValues.length);
} else if (event.sensor.getType() == Sensor.TYPE_GRAVITY) {
gravityValues = event.values.clone();
System.arraycopy(event.values, 0, gravityValues, 0, gravityValues.length);
}
if (magnitudeValues != null && gravityValues != null) {
float[] i = new float[16];
// Fuse gravity-sensor (virtual sensor) with compass
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, gravityValues, magnitudeValues);
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, tmpFloat, gravityValues, magnitudeValues);
// Transform rotation matrix to quaternion
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);
}

26
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/ImprovedOrientationSensor1Provider.java
View File

@@ -126,6 +126,13 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
*/
private static final int PANIC_THRESHOLD = 60;
/**
* Some temporary variables to save allocations
*/
final private float[] tmpFloat = new float[4];
final private Quaternion correctedQuaternion = new Quaternion();
final private Quaternion interpolateQuaternion = new Quaternion();
/**
* Initialises a new ImprovedOrientationSensor1Provider
*
@@ -144,13 +151,11 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
// Process rotation vector (just safe it)
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
SensorManager.getQuaternionFromVector(tmpFloat, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
quaternionRotationVector.setXYZW(tmpFloat[1], tmpFloat[2], tmpFloat[3], -tmpFloat[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
@@ -213,13 +218,12 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
// Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
// The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
Quaternion interpolate = new Quaternion();
quaternionGyroscope.slerp(quaternionRotationVector, interpolate, DIRECT_INTERPOLATION_WEIGHT);
quaternionGyroscope.slerp(quaternionRotationVector, interpolateQuaternion, DIRECT_INTERPOLATION_WEIGHT);
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
setOrientationQuaternionAndMatrix(interpolateQuaternion);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
quaternionGyroscope.copyVec4(interpolateQuaternion);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
@@ -257,17 +261,17 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
* @param quaternion The Quaternion to set (the result of the sensor fusion)
*/
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
correctedQuaternion.set(quaternion);
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
correctedQuaternion.w(-correctedQuaternion.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());
}
}
}

26
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/ImprovedOrientationSensor2Provider.java
View File

@@ -126,6 +126,13 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
*/
private static final int PANIC_THRESHOLD = 60;
/**
* Some temporary variable to save allocations.
*/
final private Quaternion correctedQuaternion = new Quaternion();
final private Quaternion interpolateQuaternion = new Quaternion();
final private float[] tmpFloat = new float[4];
/**
* Initialises a new ImprovedOrientationSensor2Provider
*
@@ -144,13 +151,11 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
// Process rotation vector (just safe it)
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
SensorManager.getQuaternionFromVector(tmpFloat, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
quaternionRotationVector.setXYZW(tmpFloat[1], tmpFloat[2], tmpFloat[3], -tmpFloat[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
@@ -213,14 +218,13 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
// Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
// The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
Quaternion interpolate = new Quaternion();
quaternionGyroscope.slerp(quaternionRotationVector, interpolate,
quaternionGyroscope.slerp(quaternionRotationVector, interpolateQuaternion,
(float) (INDIRECT_INTERPOLATION_WEIGHT * gyroscopeRotationVelocity));
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
setOrientationQuaternionAndMatrix(interpolateQuaternion);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
quaternionGyroscope.copyVec4(interpolateQuaternion);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
@@ -258,17 +262,17 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
* @param quaternion The Quaternion to set (the result of the sensor fusion)
*/
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
correctedQuaternion.set(quaternion);
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
correctedQuaternion.w(-correctedQuaternion.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());
}
}
}

28
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/OrientationProvider.java
View File

@@ -6,8 +6,7 @@ package org.hitlabnz.sensor_fusion_demo.orientationProvider;
import java.util.ArrayList;
import java.util.List;
import org.hitlabnz.sensor_fusion_demo.representation.EulerAngles;
import org.hitlabnz.sensor_fusion_demo.representation.Matrixf4x4;
import org.hitlabnz.sensor_fusion_demo.representation.MatrixF4x4;
import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;
import android.hardware.Sensor;
@@ -39,7 +38,7 @@ public abstract class OrientationProvider implements SensorEventListener {
/**
* The matrix that holds the current rotation
*/
protected final Matrixf4x4 currentOrientationRotationMatrix;
protected final MatrixF4x4 currentOrientationRotationMatrix;
/**
* The quaternion that holds the current rotation
@@ -61,7 +60,7 @@ public abstract class OrientationProvider implements SensorEventListener {
this.sensorManager = sensorManager;
// Initialise with identity
currentOrientationRotationMatrix = new Matrixf4x4();
currentOrientationRotationMatrix = new MatrixF4x4();
// Initialise with identity
currentOrientationQuaternion = new Quaternion();
@@ -97,34 +96,29 @@ public abstract class OrientationProvider implements SensorEventListener {
}
/**
* @return Returns the current rotation of the device in the rotation matrix
* format (4x4 matrix)
* Get the current rotation of the device in the rotation matrix format (4x4 matrix)
*/
public Matrixf4x4 getRotationMatrix() {
public void getRotationMatrix(MatrixF4x4 matrix) {
synchronized (syncToken) {
return currentOrientationRotationMatrix;
matrix.set(currentOrientationRotationMatrix);
}
}
/**
* @return Returns the current rotation of the device in the quaternion
* format (vector4f)
* Get the current rotation of the device in the quaternion format (vector4f)
*/
public Quaternion getQuaternion() {
public void getQuaternion(Quaternion quaternion) {
synchronized (syncToken) {
return currentOrientationQuaternion.clone();
quaternion.set(currentOrientationQuaternion);
}
}
/**
* @return Returns the current rotation of the device in the Euler-Angles
* Get the current rotation of the device in the Euler angles
*/
public EulerAngles getEulerAngles() {
public void getEulerAngles(float angles[]) {
synchronized (syncToken) {
float[] angles = new float[3];
SensorManager.getOrientation(currentOrientationRotationMatrix.matrix, angles);
return new EulerAngles(angles[0], angles[1], angles[2]);
}
}
}

11
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/RotationVectorProvider.java
View File

@@ -13,6 +13,12 @@ import android.hardware.SensorManager;
*/
public class RotationVectorProvider extends OrientationProvider {
/**
* Temporary variable to save allocation
*/
final private float[] tmpFloat = new float[4];
/**
* Initialises a new RotationVectorProvider
*
@@ -36,10 +42,9 @@ public class RotationVectorProvider extends OrientationProvider {
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, event.values);
// Get Quaternion
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
currentOrientationQuaternion.setXYZW(q[1], q[2], q[3], -q[0]);
SensorManager.getQuaternionFromVector(tmpFloat, event.values);
currentOrientationQuaternion.setXYZW(tmpFloat[1], tmpFloat[2], tmpFloat[3], -tmpFloat[0]);
}
}
}

26
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/EulerAngles.java
View File

@@ -1,26 +0,0 @@
package org.hitlabnz.sensor_fusion_demo.representation;
public class EulerAngles {
private float yaw;
private float pitch;
private float roll;
public EulerAngles(float yaw, float pitch, float roll) {
this.yaw = yaw;
this.pitch = pitch;
this.roll = roll;
}
public float getYaw() {
return yaw;
}
public float getPitch() {
return pitch;
}
public float getRoll() {
return roll;
}
}

463
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/MatrixF4x4.java Normal file
View File

@@ -0,0 +1,463 @@
package org.hitlabnz.sensor_fusion_demo.representation;
import android.util.Log;
/**
* The Class MatrixF4x4.
*
* Internal the matrix is structured as
*
* [ x0 , y0 , z0 , w0 ] [ x1 , y1 , z1 , w1 ] [ x2 , y2 , z2 , w2 ] [ x3 , y3 , z3 , w3 ]
*
* it is recommend that when setting the matrix values individually that you use the set{x,#} methods, where 'x' is
* either x, y, z or w and # is either 0, 1, 2 or 3, setY1 for example. The reason you should use these functions is
* because it will map directly to that part of the matrix regardless of whether or not the internal matrix is column
* major or not. If the matrix is either or length 9 or 16 it will be able to determine if it can set the value or not.
* If the matrix is of size 9 but you set say w2, the value will not be set and the set method will return without any
* error.
*
*/
public class MatrixF4x4 {
public static final int[] matIndCol9_3x3 = { 0, 1, 2, 3, 4, 5, 6, 7, 8 };
public static final int[] matIndCol16_3x3 = { 0, 1, 2, 4, 5, 6, 8, 9, 10 };
public static final int[] matIndRow9_3x3 = { 0, 3, 6, 1, 4, 7, 3, 5, 8 };
public static final int[] matIndRow16_3x3 = { 0, 4, 8, 1, 5, 9, 2, 6, 10 };
public static final int[] matIndCol16_4x4 = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };
public static final int[] matIndRow16_4x4 = { 0, 4, 8, 12, 1, 5, 9, 13, 2, 6, 10, 14, 3, 7, 11, 15 };
private boolean colMaj = true;
/** The matrix. */
public float[] matrix;
/**
* Instantiates a new matrixf4x4. The Matrix is assumed to be Column major, however you can change this by using the
* setColumnMajor function to false and it will operate like a row major matrix.
*/
public MatrixF4x4() {
// The matrix is defined as float[column][row]
this.matrix = new float[16];
Matrix.setIdentityM(this.matrix, 0);
}
/**
* Gets the matrix.
*
* @return the matrix, can be null if the matrix is invalid
*/
public float[] getMatrix() {
return this.matrix;
}
public int size() {
return matrix.length;
}
/**
* Sets the matrix from a float[16] array. If the matrix you set isn't 16 long then the matrix will be set as
* invalid.
*
* @param matrix the new matrix
*/
public void setMatrix(float[] matrix) {
if (matrix.length == 16 || matrix.length == 9)
this.matrix = matrix;
else {
throw new IllegalArgumentException("Matrix set is invalid, size is " + matrix.length + " expected 9 or 16");
}
}
public void set(MatrixF4x4 source) {
System.arraycopy(source.matrix, 0, matrix, 0, matrix.length);
}
/**
* Set whether the internal data is col major by passing true, or false for a row major matrix. The matrix is column
* major by default.
*
* @param colMajor
*/
public void setColumnMajor(boolean colMajor) {
this.colMaj = colMajor;
}
/**
* Find out if the stored matrix is column major
*
* @return
*/
public boolean isColumnMajor() {
return colMaj;
}
/**
* Multiply the given vector by this matrix. This should only be used if the matrix is of size 16 (use the
* matrix.size() method).
*
* @param vector A vector of length 4.
*/
public void multiplyVector4fByMatrix(Vector4f vector) {
if (matrix.length == 16) {
float x = 0;
float y = 0;
float z = 0;
float w = 0;
float[] vectorArray = vector.array();
if (colMaj) {
for (int i = 0; i < 4; i++) {
int k = i * 4;
x += this.matrix[k + 0] * vectorArray[i];
y += this.matrix[k + 1] * vectorArray[i];
z += this.matrix[k + 2] * vectorArray[i];
w += this.matrix[k + 3] * vectorArray[i];
}
} else {
for (int i = 0; i < 4; i++) {
x += this.matrix[0 + i] * vectorArray[i];
y += this.matrix[4 + i] * vectorArray[i];
z += this.matrix[8 + i] * vectorArray[i];
w += this.matrix[12 + i] * vectorArray[i];
}
}
vector.setX(x);
vector.setY(y);
vector.setZ(z);
vector.setW(w);
} else
Log.e("matrix", "Matrix is invalid, is " + matrix.length + " long, this equation expects a 16 value matrix");
}
/**
* Multiply the given vector by this matrix. This should only be used if the matrix is of size 9 (use the
* matrix.size() method).
*
* @param vector A vector of length 3.
*/
public void multiplyVector3fByMatrix(Vector3f vector) {
if (matrix.length == 9) {
float x = 0;
float y = 0;
float z = 0;
float[] vectorArray = vector.toArray();
if (!colMaj) {
for (int i = 0; i < 3; i++) {
int k = i * 3;
x += this.matrix[k + 0] * vectorArray[i];
y += this.matrix[k + 1] * vectorArray[i];
z += this.matrix[k + 2] * vectorArray[i];
}
} else {
for (int i = 0; i < 3; i++) {
x += this.matrix[0 + i] * vectorArray[i];
y += this.matrix[3 + i] * vectorArray[i];
z += this.matrix[6 + i] * vectorArray[i];
}
}
vector.setX(x);
vector.setY(y);
vector.setZ(z);
} else
Log.e("matrix", "Matrix is invalid, is " + matrix.length
+ " long, this function expects the internal matrix to be of size 9");
}
/**
* Multiply matrix4x4 by matrix.
*
* @param matrixf the matrixf
*/
public void multiplyMatrix4x4ByMatrix(MatrixF4x4 matrixf) {
// TODO implement Strassen Algorithm in place of this slower naive one.
float[] bufferMatrix = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float[] matrix = matrixf.getMatrix();
/**
* for(int i = 0; i < 4; i++){ for(int j = 0; j < 4; j++){
*
* int k = i * 4; bufferMatrix[0 + j] += this.matrix[k + j] * matrix[0 * 4 + i]; bufferMatrix[1 * 4 + j] +=
* this.matrix[k + j] * matrix[1 * 4 + i]; bufferMatrix[2 * 4 + j] += this.matrix[k + j] * matrix[2 * 4 +
* i]; bufferMatrix[3 * 4 + j] += this.matrix[k + j] * matrix[3 * 4 + i]; } }
*/
multiplyMatrix(matrix, 0, bufferMatrix, 0);
matrixf.setMatrix(bufferMatrix);
}
public void multiplyMatrix(float[] input, int inputOffset, float[] output, int outputOffset) {
float[] bufferMatrix = output;
float[] matrix = input;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
int k = i * 4;
bufferMatrix[outputOffset + 0 + j] += this.matrix[k + j] * matrix[inputOffset + 0 * 4 + i];
bufferMatrix[outputOffset + 1 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 1 * 4 + i];
bufferMatrix[outputOffset + 2 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 2 * 4 + i];
bufferMatrix[outputOffset + 3 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 3 * 4 + i];
}
}
}
/**
* This will rearrange the internal structure of the matrix. Be careful though as this is an expensive operation.
*/
public void transpose() {
if (this.matrix.length == 16) {
float[] newMatrix = new float[16];
for (int i = 0; i < 4; i++) {
int k = i * 4;
newMatrix[k] = matrix[i];
newMatrix[k + 1] = matrix[4 + i];
newMatrix[k + 2] = matrix[8 + i];
newMatrix[k + 3] = matrix[12 + i];
}
matrix = newMatrix;
} else {
float[] newMatrix = new float[9];
for (int i = 0; i < 3; i++) {
int k = i * 3;
newMatrix[k] = matrix[i];
newMatrix[k + 1] = matrix[3 + i];
newMatrix[k + 2] = matrix[6 + i];
}
matrix = newMatrix;
}
}
public void setX0(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[0]] = value;
else
matrix[matIndRow16_3x3[0]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[0]] = value;
else
matrix[matIndRow9_3x3[0]] = value;
}
}
public void setX1(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[1]] = value;
else
matrix[matIndRow16_3x3[1]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[1]] = value;
else
matrix[matIndRow9_3x3[1]] = value;
}
}
public void setX2(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[2]] = value;
else
matrix[matIndRow16_3x3[2]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[2]] = value;
else
matrix[matIndRow9_3x3[2]] = value;
}
}
public void setY0(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[3]] = value;
else
matrix[matIndRow16_3x3[3]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[3]] = value;
else
matrix[matIndRow9_3x3[3]] = value;
}
}
public void setY1(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[4]] = value;
else
matrix[matIndRow16_3x3[4]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[4]] = value;
else
matrix[matIndRow9_3x3[4]] = value;
}
}
public void setY2(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[5]] = value;
else
matrix[matIndRow16_3x3[5]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[5]] = value;
else
matrix[matIndRow9_3x3[5]] = value;
}
}
public void setZ0(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[6]] = value;
else
matrix[matIndRow16_3x3[6]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[6]] = value;
else
matrix[matIndRow9_3x3[6]] = value;
}
}
public void setZ1(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[7]] = value;
else
matrix[matIndRow16_3x3[7]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[7]] = value;
else
matrix[matIndRow9_3x3[7]] = value;
}
}
public void setZ2(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[8]] = value;
else
matrix[matIndRow16_3x3[8]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[8]] = value;
else
matrix[matIndRow9_3x3[8]] = value;
}
}
public void setX3(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[3]] = value;
else
matrix[matIndRow16_4x4[3]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setY3(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[7]] = value;
else
matrix[matIndRow16_4x4[7]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setZ3(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[11]] = value;
else
matrix[matIndRow16_4x4[11]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setW0(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[12]] = value;
else
matrix[matIndRow16_4x4[12]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setW1(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[13]] = value;
else
matrix[matIndRow16_4x4[13]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setW2(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[14]] = value;
else
matrix[matIndRow16_4x4[14]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
public void setW3(float value) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[15]] = value;
else
matrix[matIndRow16_4x4[15]] = value;
}else
throw new IllegalStateException("length of matrix should be 16");
}
}

512
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Matrixf4x4.java
View File

@@ -1,512 +0,0 @@
package org.hitlabnz.sensor_fusion_demo.representation;
import android.util.Log;
/**
* The Class Matrixf4x4.
*
* Internal the matrix is structured as
*
* [ x0 , y0 , z0 , w0 ] [ x1 , y1 , z1 , w1 ] [ x2 , y2 , z2 , w2 ] [ x3 , y3 , z3 , w3 ]
*
* it is recommend that when setting the matrix values individually that you use the set{x,#} methods, where 'x' is
* either x, y, z or w and # is either 0, 1, 2 or 3, setY1 for example. The reason you should use these functions is
* because it will map directly to that part of the matrix regardless of whether or not the internal matrix is column
* major or not. If the matrix is either or length 9 or 16 it will be able to determine if it can set the value or not.
* If the matrix is of size 9 but you set say w2, the value will not be set and the set method will return without any
* error.
*
*/
public class Matrixf4x4 {
public static final int[] matIndCol9_3x3 = { 0, 1, 2, 3, 4, 5, 6, 7, 8 };
public static final int[] matIndCol16_3x3 = { 0, 1, 2, 4, 5, 6, 8, 9, 10 };
public static final int[] matIndRow9_3x3 = { 0, 3, 6, 1, 4, 7, 3, 5, 8 };
public static final int[] matIndRow16_3x3 = { 0, 4, 8, 1, 5, 9, 2, 6, 10 };
public static final int[] matIndCol16_4x4 = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };
public static final int[] matIndRow16_4x4 = { 0, 4, 8, 12, 1, 5, 9, 13, 2, 6, 10, 14, 3, 7, 11, 15 };
private boolean colMaj = true;
private boolean matrixValid = false;
/** The matrix. */
public float[] matrix;
/**
* Instantiates a new matrixf4x4. The Matrix is assumed to be Column major, however you can change this by using the
* setColumnMajor function to false and it will operate like a row major matrix.
*/
public Matrixf4x4() {
// The matrix is defined as float[column][row]
this.matrix = new float[16];
Matrix.setIdentityM(this.matrix, 0);
matrixValid = true;
}
/**
* Gets the matrix.
*
* @return the matrix, can be null if the matrix is invalid
*/
public float[] getMatrix() {
return this.matrix;
}
public int size() {
return matrix.length;
}
/**
* Sets the matrix from a float[16] array. If the matrix you set isn't 16 long then the matrix will be set as
* invalid.
*
* @param matrix the new matrix
*/
public void setMatrix(float[] matrix) {
this.matrix = matrix;
if (matrix.length == 16 || matrix.length == 9)
this.matrixValid = true;
else {
this.matrixValid = false;
Log.e("matrix", "Matrix set is invalid, size is " + matrix.length + " expected 9 or 16");
}
}
public void setMatrixValues(float[] otherMatrix) {
if (this.matrix.length != otherMatrix.length) {
Log.e("matrix", "Matrix set is invalid, size is " + otherMatrix.length + " expected 9 or 16");
}
for (int i = 0; i < otherMatrix.length; i++) {
this.matrix[i] = otherMatrix[i];
}
}
/**
* Set whether the internal data is col major by passing true, or false for a row major matrix. The matrix is column
* major by default.
*
* @param colMajor
*/
public void setColumnMajor(boolean colMajor) {
this.colMaj = colMajor;
}
/**
* Find out if the stored matrix is column major
*
* @return
*/
public boolean isColumnMajor() {
return colMaj;
}
/**
* Multiply the given vector by this matrix. This should only be used if the matrix is of size 16 (use the
* matrix.size() method).
*
* @param vector A vector of length 4.
*/
public void multiplyVector4fByMatrix(Vector4f vector) {
if (matrixValid && matrix.length == 16) {
float x = 0;
float y = 0;
float z = 0;
float w = 0;
float[] vectorArray = vector.ToArray();
if (colMaj) {
for (int i = 0; i < 4; i++) {
int k = i * 4;
x += this.matrix[k + 0] * vectorArray[i];
y += this.matrix[k + 1] * vectorArray[i];
z += this.matrix[k + 2] * vectorArray[i];
w += this.matrix[k + 3] * vectorArray[i];
}
} else {
for (int i = 0; i < 4; i++) {
x += this.matrix[0 + i] * vectorArray[i];
y += this.matrix[4 + i] * vectorArray[i];
z += this.matrix[8 + i] * vectorArray[i];
w += this.matrix[12 + i] * vectorArray[i];
}
}
vector.setX(x);
vector.setY(y);
vector.setZ(z);
vector.setW(w);
} else
Log.e("matrix", "Matrix is invalid, is " + matrix.length + " long, this equation expects a 16 value matrix");
}
/**
* Multiply the given vector by this matrix. This should only be used if the matrix is of size 9 (use the
* matrix.size() method).
*
* @param vector A vector of length 3.
*/
public void multiplyVector3fByMatrix(Vector3f vector) {
if (matrixValid && matrix.length == 9) {
float x = 0;
float y = 0;
float z = 0;
float[] vectorArray = vector.toArray();
if (!colMaj) {
for (int i = 0; i < 3; i++) {
int k = i * 3;
x += this.matrix[k + 0] * vectorArray[i];
y += this.matrix[k + 1] * vectorArray[i];
z += this.matrix[k + 2] * vectorArray[i];
}
} else {
for (int i = 0; i < 3; i++) {
x += this.matrix[0 + i] * vectorArray[i];
y += this.matrix[3 + i] * vectorArray[i];
z += this.matrix[6 + i] * vectorArray[i];
}
}
vector.setX(x);
vector.setY(y);
vector.setZ(z);
} else
Log.e("matrix", "Matrix is invalid, is " + matrix.length
+ " long, this function expects the internal matrix to be of size 9");
}
public boolean isMatrixValid() {
return matrixValid;
}
/**
* Multiply matrix4x4 by matrix.
*
* @param matrixf the matrixf
*/
public void multiplyMatrix4x4ByMatrix(Matrixf4x4 matrixf) {
// TODO implement Strassen Algorithm in place of this slower naive one.
if (matrixValid && matrixf.isMatrixValid()) {
float[] bufferMatrix = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float[] matrix = matrixf.getMatrix();
/**
* for(int i = 0; i < 4; i++){ for(int j = 0; j < 4; j++){
*
* int k = i * 4; bufferMatrix[0 + j] += this.matrix[k + j] * matrix[0 * 4 + i]; bufferMatrix[1 * 4 + j] +=
* this.matrix[k + j] * matrix[1 * 4 + i]; bufferMatrix[2 * 4 + j] += this.matrix[k + j] * matrix[2 * 4 +
* i]; bufferMatrix[3 * 4 + j] += this.matrix[k + j] * matrix[3 * 4 + i]; } }
*/
multiplyMatrix(matrix, 0, bufferMatrix, 0);
matrixf.setMatrix(bufferMatrix);
} else
Log.e("matrix", "Matrix is invalid, internal is " + matrix.length + " long" + " , input matrix is "
+ matrixf.getMatrix().length + " long");
}
public void multiplyMatrix(float[] input, int inputOffset, float[] output, int outputOffset) {
float[] bufferMatrix = output;
float[] matrix = input;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
int k = i * 4;
bufferMatrix[outputOffset + 0 + j] += this.matrix[k + j] * matrix[inputOffset + 0 * 4 + i];
bufferMatrix[outputOffset + 1 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 1 * 4 + i];
bufferMatrix[outputOffset + 2 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 2 * 4 + i];
bufferMatrix[outputOffset + 3 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 3 * 4 + i];
}
}
}
/**
* This will rearrange the internal structure of the matrix. Be careful though as this is an expensive operation.
*/
public void transpose() {
if (matrixValid) {
if (this.matrix.length == 16) {
float[] newMatrix = new float[16];
for (int i = 0; i < 4; i++) {
int k = i * 4;
newMatrix[k] = matrix[i];
newMatrix[k + 1] = matrix[4 + i];
newMatrix[k + 2] = matrix[8 + i];
newMatrix[k + 3] = matrix[12 + i];
}
matrix = newMatrix;
} else {
float[] newMatrix = new float[9];
for (int i = 0; i < 3; i++) {
int k = i * 3;
newMatrix[k] = matrix[i];
newMatrix[k + 1] = matrix[3 + i];
newMatrix[k + 2] = matrix[6 + i];
}
matrix = newMatrix;
}
}
}
public void setX0(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[0]] = value;
else
matrix[matIndRow16_3x3[0]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[0]] = value;
else
matrix[matIndRow9_3x3[0]] = value;
}
}
}
public void setX1(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[1]] = value;
else
matrix[matIndRow16_3x3[1]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[1]] = value;
else
matrix[matIndRow9_3x3[1]] = value;
}
}
}
public void setX2(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[2]] = value;
else
matrix[matIndRow16_3x3[2]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[2]] = value;
else
matrix[matIndRow9_3x3[2]] = value;
}
}
}
public void setY0(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[3]] = value;
else
matrix[matIndRow16_3x3[3]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[3]] = value;
else
matrix[matIndRow9_3x3[3]] = value;
}
}
}
public void setY1(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[4]] = value;
else
matrix[matIndRow16_3x3[4]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[4]] = value;
else
matrix[matIndRow9_3x3[4]] = value;
}
}
}
public void setY2(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[5]] = value;
else
matrix[matIndRow16_3x3[5]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[5]] = value;
else
matrix[matIndRow9_3x3[5]] = value;
}
}
}
public void setZ0(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[6]] = value;
else
matrix[matIndRow16_3x3[6]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[6]] = value;
else
matrix[matIndRow9_3x3[6]] = value;
}
}
}
public void setZ1(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[7]] = value;
else
matrix[matIndRow16_3x3[7]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[7]] = value;
else
matrix[matIndRow9_3x3[7]] = value;
}
}
}
public void setZ2(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_3x3[8]] = value;
else
matrix[matIndRow16_3x3[8]] = value;
} else {
if (colMaj)
matrix[matIndCol9_3x3[8]] = value;
else
matrix[matIndRow9_3x3[8]] = value;
}
}
}
public void setX3(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[3]] = value;
else
matrix[matIndRow16_4x4[3]] = value;
}
}
}
public void setY3(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[7]] = value;
else
matrix[matIndRow16_4x4[7]] = value;
}
}
}
public void setZ3(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[11]] = value;
else
matrix[matIndRow16_4x4[11]] = value;
}
}
}
public void setW0(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[12]] = value;
else
matrix[matIndRow16_4x4[12]] = value;
}
}
}
public void setW1(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[13]] = value;
else
matrix[matIndRow16_4x4[13]] = value;
}
}
}
public void setW2(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[14]] = value;
else
matrix[matIndRow16_4x4[14]] = value;
}
}
}
public void setW3(float value) {
if (matrixValid) {
if (matrix.length == 16) {
if (colMaj)
matrix[matIndCol16_4x4[15]] = value;
else
matrix[matIndRow16_4x4[15]] = value;
}
}
}
}

72
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java
View File

@@ -20,17 +20,12 @@ package org.hitlabnz.sensor_fusion_demo.representation;
*/
public class Quaternion extends Vector4f {
/**
* A randomly generated UID to make the Quaternion object serialisable.
*/
private static final long serialVersionUID = -7148812599404359073L;
/**
* Rotation matrix that contains the same rotation as the Quaternion in a 4x4 homogenised rotation matrix.
* Remember that for performance reasons, this matrix is only updated, when it is accessed and not on every change
* of the quaternion-values.
*/
private Matrixf4x4 matrix;
private MatrixF4x4 matrix;
/**
* This variable is used to synchronise the rotation matrix with the current quaternion values. If someone has
@@ -40,22 +35,18 @@ public class Quaternion extends Vector4f {
*/
private boolean dirty = false;
private Vector4f tmpVector = new Vector4f();
private Quaternion tmpQuaternion;
/**
* Creates a new Quaternion object and initialises it with the identity Quaternion
*/
public Quaternion() {
super();
matrix = new Matrixf4x4();
matrix = new MatrixF4x4();
loadIdentityQuat();
}
@Override
public Quaternion clone() {
Quaternion clone = new Quaternion();
clone.copyVec4(this);
return clone;
}
/**
* Normalise this Quaternion into a unity Quaternion.
*/
@@ -91,7 +82,7 @@ public class Quaternion extends Vector4f {
* @param output
*/
public void multiplyByQuat(Quaternion input, Quaternion output) {
Vector4f inputCopy = new Vector4f();
if (input != output) {
output.points[3] = (points[3] * input.points[3] - points[0] * input.points[0] - points[1] * input.points[1] - points[2]
* input.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2
@@ -102,19 +93,19 @@ public class Quaternion extends Vector4f {
output.points[2] = (points[3] * input.points[2] + points[2] * input.points[3] + points[0] * input.points[1] - points[1]
* input.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2
} else {
inputCopy.points[0] = input.points[0];
inputCopy.points[1] = input.points[1];
inputCopy.points[2] = input.points[2];
inputCopy.points[3] = input.points[3];
tmpVector.points[0] = input.points[0];
tmpVector.points[1] = input.points[1];
tmpVector.points[2] = input.points[2];
tmpVector.points[3] = input.points[3];
output.points[3] = (points[3] * inputCopy.points[3] - points[0] * inputCopy.points[0] - points[1]
* inputCopy.points[1] - points[2] * inputCopy.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2
output.points[0] = (points[3] * inputCopy.points[0] + points[0] * inputCopy.points[3] + points[1]
* inputCopy.points[2] - points[2] * inputCopy.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2
output.points[1] = (points[3] * inputCopy.points[1] + points[1] * inputCopy.points[3] + points[2]
* inputCopy.points[0] - points[0] * inputCopy.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2
output.points[2] = (points[3] * inputCopy.points[2] + points[2] * inputCopy.points[3] + points[0]
* inputCopy.points[1] - points[1] * inputCopy.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2
output.points[3] = (points[3] * tmpVector.points[3] - points[0] * tmpVector.points[0] - points[1]
* tmpVector.points[1] - points[2] * tmpVector.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2
output.points[0] = (points[3] * tmpVector.points[0] + points[0] * tmpVector.points[3] + points[1]
* tmpVector.points[2] - points[2] * tmpVector.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2
output.points[1] = (points[3] * tmpVector.points[1] + points[1] * tmpVector.points[3] + points[2]
* tmpVector.points[0] - points[0] * tmpVector.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2
output.points[2] = (points[3] * tmpVector.points[2] + points[2] * tmpVector.points[3] + points[0]
* tmpVector.points[1] - points[1] * tmpVector.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2
}
}
@@ -124,16 +115,12 @@ public class Quaternion extends Vector4f {
* @param input
* @param output
*/
Quaternion bufferQuaternion;
public void multiplyByQuat(Quaternion input) {
if (bufferQuaternion == null) {
bufferQuaternion = new Quaternion();
}
this.dirty = true;
bufferQuaternion.copyVec4(this);
multiplyByQuat(input, bufferQuaternion);
this.copyVec4(bufferQuaternion);
if(tmpQuaternion == null) tmpQuaternion = new Quaternion();
tmpQuaternion.copyVec4(this);
multiplyByQuat(input, tmpQuaternion);
this.copyVec4(tmpQuaternion);
}
/**
@@ -302,15 +289,15 @@ public class Quaternion extends Vector4f {
if (this.matrix.size() == 16) {
if (this.matrix.isColumnMajor()) {
indices = Matrixf4x4.matIndCol16_3x3;
indices = MatrixF4x4.matIndCol16_3x3;
} else {
indices = Matrixf4x4.matIndRow16_3x3;
indices = MatrixF4x4.matIndRow16_3x3;
}
} else {
if (this.matrix.isColumnMajor()) {
indices = Matrixf4x4.matIndCol9_3x3;
indices = MatrixF4x4.matIndCol9_3x3;
} else {
indices = Matrixf4x4.matIndRow9_3x3;
indices = MatrixF4x4.matIndRow9_3x3;
}
}
@@ -445,7 +432,7 @@ public class Quaternion extends Vector4f {
/**
* @return Returns this Quaternion in the Rotation Matrix representation
*/
public Matrixf4x4 getMatrix4x4() {
public MatrixF4x4 getMatrix4x4() {
//toMatrixColMajor();
if (dirty) {
convertQuatToMatrix();
@@ -470,11 +457,12 @@ public class Quaternion extends Vector4f {
public void slerp(Quaternion input, Quaternion output, float t) {
// Calculate angle between them.
//double cosHalftheta = this.dotProduct(input);
Quaternion bufferQuat = null;
Quaternion bufferQuat;
float cosHalftheta = this.dotProduct(input);
if (cosHalftheta < 0) {
bufferQuat = new Quaternion();
if(tmpQuaternion == null) tmpQuaternion = new Quaternion();
bufferQuat = tmpQuaternion;
cosHalftheta = -cosHalftheta;
bufferQuat.points[0] = (-input.points[0]);
bufferQuat.points[1] = (-input.points[1]);

39
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Renderable.java
View File

@@ -1,39 +0,0 @@
package org.hitlabnz.sensor_fusion_demo.representation;
import java.io.Serializable;
import java.util.concurrent.locks.ReentrantLock;
/**
* @author Leigh Beattie
*
* At the moment this is a place holder for objects that can be put in the scene graph. There may be some
* requirements later specified.
*/
public class Renderable implements Serializable {
/**
* ID for serialisation
*/
private static final long serialVersionUID = 6701586807666461858L;
//Used in data managemenst and synchronisation. If you make a renderable then you should change this boolean to true.
protected boolean dirty = true;
protected ReentrantLock lock = new ReentrantLock();
public boolean dirty() {
return dirty;
}
public void setClean() {
this.dirty = false;
}
public void setDirty() {
this.dirty = true;
}
public ReentrantLock getLock() {
return this.lock;
}
}

27
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java
View File

@@ -3,12 +3,7 @@ package org.hitlabnz.sensor_fusion_demo.representation;
/**
* 3-dimensional vector with conventient getters and setters. Additionally this class is serializable and
*/
public class Vector3f extends Renderable {
/**
* ID for serialisation
*/
private static final long serialVersionUID = -4565578579900616220L;
public class Vector3f {
/**
* A float array was chosen instead of individual variables due to performance concerns. Converting the points into
@@ -248,13 +243,7 @@ public class Vector3f extends Renderable {
outputVec.setY(points[2] * inputVec.points[0] - points[0] * inputVec.points[2]);
outputVec.setZ(points[0] * inputVec.points[1] - points[1] * inputVec.points[0]);
}
public Vector3f crossProduct(Vector3f in) {
Vector3f out = new Vector3f();
crossProduct(in, out);
return out;
}
/**
* If you need to get the length of a vector then use this function.
*
@@ -274,11 +263,8 @@ public class Vector3f extends Renderable {
*
* @param source The vector you want to clone.
*/
public void clone(Vector3f source) {
// this.points[0] = source.points[0];
// this.points[1] = source.points[1];
// this.points[2] = source.points[2];
System.arraycopy(source.points, 0, points, 0, 3);
public void set(Vector3f source) {
set(source.points);
}
/**
@@ -286,10 +272,7 @@ public class Vector3f extends Renderable {
*
* @param source The vector you want to clone.
*/
public void clone(float[] source) {
// this.points[0] = source[0];
// this.points[1] = source[1];
// this.points[2] = source[2];
public void set(float[] source) {
System.arraycopy(source, 0, points, 0, 3);
}
}

10
app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java
View File

@@ -1,16 +1,10 @@
package org.hitlabnz.sensor_fusion_demo.representation;
import java.io.Serializable;
/**
* Representation of a four-dimensional float-vector
*/
public class Vector4f extends Renderable implements Serializable {
public class Vector4f {
/**
* ID for Serialisation
*/
private static final long serialVersionUID = 1L;
/** The points. */
protected float points[] = { 0, 0, 0, 0 };
@@ -51,7 +45,7 @@ public class Vector4f extends Renderable implements Serializable {
*
* @return the float[]
*/
public float[] ToArray() {
public float[] array() {
return points;
}

2
build.gradle
View File

@@ -5,7 +5,7 @@ buildscript {
maven { url 'http://repo1.maven.org/maven2' }
}
dependencies {
classpath 'com.android.tools.build:gradle:2.1.0'
classpath 'com.android.tools.build:gradle:2.1.3'
}
}

4
gradle/wrapper/gradle-wrapper.properties vendored
View File

@@ -1,6 +1,6 @@
#Mon Dec 28 10:00:20 PST 2015
#Mon Sep 12 12:33:18 EDT 2016
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-2.10-all.zip
distributionUrl=https\://services.gradle.org/distributions/gradle-2.14.1-all.zip