Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/sse/3dmath.h

raw
1 /*
2 * Copyright (C) 2025 Linux Studio Plugins Project <https://lsp-plug.in/>
3 * (C) 2025 Vladimir Sadovnikov <[email protected]>
4 *
5 * This file is part of lsp-dsp-lib
6 * Created on: 31 мар. 2020 г.
7 *
8 * lsp-dsp-lib is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU Lesser General Public License as published by
10 * the Free Software Foundation, either version 3 of the License, or
11 * any later version.
12 *
13 * lsp-dsp-lib is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public License
19 * along with lsp-dsp-lib. If not, see <https://www.gnu.org/licenses/>.
20 */
21
22 #ifndef PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_
23 #define PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_SSE_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_SSE_IMPL */
28
29 /* Input:
30 * x0 = a0 a1 a2 a3
31 * x1 = b0 b1 b2 b3
32 *
33 * Output:
34 * x0 = b0 b1 b2 b3
35 * x1 = a0 a1 a2 a3
36 */
37 #define VECTOR_XCHG(x0, x1) \
38 __ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = b ^ a */ \
39 __ASM_EMIT("xorps %" x1 ", %" x0) /* xmm0 = a ^ ( b ^ a) = b */ \
40 __ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = (b ^ a) ^ b = a */
41
42 /* Input:
43 * x0 = a0 a1 a2 ?
44 * x1 = b0 b1 b2 ?
45 * x2 = c0 c1 c2 ?
46 *
47 * Output:
48 * x0 = a0 a1 a2 ?
49 * x1 = b0 b1 b2 ?
50 * x3 = c0 c1 c2 ?
51 */
52 #define MAT3_TRANSPOSE(x0, x1, x2, x3) \
53 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = a0 a1 a2 ? */ \
54 __ASM_EMIT("unpcklps %" x1 ", %" x0) /* xmm0 = a0 b0 a1 b1 */ \
55 __ASM_EMIT("unpckhps %" x1 ", %" x3) /* xmm3 = a2 b2 ? ? */ \
56 __ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = a1 b1 b2 ? */ \
57 __ASM_EMIT("movlhps %" x2 ", %" x0) /* xmm0 = a0 b0 c0 c1 ? */ \
58 __ASM_EMIT("shufps $0x54, %" x2 ", %" x1) /* xmm1 = a1 b1 c1 c1 */ \
59 __ASM_EMIT("shufps $0xa4, %" x2 ", %" x3) /* xmm3 = a2 b2 c2 c2 */
60
61 /* 3x scalar multiplication
62 * Input:
63 * x0 = vector1 [dx dy dz ? ]
64 * x1 = vector2 [dx dy dz ? ]
65 * x2 = vector3 [dx dy dz ? ]
66 *
67 * Output:
68 * x0 = vector1 * vector2 [ S0 ? ? ? ]
69 * x1 = vector2 * vector3 [ S1 ? ? ? ]
70 * x2 = vector3 * vector1 [ S2 ? ? ? ]
71 */
72 #define SCALAR_MUL3(x0, x1, x2, x3) \
73 __ASM_EMIT("movaps %" x0 ", %" x3) \
74 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 dw0*dw1 */ \
75 __ASM_EMIT("mulps %" x2 ", %" x1) /* xmm1 = dx1*dx2 dy1*dy2 dz1*dz2 dw1*dw2 */ \
76 __ASM_EMIT("mulps %" x3 ", %" x2) /* xmm2 = dx2*dx0 dy2*dy0 dz2*dz0 dw2*dw0 */ \
77 MAT3_TRANSPOSE(x0, x1, x2, x3) \
78 __ASM_EMIT("addps %" x1 ", %" x0) \
79 __ASM_EMIT("addps %" x3 ", %" x0) /* xmm0 = S0 S1 S2 ? */ \
80 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = r2 ? ? ? */ \
81 __ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = r0 r0 r1 r1 */ \
82 __ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = r1 r1 ? ? */
83
84 /* 1x scalar multiplication of 3 coordinates
85 * Input:
86 * x0 = vector1 [dx dy dz ? ]
87 * x1 = vector2 [dx dy dz ? ]
88 * x2 = temporary
89 *
90 * Output:
91 * x0 = result (scalar)
92 */
93 #define VECTOR_DPPS3(x0, x1, x2) \
94 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 ? */ \
95 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = dz0*dz1 ? ? ? */ \
96 __ASM_EMIT("addss %" x0 ", %" x2) /* xmm2 = dz0*dz1 + dx0*dx1 */ \
97 __ASM_EMIT("shufps $0x55, %" x0 ", %" x0) /* xmm0 = dy0*dy1 dy0*dy1 dy0*dy1 dy0*dy1 */ \
98 __ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = dz0*dz1 + dx0*dx1 + dy0*dy1 */ \
99
100 /* 1x scalar multiplication of 4 coordinates
101 * Input:
102 * x0 = vector1 [dx dy dz dw ]
103 * x1 = vector2 [dx dy dz dw ]
104 * x2 = temporary
105 *
106 * Output:
107 * x0 = result (scalar)
108 */
109 #define VECTOR_DPPS4(x0, x1, x2) \
110 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 dw0*dw1 = A B C D */ \
111 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = C D */ \
112 __ASM_EMIT("addps %" x2 ", %" x0) /* xmm0 = A+C B+D */ \
113 __ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = A+C A+C B+D B+D */ \
114 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = B+D B+D */ \
115 __ASM_EMIT("addps %" x2 ", %" x0) /* xmm0 = A+C+B+D */
116
117 /* 3x vector multiplication
118 * Input:
119 * x0 = vector1 [dx dy dz 0 ]
120 * x1 = vector2 [dx dy dz 0 ]
121 * x2 = vector3 [dx dy dz 0 ]
122 *
123 * Output:
124 * x0 = vector1 * vector2 [ vz vx vy 0 ]
125 * x1 = vector2 * vector3 [ vz vx vy 0 ]
126 * x2 = vector3 * vector1 [ vz vx vy 0 ]
127 */
128 #define VECTOR_MUL3(x0, x1, x2, x3, x4, x5, x6, x7) \
129 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = dx0 dy0 dz0 0 */ \
130 __ASM_EMIT("movaps %" x1 ", %" x4) /* xmm4 = dx1 dy1 dz1 0 */ \
131 __ASM_EMIT("movaps %" x2 ", %" x5) /* xmm5 = dx2 dy2 dz2 0 */ \
132 __ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy0 dz0 dx0 0 */ \
133 __ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = dy1 dz1 dx1 0 */ \
134 __ASM_EMIT("shufps $0xc9, %" x5 ", %" x5) /* xmm5 = dy2 dz2 dx2 0 */ \
135 __ASM_EMIT("movaps %" x0 ", %" x6) /* xmm6 = dx0 dy0 dz0 0 */ \
136 __ASM_EMIT("movaps %" x3 ", %" x7) /* xmm7 = dy0 dz0 dx0 0 */ \
137 \
138 __ASM_EMIT("mulps %" x4 ", %" x0) /* xmm0 = dx0*dy1 dy0*dz1 dz0*dx1 0 */ \
139 __ASM_EMIT("mulps %" x1 ", %" x3) /* xmm3 = dy0*dx1 dz0*dy1 dx0*dz1 0 */ \
140 __ASM_EMIT("mulps %" x5 ", %" x1) /* xmm1 = dx1*dy2 dy1*dz2 dz1*dx2 0 */ \
141 __ASM_EMIT("mulps %" x2 ", %" x4) /* xmm4 = dy1*dx2 dz1*dy2 dx1*dz2 0 */ \
142 __ASM_EMIT("mulps %" x7 ", %" x2) /* xmm2 = dx2*dy0 dy2*dz0 dz2*dx0 0 */ \
143 __ASM_EMIT("mulps %" x6 ", %" x5) /* xmm5 = dy2*dx0 dz2*dy0 dx2*dz0 0 */ \
144 __ASM_EMIT("subps %" x3 ", %" x0) /* xmm0 = nz0 nx0 ny0 0 */ \
145 __ASM_EMIT("subps %" x4 ", %" x1) /* xmm1 = nz1 nx1 ny1 0 */ \
146 __ASM_EMIT("subps %" x5 ", %" x2) /* xmm2 = nz2 nx2 ny2 0 */
147
148 /* 1x vector multiplication
149 * Input:
150 * x0 = vector1 [dx dy dz 0 ]
151 * x1 = vector2 [dx dy dz 0 ]
152 * x2 = temporary
153 * x3 = temporary
154 *
155 * Output:
156 * x0 = vector1 * vector2 [ vz vx vy 0 ]
157 */
158 #define VECTOR_MUL(x0, x1, x2, x3) \
159 __ASM_EMIT("movaps %" x0 ", %" x2) /* xmm2 = dx1 dy1 dz1 0 */ \
160 __ASM_EMIT("movaps %" x1 ", %" x3) /* xmm3 = dx2 dy2 dz2 0 */ \
161 __ASM_EMIT("shufps $0xc9, %" x2 ", %" x2) /* xmm2 = dy1 dz1 dx1 0 */ \
162 __ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy2 dz2 dx2 0 */ \
163 __ASM_EMIT("mulps %" x2 ", %" x1) /* xmm1 = dx2*dy1 dy2*dz1 dz2*dx1 dw2*dw2 */ \
164 __ASM_EMIT("mulps %" x3 ", %" x0) /* xmm0 = dx1*dy2 dy1*dz2 dz1*dx2 dw1*dw1 */ \
165 __ASM_EMIT("subps %" x1 ", %" x0) /* xmm0 = dx1*dy2-dx2*dy1 dy1*dz2-dy2*dz1 dz1*dx2-dz2*dx1 dw1*dw1-dw2*dw2 = NY NZ NX NW */
166
167 /* 1x matrix transpose
168 * Input:
169 * x0 = row 0 (a1 a2 a3 a4)
170 * x1 = row 1 (b1 b2 b3 b4)
171 * x2 = row 2 (c1 c2 c3 c4)
172 * x3 = row 3 (d1 d2 d3 d4)
173 * x4 = temporary
174 *
175 * Output:
176 * x0 = row 0 (a1 b1 c1 d1)
177 * x1 = row 1 (a2 b2 c2 d2)
178 * x2 = row 2 (a3 b3 c3 d3)
179 * x3 = row 3 (a4 b4 c4 d4)
180 */
181 #define MAT4_TRANSPOSE(x0, x1, x2, x3, x4) \
182 __ASM_EMIT("movaps %" x2 ", %" x4) /* xmm4 = c1 c2 c3 c4 */ \
183 __ASM_EMIT("unpcklps %" x3 ", %" x2) /* xmm2 = c1 d1 c2 d2 */ \
184 __ASM_EMIT("unpckhps %" x3 ", %" x4) /* xmm4 = c3 d3 c4 d4 */ \
185 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = a1 a2 a3 a4 */ \
186 __ASM_EMIT("unpcklps %" x1 ", %" x0) /* xmm0 = a1 b1 a2 b2 */ \
187 __ASM_EMIT("unpckhps %" x1 ", %" x3) /* xmm3 = a3 b3 a4 b4 */ \
188 __ASM_EMIT("movaps %" x2 ", %" x1) /* xmm1 = c1 d1 c2 d2 */ \
189 __ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = a2 b2 c2 d2 */ \
190 __ASM_EMIT("movlhps %" x2 ", %" x0) /* xmm0 = a1 b1 c1 d1 */ \
191 __ASM_EMIT("movaps %" x3 ", %" x2) /* xmm2 = a3 b3 a4 b4 */ \
192 __ASM_EMIT("movaps %" x4 ", %" x3) /* xmm3 = c3 d3 c4 d4 */ \
193 __ASM_EMIT("movhlps %" x2 ", %" x3) /* xmm3 = a4 b4 c4 d4 */ \
194 __ASM_EMIT("movlhps %" x4 ", %" x2) /* xmm2 = a3 b3 c3 d3 */
195
196 #define MATRIX_LOAD(ptr, x0, x1, x2, x3) \
197 __ASM_EMIT("movups 0x00(%[" ptr "]), %" x0 ) \
198 __ASM_EMIT("movups 0x10(%[" ptr "]), %" x1 ) \
199 __ASM_EMIT("movups 0x20(%[" ptr "]), %" x2 ) \
200 __ASM_EMIT("movups 0x30(%[" ptr "]), %" x3 )
201
202 #define MATRIX_STORE(ptr, x0, x1, x2, x3) \
203 __ASM_EMIT("movups %" x0 ", 0x00(%[" ptr "])") \
204 __ASM_EMIT("movups %" x1 ", 0x10(%[" ptr "])") \
205 __ASM_EMIT("movups %" x2 ", 0x20(%[" ptr "])") \
206 __ASM_EMIT("movups %" x3 ", 0x30(%[" ptr "])")
207
208 #define NORMALIZE(x0, x1, x2) /* x0 = vector/point to normalize */ \
209 __ASM_EMIT("movaps %" x0 ", %" x2) /* xmm2 = x y z w */ \
210 __ASM_EMIT("mulps %" x2 ", %" x2) /* xmm2 = x*x y*y z*z w*w */ \
211 __ASM_EMIT("movhlps %" x2 ", %" x1) /* xmm1 = z*z w*w ? ? */ \
212 __ASM_EMIT("addss %" x1 ", %" x2) /* xmm2 = x*x+z*z y*y z*z w*w */ \
213 __ASM_EMIT("unpcklps %" x2 ", %" x2) /* xmm2 = x*x+z*z x*x+z*z y*y y*y */ \
214 __ASM_EMIT("movhlps %" x2 ", %" x1) /* xmm1 = y*y y*y ? ? */ \
215 __ASM_EMIT("addss %" x1 ", %" x2) /* xmm2 = x*x+y*y+z*z */ \
216 __ASM_EMIT("xorps %" x1 ", %" x1) /* xmm1 = 0 */ \
217 __ASM_EMIT("shufps $0x00, %" x2 ", %" x2) /* xmm2 = w w w w */ \
218 __ASM_EMIT("sqrtps %" x2 ", %" x2) /* xmm2 = sqrt(x*x + y*y + z*z) */ \
219 __ASM_EMIT("cmpps $4, %" x2 ", %" x1) /* xmm1 = w != 0 */ \
220 __ASM_EMIT("divps %" x2 ", %" x0) /* xmm0 = x/w y/w z/w w/w */ \
221 __ASM_EMIT("andps %" x1 ", %" x0) /* xmm0 = (w != 0) ? x/w y/w z/w w/w : 0 */ \
222
223 /*
224 * Input:
225 * x0 = vector/point to scale
226 * x1 = radius-vector to match length
227 *
228 * Output:
229 * x0 = scaled vector
230 */
231 #define SCALE(x0, x1, x2, x3) \
232 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = x y z w */ \
233 __ASM_EMIT("mulps %" x3 ", %" x3) /* xmm3 = x*x y*y z*z w*w */ \
234 __ASM_EMIT("movhlps %" x3 ", %" x2) /* xmm2 = z*z w*w ? ? */ \
235 __ASM_EMIT("addss %" x2 ", %" x3) /* xmm3 = x*x+z*z y*y z*z w*w */ \
236 __ASM_EMIT("unpcklps %" x3 ", %" x3) /* xmm3 = x*x+z*z x*x+z*z y*y y*y */ \
237 __ASM_EMIT("movhlps %" x3 ", %" x2) /* xmm2 = y*y y*y ? ? */ \
238 __ASM_EMIT("addss %" x2 ", %" x3) /* xmm3 = x*x+y*y+z*z */ \
239 __ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \
240 __ASM_EMIT("sqrtss %" x3", %" x3) /* xmm3 = sqrt(x*x + y*y + z*z) */ \
241 __ASM_EMIT("ucomiss %" x2 ", %" x3) /* xmm3 =?= xmm2 */ \
242 __ASM_EMIT("jle 1000000f") \
243 __ASM_EMIT("divss %" x3 ", %" x1) /* xmm1 = r/w */ \
244 __ASM_EMIT("shufps $0x00, %" x1 ", %" x1) /* xmm1 = r/w r/w r/w r/w */ \
245 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*r/w y*r/w z*r/w w*r/w */ \
246 __ASM_EMIT("1000000:")
247
248 /*
249 * Input:
250 * x0 = vector 1 = dx1 dy1 dz1 dw1
251 * x1 = vector 2 = dx2 dy2 dz2 dw2
252 *
253 * Output:
254 * x0 = dx1*dx2 + dy1*dy2 + dz1*dz2
255 */
256 #define SCALAR_MUL(x0, x1, x2) \
257 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*x y*y z*z w*w */ \
258 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = z*z w*w ? ? */ \
259 __ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = x*x+z*z y*y z*z w*w */ \
260 __ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = x*x+z*z x*x+z*z y*y y*y */ \
261 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = y*y y*y ? ? */ \
262 __ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = x*x+y*y+z*z */
263
264 #define TRIPLET_CHECK(x0, x1, x2, x3, x4) /* x0 = normal; x1, x2 = vectors */ \
265 __ASM_EMIT("movaps %" x1 ", %" x3) /* xmm3 = x1 y1 z1 w1 */ \
266 __ASM_EMIT("movaps %" x2 ", %" x4) /* xmm4 = x2 y2 z2 w2 */ \
267 __ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = y1 z1 x1 w1 */ \
268 __ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = y2 z2 x2 w2 */ \
269 __ASM_EMIT("shufps $0xd2, %" x0 ", %" x0) /* xmm0 = nz nx ny nw */ \
270 __ASM_EMIT("mulps %" x4 ", %" x1) /* xmm1 = x1*y2 y1*z2 z1*x2 w1*w2 */ \
271 __ASM_EMIT("mulps %" x3 ", %" x2) /* xmm2 = y1*x2 z1*y2 x1*z2 w1*w2 */ \
272 __ASM_EMIT("subps %" x2 ", %" x1) /* xmm1 = x1*y2-y1*x2 y1*z2-z1*y2 z1*x2-x1*z2 w1*w2-w1*w2 = vz vx vy vw */ \
273 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = nz*vz nx*vx ny*vy nw*vw */ \
274 __ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = ny*vy nw*vw ? ? */ \
275 __ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = nz*vz nz*vz nx*vx nx*vx */ \
276 __ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = nx*vx nx*vx ? ? */ \
277 __ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = nz*vz + ny*vy */ \
278 __ASM_EMIT("addss %" x1 ", %" x0) /* xmm0 = nz*vz + ny*vy + nx*vx */
279
280 namespace lsp
281 {
282 namespace sse
283 {
284 using namespace dsp;
285
286 IF_ARCH_X86(
287 static const float IDENTITY0[4] __lsp_aligned16 = { 1.0f, 0.0f, 0.0f, 0.0f };
288 static const float IDENTITY1[4] __lsp_aligned16 = { 0.0f, 1.0f, 0.0f, 0.0f };
289 static const float IDENTITY2[4] __lsp_aligned16 = { 0.0f, 0.0f, 1.0f, 0.0f };
290 static const float IDENTITY3[4] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f };
291
292 static const uint32_t X_MASK0001[] __lsp_aligned16 = { 0xffffffff, 0, 0, 0 };
293 static const uint32_t X_MASK0111[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
294 static const uint32_t X_SMASK0010[] __lsp_aligned16 = { 0, 0x80000000, 0, 0 };
295 static const uint32_t X_SMASK0001[] __lsp_aligned16 = { 0x80000000, 0, 0, 0 };
296 static const uint32_t X_SMASK0111[] __lsp_aligned16 = { 0x80000000, 0x80000000, 0x80000000, 0 };
297
298 static const uint32_t X_3DMASK[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
299 static const float X_3DPOINT[] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f };
300
301 static const float X_3D_TOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(DSP_3D_TOLERANCE) };
302 static const float X_3D_MTOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(-DSP_3D_TOLERANCE) };
303
304 static const float ONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1.0f) };
305 static const float X_MINUS_ONE[] __lsp_aligned16 = { LSP_DSP_VEC4(-1.0f) };
306 static const uint32_t IONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1) };
307 )
308
309 void init_point_xyz(point3d_t *p, float x, float y, float z)
310 {
311 ARCH_X86_ASM
312 (
313 __ASM_EMIT("movlhps %[x1], %[x0]") // x0 = x ? y ?
314 __ASM_EMIT("movhps %[id], %[x2]") // x2 = z ? 1 0
315 __ASM_EMIT("shufps $0x88, %[x2], %[x0]") // x0 = x y z 1
316 __ASM_EMIT("movups %[x0], (%[p])")
317
318 : [x0] "+x" (x), [x1] "+x" (y), [x2] "+x"(z)
319 : [p] "r" (p),
320 [id] "m" (IDENTITY0)
321 : "memory"
322 );
323 }
324
325 void init_point(point3d_t *p, const point3d_t *s)
326 {
327 float x0;
328
329 ARCH_X86_ASM
330 (
331 __ASM_EMIT("movups (%[s]), %[x0]")
332 __ASM_EMIT("movups %[x0], (%[p])")
333 : [x0] "=&x" (x0)
334 : [s] "r" (s), [p] "r" (p)
335 : "memory"
336 );
337 }
338
339 void normalize_point(point3d_t *p)
340 {
341 float x0, x1, x2;
342
343 ARCH_X86_ASM
344 (
345 __ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
346 NORMALIZE("[x0]", "[x1]", "[x2]")
347 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
348 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
349 __ASM_EMIT("movups %[x0], (%[p])")
350
351 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
352 : [p] "r" (p),
353 [zmask] "m" (X_MASK0111),
354 [omask] "m" (X_3DPOINT)
355 : "cc", "memory"
356 );
357 }
358
359 void scale_point1(point3d_t *p, float r)
360 {
361 float x0, x1, x2;
362
363 ARCH_X86_ASM
364 (
365 __ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
366 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
367 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
368 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
369 __ASM_EMIT("movups %[x0], (%[p])")
370
371 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
372 : [p] "r" (p),
373 [zmask] "m" (X_MASK0111),
374 [omask] "m" (X_3DPOINT)
375 : "cc", "memory"
376 );
377 }
378
379 void scale_point2(point3d_t *p, const point3d_t *s, float r)
380 {
381 float x0, x1, x2;
382
383 ARCH_X86_ASM
384 (
385 __ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = x y z w
386 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
387 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
388 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
389 __ASM_EMIT("movups %[x0], (%[p])")
390
391 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
392 : [p] "r" (p), [s] "r" (s),
393 [zmask] "m" (X_MASK0111),
394 [omask] "m" (X_3DPOINT)
395 : "cc", "memory"
396 );
397 }
398
399 void init_vector_dxyz(vector3d_t *p, float dx, float dy, float dz)
400 {
401 ARCH_X86_ASM
402 (
403 __ASM_EMIT("movlhps %[x1], %[x0]") // x0 = dx ? dy ?
404 __ASM_EMIT("movhps %[id], %[x2]") // x2 = dz ? 1 0
405 __ASM_EMIT("shufps $0xc8, %[x2], %[x0]") // x0 = dx dy dz 0
406 __ASM_EMIT("movups %[x0], (%[p])")
407
408 : [x0] "+x" (dx), [x1] "+x" (dy), [x2] "+x"(dz)
409 : [p] "r" (p),
410 [id] "m" (IDENTITY0)
411 : "memory"
412 );
413 }
414
415 void init_vector(vector3d_t *p, const vector3d_t *s)
416 {
417 float x0;
418
419 ARCH_X86_ASM
420 (
421 __ASM_EMIT("movups (%[s]), %[x0]")
422 __ASM_EMIT("movups %[x0], (%[p])")
423 : [x0] "=&x" (x0)
424 : [s] "r" (s), [p] "r" (p)
425 : "memory"
426 );
427 }
428
429 void add_vector_pv1(point3d_t *p, const vector3d_t *dv)
430 {
431 float x0, x1;
432
433 ARCH_X86_ASM
434 (
435 __ASM_EMIT("movups (%[p]), %[x0]")
436 __ASM_EMIT("movups (%[dv]), %[x1]")
437 __ASM_EMIT("addps %[x1], %[x0]")
438 __ASM_EMIT("movups %[x0], (%[p])")
439 : [x0] "=&x" (x0), [x1] "=&x" (x1)
440 : [p] "r" (p), [dv] "r" (dv)
441 : "memory"
442 );
443 }
444
445 void add_vector_pv2(point3d_t *p, const point3d_t *sp, const vector3d_t *dv)
446 {
447 float x0, x1;
448
449 ARCH_X86_ASM
450 (
451 __ASM_EMIT("movups (%[sp]), %[x0]")
452 __ASM_EMIT("movups (%[dv]), %[x1]")
453 __ASM_EMIT("addps %[x1], %[x0]")
454 __ASM_EMIT("movups %[x0], (%[p])")
455 : [x0] "=&x" (x0), [x1] "=&x" (x1)
456 : [p] "r" (p), [sp] "r" (sp), [dv] "r" (dv)
457 : "memory"
458 );
459 }
460
461 void add_vector_pvk1(point3d_t *p, const vector3d_t *dv, float k)
462 {
463 float x0, x1;
464
465 ARCH_X86_ASM
466 (
467 __ASM_EMIT("movups (%[dv]), %[x1]")
468 __ASM_EMIT("shufps $0x00, %[x2], %[x2]")
469 __ASM_EMIT("movups (%[p]), %[x0]")
470 __ASM_EMIT("mulps %[x2], %[x1]")
471 __ASM_EMIT("addps %[x1], %[x0]")
472 __ASM_EMIT("movups %[x0], (%[p])")
473 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "+x" (k)
474 : [p] "r" (p), [dv] "r" (dv)
475 : "memory"
476 );
477 }
478
479 void add_vector_pvk2(point3d_t *p, const point3d_t *sp, const vector3d_t *dv, float k)
480 {
481 float x0, x1;
482
483 ARCH_X86_ASM
484 (
485 __ASM_EMIT("movups (%[dv]), %[x1]")
486 __ASM_EMIT("shufps $0x00, %[x2], %[x2]")
487 __ASM_EMIT("movups (%[sp]), %[x0]")
488 __ASM_EMIT("mulps %[x2], %[x1]")
489 __ASM_EMIT("addps %[x1], %[x0]")
490 __ASM_EMIT("movups %[x0], (%[p])")
491 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "+x" (k)
492 : [p] "r" (p), [sp] "r" (sp), [dv] "r" (dv)
493 : "memory"
494 );
495 }
496
497 void normalize_vector(vector3d_t *v)
498 {
499 float x0, x1, x2;
500
501 ARCH_X86_ASM
502 (
503 __ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
504 NORMALIZE("[x0]", "[x1]", "[x2]")
505 __ASM_EMIT("movups %[x0], (%[v])")
506
507 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
508 : [v] "r" (v)
509 : "cc", "memory"
510 );
511 }
512
513 void normalize_vector2(vector3d_t *v, const vector3d_t *sv)
514 {
515 float x0, x1, x2;
516
517 ARCH_X86_ASM
518 (
519 __ASM_EMIT("movups (%[sv]), %[x0]") // xmm0 = dx dy dz dw
520 NORMALIZE("[x0]", "[x1]", "[x2]")
521 __ASM_EMIT("movups %[x0], (%[v])")
522
523 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
524 : [v] "r" (v), [sv] "r" (sv)
525 : "cc", "memory"
526 );
527 }
528
529 void flip_vector_v1(vector3d_t *v)
530 {
531 ARCH_X86_ASM
532 (
533 __ASM_EMIT("movups (%[v]), %%xmm0") // xmm0 = dx dy dz dw
534 __ASM_EMIT("xorps %[mask], %%xmm0")
535 __ASM_EMIT("movups %%xmm0, (%[v])")
536 :
537 : [v] "r" (v),
538 [mask] "m" (X_SMASK0111)
539 : "memory",
540 "xmm0"
541 );
542 }
543
544 void flip_vector_v2(vector3d_t *v, const vector3d_t *sv)
545 {
546 ARCH_X86_ASM
547 (
548 __ASM_EMIT("movups (%[sv]), %%xmm0") // xmm0 = dx dy dz dw
549 __ASM_EMIT("xorps %[mask], %%xmm0")
550 __ASM_EMIT("movups %%xmm0, (%[v])")
551 :
552 : [v] "r" (v), [sv] "r" (sv),
553 [mask] "m" (X_SMASK0111)
554 : "memory",
555 "xmm0"
556 );
557 }
558
559 void scale_vector1(vector3d_t *v, float r)
560 {
561 float x0, x1, x2;
562
563 ARCH_X86_ASM
564 (
565 __ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
566 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
567 __ASM_EMIT("andps %[zmask], %[x0]")
568 __ASM_EMIT("movups %[x0], (%[v])")
569
570 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
571 : [v] "r" (v),
572 [zmask] "m" (X_MASK0111)
573 : "cc", "memory"
574 );
575 }
576
577 void scale_vector2(vector3d_t *v, const vector3d_t *s, float r)
578 {
579 float x0, x1, x2;
580
581 ARCH_X86_ASM
582 (
583 __ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = dx dy dz dw
584 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
585 __ASM_EMIT("andps %[zmask], %[x0]")
586 __ASM_EMIT("movups %[x0], (%[v])")
587
588 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
589 : [v] "r" (v), [s] "r" (s),
590 [zmask] "m" (X_MASK0111)
591 : "cc", "memory"
592 );
593 }
594
595 void init_matrix3d(matrix3d_t *dst, const matrix3d_t *src)
596 {
597 ARCH_X86_ASM
598 (
599 MATRIX_LOAD("s", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
600 MATRIX_STORE("d", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
601 :
602 : [s] "r" (src), [d] "r" (dst)
603 : "memory",
604 "%xmm0", "%xmm1", "%xmm2", "%xmm3"
605 );
606 }
607
608 void init_matrix3d_zero(matrix3d_t *m)
609 {
610 ARCH_X86_ASM
611 (
612 __ASM_EMIT("xorps %%xmm0, %%xmm0")
613 MATRIX_STORE("m", "%xmm0", "%xmm0", "%xmm0", "%xmm0")
614 :
615 : [m] "r" (m)
616 : "memory",
617 "%xmm0"
618 );
619 }
620
621 void init_matrix3d_one(matrix3d_t *m)
622 {
623 ARCH_X86_ASM
624 (
625 __ASM_EMIT("movaps %[one], %%xmm0")
626 MATRIX_STORE("m", "%xmm0", "%xmm0", "%xmm0", "%xmm0")
627 :
628 : [m] "r" (m), [one] "m" (ONE)
629 : "memory",
630 "%xmm0"
631 );
632 }
633
634 void init_matrix3d_identity(matrix3d_t *m)
635 {
636 ARCH_X86_ASM
637 (
638 __ASM_EMIT("movaps %[idm0], %%xmm0")
639 __ASM_EMIT("movaps %[idm1], %%xmm1")
640 __ASM_EMIT("movaps %[idm2], %%xmm2")
641 __ASM_EMIT("movaps %[idm3], %%xmm3")
642 MATRIX_STORE("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
643 :
644 : [m] "r" (m),
645 [idm0] "m" (IDENTITY0),
646 [idm1] "m" (IDENTITY1),
647 [idm2] "m" (IDENTITY2),
648 [idm3] "m" (IDENTITY3)
649 : "memory",
650 "%xmm0", "%xmm1", "%xmm2", "%xmm3"
651 );
652 }
653
654 void init_matrix3d_translate(matrix3d_t *m, float dx, float dy, float dz)
655 {
656 float x3, x4, x5;
657
658 ARCH_X86_ASM
659 (
660 __ASM_EMIT("movaps %[idm0], %[x3]") // xmm3 = 1 0 0 0
661 __ASM_EMIT("movaps %[idm1], %[x4]") // xmm4 = 0 1 0 0
662 __ASM_EMIT("movaps %[idm2], %[x5]") // xmm5 = 0 0 1 0
663 __ASM_EMIT("movlhps %[x3], %[x2]") // xmm2 = dz ? 1 0
664 __ASM_EMIT("movlhps %[x1], %[x0]") // xmm0 = dx ? dy ?
665 __ASM_EMIT("shufps $0x88, %[x2], %[x0]") // xmm0 = dx dy dz 1
666
667 MATRIX_STORE("m", "[x3]", "[x4]", "[x5]", "[x0]")
668
669 : [x0] "+x" (dx), [x1] "+x" (dy), [x2] "+x"(dz),
670 [x3] "=&x" (x3), [x4] "=&x" (x4), [x5] "=&x" (x5)
671 : [m] "r" (m),
672 [idm0] "m" (IDENTITY0),
673 [idm1] "m" (IDENTITY1),
674 [idm2] "m" (IDENTITY2),
675 [idm3] "m" (IDENTITY3)
676 : "memory"
677 );
678 }
679
680 void init_matrix3d_scale(matrix3d_t *m, float sx, float sy, float sz)
681 {
682 float x3;
683
684 ARCH_X86_ASM
685 (
686 __ASM_EMIT("andps %[mask], %[x0]") // xmm0 = sx 0 0 0
687 __ASM_EMIT("andps %[mask], %[x1]") // xmm1 = sy 0 0 0
688 __ASM_EMIT("andps %[mask], %[x2]") // xmm2 = sz 0 0 0
689 __ASM_EMIT("shufps $0xf3, %[x1], %[x1]") // xmm1 = 0 sy 0 0
690 __ASM_EMIT("shufps $0xcf, %[x2], %[x2]") // xmm2 = 0 0 sz 0
691 __ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
692
693 MATRIX_STORE("m", "[x0]", "[x1]", "[x2]", "[x3]")
694
695 : [x0] "+x" (sx), [x1] "+x" (sy), [x2] "+x"(sz),
696 [x3] "=&x" (x3)
697 : [m] "r" (m),
698 [idm3] "m" (IDENTITY3),
699 [mask] "m" (X_MASK0001)
700 : "memory"
701 );
702 }
703
704 void init_matrix3d_rotate_x(matrix3d_t *m, float angle)
705 {
706 float s = sinf(angle);
707 float c = cosf(angle);
708 float x2, x3;
709
710 ARCH_X86_ASM
711 (
712 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
713 __ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
714 __ASM_EMIT("shufps $0xc3, %[x1], %[x0]") // xmm0 = 0 s c 0
715 __ASM_EMIT("movaps %[idm0], %[x2]") // xmm2 = 1 0 0 0
716 __ASM_EMIT("movaps %[x0], %[x1]") // xmm1 = 0 s c 0
717 __ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
718 __ASM_EMIT("xorps %[mask], %[x0]") // xmm0 = 0 -s c 0
719 __ASM_EMIT("shufps $0xd8, %[x1], %[x1]") // xmm1 = 0 c s 0
720
721 MATRIX_STORE("m", "[x2]", "[x1]", "[x0]", "[x3]")
722 : [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3)
723 : [m] "r" (m),
724 [idm0] "m" (IDENTITY0),
725 [idm3] "m" (IDENTITY3),
726 [zmask] "m" (X_MASK0001),
727 [mask] "m" (X_SMASK0010)
728 : "memory"
729 );
730 }
731
732 void init_matrix3d_rotate_y(matrix3d_t *m, float angle)
733 {
734 float s = sinf(angle);
735 float c = cosf(angle);
736 float x2, x3;
737
738 ARCH_X86_ASM
739 (
740 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
741 __ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
742 __ASM_EMIT("movlhps %[x1], %[x0]") // xmm0 = s 0 c 0
743 __ASM_EMIT("movaps %[x0], %[x1]") // xmm1 = s 0 c 0
744 __ASM_EMIT("movaps %[idm1], %[x2]") // xmm2 = 0 1 0 0
745 __ASM_EMIT("xorps %[mask], %[x1]") // xmm1 = -s 0 c 0
746 __ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
747 __ASM_EMIT("shufps $0xc6, %[x1], %[x1]") // xmm1 = c 0 -s 0
748
749 MATRIX_STORE("m", "[x1]", "[x2]", "[x0]", "[x3]")
750 : [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3)
751 : [m] "r" (m),
752 [idm1] "m" (IDENTITY1),
753 [idm3] "m" (IDENTITY3),
754 [zmask] "m" (X_MASK0001),
755 [mask] "m" (X_SMASK0001)
756 : "memory"
757 );
758 }
759
760 void init_matrix3d_rotate_z(matrix3d_t *m, float angle)
761 {
762 float s = sinf(angle);
763 float c = cosf(angle);
764 float x2, x3, x4;
765
766 ARCH_X86_ASM
767 (
768 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
769 __ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
770 __ASM_EMIT("movaps %[x0], %[x2]") // xmm2 = s 0 0 0
771 __ASM_EMIT("movaps %[idm2], %[x3]") // xmm3 = 0 0 1 0
772 __ASM_EMIT("unpcklps %[x1], %[x0]") // xmm0 = s c 0 0
773 __ASM_EMIT("movaps %[idm3], %[x4]") // xmm3 = 0 0 0 1
774 __ASM_EMIT("unpcklps %[x2], %[x1]") // xmm1 = c s 0 0
775 __ASM_EMIT("xorps %[mask], %[x0]") // xmm0 = -s c 0 0
776
777 MATRIX_STORE("m", "[x1]", "[x0]", "[x3]", "[x4]")
778 : [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
779 : [m] "r" (m),
780 [idm2] "m" (IDENTITY2),
781 [idm3] "m" (IDENTITY3),
782 [zmask] "m" (X_MASK0001),
783 [mask] "m" (X_SMASK0001)
784 : "memory"
785 );
786 }
787
788 void init_matrix3d_rotate_xyz(matrix3d_t *m, float x, float y, float z, float angle)
789 {
790 if (x == 0.0f)
791 {
792 if (y == 0.0f)
793 {
794 if (z > 0.0f)
795 init_matrix3d_rotate_z(m, angle);
796 else if (z < 0.0f)
797 init_matrix3d_rotate_z(m, -angle);
798 else // (z == 0.0)
799 init_matrix3d_identity(m);
800 return;
801 }
802 else if (z == 0.0f)
803 {
804 if (y > 0.0f)
805 init_matrix3d_rotate_y(m, angle);
806 else if (y < 0.0f)
807 init_matrix3d_rotate_y(m, -angle);
808 return;
809 }
810 }
811 else if ((y == 0.0) && (z == 0.0))
812 {
813 if (x > 0.0f)
814 init_matrix3d_rotate_x(m, angle);
815 else
816 init_matrix3d_rotate_x(m, -angle);
817 return;
818 }
819
820 float s = sinf(angle);
821 float c = cosf(angle);
822
823 float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
824 float mag = sqrtf(x*x + y*y + z*z);
825
826 // if (mag <= 1.0e-4)
827 // return *this;
828
829 x /= mag;
830 y /= mag;
831 z /= mag;
832
833 xx = x * x;
834 yy = y * y;
835 zz = z * z;
836 xy = x * y;
837 yz = y * z;
838 zx = z * x;
839 xs = x * s;
840 ys = y * s;
841 zs = z * s;
842 one_c = 1.0f - c;
843
844 float *M = m->m;
845 M[0] = (one_c * xx) + c;
846 M[1] = (one_c * xy) + zs;
847 M[2] = (one_c * zx) - ys;
848 M[3] = 0.0f;
849 M[4] = (one_c * xy) - zs;
850 M[5] = (one_c * yy) + c;
851 M[6] = (one_c * yz) + xs;
852 M[7] = 0.0f;
853 M[8] = (one_c * zx) + ys;
854 M[9] = (one_c * yz) - xs;
855 M[10] = (one_c * zz) + c;
856 M[11] = 0.0f;
857 M[12] = 0.0f;
858 M[13] = 0.0f;
859 M[14] = 0.0f;
860 M[15] = 0.0f;
861 }
862
863 void apply_matrix3d_mv2(vector3d_t *r, const vector3d_t *v, const matrix3d_t *m)
864 {
865 ARCH_X86_ASM
866 (
867 __ASM_EMIT("movups (%[v]), %%xmm0") // xmm0 = vx vy vz vw
868 __ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
869 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = vx vy vz vw
870 __ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
871 __ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = vx vy vz vw
872 __ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
873 __ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = vx vx vx vx
874 __ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = vy vy vy vy
875 __ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm2 = vz vz vz vz
876
877 __ASM_EMIT("mulps %%xmm4, %%xmm0")
878 __ASM_EMIT("mulps %%xmm5, %%xmm1")
879 __ASM_EMIT("mulps %%xmm6, %%xmm2")
880 __ASM_EMIT("addps %%xmm1, %%xmm0")
881 __ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
882
883 __ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
884 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rdx rdy rdz rdw
885 __ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rdw rdw rdw rdw
886 __ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
887 __ASM_EMIT("je 1f")
888 __ASM_EMIT("divps %%xmm1, %%xmm0")
889 __ASM_EMIT("1:")
890 __ASM_EMIT("movups %%xmm0, (%[r])")
891 :
892 : [r] "r" (r), [v] "r" (v), [m] "r" (m)
893 : "cc", "memory",
894 "%xmm0", "%xmm1", "%xmm2",
895 "%xmm4", "%xmm5", "%xmm6"
896 );
897 }
898
899 void apply_matrix3d_mv1(vector3d_t *r, const matrix3d_t *m)
900 {
901 ARCH_X86_ASM
902 (
903 __ASM_EMIT("movups (%[r]), %%xmm0") // xmm0 = vx vy vz vw
904 __ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
905 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = vx vy vz vw
906 __ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
907 __ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = vx vy vz vw
908 __ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
909 __ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = vx vx vx vx
910 __ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = vy vy vy vy
911 __ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm2 = vz vz vz vz
912
913 __ASM_EMIT("mulps %%xmm4, %%xmm0")
914 __ASM_EMIT("mulps %%xmm5, %%xmm1")
915 __ASM_EMIT("mulps %%xmm6, %%xmm2")
916 __ASM_EMIT("addps %%xmm1, %%xmm0")
917 __ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
918
919 __ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
920 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rdx rdy rdz rdw
921 __ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rdw rdw rdw rdw
922 __ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
923 __ASM_EMIT("je 1f")
924 __ASM_EMIT("divps %%xmm1, %%xmm0")
925 __ASM_EMIT("1:")
926 __ASM_EMIT("movups %%xmm0, (%[r])")
927 :
928 : [r] "r" (r), [m] "r" (m)
929 : "cc", "memory",
930 "%xmm0", "%xmm1", "%xmm2",
931 "%xmm4", "%xmm5", "%xmm6"
932 );
933 }
934
935 void apply_matrix3d_mp2(point3d_t *r, const point3d_t *p, const matrix3d_t *m)
936 {
937 ARCH_X86_ASM
938 (
939 __ASM_EMIT("movups (%[p]), %%xmm0") // xmm0 = px py pz pw
940 __ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
941 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = px py pz pw
942 __ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
943 __ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = px py pz pw
944 __ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
945 __ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = px px px px
946 __ASM_EMIT("movups 0x30(%[m]), %%xmm7") // xmm7 = m12 m13 m14 m15
947 __ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = py py py py
948 __ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm1 = pz pz pz pz
949
950 __ASM_EMIT("mulps %%xmm4, %%xmm0")
951 __ASM_EMIT("mulps %%xmm5, %%xmm1")
952 __ASM_EMIT("mulps %%xmm6, %%xmm2")
953 __ASM_EMIT("addps %%xmm1, %%xmm0")
954 __ASM_EMIT("addps %%xmm7, %%xmm2")
955 __ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
956
957 __ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
958 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rx ry rz rw
959 __ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rw rw rw rw
960 __ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
961 __ASM_EMIT("je 1f")
962 __ASM_EMIT("divps %%xmm1, %%xmm0")
963 __ASM_EMIT("1:")
964 __ASM_EMIT("movups %%xmm0, (%[r])")
965 :
966 : [r] "r" (r), [p] "r" (p), [m] "r" (m)
967 : "cc", "memory",
968 "%xmm0", "%xmm1", "%xmm2",
969 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
970 );
971 }
972
973 void apply_matrix3d_mp1(point3d_t *r, const matrix3d_t *m)
974 {
975 ARCH_X86_ASM
976 (
977 __ASM_EMIT("movups (%[r]), %%xmm0") // xmm0 = px py pz pw
978 __ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
979 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = px py pz pw
980 __ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
981 __ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = px py pz pw
982 __ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
983 __ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = px px px px
984 __ASM_EMIT("movups 0x30(%[m]), %%xmm7") // xmm7 = m12 m13 m14 m15
985 __ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = py py py py
986 __ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm1 = pz pz pz pz
987
988 __ASM_EMIT("mulps %%xmm4, %%xmm0")
989 __ASM_EMIT("mulps %%xmm5, %%xmm1")
990 __ASM_EMIT("mulps %%xmm6, %%xmm2")
991 __ASM_EMIT("addps %%xmm1, %%xmm0")
992 __ASM_EMIT("addps %%xmm7, %%xmm2")
993 __ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
994
995 __ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
996 __ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rx ry rz rw
997 __ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rw rw rw rw
998 __ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
999 __ASM_EMIT("je 1f")
1000 __ASM_EMIT("divps %%xmm1, %%xmm0")
1001 __ASM_EMIT("1:")
1002 __ASM_EMIT("movups %%xmm0, (%[r])")
1003 :
1004 : [r] "r" (r), [m] "r" (m)
1005 : "cc", "memory",
1006 "%xmm0", "%xmm1", "%xmm2",
1007 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
1008 );
1009 }
1010
1011 #define MATMUL_CORE(x0, x1, x2, x3, x4, x5, x6, x7) \
1012 __ASM_EMIT("movaps %" x4 ", %" x5) /* xmm5 = b0 b1 b2 b3 */ \
1013 __ASM_EMIT("movaps %" x4 ", %" x6) /* xmm6 = b0 b1 b2 b3 */ \
1014 __ASM_EMIT("movaps %" x5 ", %" x7) /* xmm7 = b0 b1 b2 b3 */ \
1015 __ASM_EMIT("shufps $0x00, %" x4 ", %" x4) /* xmm4 = b0 b0 b0 b0 */ \
1016 __ASM_EMIT("shufps $0x55, %" x5 ", %" x5) /* xmm5 = b1 b1 b1 b1 */ \
1017 __ASM_EMIT("shufps $0xaa, %" x6 ", %" x6) /* xmm6 = b2 b2 b2 b2 */ \
1018 __ASM_EMIT("shufps $0xff, %" x7 ", %" x7) /* xmm7 = b3 b3 b3 b3 */ \
1019 __ASM_EMIT("mulps %" x0 ", %" x4) \
1020 __ASM_EMIT("mulps %" x1 ", %" x5) \
1021 __ASM_EMIT("mulps %" x2 ", %" x6) \
1022 __ASM_EMIT("mulps %" x3 ", %" x7) \
1023 __ASM_EMIT("addps %" x5 ", %" x4) \
1024 __ASM_EMIT("addps %" x7 ", %" x6) \
1025 __ASM_EMIT("addps %" x6 ", %" x4)
1026
1027 void apply_matrix3d_mm2(matrix3d_t *r, const matrix3d_t *s, const matrix3d_t *m)
1028 {
1029 ARCH_X86_ASM
1030 (
1031 MATRIX_LOAD("A", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1032
1033 // Multiply matrix and store
1034 __ASM_EMIT("movups 0x00(%[B]), %%xmm4") // xmm4 = b0 b1 b2 b3
1035 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
1036 __ASM_EMIT("movups %%xmm4, 0x00(%[R])")
1037
1038 __ASM_EMIT("movups 0x10(%[B]), %%xmm5") // xmm5 = b4 b5 b6 b7
1039 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm5", "%xmm6", "%xmm7", "%xmm4")
1040 __ASM_EMIT("movups %%xmm5, 0x10(%[R])")
1041
1042 __ASM_EMIT("movups 0x20(%[B]), %%xmm6") // xmm6 = b4 b5 b6 b7
1043 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm6", "%xmm7", "%xmm4", "%xmm5")
1044 __ASM_EMIT("movups %%xmm6, 0x20(%[R])")
1045
1046 __ASM_EMIT("movups 0x30(%[B]), %%xmm7") // xmm7 = b4 b5 b6 b7
1047 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm7", "%xmm4", "%xmm5", "%xmm6")
1048 __ASM_EMIT("movups %%xmm7, 0x30(%[R])")
1049
1050 :
1051 : [A] "r" (s), [B] "r" (m), [R] "r" (r)
1052 : "memory",
1053 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
1054 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
1055 );
1056 }
1057
1058 void apply_matrix3d_mm1(matrix3d_t *r, const matrix3d_t *m)
1059 {
1060 matrix3d_t __lsp_aligned16 t;
1061
1062 ARCH_X86_ASM
1063 (
1064 MATRIX_LOAD("A", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1065
1066 // Multiply matrix and store
1067 __ASM_EMIT("movups 0x00(%[B]), %%xmm4") // xmm4 = b0 b1 b2 b3
1068 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
1069 __ASM_EMIT("movaps %%xmm4, 0x00(%[T])")
1070
1071 __ASM_EMIT("movups 0x10(%[B]), %%xmm5") // xmm5 = b4 b5 b6 b7
1072 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm5", "%xmm6", "%xmm7", "%xmm4")
1073 __ASM_EMIT("movaps %%xmm5, 0x10(%[T])")
1074
1075 __ASM_EMIT("movups 0x20(%[B]), %%xmm6") // xmm6 = b4 b5 b6 b7
1076 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm6", "%xmm7", "%xmm4", "%xmm5")
1077 __ASM_EMIT("movaps %%xmm6, 0x20(%[T])")
1078
1079 __ASM_EMIT("movups 0x30(%[B]), %%xmm7") // xmm7 = b4 b5 b6 b7
1080 MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm7", "%xmm4", "%xmm5", "%xmm6")
1081
1082 // Load matrix
1083 __ASM_EMIT("movaps 0x00(%[T]), %%xmm4")
1084 __ASM_EMIT("movaps 0x10(%[T]), %%xmm5")
1085 __ASM_EMIT("movaps 0x20(%[T]), %%xmm6")
1086
1087 // Store matrix
1088 MATRIX_STORE("A", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
1089
1090 :
1091 : [A] "r" (r), [B] "r" (m), [T] "r" (&t)
1092 : "memory",
1093 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
1094 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
1095 );
1096 }
1097
1098 void transpose_matrix3d1(matrix3d_t *r)
1099 {
1100 ARCH_X86_ASM
1101 (
1102 MATRIX_LOAD("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1103 MAT4_TRANSPOSE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4")
1104 MATRIX_STORE("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1105 :
1106 : [m] "r" (r)
1107 : "memory",
1108 "%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4"
1109 );
1110 }
1111
1112 void transpose_matrix3d2(matrix3d_t *r, const matrix3d_t *m)
1113 {
1114 ARCH_X86_ASM
1115 (
1116 MATRIX_LOAD("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1117 MAT4_TRANSPOSE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4")
1118 MATRIX_STORE("r", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
1119 :
1120 : [r] "r" (r), [m] "r" (m)
1121 : "memory",
1122 "%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4"
1123 );
1124 }
1125
1126 float check_triplet3d_p3n(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const vector3d_t *n)
1127 {
1128 float x0, x1, x2, x3, x4;
1129
1130 ARCH_X86_ASM
1131 (
1132 __ASM_EMIT("movups (%[p2]), %[x1]") // xmm1 = px2 py2 pz2 pw2
1133 __ASM_EMIT("movups (%[p3]), %[x2]") // xmm2 = px3 py3 pz3 pw3
1134 __ASM_EMIT("movups (%[p1]), %[x4]") // xmm4 = px1 py1 pz1 pw1
1135 __ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x2 y2 z2 w2
1136 __ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
1137 __ASM_EMIT("subps %[x4], %[x1]") // xmm1 = x1 y1 z1 w1
1138
1139 TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
1140
1141 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
1142 : [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [n] "r" (n)
1143 );
1144
1145 return x0;
1146 }
1147
1148 float check_triplet3d_pvn(const point3d_t *pv, const vector3d_t *n)
1149 {
1150 float x0, x1, x2, x3, x4;
1151
1152 ARCH_X86_ASM
1153 (
1154 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") // xmm1 = px2 py2 pz2 pw2
1155 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") // xmm2 = px3 py3 pz3 pw3
1156 __ASM_EMIT("movups 0x00(%[pv]), %[x4]") // xmm4 = px1 py1 pz1 pw1
1157 __ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x2 y2 z2 w2
1158 __ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
1159 __ASM_EMIT("subps %[x4], %[x1]") // xmm1 = x1 y1 z1 w1
1160
1161 TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
1162
1163 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
1164 : [pv] "r" (pv), [n] "r" (n)
1165 );
1166
1167 return x0;
1168 }
1169
1170 float check_triplet3d_v2n(const vector3d_t *v1, const vector3d_t *v2, const vector3d_t *n)
1171 {
1172 float x0, x1, x2, x3, x4;
1173
1174 ARCH_X86_ASM
1175 (
1176 __ASM_EMIT("movups (%[v1]), %[x1]") // xmm1 = x1 y1 z1 w1
1177 __ASM_EMIT("movups (%[v2]), %[x2]") // xmm2 = x2 y2 z2 w2
1178 __ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
1179
1180 TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
1181
1182 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
1183 : [v1] "r" (v1), [v2] "r" (v2), [n] "r" (n)
1184 );
1185
1186 return x0;
1187 }
1188
1189 float check_triplet3d_vvn(const vector3d_t *v, const vector3d_t *n)
1190 {
1191 float x0, x1, x2, x3, x4;
1192
1193 ARCH_X86_ASM
1194 (
1195 __ASM_EMIT("movups 0x00(%[v]), %[x1]") // xmm1 = x1 y1 z1 w1
1196 __ASM_EMIT("movups 0x10(%[v]), %[x2]") // xmm2 = x2 y2 z2 w2
1197 __ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
1198
1199 TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
1200
1201 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
1202 : [v] "r" (v), [n] "r" (n)
1203 );
1204
1205 return x0;
1206 }
1207
1208 float check_triplet3d_vv(const vector3d_t *v)
1209 {
1210 float x0, x1, x2, x3, x4;
1211
1212 ARCH_X86_ASM
1213 (
1214 __ASM_EMIT("movups 0x00(%[v]), %[x1]") // xmm1 = x1 y1 z1 w1
1215 __ASM_EMIT("movups 0x10(%[v]), %[x2]") // xmm2 = x2 y2 z2 w2
1216 __ASM_EMIT("movups 0x20(%[v]), %[x0]") // xmm0 = nx ny nz nw
1217
1218 TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
1219
1220 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
1221 : [v] "r" (v)
1222 );
1223
1224 return x0;
1225 }
1226
1227 size_t longest_edge3d_p3(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3)
1228 {
1229 size_t res;
1230 float x0, x1, x2, x3;
1231
1232 ARCH_X86_ASM
1233 (
1234 // Calc vectors
1235 __ASM_EMIT("movups (%[p1]), %[x2]") // xmm2 = x0 y0 z0 w0
1236 __ASM_EMIT("movups (%[p2]), %[x0]") // xmm0 = x1 y1 z1 w1
1237 __ASM_EMIT("movups (%[p3]), %[x1]") // xmm1 = x2 y2 z2 w2
1238 __ASM_EMIT("movaps %[x2], %[x3]") // xmm3 = x0 y0 z0 w0
1239 __ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x0-x2 y0-y2 z0-z2 w0-w2 = dx2 dy2 dz2 0
1240 __ASM_EMIT("subps %[x0], %[x1]") // xmm1 = x2-x1 y2-y1 z2-z1 w2-w1 = dx1 dy1 dz1 0
1241 __ASM_EMIT("subps %[x3], %[x0]") // xmm0 = x1-x0 y1-y0 z1-z0 w1-w0 = dx0 dy0 dz0 0
1242
1243 // Calc scalar multiplication
1244 __ASM_EMIT("mulps %[x2], %[x2]") // xmm2 = cx2 cy2 cz2 0
1245 __ASM_EMIT("mulps %[x1], %[x1]") // xmm1 = bx2 by2 bz2 0
1246 __ASM_EMIT("mulps %[x0], %[x0]") // xmm0 = ax2 ay2 az2 0
1247 MAT3_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]")
1248 __ASM_EMIT("addps %[x1], %[x0]")
1249 __ASM_EMIT("addps %[x3], %[x0]") // xmm0 = A2 B2 C2 ?
1250
1251 __ASM_EMIT("xor %[res], %[res]") // res = 0
1252 __ASM_EMIT("movhlps %[x0], %[x2]") // xmm2 = C2 ? ? ?
1253 __ASM_EMIT("unpcklps %[x0], %[x0]") // xmm0 = A2 A2 B2 B2
1254 __ASM_EMIT("movhlps %[x0], %[x1]") // xmm1 = B2 B2 ? ?
1255
1256 __ASM_EMIT("ucomiss %[x1], %[x0]") // A2 <?> B2
1257 __ASM_EMIT("jae 1f")
1258 // A2 < B2
1259 __ASM_EMIT("ucomiss %[x1], %[x2]") // C2 <?> B2
1260 __ASM_EMIT("seta %%al") // res = (B2 >= C2) ? 0 : 1
1261 __ASM_EMIT("add $1, %[res]") // res = (B2 >= C2) ? 1 : 2
1262 __ASM_EMIT("jmp 2f")
1263 __ASM_EMIT("1:")
1264 // A2 >= B2
1265 __ASM_EMIT("ucomiss %[x2], %[x0]") // A2 <?> C2
1266 __ASM_EMIT("jae 2f")
1267 __ASM_EMIT("mov $2, %[res]") // res = (A2 >= C2) ? 0 : 2
1268 __ASM_EMIT("2:")
1269 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1270 [res] "=a" (res)
1271 : [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3)
1272 );
1273
1274 return res;
1275 }
1276
1277 size_t longest_edge3d_pv(const point3d_t *p)
1278 {
1279 size_t res;
1280 float x0, x1, x2, x3;
1281
1282 ARCH_X86_ASM
1283 (
1284 // Calc vectors
1285 __ASM_EMIT("movups 0x00(%[p]), %[x2]") // xmm2 = x0 y0 z0 w0
1286 __ASM_EMIT("movups 0x10(%[p]), %[x0]") // xmm0 = x1 y1 z1 w1
1287 __ASM_EMIT("movups 0x20(%[p]), %[x1]") // xmm1 = x2 y2 z2 w2
1288 __ASM_EMIT("movaps %[x2], %[x3]") // xmm3 = x0 y0 z0 w0
1289 __ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x0-x2 y0-y2 z0-z2 w0-w2 = dx2 dy2 dz2 dw2
1290 __ASM_EMIT("subps %[x0], %[x1]") // xmm1 = x2-x1 y2-y1 z2-z1 w2-w1 = dx1 dy1 dz1 dw1
1291 __ASM_EMIT("subps %[x3], %[x0]") // xmm0 = x1-x0 y1-y0 z1-z0 w1-w0 = dx0 dy0 dz0 dw0
1292
1293 // Calc scalar multiplication
1294 __ASM_EMIT("mulps %[x2], %[x2]") // xmm2 = sx2 sy2 sz2 sw2
1295 __ASM_EMIT("mulps %[x1], %[x1]") // xmm1 = sx2 sy2 sz2 sw2
1296 __ASM_EMIT("mulps %[x0], %[x0]") // xmm0 = sx2 sy2 sz2 sw2
1297 MAT3_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]")
1298 __ASM_EMIT("addps %[x1], %[x0]")
1299 __ASM_EMIT("addps %[x3], %[x0]") // xmm0 = S0 S1 S2 ?
1300
1301 __ASM_EMIT("xor %[res], %[res]")
1302 __ASM_EMIT("movhlps %[x0], %[x2]") // xmm2 = S2 ? ? ? ?
1303 __ASM_EMIT("unpcklps %[x0], %[x0]") // xmm0 = S0 S0 S1 S1
1304 __ASM_EMIT("movhlps %[x0], %[x1]") // xmm1 = S1 ? ? ? ?
1305
1306 __ASM_EMIT("ucomiss %[x1], %[x0]") // A2 <?> B2
1307 __ASM_EMIT("jae 1f")
1308 // A2 < B2
1309 __ASM_EMIT("ucomiss %[x1], %[x2]") // C2 <?> B2
1310 __ASM_EMIT("seta %%al") // res = (B2 >= C2) ? 0 : 1
1311 __ASM_EMIT("add $1, %[res]") // res = (B2 >= C2) ? 1 : 2
1312 __ASM_EMIT("jmp 2f")
1313 __ASM_EMIT("1:")
1314 // A2 >= B2
1315 __ASM_EMIT("ucomiss %[x2], %[x0]") // A2 <?> C2
1316 __ASM_EMIT("jae 2f")
1317 __ASM_EMIT("mov $2, %[res]") // res = (A2 >= C2) ? 0 : 2
1318 __ASM_EMIT("2:")
1319 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1320 [res] "=a" (res)
1321 : [p] "r" (p)
1322 );
1323
1324 return res;
1325 }
1326
1327 float check_point3d_on_triangle_pvp(const point3d_t *pv, const point3d_t *p)
1328 {
1329 float x0, x1, x2, x3, x4, x5, x6, x7;
1330 uint32_t mask;
1331
1332 ARCH_X86_ASM
1333 (
1334 /* Load vectors */
1335 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz 1 */
1336 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 1 */
1337 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 1 */
1338 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 1 */
1339 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = v0 = dx0 dy0 dz0 0 */
1340 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = v1 = dx1 dy1 dz1 0 */
1341 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = v2 = dx2 dy2 dz2 0 */
1342 /* 3x vector multiplications */
1343 VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
1344 /* 3x scalar multiplications */
1345 /* xmm0 = m0 = v0 cross v1 */
1346 /* xmm1 = m1 = v1 cross v2 */
1347 /* xmm2 = m2 = v2 cross v0 */
1348 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
1349 /* Compare with zeros */
1350 /* xmm0 = r0 = m0 dot m1 */
1351 /* xmm1 = r1 = m1 dot m2 */
1352 /* xmm2 = r2 = m2 dot m0 */
1353 __ASM_EMIT("unpcklps %[x1], %[x0]") /* xmm0 = r0 r1 ? ? */
1354 __ASM_EMIT("movlhps %[x2], %[x0]") /* xmm0 = r0 r1 r2 ? */
1355 __ASM_EMIT("xorps %[x4], %[x4]") /* xmm4 = 0 */
1356 __ASM_EMIT("cmpps $6, %[x0], %[x4]") /* xmm4 = r0 < 0 r1 < 0 r2 < 0 */
1357 __ASM_EMIT("movmskps %[x4], %[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
1358 __ASM_EMIT("testl $0x07, %k[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
1359 __ASM_EMIT("jnz 100f")
1360 __ASM_EMIT("mulss %[x1], %[x0]") /* x0 = r0 * r1 */
1361 __ASM_EMIT("mulss %[x2], %[x0]") /* x0 = r0 * r1 * r2 */
1362 __ASM_EMIT("jmp 110f")
1363 __ASM_EMIT("100:")
1364 __ASM_EMIT("movss %[NEG], %[x0]") /* x0 = negative value */
1365 /* End */
1366 __ASM_EMIT("110:")
1367 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1368 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7),
1369 [mask] "=&r"(mask)
1370 : [pv] "r" (pv), [p] "r" (p),
1371 [NEG] "m" (X_3D_MTOLERANCE)
1372 : "cc"
1373 );
1374
1375 return x0;
1376 }
1377
1378 float check_point3d_on_triangle_p3p(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const point3d_t *p)
1379 {
1380 float x0, x1, x2, x3, x4, x5, x6, x7;
1381 uint32_t mask;
1382
1383 ARCH_X86_ASM
1384 (
1385 /* Load vectors */
1386 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz 1 */
1387 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 1 */
1388 __ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 1 */
1389 __ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 1 */
1390 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = v0 = dx0 dy0 dz0 0 */
1391 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = v1 = dx1 dy1 dz1 0 */
1392 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = v2 = dx2 dy2 dz2 0 */
1393 /* 3x vector multiplications */
1394 VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
1395 /* 3x scalar multiplications */
1396 /* xmm0 = m0 = v0 cross v1 */
1397 /* xmm1 = m1 = v1 cross v2 */
1398 /* xmm2 = m2 = v2 cross v0 */
1399 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
1400 /* Compare with zeros */
1401 /* xmm0 = r0 = m0 dot m1 */
1402 /* xmm1 = r1 = m1 dot m2 */
1403 /* xmm2 = r2 = m2 dot m0 */
1404 __ASM_EMIT("unpcklps %[x1], %[x0]") /* xmm0 = r0 r1 ? ? */
1405 __ASM_EMIT("movlhps %[x2], %[x0]") /* xmm0 = r0 r1 r2 ? */
1406 __ASM_EMIT("xorps %[x4], %[x4]") /* xmm4 = 0 */
1407 __ASM_EMIT("cmpps $6, %[x0], %[x4]") /* xmm4 = r0 < 0 r1 < 0 r2 < 0 */
1408 __ASM_EMIT("movmskps %[x4], %[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
1409 __ASM_EMIT("testl $0x07, %k[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
1410 __ASM_EMIT("jnz 100f")
1411 __ASM_EMIT("mulss %[x1], %[x0]") /* x0 = r0 * r1 */
1412 __ASM_EMIT("mulss %[x2], %[x0]") /* x0 = r0 * r1 * r2 */
1413 __ASM_EMIT("jmp 110f")
1414 __ASM_EMIT("100:")
1415 __ASM_EMIT("movss %[NEG], %[x0]") /* x0 = negative value */
1416 /* End */
1417 __ASM_EMIT("110:")
1418 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1419 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7),
1420 [mask] "=&r"(mask)
1421 : [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [p] "r" (p),
1422 [NEG] "m" (X_3D_MTOLERANCE)
1423 : "cc"
1424 );
1425
1426 return x0;
1427 }
1428
1429 void calc_normal3d_v2(vector3d_t *n, const vector3d_t *v1, const vector3d_t *v2)
1430 {
1431 float x0, x1, x2, x3;
1432
1433 ARCH_X86_ASM
1434 (
1435 __ASM_EMIT("movups (%[v1]), %[x0]") /* xmm0 = dx1 dy1 dz1 dw1 */
1436 __ASM_EMIT("movups (%[v2]), %[x1]") /* xmm1 = dx2 dy2 dz2 dw2 */
1437 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1438 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1439 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1440 __ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
1441 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
1442 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1443 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1444 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1445 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
1446 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1447 __ASM_EMIT("movups %[x1], (%[n])")
1448
1449 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1450 : [n] "r" (n), [v1] "r" (v1), [v2] "r" (v2)
1451 : "memory"
1452 );
1453 }
1454
1455 void calc_normal3d_vv(vector3d_t *n, const vector3d_t *vv)
1456 {
1457 float x0, x1, x2, x3;
1458
1459 ARCH_X86_ASM
1460 (
1461 __ASM_EMIT("movups 0x00(%[vv]), %[x0]") /* xmm0 = dx1 dy1 dz1 dw1 */
1462 __ASM_EMIT("movups 0x10(%[vv]), %[x1]") /* xmm1 = dx2 dy2 dz2 dw2 */
1463 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1464 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1465 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1466 __ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
1467 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
1468 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1469 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1470 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1471 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
1472 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1473 __ASM_EMIT("movups %[x1], (%[n])")
1474
1475 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1476 : [n] "r" (n), [vv] "r" (vv)
1477 : "memory"
1478 );
1479 }
1480
1481 void calc_normal3d_p3(vector3d_t *n, const point3d_t *p1, const point3d_t *p2, const point3d_t *p3)
1482 {
1483 float x0, x1, x2, x3;
1484
1485 ARCH_X86_ASM
1486 (
1487 __ASM_EMIT("movups (%[p1]), %[x2]") /* xmm0 = x1 y1 z1 w1 */
1488 __ASM_EMIT("movups (%[p2]), %[x0]") /* xmm1 = x2 y2 z2 w2 */
1489 __ASM_EMIT("movups (%[p3]), %[x1]") /* xmm2 = x3 y3 z3 w3 */
1490 __ASM_EMIT("subps %[x0], %[x1]") /* xmm0 = dx1 dy1 dz1 dw1 */
1491 __ASM_EMIT("subps %[x2], %[x0]") /* xmm1 = dx2 dy2 dz2 dw2 */
1492 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1493 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1494 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1495 __ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
1496 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
1497 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1498 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1499 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1500 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
1501 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1502 __ASM_EMIT("movups %[x1], (%[n])")
1503
1504 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1505 : [n] "r" (n), [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3)
1506 : "memory"
1507 );
1508 }
1509
1510 void calc_normal3d_pv(vector3d_t *n, const point3d_t *pv)
1511 {
1512 float x0, x1, x2, x3;
1513
1514 ARCH_X86_ASM
1515 (
1516 __ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm0 = x1 y1 z1 w1 */
1517 __ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm1 = x2 y2 z2 w2 */
1518 __ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm2 = x3 y3 z3 w3 */
1519 __ASM_EMIT("subps %[x0], %[x1]") /* xmm0 = dx1 dy1 dz1 dw1 */
1520 __ASM_EMIT("subps %[x2], %[x0]") /* xmm1 = dx2 dy2 dz2 dw2 */
1521 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1522 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1523 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1524 __ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
1525 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
1526 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1527 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1528 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1529 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
1530 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1531 __ASM_EMIT("movups %[x1], (%[n])")
1532
1533 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1534 : [n] "r" (n), [pv] "r" (pv)
1535 : "memory"
1536 );
1537 }
1538
1539 float calc_area_p3(const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
1540 {
1541 float x0, x1, x2, x3;
1542
1543 ARCH_X86_ASM
1544 (
1545 __ASM_EMIT("movups (%[p0]), %[x2]") /* xmm0 = x0 y0 z0 w0 */
1546 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm1 = x1 y1 z1 w1 */
1547 __ASM_EMIT("movups (%[p2]), %[x1]") /* xmm2 = x2 y2 z2 w2 */
1548 __ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
1549 __ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
1550 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1551 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ */
1552 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(NX*NX + NY*NY + NZ*NZ) */
1553
1554 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1555 : [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2)
1556 : "memory"
1557 );
1558
1559 return x0;
1560 }
1561
1562 float calc_area_pv(const point3d_t *pv)
1563 {
1564 float x0, x1, x2, x3;
1565
1566 ARCH_X86_ASM
1567 (
1568 __ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm0 = x0 y0 z0 w0 */
1569 __ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm1 = x1 y1 z1 w1 */
1570 __ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm2 = x2 y2 z2 w2 */
1571 __ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
1572 __ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
1573 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
1574 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ */
1575 __ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(NX*NX + NY*NY + NZ*NZ) */
1576
1577 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1578 : [pv] "r" (pv)
1579 : "memory"
1580 );
1581
1582 return x0;
1583 }
1584
1585 float calc_plane_p3(vector3d_t *v, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
1586 {
1587 float x0, x1, x2, x3;
1588
1589 ARCH_X86_ASM
1590 (
1591 __ASM_EMIT("movups (%[p0]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
1592 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
1593 __ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x2 y2 z2 w2 */
1594 __ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
1595 __ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
1596 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
1597 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1598 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1599 __ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
1600 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1601 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1602 __ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
1603 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1604 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
1605 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1606 __ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
1607 __ASM_EMIT("movups (%[p0]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
1608 __ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz nz */
1609 VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
1610 __ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
1611 __ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
1612 __ASM_EMIT("movups %[x2], (%[v])")
1613
1614 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1615 : [v] "r" (v), [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2),
1616 [X_ISIGN] "m" (X_SMASK0111)
1617 : "memory"
1618 );
1619
1620 return x0;
1621 }
1622
1623 float calc_plane_pv(vector3d_t *v, const point3d_t *pv)
1624 {
1625 float x0, x1, x2, x3;
1626
1627 ARCH_X86_ASM
1628 (
1629 __ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
1630 __ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
1631 __ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm1 = x2 y2 z2 w2 */
1632 __ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
1633 __ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
1634 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
1635 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1636 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1637 __ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
1638 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1639 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1640 __ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
1641 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1642 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
1643 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1644 __ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
1645 __ASM_EMIT("movups 0x00(%[pv]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
1646 __ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz -nz */
1647 VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
1648 __ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
1649 __ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
1650 __ASM_EMIT("movups %[x2], (%[v])")
1651
1652 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1653 : [v] "r" (v), [pv] "r" (pv),
1654 [X_ISIGN] "m" (X_SMASK0111)
1655 : "memory"
1656 );
1657
1658 return x0;
1659 }
1660
1661 float calc_plane_v1p2(vector3d_t *v, const vector3d_t *v0, const point3d_t *p0, const point3d_t *p1)
1662 {
1663 float x0, x1, x2, x3;
1664
1665 ARCH_X86_ASM
1666 (
1667 __ASM_EMIT("movups (%[p0]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
1668 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
1669 __ASM_EMIT("movups (%[v0]), %[x1]") /* xmm1 = v = dx2 dy2 dz2 0 */
1670 __ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
1671 VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
1672 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
1673 VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
1674 __ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
1675 __ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
1676 __ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
1677 __ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
1678 __ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
1679 __ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
1680 __ASM_EMIT("andps %[x3], %[x1]") /* xmm1 = (NX/W) & [W!=0] (NY/W) & [W!=0] (NZ/W) & [W!=0] (NZ/W) & [W!=0] = nx ny nz nz */
1681 __ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
1682 __ASM_EMIT("movups (%[p0]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
1683 __ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz -nz */
1684 VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
1685 __ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
1686 __ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
1687 __ASM_EMIT("movups %[x2], (%[v])")
1688
1689 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1690 : [v] "r" (v), [v0] "r" (v0), [p0] "r" (p0), [p1] "r" (p1),
1691 [X_ISIGN] "m" (X_SMASK0111)
1692 : "memory"
1693 );
1694
1695 return x0;
1696 }
1697
1698 void calc_split_point_p2v1(point3d_t *sp, const point3d_t *l0, const point3d_t *l1, const vector3d_t *pl)
1699 {
1700 float x0, x1, x2, x3, x4, x5, x6;
1701
1702 ARCH_X86_ASM
1703 (
1704 __ASM_EMIT("movups (%[l0]), %[x1]") /* xmm1 = l0 = lx0 ly0 lz0 ? */
1705 __ASM_EMIT("movups (%[l1]), %[x0]") /* xmm0 = l1 = lx1 ly1 lz1 ? */
1706 __ASM_EMIT("movups (%[pl]), %[x2]") /* xmm2 = pl = nx ny nz nw */
1707 __ASM_EMIT("subps %[x1], %[x0]") /* xmm0 = d = l1 - l0 = dx dy dz ? */
1708 __ASM_EMIT("movaps %[x2], %[x3]") /* xmm3 = pl */
1709 __ASM_EMIT("movaps %[x2], %[x4]") /* xmm4 = pl */
1710 __ASM_EMIT("mulps %[x1], %[x2]") /* xmm2 = l0 * pl = lx0*nx ly0*ny lz0*nz ? */
1711 __ASM_EMIT("mulps %[x0], %[x3]") /* xmm3 = d * pl = dx*nx dy*ny dz*nz ? */
1712 __ASM_EMIT("movhlps %[x2], %[x5]") /* xmm5 = lz0*nz ? */
1713 __ASM_EMIT("shufps $0xff, %[x4], %[x4]") /* xmm4 = nw nw nw nw */
1714 __ASM_EMIT("addss %[x2], %[x5]") /* xmm5 = lz0*nz + lx0*nx */
1715 __ASM_EMIT("movhlps %[x3], %[x6]") /* xmm6 = dz*nz ? */
1716 __ASM_EMIT("shufps $0x55, %[x2], %[x2]") /* xmm2 = ly0*ny ly0*ny ly0*ny ly0*ny */
1717 __ASM_EMIT("addss %[x3], %[x6]") /* xmm6 = dz*nz + dx*nx */
1718 __ASM_EMIT("addss %[x5], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz */
1719 __ASM_EMIT("shufps $0x55, %[x3], %[x3]") /* xmm3 = dy*ny dy*ny dy*ny dy*ny */
1720 __ASM_EMIT("addss %[x4], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz + nw */
1721 __ASM_EMIT("addss %[x6], %[x3]") /* xmm3 = dx*nx + dy*ny + dz*nz */
1722 __ASM_EMIT("divss %[x3], %[x2]") /* xmm2 = t = (lx0*nx + ly0*ny + lz0*nz + nw) / (dx*nx + dy*ny + dz*nz) */
1723 __ASM_EMIT("shufps $0x00, %[x2], %[x2]") /* xmm2 = t t t t */
1724 __ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = dx*t dy*t dz*t ? */
1725 __ASM_EMIT("subps %[x0], %[x1]") /* xmm1 = l0 - d*t */
1726 __ASM_EMIT("andps %[X_3DMASK], %[x1]")
1727 __ASM_EMIT("orps %[X_3DPOINT], %[x1]")
1728 __ASM_EMIT("movups %[x1], (%[sp])")
1729 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1730 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6)
1731 : [sp] "r" (sp), [l0] "r" (l0), [l1] "r" (l1), [pl] "r" (pl),
1732 [X_3DMASK] "m" (X_3DMASK),
1733 [X_3DPOINT] "m" (X_3DPOINT)
1734 : "memory"
1735 );
1736 }
1737
1738 void calc_split_point_pvv1(point3d_t *sp, const point3d_t *lv, const vector3d_t *pl)
1739 {
1740 float x0, x1, x2, x3, x4, x5, x6;
1741
1742 ARCH_X86_ASM
1743 (
1744 __ASM_EMIT("movups 0x00(%[lv]), %[x1]") /* xmm1 = l0 = lx0 ly0 lz0 ? */
1745 __ASM_EMIT("movups 0x10(%[lv]), %[x0]") /* xmm0 = l1 = lx1 ly1 lz1 ? */
1746 __ASM_EMIT("movups (%[pl]), %[x2]") /* xmm2 = pl = nx ny nz nw */
1747 __ASM_EMIT("subps %[x1], %[x0]") /* xmm0 = d = l1 - l0 = dx dy dz ? */
1748 __ASM_EMIT("movaps %[x2], %[x3]") /* xmm3 = pl */
1749 __ASM_EMIT("movaps %[x2], %[x4]") /* xmm4 = pl */
1750 __ASM_EMIT("mulps %[x1], %[x2]") /* xmm2 = l0 * pl = lx0*nx ly0*ny lz0*nz ? */
1751 __ASM_EMIT("mulps %[x0], %[x3]") /* xmm3 = d * pl = dx*nx dy*ny dz*nz ? */
1752 __ASM_EMIT("movhlps %[x2], %[x5]") /* xmm5 = lz0*nz ? */
1753 __ASM_EMIT("shufps $0xff, %[x4], %[x4]") /* xmm4 = nw nw nw nw */
1754 __ASM_EMIT("addss %[x2], %[x5]") /* xmm5 = lz0*nz + lx0*nx */
1755 __ASM_EMIT("movhlps %[x3], %[x6]") /* xmm6 = dz*nz ? */
1756 __ASM_EMIT("shufps $0x55, %[x2], %[x2]") /* xmm2 = ly0*ny ly0*ny ly0*ny ly0*ny */
1757 __ASM_EMIT("addss %[x3], %[x6]") /* xmm6 = dz*nz + dx*nx */
1758 __ASM_EMIT("addss %[x5], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz */
1759 __ASM_EMIT("shufps $0x55, %[x3], %[x3]") /* xmm3 = dy*ny dy*ny dy*ny dy*ny */
1760 __ASM_EMIT("addss %[x4], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz + nw */
1761 __ASM_EMIT("addss %[x6], %[x3]") /* xmm3 = dx*nx + dy*ny + dz*nz */
1762 __ASM_EMIT("divss %[x3], %[x2]") /* xmm2 = t = (lx0*nx + ly0*ny + lz0*nz + nw) / (dx*nx + dy*ny + dz*nz) */
1763 __ASM_EMIT("shufps $0x00, %[x2], %[x2]") /* xmm2 = t t t t */
1764 __ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = dx*t dy*t dz*t ? */
1765 __ASM_EMIT("subps %[x0], %[x1]") /* xmm1 = l0 - d*t */
1766 __ASM_EMIT("andps %[X_3DMASK], %[x1]")
1767 __ASM_EMIT("orps %[X_3DPOINT], %[x1]")
1768 __ASM_EMIT("movups %[x1], (%[sp])")
1769 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1770 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6)
1771 : [sp] "r" (sp), [lv] "r" (lv), [pl] "r" (pl),
1772 [X_3DMASK] "m" (X_3DMASK),
1773 [X_3DPOINT] "m" (X_3DPOINT)
1774 : "memory"
1775 );
1776 }
1777
1778 float calc_min_distance_p3(const point3d_t *sp, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
1779 {
1780 float x0, x1, x2, x3, x4;
1781
1782 ARCH_X86_ASM
1783 (
1784 __ASM_EMIT("movups (%[sp]), %[x3]") /* xmm3 = pl */
1785 __ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
1786 __ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
1787 __ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */
1788
1789 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = d0 */
1790 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = d1 */
1791 __ASM_EMIT("mulps %[x0], %[x0]") /* xmm0 = d0 * d0 */
1792 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = d2 */
1793 __ASM_EMIT("mulps %[x1], %[x1]") /* xmm1 = d1 * d1 */
1794 __ASM_EMIT("mulps %[x2], %[x2]") /* xmm2 = d2 * d2 */
1795 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
1796 /* xmm0 = x0 x1 x2 ? */
1797 /* xmm1 = y0 y1 y2 ? */
1798 /* xmm2 = z0 z1 z2 ? */
1799 /* xmm3 = w0 w1 w2 ? */
1800 __ASM_EMIT("addps %[x1], %[x0]")
1801 __ASM_EMIT("addps %[x3], %[x2]")
1802 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
1803 __ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k1) sqrtf(k2) ? */
1804 __ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = sqrtf(k2) */
1805 __ASM_EMIT("unpcklps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k0) sqrtf(k1) sqrtf(k1) */
1806 __ASM_EMIT("movhlps %[x0], %[x2]") /* xmm1 = sqrtf(k2) */
1807 __ASM_EMIT("minss %[x1], %[x0]")
1808 __ASM_EMIT("minss %[x2], %[x0]")
1809
1810 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1811 [x4] "=&x" (x4)
1812 : [sp] "r" (sp), [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2)
1813 :
1814 );
1815
1816 return x0;
1817 }
1818
1819 float calc_min_distance_pv(const point3d_t *sp, const point3d_t *pv)
1820 {
1821 float x0, x1, x2, x3, x4;
1822
1823 ARCH_X86_ASM
1824 (
1825 __ASM_EMIT("movups (%[sp]), %[x3]") /* xmm3 = pl */
1826 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
1827 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
1828 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */
1829
1830 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = d0 */
1831 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = d1 */
1832 __ASM_EMIT("mulps %[x0], %[x0]") /* xmm0 = d0 * d0 */
1833 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = d2 */
1834 __ASM_EMIT("mulps %[x1], %[x1]") /* xmm1 = d1 * d1 */
1835 __ASM_EMIT("mulps %[x2], %[x2]") /* xmm2 = d2 * d2 */
1836 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
1837 /* xmm0 = x0 x1 x2 ? */
1838 /* xmm1 = y0 y1 y2 ? */
1839 /* xmm2 = z0 z1 z2 ? */
1840 /* xmm3 = w0 w1 w2 ? */
1841 __ASM_EMIT("addps %[x1], %[x0]")
1842 __ASM_EMIT("addps %[x3], %[x2]")
1843 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
1844 __ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k1) sqrtf(k2) ? */
1845 __ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = sqrtf(k2) */
1846 __ASM_EMIT("unpcklps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k0) sqrtf(k1) sqrtf(k1) */
1847 __ASM_EMIT("movhlps %[x0], %[x2]") /* xmm1 = sqrtf(k2) */
1848 __ASM_EMIT("minss %[x1], %[x0]")
1849 __ASM_EMIT("minss %[x2], %[x0]")
1850
1851 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1852 [x4] "=&x" (x4)
1853 : [sp] "r" (sp), [pv] "r" (pv)
1854 :
1855 );
1856
1857 return x0;
1858 }
1859
1860 size_t colocation_x2_v1p2(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1)
1861 {
1862 float x0, x1, x2, x3;
1863
1864 ARCH_X86_ASM
1865 (
1866 __ASM_EMIT("movups (%[pl]), %[x2]") /* xmm3 = pl */
1867 __ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
1868 __ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
1869
1870 __ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = x0*px y0*py z0*pz w0*pw */
1871 __ASM_EMIT("mulps %[x2], %[x1]") /* xmm1 = x1*px y1*py z1*pz w1*pw */
1872 __ASM_EMIT("movaps %[x0], %[x3]") /* xmm3 = x0*px y0*py z0*pz w0*pw */
1873 __ASM_EMIT("movlhps %[x1], %[x0]") /* xmm0 = x0*px y0*py x1*px y1*py */
1874 __ASM_EMIT("movhlps %[x3], %[x1]") /* xmm1 = z0*pz w0*pw z1*pz w1*pw */
1875 __ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+z0*pz y0*py+w0*pw x1*px+z1*pz y1*py+w1*pw */
1876 __ASM_EMIT("shufps $0xd8, %[x0], %[x0]") /* xmm0 = x0*px+z0*pz x1*px+z1*pz y0*py+w0*pw y1*py+w1*pw */
1877 __ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = y0*py+w0*pw y1*py+w1*pw ? ? */
1878 __ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+y0*py+z0*pz+w0*pw x1*px+y1*py+z1*pz+w1*pw ? ? = k0 k1 ? ? */
1879 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 ? ? */
1880
1881 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL ? ? */
1882 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL ? ? */
1883 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] ? ? */
1884 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] ? ? */
1885 __ASM_EMIT("addps %[x1], %[x0]")
1886 __ASM_EMIT32("cvttss2si %[x0], %[p0]")
1887 __ASM_EMIT64("cvttss2si %[x0], %k[p0]")
1888 __ASM_EMIT("shufps $0x55, %[x0], %[x0]")
1889 __ASM_EMIT32("cvttss2si %[x0], %[p1]")
1890 __ASM_EMIT64("cvttss2si %[x0], %k[p1]")
1891 __ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]")
1892 : [p0] "+r" (p0), [p1] "+r" (p1),
1893 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1894 : [pl] "r" (pl),
1895 [PTOL] "m" (X_3D_TOLERANCE),
1896 [MTOL] "m" (X_3D_MTOLERANCE),
1897 [ONE] "m" (ONE)
1898 : "cc"
1899 );
1900
1901 return size_t(p0);
1902 }
1903
1904 size_t colocation_x2_v1pv(const vector3d_t *pl, const point3d_t *pv)
1905 {
1906 float x0, x1, x2, x3;
1907
1908 ARCH_X86_ASM
1909 (
1910 __ASM_EMIT("movups (%[pl]), %[x2]") /* xmm3 = pl */
1911 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
1912 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
1913
1914 __ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = x0*px y0*py z0*pz w0*pw */
1915 __ASM_EMIT("mulps %[x2], %[x1]") /* xmm1 = x1*px y1*py z1*pz w1*pw */
1916 __ASM_EMIT("movaps %[x0], %[x3]") /* xmm3 = x0*px y0*py z0*pz w0*pw */
1917 __ASM_EMIT("movlhps %[x1], %[x0]") /* xmm0 = x0*px y0*py x1*px y1*py */
1918 __ASM_EMIT("movhlps %[x3], %[x1]") /* xmm1 = z0*pz w0*pw z1*pz w1*pw */
1919 __ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+z0*pz y0*py+w0*pw x1*px+z1*pz y1*py+w1*pw */
1920 __ASM_EMIT("shufps $0xd8, %[x0], %[x0]") /* xmm0 = x0*px+z0*pz x1*px+z1*pz y0*py+w0*pw y1*py+w1*pw */
1921 __ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = y0*py+w0*pw y1*py+w1*pw ? ? */
1922 __ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+y0*py+z0*pz+w0*pw x1*px+y1*py+z1*pz+w1*pw ? ? = k0 k1 ? ? */
1923 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 ? ? */
1924
1925 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL ? ? */
1926 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL ? ? */
1927 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] ? ? */
1928 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] ? ? */
1929 __ASM_EMIT("addps %[x1], %[x0]")
1930 __ASM_EMIT32("cvttss2si %[x0], %[pl]")
1931 __ASM_EMIT64("cvttss2si %[x0], %k[pl]")
1932 __ASM_EMIT("shufps $0x55, %[x0], %[x0]")
1933 __ASM_EMIT32("cvttss2si %[x0], %[pv]")
1934 __ASM_EMIT64("cvttss2si %[x0], %k[pv]")
1935 __ASM_EMIT("lea (%[pl], %[pv], 4), %[pl]")
1936
1937 : [pl] "+r" (pl), [pv] "+r" (pv),
1938 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
1939 : [PTOL] "m" (X_3D_TOLERANCE),
1940 [MTOL] "m" (X_3D_MTOLERANCE),
1941 [ONE] "m" (ONE)
1942 : "cc"
1943 );
1944
1945 return size_t(pl);
1946 }
1947
1948 size_t colocation_x3_v1p3(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
1949 {
1950 float x0, x1, x2, x3, x4;
1951
1952 ARCH_X86_ASM
1953 (
1954 __ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
1955 __ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
1956 __ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
1957 __ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */
1958
1959 __ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = p0 * pl */
1960 __ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = p1 * pl */
1961 __ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = p2 * pl */
1962 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
1963 /* xmm0 = x0 x1 x2 ? */
1964 /* xmm1 = y0 y1 y2 ? */
1965 /* xmm2 = z0 z1 z2 ? */
1966 /* xmm3 = w0 w1 w2 ? */
1967 __ASM_EMIT("addps %[x1], %[x0]")
1968 __ASM_EMIT("addps %[x3], %[x2]")
1969 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
1970 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
1971 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
1972 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
1973 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
1974 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
1975 __ASM_EMIT("addps %[x1], %[x0]")
1976 __ASM_EMIT32("cvttss2si %[x0], %[p0]")
1977 __ASM_EMIT64("cvttss2si %[x0], %k[p0]")
1978 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
1979 __ASM_EMIT32("cvttss2si %[x0], %[p1]")
1980 __ASM_EMIT64("cvttss2si %[x0], %k[p1]")
1981 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
1982 __ASM_EMIT32("cvttss2si %[x0], %[p2]")
1983 __ASM_EMIT64("cvttss2si %[x0], %k[p2]")
1984 __ASM_EMIT("lea (%[p1], %[p2], 4), %[p1]")
1985 __ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]")
1986 : [p0] "+r" (p0), [p1] "+r" (p1), [p2] "+r" (p2),
1987 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
1988 [x4] "=&x" (x4)
1989 : [pl] "r" (pl),
1990 [PTOL] "m" (X_3D_TOLERANCE),
1991 [MTOL] "m" (X_3D_MTOLERANCE),
1992 [ONE] "m" (ONE)
1993 : "cc"
1994 );
1995
1996 return size_t(p0);
1997 }
1998
1999 size_t colocation_x3_v1pv(const vector3d_t *pl, const point3d_t *pv)
2000 {
2001 float x0, x1, x2, x3, x4;
2002 size_t pt;
2003
2004 ARCH_X86_ASM
2005 (
2006 __ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
2007 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
2008 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
2009 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */
2010
2011 __ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = p0 * pl */
2012 __ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = p1 * pl */
2013 __ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = p2 * pl */
2014 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
2015 /* xmm0 = x0 x1 x2 ? */
2016 /* xmm1 = y0 y1 y2 ? */
2017 /* xmm2 = z0 z1 z2 ? */
2018 /* xmm3 = w0 w1 w2 ? */
2019 __ASM_EMIT("addps %[x1], %[x0]")
2020 __ASM_EMIT("addps %[x3], %[x2]")
2021 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
2022 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
2023 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
2024 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
2025 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
2026 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
2027 __ASM_EMIT("addps %[x1], %[x0]")
2028 __ASM_EMIT32("cvttss2si %[x0], %[pl]")
2029 __ASM_EMIT64("cvttss2si %[x0], %k[pl]")
2030 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2031 __ASM_EMIT32("cvttss2si %[x0], %[pv]")
2032 __ASM_EMIT64("cvttss2si %[x0], %k[pv]")
2033 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2034 __ASM_EMIT32("cvttss2si %[x0], %[pt]")
2035 __ASM_EMIT64("cvttss2si %[x0], %k[pt]")
2036 __ASM_EMIT("lea (%[pv], %[pt], 4), %[pt]")
2037 __ASM_EMIT("lea (%[pl], %[pt], 4), %[pt]")
2038 : [pl] "+r" (pl), [pv] "+r" (pv), [pt] "=&r" (pt),
2039 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
2040 [x4] "=&x" (x4)
2041 : [PTOL] "m" (X_3D_TOLERANCE),
2042 [MTOL] "m" (X_3D_MTOLERANCE),
2043 [ONE] "m" (ONE)
2044 : "cc"
2045 );
2046
2047 return pt;
2048 }
2049
2050 size_t colocation_x3_v3p1(const vector3d_t *v0, const vector3d_t *v1, const vector3d_t *v2, const point3d_t *p)
2051 {
2052 float x0, x1, x2, x3, x4;
2053
2054 ARCH_X86_ASM
2055 (
2056 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = p */
2057 __ASM_EMIT("movups (%[v0]), %[x0]") /* xmm0 = v0 */
2058 __ASM_EMIT("movups (%[v1]), %[x1]") /* xmm1 = v1 */
2059 __ASM_EMIT("movups (%[v2]), %[x2]") /* xmm2 = v2 */
2060
2061 __ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = v0 * p */
2062 __ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = v1 * p */
2063 __ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = v2 * p */
2064 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
2065 /* xmm0 = x0 x1 x2 ? */
2066 /* xmm1 = y0 y1 y2 ? */
2067 /* xmm2 = z0 z1 z2 ? */
2068 /* xmm3 = w0 w1 w2 ? */
2069 __ASM_EMIT("addps %[x1], %[x0]")
2070 __ASM_EMIT("addps %[x3], %[x2]")
2071 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
2072 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
2073 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
2074 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
2075 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
2076 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
2077 __ASM_EMIT("addps %[x1], %[x0]")
2078 __ASM_EMIT32("cvttss2si %[x0], %[v0]")
2079 __ASM_EMIT64("cvttss2si %[x0], %k[v0]")
2080 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2081 __ASM_EMIT32("cvttss2si %[x0], %[v1]")
2082 __ASM_EMIT64("cvttss2si %[x0], %k[v1]")
2083 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2084 __ASM_EMIT32("cvttss2si %[x0], %[v2]")
2085 __ASM_EMIT64("cvttss2si %[x0], %k[v2]")
2086 __ASM_EMIT("lea (%[v1], %[v2], 4), %[v1]")
2087 __ASM_EMIT("lea (%[v0], %[v1], 4), %[v0]")
2088
2089 : [v0] "+r" (v0), [v1] "+r" (v1), [v2] "+r" (v2),
2090 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
2091 [x4] "=&x" (x4)
2092 : [p] "r" (p),
2093 [PTOL] "m" (X_3D_TOLERANCE),
2094 [MTOL] "m" (X_3D_MTOLERANCE),
2095 [ONE] "m" (ONE)
2096 : "cc"
2097 );
2098
2099 return size_t(v0);
2100 }
2101
2102 size_t colocation_x3_vvp1(const vector3d_t *vv, const point3d_t *p)
2103 {
2104 float x0, x1, x2, x3, x4;
2105 size_t pt;
2106
2107 ARCH_X86_ASM
2108 (
2109 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = p */
2110 __ASM_EMIT("movups 0x00(%[vv]), %[x0]") /* xmm0 = v0 */
2111 __ASM_EMIT("movups 0x10(%[vv]), %[x1]") /* xmm1 = v1 */
2112 __ASM_EMIT("movups 0x20(%[vv]), %[x2]") /* xmm2 = v2 */
2113
2114 __ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = v0 * p */
2115 __ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = v1 * p */
2116 __ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = v2 * p */
2117 MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
2118 /* xmm0 = x0 x1 x2 ? */
2119 /* xmm1 = y0 y1 y2 ? */
2120 /* xmm2 = z0 z1 z2 ? */
2121 /* xmm3 = w0 w1 w2 ? */
2122 __ASM_EMIT("addps %[x1], %[x0]")
2123 __ASM_EMIT("addps %[x3], %[x2]")
2124 __ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
2125 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
2126 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
2127 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
2128 __ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
2129 __ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
2130 __ASM_EMIT("addps %[x1], %[x0]")
2131 __ASM_EMIT32("cvttss2si %[x0], %[vv]")
2132 __ASM_EMIT64("cvttss2si %[x0], %k[vv]")
2133 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2134 __ASM_EMIT32("cvttss2si %[x0], %[p]")
2135 __ASM_EMIT64("cvttss2si %[x0], %k[p]")
2136 __ASM_EMIT("shufps $0x39, %[x0], %[x0]")
2137 __ASM_EMIT32("cvttss2si %[x0], %[pt]")
2138 __ASM_EMIT64("cvttss2si %[x0], %k[pt]")
2139 __ASM_EMIT("lea (%[p], %[pt], 4), %[pt]")
2140 __ASM_EMIT("lea (%[vv], %[pt], 4), %[pt]")
2141
2142 : [vv] "+r" (vv), [p] "+r" (p), [pt] "=&r" (pt),
2143 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
2144 [x4] "=&x" (x4)
2145 : [PTOL] "m" (X_3D_TOLERANCE),
2146 [MTOL] "m" (X_3D_MTOLERANCE),
2147 [ONE] "m" (ONE)
2148 : "cc"
2149 );
2150
2151 return pt;
2152 }
2153 } /* namespace sse */
2154 } /* namespace lsp */
2155
2156 #undef SCALAR_MUL
2157 #undef MAT3_TRANSPOSE
2158 #undef MAT4_TRANSPOSE
2159 #undef MATRIX_LOAD
2160 #undef MATRIX_STORE
2161 #undef NORMALIZE
2162 #undef TRIPLET_CHECK
2163 #undef SCALAR_MUL3
2164 #undef VECTOR_MUL3
2165 #undef VECTOR_MUL
2166 #undef VECTOR_XCHG
2167
2168 #endif /* PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_ */