Project Ne10
An Open Optimized Software Library Project for the ARM Architecture
NE10_invmat.c
1 /*
2  * Copyright 2011-15 ARM Limited and Contributors.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of ARM Limited nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY ARM LIMITED AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL ARM LIMITED AND CONTRIBUTORS BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 /*
29  * NE10 Library : math/NE10_invmat.c
30  */
31 
32 #include "NE10_types.h"
33 #include "macros.h"
34 #include "NE10_detmat.c.h"
35 #include <math.h>
36 
37 #include <assert.h>
38 
39 // This macro is used to determine floating point values that are small enough to be consiedered nearly zero
40 #define IS_FLOAT_NEAR_ZERO(x) ( ((fabs(x))<(1e-12)) ? 1 : 0 )
41 
42 ne10_result_t ne10_invmat_2x2f_c (ne10_mat2x2f_t * dst, ne10_mat2x2f_t * src, ne10_uint32_t count)
43 {
44  ne10_float32_t det = 0.0f;
45 
46  NE10_DETMAT_OPERATION_X_C
47  (
48  det = DET2x2 (&src[ itr ]);
49 
50  if (1 == IS_FLOAT_NEAR_ZERO (det))
51  {
52  det = 1.0f;
53  }
54 
55  det = 1.0f / det;
56  dst[ itr ].c1.r1 = det * src[ itr ].c2.r2;
57  dst[ itr ].c1.r2 = -1 * det * src[ itr ].c1.r2;
58  dst[ itr ].c2.r1 = -1 * det * src[ itr ].c2.r1;
59  dst[ itr ].c2.r2 = det * src[ itr ].c1.r1;
60  );
61 }
62 
63 ne10_result_t ne10_invmat_3x3f_c (ne10_mat3x3f_t * dst, ne10_mat3x3f_t * src, ne10_uint32_t count)
64 {
65 #define aa (src[ itr ].c1.r1)
66 #define bb (src[ itr ].c1.r2)
67 #define cc (src[ itr ].c1.r3)
68 #define dd (src[ itr ].c2.r1)
69 #define ee (src[ itr ].c2.r2)
70 #define ff (src[ itr ].c2.r3)
71 #define gg (src[ itr ].c3.r1)
72 #define hh (src[ itr ].c3.r2)
73 #define ii (src[ itr ].c3.r3)
74 
75  ne10_float32_t det = 0.0f;
76  ne10_mat2x2f_t A, B, C, D, E, F, G, H, I;
77 
78  NE10_DETMAT_OPERATION_X_C
79  (
80  det = DET3x3 (&src[ itr ]);
81 
82  if (1 == IS_FLOAT_NEAR_ZERO (det))
83  {
84  det = 1.0f;
85  }
86  det = 1.0f / det;
87 
88  // Calculate the coefficients
89  createColumnMajorMatrix2x2 (&A, ee, ff, hh, ii);
90  createColumnMajorMatrix2x2 (&B, dd, ff, gg, ii);
91  createColumnMajorMatrix2x2 (&C, dd, ee, gg, hh);
92  createColumnMajorMatrix2x2 (&D, bb, cc, hh, ii);
93  createColumnMajorMatrix2x2 (&E, aa, cc, gg, ii);
94  createColumnMajorMatrix2x2 (&F, aa, bb, gg, hh);
95  createColumnMajorMatrix2x2 (&G, bb, cc, ee, ff);
96  createColumnMajorMatrix2x2 (&H, aa, cc, dd, ff);
97  createColumnMajorMatrix2x2 (&I, aa, bb, dd, ee);
98 
99  dst[ itr ].c1.r1 = det * DET2x2 (&A);
100  dst[ itr ].c1.r2 = -1.0f * det * DET2x2 (&D);
101  dst[ itr ].c1.r3 = det * DET2x2 (&G);
102 
103  dst[ itr ].c2.r1 = -1.0f * det * DET2x2 (&B);
104  dst[ itr ].c2.r2 = det * DET2x2 (&E);
105  dst[ itr ].c2.r3 = -1.0f * det * DET2x2 (&H);
106 
107  dst[ itr ].c3.r1 = det * DET2x2 (&C);
108  dst[ itr ].c3.r2 = -1.0f * det * DET2x2 (&F);
109  dst[ itr ].c3.r3 = det * DET2x2 (&I);
110  );
111 
112 #undef aa
113 #undef bb
114 #undef cc
115 #undef dd
116 #undef ee
117 #undef ff
118 #undef gg
119 #undef hh
120 #undef ii
121 }
122 
123 ne10_result_t ne10_invmat_4x4f_c (ne10_mat4x4f_t * dst, ne10_mat4x4f_t * src, ne10_uint32_t count)
124 {
125 #define aa (src[ itr ].c1.r1)
126 #define bb (src[ itr ].c1.r2)
127 #define cc (src[ itr ].c1.r3)
128 #define dd (src[ itr ].c1.r4)
129 
130 #define ee (src[ itr ].c2.r1)
131 #define ff (src[ itr ].c2.r2)
132 #define gg (src[ itr ].c2.r3)
133 #define hh (src[ itr ].c2.r4)
134 
135 #define ii (src[ itr ].c3.r1)
136 #define jj (src[ itr ].c3.r2)
137 #define kk (src[ itr ].c3.r3)
138 #define ll (src[ itr ].c3.r4)
139 
140 #define mm (src[ itr ].c4.r1)
141 #define nn (src[ itr ].c4.r2)
142 #define oo (src[ itr ].c4.r3)
143 #define pp (src[ itr ].c4.r4)
144 
145  ne10_float32_t det = 0.0f;
146  ne10_mat3x3f_t A, B, C, D, E, F, G, H, I, J, K, L, M, N, O, P;
147 
148  NE10_DETMAT_OPERATION_X_C
149  (
150  det = DET4x4 (&src[ itr ]);
151 
152  if (1 == IS_FLOAT_NEAR_ZERO (det))
153  {
154  det = 1.0f;
155  }
156  det = 1.0f / det;
157 
158  // Calculate the coefficients
159  createColumnMajorMatrix3x3 (&A, ff, gg, hh, jj, kk, ll, nn, oo, pp);
160  createColumnMajorMatrix3x3 (&B, ee, gg, hh, ii, kk, ll, mm, oo, pp);
161  createColumnMajorMatrix3x3 (&C, ee, ff, hh, ii, jj, ll, mm, nn, pp);
162  createColumnMajorMatrix3x3 (&D, ee, ff, gg, ii, jj, kk, mm, nn, oo);
163  createColumnMajorMatrix3x3 (&E, bb, cc, dd, jj, kk, ll, nn, oo, pp);
164  createColumnMajorMatrix3x3 (&F, aa, cc, dd, ii, kk, ll, mm, oo, pp);
165  createColumnMajorMatrix3x3 (&G, aa, bb, dd, ii, jj, ll, mm, nn, pp);
166  createColumnMajorMatrix3x3 (&H, aa, bb, cc, ii, jj, kk, mm, nn, oo);
167  createColumnMajorMatrix3x3 (&I, bb, cc, dd, ff, gg, hh, nn, oo, pp);
168  createColumnMajorMatrix3x3 (&J, aa, cc, dd, ee, gg, hh, mm, oo, pp);
169  createColumnMajorMatrix3x3 (&K, aa, bb, dd, ee, ff, hh, mm, nn, pp);
170  createColumnMajorMatrix3x3 (&L, aa, bb, cc, ee, ff, gg, mm, nn, oo);
171  createColumnMajorMatrix3x3 (&M, bb, cc, dd, ff, gg, hh, jj, kk, ll);
172  createColumnMajorMatrix3x3 (&N, aa, cc, dd, ee, gg, hh, ii, kk, ll);
173  createColumnMajorMatrix3x3 (&O, aa, bb, dd, ee, ff, hh, ii, jj, ll);
174  createColumnMajorMatrix3x3 (&P, aa, bb, cc, ee, ff, gg, ii, jj, kk);
175 
176 
177  dst[ itr ].c1.r1 = det * DET3x3 (&A);
178  dst[ itr ].c1.r2 = -1.0f * det * DET3x3 (&E);
179  dst[ itr ].c1.r3 = det * DET3x3 (&I);
180  dst[ itr ].c1.r4 = -1.0f * det * DET3x3 (&M);
181 
182  dst[ itr ].c2.r1 = -1.0f * det * DET3x3 (&B);
183  dst[ itr ].c2.r2 = det * DET3x3 (&F);
184  dst[ itr ].c2.r3 = -1.0f * det * DET3x3 (&J);
185  dst[ itr ].c2.r4 = det * DET3x3 (&N);
186 
187  dst[ itr ].c3.r1 = det * DET3x3 (&C);
188  dst[ itr ].c3.r2 = -1.0f * det * DET3x3 (&G);
189  dst[ itr ].c3.r3 = det * DET3x3 (&K);
190  dst[ itr ].c3.r4 = -1.0f * det * DET3x3 (&O);
191 
192  dst[ itr ].c4.r1 = -1.0f * det * DET3x3 (&D);
193  dst[ itr ].c4.r2 = det * DET3x3 (&H);
194  dst[ itr ].c4.r3 = -1.0f * det * DET3x3 (&L);
195  dst[ itr ].c4.r4 = det * DET3x3 (&P);
196  );
197 
198 #undef aa
199 #undef bb
200 #undef cc
201 #undef dd
202 #undef ee
203 #undef ff
204 #undef gg
205 #undef hh
206 #undef ii
207 #undef jj
208 #undef kk
209 #undef ll
210 #undef mm
211 #undef nn
212 #undef oo
213 #undef pp
214 }