1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 package com.android.cts.verifier.sensors.sixdof.Renderer.RenderUtils; 17 18 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.MATRIX_4X4; 19 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.X; 20 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.Y; 21 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.Z; 22 23 import android.opengl.Matrix; 24 25 import com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils; 26 27 28 /** 29 * Utility class to manage the calculation of a Model Matrix from the translation and quaternion 30 * arrays obtained from an PoseData object. 31 */ 32 public class ModelMatrixCalculator { 33 34 protected static final int MATRIX_4X4_TRANSLATION_X = 12; 35 protected static final int MATRIX_4X4_TRANSLATION_Y = 13; 36 protected static final int MATRIX_4X4_TRANSLATION_Z = 14; 37 38 public static final float[] TANGO_TO_OPENGL = new float[]{1.0f, 0.0f, 0.0f, 0.0f, 39 0.0f, 0.0f, -1.0f, 0.0f, 40 0.0f, 1.0f, 0.0f, 0.0f, 41 0.0f, 0.0f, 0.0f, 1.0f}; 42 43 protected float[] mModelMatrix = new float[MATRIX_4X4]; 44 45 // Set these to identity matrix. 46 protected float[] mDevice2IMUMatrix = new float[]{1.0f, 0.0f, 0.0f, 0.0f, 47 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 48 1.0f}; 49 protected float[] mColorCamera2IMUMatrix = new float[]{1.0f, 0.0f, 0.0f, 50 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 51 0.0f, 1.0f}; 52 protected float[] mOpengl2ColorCameraMatrix = new float[]{1.0f, 0.0f, 0.0f, 53 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 54 0.0f, 1.0f}; 55 56 protected int mToRotate = 0; 57 58 public ModelMatrixCalculator(int toRotate) { 59 Matrix.setIdentityM(mModelMatrix, 0); 60 mToRotate = toRotate; 61 } 62 63 /** 64 * Calculates a new model matrix, taking into account extrinsics and the latest pose 65 * data. 66 * 67 * @param translationInOpenGlCoordinates latest translation from pose data in OpenGl coordinate 68 * system. 69 * @param quaternion latest rotation from pose data. 70 * @return the new model matrix. 71 */ 72 protected float[] calculateModelMatrix(float[] translationInOpenGlCoordinates, 73 float[] quaternion) { 74 float[] newModelMatrix = new float[MATRIX_4X4]; 75 76 // Calculate an initial matrix with extrinsics taken into account. 77 float[] imu2OpenGlMatrix = new float[MATRIX_4X4]; 78 Matrix.setIdentityM(imu2OpenGlMatrix, 0); 79 Matrix.multiplyMM(imu2OpenGlMatrix, 0, mColorCamera2IMUMatrix, 0, 80 mOpengl2ColorCameraMatrix, 0); 81 float[] invertedDevice2ImuMatrix = new float[MATRIX_4X4]; 82 Matrix.setIdentityM(invertedDevice2ImuMatrix, 0); 83 Matrix.invertM(invertedDevice2ImuMatrix, 0, mDevice2IMUMatrix, 0); 84 float[] extrinsicsBasedMatrix = new float[MATRIX_4X4]; 85 Matrix.setIdentityM(extrinsicsBasedMatrix, 0); 86 Matrix.multiplyMM(extrinsicsBasedMatrix, 0, invertedDevice2ImuMatrix, 0, 87 imu2OpenGlMatrix, 0); 88 89 // Do any translations that need to be done before rotating. Only used for the Cone offset. 90 float[] requiredTranslations = getRequiredTranslations(); 91 Matrix.translateM(extrinsicsBasedMatrix, 0, requiredTranslations[X], requiredTranslations[Y], 92 requiredTranslations[Z]); 93 94 // Rotate based on rotation pose data. 95 float[] quaternionMatrix = new float[MATRIX_4X4]; 96 Matrix.setIdentityM(quaternionMatrix, 0); 97 quaternionMatrix = MathsUtils.quaternionMatrixOpenGL(quaternion); 98 float[] rotatedMatrix = new float[MATRIX_4X4]; 99 float[] deviceOrientationMatrix = new float[MATRIX_4X4]; 100 Matrix.setIdentityM(rotatedMatrix, 0); 101 Matrix.setIdentityM(newModelMatrix, 0); 102 Matrix.setIdentityM(deviceOrientationMatrix, 0); 103 Matrix.multiplyMM(rotatedMatrix, 0, quaternionMatrix, 0, 104 extrinsicsBasedMatrix, 0); 105 Matrix.multiplyMM(deviceOrientationMatrix, 0, TANGO_TO_OPENGL, 0, 106 rotatedMatrix, 0); 107 108 Matrix.multiplyMM(newModelMatrix, 0, deviceOrientationMatrix, 0, 109 MathsUtils.getDeviceOrientationMatrix(mToRotate), 0); 110 111 // Finally, add the translations from the pose data. 112 newModelMatrix[MATRIX_4X4_TRANSLATION_X] += translationInOpenGlCoordinates[X]; 113 newModelMatrix[MATRIX_4X4_TRANSLATION_Y] += translationInOpenGlCoordinates[Y]; 114 newModelMatrix[MATRIX_4X4_TRANSLATION_Z] += translationInOpenGlCoordinates[Z]; 115 116 return newModelMatrix; 117 } 118 119 /** 120 * Updates the model matrix (rotation and translation). 121 * 122 * @param translation a three-element array of translation data. 123 * @param quaternion a four-element array of rotation data. 124 */ 125 public void updateModelMatrix(float[] translation, float[] quaternion) { 126 float[] convertedTranslation = MathsUtils.convertToOpenGlCoordinates(translation, mToRotate); 127 mModelMatrix = calculateModelMatrix(convertedTranslation, quaternion); 128 } 129 130 public void setDevice2IMUMatrix(float[] translation, float[] quaternion) { 131 mDevice2IMUMatrix = MathsUtils.quaternionMatrixOpenGL(quaternion); 132 mDevice2IMUMatrix[MATRIX_4X4_TRANSLATION_X] = translation[X]; 133 mDevice2IMUMatrix[MATRIX_4X4_TRANSLATION_Y] = translation[Y]; 134 mDevice2IMUMatrix[MATRIX_4X4_TRANSLATION_Z] = translation[Z]; 135 } 136 137 public void setColorCamera2IMUMatrix(float[] translation, float[] quaternion) { 138 mOpengl2ColorCameraMatrix = new float[]{1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 139 -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 140 1.0f}; 141 mColorCamera2IMUMatrix = MathsUtils.quaternionMatrixOpenGL(quaternion); 142 mColorCamera2IMUMatrix[MATRIX_4X4_TRANSLATION_X] = translation[X]; 143 mColorCamera2IMUMatrix[MATRIX_4X4_TRANSLATION_Y] = translation[Y]; 144 mColorCamera2IMUMatrix[MATRIX_4X4_TRANSLATION_Z] = translation[Z]; 145 } 146 147 public float[] getModelMatrix() { 148 return mModelMatrix; 149 } 150 151 /** 152 * Translations that need to be done before rotating. Used for calculating the CONE_OFFSET. 153 * 154 * @return no translation. 155 */ 156 protected float[] getRequiredTranslations() { 157 return new float[]{0.0f, 0.0f, 0.0f}; 158 } 159 } 160
