| 1 |
/* |
| 2 |
* Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/> |
| 3 |
* (C) 2020 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_SSE4_3DMATH_H_ |
| 23 |
#define PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_ |
| 24 |
|
| 25 |
#ifndef PRIVATE_DSP_ARCH_X86_SSE4_IMPL |
| 26 |
#error "This header should not be included directly" |
| 27 |
#endif /* PRIVATE_DSP_ARCH_X86_SSE4_IMPL */ |
| 28 |
|
| 29 |
namespace lsp |
| 30 |
{ |
| 31 |
namespace sse4 |
| 32 |
{ |
| 33 |
using namespace dsp; |
| 34 |
|
| 35 |
IF_ARCH_X86( |
| 36 |
static const uint32_t X_MASK0111[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 }; |
| 37 |
static const float X_3DPOINT[] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f }; |
| 38 |
static const float X_3D_TOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(DSP_3D_TOLERANCE) }; |
| 39 |
static const float X_3D_MTOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(-DSP_3D_TOLERANCE) }; |
| 40 |
static const uint32_t IONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1) }; |
| 41 |
) |
| 42 |
|
| 43 |
/* Input: |
| 44 |
* x0 = a0 a1 a2 a3 |
| 45 |
* x1 = b0 b1 b2 b3 |
| 46 |
* |
| 47 |
* Output: |
| 48 |
* x0 = b0 b1 b2 b3 |
| 49 |
* x1 = a0 a1 a2 a3 |
| 50 |
*/ |
| 51 |
#define VECTOR_XCHG(x0, x1) \ |
| 52 |
__ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = b ^ a */ \ |
| 53 |
__ASM_EMIT("xorps %" x1 ", %" x0) /* xmm0 = a ^ ( b ^ a) = b */ \ |
| 54 |
__ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = (b ^ a) ^ b = a */ |
| 55 |
|
| 56 |
/* 3x vector multiplication |
| 57 |
* Input: |
| 58 |
* x0 = vector1 [dx dy dz ? ] |
| 59 |
* x1 = vector2 [dx dy dz ? ] |
| 60 |
* x2 = vector3 [dx dy dz ? ] |
| 61 |
* |
| 62 |
* Output: |
| 63 |
* x0 = vector1 * vector2 [ vz vx vy ? ] |
| 64 |
* x1 = vector2 * vector3 [ vz vx vy ? ] |
| 65 |
* x2 = vector3 * vector1 [ vz vx vy ? ] |
| 66 |
*/ |
| 67 |
#define VECTOR_MUL3(x0, x1, x2, x3, x4, x5, x6, x7) \ |
| 68 |
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = dx0 dy0 dz0 dw0 */ \ |
| 69 |
__ASM_EMIT("movaps %" x1 ", %" x4) /* xmm4 = dx1 dy1 dz1 dw1 */ \ |
| 70 |
__ASM_EMIT("movaps %" x2 ", %" x5) /* xmm5 = dx2 dy2 dz2 dw2 */ \ |
| 71 |
__ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy0 dz0 dx0 dw0 */ \ |
| 72 |
__ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = dy1 dz1 dx1 dw1 */ \ |
| 73 |
__ASM_EMIT("shufps $0xc9, %" x5 ", %" x5) /* xmm5 = dy2 dz2 dx2 dw2 */ \ |
| 74 |
__ASM_EMIT("movaps %" x0 ", %" x6) /* xmm6 = dx0 dy0 dz0 dw0 */ \ |
| 75 |
__ASM_EMIT("movaps %" x3 ", %" x7) /* xmm7 = dy0 dz0 dx0 dw0 */ \ |
| 76 |
\ |
| 77 |
__ASM_EMIT("mulps %" x4 ", %" x0) /* xmm0 = dx0*dy1 dy0*dz1 dz0*dx1 dw0*dw1 */ \ |
| 78 |
__ASM_EMIT("mulps %" x1 ", %" x3) /* xmm3 = dy0*dx1 dz0*dy1 dx0*dz1 dw0*dw1 */ \ |
| 79 |
__ASM_EMIT("mulps %" x5 ", %" x1) /* xmm1 = dx1*dy2 dy1*dz2 dz1*dx2 dw1*dw2 */ \ |
| 80 |
__ASM_EMIT("mulps %" x2 ", %" x4) /* xmm4 = dy1*dx2 dz1*dy2 dx1*dz2 dw1*dw2 */ \ |
| 81 |
__ASM_EMIT("mulps %" x7 ", %" x2) /* xmm2 = dx2*dy0 dy2*dz0 dz2*dx0 dw2*dw0 */ \ |
| 82 |
__ASM_EMIT("mulps %" x6 ", %" x5) /* xmm5 = dy2*dx0 dz2*dy0 dx2*dz0 dw2*dw0 */ \ |
| 83 |
__ASM_EMIT("subps %" x3 ", %" x0) /* xmm0 = nz0 nx0 ny0 nw0 */ \ |
| 84 |
__ASM_EMIT("subps %" x4 ", %" x1) /* xmm1 = nz1 nx1 ny1 nw1 */ \ |
| 85 |
__ASM_EMIT("subps %" x5 ", %" x2) /* xmm2 = nz2 nx2 ny2 nw2 */ |
| 86 |
|
| 87 |
/* 3x scalar multiplication |
| 88 |
* Input: |
| 89 |
* x0 = vector1 [dx dy dz ? ] |
| 90 |
* x1 = vector2 [dx dy dz ? ] |
| 91 |
* x2 = vector3 [dx dy dz ? ] |
| 92 |
* |
| 93 |
* Output: |
| 94 |
* x0 = vector1 * vector2 [ S0 ? ? ? ] |
| 95 |
* x1 = vector2 * vector3 [ S1 ? ? ? ] |
| 96 |
* x2 = vector3 * vector1 [ S2 ? ? ? ] |
| 97 |
*/ |
| 98 |
#define SCALAR_MUL3(x0, x1, x2, x3) \ |
| 99 |
__ASM_EMIT("movaps %" x0 ", %" x3) \ |
| 100 |
__ASM_EMIT("dpps $0x71, %" x1 ", %" x0) /* xmm0 = dx0*dx1+dy0*dy1+dz0*dz1 */ \ |
| 101 |
__ASM_EMIT("dpps $0x71, %" x2 ", %" x1) /* xmm1 = dx0*dx1+dy0*dy1+dz0*dz1 */ \ |
| 102 |
__ASM_EMIT("dpps $0x71, %" x3 ", %" x2) /* xmm2 = dx0*dx1+dy0*dy1+dz0*dz1 */ |
| 103 |
|
| 104 |
/* |
| 105 |
* Input: |
| 106 |
* x0 = vector 1 = dx1 dy1 dz1 dw1 |
| 107 |
* x1 = vector 2 = dx2 dy2 dz2 dw2 |
| 108 |
* |
| 109 |
* Output: |
| 110 |
* x0 = dx1*dx2 + dy1*dy2 + dz1*dz2 |
| 111 |
*/ |
| 112 |
#define SCALAR_MUL(x0, x1, x2) \ |
| 113 |
__ASM_EMIT("dpps $0x71, %" x1 ", %" x0) /* xmm0 = dx1*dx2 + dy1*dy2 + dz1*dz2 */ |
| 114 |
|
| 115 |
/* |
| 116 |
* Input: |
| 117 |
* x0 = vector/point to normalize |
| 118 |
* Output: |
| 119 |
* x0 = normalized vector |
| 120 |
*/ |
| 121 |
#define NORMALIZE(x0, x1, x2) \ |
| 122 |
__ASM_EMIT("movaps %" x0 ", %" x1) /* xmm1 = dx dy dz dw */ \ |
| 123 |
__ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \ |
| 124 |
__ASM_EMIT("dpps $0x7f, %" x1 ", %" x1) /* xmm1 = dx*dx+dy*dy+dz*dz */ \ |
| 125 |
__ASM_EMIT("sqrtps %" x1", %" x1) /* xmm1 = sqrt(x*x + y*y + z*z) = w */ \ |
| 126 |
__ASM_EMIT("ucomiss %" x2 ", %" x1) /* xmm2 =?= xmm1 */ \ |
| 127 |
__ASM_EMIT("jle 1000000f") \ |
| 128 |
__ASM_EMIT("divps %" x1 ", %" x0) /* xmm0 = dx/w dy/w dz/w dw/w */ \ |
| 129 |
__ASM_EMIT("1000000:") |
| 130 |
|
| 131 |
/* |
| 132 |
* Input: |
| 133 |
* x0 = vector/point to scale |
| 134 |
* x1 = radius-vector to match length |
| 135 |
* |
| 136 |
* Output: |
| 137 |
* x0 = scaled vector |
| 138 |
*/ |
| 139 |
#define SCALE(x0, x1, x2, x3) \ |
| 140 |
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = x y z w */ \ |
| 141 |
__ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \ |
| 142 |
__ASM_EMIT("dpps $0x7f, %" x3 ", %" x3) /* xmm3 = dx*dx+dy*dy+dz*dz */ \ |
| 143 |
__ASM_EMIT("shufps $0x00, %" x1 ", %" x1) /* xmm1 = r r r r */ \ |
| 144 |
__ASM_EMIT("sqrtps %" x3", %" x3) /* xmm3 = sqrt(x*x + y*y + z*z) = W W W W */ \ |
| 145 |
__ASM_EMIT("ucomiss %" x2 ", %" x3) /* xmm3 =?= xmm2 */ \ |
| 146 |
__ASM_EMIT("jle 1000000f") \ |
| 147 |
__ASM_EMIT("divps %" x3 ", %" x0) /* xmm0 = r/W r/W r/W r/W */ \ |
| 148 |
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*r/W y*r/W z*r/W w*r/W */ \ |
| 149 |
__ASM_EMIT("1000000:") |
| 150 |
|
| 151 |
void normalize_point(point3d_t *p) |
| 152 |
{ |
| 153 |
float x0, x1, x2; |
| 154 |
|
| 155 |
ARCH_X86_ASM |
| 156 |
( |
| 157 |
__ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w |
| 158 |
NORMALIZE("[x0]", "[x1]", "[x2]") |
| 159 |
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0 |
| 160 |
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1 |
| 161 |
__ASM_EMIT("movups %[x0], (%[p])") |
| 162 |
|
| 163 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2) |
| 164 |
: [p] "r" (p), |
| 165 |
[zmask] "m" (X_MASK0111), |
| 166 |
[omask] "m" (X_3DPOINT) |
| 167 |
: "cc", "memory" |
| 168 |
); |
| 169 |
} |
| 170 |
|
| 171 |
void scale_point1(point3d_t *p, float r) |
| 172 |
{ |
| 173 |
float x0, x1, x2; |
| 174 |
|
| 175 |
ARCH_X86_ASM |
| 176 |
( |
| 177 |
__ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w |
| 178 |
SCALE("[x0]", "[x3]", "[x1]", "[x2]") |
| 179 |
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0 |
| 180 |
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1 |
| 181 |
__ASM_EMIT("movups %[x0], (%[p])") |
| 182 |
|
| 183 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r) |
| 184 |
: [p] "r" (p), |
| 185 |
[zmask] "m" (X_MASK0111), |
| 186 |
[omask] "m" (X_3DPOINT) |
| 187 |
: "cc", "memory" |
| 188 |
); |
| 189 |
} |
| 190 |
|
| 191 |
void scale_point2(point3d_t *p, const point3d_t *s, float r) |
| 192 |
{ |
| 193 |
float x0, x1, x2; |
| 194 |
|
| 195 |
ARCH_X86_ASM |
| 196 |
( |
| 197 |
__ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = x y z w |
| 198 |
SCALE("[x0]", "[x3]", "[x1]", "[x2]") |
| 199 |
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0 |
| 200 |
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1 |
| 201 |
__ASM_EMIT("movups %[x0], (%[p])") |
| 202 |
|
| 203 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r) |
| 204 |
: [p] "r" (p), [s] "r" (s), |
| 205 |
[zmask] "m" (X_MASK0111), |
| 206 |
[omask] "m" (X_3DPOINT) |
| 207 |
: "cc", "memory" |
| 208 |
); |
| 209 |
} |
| 210 |
|
| 211 |
void normalize_vector(vector3d_t *v) |
| 212 |
{ |
| 213 |
float x0, x1, x2; |
| 214 |
|
| 215 |
ARCH_X86_ASM |
| 216 |
( |
| 217 |
__ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw |
| 218 |
NORMALIZE("[x0]", "[x1]", "[x2]") |
| 219 |
__ASM_EMIT("movups %[x0], (%[v])") |
| 220 |
|
| 221 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2) |
| 222 |
: [v] "r" (v) |
| 223 |
: "cc", "memory" |
| 224 |
); |
| 225 |
} |
| 226 |
|
| 227 |
void scale_vector1(vector3d_t *v, float r) |
| 228 |
{ |
| 229 |
float x0, x1, x2; |
| 230 |
|
| 231 |
ARCH_X86_ASM |
| 232 |
( |
| 233 |
__ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw |
| 234 |
SCALE("[x0]", "[x3]", "[x1]", "[x2]") |
| 235 |
__ASM_EMIT("andps %[zmask], %[x0]") |
| 236 |
__ASM_EMIT("movups %[x0], (%[v])") |
| 237 |
|
| 238 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r) |
| 239 |
: [v] "r" (v), |
| 240 |
[zmask] "m" (X_MASK0111) |
| 241 |
: "cc", "memory" |
| 242 |
); |
| 243 |
} |
| 244 |
|
| 245 |
void scale_vector2(vector3d_t *v, const vector3d_t *s, float r) |
| 246 |
{ |
| 247 |
float x0, x1, x2; |
| 248 |
|
| 249 |
ARCH_X86_ASM |
| 250 |
( |
| 251 |
__ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = dx dy dz dw |
| 252 |
SCALE("[x0]", "[x3]", "[x1]", "[x2]") |
| 253 |
__ASM_EMIT("andps %[zmask], %[x0]") |
| 254 |
__ASM_EMIT("movups %[x0], (%[v])") |
| 255 |
|
| 256 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r) |
| 257 |
: [v] "r" (v), [s] "r" (s), |
| 258 |
[zmask] "m" (X_MASK0111) |
| 259 |
: "cc", "memory" |
| 260 |
); |
| 261 |
} |
| 262 |
|
| 263 |
float check_point3d_on_triangle_pvp(const point3d_t *pv, const point3d_t *p) |
| 264 |
{ |
| 265 |
float x0, x1, x2, x3, x4, x5, x6, x7; |
| 266 |
|
| 267 |
ARCH_X86_ASM |
| 268 |
( |
| 269 |
/* Load vectors */ |
| 270 |
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */ |
| 271 |
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 w0 */ |
| 272 |
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 w1 */ |
| 273 |
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 w2 */ |
| 274 |
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */ |
| 275 |
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */ |
| 276 |
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */ |
| 277 |
/* 3x vector multiplications */ |
| 278 |
VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]") |
| 279 |
/* 3x scalar multiplications */ |
| 280 |
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]") |
| 281 |
/* Compare with zeros */ |
| 282 |
__ASM_EMIT("xorps %[x4], %[x4]") |
| 283 |
__ASM_EMIT("ucomiss %[x4], %[x0]") |
| 284 |
__ASM_EMIT("jb 110f") |
| 285 |
__ASM_EMIT("ucomiss %[x4], %[x1]") |
| 286 |
__ASM_EMIT("jb 109f") |
| 287 |
__ASM_EMIT("ucomiss %[x4], %[x2]") |
| 288 |
__ASM_EMIT("jb 108f") |
| 289 |
__ASM_EMIT("mulss %[x1], %[x0]") |
| 290 |
__ASM_EMIT("mulss %[x2], %[x0]") |
| 291 |
__ASM_EMIT("ucomiss %[x4], %[x0]") |
| 292 |
__ASM_EMIT("jne 110f") |
| 293 |
/* There is somewhere zero, need additional check */ |
| 294 |
/* Load vectors */ |
| 295 |
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */ |
| 296 |
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 w0 */ |
| 297 |
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 w1 */ |
| 298 |
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 w2 */ |
| 299 |
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */ |
| 300 |
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */ |
| 301 |
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */ |
| 302 |
/* Do 3x scalar multiplications */ |
| 303 |
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]") |
| 304 |
__ASM_EMIT("mulss %[x1], %[x0]") |
| 305 |
__ASM_EMIT("mulss %[x2], %[x0]") |
| 306 |
__ASM_EMIT("jmp 110f") |
| 307 |
|
| 308 |
__ASM_EMIT("108:") |
| 309 |
__ASM_EMIT("movss %[x2], %[x0]") |
| 310 |
__ASM_EMIT("jmp 110f") |
| 311 |
__ASM_EMIT("109:") |
| 312 |
__ASM_EMIT("movss %[x1], %[x0]") |
| 313 |
__ASM_EMIT("110:") |
| 314 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), |
| 315 |
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7) |
| 316 |
: [pv] "r" (pv), [p] "r" (p) |
| 317 |
); |
| 318 |
|
| 319 |
return x0; |
| 320 |
} |
| 321 |
|
| 322 |
float check_point3d_on_triangle_p3p(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const point3d_t *p) |
| 323 |
{ |
| 324 |
float x0, x1, x2, x3, x4, x5, x6, x7; |
| 325 |
|
| 326 |
ARCH_X86_ASM |
| 327 |
( |
| 328 |
/* Load vectors */ |
| 329 |
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */ |
| 330 |
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 w0 */ |
| 331 |
__ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 w1 */ |
| 332 |
__ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 w2 */ |
| 333 |
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */ |
| 334 |
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */ |
| 335 |
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */ |
| 336 |
/* 3x vector multiplications */ |
| 337 |
VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]") |
| 338 |
/* 3x scalar multiplications */ |
| 339 |
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]") |
| 340 |
/* Compare with zeros */ |
| 341 |
__ASM_EMIT("xorps %[x4], %[x4]") |
| 342 |
__ASM_EMIT("ucomiss %[x4], %[x0]") |
| 343 |
__ASM_EMIT("jb 110f") |
| 344 |
__ASM_EMIT("ucomiss %[x4], %[x1]") |
| 345 |
__ASM_EMIT("jb 109f") |
| 346 |
__ASM_EMIT("ucomiss %[x4], %[x2]") |
| 347 |
__ASM_EMIT("jb 108f") |
| 348 |
__ASM_EMIT("mulss %[x1], %[x0]") |
| 349 |
__ASM_EMIT("mulss %[x2], %[x0]") |
| 350 |
__ASM_EMIT("ucomiss %[x4], %[x0]") |
| 351 |
__ASM_EMIT("jne 110f") |
| 352 |
/* There is somewhere zero, need additional check */ |
| 353 |
/* Load vectors */ |
| 354 |
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */ |
| 355 |
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 w0 */ |
| 356 |
__ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 w1 */ |
| 357 |
__ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 w2 */ |
| 358 |
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */ |
| 359 |
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */ |
| 360 |
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */ |
| 361 |
/* Do 3x scalar multiplications */ |
| 362 |
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]") |
| 363 |
__ASM_EMIT("mulss %[x1], %[x0]") |
| 364 |
__ASM_EMIT("mulss %[x2], %[x0]") |
| 365 |
__ASM_EMIT("jmp 110f") |
| 366 |
|
| 367 |
__ASM_EMIT("108:") |
| 368 |
__ASM_EMIT("movss %[x2], %[x0]") |
| 369 |
__ASM_EMIT("jmp 110f") |
| 370 |
__ASM_EMIT("109:") |
| 371 |
__ASM_EMIT("movss %[x1], %[x0]") |
| 372 |
__ASM_EMIT("110:") |
| 373 |
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), |
| 374 |
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7) |
| 375 |
: [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [p] "r" (p) |
| 376 |
); |
| 377 |
|
| 378 |
return x0; |
| 379 |
} |
| 380 |
|
| 381 |
size_t colocation_v1p3(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2) |
| 382 |
{ |
| 383 |
float x0, x1, x2, x3, x4; |
| 384 |
float res[4] __lsp_aligned16; |
| 385 |
|
| 386 |
ARCH_X86_ASM |
| 387 |
( |
| 388 |
__ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */ |
| 389 |
__ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */ |
| 390 |
__ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */ |
| 391 |
__ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */ |
| 392 |
|
| 393 |
__ASM_EMIT("dpps $0xf1, %[x3], %[x0]") /* xmm0 = p0 * pl */ |
| 394 |
__ASM_EMIT("dpps $0xf2, %[x3], %[x1]") /* xmm1 = p1 * pl */ |
| 395 |
__ASM_EMIT("dpps $0xf4, %[x3], %[x2]") /* xmm2 = p2 * pl */ |
| 396 |
__ASM_EMIT("orps %[x1], %[x0]") /* xmm0 = k0 k1 0 0 */ |
| 397 |
__ASM_EMIT("orps %[x2], %[x0]") /* xmm0 = k0 k1 k2 0 */ |
| 398 |
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 0 */ |
| 399 |
|
| 400 |
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */ |
| 401 |
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */ |
| 402 |
__ASM_EMIT("andps %[IONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */ |
| 403 |
__ASM_EMIT("andps %[IONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */ |
| 404 |
__ASM_EMIT("paddd %[x1], %[x0]") |
| 405 |
__ASM_EMIT("movdqa %[x0], (%[res])") |
| 406 |
__ASM_EMIT32("movl 0x00(%[res]), %[p0]") |
| 407 |
__ASM_EMIT32("movl 0x04(%[res]), %[p1]") |
| 408 |
__ASM_EMIT32("movl 0x08(%[res]), %[p2]") |
| 409 |
__ASM_EMIT64("movl 0x00(%[res]), %k[p0]") |
| 410 |
__ASM_EMIT64("movl 0x04(%[res]), %k[p1]") |
| 411 |
__ASM_EMIT64("movl 0x08(%[res]), %k[p2]") |
| 412 |
__ASM_EMIT("lea (%[p1], %[p2], 4), %[p1]") |
| 413 |
__ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]") |
| 414 |
: [p0] "+r" (p0), [p1] "+r" (p1), [p2] "+r" (p2), |
| 415 |
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), |
| 416 |
[x4] "=&x" (x4) |
| 417 |
: [pl] "r" (pl), |
| 418 |
[res] "r" (res), |
| 419 |
[PTOL] "m" (X_3D_TOLERANCE), |
| 420 |
[MTOL] "m" (X_3D_MTOLERANCE), |
| 421 |
[IONE] "m" (IONE) |
| 422 |
: "cc" |
| 423 |
); |
| 424 |
|
| 425 |
return size_t(p0); |
| 426 |
} |
| 427 |
|
| 428 |
size_t colocation_v1pv(const vector3d_t *pl, const point3d_t *pv) |
| 429 |
{ |
| 430 |
float x0, x1, x2, x3, x4; |
| 431 |
float res[4] __lsp_aligned16; |
| 432 |
size_t pt; |
| 433 |
|
| 434 |
ARCH_X86_ASM |
| 435 |
( |
| 436 |
__ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */ |
| 437 |
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */ |
| 438 |
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */ |
| 439 |
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */ |
| 440 |
|
| 441 |
__ASM_EMIT("dpps $0xf1, %[x3], %[x0]") /* xmm0 = p0 * pl */ |
| 442 |
__ASM_EMIT("dpps $0xf2, %[x3], %[x1]") /* xmm1 = p1 * pl */ |
| 443 |
__ASM_EMIT("dpps $0xf4, %[x3], %[x2]") /* xmm2 = p2 * pl */ |
| 444 |
__ASM_EMIT("orps %[x1], %[x0]") /* xmm0 = k0 k1 0 0 */ |
| 445 |
__ASM_EMIT("orps %[x2], %[x0]") /* xmm0 = k0 k1 k2 0 */ |
| 446 |
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 0 */ |
| 447 |
|
| 448 |
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */ |
| 449 |
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */ |
| 450 |
__ASM_EMIT("andps %[IONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */ |
| 451 |
__ASM_EMIT("andps %[IONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */ |
| 452 |
__ASM_EMIT("paddd %[x1], %[x0]") |
| 453 |
__ASM_EMIT("movdqa %[x0], (%[res])") |
| 454 |
__ASM_EMIT32("movl 0x00(%[res]), %[pt]") |
| 455 |
__ASM_EMIT32("movl 0x04(%[res]), %[pl]") |
| 456 |
__ASM_EMIT32("movl 0x08(%[res]), %[pv]") |
| 457 |
__ASM_EMIT64("movl 0x00(%[res]), %k[pt]") |
| 458 |
__ASM_EMIT64("movl 0x04(%[res]), %k[pl]") |
| 459 |
__ASM_EMIT64("movl 0x08(%[res]), %k[pv]") |
| 460 |
__ASM_EMIT("lea (%[pl], %[pv], 4), %[pl]") |
| 461 |
__ASM_EMIT("lea (%[pt], %[pl], 4), %[pt]") |
| 462 |
: [pl] "+r" (pl), [pv] "+r" (pv), [pt] "=&r" (pt), |
| 463 |
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), |
| 464 |
[x4] "=&x" (x4) |
| 465 |
: [res] "r" (res), |
| 466 |
[PTOL] "m" (X_3D_TOLERANCE), |
| 467 |
[MTOL] "m" (X_3D_MTOLERANCE), |
| 468 |
[IONE] "m" (IONE) |
| 469 |
: "cc" |
| 470 |
); |
| 471 |
|
| 472 |
return pt; |
| 473 |
} |
| 474 |
} |
| 475 |
} |
| 476 |
|
| 477 |
#endif /* PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_ */ |