1e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* 2e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Copyright (C) 2011 The Android Open Source Project 3e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 4e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Licensed under the Apache License, Version 2.0 (the "License"); 5e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * you may not use this file except in compliance with the License. 6e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * You may obtain a copy of the License at 7e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 8e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * http://www.apache.org/licenses/LICENSE-2.0 9e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 10e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Unless required by applicable law or agreed to in writing, software 11e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * distributed under the License is distributed on an "AS IS" BASIS, 12e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * See the License for the specific language governing permissions and 14e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * limitations under the License. 15e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 16e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 17e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/////////////////////////////////////////////////// 18e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// Matrixutils.h 19e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// $Id: MatrixUtils.h,v 1.5 2011/05/16 15:33:06 mbansal Exp $ 20e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 21e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 22e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#ifndef MATRIX_UTILS_H 23e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define MATRIX_UTILS_H 24e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 25e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* Simple class for 3x3 matrix, mainly used to convert from 9x1 26e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * to 3x3 27e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 28e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenclass Matrix33 { 29e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenpublic: 30e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 31e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 32e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Empty constructor 33e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 34e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Matrix33() { 35e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen initialize(); 36e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 37e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 38e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 39e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Constructor with identity initialization 40e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Arguments: 41e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * identity: Specifies wether to initialize matrix to 42e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * identity or zeros 43e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 44e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Matrix33(bool identity) { 45e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen initialize(identity); 46e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 47e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 48e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 49e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Initialize to identity matrix 50e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 51e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen void initialize(bool identity = false) { 52e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[0][1] = mat[0][2] = mat[1][0] = mat[1][2] = mat[2][0] = mat[2][1] = 0.0; 53e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if (identity) { 54e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[0][0] = mat[1][1] = mat[2][2] = 1.0; 55e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } else { 56e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[0][0] = mat[1][1] = mat[2][2] = 0.0; 57e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 58e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 59e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 60e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 61e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Conver ta 9x1 matrix to a 3x3 matrix 62e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 63e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen static void convert9to33(double out[3][3], double in[9]) { 64e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][0] = in[0]; 65e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][1] = in[1]; 66e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][2] = in[2]; 67e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 68e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][0] = in[3]; 69e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][1] = in[4]; 70e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][2] = in[5]; 71e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 72e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][0] = in[6]; 73e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][1] = in[7]; 74e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][2] = in[8]; 75e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 76e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 77e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 78e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /* Matrix data */ 79e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double mat[3][3]; 80e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 81e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen}; 82e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 83e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* Simple class for 9x1 matrix, mainly used to convert from 3x3 84e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * to 9x1 85e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 86e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenclass Matrix9 { 87e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenpublic: 88e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 89e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 90e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Empty constructor 91e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 92e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Matrix9() { 93e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen initialize(); 94e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 95e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 96e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 97e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Constructor with identity initialization 98e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Arguments: 99e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * identity: Specifies wether to initialize matrix to 100e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * identity or zeros 101e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 102e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen Matrix9(bool identity) { 103e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen initialize(identity); 104e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 105e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 106e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 107e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Initialize to identity matrix 108e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 109e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen void initialize(bool identity = false) { 110e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[1] = mat[2] = mat[3] = mat[5] = mat[6] = mat[7] = 0.0; 111e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if (identity) { 112e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[0] = mat[4] = mat[8] = 1.0; 113e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } else { 114e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen mat[0] = mat[4] = mat[8] = 0.0; 115e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 116e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 117e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 118e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /** 119e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Conver ta 3x3 matrix to a 9x1 matrix 120e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 121e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen static void convert33to9(double out[9], double in[3][3]) { 122e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0] = in[0][0]; 123e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1] = in[0][1]; 124e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2] = in[0][2]; 125e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 126e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[3] = in[1][0]; 127e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[4] = in[1][1]; 128e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[5] = in[1][2]; 129e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 130e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[6] = in[2][0]; 131e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[7] = in[2][1]; 132e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[8] = in[2][2]; 133e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 134e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 135e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 136e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /* Matrix data */ 137e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double mat[9]; 138e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 139e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen}; 140e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 141e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#endif 142