/*
* Copyright 2014 Google Inc. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.kitware.tangoutils;
import android.opengl.Matrix;
/**
* Utility class to manage the calculation of a Model Matrix from the
* translation and quaternion arrays obtained from an {@link TangoPose} object.
* Delegates some mathematical computations to the {@link MathUtils}.
*/
public class ModelMatCalculator {
private static float[] mConversionMatrix = new float[] { 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f };
private float[] mModelMatrix = new float[16];
private float[] mPointCloudModelMatrix = new float[16];
private float[] mDevice2IMUMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
1.0f };
private float[] mColorCamera2IMUMatrix = new float[] { 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f };
private float[] mOpengl2ColorCameraMatrix = new float[] { 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f };
public ModelMatCalculator() {
Matrix.setIdentityM(mModelMatrix, 0);
Matrix.setIdentityM(mPointCloudModelMatrix, 0);
}
/**
* Updates the model matrix (rotation and translation).
*
* @param translation
* a three-element array of translation data.
* @param quaternion
* a four-element array of rotation data.
*/
public void updatePointCloudModelMatrix(float[] translation,
float[] quaternion) {
float[] tempMultMatrix = new float[16];
Matrix.setIdentityM(tempMultMatrix, 0);
Matrix.multiplyMM(tempMultMatrix, 0, mColorCamera2IMUMatrix, 0,
mOpengl2ColorCameraMatrix, 0);
float[] tempInvertMatrix = new float[16];
Matrix.setIdentityM(tempInvertMatrix, 0);
Matrix.invertM(tempInvertMatrix, 0, mDevice2IMUMatrix, 0);
float[] tempMultMatrix2 = new float[16];
Matrix.setIdentityM(tempMultMatrix2, 0);
Matrix.multiplyMM(tempMultMatrix2, 0, tempInvertMatrix, 0,
tempMultMatrix, 0);
float[] quaternionMatrix = new float[16];
Matrix.setIdentityM(quaternionMatrix, 0);
quaternionMatrix = quaternionMatrixOpenGL(quaternion);
float[] tempMultMatrix3 = new float[16];
Matrix.setIdentityM(tempMultMatrix3, 0);
Matrix.setIdentityM(mPointCloudModelMatrix, 0);
Matrix.multiplyMM(tempMultMatrix3, 0, quaternionMatrix, 0,
tempMultMatrix2, 0);
Matrix.multiplyMM(mPointCloudModelMatrix, 0, mConversionMatrix, 0,
tempMultMatrix3, 0);
mPointCloudModelMatrix[12] += translation[0];
mPointCloudModelMatrix[13] += translation[2];
mPointCloudModelMatrix[14] += -1f * translation[1];
}
/**
* Updates the model matrix (rotation and translation).
*
* @param translation
* a three-element array of translation data.
* @param quaternion
* a four-element array of rotation data.
*/
public void updateModelMatrix(float[] translation, float[] quaternion) {
float[] tempMultMatrix = new float[16];
Matrix.setIdentityM(tempMultMatrix, 0);
Matrix.multiplyMM(tempMultMatrix, 0, mColorCamera2IMUMatrix, 0,
mOpengl2ColorCameraMatrix, 0);
float[] tempInvertMatrix = new float[16];
Matrix.setIdentityM(tempInvertMatrix, 0);
Matrix.invertM(tempInvertMatrix, 0, mDevice2IMUMatrix, 0);
float[] tempMultMatrix2 = new float[16];
Matrix.setIdentityM(tempMultMatrix2, 0);
Matrix.multiplyMM(tempMultMatrix2, 0, tempInvertMatrix, 0,
tempMultMatrix, 0);
float[] quaternionMatrix = new float[16];
Matrix.setIdentityM(quaternionMatrix, 0);
quaternionMatrix = quaternionMatrixOpenGL(quaternion);
float[] tempMultMatrix3 = new float[16];
Matrix.setIdentityM(tempMultMatrix3, 0);
Matrix.setIdentityM(mModelMatrix, 0);
Matrix.multiplyMM(tempMultMatrix3, 0, quaternionMatrix, 0,
tempMultMatrix2, 0);
Matrix.multiplyMM(mModelMatrix, 0, mConversionMatrix, 0,
tempMultMatrix3, 0);
mModelMatrix[12] += translation[0];
mModelMatrix[13] += translation[2];
mModelMatrix[14] += -1f * translation[1];
}
public void SetDevice2IMUMatrix(float[] translation, float[] quaternion) {
mDevice2IMUMatrix = quaternionMatrixOpenGL(quaternion);
mDevice2IMUMatrix[12] = translation[0];
mDevice2IMUMatrix[13] = translation[1];
mDevice2IMUMatrix[14] = translation[2];
}
public void SetColorCamera2IMUMatrix(float[] translation, float[] quaternion) {
mOpengl2ColorCameraMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
1.0f };
mColorCamera2IMUMatrix = quaternionMatrixOpenGL(quaternion);
mColorCamera2IMUMatrix[12] = translation[0];
mColorCamera2IMUMatrix[13] = translation[1];
mColorCamera2IMUMatrix[14] = translation[2];
}
public float[] getModelMatrix() {
return mModelMatrix;
}
public float[] getModelMatrixCopy() {
float[] modelMatCopy = new float[16];
System.arraycopy(mModelMatrix, 0, modelMatCopy, 0, 16);
return modelMatCopy;
}
public float[] getPointCloudModelMatrixCopy() {
float[] modelMatCopy = new float[16];
float[] tempMultMat = new float[16];
Matrix.setIdentityM(tempMultMat, 0);
float[] invertYandZMatrix = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
1.0f };
Matrix.multiplyMM(tempMultMat, 0, mPointCloudModelMatrix, 0,
invertYandZMatrix, 0);
System.arraycopy(tempMultMat, 0, modelMatCopy, 0, 16);
return modelMatCopy;
}
public float[] getTranslation() {
return new float[] { mModelMatrix[12], mModelMatrix[13],
mModelMatrix[14] };
}
/**
* A function to convert a quaternion to quaternion Matrix. Please note that
* Opengl.Matrix is Column Major and so we construct the matrix in Column
* Major Format. - - - - | 0 4 8 12 | | 1 5 9 13 | | 2 6 10 14 | | 3 7 11 15
* | - - - -
*
* @param quaternion
* Input quaternion with float[4]
* @return Quaternion Matrix of float[16]
*/
public static float[] quaternionMatrixOpenGL(float[] quaternion) {
float[] matrix = new float[16];
normalizeVector(quaternion);
float x = quaternion[0];
float y = quaternion[1];
float z = quaternion[2];
float w = quaternion[3];
float x2 = x * x;
float y2 = y * y;
float z2 = z * z;
float xy = x * y;
float xz = x * z;
float yz = y * z;
float wx = w * x;
float wy = w * y;
float wz = w * z;
matrix[0] = 1f - 2f * (y2 + z2);
matrix[4] = 2f * (xy - wz);
matrix[8] = 2f * (xz + wy);
matrix[12] = 0f;
matrix[1] = 2f * (xy + wz);
matrix[5] = 1f - 2f * (x2 + z2);
matrix[9] = 2f * (yz - wx);
matrix[13] = 0f;
matrix[2] = 2f * (xz - wy);
matrix[6] = 2f * (yz + wx);
matrix[10] = 1f - 2f * (x2 + y2);
matrix[14] = 0f;
matrix[3] = 0f;
matrix[7] = 0f;
matrix[11] = 0f;
matrix[15] = 1f;
return matrix;
}
/**
* Creates a unit vector in the direction of an arbitrary vector. The
* original vector is modified in place.
*
* @param v
* the vector to normalize
*/
public static void normalizeVector(float[] v) {
float mag2 = v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
if (Math.abs(mag2) > 0.00001f && Math.abs(mag2 - 1.0f) > 0.00001f) {
float mag = (float) Math.sqrt(mag2);
v[0] = v[0] / mag;
v[1] = v[1] / mag;
v[2] = v[2] / mag;
v[3] = v[3] / mag;
}
}
}
