1 /*
2  * Copyright (C) 2011 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 
17 ///////////////////////////////////////////////////
18 // Matrixutils.h
19 // $Id: MatrixUtils.h,v 1.5 2011/05/16 15:33:06 mbansal Exp $
20 
21 
22 #ifndef MATRIX_UTILS_H
23 #define MATRIX_UTILS_H
24 
25 /* Simple class for 3x3 matrix, mainly used to convert from 9x1
26  * to 3x3
27  */
28 class Matrix33 {
29 public:
30 
31   /**
32    *  Empty constructor
33    */
Matrix33()34   Matrix33() {
35     initialize();
36   }
37 
38   /**
39    *  Constructor with identity initialization
40    *  Arguments:
41    *     identity: Specifies wether to initialize matrix to
42    *     identity or zeros
43    */
Matrix33(bool identity)44   Matrix33(bool identity) {
45     initialize(identity);
46   }
47 
48   /**
49    *  Initialize to identity matrix
50    */
51   void initialize(bool identity = false) {
52     mat[0][1] = mat[0][2] = mat[1][0] = mat[1][2] = mat[2][0] = mat[2][1] = 0.0;
53     if (identity) {
54       mat[0][0] = mat[1][1] = mat[2][2] = 1.0;
55     } else {
56       mat[0][0] = mat[1][1] = mat[2][2] = 0.0;
57     }
58   }
59 
60   /**
61    *  Conver ta 9x1 matrix to a 3x3 matrix
62    */
convert9to33(double out[3][3],double in[9])63   static void convert9to33(double out[3][3], double in[9]) {
64     out[0][0] = in[0];
65     out[0][1] = in[1];
66     out[0][2] = in[2];
67 
68     out[1][0] = in[3];
69     out[1][1] = in[4];
70     out[1][2] = in[5];
71 
72     out[2][0] = in[6];
73     out[2][1] = in[7];
74     out[2][2] = in[8];
75 
76   }
77 
78   /* Matrix data */
79   double mat[3][3];
80 
81 };
82 
83 /* Simple class for 9x1 matrix, mainly used to convert from 3x3
84  * to 9x1
85  */
86 class Matrix9 {
87 public:
88 
89   /**
90    *  Empty constructor
91    */
Matrix9()92   Matrix9() {
93     initialize();
94   }
95 
96   /**
97    *  Constructor with identity initialization
98    *  Arguments:
99    *     identity: Specifies wether to initialize matrix to
100    *     identity or zeros
101    */
Matrix9(bool identity)102   Matrix9(bool identity) {
103     initialize(identity);
104   }
105 
106   /**
107    *  Initialize to identity matrix
108    */
109   void initialize(bool identity = false) {
110     mat[1] = mat[2] = mat[3] = mat[5] = mat[6] = mat[7] = 0.0;
111     if (identity) {
112       mat[0] = mat[4] = mat[8] = 1.0;
113     } else {
114       mat[0] = mat[4] = mat[8] = 0.0;
115     }
116   }
117 
118   /**
119    *  Conver ta 3x3 matrix to a 9x1 matrix
120    */
convert33to9(double out[9],double in[3][3])121   static void convert33to9(double out[9], double in[3][3]) {
122     out[0] = in[0][0];
123     out[1] = in[0][1];
124     out[2] = in[0][2];
125 
126     out[3] = in[1][0];
127     out[4] = in[1][1];
128     out[5] = in[1][2];
129 
130     out[6] = in[2][0];
131     out[7] = in[2][1];
132     out[8] = in[2][2];
133 
134   }
135 
136   /* Matrix data */
137   double mat[9];
138 
139 };
140 
141 #endif
142