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 // trsMatrix.cpp
18 // $Id: trsMatrix.cpp,v 1.9 2011/06/17 13:35:48 mbansal Exp $
19 
20 #include "stdio.h"
21 #include <math.h>
22 #include "trsMatrix.h"
23 
mult33d(double a[3][3],double b[3][3],double c[3][3])24 void mult33d(double a[3][3], double b[3][3], double c[3][3])
25 {
26     a[0][0] = b[0][0]*c[0][0] + b[0][1]*c[1][0] + b[0][2]*c[2][0];
27     a[0][1] = b[0][0]*c[0][1] + b[0][1]*c[1][1] + b[0][2]*c[2][1];
28     a[0][2] = b[0][0]*c[0][2] + b[0][1]*c[1][2] + b[0][2]*c[2][2];
29     a[1][0] = b[1][0]*c[0][0] + b[1][1]*c[1][0] + b[1][2]*c[2][0];
30     a[1][1] = b[1][0]*c[0][1] + b[1][1]*c[1][1] + b[1][2]*c[2][1];
31     a[1][2] = b[1][0]*c[0][2] + b[1][1]*c[1][2] + b[1][2]*c[2][2];
32     a[2][0] = b[2][0]*c[0][0] + b[2][1]*c[1][0] + b[2][2]*c[2][0];
33     a[2][1] = b[2][0]*c[0][1] + b[2][1]*c[1][1] + b[2][2]*c[2][1];
34     a[2][2] = b[2][0]*c[0][2] + b[2][1]*c[1][2] + b[2][2]*c[2][2];
35 }
36 
37 
38 // normProjMat33d
39 // m = input matrix
40 // return: result if successful
normProjMat33d(double m[3][3])41 int normProjMat33d(double m[3][3])
42 {
43     double m22;
44 
45     if(m[2][2] == 0.0)
46         {
47         return 0;
48 }
49 
50     m[0][0] /= m[2][2];
51     m[0][1] /= m[2][2];
52     m[0][2] /= m[2][2];
53     m[1][0] /= m[2][2];
54     m[1][1] /= m[2][2];
55     m[1][2] /= m[2][2];
56     m[2][0] /= m[2][2];
57     m[2][1] /= m[2][2];
58     m[2][2] = 1.0;
59 
60     return 1;
61 }
62 
63 // det33d
64 // m = input matrix
65 // returns: determinant
det33d(const double m[3][3])66 double det33d(const double m[3][3])
67 {
68     double result;
69 
70     result  = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
71     result += m[0][1] * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);
72     result += m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
73 
74     return result;
75 }
76 
77 // inv33d
78 //
inv33d(const double m[3][3],double out[3][3])79 void inv33d(const double m[3][3], double out[3][3])
80 {
81     double det = det33d(m);
82 
83     out[0][0] = (m[1][1]*m[2][2] - m[1][2]*m[2][1]) / det;
84     out[1][0] = (m[1][2]*m[2][0] - m[1][0]*m[2][2]) / det;
85     out[2][0] = (m[1][0]*m[2][1] - m[1][1]*m[2][0]) / det;
86 
87     out[0][1] = (m[0][2]*m[2][1] - m[0][1]*m[2][2]) / det;
88     out[1][1] = (m[0][0]*m[2][2] - m[0][2]*m[2][0]) / det;
89     out[2][1] = (m[0][1]*m[2][0] - m[0][0]*m[2][1]) / det;
90 
91     out[0][2] = (m[0][1]*m[1][2] - m[0][2]*m[1][1]) / det;
92     out[1][2] = (m[0][2]*m[1][0] - m[0][0]*m[1][2]) / det;
93     out[2][2] = (m[0][0]*m[1][1] - m[0][1]*m[1][0]) / det;
94 }
95