1 /**************************************************************************
2 *
3 * Copyright 2008 VMware, Inc.
4 * All Rights Reserved.
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a
7 * copy of this software and associated documentation files (the
8 * "Software"), to deal in the Software without restriction, including
9 * without limitation the rights to use, copy, modify, merge, publish,
10 * distribute, sub license, and/or sell copies of the Software, and to
11 * permit persons to whom the Software is furnished to do so, subject to
12 * the following conditions:
13 *
14 * The above copyright notice and this permission notice (including the
15 * next paragraph) shall be included in all copies or substantial portions
16 * of the Software.
17 *
18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
19 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
20 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
21 * IN NO EVENT SHALL VMWARE AND/OR ITS SUPPLIERS BE LIABLE FOR
22 * ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
23 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
24 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
25 *
26 **************************************************************************/
27
28
29
30 #include "pipe/p_config.h"
31 #include "util/u_math.h"
32 #include "util/u_cpu_detect.h"
33
34 #if defined(PIPE_ARCH_SSE)
35 #include <xmmintrin.h>
36 /* This is defined in pmmintrin.h, but it can only be included when -msse3 is
37 * used, so just define it here to avoid further. */
38 #ifndef _MM_DENORMALS_ZERO_MASK
39 #define _MM_DENORMALS_ZERO_MASK 0x0040
40 #endif
41 #endif
42
43
44 /** log2(x), for x in [1.0, 2.0) */
45 float log2_table[LOG2_TABLE_SIZE];
46
47
48 static void
init_log2_table(void)49 init_log2_table(void)
50 {
51 unsigned i;
52 for (i = 0; i < LOG2_TABLE_SIZE; i++)
53 log2_table[i] = (float) log2(1.0 + i * (1.0 / LOG2_TABLE_SCALE));
54 }
55
56
57 /**
58 * One time init for math utilities.
59 */
60 void
util_init_math(void)61 util_init_math(void)
62 {
63 static bool initialized = false;
64 if (!initialized) {
65 init_log2_table();
66 initialized = true;
67 }
68 }
69
70 /**
71 * Fetches the contents of the fpstate (mxcsr on x86) register.
72 *
73 * On platforms without support for it just returns 0.
74 */
75 unsigned
util_fpstate_get(void)76 util_fpstate_get(void)
77 {
78 unsigned mxcsr = 0;
79
80 #if defined(PIPE_ARCH_SSE)
81 if (util_get_cpu_caps()->has_sse) {
82 mxcsr = _mm_getcsr();
83 }
84 #endif
85
86 return mxcsr;
87 }
88
89 /**
90 * Make sure that the fp treats the denormalized floating
91 * point numbers as zero.
92 *
93 * This is the behavior required by D3D10. OpenGL doesn't care.
94 */
95 unsigned
util_fpstate_set_denorms_to_zero(unsigned current_mxcsr)96 util_fpstate_set_denorms_to_zero(unsigned current_mxcsr)
97 {
98 #if defined(PIPE_ARCH_SSE)
99 if (util_get_cpu_caps()->has_sse) {
100 /* Enable flush to zero mode */
101 current_mxcsr |= _MM_FLUSH_ZERO_MASK;
102 if (util_get_cpu_caps()->has_daz) {
103 /* Enable denormals are zero mode */
104 current_mxcsr |= _MM_DENORMALS_ZERO_MASK;
105 }
106 util_fpstate_set(current_mxcsr);
107 }
108 #endif
109 return current_mxcsr;
110 }
111
112 /**
113 * Set the state of the fpstate (mxcsr on x86) register.
114 *
115 * On platforms without support for it's a noop.
116 */
117 void
util_fpstate_set(unsigned mxcsr)118 util_fpstate_set(unsigned mxcsr)
119 {
120 #if defined(PIPE_ARCH_SSE)
121 if (util_get_cpu_caps()->has_sse) {
122 _mm_setcsr(mxcsr);
123 }
124 #endif
125 }
126
127 /**
128 * Compute inverse of 4x4 matrix.
129 *
130 * \return false if the source matrix is singular.
131 *
132 * \author
133 * Code contributed by Jacques Leroy jle@star.be
134 *
135 * Calculates the inverse matrix by performing the gaussian matrix reduction
136 * with partial pivoting followed by back/substitution with the loops manually
137 * unrolled.
138 */
139 bool
util_invert_mat4x4(float * out,const float * m)140 util_invert_mat4x4(float *out, const float *m)
141 {
142 float wtmp[4][8];
143 float m0, m1, m2, m3, s;
144 float *r0, *r1, *r2, *r3;
145
146 #define MAT(m, r, c) (m)[(c)*4 + (r)]
147 #define SWAP_ROWS(a, b) \
148 { \
149 float *_tmp = a; \
150 (a) = (b); \
151 (b) = _tmp; \
152 }
153
154 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
155
156 r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
157 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
158
159 r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
160 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
161
162 r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
163 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
164
165 r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1), r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
166 r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
167
168 /* choose pivot - or die */
169 if (fabsf(r3[0]) > fabsf(r2[0]))
170 SWAP_ROWS(r3, r2);
171 if (fabsf(r2[0]) > fabsf(r1[0]))
172 SWAP_ROWS(r2, r1);
173 if (fabsf(r1[0]) > fabsf(r0[0]))
174 SWAP_ROWS(r1, r0);
175 if (0.0F == r0[0])
176 return false;
177
178 /* eliminate first variable */
179 m1 = r1[0] / r0[0];
180 m2 = r2[0] / r0[0];
181 m3 = r3[0] / r0[0];
182 s = r0[1];
183 r1[1] -= m1 * s;
184 r2[1] -= m2 * s;
185 r3[1] -= m3 * s;
186 s = r0[2];
187 r1[2] -= m1 * s;
188 r2[2] -= m2 * s;
189 r3[2] -= m3 * s;
190 s = r0[3];
191 r1[3] -= m1 * s;
192 r2[3] -= m2 * s;
193 r3[3] -= m3 * s;
194 s = r0[4];
195 if (s != 0.0F) {
196 r1[4] -= m1 * s;
197 r2[4] -= m2 * s;
198 r3[4] -= m3 * s;
199 }
200 s = r0[5];
201 if (s != 0.0F) {
202 r1[5] -= m1 * s;
203 r2[5] -= m2 * s;
204 r3[5] -= m3 * s;
205 }
206 s = r0[6];
207 if (s != 0.0F) {
208 r1[6] -= m1 * s;
209 r2[6] -= m2 * s;
210 r3[6] -= m3 * s;
211 }
212 s = r0[7];
213 if (s != 0.0F) {
214 r1[7] -= m1 * s;
215 r2[7] -= m2 * s;
216 r3[7] -= m3 * s;
217 }
218
219 /* choose pivot - or die */
220 if (fabsf(r3[1]) > fabsf(r2[1]))
221 SWAP_ROWS(r3, r2);
222 if (fabsf(r2[1]) > fabsf(r1[1]))
223 SWAP_ROWS(r2, r1);
224 if (0.0F == r1[1])
225 return false;
226
227 /* eliminate second variable */
228 m2 = r2[1] / r1[1];
229 m3 = r3[1] / r1[1];
230 r2[2] -= m2 * r1[2];
231 r3[2] -= m3 * r1[2];
232 r2[3] -= m2 * r1[3];
233 r3[3] -= m3 * r1[3];
234 s = r1[4];
235 if (0.0F != s) {
236 r2[4] -= m2 * s;
237 r3[4] -= m3 * s;
238 }
239 s = r1[5];
240 if (0.0F != s) {
241 r2[5] -= m2 * s;
242 r3[5] -= m3 * s;
243 }
244 s = r1[6];
245 if (0.0F != s) {
246 r2[6] -= m2 * s;
247 r3[6] -= m3 * s;
248 }
249 s = r1[7];
250 if (0.0F != s) {
251 r2[7] -= m2 * s;
252 r3[7] -= m3 * s;
253 }
254
255 /* choose pivot - or die */
256 if (fabsf(r3[2]) > fabsf(r2[2]))
257 SWAP_ROWS(r3, r2);
258 if (0.0F == r2[2])
259 return false;
260
261 /* eliminate third variable */
262 m3 = r3[2] / r2[2];
263 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
264 r3[7] -= m3 * r2[7];
265
266 /* last check */
267 if (0.0F == r3[3])
268 return false;
269
270 s = 1.0F / r3[3]; /* now back substitute row 3 */
271 r3[4] *= s;
272 r3[5] *= s;
273 r3[6] *= s;
274 r3[7] *= s;
275
276 m2 = r2[3]; /* now back substitute row 2 */
277 s = 1.0F / r2[2];
278 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
279 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
280 m1 = r1[3];
281 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
282 m0 = r0[3];
283 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
284
285 m1 = r1[2]; /* now back substitute row 1 */
286 s = 1.0F / r1[1];
287 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
288 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
289 m0 = r0[2];
290 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
291
292 m0 = r0[1]; /* now back substitute row 0 */
293 s = 1.0F / r0[0];
294 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
295 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
296
297 MAT(out, 0, 0) = r0[4];
298 MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
299 MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
300 MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
301 MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
302 MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
303 MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
304 MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
305 MAT(out, 3, 3) = r3[7];
306
307 #undef MAT
308 #undef SWAP_ROWS
309
310 return true;
311 }
312