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