Migrated project to Android Studio
This commit is contained in:
Alexander Pacha
23
app/build.gradle
Normal file
23
app/build.gradle
Normal file
@@ -0,0 +1,23 @@
|
||||
apply plugin: 'com.android.application'
|
||||
|
||||
android {
|
||||
compileSdkVersion 23
|
||||
buildToolsVersion "23.0.2"
|
||||
|
||||
defaultConfig {
|
||||
applicationId "org.hitlabnz.sensor_fusion_demo"
|
||||
minSdkVersion 11
|
||||
targetSdkVersion 23
|
||||
}
|
||||
|
||||
buildTypes {
|
||||
release {
|
||||
minifyEnabled false
|
||||
proguardFiles getDefaultProguardFile('proguard-android.txt'), 'proguard-rules.txt'
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
dependencies {
|
||||
compile 'com.android.support:support-v4:23.2.0'
|
||||
}
|
||||
32
app/src/main/AndroidManifest.xml
Normal file
32
app/src/main/AndroidManifest.xml
Normal file
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
|
||||
package="org.hitlabnz.sensor_fusion_demo"
|
||||
android:versionCode="3"
|
||||
android:versionName="1.2" >
|
||||
|
||||
<uses-sdk
|
||||
android:minSdkVersion="11"
|
||||
android:targetSdkVersion="19" />
|
||||
|
||||
<application
|
||||
android:allowBackup="false"
|
||||
android:icon="@drawable/ic_launcher"
|
||||
android:label="@string/app_name"
|
||||
android:theme="@style/AppTheme" >
|
||||
<activity
|
||||
android:name="org.hitlabnz.sensor_fusion_demo.SensorSelectionActivity"
|
||||
android:label="@string/title_activity_sensor_selection"
|
||||
android:screenOrientation="nosensor" >
|
||||
<intent-filter>
|
||||
<action android:name="android.intent.action.MAIN" />
|
||||
|
||||
<category android:name="android.intent.category.LAUNCHER" />
|
||||
</intent-filter>
|
||||
</activity>
|
||||
<activity
|
||||
android:name="org.hitlabnz.sensor_fusion_demo.AboutActivity"
|
||||
android:label="@string/title_activity_about" >
|
||||
</activity>
|
||||
</application>
|
||||
|
||||
</manifest>
|
||||
57
app/src/main/assets/about/de/index.html
Normal file
57
app/src/main/assets/about/de/index.html
Normal file
@@ -0,0 +1,57 @@
|
||||
<html>
|
||||
<body link="#ff8800" vlink="#ff8800" alink="#ff8800">
|
||||
|
||||
<p>
|
||||
Diese App wurde von <a href="http://my-it.at">Alexander Pacha</a> am <a
|
||||
href="http://www.hitlabnz.org">Human Interface Technology
|
||||
Laboratory New Zealand</a> entwickelt und demonstriert die
|
||||
Leistungsfähigkeit von verschiedenen Sensoren und Sensorfusionen.
|
||||
Messungen von dem Gyroskop, Akzelerometer und Kompass werden in
|
||||
verschiedenen Weisen kombiniert und das Ergebnis wird als Würfel
|
||||
visualisiert, der durch rotieren des Gerätes gedreht werden kann.
|
||||
</p>
|
||||
<p>
|
||||
Die große Neuheit in dieser Applikation ist die Fusion von zwei
|
||||
virtuellen Sensoren: <b>Improved Orientation Sensor 1</b> und <b>Improved
|
||||
Orientation Sensor 2</b> nutzen den Android Rotation Vector mit dem
|
||||
kalibrierten Gyroskopsensor und erreichen eine nie zuvor dagewesenen Präzision
|
||||
und Reaktionsfähigkeit.
|
||||
</p>
|
||||
|
||||
<p>Neben diesen beiden Sensorfusionen gibt es noch weitere Sensoren
|
||||
zum Vergleich:</p>
|
||||
<ul>
|
||||
<li>Improved Orientation Sensor 1 (Sensorfusion des Android
|
||||
Rotation Vector und des kalibrierten Gyroskops - weniger stabil,
|
||||
dafür genauer)</li>
|
||||
<li>Improved Orientation Sensor 2 (Sensorfusion des Android
|
||||
Rotation Vector und des kalibrierten Gyroskops - stabiler, dafür
|
||||
ungenauer)</li>
|
||||
<li>Android Rotation Vector (Kalmanfilterfusion von Akzelerometer
|
||||
+ Gyroskop + Kompass) - die bisher beste verfügbare Fusion!</li>
|
||||
<li>Kalibriertes Gyroskop (Weiteres Ergebnis der
|
||||
Kalmanfilterfusion von Akzelerometer + Gyroskop + Kompass). Liefert
|
||||
nur relative Rotation, kann daher von den anderen Sensoren abweichen.</li>
|
||||
<li>Gravitation + Kompass</li>
|
||||
<li>Akzelerometer + Kompass</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
Die Anwendung wurde entwickelt um die im Rahmer der <a
|
||||
href="http://my-it.at/index.php/publikationen">Masterarbeit</a> "Sensor
|
||||
fusion for robust outdoor Augmented Reality tracking on mobile devices"
|
||||
<a href="http://my-it.at/media/MasterThesis-Pacha.pdf">(download)</a>
|
||||
entwickelte Sensorfusion zu demonstrieren. Die App wurde am <a
|
||||
href="http://www.hitlabnz.org">Human Interface Technology
|
||||
Laboratory New Zealand</a> entwickelt.
|
||||
</p>
|
||||
|
||||
<p>Lange auf den Würfel gedrückt halten, um kurzfristig in den Raummodus zu wechseln.</p>
|
||||
|
||||
<p>
|
||||
Der Quellcode ist öffentlich verfügbar auf <a
|
||||
href="https://bitbucket.org/apacha/sensor-fusion-demo/">Bitbucket</a>
|
||||
unter der MIT Lizenz.
|
||||
</p>
|
||||
</body>
|
||||
</html>
|
||||
52
app/src/main/assets/about/en/index.html
Normal file
52
app/src/main/assets/about/en/index.html
Normal file
@@ -0,0 +1,52 @@
|
||||
<html>
|
||||
<body link="#ff8800" vlink="#ff8800" alink="#ff8800">
|
||||
|
||||
<p>This application was developed by <a href="http://my-it.at">Alexander Pacha</a> at the <a href="http://www.hitlabnz.org">Human Interface
|
||||
Technology Laboratory New Zealand</a> to demonstrate the
|
||||
capabilities of various sensors and sensor fusion approaches. Data
|
||||
from the Gyroscope, Accelerometer and compass are combined in
|
||||
different ways and the result is shown as a cube that can be rotated
|
||||
by rotating the device.</p>
|
||||
<p>
|
||||
The major novelty in this application is the fusion of virtual
|
||||
sensors: <b>Improved Orientation Sensor 1</b> and <b>Improved
|
||||
Orientation Sensor 2</b> fuse the Android Rotation Vector with the
|
||||
virtual Gyroscope sensor to achieve a pose estimation with a
|
||||
previously unknown stability and precision.
|
||||
</p>
|
||||
|
||||
<p>Apart from these two sensors, the following sensors are
|
||||
available for comparison:</p>
|
||||
<ul>
|
||||
<li>Improved Orientation Sensor 1 (Sensor fusion of Android
|
||||
Rotation Vector and Calibrated Gyroscope - less stable but more
|
||||
accurate)</li>
|
||||
<li>Improved Orientation Sensor 2 (Sensor fusion of Android
|
||||
Rotation Vector and Calibrated Gyroscope - more stable but less
|
||||
accurate)</li>
|
||||
<li>Android Rotation Vector (Kalman filter fusion of
|
||||
Accelerometer + Gyroscope + Compass)</li>
|
||||
<li>Calibrated Gyroscope (Separate result of Kalman filter fusion
|
||||
of Accelerometer + Gyroscope + Compass)</li>
|
||||
<li>Gravity + Compass</li>
|
||||
<li>Accelerometer + Compass</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
This application was developed for demonstrating the sensor fusion
|
||||
approach developed for my <a
|
||||
href="http://my-it.at/index.php/publikationen">Master thesis</a> "Sensor fusion for robust outdoor Augmented Reality tracking on mobile
|
||||
devices"
|
||||
<a href="http://my-it.at/media/MasterThesis-Pacha.pdf">(download)</a> at the <a href="http://www.hitlabnz.org">Human Interface
|
||||
Technology Laboratory New Zealand</a>
|
||||
</p>
|
||||
|
||||
<p>Long-click on a cube to temporarily change into the space-mode for this fusion.</p>
|
||||
|
||||
<p>
|
||||
The source-code is publicly available at <a
|
||||
href="https://bitbucket.org/apacha/sensor-fusion-demo/">Bitbucket</a>
|
||||
and licensed under the MIT license.
|
||||
</p>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,43 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import java.util.Locale;
|
||||
|
||||
import android.app.Activity;
|
||||
import android.os.Bundle;
|
||||
import android.view.MenuItem;
|
||||
import android.webkit.WebView;
|
||||
|
||||
/**
|
||||
* Activity, that displays a single WebView with the text shown under the section About in the settings
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class AboutActivity extends Activity {
|
||||
|
||||
@Override
|
||||
protected void onCreate(Bundle savedInstanceState) {
|
||||
super.onCreate(savedInstanceState);
|
||||
setContentView(R.layout.activity_about);
|
||||
|
||||
// Get the locale substring to access the localised assets
|
||||
String localPrefix = Locale.getDefault().getLanguage().substring(0, 2).toLowerCase(Locale.US);
|
||||
|
||||
// Load the website as the only action for this activity
|
||||
WebView webView = (WebView) findViewById(R.id.webViewAbout);
|
||||
webView.loadUrl("file:///android_asset/about/" + localPrefix + "/index.html");
|
||||
|
||||
// Enable the logo in the top left corner to bring the user back to another activity.
|
||||
getActionBar().setDisplayHomeAsUpEnabled(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean onOptionsItemSelected(MenuItem item) {
|
||||
switch (item.getItemId()) {
|
||||
case android.R.id.home:
|
||||
finish();
|
||||
return true;
|
||||
}
|
||||
return super.onOptionsItemSelected(item);
|
||||
}
|
||||
}
|
||||
69
app/src/main/java/org/hitlabnz/sensor_fusion_demo/Cube.java
Normal file
69
app/src/main/java/org/hitlabnz/sensor_fusion_demo/Cube.java
Normal file
@@ -0,0 +1,69 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import java.nio.ByteOrder;
|
||||
import java.nio.FloatBuffer;
|
||||
|
||||
import javax.microedition.khronos.opengles.GL10;
|
||||
|
||||
/**
|
||||
* A simple colour-cube that is used for drawing the current rotation of the device
|
||||
*
|
||||
*/
|
||||
public class Cube {
|
||||
/**
|
||||
* Buffer for the vertices
|
||||
*/
|
||||
private FloatBuffer mVertexBuffer;
|
||||
/**
|
||||
* Buffer for the colours
|
||||
*/
|
||||
private FloatBuffer mColorBuffer;
|
||||
/**
|
||||
* Buffer for indices
|
||||
*/
|
||||
private ByteBuffer mIndexBuffer;
|
||||
|
||||
/**
|
||||
* Initialises a new instance of the cube
|
||||
*/
|
||||
public Cube() {
|
||||
final float vertices[] = { -1, -1, -1, 1, -1, -1, 1, 1, -1, -1, 1, -1, -1, -1, 1, 1, -1, 1, 1, 1, 1, -1, 1, 1, };
|
||||
|
||||
final float colors[] = { 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0,
|
||||
1, 1, 1, };
|
||||
|
||||
final byte indices[] = { 0, 4, 5, 0, 5, 1, 1, 5, 6, 1, 6, 2, 2, 6, 7, 2, 7, 3, 3, 7, 4, 3, 4, 0, 4, 7, 6, 4, 6,
|
||||
5, 3, 0, 1, 3, 1, 2 };
|
||||
|
||||
ByteBuffer vbb = ByteBuffer.allocateDirect(vertices.length * 4);
|
||||
vbb.order(ByteOrder.nativeOrder());
|
||||
mVertexBuffer = vbb.asFloatBuffer();
|
||||
mVertexBuffer.put(vertices);
|
||||
mVertexBuffer.position(0);
|
||||
|
||||
ByteBuffer cbb = ByteBuffer.allocateDirect(colors.length * 4);
|
||||
cbb.order(ByteOrder.nativeOrder());
|
||||
mColorBuffer = cbb.asFloatBuffer();
|
||||
mColorBuffer.put(colors);
|
||||
mColorBuffer.position(0);
|
||||
|
||||
mIndexBuffer = ByteBuffer.allocateDirect(indices.length);
|
||||
mIndexBuffer.put(indices);
|
||||
mIndexBuffer.position(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Draws this cube of the given GL-Surface
|
||||
*
|
||||
* @param gl The GL-Surface this cube should be drawn upon.
|
||||
*/
|
||||
public void draw(GL10 gl) {
|
||||
gl.glEnable(GL10.GL_CULL_FACE);
|
||||
gl.glFrontFace(GL10.GL_CW);
|
||||
gl.glShadeModel(GL10.GL_SMOOTH);
|
||||
gl.glVertexPointer(3, GL10.GL_FLOAT, 0, mVertexBuffer);
|
||||
gl.glColorPointer(4, GL10.GL_FLOAT, 0, mColorBuffer);
|
||||
gl.glDrawElements(GL10.GL_TRIANGLES, 36, GL10.GL_UNSIGNED_BYTE, mIndexBuffer);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import javax.microedition.khronos.egl.EGLConfig;
|
||||
import javax.microedition.khronos.opengles.GL10;
|
||||
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.OrientationProvider;
|
||||
import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;
|
||||
|
||||
import android.opengl.GLSurfaceView;
|
||||
|
||||
/**
|
||||
* Class that implements the rendering of a cube with the current rotation of the device that is provided by a
|
||||
* OrientationProvider
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class CubeRenderer implements GLSurfaceView.Renderer {
|
||||
/**
|
||||
* The colour-cube that is drawn repeatedly
|
||||
*/
|
||||
private Cube mCube;
|
||||
|
||||
/**
|
||||
* The current provider of the device orientation.
|
||||
*/
|
||||
private OrientationProvider orientationProvider = null;
|
||||
|
||||
/**
|
||||
* Initialises a new CubeRenderer
|
||||
*/
|
||||
public CubeRenderer() {
|
||||
mCube = new Cube();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the orientationProvider of this renderer. Use this method to change which sensor fusion should be currently
|
||||
* used for rendering the cube. Simply exchange it with another orientationProvider and the cube will be rendered
|
||||
* with another approach.
|
||||
*
|
||||
* @param orientationProvider The new orientation provider that delivers the current orientation of the device
|
||||
*/
|
||||
public void setOrientationProvider(OrientationProvider orientationProvider) {
|
||||
this.orientationProvider = orientationProvider;
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform the actual rendering of the cube for each frame
|
||||
*
|
||||
* @param gl The surface on which the cube should be rendered
|
||||
*/
|
||||
public void onDrawFrame(GL10 gl) {
|
||||
// clear screen
|
||||
gl.glClear(GL10.GL_COLOR_BUFFER_BIT);
|
||||
|
||||
// set-up modelview matrix
|
||||
gl.glMatrixMode(GL10.GL_MODELVIEW);
|
||||
gl.glLoadIdentity();
|
||||
|
||||
if (showCubeInsideOut) {
|
||||
float dist = 3;
|
||||
gl.glTranslatef(0, 0, -dist);
|
||||
|
||||
if (orientationProvider != null) {
|
||||
// All Orientation providers deliver Quaternion as well as rotation matrix.
|
||||
// Use your favourite representation:
|
||||
|
||||
// Get the rotation from the current orientationProvider as rotation matrix
|
||||
//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());
|
||||
}
|
||||
|
||||
// draw our object
|
||||
gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
|
||||
gl.glEnableClientState(GL10.GL_COLOR_ARRAY);
|
||||
|
||||
mCube.draw(gl);
|
||||
} else {
|
||||
|
||||
if (orientationProvider != null) {
|
||||
// All Orientation providers deliver Quaternion as well as rotation matrix.
|
||||
// Use your favourite representation:
|
||||
|
||||
// Get the rotation from the current orientationProvider as rotation matrix
|
||||
//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());
|
||||
}
|
||||
|
||||
float dist = 3;
|
||||
drawTranslatedCube(gl, 0, 0, -dist);
|
||||
drawTranslatedCube(gl, 0, 0, dist);
|
||||
drawTranslatedCube(gl, 0, -dist, 0);
|
||||
drawTranslatedCube(gl, 0, dist, 0);
|
||||
drawTranslatedCube(gl, -dist, 0, 0);
|
||||
drawTranslatedCube(gl, dist, 0, 0);
|
||||
}
|
||||
|
||||
// draw our object
|
||||
gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
|
||||
gl.glEnableClientState(GL10.GL_COLOR_ARRAY);
|
||||
|
||||
mCube.draw(gl);
|
||||
}
|
||||
|
||||
/**
|
||||
* Draws a translated cube
|
||||
*
|
||||
* @param gl the surface
|
||||
* @param translateX x-translation
|
||||
* @param translateY y-translation
|
||||
* @param translateZ z-translation
|
||||
*/
|
||||
private void drawTranslatedCube(GL10 gl, float translateX, float translateY, float translateZ) {
|
||||
gl.glPushMatrix();
|
||||
gl.glTranslatef(translateX, translateY, translateZ);
|
||||
|
||||
// draw our object
|
||||
gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
|
||||
gl.glEnableClientState(GL10.GL_COLOR_ARRAY);
|
||||
|
||||
mCube.draw(gl);
|
||||
gl.glPopMatrix();
|
||||
}
|
||||
|
||||
/**
|
||||
* Update view-port with the new surface
|
||||
*
|
||||
* @param gl the surface
|
||||
* @param width new width
|
||||
* @param height new height
|
||||
*/
|
||||
public void onSurfaceChanged(GL10 gl, int width, int height) {
|
||||
// set view-port
|
||||
gl.glViewport(0, 0, width, height);
|
||||
// set projection matrix
|
||||
float ratio = (float) width / height;
|
||||
gl.glMatrixMode(GL10.GL_PROJECTION);
|
||||
gl.glLoadIdentity();
|
||||
gl.glFrustumf(-ratio, ratio, -1, 1, 1, 10);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSurfaceCreated(GL10 gl, EGLConfig config) {
|
||||
// dither is enabled by default, we don't need it
|
||||
gl.glDisable(GL10.GL_DITHER);
|
||||
// clear screen in black
|
||||
gl.glClearColor(0, 0, 0, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Flag indicating whether you want to view inside out, or outside in
|
||||
*/
|
||||
private boolean showCubeInsideOut = true;
|
||||
|
||||
/**
|
||||
* Toggles whether the cube will be shown inside-out or outside in.
|
||||
*/
|
||||
public void toggleShowCubeInsideOut() {
|
||||
this.showCubeInsideOut = !showCubeInsideOut;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* Class that tests availability of hardware sensors.
|
||||
*
|
||||
* @author Alex
|
||||
*
|
||||
*/
|
||||
public class HardwareChecker implements SensorChecker {
|
||||
|
||||
boolean gyroscopeIsAvailable = false;
|
||||
|
||||
public HardwareChecker (SensorManager sensorManager) {
|
||||
if(sensorManager.getSensorList(Sensor.TYPE_GYROSCOPE).size() > 0) {
|
||||
gyroscopeIsAvailable = true;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean IsGyroscopeAvailable() {
|
||||
return gyroscopeIsAvailable;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.AccelerometerCompassProvider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.CalibratedGyroscopeProvider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.GravityCompassProvider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.ImprovedOrientationSensor1Provider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.ImprovedOrientationSensor2Provider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.OrientationProvider;
|
||||
import org.hitlabnz.sensor_fusion_demo.orientationProvider.RotationVectorProvider;
|
||||
|
||||
import android.hardware.SensorManager;
|
||||
import android.opengl.GLSurfaceView;
|
||||
import android.os.Bundle;
|
||||
import android.support.v4.app.Fragment;
|
||||
import android.view.LayoutInflater;
|
||||
import android.view.View;
|
||||
import android.view.ViewGroup;
|
||||
import android.view.View.OnLongClickListener;
|
||||
|
||||
/**
|
||||
* A fragment that contains the same visualisation for different orientation providers
|
||||
*/
|
||||
public class OrientationVisualisationFragment extends Fragment {
|
||||
/**
|
||||
* The surface that will be drawn upon
|
||||
*/
|
||||
private GLSurfaceView mGLSurfaceView;
|
||||
/**
|
||||
* The class that renders the cube
|
||||
*/
|
||||
private CubeRenderer mRenderer;
|
||||
/**
|
||||
* The current orientation provider that delivers device orientation.
|
||||
*/
|
||||
private OrientationProvider currentOrientationProvider;
|
||||
|
||||
/**
|
||||
* The fragment argument representing the section number for this
|
||||
* fragment.
|
||||
*/
|
||||
public static final String ARG_SECTION_NUMBER = "section_number";
|
||||
|
||||
@Override
|
||||
public void onResume() {
|
||||
// Ideally a game should implement onResume() and onPause()
|
||||
// to take appropriate action when the activity looses focus
|
||||
super.onResume();
|
||||
currentOrientationProvider.start();
|
||||
mGLSurfaceView.onResume();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onPause() {
|
||||
// Ideally a game should implement onResume() and onPause()
|
||||
// to take appropriate action when the activity looses focus
|
||||
super.onPause();
|
||||
currentOrientationProvider.stop();
|
||||
mGLSurfaceView.onPause();
|
||||
}
|
||||
|
||||
@Override
|
||||
public View onCreateView(LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) {
|
||||
// Initialise the orientationProvider
|
||||
switch (getArguments().getInt(ARG_SECTION_NUMBER)) {
|
||||
case 1:
|
||||
currentOrientationProvider = new ImprovedOrientationSensor1Provider((SensorManager) getActivity()
|
||||
.getSystemService(SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
case 2:
|
||||
currentOrientationProvider = new ImprovedOrientationSensor2Provider((SensorManager) getActivity()
|
||||
.getSystemService(SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
case 3:
|
||||
currentOrientationProvider = new RotationVectorProvider((SensorManager) getActivity().getSystemService(
|
||||
SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
case 4:
|
||||
currentOrientationProvider = new CalibratedGyroscopeProvider((SensorManager) getActivity()
|
||||
.getSystemService(SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
case 5:
|
||||
currentOrientationProvider = new GravityCompassProvider((SensorManager) getActivity().getSystemService(
|
||||
SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
case 6:
|
||||
currentOrientationProvider = new AccelerometerCompassProvider((SensorManager) getActivity()
|
||||
.getSystemService(SensorSelectionActivity.SENSOR_SERVICE));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// Create our Preview view and set it as the content of our Activity
|
||||
mRenderer = new CubeRenderer();
|
||||
mRenderer.setOrientationProvider(currentOrientationProvider);
|
||||
mGLSurfaceView = new GLSurfaceView(getActivity());
|
||||
mGLSurfaceView.setRenderer(mRenderer);
|
||||
|
||||
mGLSurfaceView.setOnLongClickListener(new OnLongClickListener() {
|
||||
|
||||
@Override
|
||||
public boolean onLongClick(View v) {
|
||||
mRenderer.toggleShowCubeInsideOut();
|
||||
return true;
|
||||
}
|
||||
});
|
||||
|
||||
return mGLSurfaceView;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
public interface SensorChecker {
|
||||
|
||||
/**
|
||||
* Checks if the device that is currently running the application has a hardware gyroscope built into it.
|
||||
*
|
||||
* @return True, if a gyroscope is available. False otherwise.
|
||||
*/
|
||||
public boolean IsGyroscopeAvailable();
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
package org.hitlabnz.sensor_fusion_demo;
|
||||
|
||||
import java.util.Locale;
|
||||
|
||||
import android.app.AlertDialog;
|
||||
import android.content.DialogInterface;
|
||||
import android.content.Intent;
|
||||
import android.hardware.SensorManager;
|
||||
import android.os.Bundle;
|
||||
import android.support.v4.app.Fragment;
|
||||
import android.support.v4.app.FragmentActivity;
|
||||
import android.support.v4.app.FragmentManager;
|
||||
import android.support.v4.app.FragmentPagerAdapter;
|
||||
import android.support.v4.view.ViewPager;
|
||||
import android.view.Menu;
|
||||
import android.view.MenuItem;
|
||||
|
||||
/**
|
||||
* The main activity where the user can select which sensor-fusion he wants to try out
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class SensorSelectionActivity extends FragmentActivity {
|
||||
|
||||
/**
|
||||
* The {@link android.support.v4.view.PagerAdapter} that will provide
|
||||
* fragments for each of the sections. We use a {@link android.support.v4.app.FragmentPagerAdapter} derivative,
|
||||
* which
|
||||
* will keep every loaded fragment in memory. If this becomes too memory
|
||||
* intensive, it may be best to switch to a {@link android.support.v4.app.FragmentStatePagerAdapter}.
|
||||
*/
|
||||
SectionsPagerAdapter mSectionsPagerAdapter;
|
||||
|
||||
/**
|
||||
* The {@link ViewPager} that will host the section contents.
|
||||
*/
|
||||
ViewPager mViewPager;
|
||||
|
||||
@Override
|
||||
protected void onCreate(Bundle savedInstanceState) {
|
||||
super.onCreate(savedInstanceState);
|
||||
setContentView(R.layout.activity_sensor_selection);
|
||||
|
||||
// Create the adapter that will return a fragment for each of the three
|
||||
// primary sections of the app.
|
||||
mSectionsPagerAdapter = new SectionsPagerAdapter(getSupportFragmentManager());
|
||||
|
||||
// Set up the ViewPager with the sections adapter.
|
||||
mViewPager = (ViewPager) findViewById(R.id.pager);
|
||||
mViewPager.setAdapter(mSectionsPagerAdapter);
|
||||
|
||||
// Check if device has a hardware gyroscope
|
||||
SensorChecker checker = new HardwareChecker((SensorManager) getSystemService(SENSOR_SERVICE));
|
||||
if(!checker.IsGyroscopeAvailable()) {
|
||||
// If a gyroscope is unavailable, display a warning.
|
||||
displayHardwareMissingWarning();
|
||||
}
|
||||
}
|
||||
|
||||
private void displayHardwareMissingWarning() {
|
||||
AlertDialog ad = new AlertDialog.Builder(this).create();
|
||||
ad.setCancelable(false); // This blocks the 'BACK' button
|
||||
ad.setTitle(getResources().getString(R.string.gyroscope_missing));
|
||||
ad.setMessage(getResources().getString(R.string.gyroscope_missing_message));
|
||||
ad.setButton(DialogInterface.BUTTON_NEUTRAL, getResources().getString(R.string.OK), new DialogInterface.OnClickListener() {
|
||||
@Override
|
||||
public void onClick(DialogInterface dialog, int which) {
|
||||
dialog.dismiss();
|
||||
}
|
||||
});
|
||||
ad.show();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean onCreateOptionsMenu(Menu menu) {
|
||||
// Inflate the menu; this adds items to the action bar if it is present.
|
||||
getMenuInflater().inflate(R.menu.sensor_selection, menu);
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean onOptionsItemSelected(MenuItem item) {
|
||||
// Handle item selection
|
||||
switch (item.getItemId()) {
|
||||
case R.id.action_about:
|
||||
Intent intent = new Intent(this, AboutActivity.class);
|
||||
startActivity(intent);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* A {@link FragmentPagerAdapter} that returns a fragment corresponding to
|
||||
* one of the sections/tabs/pages.
|
||||
*/
|
||||
public class SectionsPagerAdapter extends FragmentPagerAdapter {
|
||||
|
||||
/**
|
||||
* Initialises a new sectionPagerAdapter
|
||||
*
|
||||
* @param fm the fragment Manager
|
||||
*/
|
||||
public SectionsPagerAdapter(FragmentManager fm) {
|
||||
super(fm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Fragment getItem(int position) {
|
||||
// getItem is called to instantiate the fragment for the given page.
|
||||
// Return a DummySectionFragment (defined as a static inner class
|
||||
// below) with the page number as its lone argument.
|
||||
Fragment fragment = new OrientationVisualisationFragment();
|
||||
Bundle args = new Bundle();
|
||||
args.putInt(OrientationVisualisationFragment.ARG_SECTION_NUMBER, position + 1);
|
||||
fragment.setArguments(args);
|
||||
return fragment;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getCount() {
|
||||
// Show 6 total pages.
|
||||
return 6;
|
||||
}
|
||||
|
||||
@Override
|
||||
public CharSequence getPageTitle(int position) {
|
||||
Locale l = Locale.getDefault();
|
||||
switch (position) {
|
||||
case 0:
|
||||
return getString(R.string.title_section1).toUpperCase(l);
|
||||
case 1:
|
||||
return getString(R.string.title_section2).toUpperCase(l);
|
||||
case 2:
|
||||
return getString(R.string.title_section3).toUpperCase(l);
|
||||
case 3:
|
||||
return getString(R.string.title_section4).toUpperCase(l);
|
||||
case 4:
|
||||
return getString(R.string.title_section5).toUpperCase(l);
|
||||
case 5:
|
||||
return getString(R.string.title_section6).toUpperCase(l);
|
||||
}
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the current orientation from the {@link Sensor#TYPE_ACCELEROMETER
|
||||
* Accelerometer} and {@link Sensor#TYPE_MAGNETIC_FIELD Compass}.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class AccelerometerCompassProvider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Compass values
|
||||
*/
|
||||
private float[] magnitudeValues = new float[3];
|
||||
|
||||
/**
|
||||
* Accelerometer values
|
||||
*/
|
||||
private float[] accelerometerValues = new float[3];
|
||||
|
||||
/**
|
||||
* Initialises a new AccelerometerCompassProvider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public AccelerometerCompassProvider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//Add the compass and the accelerometer
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER));
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
|
||||
// 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();
|
||||
} else if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
|
||||
accelerometerValues = event.values.clone();
|
||||
}
|
||||
|
||||
if (magnitudeValues != null && accelerometerValues != null) {
|
||||
float[] i = new float[16];
|
||||
|
||||
// Fuse accelerometer with compass
|
||||
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, accelerometerValues,
|
||||
magnitudeValues);
|
||||
// Transform rotation matrix to quaternion
|
||||
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the relative orientation from the {@link Sensor#TYPE_GYROSCOPE
|
||||
* Gyroscope}. This sensor does not deliver an absolute orientation (with respect to magnetic north and gravity) but
|
||||
* only a relative measurement starting from the point where it started.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class CalibratedGyroscopeProvider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Constant specifying the factor between a Nano-second and a second
|
||||
*/
|
||||
private static final float NS2S = 1.0f / 1000000000.0f;
|
||||
|
||||
/**
|
||||
* The quaternion that stores the difference that is obtained by the gyroscope.
|
||||
* Basically it contains a rotational difference encoded into a quaternion.
|
||||
*
|
||||
* To obtain the absolute orientation one must add this into an initial position by
|
||||
* multiplying it with another quaternion
|
||||
*/
|
||||
private final Quaternion deltaQuaternion = new Quaternion();
|
||||
|
||||
/**
|
||||
* The time-stamp being used to record the time when the last gyroscope event occurred.
|
||||
*/
|
||||
private long timestamp;
|
||||
|
||||
/**
|
||||
* This is a filter-threshold for discarding Gyroscope measurements that are below a certain level and
|
||||
* potentially are only noise and not real motion. Values from the gyroscope are usually between 0 (stop) and
|
||||
* 10 (rapid rotation), so 0.1 seems to be a reasonable threshold to filter noise (usually smaller than 0.1) and
|
||||
* real motion (usually > 0.1). Note that there is a chance of missing real motion, if the use is turning the
|
||||
* device really slowly, so this value has to find a balance between accepting noise (threshold = 0) and missing
|
||||
* slow user-action (threshold > 0.5). 0.1 seems to work fine for most applications.
|
||||
*
|
||||
*/
|
||||
private static final double EPSILON = 0.1f;
|
||||
|
||||
/**
|
||||
* Value giving the total velocity of the gyroscope (will be high, when the device is moving fast and low when
|
||||
* the device is standing still). This is usually a value between 0 and 10 for normal motion. Heavy shaking can
|
||||
* increase it to about 25. Keep in mind, that these values are time-depended, so changing the sampling rate of
|
||||
* the sensor will affect this value!
|
||||
*/
|
||||
private double gyroscopeRotationVelocity = 0;
|
||||
|
||||
/**
|
||||
* Initialises a new CalibratedGyroscopeProvider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public CalibratedGyroscopeProvider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//Add the gyroscope
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
|
||||
// we received a sensor event. it is a good practice to check
|
||||
// that we received the proper event
|
||||
if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
|
||||
|
||||
// This timestep's delta rotation to be multiplied by the current rotation
|
||||
// after computing it from the gyro sample data.
|
||||
if (timestamp != 0) {
|
||||
final float dT = (event.timestamp - timestamp) * NS2S;
|
||||
// Axis of the rotation sample, not normalized yet.
|
||||
float axisX = event.values[0];
|
||||
float axisY = event.values[1];
|
||||
float axisZ = event.values[2];
|
||||
|
||||
// Calculate the angular speed of the sample
|
||||
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
|
||||
|
||||
// Normalize the rotation vector if it's big enough to get the axis
|
||||
if (gyroscopeRotationVelocity > EPSILON) {
|
||||
axisX /= gyroscopeRotationVelocity;
|
||||
axisY /= gyroscopeRotationVelocity;
|
||||
axisZ /= gyroscopeRotationVelocity;
|
||||
}
|
||||
|
||||
// Integrate around this axis with the angular speed by the timestep
|
||||
// in order to get a delta rotation from this sample over the timestep
|
||||
// We will convert this axis-angle representation of the delta rotation
|
||||
// into a quaternion before turning it into the rotation matrix.
|
||||
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
|
||||
double sinThetaOverTwo = Math.sin(thetaOverTwo);
|
||||
double cosThetaOverTwo = Math.cos(thetaOverTwo);
|
||||
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
|
||||
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
|
||||
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
|
||||
deltaQuaternion.setW(-(float) cosThetaOverTwo);
|
||||
|
||||
// Matrix rendering in CubeRenderer does not seem to have this problem.
|
||||
synchronized (syncToken) {
|
||||
// Move current gyro orientation if gyroscope should be used
|
||||
deltaQuaternion.multiplyByQuat(currentOrientationQuaternion, currentOrientationQuaternion);
|
||||
}
|
||||
|
||||
Quaternion correctedQuat = currentOrientationQuaternion.clone();
|
||||
// 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());
|
||||
|
||||
synchronized (syncToken) {
|
||||
// Set the rotation matrix as well to have both representations
|
||||
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix,
|
||||
correctedQuat.ToArray());
|
||||
}
|
||||
}
|
||||
timestamp = event.timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the current orientation from the {@link Sensor#TYPE_GRAVITY
|
||||
* Gravity} and {@link Sensor#TYPE_MAGNETIC_FIELD Compass}.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class GravityCompassProvider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Compass values
|
||||
*/
|
||||
private float[] magnitudeValues = new float[3];
|
||||
|
||||
/**
|
||||
* Gravity values
|
||||
*/
|
||||
private float[] gravityValues = new float[3];
|
||||
|
||||
/**
|
||||
* Initialises a new GravityCompassProvider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public GravityCompassProvider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//Add the compass and the gravity sensor
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY));
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
|
||||
// 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();
|
||||
} else if (event.sensor.getType() == Sensor.TYPE_GRAVITY) {
|
||||
gravityValues = event.values.clone();
|
||||
}
|
||||
|
||||
if (magnitudeValues != null && gravityValues != null) {
|
||||
float[] i = new float[16];
|
||||
|
||||
// Fuse gravity-sensor (virtual sensor) with compass
|
||||
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, gravityValues, magnitudeValues);
|
||||
// Transform rotation matrix to quaternion
|
||||
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,273 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
import android.util.Log;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the absolute orientation from the {@link Sensor#TYPE_GYROSCOPE
|
||||
* Gyroscope} and {@link Sensor#TYPE_ROTATION_VECTOR Android Rotation Vector sensor}.
|
||||
*
|
||||
* It mainly relies on the gyroscope, but corrects with the Android Rotation Vector which also provides an absolute
|
||||
* estimation of current orientation. The correction is a static weight.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class ImprovedOrientationSensor1Provider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Constant specifying the factor between a Nano-second and a second
|
||||
*/
|
||||
private static final float NS2S = 1.0f / 1000000000.0f;
|
||||
|
||||
/**
|
||||
* The quaternion that stores the difference that is obtained by the gyroscope.
|
||||
* Basically it contains a rotational difference encoded into a quaternion.
|
||||
*
|
||||
* To obtain the absolute orientation one must add this into an initial position by
|
||||
* multiplying it with another quaternion
|
||||
*/
|
||||
private final Quaternion deltaQuaternion = new Quaternion();
|
||||
|
||||
/**
|
||||
* The Quaternions that contain the current rotation (Angle and axis in Quaternion format) of the Gyroscope
|
||||
*/
|
||||
private Quaternion quaternionGyroscope = new Quaternion();
|
||||
|
||||
/**
|
||||
* The quaternion that contains the absolute orientation as obtained by the rotationVector sensor.
|
||||
*/
|
||||
private Quaternion quaternionRotationVector = new Quaternion();
|
||||
|
||||
/**
|
||||
* The time-stamp being used to record the time when the last gyroscope event occurred.
|
||||
*/
|
||||
private long timestamp;
|
||||
|
||||
/**
|
||||
* This is a filter-threshold for discarding Gyroscope measurements that are below a certain level and
|
||||
* potentially are only noise and not real motion. Values from the gyroscope are usually between 0 (stop) and
|
||||
* 10 (rapid rotation), so 0.1 seems to be a reasonable threshold to filter noise (usually smaller than 0.1) and
|
||||
* real motion (usually > 0.1). Note that there is a chance of missing real motion, if the use is turning the
|
||||
* device really slowly, so this value has to find a balance between accepting noise (threshold = 0) and missing
|
||||
* slow user-action (threshold > 0.5). 0.1 seems to work fine for most applications.
|
||||
*
|
||||
*/
|
||||
private static final double EPSILON = 0.1f;
|
||||
|
||||
/**
|
||||
* Value giving the total velocity of the gyroscope (will be high, when the device is moving fast and low when
|
||||
* the device is standing still). This is usually a value between 0 and 10 for normal motion. Heavy shaking can
|
||||
* increase it to about 25. Keep in mind, that these values are time-depended, so changing the sampling rate of
|
||||
* the sensor will affect this value!
|
||||
*/
|
||||
private double gyroscopeRotationVelocity = 0;
|
||||
|
||||
/**
|
||||
* Flag indicating, whether the orientations were initialised from the rotation vector or not. If false, the
|
||||
* gyroscope can not be used (since it's only meaningful to calculate differences from an initial state). If
|
||||
* true,
|
||||
* the gyroscope can be used normally.
|
||||
*/
|
||||
private boolean positionInitialised = false;
|
||||
|
||||
/**
|
||||
* Counter that sums the number of consecutive frames, where the rotationVector and the gyroscope were
|
||||
* significantly different (and the dot-product was smaller than 0.7). This event can either happen when the
|
||||
* angles of the rotation vector explode (e.g. during fast tilting) or when the device was shaken heavily and
|
||||
* the gyroscope is now completely off.
|
||||
*/
|
||||
private int panicCounter;
|
||||
|
||||
/**
|
||||
* This weight determines directly how much the rotation sensor will be used to correct (in
|
||||
* Sensor-fusion-scenario 1 - SensorSelection.GyroscopeAndRotationVector). Must be a value between 0 and 1.
|
||||
* 0 means that the system entirely relies on the gyroscope, whereas 1 means that the system relies entirely on
|
||||
* the rotationVector.
|
||||
*/
|
||||
private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates an outlier of the rotation vector. If the dot-product between the two vectors
|
||||
* (gyroscope orientation and rotationVector orientation) falls below this threshold (ideally it should be 1,
|
||||
* if they are exactly the same) the system falls back to the gyroscope values only and just ignores the
|
||||
* rotation vector.
|
||||
*
|
||||
* This value should be quite high (> 0.7) to filter even the slightest discrepancies that causes jumps when
|
||||
* tiling the device. Possible values are between 0 and 1, where a value close to 1 means that even a very small
|
||||
* difference between the two sensors will be treated as outlier, whereas a value close to zero means that the
|
||||
* almost any discrepancy between the two sensors is tolerated.
|
||||
*/
|
||||
private static final float OUTLIER_THRESHOLD = 0.85f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates a massive discrepancy between the rotation vector and the gyroscope orientation.
|
||||
* If the dot-product between the two vectors
|
||||
* (gyroscope orientation and rotationVector orientation) falls below this threshold (ideally it should be 1, if
|
||||
* they are exactly the same), the system will start increasing the panic counter (that probably indicates a
|
||||
* gyroscope failure).
|
||||
*
|
||||
* This value should be lower than OUTLIER_THRESHOLD (0.5 - 0.7) to only start increasing the panic counter,
|
||||
* when there is a
|
||||
* huge discrepancy between the two fused sensors.
|
||||
*/
|
||||
private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates that a chaos state has been established rather than just a temporary peak in the
|
||||
* rotation vector (caused by exploding angled during fast tilting).
|
||||
*
|
||||
* If the chaosCounter is bigger than this threshold, the current position will be reset to whatever the
|
||||
* rotation vector indicates.
|
||||
*/
|
||||
private static final int PANIC_THRESHOLD = 60;
|
||||
|
||||
/**
|
||||
* Initialises a new ImprovedOrientationSensor1Provider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public ImprovedOrientationSensor1Provider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//Add the gyroscope and rotation Vector
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE));
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
|
||||
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);
|
||||
|
||||
// Store in quaternion
|
||||
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
|
||||
if (!positionInitialised) {
|
||||
// Override
|
||||
quaternionGyroscope.set(quaternionRotationVector);
|
||||
positionInitialised = true;
|
||||
}
|
||||
|
||||
} else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
|
||||
// Process Gyroscope and perform fusion
|
||||
|
||||
// This timestep's delta rotation to be multiplied by the current rotation
|
||||
// after computing it from the gyro sample data.
|
||||
if (timestamp != 0) {
|
||||
final float dT = (event.timestamp - timestamp) * NS2S;
|
||||
// Axis of the rotation sample, not normalized yet.
|
||||
float axisX = event.values[0];
|
||||
float axisY = event.values[1];
|
||||
float axisZ = event.values[2];
|
||||
|
||||
// Calculate the angular speed of the sample
|
||||
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
|
||||
|
||||
// Normalize the rotation vector if it's big enough to get the axis
|
||||
if (gyroscopeRotationVelocity > EPSILON) {
|
||||
axisX /= gyroscopeRotationVelocity;
|
||||
axisY /= gyroscopeRotationVelocity;
|
||||
axisZ /= gyroscopeRotationVelocity;
|
||||
}
|
||||
|
||||
// Integrate around this axis with the angular speed by the timestep
|
||||
// in order to get a delta rotation from this sample over the timestep
|
||||
// We will convert this axis-angle representation of the delta rotation
|
||||
// into a quaternion before turning it into the rotation matrix.
|
||||
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
|
||||
double sinThetaOverTwo = Math.sin(thetaOverTwo);
|
||||
double cosThetaOverTwo = Math.cos(thetaOverTwo);
|
||||
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
|
||||
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
|
||||
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
|
||||
deltaQuaternion.setW(-(float) cosThetaOverTwo);
|
||||
|
||||
// Move current gyro orientation
|
||||
deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);
|
||||
|
||||
// Calculate dot-product to calculate whether the two orientation sensors have diverged
|
||||
// (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
|
||||
float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);
|
||||
|
||||
// If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
|
||||
if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
|
||||
// Increase panic counter
|
||||
if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
|
||||
panicCounter++;
|
||||
}
|
||||
|
||||
// Directly use Gyro
|
||||
setOrientationQuaternionAndMatrix(quaternionGyroscope);
|
||||
|
||||
} else {
|
||||
// Both are nearly saying the same. Perform normal fusion.
|
||||
|
||||
// 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);
|
||||
|
||||
// Use the interpolated value between gyro and rotationVector
|
||||
setOrientationQuaternionAndMatrix(interpolate);
|
||||
// Override current gyroscope-orientation
|
||||
quaternionGyroscope.copyVec4(interpolate);
|
||||
|
||||
// Reset the panic counter because both sensors are saying the same again
|
||||
panicCounter = 0;
|
||||
}
|
||||
|
||||
if (panicCounter > PANIC_THRESHOLD) {
|
||||
Log.d("Rotation Vector",
|
||||
"Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");
|
||||
|
||||
if (gyroscopeRotationVelocity < 3) {
|
||||
Log.d("Rotation Vector",
|
||||
"Performing Panic-reset. Resetting orientation to rotation-vector value.");
|
||||
|
||||
// Manually set position to whatever rotation vector says.
|
||||
setOrientationQuaternionAndMatrix(quaternionRotationVector);
|
||||
// Override current gyroscope-orientation with corrected value
|
||||
quaternionGyroscope.copyVec4(quaternionRotationVector);
|
||||
|
||||
panicCounter = 0;
|
||||
} else {
|
||||
Log.d("Rotation Vector",
|
||||
String.format(
|
||||
"Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
|
||||
gyroscopeRotationVelocity));
|
||||
}
|
||||
}
|
||||
}
|
||||
timestamp = event.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the output quaternion and matrix with the provided quaternion and synchronises the setting
|
||||
*
|
||||
* @param quaternion The Quaternion to set (the result of the sensor fusion)
|
||||
*/
|
||||
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
|
||||
Quaternion correctedQuat = quaternion.clone();
|
||||
// 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());
|
||||
|
||||
synchronized (syncToken) {
|
||||
// Use gyro only
|
||||
currentOrientationQuaternion.copyVec4(quaternion);
|
||||
|
||||
// Set the rotation matrix as well to have both representations
|
||||
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,274 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import org.hitlabnz.sensor_fusion_demo.representation.Quaternion;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
import android.util.Log;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the absolute orientation from the {@link Sensor#TYPE_GYROSCOPE
|
||||
* Gyroscope} and {@link Sensor#TYPE_ROTATION_VECTOR Android Rotation Vector sensor}.
|
||||
*
|
||||
* It mainly relies on the gyroscope, but corrects with the Android Rotation Vector which also provides an absolute
|
||||
* estimation of current orientation. The correction is a static weight.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class ImprovedOrientationSensor2Provider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Constant specifying the factor between a Nano-second and a second
|
||||
*/
|
||||
private static final float NS2S = 1.0f / 1000000000.0f;
|
||||
|
||||
/**
|
||||
* The quaternion that stores the difference that is obtained by the gyroscope.
|
||||
* Basically it contains a rotational difference encoded into a quaternion.
|
||||
*
|
||||
* To obtain the absolute orientation one must add this into an initial position by
|
||||
* multiplying it with another quaternion
|
||||
*/
|
||||
private final Quaternion deltaQuaternion = new Quaternion();
|
||||
|
||||
/**
|
||||
* The Quaternions that contain the current rotation (Angle and axis in Quaternion format) of the Gyroscope
|
||||
*/
|
||||
private Quaternion quaternionGyroscope = new Quaternion();
|
||||
|
||||
/**
|
||||
* The quaternion that contains the absolute orientation as obtained by the rotationVector sensor.
|
||||
*/
|
||||
private Quaternion quaternionRotationVector = new Quaternion();
|
||||
|
||||
/**
|
||||
* The time-stamp being used to record the time when the last gyroscope event occurred.
|
||||
*/
|
||||
private long timestamp;
|
||||
|
||||
/**
|
||||
* This is a filter-threshold for discarding Gyroscope measurements that are below a certain level and
|
||||
* potentially are only noise and not real motion. Values from the gyroscope are usually between 0 (stop) and
|
||||
* 10 (rapid rotation), so 0.1 seems to be a reasonable threshold to filter noise (usually smaller than 0.1) and
|
||||
* real motion (usually > 0.1). Note that there is a chance of missing real motion, if the use is turning the
|
||||
* device really slowly, so this value has to find a balance between accepting noise (threshold = 0) and missing
|
||||
* slow user-action (threshold > 0.5). 0.1 seems to work fine for most applications.
|
||||
*
|
||||
*/
|
||||
private static final double EPSILON = 0.1f;
|
||||
|
||||
/**
|
||||
* Value giving the total velocity of the gyroscope (will be high, when the device is moving fast and low when
|
||||
* the device is standing still). This is usually a value between 0 and 10 for normal motion. Heavy shaking can
|
||||
* increase it to about 25. Keep in mind, that these values are time-depended, so changing the sampling rate of
|
||||
* the sensor will affect this value!
|
||||
*/
|
||||
private double gyroscopeRotationVelocity = 0;
|
||||
|
||||
/**
|
||||
* Flag indicating, whether the orientations were initialised from the rotation vector or not. If false, the
|
||||
* gyroscope can not be used (since it's only meaningful to calculate differences from an initial state). If
|
||||
* true,
|
||||
* the gyroscope can be used normally.
|
||||
*/
|
||||
private boolean positionInitialised = false;
|
||||
|
||||
/**
|
||||
* Counter that sums the number of consecutive frames, where the rotationVector and the gyroscope were
|
||||
* significantly different (and the dot-product was smaller than 0.7). This event can either happen when the
|
||||
* angles of the rotation vector explode (e.g. during fast tilting) or when the device was shaken heavily and
|
||||
* the gyroscope is now completely off.
|
||||
*/
|
||||
private int panicCounter;
|
||||
|
||||
/**
|
||||
* This weight determines indirectly how much the rotation sensor will be used to correct. This weight will be
|
||||
* multiplied by the velocity to obtain the actual weight. (in sensor-fusion-scenario 2 -
|
||||
* SensorSelection.GyroscopeAndRotationVector2).
|
||||
* Must be a value between 0 and approx. 0.04 (because, if multiplied with a velocity of up to 25, should be still
|
||||
* less than 1, otherwise the SLERP will not correctly interpolate). Should be close to zero.
|
||||
*/
|
||||
private static final float INDIRECT_INTERPOLATION_WEIGHT = 0.01f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates an outlier of the rotation vector. If the dot-product between the two vectors
|
||||
* (gyroscope orientation and rotationVector orientation) falls below this threshold (ideally it should be 1,
|
||||
* if they are exactly the same) the system falls back to the gyroscope values only and just ignores the
|
||||
* rotation vector.
|
||||
*
|
||||
* This value should be quite high (> 0.7) to filter even the slightest discrepancies that causes jumps when
|
||||
* tiling the device. Possible values are between 0 and 1, where a value close to 1 means that even a very small
|
||||
* difference between the two sensors will be treated as outlier, whereas a value close to zero means that the
|
||||
* almost any discrepancy between the two sensors is tolerated.
|
||||
*/
|
||||
private static final float OUTLIER_THRESHOLD = 0.85f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates a massive discrepancy between the rotation vector and the gyroscope orientation.
|
||||
* If the dot-product between the two vectors
|
||||
* (gyroscope orientation and rotationVector orientation) falls below this threshold (ideally it should be 1, if
|
||||
* they are exactly the same), the system will start increasing the panic counter (that probably indicates a
|
||||
* gyroscope failure).
|
||||
*
|
||||
* This value should be lower than OUTLIER_THRESHOLD (0.5 - 0.7) to only start increasing the panic counter,
|
||||
* when there is a huge discrepancy between the two fused sensors.
|
||||
*/
|
||||
private static final float OUTLIER_PANIC_THRESHOLD = 0.75f;
|
||||
|
||||
/**
|
||||
* The threshold that indicates that a chaos state has been established rather than just a temporary peak in the
|
||||
* rotation vector (caused by exploding angled during fast tilting).
|
||||
*
|
||||
* If the chaosCounter is bigger than this threshold, the current position will be reset to whatever the
|
||||
* rotation vector indicates.
|
||||
*/
|
||||
private static final int PANIC_THRESHOLD = 60;
|
||||
|
||||
/**
|
||||
* Initialises a new ImprovedOrientationSensor2Provider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public ImprovedOrientationSensor2Provider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//Add the gyroscope and rotation Vector
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE));
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
|
||||
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);
|
||||
|
||||
// Store in quaternion
|
||||
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
|
||||
if (!positionInitialised) {
|
||||
// Override
|
||||
quaternionGyroscope.set(quaternionRotationVector);
|
||||
positionInitialised = true;
|
||||
}
|
||||
|
||||
} else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
|
||||
// Process Gyroscope and perform fusion
|
||||
|
||||
// This timestep's delta rotation to be multiplied by the current rotation
|
||||
// after computing it from the gyro sample data.
|
||||
if (timestamp != 0) {
|
||||
final float dT = (event.timestamp - timestamp) * NS2S;
|
||||
// Axis of the rotation sample, not normalized yet.
|
||||
float axisX = event.values[0];
|
||||
float axisY = event.values[1];
|
||||
float axisZ = event.values[2];
|
||||
|
||||
// Calculate the angular speed of the sample
|
||||
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
|
||||
|
||||
// Normalize the rotation vector if it's big enough to get the axis
|
||||
if (gyroscopeRotationVelocity > EPSILON) {
|
||||
axisX /= gyroscopeRotationVelocity;
|
||||
axisY /= gyroscopeRotationVelocity;
|
||||
axisZ /= gyroscopeRotationVelocity;
|
||||
}
|
||||
|
||||
// Integrate around this axis with the angular speed by the timestep
|
||||
// in order to get a delta rotation from this sample over the timestep
|
||||
// We will convert this axis-angle representation of the delta rotation
|
||||
// into a quaternion before turning it into the rotation matrix.
|
||||
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
|
||||
double sinThetaOverTwo = Math.sin(thetaOverTwo);
|
||||
double cosThetaOverTwo = Math.cos(thetaOverTwo);
|
||||
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
|
||||
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
|
||||
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
|
||||
deltaQuaternion.setW(-(float) cosThetaOverTwo);
|
||||
|
||||
// Move current gyro orientation
|
||||
deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);
|
||||
|
||||
// Calculate dot-product to calculate whether the two orientation sensors have diverged
|
||||
// (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
|
||||
float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);
|
||||
|
||||
// If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
|
||||
if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
|
||||
// Increase panic counter
|
||||
if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
|
||||
panicCounter++;
|
||||
}
|
||||
|
||||
// Directly use Gyro
|
||||
setOrientationQuaternionAndMatrix(quaternionGyroscope);
|
||||
|
||||
} else {
|
||||
// Both are nearly saying the same. Perform normal fusion.
|
||||
|
||||
// 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,
|
||||
(float) (INDIRECT_INTERPOLATION_WEIGHT * gyroscopeRotationVelocity));
|
||||
|
||||
// Use the interpolated value between gyro and rotationVector
|
||||
setOrientationQuaternionAndMatrix(interpolate);
|
||||
// Override current gyroscope-orientation
|
||||
quaternionGyroscope.copyVec4(interpolate);
|
||||
|
||||
// Reset the panic counter because both sensors are saying the same again
|
||||
panicCounter = 0;
|
||||
}
|
||||
|
||||
if (panicCounter > PANIC_THRESHOLD) {
|
||||
Log.d("Rotation Vector",
|
||||
"Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");
|
||||
|
||||
if (gyroscopeRotationVelocity < 3) {
|
||||
Log.d("Rotation Vector",
|
||||
"Performing Panic-reset. Resetting orientation to rotation-vector value.");
|
||||
|
||||
// Manually set position to whatever rotation vector says.
|
||||
setOrientationQuaternionAndMatrix(quaternionRotationVector);
|
||||
// Override current gyroscope-orientation with corrected value
|
||||
quaternionGyroscope.copyVec4(quaternionRotationVector);
|
||||
|
||||
panicCounter = 0;
|
||||
} else {
|
||||
Log.d("Rotation Vector",
|
||||
String.format(
|
||||
"Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
|
||||
gyroscopeRotationVelocity));
|
||||
}
|
||||
}
|
||||
}
|
||||
timestamp = event.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the output quaternion and matrix with the provided quaternion and synchronises the setting
|
||||
*
|
||||
* @param quaternion The Quaternion to set (the result of the sensor fusion)
|
||||
*/
|
||||
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
|
||||
Quaternion correctedQuat = quaternion.clone();
|
||||
// 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());
|
||||
|
||||
synchronized (syncToken) {
|
||||
// Use gyro only
|
||||
currentOrientationQuaternion.copyVec4(quaternion);
|
||||
|
||||
// Set the rotation matrix as well to have both representations
|
||||
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,130 @@
|
||||
/**
|
||||
*
|
||||
*/
|
||||
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.Quaternion;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEventListener;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* Classes implementing this interface provide an orientation of the device
|
||||
* either by directly accessing hardware, using Android sensor fusion or fusing
|
||||
* sensors itself.
|
||||
*
|
||||
* The orientation can be provided as rotation matrix or quaternion.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public abstract class OrientationProvider implements SensorEventListener {
|
||||
/**
|
||||
* Sync-token for syncing read/write to sensor-data from sensor manager and
|
||||
* fusion algorithm
|
||||
*/
|
||||
protected final Object syncToken = new Object();
|
||||
|
||||
/**
|
||||
* The list of sensors used by this provider
|
||||
*/
|
||||
protected List<Sensor> sensorList = new ArrayList<Sensor>();
|
||||
|
||||
/**
|
||||
* The matrix that holds the current rotation
|
||||
*/
|
||||
protected final Matrixf4x4 currentOrientationRotationMatrix;
|
||||
|
||||
/**
|
||||
* The quaternion that holds the current rotation
|
||||
*/
|
||||
protected final Quaternion currentOrientationQuaternion;
|
||||
|
||||
/**
|
||||
* The sensor manager for accessing android sensors
|
||||
*/
|
||||
protected SensorManager sensorManager;
|
||||
|
||||
/**
|
||||
* Initialises a new OrientationProvider
|
||||
*
|
||||
* @param sensorManager
|
||||
* The android sensor manager
|
||||
*/
|
||||
public OrientationProvider(SensorManager sensorManager) {
|
||||
this.sensorManager = sensorManager;
|
||||
|
||||
// Initialise with identity
|
||||
currentOrientationRotationMatrix = new Matrixf4x4();
|
||||
|
||||
// Initialise with identity
|
||||
currentOrientationQuaternion = new Quaternion();
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the sensor fusion (e.g. when resuming the activity)
|
||||
*/
|
||||
public void start() {
|
||||
// enable our sensor when the activity is resumed, ask for
|
||||
// 10 ms updates.
|
||||
for (Sensor sensor : sensorList) {
|
||||
// enable our sensors when the activity is resumed, ask for
|
||||
// 20 ms updates (Sensor_delay_game)
|
||||
sensorManager.registerListener(this, sensor,
|
||||
SensorManager.SENSOR_DELAY_GAME);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Stops the sensor fusion (e.g. when pausing/suspending the activity)
|
||||
*/
|
||||
public void stop() {
|
||||
// make sure to turn our sensors off when the activity is paused
|
||||
for (Sensor sensor : sensorList) {
|
||||
sensorManager.unregisterListener(this, sensor);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onAccuracyChanged(Sensor sensor, int accuracy) {
|
||||
// Not doing anything
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Returns the current rotation of the device in the rotation matrix
|
||||
* format (4x4 matrix)
|
||||
*/
|
||||
public Matrixf4x4 getRotationMatrix() {
|
||||
synchronized (syncToken) {
|
||||
return currentOrientationRotationMatrix;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Returns the current rotation of the device in the quaternion
|
||||
* format (vector4f)
|
||||
*/
|
||||
public Quaternion getQuaternion() {
|
||||
synchronized (syncToken) {
|
||||
return currentOrientationQuaternion.clone();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Returns the current rotation of the device in the Euler-Angles
|
||||
*/
|
||||
public EulerAngles getEulerAngles() {
|
||||
synchronized (syncToken) {
|
||||
|
||||
float[] angles = new float[3];
|
||||
SensorManager.getOrientation(currentOrientationRotationMatrix.matrix, angles);
|
||||
return new EulerAngles(angles[0], angles[1], angles[2]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.orientationProvider;
|
||||
|
||||
import android.hardware.Sensor;
|
||||
import android.hardware.SensorEvent;
|
||||
import android.hardware.SensorManager;
|
||||
|
||||
/**
|
||||
* The orientation provider that delivers the current orientation from the {@link Sensor#TYPE_ROTATION_VECTOR Android
|
||||
* Rotation Vector sensor}.
|
||||
*
|
||||
* @author Alexander Pacha
|
||||
*
|
||||
*/
|
||||
public class RotationVectorProvider extends OrientationProvider {
|
||||
|
||||
/**
|
||||
* Initialises a new RotationVectorProvider
|
||||
*
|
||||
* @param sensorManager The android sensor manager
|
||||
*/
|
||||
public RotationVectorProvider(SensorManager sensorManager) {
|
||||
super(sensorManager);
|
||||
|
||||
//The rotation vector sensor that is being used for this provider to get device orientation
|
||||
sensorList.add(sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSensorChanged(SensorEvent event) {
|
||||
// we received a sensor event. it is a good practice to check
|
||||
// that we received the proper event
|
||||
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
|
||||
// convert the rotation-vector to a 4x4 matrix. the matrix
|
||||
// is interpreted by Open GL as the inverse of the
|
||||
// rotation-vector, which is what we want.
|
||||
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]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,824 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.representation;
|
||||
|
||||
/**
|
||||
* Matrix math utilities. These methods operate on OpenGL ES format
|
||||
* matrices and vectors stored in float arrays.
|
||||
*
|
||||
* Matrices are 4 x 4 column-vector matrices stored in column-major
|
||||
* order:
|
||||
*
|
||||
* <pre>
|
||||
* m[offset + 0] m[offset + 4] m[offset + 8] m[offset + 12]
|
||||
* m[offset + 1] m[offset + 5] m[offset + 9] m[offset + 13]
|
||||
* m[offset + 2] m[offset + 6] m[offset + 10] m[offset + 14]
|
||||
* m[offset + 3] m[offset + 7] m[offset + 11] m[offset + 15]
|
||||
* </pre>
|
||||
*
|
||||
* Vectors are 4 row x 1 column column-vectors stored in order:
|
||||
*
|
||||
* <pre>
|
||||
* v[offset + 0]
|
||||
* v[offset + 1]
|
||||
* v[offset + 2]
|
||||
* v[offset + 3]
|
||||
* </pre>
|
||||
*
|
||||
*/
|
||||
public class Matrix {
|
||||
|
||||
/**
|
||||
* Temporary memory for operations that need temporary matrix data.
|
||||
*/
|
||||
private static final float[] TEMP_MATRIX_ARRAY = new float[32];
|
||||
|
||||
/**
|
||||
* Multiply two 4x4 matrices together and store the result in a third 4x4
|
||||
* matrix. In matrix notation: result = lhs x rhs. Due to the way
|
||||
* matrix multiplication works, the result matrix will have the same
|
||||
* effect as first multiplying by the rhs matrix, then multiplying by
|
||||
* the lhs matrix. This is the opposite of what you might expect.
|
||||
*
|
||||
* The same float array may be passed for result, lhs, and/or rhs. However,
|
||||
* the result element values are undefined if the result elements overlap
|
||||
* either the lhs or rhs elements.
|
||||
*
|
||||
* @param result The float array that holds the result.
|
||||
* @param resultOffset The offset into the result array where the result is
|
||||
* stored.
|
||||
* @param lhs The float array that holds the left-hand-side matrix.
|
||||
* @param lhsOffset The offset into the lhs array where the lhs is stored
|
||||
* @param rhs The float array that holds the right-hand-side matrix.
|
||||
* @param rhsOffset The offset into the rhs array where the rhs is stored.
|
||||
*
|
||||
* @throws IllegalArgumentException if result, lhs, or rhs are null, or if
|
||||
* resultOffset + 16 > result.length or lhsOffset + 16 > lhs.length or
|
||||
* rhsOffset + 16 > rhs.length.
|
||||
*/
|
||||
/**
|
||||
* public static void multiplyMM(float[] result, int resultOffset,
|
||||
* float[] lhs, int lhsOffset, float[] rhs, int rhsOffset){
|
||||
* android.opengl.Matrix.multiplyMM(result, resultOffset, lhs, lhsOffset, rhs, rhsOffset);
|
||||
* }
|
||||
*/
|
||||
|
||||
public static void multiplyMM(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs,
|
||||
int rhsOffset) {
|
||||
//for(int i = 0; i < 4; i++){
|
||||
// for(int j = 0; j < 4; j++){
|
||||
|
||||
// int k = i * 4;
|
||||
// output[outputOffset + 0 + j] += lhs[lhsOffset + k + j] * rhs[rhsOffset + 0 * 4 + i];
|
||||
// output[outputOffset + 1 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 1 * 4 + i];
|
||||
// output[outputOffset + 2 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 2 * 4 + i];
|
||||
// output[outputOffset + 3 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 3 * 4 + i];
|
||||
// }
|
||||
//}
|
||||
output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3];
|
||||
|
||||
output[outputOffset + 4] = lhs[lhsOffset + 0] * rhs[rhsOffset + 4] + lhs[lhsOffset + 4] * rhs[rhsOffset + 5]
|
||||
+ lhs[lhsOffset + 8] * rhs[rhsOffset + 6] + lhs[lhsOffset + 12] * rhs[rhsOffset + 7];
|
||||
output[outputOffset + 5] = lhs[lhsOffset + 1] * rhs[rhsOffset + 4] + lhs[lhsOffset + 5] * rhs[rhsOffset + 5]
|
||||
+ lhs[lhsOffset + 9] * rhs[rhsOffset + 6] + lhs[lhsOffset + 13] * rhs[rhsOffset + 7];
|
||||
output[outputOffset + 6] = lhs[lhsOffset + 2] * rhs[rhsOffset + 4] + lhs[lhsOffset + 6] * rhs[rhsOffset + 5]
|
||||
+ lhs[lhsOffset + 10] * rhs[rhsOffset + 6] + lhs[lhsOffset + 14] * rhs[rhsOffset + 7];
|
||||
output[outputOffset + 7] = lhs[lhsOffset + 3] * rhs[rhsOffset + 4] + lhs[lhsOffset + 7] * rhs[rhsOffset + 5]
|
||||
+ lhs[lhsOffset + 11] * rhs[rhsOffset + 6] + lhs[lhsOffset + 15] * rhs[rhsOffset + 7];
|
||||
|
||||
output[outputOffset + 8] = lhs[lhsOffset + 0] * rhs[rhsOffset + 8] + lhs[lhsOffset + 4] * rhs[rhsOffset + 9]
|
||||
+ lhs[lhsOffset + 8] * rhs[rhsOffset + 10] + lhs[lhsOffset + 12] * rhs[rhsOffset + 11];
|
||||
output[outputOffset + 9] = lhs[lhsOffset + 1] * rhs[rhsOffset + 8] + lhs[lhsOffset + 5] * rhs[rhsOffset + 9]
|
||||
+ lhs[lhsOffset + 9] * rhs[rhsOffset + 10] + lhs[lhsOffset + 13] * rhs[rhsOffset + 11];
|
||||
output[outputOffset + 10] = lhs[lhsOffset + 2] * rhs[rhsOffset + 8] + lhs[lhsOffset + 6] * rhs[rhsOffset + 9]
|
||||
+ lhs[lhsOffset + 10] * rhs[rhsOffset + 10] + lhs[lhsOffset + 14] * rhs[rhsOffset + 11];
|
||||
output[outputOffset + 11] = lhs[lhsOffset + 3] * rhs[rhsOffset + 8] + lhs[lhsOffset + 7] * rhs[rhsOffset + 9]
|
||||
+ lhs[lhsOffset + 11] * rhs[rhsOffset + 10] + lhs[lhsOffset + 15] * rhs[rhsOffset + 11];
|
||||
|
||||
output[outputOffset + 12] = lhs[lhsOffset + 0] * rhs[rhsOffset + 12] + lhs[lhsOffset + 4] * rhs[rhsOffset + 13]
|
||||
+ lhs[lhsOffset + 8] * rhs[rhsOffset + 14] + lhs[lhsOffset + 12] * rhs[rhsOffset + 15];
|
||||
output[outputOffset + 13] = lhs[lhsOffset + 1] * rhs[rhsOffset + 12] + lhs[lhsOffset + 5] * rhs[rhsOffset + 13]
|
||||
+ lhs[lhsOffset + 9] * rhs[rhsOffset + 14] + lhs[lhsOffset + 13] * rhs[rhsOffset + 15];
|
||||
output[outputOffset + 14] = lhs[lhsOffset + 2] * rhs[rhsOffset + 12] + lhs[lhsOffset + 6] * rhs[rhsOffset + 13]
|
||||
+ lhs[lhsOffset + 10] * rhs[rhsOffset + 14] + lhs[lhsOffset + 14] * rhs[rhsOffset + 15];
|
||||
output[outputOffset + 15] = lhs[lhsOffset + 3] * rhs[rhsOffset + 12] + lhs[lhsOffset + 7] * rhs[rhsOffset + 13]
|
||||
+ lhs[lhsOffset + 11] * rhs[rhsOffset + 14] + lhs[lhsOffset + 15] * rhs[rhsOffset + 15];
|
||||
}
|
||||
|
||||
public static void multiplyMM(float[] output, float[] lhs, float[] rhs) {
|
||||
output[0] = lhs[0] * rhs[0] + lhs[4] * rhs[1] + lhs[8] * rhs[2] + lhs[12] * rhs[3];
|
||||
output[1] = lhs[1] * rhs[0] + lhs[5] * rhs[1] + lhs[9] * rhs[2] + lhs[13] * rhs[3];
|
||||
output[2] = lhs[2] * rhs[0] + lhs[6] * rhs[1] + lhs[10] * rhs[2] + lhs[14] * rhs[3];
|
||||
output[3] = lhs[3] * rhs[0] + lhs[7] * rhs[1] + lhs[11] * rhs[2] + lhs[15] * rhs[3];
|
||||
|
||||
output[4] = lhs[0] * rhs[4] + lhs[4] * rhs[5] + lhs[8] * rhs[6] + lhs[12] * rhs[7];
|
||||
output[5] = lhs[1] * rhs[4] + lhs[5] * rhs[5] + lhs[9] * rhs[6] + lhs[13] * rhs[7];
|
||||
output[6] = lhs[2] * rhs[4] + lhs[6] * rhs[5] + lhs[10] * rhs[6] + lhs[14] * rhs[7];
|
||||
output[7] = lhs[3] * rhs[4] + lhs[7] * rhs[5] + lhs[11] * rhs[6] + lhs[15] * rhs[7];
|
||||
|
||||
output[8] = lhs[0] * rhs[8] + lhs[4] * rhs[9] + lhs[8] * rhs[10] + lhs[12] * rhs[11];
|
||||
output[9] = lhs[1] * rhs[8] + lhs[5] * rhs[9] + lhs[9] * rhs[10] + lhs[13] * rhs[11];
|
||||
output[10] = lhs[2] * rhs[8] + lhs[6] * rhs[9] + lhs[10] * rhs[10] + lhs[14] * rhs[11];
|
||||
output[11] = lhs[3] * rhs[8] + lhs[7] * rhs[9] + lhs[11] * rhs[10] + lhs[15] * rhs[11];
|
||||
|
||||
output[12] = lhs[0] * rhs[12] + lhs[4] * rhs[13] + lhs[8] * rhs[14] + lhs[12] * rhs[15];
|
||||
output[13] = lhs[1] * rhs[12] + lhs[5] * rhs[13] + lhs[9] * rhs[14] + lhs[13] * rhs[15];
|
||||
output[14] = lhs[2] * rhs[12] + lhs[6] * rhs[13] + lhs[10] * rhs[14] + lhs[14] * rhs[15];
|
||||
output[15] = lhs[3] * rhs[12] + lhs[7] * rhs[13] + lhs[11] * rhs[14] + lhs[15] * rhs[15];
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply a 4 element vector by a 4x4 matrix and store the result in a 4
|
||||
* element column vector. In matrix notation: result = lhs x rhs
|
||||
*
|
||||
* The same float array may be passed for resultVec, lhsMat, and/or rhsVec.
|
||||
* However, the resultVec element values are undefined if the resultVec
|
||||
* elements overlap either the lhsMat or rhsVec elements.
|
||||
*
|
||||
* @param resultVec The float array that holds the result vector.
|
||||
* @param resultVecOffset The offset into the result array where the result
|
||||
* vector is stored.
|
||||
* @param lhsMat The float array that holds the left-hand-side matrix.
|
||||
* @param lhsMatOffset The offset into the lhs array where the lhs is stored
|
||||
* @param rhsVec The float array that holds the right-hand-side vector.
|
||||
* @param rhsVecOffset The offset into the rhs vector where the rhs vector
|
||||
* is stored.
|
||||
*
|
||||
* @throws IllegalArgumentException if resultVec, lhsMat,
|
||||
* or rhsVec are null, or if resultVecOffset + 4 > resultVec.length
|
||||
* or lhsMatOffset + 16 > lhsMat.length or
|
||||
* rhsVecOffset + 4 > rhsVec.length.
|
||||
*/
|
||||
/* public static void multiplyMV(float[] resultVec,
|
||||
* int resultVecOffset, float[] lhsMat, int lhsMatOffset,
|
||||
* float[] rhsVec, int rhsVecOffset){
|
||||
* android.opengl.Matrix.multiplyMV(resultVec, resultVecOffset, lhsMat, lhsMatOffset, rhsVec, rhsVecOffset);
|
||||
* } */
|
||||
public static void multiplyMV(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs,
|
||||
int rhsOffset) {
|
||||
/* wrong implementation (this is for row major matrices)
|
||||
* output[outputOffset +0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 1] * rhs[rhsOffset + 1]
|
||||
* + lhs[lhsOffset + 2] * rhs[rhsOffset + 2] + lhs[lhsOffset + 3] * rhs[rhsOffset + 3];
|
||||
* output[outputOffset +1] = lhs[lhsOffset + 4] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] +
|
||||
* lhs[lhsOffset + 6] * rhs[rhsOffset + 2] + lhs[lhsOffset + 7] * rhs[rhsOffset + 3];
|
||||
* output[outputOffset +2] = lhs[lhsOffset + 8] * rhs[rhsOffset + 0] + lhs[lhsOffset + 9] * rhs[rhsOffset + 1] +
|
||||
* lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 11] * rhs[rhsOffset + 3];
|
||||
* output[outputOffset +3] = lhs[lhsOffset + 12] * rhs[rhsOffset + 0] + lhs[lhsOffset + 13] * rhs[rhsOffset + 1]
|
||||
* + lhs[lhsOffset + 14] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; */
|
||||
// correct implementation for column major matrices (which is for OpenGL)
|
||||
output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3];
|
||||
output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1]
|
||||
+ lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3];
|
||||
|
||||
}
|
||||
|
||||
public static void multiplyMV(float[] outputV, float[] inputM, float[] inputV) {
|
||||
outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * inputV[3];
|
||||
outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * inputV[3];
|
||||
outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * inputV[3];
|
||||
outputV[3] = inputM[3] * inputV[0] + inputM[7] * inputV[1] + inputM[11] * inputV[2] + inputM[15] * inputV[3];
|
||||
}
|
||||
|
||||
public static void multiplyMV3(float[] outputV, float[] inputM, float[] inputV, float w) {
|
||||
outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * w;
|
||||
outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * w;
|
||||
outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * w;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transposes a 4 x 4 matrix.
|
||||
*
|
||||
* @param mTrans the array that holds the output inverted matrix
|
||||
* @param mTransOffset an offset into mInv where the inverted matrix is
|
||||
* stored.
|
||||
* @param m the input array
|
||||
* @param mOffset an offset into m where the matrix is stored.
|
||||
*/
|
||||
public static void transposeM(float[] mTrans, int mTransOffset, float[] m, int mOffset) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int mBase = i * 4 + mOffset;
|
||||
mTrans[i + mTransOffset] = m[mBase];
|
||||
mTrans[i + 4 + mTransOffset] = m[mBase + 1];
|
||||
mTrans[i + 8 + mTransOffset] = m[mBase + 2];
|
||||
mTrans[i + 12 + mTransOffset] = m[mBase + 3];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Inverts a 4 x 4 matrix.
|
||||
*
|
||||
* @param mInv the array that holds the output inverted matrix
|
||||
* @param mInvOffset an offset into mInv where the inverted matrix is
|
||||
* stored.
|
||||
* @param m the input array
|
||||
* @param mOffset an offset into m where the matrix is stored.
|
||||
* @return true if the matrix could be inverted, false if it could not.
|
||||
*/
|
||||
public static boolean invertM(float[] mInv, int mInvOffset, float[] m, int mOffset) {
|
||||
// Invert a 4 x 4 matrix using Cramer's Rule
|
||||
|
||||
// transpose matrix
|
||||
final float src0 = m[mOffset + 0];
|
||||
final float src4 = m[mOffset + 1];
|
||||
final float src8 = m[mOffset + 2];
|
||||
final float src12 = m[mOffset + 3];
|
||||
|
||||
final float src1 = m[mOffset + 4];
|
||||
final float src5 = m[mOffset + 5];
|
||||
final float src9 = m[mOffset + 6];
|
||||
final float src13 = m[mOffset + 7];
|
||||
|
||||
final float src2 = m[mOffset + 8];
|
||||
final float src6 = m[mOffset + 9];
|
||||
final float src10 = m[mOffset + 10];
|
||||
final float src14 = m[mOffset + 11];
|
||||
|
||||
final float src3 = m[mOffset + 12];
|
||||
final float src7 = m[mOffset + 13];
|
||||
final float src11 = m[mOffset + 14];
|
||||
final float src15 = m[mOffset + 15];
|
||||
|
||||
// calculate pairs for first 8 elements (cofactors)
|
||||
final float atmp0 = src10 * src15;
|
||||
final float atmp1 = src11 * src14;
|
||||
final float atmp2 = src9 * src15;
|
||||
final float atmp3 = src11 * src13;
|
||||
final float atmp4 = src9 * src14;
|
||||
final float atmp5 = src10 * src13;
|
||||
final float atmp6 = src8 * src15;
|
||||
final float atmp7 = src11 * src12;
|
||||
final float atmp8 = src8 * src14;
|
||||
final float atmp9 = src10 * src12;
|
||||
final float atmp10 = src8 * src13;
|
||||
final float atmp11 = src9 * src12;
|
||||
|
||||
// calculate first 8 elements (cofactors)
|
||||
final float dst0 = (atmp0 * src5 + atmp3 * src6 + atmp4 * src7) - (atmp1 * src5 + atmp2 * src6 + atmp5 * src7);
|
||||
final float dst1 = (atmp1 * src4 + atmp6 * src6 + atmp9 * src7) - (atmp0 * src4 + atmp7 * src6 + atmp8 * src7);
|
||||
final float dst2 = (atmp2 * src4 + atmp7 * src5 + atmp10 * src7)
|
||||
- (atmp3 * src4 + atmp6 * src5 + atmp11 * src7);
|
||||
final float dst3 = (atmp5 * src4 + atmp8 * src5 + atmp11 * src6)
|
||||
- (atmp4 * src4 + atmp9 * src5 + atmp10 * src6);
|
||||
final float dst4 = (atmp1 * src1 + atmp2 * src2 + atmp5 * src3) - (atmp0 * src1 + atmp3 * src2 + atmp4 * src3);
|
||||
final float dst5 = (atmp0 * src0 + atmp7 * src2 + atmp8 * src3) - (atmp1 * src0 + atmp6 * src2 + atmp9 * src3);
|
||||
final float dst6 = (atmp3 * src0 + atmp6 * src1 + atmp11 * src3)
|
||||
- (atmp2 * src0 + atmp7 * src1 + atmp10 * src3);
|
||||
final float dst7 = (atmp4 * src0 + atmp9 * src1 + atmp10 * src2)
|
||||
- (atmp5 * src0 + atmp8 * src1 + atmp11 * src2);
|
||||
|
||||
// calculate pairs for second 8 elements (cofactors)
|
||||
final float btmp0 = src2 * src7;
|
||||
final float btmp1 = src3 * src6;
|
||||
final float btmp2 = src1 * src7;
|
||||
final float btmp3 = src3 * src5;
|
||||
final float btmp4 = src1 * src6;
|
||||
final float btmp5 = src2 * src5;
|
||||
final float btmp6 = src0 * src7;
|
||||
final float btmp7 = src3 * src4;
|
||||
final float btmp8 = src0 * src6;
|
||||
final float btmp9 = src2 * src4;
|
||||
final float btmp10 = src0 * src5;
|
||||
final float btmp11 = src1 * src4;
|
||||
|
||||
// calculate second 8 elements (cofactors)
|
||||
final float dst8 = (btmp0 * src13 + btmp3 * src14 + btmp4 * src15)
|
||||
- (btmp1 * src13 + btmp2 * src14 + btmp5 * src15);
|
||||
final float dst9 = (btmp1 * src12 + btmp6 * src14 + btmp9 * src15)
|
||||
- (btmp0 * src12 + btmp7 * src14 + btmp8 * src15);
|
||||
final float dst10 = (btmp2 * src12 + btmp7 * src13 + btmp10 * src15)
|
||||
- (btmp3 * src12 + btmp6 * src13 + btmp11 * src15);
|
||||
final float dst11 = (btmp5 * src12 + btmp8 * src13 + btmp11 * src14)
|
||||
- (btmp4 * src12 + btmp9 * src13 + btmp10 * src14);
|
||||
final float dst12 = (btmp2 * src10 + btmp5 * src11 + btmp1 * src9)
|
||||
- (btmp4 * src11 + btmp0 * src9 + btmp3 * src10);
|
||||
final float dst13 = (btmp8 * src11 + btmp0 * src8 + btmp7 * src10)
|
||||
- (btmp6 * src10 + btmp9 * src11 + btmp1 * src8);
|
||||
final float dst14 = (btmp6 * src9 + btmp11 * src11 + btmp3 * src8)
|
||||
- (btmp10 * src11 + btmp2 * src8 + btmp7 * src9);
|
||||
final float dst15 = (btmp10 * src10 + btmp4 * src8 + btmp9 * src9)
|
||||
- (btmp8 * src9 + btmp11 * src10 + btmp5 * src8);
|
||||
|
||||
// calculate determinant
|
||||
final float det = src0 * dst0 + src1 * dst1 + src2 * dst2 + src3 * dst3;
|
||||
|
||||
if (det == 0.0f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate matrix inverse
|
||||
final float invdet = 1.0f / det;
|
||||
mInv[mInvOffset] = dst0 * invdet;
|
||||
mInv[1 + mInvOffset] = dst1 * invdet;
|
||||
mInv[2 + mInvOffset] = dst2 * invdet;
|
||||
mInv[3 + mInvOffset] = dst3 * invdet;
|
||||
|
||||
mInv[4 + mInvOffset] = dst4 * invdet;
|
||||
mInv[5 + mInvOffset] = dst5 * invdet;
|
||||
mInv[6 + mInvOffset] = dst6 * invdet;
|
||||
mInv[7 + mInvOffset] = dst7 * invdet;
|
||||
|
||||
mInv[8 + mInvOffset] = dst8 * invdet;
|
||||
mInv[9 + mInvOffset] = dst9 * invdet;
|
||||
mInv[10 + mInvOffset] = dst10 * invdet;
|
||||
mInv[11 + mInvOffset] = dst11 * invdet;
|
||||
|
||||
mInv[12 + mInvOffset] = dst12 * invdet;
|
||||
mInv[13 + mInvOffset] = dst13 * invdet;
|
||||
mInv[14 + mInvOffset] = dst14 * invdet;
|
||||
mInv[15 + mInvOffset] = dst15 * invdet;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes an orthographic projection matrix.
|
||||
*
|
||||
* @param m returns the result
|
||||
* @param mOffset
|
||||
* @param left
|
||||
* @param right
|
||||
* @param bottom
|
||||
* @param top
|
||||
* @param near
|
||||
* @param far
|
||||
*/
|
||||
|
||||
public static void orthoM(float[] m, int mOffset, float left, float right, float bottom, float top, float near,
|
||||
float far) {
|
||||
if (left == right) {
|
||||
throw new IllegalArgumentException("left == right");
|
||||
}
|
||||
if (bottom == top) {
|
||||
throw new IllegalArgumentException("bottom == top");
|
||||
}
|
||||
if (near == far) {
|
||||
throw new IllegalArgumentException("near == far");
|
||||
}
|
||||
|
||||
final float r_width = 1.0f / (right - left);
|
||||
final float r_height = 1.0f / (top - bottom);
|
||||
final float r_depth = 1.0f / (far - near);
|
||||
final float x = 2.0f * (r_width);
|
||||
final float y = 2.0f * (r_height);
|
||||
final float z = -2.0f * (r_depth);
|
||||
final float tx = -(right + left) * r_width;
|
||||
final float ty = -(top + bottom) * r_height;
|
||||
final float tz = -(far + near) * r_depth;
|
||||
m[mOffset + 0] = x;
|
||||
m[mOffset + 5] = y;
|
||||
m[mOffset + 10] = z;
|
||||
m[mOffset + 12] = tx;
|
||||
m[mOffset + 13] = ty;
|
||||
m[mOffset + 14] = tz;
|
||||
m[mOffset + 15] = 1.0f;
|
||||
m[mOffset + 1] = 0.0f;
|
||||
m[mOffset + 2] = 0.0f;
|
||||
m[mOffset + 3] = 0.0f;
|
||||
m[mOffset + 4] = 0.0f;
|
||||
m[mOffset + 6] = 0.0f;
|
||||
m[mOffset + 7] = 0.0f;
|
||||
m[mOffset + 8] = 0.0f;
|
||||
m[mOffset + 9] = 0.0f;
|
||||
m[mOffset + 11] = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Define a projection matrix in terms of six clip planes
|
||||
*
|
||||
* @param m the float array that holds the perspective matrix
|
||||
* @param offset the offset into float array m where the perspective
|
||||
* matrix data is written
|
||||
* @param left
|
||||
* @param right
|
||||
* @param bottom
|
||||
* @param top
|
||||
* @param near
|
||||
* @param far
|
||||
*/
|
||||
|
||||
public static void frustumM(float[] m, int offset, float left, float right, float bottom, float top, float near,
|
||||
float far) {
|
||||
if (left == right) {
|
||||
throw new IllegalArgumentException("left == right");
|
||||
}
|
||||
if (top == bottom) {
|
||||
throw new IllegalArgumentException("top == bottom");
|
||||
}
|
||||
if (near == far) {
|
||||
throw new IllegalArgumentException("near == far");
|
||||
}
|
||||
if (near <= 0.0f) {
|
||||
throw new IllegalArgumentException("near <= 0.0f");
|
||||
}
|
||||
if (far <= 0.0f) {
|
||||
throw new IllegalArgumentException("far <= 0.0f");
|
||||
}
|
||||
final float r_width = 1.0f / (right - left);
|
||||
final float r_height = 1.0f / (top - bottom);
|
||||
final float r_depth = 1.0f / (near - far);
|
||||
final float x = 2.0f * (near * r_width);
|
||||
final float y = 2.0f * (near * r_height);
|
||||
final float A = 2.0f * ((right + left) * r_width);
|
||||
final float B = (top + bottom) * r_height;
|
||||
final float C = (far + near) * r_depth;
|
||||
final float D = 2.0f * (far * near * r_depth);
|
||||
m[offset + 0] = x;
|
||||
m[offset + 5] = y;
|
||||
m[offset + 8] = A;
|
||||
m[offset + 9] = B;
|
||||
m[offset + 10] = C;
|
||||
m[offset + 14] = D;
|
||||
m[offset + 11] = -1.0f;
|
||||
m[offset + 1] = 0.0f;
|
||||
m[offset + 2] = 0.0f;
|
||||
m[offset + 3] = 0.0f;
|
||||
m[offset + 4] = 0.0f;
|
||||
m[offset + 6] = 0.0f;
|
||||
m[offset + 7] = 0.0f;
|
||||
m[offset + 12] = 0.0f;
|
||||
m[offset + 13] = 0.0f;
|
||||
m[offset + 15] = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Define a projection matrix in terms of a field of view angle, an
|
||||
* aspect ratio, and z clip planes
|
||||
*
|
||||
* @param m the float array that holds the perspective matrix
|
||||
* @param offset the offset into float array m where the perspective
|
||||
* matrix data is written
|
||||
* @param fovy field of view in y direction, in degrees
|
||||
* @param aspect width to height aspect ratio of the viewport
|
||||
* @param zNear
|
||||
* @param zFar
|
||||
*/
|
||||
public static void perspectiveM(float[] m, int offset, float fovy, float aspect, float zNear, float zFar) {
|
||||
float f = 1.0f / (float) Math.tan(fovy * (Math.PI / 360.0));
|
||||
float rangeReciprocal = 1.0f / (zNear - zFar);
|
||||
|
||||
m[offset + 0] = f / aspect;
|
||||
m[offset + 1] = 0.0f;
|
||||
m[offset + 2] = 0.0f;
|
||||
m[offset + 3] = 0.0f;
|
||||
|
||||
m[offset + 4] = 0.0f;
|
||||
m[offset + 5] = f;
|
||||
m[offset + 6] = 0.0f;
|
||||
m[offset + 7] = 0.0f;
|
||||
|
||||
m[offset + 8] = 0.0f;
|
||||
m[offset + 9] = 0.0f;
|
||||
m[offset + 10] = (zFar + zNear) * rangeReciprocal;
|
||||
m[offset + 11] = -1.0f;
|
||||
|
||||
m[offset + 12] = 0.0f;
|
||||
m[offset + 13] = 0.0f;
|
||||
m[offset + 14] = 2.0f * zFar * zNear * rangeReciprocal;
|
||||
m[offset + 15] = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the length of a vector
|
||||
*
|
||||
* @param x x coordinate of a vector
|
||||
* @param y y coordinate of a vector
|
||||
* @param z z coordinate of a vector
|
||||
* @return the length of a vector
|
||||
*/
|
||||
public static float length(float x, float y, float z) {
|
||||
return (float) Math.sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets matrix m to the identity matrix.
|
||||
*
|
||||
* @param sm returns the result
|
||||
* @param smOffset index into sm where the result matrix starts
|
||||
*/
|
||||
public static void setIdentityM(float[] sm, int smOffset) {
|
||||
for (int i = 0; i < 16; i++) {
|
||||
sm[smOffset + i] = 0;
|
||||
}
|
||||
for (int i = 0; i < 16; i += 5) {
|
||||
sm[smOffset + i] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Scales matrix m by x, y, and z, putting the result in sm
|
||||
*
|
||||
* @param sm returns the result
|
||||
* @param smOffset index into sm where the result matrix starts
|
||||
* @param m source matrix
|
||||
* @param mOffset index into m where the source matrix starts
|
||||
* @param x scale factor x
|
||||
* @param y scale factor y
|
||||
* @param z scale factor z
|
||||
*/
|
||||
public static void scaleM(float[] sm, int smOffset, float[] m, int mOffset, float x, float y, float z) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int smi = smOffset + i;
|
||||
int mi = mOffset + i;
|
||||
sm[smi] = m[mi] * x;
|
||||
sm[4 + smi] = m[4 + mi] * y;
|
||||
sm[8 + smi] = m[8 + mi] * z;
|
||||
sm[12 + smi] = m[12 + mi];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Scales matrix m in place by sx, sy, and sz
|
||||
*
|
||||
* @param m matrix to scale
|
||||
* @param mOffset index into m where the matrix starts
|
||||
* @param x scale factor x
|
||||
* @param y scale factor y
|
||||
* @param z scale factor z
|
||||
*/
|
||||
public static void scaleM(float[] m, int mOffset, float x, float y, float z) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int mi = mOffset + i;
|
||||
m[mi] *= x;
|
||||
m[4 + mi] *= y;
|
||||
m[8 + mi] *= z;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Translates matrix m by x, y, and z, putting the result in tm
|
||||
*
|
||||
* @param tm returns the result
|
||||
* @param tmOffset index into sm where the result matrix starts
|
||||
* @param m source matrix
|
||||
* @param mOffset index into m where the source matrix starts
|
||||
* @param x translation factor x
|
||||
* @param y translation factor y
|
||||
* @param z translation factor z
|
||||
*/
|
||||
public static void translateM(float[] tm, int tmOffset, float[] m, int mOffset, float x, float y, float z) {
|
||||
for (int i = 0; i < 12; i++) {
|
||||
tm[tmOffset + i] = m[mOffset + i];
|
||||
}
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int tmi = tmOffset + i;
|
||||
int mi = mOffset + i;
|
||||
tm[12 + tmi] = m[mi] * x + m[4 + mi] * y + m[8 + mi] * z + m[12 + mi];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Translates matrix m by x, y, and z in place.
|
||||
*
|
||||
* @param m matrix
|
||||
* @param mOffset index into m where the matrix starts
|
||||
* @param x translation factor x
|
||||
* @param y translation factor y
|
||||
* @param z translation factor z
|
||||
*/
|
||||
public static void translateM(float[] m, int mOffset, float x, float y, float z) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
int mi = mOffset + i;
|
||||
m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotates matrix m by angle a (in degrees) around the axis (x, y, z)
|
||||
*
|
||||
* @param rm returns the result
|
||||
* @param rmOffset index into rm where the result matrix starts
|
||||
* @param m source matrix
|
||||
* @param mOffset index into m where the source matrix starts
|
||||
* @param a angle to rotate in degrees
|
||||
* @param x scale factor x
|
||||
* @param y scale factor y
|
||||
* @param z scale factor z
|
||||
*/
|
||||
public static void rotateM(float[] rm, int rmOffset, float[] m, int mOffset, float a, float x, float y, float z) {
|
||||
synchronized (TEMP_MATRIX_ARRAY) {
|
||||
setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z);
|
||||
multiplyMM(rm, rmOffset, m, mOffset, TEMP_MATRIX_ARRAY, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotates matrix m in place by angle a (in degrees)
|
||||
* around the axis (x, y, z)
|
||||
*
|
||||
* @param m source matrix
|
||||
* @param mOffset index into m where the matrix starts
|
||||
* @param a angle to rotate in degrees
|
||||
* @param x scale factor x
|
||||
* @param y scale factor y
|
||||
* @param z scale factor z
|
||||
*/
|
||||
public static void rotateM(float[] m, int mOffset, float a, float x, float y, float z) {
|
||||
synchronized (TEMP_MATRIX_ARRAY) {
|
||||
setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z);
|
||||
multiplyMM(TEMP_MATRIX_ARRAY, 16, m, mOffset, TEMP_MATRIX_ARRAY, 0);
|
||||
System.arraycopy(TEMP_MATRIX_ARRAY, 16, m, mOffset, 16);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotates matrix m by angle a (in degrees) around the axis (x, y, z)
|
||||
*
|
||||
* @param rm returns the result
|
||||
* @param rmOffset index into rm where the result matrix starts
|
||||
* @param a angle to rotate in degrees
|
||||
* @param x scale factor x
|
||||
* @param y scale factor y
|
||||
* @param z scale factor z
|
||||
*/
|
||||
public static void setRotateM(float[] rm, int rmOffset, float a, float x, float y, float z) {
|
||||
rm[rmOffset + 3] = 0;
|
||||
rm[rmOffset + 7] = 0;
|
||||
rm[rmOffset + 11] = 0;
|
||||
rm[rmOffset + 12] = 0;
|
||||
rm[rmOffset + 13] = 0;
|
||||
rm[rmOffset + 14] = 0;
|
||||
rm[rmOffset + 15] = 1;
|
||||
a *= (float) (Math.PI / 180.0f);
|
||||
float s = (float) Math.sin(a);
|
||||
float c = (float) Math.cos(a);
|
||||
if (1.0f == x && 0.0f == y && 0.0f == z) {
|
||||
rm[rmOffset + 5] = c;
|
||||
rm[rmOffset + 10] = c;
|
||||
rm[rmOffset + 6] = s;
|
||||
rm[rmOffset + 9] = -s;
|
||||
rm[rmOffset + 1] = 0;
|
||||
rm[rmOffset + 2] = 0;
|
||||
rm[rmOffset + 4] = 0;
|
||||
rm[rmOffset + 8] = 0;
|
||||
rm[rmOffset + 0] = 1;
|
||||
} else if (0.0f == x && 1.0f == y && 0.0f == z) {
|
||||
rm[rmOffset + 0] = c;
|
||||
rm[rmOffset + 10] = c;
|
||||
rm[rmOffset + 8] = s;
|
||||
rm[rmOffset + 2] = -s;
|
||||
rm[rmOffset + 1] = 0;
|
||||
rm[rmOffset + 4] = 0;
|
||||
rm[rmOffset + 6] = 0;
|
||||
rm[rmOffset + 9] = 0;
|
||||
rm[rmOffset + 5] = 1;
|
||||
} else if (0.0f == x && 0.0f == y && 1.0f == z) {
|
||||
rm[rmOffset + 0] = c;
|
||||
rm[rmOffset + 5] = c;
|
||||
rm[rmOffset + 1] = s;
|
||||
rm[rmOffset + 4] = -s;
|
||||
rm[rmOffset + 2] = 0;
|
||||
rm[rmOffset + 6] = 0;
|
||||
rm[rmOffset + 8] = 0;
|
||||
rm[rmOffset + 9] = 0;
|
||||
rm[rmOffset + 10] = 1;
|
||||
} else {
|
||||
float len = length(x, y, z);
|
||||
if (1.0f != len) {
|
||||
float recipLen = 1.0f / len;
|
||||
x *= recipLen;
|
||||
y *= recipLen;
|
||||
z *= recipLen;
|
||||
}
|
||||
float nc = 1.0f - c;
|
||||
float xy = x * y;
|
||||
float yz = y * z;
|
||||
float zx = z * x;
|
||||
float xs = x * s;
|
||||
float ys = y * s;
|
||||
float zs = z * s;
|
||||
rm[rmOffset + 0] = x * x * nc + c;
|
||||
rm[rmOffset + 4] = xy * nc - zs;
|
||||
rm[rmOffset + 8] = zx * nc + ys;
|
||||
rm[rmOffset + 1] = xy * nc + zs;
|
||||
rm[rmOffset + 5] = y * y * nc + c;
|
||||
rm[rmOffset + 9] = yz * nc - xs;
|
||||
rm[rmOffset + 2] = zx * nc - ys;
|
||||
rm[rmOffset + 6] = yz * nc + xs;
|
||||
rm[rmOffset + 10] = z * z * nc + c;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts Euler angles to a rotation matrix
|
||||
*
|
||||
* @param rm returns the result
|
||||
* @param rmOffset index into rm where the result matrix starts
|
||||
* @param x angle of rotation, in degrees
|
||||
* @param y angle of rotation, in degrees
|
||||
* @param z angle of rotation, in degrees
|
||||
*/
|
||||
public static void setRotateEulerM(float[] rm, int rmOffset, float x, float y, float z) {
|
||||
x *= (float) (Math.PI / 180.0f);
|
||||
y *= (float) (Math.PI / 180.0f);
|
||||
z *= (float) (Math.PI / 180.0f);
|
||||
float cx = (float) Math.cos(x);
|
||||
float sx = (float) Math.sin(x);
|
||||
float cy = (float) Math.cos(y);
|
||||
float sy = (float) Math.sin(y);
|
||||
float cz = (float) Math.cos(z);
|
||||
float sz = (float) Math.sin(z);
|
||||
float cxsy = cx * sy;
|
||||
float sxsy = sx * sy;
|
||||
|
||||
rm[rmOffset + 0] = cy * cz;
|
||||
rm[rmOffset + 1] = -cy * sz;
|
||||
rm[rmOffset + 2] = sy;
|
||||
rm[rmOffset + 3] = 0.0f;
|
||||
|
||||
rm[rmOffset + 4] = cxsy * cz + cx * sz;
|
||||
rm[rmOffset + 5] = -cxsy * sz + cx * cz;
|
||||
rm[rmOffset + 6] = -sx * cy;
|
||||
rm[rmOffset + 7] = 0.0f;
|
||||
|
||||
rm[rmOffset + 8] = -sxsy * cz + sx * sz;
|
||||
rm[rmOffset + 9] = sxsy * sz + sx * cz;
|
||||
rm[rmOffset + 10] = cx * cy;
|
||||
rm[rmOffset + 11] = 0.0f;
|
||||
|
||||
rm[rmOffset + 12] = 0.0f;
|
||||
rm[rmOffset + 13] = 0.0f;
|
||||
rm[rmOffset + 14] = 0.0f;
|
||||
rm[rmOffset + 15] = 1.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Define a viewing transformation in terms of an eye point, a center of
|
||||
* view, and an up vector.
|
||||
*
|
||||
* @param rm returns the result
|
||||
* @param rmOffset index into rm where the result matrix starts
|
||||
* @param eyeX eye point X
|
||||
* @param eyeY eye point Y
|
||||
* @param eyeZ eye point Z
|
||||
* @param centerX center of view X
|
||||
* @param centerY center of view Y
|
||||
* @param centerZ center of view Z
|
||||
* @param upX up vector X
|
||||
* @param upY up vector Y
|
||||
* @param upZ up vector Z
|
||||
*/
|
||||
public static void setLookAtM(float[] rm, int rmOffset, float eyeX, float eyeY, float eyeZ, float centerX,
|
||||
float centerY, float centerZ, float upX, float upY, float upZ) {
|
||||
|
||||
// See the OpenGL GLUT documentation for gluLookAt for a description
|
||||
// of the algorithm. We implement it in a straightforward way:
|
||||
|
||||
float fx = centerX - eyeX;
|
||||
float fy = centerY - eyeY;
|
||||
float fz = centerZ - eyeZ;
|
||||
|
||||
// Normalize f
|
||||
float rlf = 1.0f / Matrix.length(fx, fy, fz);
|
||||
fx *= rlf;
|
||||
fy *= rlf;
|
||||
fz *= rlf;
|
||||
|
||||
// compute s = f x up (x means "cross product")
|
||||
float sx = fy * upZ - fz * upY;
|
||||
float sy = fz * upX - fx * upZ;
|
||||
float sz = fx * upY - fy * upX;
|
||||
|
||||
// and normalize s
|
||||
float rls = 1.0f / Matrix.length(sx, sy, sz);
|
||||
sx *= rls;
|
||||
sy *= rls;
|
||||
sz *= rls;
|
||||
|
||||
// compute u = s x f
|
||||
float ux = sy * fz - sz * fy;
|
||||
float uy = sz * fx - sx * fz;
|
||||
float uz = sx * fy - sy * fx;
|
||||
|
||||
rm[rmOffset + 0] = sx;
|
||||
rm[rmOffset + 1] = ux;
|
||||
rm[rmOffset + 2] = -fx;
|
||||
rm[rmOffset + 3] = 0.0f;
|
||||
|
||||
rm[rmOffset + 4] = sy;
|
||||
rm[rmOffset + 5] = uy;
|
||||
rm[rmOffset + 6] = -fy;
|
||||
rm[rmOffset + 7] = 0.0f;
|
||||
|
||||
rm[rmOffset + 8] = sz;
|
||||
rm[rmOffset + 9] = uz;
|
||||
rm[rmOffset + 10] = -fz;
|
||||
rm[rmOffset + 11] = 0.0f;
|
||||
|
||||
rm[rmOffset + 12] = 0.0f;
|
||||
rm[rmOffset + 13] = 0.0f;
|
||||
rm[rmOffset + 14] = 0.0f;
|
||||
rm[rmOffset + 15] = 1.0f;
|
||||
|
||||
translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,512 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,536 @@
|
||||
package org.hitlabnz.sensor_fusion_demo.representation;
|
||||
|
||||
/**
|
||||
* The Quaternion class. A Quaternion is a four-dimensional vector that is used to represent rotations of a rigid body
|
||||
* in the 3D space. It is very similar to a rotation vector; it contains an angle, encoded into the w component
|
||||
* and three components to describe the rotation-axis (encoded into x, y, z).
|
||||
*
|
||||
* <p>
|
||||
* Quaternions allow for elegant descriptions of 3D rotations, interpolations as well as extrapolations and compared to
|
||||
* Euler angles, they don't suffer from gimbal lock. Interpolations between two Quaternions are called SLERP (Spherical
|
||||
* Linear Interpolation).
|
||||
* </p>
|
||||
*
|
||||
* <p>
|
||||
* This class also contains the representation of the same rotation as a Quaternion and 4x4-Rotation-Matrix.
|
||||
* </p>
|
||||
*
|
||||
* @author Leigh Beattie, Alexander Pacha
|
||||
*
|
||||
*/
|
||||
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;
|
||||
|
||||
/**
|
||||
* This variable is used to synchronise the rotation matrix with the current quaternion values. If someone has
|
||||
* changed the
|
||||
* quaternion numbers then the matrix will need to be updated. To save on processing we only really want to update
|
||||
* the matrix when someone wants to fetch it, instead of whenever someone sets a quaternion value.
|
||||
*/
|
||||
private boolean dirty = false;
|
||||
|
||||
/**
|
||||
* Creates a new Quaternion object and initialises it with the identity Quaternion
|
||||
*/
|
||||
public Quaternion() {
|
||||
super();
|
||||
matrix = new Matrixf4x4();
|
||||
loadIdentityQuat();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Quaternion clone() {
|
||||
Quaternion clone = new Quaternion();
|
||||
clone.copyVec4(this);
|
||||
return clone;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalise this Quaternion into a unity Quaternion.
|
||||
*/
|
||||
public void normalise() {
|
||||
this.dirty = true;
|
||||
float mag = (float) Math.sqrt(points[3] * points[3] + points[0] * points[0] + points[1] * points[1] + points[2]
|
||||
* points[2]);
|
||||
points[3] = points[3] / mag;
|
||||
points[0] = points[0] / mag;
|
||||
points[1] = points[1] / mag;
|
||||
points[2] = points[2] / mag;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void normalize() {
|
||||
normalise();
|
||||
}
|
||||
|
||||
/**
|
||||
* Copies the values from the given quaternion to this one
|
||||
*
|
||||
* @param quat The quaternion to copy from
|
||||
*/
|
||||
public void set(Quaternion quat) {
|
||||
this.dirty = true;
|
||||
copyVec4(quat);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply this quaternion by the input quaternion and store the result in the out quaternion
|
||||
*
|
||||
* @param input
|
||||
* @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
|
||||
output.points[0] = (points[3] * input.points[0] + points[0] * input.points[3] + points[1] * input.points[2] - points[2]
|
||||
* input.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2
|
||||
output.points[1] = (points[3] * input.points[1] + points[1] * input.points[3] + points[2] * input.points[0] - points[0]
|
||||
* input.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2
|
||||
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];
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply this quaternion by the input quaternion and store the result in the out quaternion
|
||||
*
|
||||
* @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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies this Quaternion with a scalar
|
||||
*
|
||||
* @param scalar the value that the vector should be multiplied with
|
||||
*/
|
||||
public void multiplyByScalar(float scalar) {
|
||||
this.dirty = true;
|
||||
multiplyByScalar(scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a quaternion to this quaternion
|
||||
*
|
||||
* @param input The quaternion that you want to add to this one
|
||||
*/
|
||||
public void addQuat(Quaternion input) {
|
||||
this.dirty = true;
|
||||
addQuat(input, this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add this quaternion and another quaternion together and store the result in the output quaternion
|
||||
*
|
||||
* @param input The quaternion you want added to this quaternion
|
||||
* @param output The quaternion you want to store the output in.
|
||||
*/
|
||||
public void addQuat(Quaternion input, Quaternion output) {
|
||||
output.setX(getX() + input.getX());
|
||||
output.setY(getY() + input.getY());
|
||||
output.setZ(getZ() + input.getZ());
|
||||
output.setW(getW() + input.getW());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtract a quaternion to this quaternion
|
||||
*
|
||||
* @param input The quaternion that you want to subtracted from this one
|
||||
*/
|
||||
public void subQuat(Quaternion input) {
|
||||
this.dirty = true;
|
||||
subQuat(input, this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtract another quaternion from this quaternion and store the result in the output quaternion
|
||||
*
|
||||
* @param input The quaternion you want subtracted from this quaternion
|
||||
* @param output The quaternion you want to store the output in.
|
||||
*/
|
||||
public void subQuat(Quaternion input, Quaternion output) {
|
||||
output.setX(getX() - input.getX());
|
||||
output.setY(getY() - input.getY());
|
||||
output.setZ(getZ() - input.getZ());
|
||||
output.setW(getW() - input.getW());
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts this Quaternion into the Rotation-Matrix representation which can be accessed by
|
||||
* {@link Quaternion#getMatrix4x4 getMatrix4x4}
|
||||
*/
|
||||
private void convertQuatToMatrix() {
|
||||
float x = points[0];
|
||||
float y = points[1];
|
||||
float z = points[2];
|
||||
float w = points[3];
|
||||
|
||||
matrix.setX0(1 - 2 * (y * y) - 2 * (z * z)); //1 - 2y2 - 2z2
|
||||
matrix.setX1(2 * (x * y) + 2 * (w * z)); // 2xy - 2wz
|
||||
matrix.setX2(2 * (x * z) - 2 * (w * y)); //2xz + 2wy
|
||||
matrix.setX3(0);
|
||||
matrix.setY0(2 * (x * y) - 2 * (w * z)); //2xy + 2wz
|
||||
matrix.setY1(1 - 2 * (x * x) - 2 * (z * z)); //1 - 2x2 - 2z2
|
||||
matrix.setY2(2 * (y * z) + 2 * (w * x)); // 2yz + 2wx
|
||||
matrix.setY3(0);
|
||||
matrix.setZ0(2 * (x * z) + 2 * (w * y)); //2xz + 2wy
|
||||
matrix.setZ1(2 * (y * z) - 2 * (w * x)); //2yz - 2wx
|
||||
matrix.setZ2(1 - 2 * (x * x) - 2 * (y * y)); //1 - 2x2 - 2y2
|
||||
matrix.setZ3(0);
|
||||
matrix.setW0(0);
|
||||
matrix.setW1(0);
|
||||
matrix.setW2(0);
|
||||
matrix.setW3(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get an axis angle representation of this quaternion.
|
||||
*
|
||||
* @param output Vector4f axis angle.
|
||||
*/
|
||||
public void toAxisAngle(Vector4f output) {
|
||||
if (getW() > 1) {
|
||||
normalise(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
|
||||
}
|
||||
float angle = 2 * (float) Math.toDegrees(Math.acos(getW()));
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
float s = (float) Math.sqrt(1 - getW() * getW()); // assuming quaternion normalised then w is less than 1, so term always positive.
|
||||
if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt
|
||||
// if s close to zero then direction of axis not important
|
||||
x = points[0]; // if it is important that axis is normalised then replace with x=1; y=z=0;
|
||||
y = points[1];
|
||||
z = points[2];
|
||||
} else {
|
||||
x = points[0] / s; // normalise axis
|
||||
y = points[1] / s;
|
||||
z = points[2] / s;
|
||||
}
|
||||
|
||||
output.points[0] = x;
|
||||
output.points[1] = y;
|
||||
output.points[2] = z;
|
||||
output.points[3] = angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading, attitude and bank of this quaternion as euler angles in the double array respectively
|
||||
*
|
||||
* @return An array of size 3 containing the euler angles for this quaternion
|
||||
*/
|
||||
public double[] toEulerAngles() {
|
||||
double[] ret = new double[3];
|
||||
|
||||
ret[0] = Math.atan2(2 * points[1] * getW() - 2 * points[0] * points[2], 1 - 2 * (points[1] * points[1]) - 2
|
||||
* (points[2] * points[2])); // atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
|
||||
ret[1] = Math.asin(2 * points[0] * points[1] + 2 * points[2] * getW()); // asin(2*qx*qy + 2*qz*qw)
|
||||
ret[2] = Math.atan2(2 * points[0] * getW() - 2 * points[1] * points[2], 1 - 2 * (points[0] * points[0]) - 2
|
||||
* (points[2] * points[2])); // atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the quaternion to an identity quaternion of 0,0,0,1.
|
||||
*/
|
||||
public void loadIdentityQuat() {
|
||||
this.dirty = true;
|
||||
setX(0);
|
||||
setY(0);
|
||||
setZ(0);
|
||||
setW(1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return "{X: " + getX() + ", Y:" + getY() + ", Z:" + getZ() + ", W:" + getW() + "}";
|
||||
}
|
||||
|
||||
/**
|
||||
* This is an internal method used to build a quaternion from a rotation matrix and then sets the current quaternion
|
||||
* from that matrix.
|
||||
*
|
||||
*/
|
||||
private void generateQuaternionFromMatrix() {
|
||||
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float qw;
|
||||
|
||||
float[] mat = matrix.getMatrix();
|
||||
int[] indices = null;
|
||||
|
||||
if (this.matrix.size() == 16) {
|
||||
if (this.matrix.isColumnMajor()) {
|
||||
indices = Matrixf4x4.matIndCol16_3x3;
|
||||
} else {
|
||||
indices = Matrixf4x4.matIndRow16_3x3;
|
||||
}
|
||||
} else {
|
||||
if (this.matrix.isColumnMajor()) {
|
||||
indices = Matrixf4x4.matIndCol9_3x3;
|
||||
} else {
|
||||
indices = Matrixf4x4.matIndRow9_3x3;
|
||||
}
|
||||
}
|
||||
|
||||
int m00 = indices[0];
|
||||
int m01 = indices[1];
|
||||
int m02 = indices[2];
|
||||
|
||||
int m10 = indices[3];
|
||||
int m11 = indices[4];
|
||||
int m12 = indices[5];
|
||||
|
||||
int m20 = indices[6];
|
||||
int m21 = indices[7];
|
||||
int m22 = indices[8];
|
||||
|
||||
float tr = mat[m00] + mat[m11] + mat[m22];
|
||||
if (tr > 0) {
|
||||
float s = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw
|
||||
qw = 0.25f * s;
|
||||
qx = (mat[m21] - mat[m12]) / s;
|
||||
qy = (mat[m02] - mat[m20]) / s;
|
||||
qz = (mat[m10] - mat[m01]) / s;
|
||||
} else if ((mat[m00] > mat[m11]) & (mat[m00] > mat[m22])) {
|
||||
float s = (float) Math.sqrt(1.0 + mat[m00] - mat[m11] - mat[m22]) * 2; // S=4*qx
|
||||
qw = (mat[m21] - mat[m12]) / s;
|
||||
qx = 0.25f * s;
|
||||
qy = (mat[m01] + mat[m10]) / s;
|
||||
qz = (mat[m02] + mat[m20]) / s;
|
||||
} else if (mat[m11] > mat[m22]) {
|
||||
float s = (float) Math.sqrt(1.0 + mat[m11] - mat[m00] - mat[m22]) * 2; // S=4*qy
|
||||
qw = (mat[m02] - mat[m20]) / s;
|
||||
qx = (mat[m01] + mat[m10]) / s;
|
||||
qy = 0.25f * s;
|
||||
qz = (mat[m12] + mat[m21]) / s;
|
||||
} else {
|
||||
float s = (float) Math.sqrt(1.0 + mat[m22] - mat[m00] - mat[m11]) * 2; // S=4*qz
|
||||
qw = (mat[m10] - mat[m01]) / s;
|
||||
qx = (mat[m02] + mat[m20]) / s;
|
||||
qy = (mat[m12] + mat[m21]) / s;
|
||||
qz = 0.25f * s;
|
||||
}
|
||||
|
||||
setX(qx);
|
||||
setY(qy);
|
||||
setZ(qz);
|
||||
setW(qw);
|
||||
}
|
||||
|
||||
/**
|
||||
* You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a
|
||||
* rotation matrix this will fail. You MUST provide a 4x4 matrix.
|
||||
*
|
||||
* @param matrix A column major rotation matrix
|
||||
*/
|
||||
public void setColumnMajor(float[] matrix) {
|
||||
|
||||
this.matrix.setMatrix(matrix);
|
||||
this.matrix.setColumnMajor(true);
|
||||
|
||||
generateQuaternionFromMatrix();
|
||||
}
|
||||
|
||||
/**
|
||||
* You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a
|
||||
* rotation matrix this will fail.
|
||||
*
|
||||
* @param matrix A column major rotation matrix
|
||||
*/
|
||||
public void setRowMajor(float[] matrix) {
|
||||
|
||||
this.matrix.setMatrix(matrix);
|
||||
this.matrix.setColumnMajor(false);
|
||||
|
||||
generateQuaternionFromMatrix();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set this quaternion from axis angle values. All rotations are in degrees.
|
||||
*
|
||||
* @param azimuth The rotation around the z axis
|
||||
* @param pitch The rotation around the y axis
|
||||
* @param roll The rotation around the x axis
|
||||
*/
|
||||
public void setEulerAngle(float azimuth, float pitch, float roll) {
|
||||
|
||||
double heading = Math.toRadians(roll);
|
||||
double attitude = Math.toRadians(pitch);
|
||||
double bank = Math.toRadians(azimuth);
|
||||
|
||||
double c1 = Math.cos(heading / 2);
|
||||
double s1 = Math.sin(heading / 2);
|
||||
double c2 = Math.cos(attitude / 2);
|
||||
double s2 = Math.sin(attitude / 2);
|
||||
double c3 = Math.cos(bank / 2);
|
||||
double s3 = Math.sin(bank / 2);
|
||||
double c1c2 = c1 * c2;
|
||||
double s1s2 = s1 * s2;
|
||||
setW((float) (c1c2 * c3 - s1s2 * s3));
|
||||
setX((float) (c1c2 * s3 + s1s2 * c3));
|
||||
setY((float) (s1 * c2 * c3 + c1 * s2 * s3));
|
||||
setZ((float) (c1 * s2 * c3 - s1 * c2 * s3));
|
||||
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotation is in degrees. Set this quaternion from the supplied axis angle.
|
||||
*
|
||||
* @param vec The vector of rotation
|
||||
* @param rot The angle of rotation around that vector in degrees.
|
||||
*/
|
||||
public void setAxisAngle(Vector3f vec, float rot) {
|
||||
double s = Math.sin(Math.toRadians(rot / 2));
|
||||
setX(vec.getX() * (float) s);
|
||||
setY(vec.getY() * (float) s);
|
||||
setZ(vec.getZ() * (float) s);
|
||||
setW((float) Math.cos(Math.toRadians(rot / 2)));
|
||||
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
public void setAxisAngleRad(Vector3f vec, double rot) {
|
||||
double s = rot / 2;
|
||||
setX(vec.getX() * (float) s);
|
||||
setY(vec.getY() * (float) s);
|
||||
setZ(vec.getZ() * (float) s);
|
||||
setW((float) rot / 2);
|
||||
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return Returns this Quaternion in the Rotation Matrix representation
|
||||
*/
|
||||
public Matrixf4x4 getMatrix4x4() {
|
||||
//toMatrixColMajor();
|
||||
if (dirty) {
|
||||
convertQuatToMatrix();
|
||||
dirty = false;
|
||||
}
|
||||
return this.matrix;
|
||||
}
|
||||
|
||||
public void copyFromVec3(Vector3f vec, float w) {
|
||||
copyFromV3f(vec, w);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get a linear interpolation between this quaternion and the input quaternion, storing the result in the output
|
||||
* quaternion.
|
||||
*
|
||||
* @param input The quaternion to be slerped with this quaternion.
|
||||
* @param output The quaternion to store the result in.
|
||||
* @param t The ratio between the two quaternions where 0 <= t <= 1.0 . Increase value of t will bring rotation
|
||||
* closer to the input quaternion.
|
||||
*/
|
||||
public void slerp(Quaternion input, Quaternion output, float t) {
|
||||
// Calculate angle between them.
|
||||
//double cosHalftheta = this.dotProduct(input);
|
||||
Quaternion bufferQuat = null;
|
||||
float cosHalftheta = this.dotProduct(input);
|
||||
|
||||
if (cosHalftheta < 0) {
|
||||
bufferQuat = new Quaternion();
|
||||
cosHalftheta = -cosHalftheta;
|
||||
bufferQuat.points[0] = (-input.points[0]);
|
||||
bufferQuat.points[1] = (-input.points[1]);
|
||||
bufferQuat.points[2] = (-input.points[2]);
|
||||
bufferQuat.points[3] = (-input.points[3]);
|
||||
} else {
|
||||
bufferQuat = input;
|
||||
}
|
||||
/**
|
||||
* if(dot < 0.95f){
|
||||
* double angle = Math.acos(dot);
|
||||
* double ratioA = Math.sin((1 - t) * angle);
|
||||
* double ratioB = Math.sin(t * angle);
|
||||
* double divisor = Math.sin(angle);
|
||||
*
|
||||
* //Calculate Quaternion
|
||||
* output.setW((float)((this.getW() * ratioA + input.getW() * ratioB)/divisor));
|
||||
* output.setX((float)((this.getX() * ratioA + input.getX() * ratioB)/divisor));
|
||||
* output.setY((float)((this.getY() * ratioA + input.getY() * ratioB)/divisor));
|
||||
* output.setZ((float)((this.getZ() * ratioA + input.getZ() * ratioB)/divisor));
|
||||
* }
|
||||
* else{
|
||||
* lerp(input, output, t);
|
||||
* }
|
||||
*/
|
||||
// if qa=qb or qa=-qb then theta = 0 and we can return qa
|
||||
if (Math.abs(cosHalftheta) >= 1.0) {
|
||||
output.points[0] = (this.points[0]);
|
||||
output.points[1] = (this.points[1]);
|
||||
output.points[2] = (this.points[2]);
|
||||
output.points[3] = (this.points[3]);
|
||||
} else {
|
||||
double sinHalfTheta = Math.sqrt(1.0 - cosHalftheta * cosHalftheta);
|
||||
// if theta = 180 degrees then result is not fully defined
|
||||
// we could rotate around any axis normal to qa or qb
|
||||
//if(Math.abs(sinHalfTheta) < 0.001){
|
||||
//output.setW(this.getW() * 0.5f + input.getW() * 0.5f);
|
||||
//output.setX(this.getX() * 0.5f + input.getX() * 0.5f);
|
||||
//output.setY(this.getY() * 0.5f + input.getY() * 0.5f);
|
||||
//output.setZ(this.getZ() * 0.5f + input.getZ() * 0.5f);
|
||||
// lerp(bufferQuat, output, t);
|
||||
//}
|
||||
//else{
|
||||
double halfTheta = Math.acos(cosHalftheta);
|
||||
|
||||
double ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta;
|
||||
double ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
|
||||
|
||||
//Calculate Quaternion
|
||||
output.points[3] = ((float) (points[3] * ratioA + bufferQuat.points[3] * ratioB));
|
||||
output.points[0] = ((float) (this.points[0] * ratioA + bufferQuat.points[0] * ratioB));
|
||||
output.points[1] = ((float) (this.points[1] * ratioA + bufferQuat.points[1] * ratioB));
|
||||
output.points[2] = ((float) (this.points[2] * ratioA + bufferQuat.points[2] * ratioB));
|
||||
|
||||
//}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,295 @@
|
||||
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;
|
||||
|
||||
/**
|
||||
* A float array was chosen instead of individual variables due to performance concerns. Converting the points into
|
||||
* an array at run time can cause slowness so instead we use one array and extract the individual variables with get
|
||||
* methods.
|
||||
*/
|
||||
protected float[] points = new float[3];
|
||||
|
||||
/**
|
||||
* Initialises the vector with the given values
|
||||
*
|
||||
* @param x the x-component
|
||||
* @param y the y-component
|
||||
* @param z the z-component
|
||||
*/
|
||||
public Vector3f(float x, float y, float z) {
|
||||
this.points[0] = x;
|
||||
this.points[1] = y;
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialises all components of this vector with the given same value.
|
||||
*
|
||||
* @param value Initialisation value for all components
|
||||
*/
|
||||
public Vector3f(float value) {
|
||||
this.points[0] = value;
|
||||
this.points[1] = value;
|
||||
this.points[2] = value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Instantiates a new vector3f.
|
||||
*/
|
||||
public Vector3f() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
public Vector3f(Vector3f vector) {
|
||||
this.points[0] = vector.points[0];
|
||||
this.points[1] = vector.points[1];
|
||||
this.points[2] = vector.points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialises this vector from a 4-dimensional vector. If the fourth component is not zero, a normalisation of all
|
||||
* components will be performed.
|
||||
*
|
||||
* @param vector The 4-dimensional vector that should be used for initialisation
|
||||
*/
|
||||
public Vector3f(Vector4f vector) {
|
||||
if (vector.w() != 0) {
|
||||
this.points[0] = vector.x() / vector.w();
|
||||
this.points[1] = vector.y() / vector.w();
|
||||
this.points[2] = vector.z() / vector.w();
|
||||
} else {
|
||||
this.points[0] = vector.x();
|
||||
this.points[1] = vector.y();
|
||||
this.points[2] = vector.z();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns this vector as float-array.
|
||||
*
|
||||
* @return the float[]
|
||||
*/
|
||||
public float[] toArray() {
|
||||
return this.points;
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a vector to this vector
|
||||
*
|
||||
* @param summand the vector that should be added component-wise
|
||||
*/
|
||||
public void add(Vector3f summand) {
|
||||
this.points[0] += summand.points[0];
|
||||
this.points[1] += summand.points[1];
|
||||
this.points[2] += summand.points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the value to all components of this vector
|
||||
*
|
||||
* @param summand The value that should be added to all components
|
||||
*/
|
||||
public void add(float summand) {
|
||||
this.points[0] += summand;
|
||||
this.points[1] += summand;
|
||||
this.points[2] += summand;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @param subtrahend
|
||||
*/
|
||||
public void subtract(Vector3f subtrahend) {
|
||||
this.points[0] -= subtrahend.points[0];
|
||||
this.points[1] -= subtrahend.points[1];
|
||||
this.points[2] -= subtrahend.points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply by scalar.
|
||||
*
|
||||
* @param scalar the scalar
|
||||
*/
|
||||
public void multiplyByScalar(float scalar) {
|
||||
this.points[0] *= scalar;
|
||||
this.points[1] *= scalar;
|
||||
this.points[2] *= scalar;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize.
|
||||
*/
|
||||
public void normalize() {
|
||||
|
||||
double a = Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]);
|
||||
this.points[0] = (float) (this.points[0] / a);
|
||||
this.points[1] = (float) (this.points[1] / a);
|
||||
this.points[2] = (float) (this.points[2] / a);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the x.
|
||||
*
|
||||
* @return the x
|
||||
*/
|
||||
public float getX() {
|
||||
return points[0];
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the y.
|
||||
*
|
||||
* @return the y
|
||||
*/
|
||||
public float getY() {
|
||||
return points[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the z.
|
||||
*
|
||||
* @return the z
|
||||
*/
|
||||
public float getZ() {
|
||||
return points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the x.
|
||||
*
|
||||
* @param x the new x
|
||||
*/
|
||||
public void setX(float x) {
|
||||
this.points[0] = x;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the y.
|
||||
*
|
||||
* @param y the new y
|
||||
*/
|
||||
public void setY(float y) {
|
||||
this.points[1] = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the z.
|
||||
*
|
||||
* @param z the new z
|
||||
*/
|
||||
public void setZ(float z) {
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Functions for convenience
|
||||
*/
|
||||
|
||||
public float x() {
|
||||
return this.points[0];
|
||||
}
|
||||
|
||||
public float y() {
|
||||
return this.points[1];
|
||||
}
|
||||
|
||||
public float z() {
|
||||
return this.points[2];
|
||||
}
|
||||
|
||||
public void x(float x) {
|
||||
this.points[0] = x;
|
||||
}
|
||||
|
||||
public void y(float y) {
|
||||
this.points[1] = y;
|
||||
}
|
||||
|
||||
public void z(float z) {
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
public void setXYZ(float x, float y, float z) {
|
||||
this.points[0] = x;
|
||||
this.points[1] = y;
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the dot product of this vector with the input vector
|
||||
*
|
||||
* @param inputVec The vector you want to do the dot product with against this vector.
|
||||
* @return Float value representing the scalar of the dot product operation
|
||||
*/
|
||||
public float dotProduct(Vector3f inputVec) {
|
||||
return points[0] * inputVec.points[0] + points[1] * inputVec.points[1] + points[2] * inputVec.points[2];
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the cross product of this vector and another vector. The result will be stored in the output vector.
|
||||
*
|
||||
* @param inputVec The vector you want to get the dot product of against this vector.
|
||||
* @param outputVec The vector to store the result in.
|
||||
*/
|
||||
public void crossProduct(Vector3f inputVec, Vector3f outputVec) {
|
||||
outputVec.setX(points[1] * inputVec.points[2] - points[2] * inputVec.points[1]);
|
||||
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.
|
||||
*
|
||||
* @return The length of the vector
|
||||
*/
|
||||
public float getLength() {
|
||||
return (float) Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Clone the input vector so that this vector has the same values.
|
||||
*
|
||||
* @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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clone the input vector so that this vector has the same values.
|
||||
*
|
||||
* @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];
|
||||
System.arraycopy(source, 0, points, 0, 3);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,296 @@
|
||||
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 {
|
||||
|
||||
/**
|
||||
* ID for Serialisation
|
||||
*/
|
||||
private static final long serialVersionUID = 1L;
|
||||
/** The points. */
|
||||
protected float points[] = { 0, 0, 0, 0 };
|
||||
|
||||
/**
|
||||
* Instantiates a new vector4f.
|
||||
*
|
||||
* @param x the x
|
||||
* @param y the y
|
||||
* @param z the z
|
||||
* @param w the w
|
||||
*/
|
||||
public Vector4f(float x, float y, float z, float w) {
|
||||
this.points[0] = x;
|
||||
this.points[1] = y;
|
||||
this.points[2] = z;
|
||||
this.points[3] = w;
|
||||
}
|
||||
|
||||
/**
|
||||
* Instantiates a new vector4f.
|
||||
*/
|
||||
public Vector4f() {
|
||||
this.points[0] = 0;
|
||||
this.points[1] = 0;
|
||||
this.points[2] = 0;
|
||||
this.points[3] = 0;
|
||||
}
|
||||
|
||||
public Vector4f(Vector3f vector3f, float w) {
|
||||
this.points[0] = vector3f.x();
|
||||
this.points[1] = vector3f.y();
|
||||
this.points[2] = vector3f.z();
|
||||
this.points[3] = w;
|
||||
}
|
||||
|
||||
/**
|
||||
* To array.
|
||||
*
|
||||
* @return the float[]
|
||||
*/
|
||||
public float[] ToArray() {
|
||||
return points;
|
||||
}
|
||||
|
||||
public void copyVec4(Vector4f vec) {
|
||||
this.points[0] = vec.points[0];
|
||||
this.points[1] = vec.points[1];
|
||||
this.points[2] = vec.points[2];
|
||||
this.points[3] = vec.points[3];
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the.
|
||||
*
|
||||
* @param vector the vector
|
||||
*/
|
||||
public void add(Vector4f vector) {
|
||||
this.points[0] += vector.points[0];
|
||||
this.points[1] += vector.points[1];
|
||||
this.points[2] += vector.points[2];
|
||||
this.points[3] += vector.points[3];
|
||||
}
|
||||
|
||||
public void add(Vector3f vector, float w) {
|
||||
this.points[0] += vector.x();
|
||||
this.points[1] += vector.y();
|
||||
this.points[2] += vector.z();
|
||||
this.points[3] += w;
|
||||
}
|
||||
|
||||
public void subtract(Vector4f vector) {
|
||||
this.points[0] -= vector.points[0];
|
||||
this.points[1] -= vector.points[1];
|
||||
this.points[2] -= vector.points[2];
|
||||
this.points[3] -= vector.points[3];
|
||||
}
|
||||
|
||||
public void subtract(Vector4f vector, Vector4f output) {
|
||||
output.setXYZW(this.points[0] - vector.points[0], this.points[1] - vector.points[1], this.points[2]
|
||||
- vector.points[2], this.points[3] - vector.points[3]);
|
||||
}
|
||||
|
||||
public void subdivide(Vector4f vector) {
|
||||
this.points[0] /= vector.points[0];
|
||||
this.points[1] /= vector.points[1];
|
||||
this.points[2] /= vector.points[2];
|
||||
this.points[3] /= vector.points[3];
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply by scalar.
|
||||
*
|
||||
* @param scalar the scalar
|
||||
*/
|
||||
public void multiplyByScalar(float scalar) {
|
||||
this.points[0] *= scalar;
|
||||
this.points[1] *= scalar;
|
||||
this.points[2] *= scalar;
|
||||
this.points[3] *= scalar;
|
||||
}
|
||||
|
||||
public float dotProduct(Vector4f input) {
|
||||
return this.points[0] * input.points[0] + this.points[1] * input.points[1] + this.points[2] * input.points[2]
|
||||
+ this.points[3] * input.points[3];
|
||||
}
|
||||
|
||||
/**
|
||||
* Linear interpolation between two vectors storing the result in the output variable.
|
||||
*
|
||||
* @param input
|
||||
* @param output
|
||||
* @param t
|
||||
*/
|
||||
public void lerp(Vector4f input, Vector4f output, float t) {
|
||||
output.points[0] = (points[0] * (1.0f * t) + input.points[0] * t);
|
||||
output.points[1] = (points[1] * (1.0f * t) + input.points[1] * t);
|
||||
output.points[2] = (points[2] * (1.0f * t) + input.points[2] * t);
|
||||
output.points[3] = (points[3] * (1.0f * t) + input.points[3] * t);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize.
|
||||
*/
|
||||
public void normalize() {
|
||||
if (points[3] == 0)
|
||||
return;
|
||||
|
||||
points[0] /= points[3];
|
||||
points[1] /= points[3];
|
||||
points[2] /= points[3];
|
||||
|
||||
double a = Math.sqrt(this.points[0] * this.points[0] + this.points[1] * this.points[1] + this.points[2]
|
||||
* this.points[2]);
|
||||
points[0] = (float) (this.points[0] / a);
|
||||
points[1] = (float) (this.points[1] / a);
|
||||
points[2] = (float) (this.points[2] / a);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the x.
|
||||
*
|
||||
* @return the x
|
||||
*/
|
||||
public float getX() {
|
||||
return this.points[0];
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the y.
|
||||
*
|
||||
* @return the y
|
||||
*/
|
||||
public float getY() {
|
||||
return this.points[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the z.
|
||||
*
|
||||
* @return the z
|
||||
*/
|
||||
public float getZ() {
|
||||
return this.points[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the w.
|
||||
*
|
||||
* @return the w
|
||||
*/
|
||||
public float getW() {
|
||||
return this.points[3];
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the x.
|
||||
*
|
||||
* @param x the new x
|
||||
*/
|
||||
public void setX(float x) {
|
||||
this.points[0] = x;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the y.
|
||||
*
|
||||
* @param y the new y
|
||||
*/
|
||||
public void setY(float y) {
|
||||
this.points[1] = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the z.
|
||||
*
|
||||
* @param z the new z
|
||||
*/
|
||||
public void setZ(float z) {
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the w.
|
||||
*
|
||||
* @param w the new w
|
||||
*/
|
||||
public void setW(float w) {
|
||||
this.points[3] = w;
|
||||
}
|
||||
|
||||
public float x() {
|
||||
return this.points[0];
|
||||
}
|
||||
|
||||
public float y() {
|
||||
return this.points[1];
|
||||
}
|
||||
|
||||
public float z() {
|
||||
return this.points[2];
|
||||
}
|
||||
|
||||
public float w() {
|
||||
return this.points[3];
|
||||
}
|
||||
|
||||
public void x(float x) {
|
||||
this.points[0] = x;
|
||||
}
|
||||
|
||||
public void y(float y) {
|
||||
this.points[1] = y;
|
||||
}
|
||||
|
||||
public void z(float z) {
|
||||
this.points[2] = z;
|
||||
}
|
||||
|
||||
public void w(float w) {
|
||||
this.points[3] = w;
|
||||
}
|
||||
|
||||
public void setXYZW(float x, float y, float z, float w) {
|
||||
this.points[0] = x;
|
||||
this.points[1] = y;
|
||||
this.points[2] = z;
|
||||
this.points[3] = w;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compare this vector4f to the supplied one
|
||||
*
|
||||
* @param rhs True if they match, false other wise.
|
||||
* @return
|
||||
*/
|
||||
public boolean compareTo(Vector4f rhs) {
|
||||
boolean ret = false;
|
||||
if (this.points[0] == rhs.points[0] && this.points[1] == rhs.points[1] && this.points[2] == rhs.points[2]
|
||||
&& this.points[3] == rhs.points[3])
|
||||
ret = true;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Copies the data from the supplied vec3 into this vec4 plus the supplied w.
|
||||
*
|
||||
* @param input The x y z values to copy in.
|
||||
* @param w The extra w element to copy in
|
||||
*/
|
||||
public void copyFromV3f(Vector3f input, float w) {
|
||||
points[0] = (input.x());
|
||||
points[1] = (input.y());
|
||||
points[2] = (input.z());
|
||||
points[3] = (w);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2] + " W:" + points[3];
|
||||
}
|
||||
|
||||
}
|
||||
BIN
app/src/main/res/drawable-hdpi/ic_launcher.png
Normal file
BIN
app/src/main/res/drawable-hdpi/ic_launcher.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 7.0 KiB |
BIN
app/src/main/res/drawable-mdpi/ic_launcher.png
Normal file
BIN
app/src/main/res/drawable-mdpi/ic_launcher.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.1 KiB |
BIN
app/src/main/res/drawable-xhdpi/ic_launcher.png
Normal file
BIN
app/src/main/res/drawable-xhdpi/ic_launcher.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 9.6 KiB |
BIN
app/src/main/res/drawable-xxhdpi/ic_launcher.png
Normal file
BIN
app/src/main/res/drawable-xxhdpi/ic_launcher.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
14
app/src/main/res/layout/activity_about.xml
Normal file
14
app/src/main/res/layout/activity_about.xml
Normal file
@@ -0,0 +1,14 @@
|
||||
<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
|
||||
xmlns:tools="http://schemas.android.com/tools"
|
||||
android:layout_width="match_parent"
|
||||
android:layout_height="match_parent"
|
||||
tools:context=".AboutActivity" >
|
||||
|
||||
<WebView
|
||||
android:id="@+id/webViewAbout"
|
||||
android:layout_width="match_parent"
|
||||
android:layout_height="match_parent"
|
||||
android:layout_alignParentLeft="true"
|
||||
android:layout_alignParentTop="true" />
|
||||
|
||||
</RelativeLayout>
|
||||
23
app/src/main/res/layout/activity_sensor_selection.xml
Normal file
23
app/src/main/res/layout/activity_sensor_selection.xml
Normal file
@@ -0,0 +1,23 @@
|
||||
<android.support.v4.view.ViewPager xmlns:android="http://schemas.android.com/apk/res/android"
|
||||
xmlns:tools="http://schemas.android.com/tools"
|
||||
android:id="@+id/pager"
|
||||
android:layout_width="match_parent"
|
||||
android:layout_height="match_parent"
|
||||
tools:context=".SensorSelectionActivity" >
|
||||
|
||||
<!--
|
||||
This title strip will display the currently visible page title, as well as the page
|
||||
titles for adjacent pages.
|
||||
-->
|
||||
|
||||
<android.support.v4.view.PagerTitleStrip
|
||||
android:id="@+id/pager_title_strip"
|
||||
android:layout_width="match_parent"
|
||||
android:layout_height="wrap_content"
|
||||
android:layout_gravity="top"
|
||||
android:background="#33b5e5"
|
||||
android:paddingBottom="4dp"
|
||||
android:paddingTop="4dp"
|
||||
android:textColor="#fff" />
|
||||
|
||||
</android.support.v4.view.ViewPager>
|
||||
9
app/src/main/res/menu/sensor_selection.xml
Normal file
9
app/src/main/res/menu/sensor_selection.xml
Normal file
@@ -0,0 +1,9 @@
|
||||
<menu xmlns:android="http://schemas.android.com/apk/res/android" >
|
||||
|
||||
<item
|
||||
android:id="@+id/action_about"
|
||||
android:orderInCategory="100"
|
||||
android:showAsAction="always"
|
||||
android:title="@string/action_about"/>
|
||||
|
||||
</menu>
|
||||
19
app/src/main/res/values-de/strings.xml
Normal file
19
app/src/main/res/values-de/strings.xml
Normal file
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<resources>
|
||||
|
||||
<string name="app_name">Sensorfusions Demo</string>
|
||||
<string name="action_about">Über</string>
|
||||
<string name="title_main_activity">Sensorfusions Demo</string>
|
||||
<string name="title_section1">Improved Orientation Sensor 1</string>
|
||||
<string name="title_section2">Improved Orientation Sensor 2</string>
|
||||
<string name="title_section3">Android Rotation Vector</string>
|
||||
<string name="title_section4">Kalibriertes Gyroskop</string>
|
||||
<string name="title_section5">Gravitation und Kompass</string>
|
||||
<string name="title_section6">Akzelerometer und Kompass</string>
|
||||
<string name="title_activity_sensor_selection">Sensorfusions Demo</string>
|
||||
<string name="title_activity_about">Über</string>
|
||||
<string name="gyroscope_missing">Fehlendes Gyroskop</string>
|
||||
<string name="gyroscope_missing_message">Dieses Gerät hat keinen Gyroskop-Sensor der aber notwendig ist, damit diese App richtig funktioniert. Bitte verwende ein aktuelleres Gerät, das ein Gyroskop hat.</string>
|
||||
<string name="OK">OK</string>
|
||||
|
||||
</resources>
|
||||
8
app/src/main/res/values-sw600dp/dimens.xml
Normal file
8
app/src/main/res/values-sw600dp/dimens.xml
Normal file
@@ -0,0 +1,8 @@
|
||||
<resources>
|
||||
|
||||
<!--
|
||||
Customize dimensions originally defined in res/values/dimens.xml (such as
|
||||
screen margins) for sw600dp devices (e.g. 7" tablets) here.
|
||||
-->
|
||||
|
||||
</resources>
|
||||
9
app/src/main/res/values-sw720dp-land/dimens.xml
Normal file
9
app/src/main/res/values-sw720dp-land/dimens.xml
Normal file
@@ -0,0 +1,9 @@
|
||||
<resources>
|
||||
|
||||
<!--
|
||||
Customize dimensions originally defined in res/values/dimens.xml (such as
|
||||
screen margins) for sw720dp devices (e.g. 10" tablets) in landscape here.
|
||||
-->
|
||||
<dimen name="activity_horizontal_margin">128dp</dimen>
|
||||
|
||||
</resources>
|
||||
11
app/src/main/res/values-v11/styles.xml
Normal file
11
app/src/main/res/values-v11/styles.xml
Normal file
@@ -0,0 +1,11 @@
|
||||
<resources>
|
||||
|
||||
<!--
|
||||
Base application theme for API 11+. This theme completely replaces
|
||||
AppBaseTheme from res/values/styles.xml on API 11+ devices.
|
||||
-->
|
||||
<style name="AppBaseTheme" parent="android:Theme.Holo.Light">
|
||||
<!-- API 11 theme customizations can go here. -->
|
||||
</style>
|
||||
|
||||
</resources>
|
||||
12
app/src/main/res/values-v14/styles.xml
Normal file
12
app/src/main/res/values-v14/styles.xml
Normal file
@@ -0,0 +1,12 @@
|
||||
<resources>
|
||||
|
||||
<!--
|
||||
Base application theme for API 14+. This theme completely replaces
|
||||
AppBaseTheme from BOTH res/values/styles.xml and
|
||||
res/values-v11/styles.xml on API 14+ devices.
|
||||
-->
|
||||
<style name="AppBaseTheme" parent="android:Theme.Holo.Light.DarkActionBar">
|
||||
<!-- API 14 theme customizations can go here. -->
|
||||
</style>
|
||||
|
||||
</resources>
|
||||
7
app/src/main/res/values/dimens.xml
Normal file
7
app/src/main/res/values/dimens.xml
Normal file
@@ -0,0 +1,7 @@
|
||||
<resources>
|
||||
|
||||
<!-- Default screen margins, per the Android Design guidelines. -->
|
||||
<dimen name="activity_horizontal_margin">16dp</dimen>
|
||||
<dimen name="activity_vertical_margin">16dp</dimen>
|
||||
|
||||
</resources>
|
||||
19
app/src/main/res/values/strings.xml
Normal file
19
app/src/main/res/values/strings.xml
Normal file
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<resources>
|
||||
|
||||
<string name="app_name">Sensor fusion demo</string>
|
||||
<string name="action_about">About</string>
|
||||
<string name="title_main_activity">Sensor fusion demo</string>
|
||||
<string name="title_section1">Improved Orientation Sensor 1</string>
|
||||
<string name="title_section2">Improved Orientation Sensor 2</string>
|
||||
<string name="title_section3">Android Rotation Vector</string>
|
||||
<string name="title_section4">Calibrated Gyroscope</string>
|
||||
<string name="title_section5">Gravity and Compass</string>
|
||||
<string name="title_section6">Accelerometer and Compass</string>
|
||||
<string name="title_activity_sensor_selection">Sensor fusion demo</string>
|
||||
<string name="title_activity_about">About</string>
|
||||
<string name="gyroscope_missing">Gyroscope Missing</string>
|
||||
<string name="gyroscope_missing_message">Your device has no hardware gyroscope sensor, which would be necessary for this app to work properly. Please run it on a newer device that has a gyroscope.</string>
|
||||
<string name="OK">OK</string>
|
||||
|
||||
</resources>
|
||||
20
app/src/main/res/values/styles.xml
Normal file
20
app/src/main/res/values/styles.xml
Normal file
@@ -0,0 +1,20 @@
|
||||
<resources>
|
||||
|
||||
<!--
|
||||
Base application theme, dependent on API level. This theme is replaced
|
||||
by AppBaseTheme from res/values-vXX/styles.xml on newer devices.
|
||||
-->
|
||||
<style name="AppBaseTheme" parent="android:Theme.Light">
|
||||
<!--
|
||||
Theme customizations available in newer API levels can go in
|
||||
res/values-vXX/styles.xml, while customizations related to
|
||||
backward-compatibility can go here.
|
||||
-->
|
||||
</style>
|
||||
|
||||
<!-- Application theme. -->
|
||||
<style name="AppTheme" parent="AppBaseTheme">
|
||||
<!-- All customizations that are NOT specific to a particular API-level can go here. -->
|
||||
</style>
|
||||
|
||||
</resources>
|
||||
Reference in New Issue
Block a user