1 /* 2 * Copyright (C) 2023 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.server.uwb.correction.math; 17 18 import androidx.annotation.NonNull; 19 20 import java.util.Locale; 21 22 /** 23 * 4x4 Matrix representing translation, scale, and rotation. Column major, right handed: 24 * 25 * <pre> 26 * [0, 4, 8, 12] 27 * [1, 5, 9, 13] 28 * [2, 6, 10, 14] 29 * [3, 7, 11, 15] 30 * </pre> 31 */ 32 public class Matrix { 33 private static final float[] IDENTITY_DATA = 34 new float[] { 35 1.0f, 0.0f, 0.0f, 0.0f, 36 0.0f, 1.0f, 0.0f, 0.0f, 37 0.0f, 0.0f, 1.0f, 0.0f, 38 0.0f, 0.0f, 0.0f, 1.0f 39 }; 40 41 public final float[] data = new float[16]; 42 Matrix()43 private Matrix() {} 44 45 /** 46 * Creates a new instance of the Matrix class. 47 * @param data A 16-element array of the matrix contents. 48 */ Matrix(float[] data)49 public Matrix(float[] data) { 50 set(data); 51 } 52 set(float[] data)53 private void set(float[] data) { 54 if (data.length != 16) { 55 throw new IllegalArgumentException("Cannot set Matrix, invalid data."); 56 } 57 58 System.arraycopy(data, 0, this.data, 0, 16); 59 } 60 61 /** 62 * Gets the identity matrix. 63 * 64 * @return The identity matrix. 65 */ identity()66 public static Matrix identity() { 67 return new Matrix(IDENTITY_DATA); 68 } 69 70 /** Transform a Vector3 with this Matrix. */ transformPoint(Vector3 vector)71 public Vector3 transformPoint(Vector3 vector) { 72 float vx = vector.x; 73 float vy = vector.y; 74 float vz = vector.z; 75 76 float x, y, z; 77 x = data[0] * vx; 78 x += data[4] * vy; 79 x += data[8] * vz; 80 x += data[12]; // *1 81 82 y = data[1] * vx; 83 y += data[5] * vy; 84 y += data[9] * vz; 85 y += data[13]; // *1 86 87 z = data[2] * vx; 88 z += data[6] * vy; 89 z += data[10] * vz; 90 z += data[14]; // *1 91 return new Vector3(x, y, z); 92 } 93 94 /** Multiply two Matrices together and store the result in a third Matrix. */ multiply(Matrix lhs, Matrix rhs)95 public static Matrix multiply(Matrix lhs, Matrix rhs) { 96 float[] result = new float[16]; 97 98 for (int row = 0; row < 4; row++) { 99 for (int col = 0; col < 4; col++) { 100 float sum = 0; 101 for (int k = 0; k < 4; k++) { 102 sum += lhs.data[row * 4 + k] * rhs.data[k * 4 + col]; 103 } 104 result[row * 4 + col] = sum; 105 } 106 } 107 return new Matrix(result); 108 } 109 110 /** Returns a matrix that is an inversion of this one. */ invert()111 public Matrix invert() { 112 float a00 = data[0], a01 = data[1], a02 = data[2], a03 = data[3]; 113 float a10 = data[4], a11 = data[5], a12 = data[6], a13 = data[7]; 114 float a20 = data[8], a21 = data[9], a22 = data[10], a23 = data[11]; 115 float a30 = data[12], a31 = data[13], a32 = data[14], a33 = data[15]; 116 117 float b00 = a00 * a11 - a01 * a10; 118 float b01 = a00 * a12 - a02 * a10; 119 float b02 = a00 * a13 - a03 * a10; 120 float b03 = a01 * a12 - a02 * a11; 121 float b04 = a01 * a13 - a03 * a11; 122 float b05 = a02 * a13 - a03 * a12; 123 float b06 = a20 * a31 - a21 * a30; 124 float b07 = a20 * a32 - a22 * a30; 125 float b08 = a20 * a33 - a23 * a30; 126 float b09 = a21 * a32 - a22 * a31; 127 float b10 = a21 * a33 - a23 * a31; 128 float b11 = a22 * a33 - a23 * a32; 129 130 float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; 131 if (det == 0) { 132 return null; 133 } 134 135 float[] m = new float[16]; 136 137 float invDet = 1.0f / det; 138 m[0] = (a11 * b11 - a12 * b10 + a13 * b09) * invDet; 139 m[1] = (-a01 * b11 + a02 * b10 - a03 * b09) * invDet; 140 m[2] = (a31 * b05 - a32 * b04 + a33 * b03) * invDet; 141 m[3] = (-a21 * b05 + a22 * b04 - a23 * b03) * invDet; 142 m[4] = (-a10 * b11 + a12 * b08 - a13 * b07) * invDet; 143 m[5] = (a00 * b11 - a02 * b08 + a03 * b07) * invDet; 144 m[6] = (-a30 * b05 + a32 * b02 - a33 * b01) * invDet; 145 m[7] = (a20 * b05 - a22 * b02 + a23 * b01) * invDet; 146 m[8] = (a10 * b10 - a11 * b08 + a13 * b06) * invDet; 147 m[9] = (-a00 * b10 + a01 * b08 - a03 * b06) * invDet; 148 m[10] = (a30 * b04 - a31 * b02 + a33 * b00) * invDet; 149 m[11] = (-a20 * b04 + a21 * b02 - a23 * b00) * invDet; 150 m[12] = (-a10 * b09 + a11 * b07 - a12 * b06) * invDet; 151 m[13] = (a00 * b09 - a01 * b07 + a02 * b06) * invDet; 152 m[14] = (-a30 * b03 + a31 * b01 - a32 * b00) * invDet; 153 m[15] = (a20 * b03 - a21 * b01 + a22 * b00) * invDet; 154 return new Matrix(m); 155 } 156 157 /** Set the translation component of this Matrix. */ setTranslation(Vector3 translation)158 public void setTranslation(Vector3 translation) { 159 data[12] = translation.x; 160 data[13] = translation.y; 161 data[14] = translation.z; 162 } 163 164 /** Make this a translation Matrix. */ makeTranslation(Vector3 translation)165 public void makeTranslation(Vector3 translation) { 166 set(IDENTITY_DATA); 167 setTranslation(translation); 168 } 169 170 /** Make this a rotation Matrix. */ makeRotation(Quaternion q)171 public void makeRotation(Quaternion q) { 172 set(IDENTITY_DATA); 173 174 float tx = 2 * q.x; 175 float ty = 2 * q.y; 176 float tz = 2 * q.z; 177 178 float mdsqx = 1 - tx * q.x; 179 float dqxy = ty * q.x; 180 float dsqy = ty * q.y; 181 float dqyz = tz * q.y; 182 float dsqz = tz * q.z; 183 float dqxz = tx * q.z; 184 float dqxw = tx * q.w; 185 float dqyw = ty * q.w; 186 float dqzw = tz * q.w; 187 188 data[0] = 1 - dsqy - dsqz; 189 data[4] = dqxy - dqzw; 190 data[8] = dqxz + dqyw; 191 192 data[1] = dqxy + dqzw; 193 data[5] = mdsqx - dsqz; 194 data[9] = dqyz - dqxw; 195 196 data[2] = dqxz - dqyw; 197 data[6] = dqyz + dqxw; 198 data[10] = mdsqx - dsqy; 199 } 200 201 /** Make this a rigid transform Matrix. */ makeRigidTransform(Pose pose)202 public void makeRigidTransform(Pose pose) { 203 makeRotation(pose.rotation); 204 setTranslation(pose.translation); 205 } 206 207 /** Make this a uniform scale Matrix. */ makeScale(float scale)208 public void makeScale(float scale) { 209 set(IDENTITY_DATA); 210 211 data[0] = scale; 212 data[5] = scale; 213 data[10] = scale; 214 } 215 216 /** Make this a scale Matrix. */ makeScale(Vector3 scale)217 public void makeScale(Vector3 scale) { 218 set(IDENTITY_DATA); 219 220 data[0] = scale.x; 221 data[5] = scale.y; 222 data[10] = scale.z; 223 } 224 225 /** 226 * Make this a translation, rotation and scale Matrix. See {@link 227 * #newTrs(Vector3,Quaternion,Vector3 )}. 228 */ makeTrs(Vector3 translation, Quaternion q, Vector3 scale)229 public void makeTrs(Vector3 translation, Quaternion q, Vector3 scale) { 230 float tx = 2 * q.x; 231 float ty = 2 * q.y; 232 float tz = 2 * q.z; 233 234 float mdsqx = 1 - tx * q.x; 235 float dqxy = ty * q.x; 236 float dsqy = ty * q.y; 237 float dqyz = tz * q.y; 238 float dsqz = tz * q.z; 239 float dqxz = tx * q.z; 240 float dqxw = tx * q.w; 241 float dqyw = ty * q.w; 242 float dqzw = tz * q.w; 243 244 data[0] = (1 - dsqy - dsqz) * scale.x; 245 data[4] = (dqxy - dqzw) * scale.y; 246 data[8] = (dqxz + dqyw) * scale.z; 247 248 data[1] = (dqxy + dqzw) * scale.x; 249 data[5] = (mdsqx - dsqz) * scale.y; 250 data[9] = (dqyz - dqxw) * scale.z; 251 252 data[2] = (dqxz - dqyw) * scale.x; 253 data[6] = (dqyz + dqxw) * scale.y; 254 data[10] = (mdsqx - dsqy) * scale.z; 255 256 data[12] = translation.x; 257 data[13] = translation.y; 258 data[14] = translation.z; 259 260 data[3] = 0.0f; 261 data[7] = 0.0f; 262 data[11] = 0.0f; 263 data[15] = 1.0f; 264 } 265 266 /** Create a new translation Matrix. */ newTranslation(Vector3 translation)267 public static Matrix newTranslation(Vector3 translation) { 268 Matrix m = new Matrix(); 269 m.makeTranslation(translation); 270 return m; 271 } 272 273 /** Create a new rotation Matrix. */ newRotation(Quaternion rotation)274 public static Matrix newRotation(Quaternion rotation) { 275 Matrix m = new Matrix(); 276 m.makeRotation(rotation); 277 return m; 278 } 279 280 /** Create a new rigid transform Matrix. */ newRigidTransform(Pose pose)281 public static Matrix newRigidTransform(Pose pose) { 282 Matrix m = new Matrix(); 283 m.makeRigidTransform(pose); 284 return m; 285 } 286 287 /** Create a new uniform scale Matrix. */ newScale(float scale)288 public static Matrix newScale(float scale) { 289 Matrix m = new Matrix(); 290 m.makeScale(scale); 291 return m; 292 } 293 294 /** Create a new scale Matrix. */ newScale(Vector3 scale)295 public static Matrix newScale(Vector3 scale) { 296 Matrix m = new Matrix(); 297 m.makeScale(scale); 298 return m; 299 } 300 301 /** 302 * Creates a new translation, rotation and scale Matrix. The returned matrix is such that it 303 * first scales objects, then rotates them, and finally translates them. 304 * 305 * <p>See 306 * https://www.opengl-tutorial.org/beginners-tutorials/tutorial-3-matrices/#cumulating-transformations 307 */ newTrs(Vector3 translation, Quaternion rotation, Vector3 scale)308 public static Matrix newTrs(Vector3 translation, Quaternion rotation, Vector3 scale) { 309 Matrix m = new Matrix(); 310 m.makeTrs(translation, rotation, scale); 311 return m; 312 } 313 314 @NonNull 315 @Override toString()316 public String toString() { 317 return String.format(Locale.getDefault(), 318 "% 5.1f, % 5.1f, % 5.1f, % 5.1f / " 319 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f / " 320 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f / " 321 + "% 5.1f, % 5.1f, % 5.1f, % 5.1f", 322 data[0], data[1], data[2], data[3], 323 data[4], data[5], data[6], data[7], 324 data[8], data[9], data[10], data[11], 325 data[12], data[13], data[14], data[15] 326 ); 327 } 328 } 329