Project Ne10
An Open Optimized Software Library Project for the ARM Architecture
Loading...
Searching...
No Matches
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
42ne10_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
63ne10_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
123ne10_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}