/*
* Copyright (C) 2025 Linux Studio Plugins Project
* (C) 2025 Vladimir Sadovnikov
*
* This file is part of lsp-dsp-lib
* Created on: 31 мар. 2020 г.
*
* lsp-dsp-lib is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* lsp-dsp-lib is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with lsp-dsp-lib. If not, see .
*/
#ifndef PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_
#define PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_
#ifndef PRIVATE_DSP_ARCH_X86_SSE_IMPL
#error "This header should not be included directly"
#endif /* PRIVATE_DSP_ARCH_X86_SSE_IMPL */
/* Input:
* x0 = a0 a1 a2 a3
* x1 = b0 b1 b2 b3
*
* Output:
* x0 = b0 b1 b2 b3
* x1 = a0 a1 a2 a3
*/
#define VECTOR_XCHG(x0, x1) \
__ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = b ^ a */ \
__ASM_EMIT("xorps %" x1 ", %" x0) /* xmm0 = a ^ ( b ^ a) = b */ \
__ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = (b ^ a) ^ b = a */
/* Input:
* x0 = a0 a1 a2 ?
* x1 = b0 b1 b2 ?
* x2 = c0 c1 c2 ?
*
* Output:
* x0 = a0 a1 a2 ?
* x1 = b0 b1 b2 ?
* x3 = c0 c1 c2 ?
*/
#define MAT3_TRANSPOSE(x0, x1, x2, x3) \
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = a0 a1 a2 ? */ \
__ASM_EMIT("unpcklps %" x1 ", %" x0) /* xmm0 = a0 b0 a1 b1 */ \
__ASM_EMIT("unpckhps %" x1 ", %" x3) /* xmm3 = a2 b2 ? ? */ \
__ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = a1 b1 b2 ? */ \
__ASM_EMIT("movlhps %" x2 ", %" x0) /* xmm0 = a0 b0 c0 c1 ? */ \
__ASM_EMIT("shufps $0x54, %" x2 ", %" x1) /* xmm1 = a1 b1 c1 c1 */ \
__ASM_EMIT("shufps $0xa4, %" x2 ", %" x3) /* xmm3 = a2 b2 c2 c2 */
/* 3x scalar multiplication
* Input:
* x0 = vector1 [dx dy dz ? ]
* x1 = vector2 [dx dy dz ? ]
* x2 = vector3 [dx dy dz ? ]
*
* Output:
* x0 = vector1 * vector2 [ S0 ? ? ? ]
* x1 = vector2 * vector3 [ S1 ? ? ? ]
* x2 = vector3 * vector1 [ S2 ? ? ? ]
*/
#define SCALAR_MUL3(x0, x1, x2, x3) \
__ASM_EMIT("movaps %" x0 ", %" x3) \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 dw0*dw1 */ \
__ASM_EMIT("mulps %" x2 ", %" x1) /* xmm1 = dx1*dx2 dy1*dy2 dz1*dz2 dw1*dw2 */ \
__ASM_EMIT("mulps %" x3 ", %" x2) /* xmm2 = dx2*dx0 dy2*dy0 dz2*dz0 dw2*dw0 */ \
MAT3_TRANSPOSE(x0, x1, x2, x3) \
__ASM_EMIT("addps %" x1 ", %" x0) \
__ASM_EMIT("addps %" x3 ", %" x0) /* xmm0 = S0 S1 S2 ? */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = r2 ? ? ? */ \
__ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = r0 r0 r1 r1 */ \
__ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = r1 r1 ? ? */
/* 1x scalar multiplication of 3 coordinates
* Input:
* x0 = vector1 [dx dy dz ? ]
* x1 = vector2 [dx dy dz ? ]
* x2 = temporary
*
* Output:
* x0 = result (scalar)
*/
#define VECTOR_DPPS3(x0, x1, x2) \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 ? */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = dz0*dz1 ? ? ? */ \
__ASM_EMIT("addss %" x0 ", %" x2) /* xmm2 = dz0*dz1 + dx0*dx1 */ \
__ASM_EMIT("shufps $0x55, %" x0 ", %" x0) /* xmm0 = dy0*dy1 dy0*dy1 dy0*dy1 dy0*dy1 */ \
__ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = dz0*dz1 + dx0*dx1 + dy0*dy1 */ \
/* 1x scalar multiplication of 4 coordinates
* Input:
* x0 = vector1 [dx dy dz dw ]
* x1 = vector2 [dx dy dz dw ]
* x2 = temporary
*
* Output:
* x0 = result (scalar)
*/
#define VECTOR_DPPS4(x0, x1, x2) \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = dx0*dx1 dy0*dy1 dz0*dz1 dw0*dw1 = A B C D */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = C D */ \
__ASM_EMIT("addps %" x2 ", %" x0) /* xmm0 = A+C B+D */ \
__ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = A+C A+C B+D B+D */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = B+D B+D */ \
__ASM_EMIT("addps %" x2 ", %" x0) /* xmm0 = A+C+B+D */
/* 3x vector multiplication
* Input:
* x0 = vector1 [dx dy dz 0 ]
* x1 = vector2 [dx dy dz 0 ]
* x2 = vector3 [dx dy dz 0 ]
*
* Output:
* x0 = vector1 * vector2 [ vz vx vy 0 ]
* x1 = vector2 * vector3 [ vz vx vy 0 ]
* x2 = vector3 * vector1 [ vz vx vy 0 ]
*/
#define VECTOR_MUL3(x0, x1, x2, x3, x4, x5, x6, x7) \
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = dx0 dy0 dz0 0 */ \
__ASM_EMIT("movaps %" x1 ", %" x4) /* xmm4 = dx1 dy1 dz1 0 */ \
__ASM_EMIT("movaps %" x2 ", %" x5) /* xmm5 = dx2 dy2 dz2 0 */ \
__ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy0 dz0 dx0 0 */ \
__ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = dy1 dz1 dx1 0 */ \
__ASM_EMIT("shufps $0xc9, %" x5 ", %" x5) /* xmm5 = dy2 dz2 dx2 0 */ \
__ASM_EMIT("movaps %" x0 ", %" x6) /* xmm6 = dx0 dy0 dz0 0 */ \
__ASM_EMIT("movaps %" x3 ", %" x7) /* xmm7 = dy0 dz0 dx0 0 */ \
\
__ASM_EMIT("mulps %" x4 ", %" x0) /* xmm0 = dx0*dy1 dy0*dz1 dz0*dx1 0 */ \
__ASM_EMIT("mulps %" x1 ", %" x3) /* xmm3 = dy0*dx1 dz0*dy1 dx0*dz1 0 */ \
__ASM_EMIT("mulps %" x5 ", %" x1) /* xmm1 = dx1*dy2 dy1*dz2 dz1*dx2 0 */ \
__ASM_EMIT("mulps %" x2 ", %" x4) /* xmm4 = dy1*dx2 dz1*dy2 dx1*dz2 0 */ \
__ASM_EMIT("mulps %" x7 ", %" x2) /* xmm2 = dx2*dy0 dy2*dz0 dz2*dx0 0 */ \
__ASM_EMIT("mulps %" x6 ", %" x5) /* xmm5 = dy2*dx0 dz2*dy0 dx2*dz0 0 */ \
__ASM_EMIT("subps %" x3 ", %" x0) /* xmm0 = nz0 nx0 ny0 0 */ \
__ASM_EMIT("subps %" x4 ", %" x1) /* xmm1 = nz1 nx1 ny1 0 */ \
__ASM_EMIT("subps %" x5 ", %" x2) /* xmm2 = nz2 nx2 ny2 0 */
/* 1x vector multiplication
* Input:
* x0 = vector1 [dx dy dz 0 ]
* x1 = vector2 [dx dy dz 0 ]
* x2 = temporary
* x3 = temporary
*
* Output:
* x0 = vector1 * vector2 [ vz vx vy 0 ]
*/
#define VECTOR_MUL(x0, x1, x2, x3) \
__ASM_EMIT("movaps %" x0 ", %" x2) /* xmm2 = dx1 dy1 dz1 0 */ \
__ASM_EMIT("movaps %" x1 ", %" x3) /* xmm3 = dx2 dy2 dz2 0 */ \
__ASM_EMIT("shufps $0xc9, %" x2 ", %" x2) /* xmm2 = dy1 dz1 dx1 0 */ \
__ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy2 dz2 dx2 0 */ \
__ASM_EMIT("mulps %" x2 ", %" x1) /* xmm1 = dx2*dy1 dy2*dz1 dz2*dx1 dw2*dw2 */ \
__ASM_EMIT("mulps %" x3 ", %" x0) /* xmm0 = dx1*dy2 dy1*dz2 dz1*dx2 dw1*dw1 */ \
__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 */
/* 1x matrix transpose
* Input:
* x0 = row 0 (a1 a2 a3 a4)
* x1 = row 1 (b1 b2 b3 b4)
* x2 = row 2 (c1 c2 c3 c4)
* x3 = row 3 (d1 d2 d3 d4)
* x4 = temporary
*
* Output:
* x0 = row 0 (a1 b1 c1 d1)
* x1 = row 1 (a2 b2 c2 d2)
* x2 = row 2 (a3 b3 c3 d3)
* x3 = row 3 (a4 b4 c4 d4)
*/
#define MAT4_TRANSPOSE(x0, x1, x2, x3, x4) \
__ASM_EMIT("movaps %" x2 ", %" x4) /* xmm4 = c1 c2 c3 c4 */ \
__ASM_EMIT("unpcklps %" x3 ", %" x2) /* xmm2 = c1 d1 c2 d2 */ \
__ASM_EMIT("unpckhps %" x3 ", %" x4) /* xmm4 = c3 d3 c4 d4 */ \
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = a1 a2 a3 a4 */ \
__ASM_EMIT("unpcklps %" x1 ", %" x0) /* xmm0 = a1 b1 a2 b2 */ \
__ASM_EMIT("unpckhps %" x1 ", %" x3) /* xmm3 = a3 b3 a4 b4 */ \
__ASM_EMIT("movaps %" x2 ", %" x1) /* xmm1 = c1 d1 c2 d2 */ \
__ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = a2 b2 c2 d2 */ \
__ASM_EMIT("movlhps %" x2 ", %" x0) /* xmm0 = a1 b1 c1 d1 */ \
__ASM_EMIT("movaps %" x3 ", %" x2) /* xmm2 = a3 b3 a4 b4 */ \
__ASM_EMIT("movaps %" x4 ", %" x3) /* xmm3 = c3 d3 c4 d4 */ \
__ASM_EMIT("movhlps %" x2 ", %" x3) /* xmm3 = a4 b4 c4 d4 */ \
__ASM_EMIT("movlhps %" x4 ", %" x2) /* xmm2 = a3 b3 c3 d3 */
#define MATRIX_LOAD(ptr, x0, x1, x2, x3) \
__ASM_EMIT("movups 0x00(%[" ptr "]), %" x0 ) \
__ASM_EMIT("movups 0x10(%[" ptr "]), %" x1 ) \
__ASM_EMIT("movups 0x20(%[" ptr "]), %" x2 ) \
__ASM_EMIT("movups 0x30(%[" ptr "]), %" x3 )
#define MATRIX_STORE(ptr, x0, x1, x2, x3) \
__ASM_EMIT("movups %" x0 ", 0x00(%[" ptr "])") \
__ASM_EMIT("movups %" x1 ", 0x10(%[" ptr "])") \
__ASM_EMIT("movups %" x2 ", 0x20(%[" ptr "])") \
__ASM_EMIT("movups %" x3 ", 0x30(%[" ptr "])")
#define NORMALIZE(x0, x1, x2) /* x0 = vector/point to normalize */ \
__ASM_EMIT("movaps %" x0 ", %" x2) /* xmm2 = x y z w */ \
__ASM_EMIT("mulps %" x2 ", %" x2) /* xmm2 = x*x y*y z*z w*w */ \
__ASM_EMIT("movhlps %" x2 ", %" x1) /* xmm1 = z*z w*w ? ? */ \
__ASM_EMIT("addss %" x1 ", %" x2) /* xmm2 = x*x+z*z y*y z*z w*w */ \
__ASM_EMIT("unpcklps %" x2 ", %" x2) /* xmm2 = x*x+z*z x*x+z*z y*y y*y */ \
__ASM_EMIT("movhlps %" x2 ", %" x1) /* xmm1 = y*y y*y ? ? */ \
__ASM_EMIT("addss %" x1 ", %" x2) /* xmm2 = x*x+y*y+z*z */ \
__ASM_EMIT("xorps %" x1 ", %" x1) /* xmm1 = 0 */ \
__ASM_EMIT("shufps $0x00, %" x2 ", %" x2) /* xmm2 = w w w w */ \
__ASM_EMIT("sqrtps %" x2 ", %" x2) /* xmm2 = sqrt(x*x + y*y + z*z) */ \
__ASM_EMIT("cmpps $4, %" x2 ", %" x1) /* xmm1 = w != 0 */ \
__ASM_EMIT("divps %" x2 ", %" x0) /* xmm0 = x/w y/w z/w w/w */ \
__ASM_EMIT("andps %" x1 ", %" x0) /* xmm0 = (w != 0) ? x/w y/w z/w w/w : 0 */ \
/*
* Input:
* x0 = vector/point to scale
* x1 = radius-vector to match length
*
* Output:
* x0 = scaled vector
*/
#define SCALE(x0, x1, x2, x3) \
__ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = x y z w */ \
__ASM_EMIT("mulps %" x3 ", %" x3) /* xmm3 = x*x y*y z*z w*w */ \
__ASM_EMIT("movhlps %" x3 ", %" x2) /* xmm2 = z*z w*w ? ? */ \
__ASM_EMIT("addss %" x2 ", %" x3) /* xmm3 = x*x+z*z y*y z*z w*w */ \
__ASM_EMIT("unpcklps %" x3 ", %" x3) /* xmm3 = x*x+z*z x*x+z*z y*y y*y */ \
__ASM_EMIT("movhlps %" x3 ", %" x2) /* xmm2 = y*y y*y ? ? */ \
__ASM_EMIT("addss %" x2 ", %" x3) /* xmm3 = x*x+y*y+z*z */ \
__ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \
__ASM_EMIT("sqrtss %" x3", %" x3) /* xmm3 = sqrt(x*x + y*y + z*z) */ \
__ASM_EMIT("ucomiss %" x2 ", %" x3) /* xmm3 =?= xmm2 */ \
__ASM_EMIT("jle 1000000f") \
__ASM_EMIT("divss %" x3 ", %" x1) /* xmm1 = r/w */ \
__ASM_EMIT("shufps $0x00, %" x1 ", %" x1) /* xmm1 = r/w r/w r/w r/w */ \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*r/w y*r/w z*r/w w*r/w */ \
__ASM_EMIT("1000000:")
/*
* Input:
* x0 = vector 1 = dx1 dy1 dz1 dw1
* x1 = vector 2 = dx2 dy2 dz2 dw2
*
* Output:
* x0 = dx1*dx2 + dy1*dy2 + dz1*dz2
*/
#define SCALAR_MUL(x0, x1, x2) \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*x y*y z*z w*w */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = z*z w*w ? ? */ \
__ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = x*x+z*z y*y z*z w*w */ \
__ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = x*x+z*z x*x+z*z y*y y*y */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = y*y y*y ? ? */ \
__ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = x*x+y*y+z*z */
#define TRIPLET_CHECK(x0, x1, x2, x3, x4) /* x0 = normal; x1, x2 = vectors */ \
__ASM_EMIT("movaps %" x1 ", %" x3) /* xmm3 = x1 y1 z1 w1 */ \
__ASM_EMIT("movaps %" x2 ", %" x4) /* xmm4 = x2 y2 z2 w2 */ \
__ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = y1 z1 x1 w1 */ \
__ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = y2 z2 x2 w2 */ \
__ASM_EMIT("shufps $0xd2, %" x0 ", %" x0) /* xmm0 = nz nx ny nw */ \
__ASM_EMIT("mulps %" x4 ", %" x1) /* xmm1 = x1*y2 y1*z2 z1*x2 w1*w2 */ \
__ASM_EMIT("mulps %" x3 ", %" x2) /* xmm2 = y1*x2 z1*y2 x1*z2 w1*w2 */ \
__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 */ \
__ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = nz*vz nx*vx ny*vy nw*vw */ \
__ASM_EMIT("movhlps %" x0 ", %" x2) /* xmm2 = ny*vy nw*vw ? ? */ \
__ASM_EMIT("unpcklps %" x0 ", %" x0) /* xmm0 = nz*vz nz*vz nx*vx nx*vx */ \
__ASM_EMIT("movhlps %" x0 ", %" x1) /* xmm1 = nx*vx nx*vx ? ? */ \
__ASM_EMIT("addss %" x2 ", %" x0) /* xmm0 = nz*vz + ny*vy */ \
__ASM_EMIT("addss %" x1 ", %" x0) /* xmm0 = nz*vz + ny*vy + nx*vx */
namespace lsp
{
namespace sse
{
using namespace dsp;
IF_ARCH_X86(
static const float IDENTITY0[4] __lsp_aligned16 = { 1.0f, 0.0f, 0.0f, 0.0f };
static const float IDENTITY1[4] __lsp_aligned16 = { 0.0f, 1.0f, 0.0f, 0.0f };
static const float IDENTITY2[4] __lsp_aligned16 = { 0.0f, 0.0f, 1.0f, 0.0f };
static const float IDENTITY3[4] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f };
static const uint32_t X_MASK0001[] __lsp_aligned16 = { 0xffffffff, 0, 0, 0 };
static const uint32_t X_MASK0111[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
static const uint32_t X_SMASK0010[] __lsp_aligned16 = { 0, 0x80000000, 0, 0 };
static const uint32_t X_SMASK0001[] __lsp_aligned16 = { 0x80000000, 0, 0, 0 };
static const uint32_t X_SMASK0111[] __lsp_aligned16 = { 0x80000000, 0x80000000, 0x80000000, 0 };
static const uint32_t X_3DMASK[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
static const float X_3DPOINT[] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f };
static const float X_3D_TOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(DSP_3D_TOLERANCE) };
static const float X_3D_MTOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(-DSP_3D_TOLERANCE) };
static const float ONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1.0f) };
static const float X_MINUS_ONE[] __lsp_aligned16 = { LSP_DSP_VEC4(-1.0f) };
static const uint32_t IONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1) };
)
void init_point_xyz(point3d_t *p, float x, float y, float z)
{
ARCH_X86_ASM
(
__ASM_EMIT("movlhps %[x1], %[x0]") // x0 = x ? y ?
__ASM_EMIT("movhps %[id], %[x2]") // x2 = z ? 1 0
__ASM_EMIT("shufps $0x88, %[x2], %[x0]") // x0 = x y z 1
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "+x" (x), [x1] "+x" (y), [x2] "+x"(z)
: [p] "r" (p),
[id] "m" (IDENTITY0)
: "memory"
);
}
void init_point(point3d_t *p, const point3d_t *s)
{
float x0;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[s]), %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0)
: [s] "r" (s), [p] "r" (p)
: "memory"
);
}
void normalize_point(point3d_t *p)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
NORMALIZE("[x0]", "[x1]", "[x2]")
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
: [p] "r" (p),
[zmask] "m" (X_MASK0111),
[omask] "m" (X_3DPOINT)
: "cc", "memory"
);
}
void scale_point1(point3d_t *p, float r)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
SCALE("[x0]", "[x3]", "[x1]", "[x2]")
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
: [p] "r" (p),
[zmask] "m" (X_MASK0111),
[omask] "m" (X_3DPOINT)
: "cc", "memory"
);
}
void scale_point2(point3d_t *p, const point3d_t *s, float r)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = x y z w
SCALE("[x0]", "[x3]", "[x1]", "[x2]")
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
__ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
: [p] "r" (p), [s] "r" (s),
[zmask] "m" (X_MASK0111),
[omask] "m" (X_3DPOINT)
: "cc", "memory"
);
}
void init_vector_dxyz(vector3d_t *p, float dx, float dy, float dz)
{
ARCH_X86_ASM
(
__ASM_EMIT("movlhps %[x1], %[x0]") // x0 = dx ? dy ?
__ASM_EMIT("movhps %[id], %[x2]") // x2 = dz ? 1 0
__ASM_EMIT("shufps $0xc8, %[x2], %[x0]") // x0 = dx dy dz 0
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "+x" (dx), [x1] "+x" (dy), [x2] "+x"(dz)
: [p] "r" (p),
[id] "m" (IDENTITY0)
: "memory"
);
}
void init_vector(vector3d_t *p, const vector3d_t *s)
{
float x0;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[s]), %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0)
: [s] "r" (s), [p] "r" (p)
: "memory"
);
}
void add_vector_pv1(point3d_t *p, const vector3d_t *dv)
{
float x0, x1;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %[x0]")
__ASM_EMIT("movups (%[dv]), %[x1]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1)
: [p] "r" (p), [dv] "r" (dv)
: "memory"
);
}
void add_vector_pv2(point3d_t *p, const point3d_t *sp, const vector3d_t *dv)
{
float x0, x1;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[sp]), %[x0]")
__ASM_EMIT("movups (%[dv]), %[x1]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1)
: [p] "r" (p), [sp] "r" (sp), [dv] "r" (dv)
: "memory"
);
}
void add_vector_pvk1(point3d_t *p, const vector3d_t *dv, float k)
{
float x0, x1;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[dv]), %[x1]")
__ASM_EMIT("shufps $0x00, %[x2], %[x2]")
__ASM_EMIT("movups (%[p]), %[x0]")
__ASM_EMIT("mulps %[x2], %[x1]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "+x" (k)
: [p] "r" (p), [dv] "r" (dv)
: "memory"
);
}
void add_vector_pvk2(point3d_t *p, const point3d_t *sp, const vector3d_t *dv, float k)
{
float x0, x1;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[dv]), %[x1]")
__ASM_EMIT("shufps $0x00, %[x2], %[x2]")
__ASM_EMIT("movups (%[sp]), %[x0]")
__ASM_EMIT("mulps %[x2], %[x1]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("movups %[x0], (%[p])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "+x" (k)
: [p] "r" (p), [sp] "r" (sp), [dv] "r" (dv)
: "memory"
);
}
void normalize_vector(vector3d_t *v)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
NORMALIZE("[x0]", "[x1]", "[x2]")
__ASM_EMIT("movups %[x0], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
: [v] "r" (v)
: "cc", "memory"
);
}
void normalize_vector2(vector3d_t *v, const vector3d_t *sv)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[sv]), %[x0]") // xmm0 = dx dy dz dw
NORMALIZE("[x0]", "[x1]", "[x2]")
__ASM_EMIT("movups %[x0], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
: [v] "r" (v), [sv] "r" (sv)
: "cc", "memory"
);
}
void flip_vector_v1(vector3d_t *v)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v]), %%xmm0") // xmm0 = dx dy dz dw
__ASM_EMIT("xorps %[mask], %%xmm0")
__ASM_EMIT("movups %%xmm0, (%[v])")
:
: [v] "r" (v),
[mask] "m" (X_SMASK0111)
: "memory",
"xmm0"
);
}
void flip_vector_v2(vector3d_t *v, const vector3d_t *sv)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[sv]), %%xmm0") // xmm0 = dx dy dz dw
__ASM_EMIT("xorps %[mask], %%xmm0")
__ASM_EMIT("movups %%xmm0, (%[v])")
:
: [v] "r" (v), [sv] "r" (sv),
[mask] "m" (X_SMASK0111)
: "memory",
"xmm0"
);
}
void scale_vector1(vector3d_t *v, float r)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
SCALE("[x0]", "[x3]", "[x1]", "[x2]")
__ASM_EMIT("andps %[zmask], %[x0]")
__ASM_EMIT("movups %[x0], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
: [v] "r" (v),
[zmask] "m" (X_MASK0111)
: "cc", "memory"
);
}
void scale_vector2(vector3d_t *v, const vector3d_t *s, float r)
{
float x0, x1, x2;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = dx dy dz dw
SCALE("[x0]", "[x3]", "[x1]", "[x2]")
__ASM_EMIT("andps %[zmask], %[x0]")
__ASM_EMIT("movups %[x0], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
: [v] "r" (v), [s] "r" (s),
[zmask] "m" (X_MASK0111)
: "cc", "memory"
);
}
void init_matrix3d(matrix3d_t *dst, const matrix3d_t *src)
{
ARCH_X86_ASM
(
MATRIX_LOAD("s", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
MATRIX_STORE("d", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
:
: [s] "r" (src), [d] "r" (dst)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3"
);
}
void init_matrix3d_zero(matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("xorps %%xmm0, %%xmm0")
MATRIX_STORE("m", "%xmm0", "%xmm0", "%xmm0", "%xmm0")
:
: [m] "r" (m)
: "memory",
"%xmm0"
);
}
void init_matrix3d_one(matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movaps %[one], %%xmm0")
MATRIX_STORE("m", "%xmm0", "%xmm0", "%xmm0", "%xmm0")
:
: [m] "r" (m), [one] "m" (ONE)
: "memory",
"%xmm0"
);
}
void init_matrix3d_identity(matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movaps %[idm0], %%xmm0")
__ASM_EMIT("movaps %[idm1], %%xmm1")
__ASM_EMIT("movaps %[idm2], %%xmm2")
__ASM_EMIT("movaps %[idm3], %%xmm3")
MATRIX_STORE("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
:
: [m] "r" (m),
[idm0] "m" (IDENTITY0),
[idm1] "m" (IDENTITY1),
[idm2] "m" (IDENTITY2),
[idm3] "m" (IDENTITY3)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3"
);
}
void init_matrix3d_translate(matrix3d_t *m, float dx, float dy, float dz)
{
float x3, x4, x5;
ARCH_X86_ASM
(
__ASM_EMIT("movaps %[idm0], %[x3]") // xmm3 = 1 0 0 0
__ASM_EMIT("movaps %[idm1], %[x4]") // xmm4 = 0 1 0 0
__ASM_EMIT("movaps %[idm2], %[x5]") // xmm5 = 0 0 1 0
__ASM_EMIT("movlhps %[x3], %[x2]") // xmm2 = dz ? 1 0
__ASM_EMIT("movlhps %[x1], %[x0]") // xmm0 = dx ? dy ?
__ASM_EMIT("shufps $0x88, %[x2], %[x0]") // xmm0 = dx dy dz 1
MATRIX_STORE("m", "[x3]", "[x4]", "[x5]", "[x0]")
: [x0] "+x" (dx), [x1] "+x" (dy), [x2] "+x"(dz),
[x3] "=&x" (x3), [x4] "=&x" (x4), [x5] "=&x" (x5)
: [m] "r" (m),
[idm0] "m" (IDENTITY0),
[idm1] "m" (IDENTITY1),
[idm2] "m" (IDENTITY2),
[idm3] "m" (IDENTITY3)
: "memory"
);
}
void init_matrix3d_scale(matrix3d_t *m, float sx, float sy, float sz)
{
float x3;
ARCH_X86_ASM
(
__ASM_EMIT("andps %[mask], %[x0]") // xmm0 = sx 0 0 0
__ASM_EMIT("andps %[mask], %[x1]") // xmm1 = sy 0 0 0
__ASM_EMIT("andps %[mask], %[x2]") // xmm2 = sz 0 0 0
__ASM_EMIT("shufps $0xf3, %[x1], %[x1]") // xmm1 = 0 sy 0 0
__ASM_EMIT("shufps $0xcf, %[x2], %[x2]") // xmm2 = 0 0 sz 0
__ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
MATRIX_STORE("m", "[x0]", "[x1]", "[x2]", "[x3]")
: [x0] "+x" (sx), [x1] "+x" (sy), [x2] "+x"(sz),
[x3] "=&x" (x3)
: [m] "r" (m),
[idm3] "m" (IDENTITY3),
[mask] "m" (X_MASK0001)
: "memory"
);
}
void init_matrix3d_rotate_x(matrix3d_t *m, float angle)
{
float s = sinf(angle);
float c = cosf(angle);
float x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
__ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
__ASM_EMIT("shufps $0xc3, %[x1], %[x0]") // xmm0 = 0 s c 0
__ASM_EMIT("movaps %[idm0], %[x2]") // xmm2 = 1 0 0 0
__ASM_EMIT("movaps %[x0], %[x1]") // xmm1 = 0 s c 0
__ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
__ASM_EMIT("xorps %[mask], %[x0]") // xmm0 = 0 -s c 0
__ASM_EMIT("shufps $0xd8, %[x1], %[x1]") // xmm1 = 0 c s 0
MATRIX_STORE("m", "[x2]", "[x1]", "[x0]", "[x3]")
: [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [m] "r" (m),
[idm0] "m" (IDENTITY0),
[idm3] "m" (IDENTITY3),
[zmask] "m" (X_MASK0001),
[mask] "m" (X_SMASK0010)
: "memory"
);
}
void init_matrix3d_rotate_y(matrix3d_t *m, float angle)
{
float s = sinf(angle);
float c = cosf(angle);
float x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
__ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
__ASM_EMIT("movlhps %[x1], %[x0]") // xmm0 = s 0 c 0
__ASM_EMIT("movaps %[x0], %[x1]") // xmm1 = s 0 c 0
__ASM_EMIT("movaps %[idm1], %[x2]") // xmm2 = 0 1 0 0
__ASM_EMIT("xorps %[mask], %[x1]") // xmm1 = -s 0 c 0
__ASM_EMIT("movaps %[idm3], %[x3]") // xmm3 = 0 0 0 1
__ASM_EMIT("shufps $0xc6, %[x1], %[x1]") // xmm1 = c 0 -s 0
MATRIX_STORE("m", "[x1]", "[x2]", "[x0]", "[x3]")
: [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [m] "r" (m),
[idm1] "m" (IDENTITY1),
[idm3] "m" (IDENTITY3),
[zmask] "m" (X_MASK0001),
[mask] "m" (X_SMASK0001)
: "memory"
);
}
void init_matrix3d_rotate_z(matrix3d_t *m, float angle)
{
float s = sinf(angle);
float c = cosf(angle);
float x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = s 0 0 0
__ASM_EMIT("andps %[zmask], %[x1]") // xmm1 = c 0 0 0
__ASM_EMIT("movaps %[x0], %[x2]") // xmm2 = s 0 0 0
__ASM_EMIT("movaps %[idm2], %[x3]") // xmm3 = 0 0 1 0
__ASM_EMIT("unpcklps %[x1], %[x0]") // xmm0 = s c 0 0
__ASM_EMIT("movaps %[idm3], %[x4]") // xmm3 = 0 0 0 1
__ASM_EMIT("unpcklps %[x2], %[x1]") // xmm1 = c s 0 0
__ASM_EMIT("xorps %[mask], %[x0]") // xmm0 = -s c 0 0
MATRIX_STORE("m", "[x1]", "[x0]", "[x3]", "[x4]")
: [x0] "+x" (s), [x1] "+x" (c), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [m] "r" (m),
[idm2] "m" (IDENTITY2),
[idm3] "m" (IDENTITY3),
[zmask] "m" (X_MASK0001),
[mask] "m" (X_SMASK0001)
: "memory"
);
}
void init_matrix3d_rotate_xyz(matrix3d_t *m, float x, float y, float z, float angle)
{
if (x == 0.0f)
{
if (y == 0.0f)
{
if (z > 0.0f)
init_matrix3d_rotate_z(m, angle);
else if (z < 0.0f)
init_matrix3d_rotate_z(m, -angle);
else // (z == 0.0)
init_matrix3d_identity(m);
return;
}
else if (z == 0.0f)
{
if (y > 0.0f)
init_matrix3d_rotate_y(m, angle);
else if (y < 0.0f)
init_matrix3d_rotate_y(m, -angle);
return;
}
}
else if ((y == 0.0) && (z == 0.0))
{
if (x > 0.0f)
init_matrix3d_rotate_x(m, angle);
else
init_matrix3d_rotate_x(m, -angle);
return;
}
float s = sinf(angle);
float c = cosf(angle);
float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
float mag = sqrtf(x*x + y*y + z*z);
// if (mag <= 1.0e-4)
// return *this;
x /= mag;
y /= mag;
z /= mag;
xx = x * x;
yy = y * y;
zz = z * z;
xy = x * y;
yz = y * z;
zx = z * x;
xs = x * s;
ys = y * s;
zs = z * s;
one_c = 1.0f - c;
float *M = m->m;
M[0] = (one_c * xx) + c;
M[1] = (one_c * xy) + zs;
M[2] = (one_c * zx) - ys;
M[3] = 0.0f;
M[4] = (one_c * xy) - zs;
M[5] = (one_c * yy) + c;
M[6] = (one_c * yz) + xs;
M[7] = 0.0f;
M[8] = (one_c * zx) + ys;
M[9] = (one_c * yz) - xs;
M[10] = (one_c * zz) + c;
M[11] = 0.0f;
M[12] = 0.0f;
M[13] = 0.0f;
M[14] = 0.0f;
M[15] = 0.0f;
}
void apply_matrix3d_mv2(vector3d_t *r, const vector3d_t *v, const matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v]), %%xmm0") // xmm0 = vx vy vz vw
__ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = vx vy vz vw
__ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
__ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = vx vy vz vw
__ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
__ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = vx vx vx vx
__ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = vy vy vy vy
__ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm2 = vz vz vz vz
__ASM_EMIT("mulps %%xmm4, %%xmm0")
__ASM_EMIT("mulps %%xmm5, %%xmm1")
__ASM_EMIT("mulps %%xmm6, %%xmm2")
__ASM_EMIT("addps %%xmm1, %%xmm0")
__ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
__ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rdx rdy rdz rdw
__ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rdw rdw rdw rdw
__ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
__ASM_EMIT("je 1f")
__ASM_EMIT("divps %%xmm1, %%xmm0")
__ASM_EMIT("1:")
__ASM_EMIT("movups %%xmm0, (%[r])")
:
: [r] "r" (r), [v] "r" (v), [m] "r" (m)
: "cc", "memory",
"%xmm0", "%xmm1", "%xmm2",
"%xmm4", "%xmm5", "%xmm6"
);
}
void apply_matrix3d_mv1(vector3d_t *r, const matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[r]), %%xmm0") // xmm0 = vx vy vz vw
__ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = vx vy vz vw
__ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
__ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = vx vy vz vw
__ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
__ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = vx vx vx vx
__ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = vy vy vy vy
__ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm2 = vz vz vz vz
__ASM_EMIT("mulps %%xmm4, %%xmm0")
__ASM_EMIT("mulps %%xmm5, %%xmm1")
__ASM_EMIT("mulps %%xmm6, %%xmm2")
__ASM_EMIT("addps %%xmm1, %%xmm0")
__ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
__ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rdx rdy rdz rdw
__ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rdw rdw rdw rdw
__ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
__ASM_EMIT("je 1f")
__ASM_EMIT("divps %%xmm1, %%xmm0")
__ASM_EMIT("1:")
__ASM_EMIT("movups %%xmm0, (%[r])")
:
: [r] "r" (r), [m] "r" (m)
: "cc", "memory",
"%xmm0", "%xmm1", "%xmm2",
"%xmm4", "%xmm5", "%xmm6"
);
}
void apply_matrix3d_mp2(point3d_t *r, const point3d_t *p, const matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %%xmm0") // xmm0 = px py pz pw
__ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = px py pz pw
__ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
__ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = px py pz pw
__ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
__ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = px px px px
__ASM_EMIT("movups 0x30(%[m]), %%xmm7") // xmm7 = m12 m13 m14 m15
__ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = py py py py
__ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm1 = pz pz pz pz
__ASM_EMIT("mulps %%xmm4, %%xmm0")
__ASM_EMIT("mulps %%xmm5, %%xmm1")
__ASM_EMIT("mulps %%xmm6, %%xmm2")
__ASM_EMIT("addps %%xmm1, %%xmm0")
__ASM_EMIT("addps %%xmm7, %%xmm2")
__ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
__ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rx ry rz rw
__ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rw rw rw rw
__ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
__ASM_EMIT("je 1f")
__ASM_EMIT("divps %%xmm1, %%xmm0")
__ASM_EMIT("1:")
__ASM_EMIT("movups %%xmm0, (%[r])")
:
: [r] "r" (r), [p] "r" (p), [m] "r" (m)
: "cc", "memory",
"%xmm0", "%xmm1", "%xmm2",
"%xmm4", "%xmm5", "%xmm6", "%xmm7"
);
}
void apply_matrix3d_mp1(point3d_t *r, const matrix3d_t *m)
{
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[r]), %%xmm0") // xmm0 = px py pz pw
__ASM_EMIT("movups 0x00(%[m]), %%xmm4") // xmm4 = m0 m1 m2 m3
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = px py pz pw
__ASM_EMIT("movups 0x10(%[m]), %%xmm5") // xmm5 = m4 m5 m6 m7
__ASM_EMIT("movaps %%xmm0, %%xmm2") // xmm2 = px py pz pw
__ASM_EMIT("movups 0x20(%[m]), %%xmm6") // xmm6 = m8 m9 m10 m11
__ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = px px px px
__ASM_EMIT("movups 0x30(%[m]), %%xmm7") // xmm7 = m12 m13 m14 m15
__ASM_EMIT("shufps $0x55, %%xmm1, %%xmm1") // xmm1 = py py py py
__ASM_EMIT("shufps $0xaa, %%xmm2, %%xmm2") // xmm1 = pz pz pz pz
__ASM_EMIT("mulps %%xmm4, %%xmm0")
__ASM_EMIT("mulps %%xmm5, %%xmm1")
__ASM_EMIT("mulps %%xmm6, %%xmm2")
__ASM_EMIT("addps %%xmm1, %%xmm0")
__ASM_EMIT("addps %%xmm7, %%xmm2")
__ASM_EMIT("addps %%xmm2, %%xmm0") // xmm0 = rx ry rz rw
__ASM_EMIT("xorps %%xmm2, %%xmm2") // xmm2 = 0 0 0 0
__ASM_EMIT("movaps %%xmm0, %%xmm1") // xmm1 = rx ry rz rw
__ASM_EMIT("shufps $0xff, %%xmm1, %%xmm1") // xmm1 = rw rw rw rw
__ASM_EMIT("ucomiss %%xmm1, %%xmm2") // xmm2 =?= xmm1
__ASM_EMIT("je 1f")
__ASM_EMIT("divps %%xmm1, %%xmm0")
__ASM_EMIT("1:")
__ASM_EMIT("movups %%xmm0, (%[r])")
:
: [r] "r" (r), [m] "r" (m)
: "cc", "memory",
"%xmm0", "%xmm1", "%xmm2",
"%xmm4", "%xmm5", "%xmm6", "%xmm7"
);
}
#define MATMUL_CORE(x0, x1, x2, x3, x4, x5, x6, x7) \
__ASM_EMIT("movaps %" x4 ", %" x5) /* xmm5 = b0 b1 b2 b3 */ \
__ASM_EMIT("movaps %" x4 ", %" x6) /* xmm6 = b0 b1 b2 b3 */ \
__ASM_EMIT("movaps %" x5 ", %" x7) /* xmm7 = b0 b1 b2 b3 */ \
__ASM_EMIT("shufps $0x00, %" x4 ", %" x4) /* xmm4 = b0 b0 b0 b0 */ \
__ASM_EMIT("shufps $0x55, %" x5 ", %" x5) /* xmm5 = b1 b1 b1 b1 */ \
__ASM_EMIT("shufps $0xaa, %" x6 ", %" x6) /* xmm6 = b2 b2 b2 b2 */ \
__ASM_EMIT("shufps $0xff, %" x7 ", %" x7) /* xmm7 = b3 b3 b3 b3 */ \
__ASM_EMIT("mulps %" x0 ", %" x4) \
__ASM_EMIT("mulps %" x1 ", %" x5) \
__ASM_EMIT("mulps %" x2 ", %" x6) \
__ASM_EMIT("mulps %" x3 ", %" x7) \
__ASM_EMIT("addps %" x5 ", %" x4) \
__ASM_EMIT("addps %" x7 ", %" x6) \
__ASM_EMIT("addps %" x6 ", %" x4)
void apply_matrix3d_mm2(matrix3d_t *r, const matrix3d_t *s, const matrix3d_t *m)
{
ARCH_X86_ASM
(
MATRIX_LOAD("A", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
// Multiply matrix and store
__ASM_EMIT("movups 0x00(%[B]), %%xmm4") // xmm4 = b0 b1 b2 b3
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
__ASM_EMIT("movups %%xmm4, 0x00(%[R])")
__ASM_EMIT("movups 0x10(%[B]), %%xmm5") // xmm5 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm5", "%xmm6", "%xmm7", "%xmm4")
__ASM_EMIT("movups %%xmm5, 0x10(%[R])")
__ASM_EMIT("movups 0x20(%[B]), %%xmm6") // xmm6 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm6", "%xmm7", "%xmm4", "%xmm5")
__ASM_EMIT("movups %%xmm6, 0x20(%[R])")
__ASM_EMIT("movups 0x30(%[B]), %%xmm7") // xmm7 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm7", "%xmm4", "%xmm5", "%xmm6")
__ASM_EMIT("movups %%xmm7, 0x30(%[R])")
:
: [A] "r" (s), [B] "r" (m), [R] "r" (r)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3",
"%xmm4", "%xmm5", "%xmm6", "%xmm7"
);
}
void apply_matrix3d_mm1(matrix3d_t *r, const matrix3d_t *m)
{
matrix3d_t __lsp_aligned16 t;
ARCH_X86_ASM
(
MATRIX_LOAD("A", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
// Multiply matrix and store
__ASM_EMIT("movups 0x00(%[B]), %%xmm4") // xmm4 = b0 b1 b2 b3
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
__ASM_EMIT("movaps %%xmm4, 0x00(%[T])")
__ASM_EMIT("movups 0x10(%[B]), %%xmm5") // xmm5 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm5", "%xmm6", "%xmm7", "%xmm4")
__ASM_EMIT("movaps %%xmm5, 0x10(%[T])")
__ASM_EMIT("movups 0x20(%[B]), %%xmm6") // xmm6 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm6", "%xmm7", "%xmm4", "%xmm5")
__ASM_EMIT("movaps %%xmm6, 0x20(%[T])")
__ASM_EMIT("movups 0x30(%[B]), %%xmm7") // xmm7 = b4 b5 b6 b7
MATMUL_CORE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm7", "%xmm4", "%xmm5", "%xmm6")
// Load matrix
__ASM_EMIT("movaps 0x00(%[T]), %%xmm4")
__ASM_EMIT("movaps 0x10(%[T]), %%xmm5")
__ASM_EMIT("movaps 0x20(%[T]), %%xmm6")
// Store matrix
MATRIX_STORE("A", "%xmm4", "%xmm5", "%xmm6", "%xmm7")
:
: [A] "r" (r), [B] "r" (m), [T] "r" (&t)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3",
"%xmm4", "%xmm5", "%xmm6", "%xmm7"
);
}
void transpose_matrix3d1(matrix3d_t *r)
{
ARCH_X86_ASM
(
MATRIX_LOAD("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
MAT4_TRANSPOSE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4")
MATRIX_STORE("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
:
: [m] "r" (r)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4"
);
}
void transpose_matrix3d2(matrix3d_t *r, const matrix3d_t *m)
{
ARCH_X86_ASM
(
MATRIX_LOAD("m", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
MAT4_TRANSPOSE("%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4")
MATRIX_STORE("r", "%xmm0", "%xmm1", "%xmm2", "%xmm3")
:
: [r] "r" (r), [m] "r" (m)
: "memory",
"%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4"
);
}
float check_triplet3d_p3n(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const vector3d_t *n)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p2]), %[x1]") // xmm1 = px2 py2 pz2 pw2
__ASM_EMIT("movups (%[p3]), %[x2]") // xmm2 = px3 py3 pz3 pw3
__ASM_EMIT("movups (%[p1]), %[x4]") // xmm4 = px1 py1 pz1 pw1
__ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x2 y2 z2 w2
__ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
__ASM_EMIT("subps %[x4], %[x1]") // xmm1 = x1 y1 z1 w1
TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [n] "r" (n)
);
return x0;
}
float check_triplet3d_pvn(const point3d_t *pv, const vector3d_t *n)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") // xmm1 = px2 py2 pz2 pw2
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") // xmm2 = px3 py3 pz3 pw3
__ASM_EMIT("movups 0x00(%[pv]), %[x4]") // xmm4 = px1 py1 pz1 pw1
__ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x2 y2 z2 w2
__ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
__ASM_EMIT("subps %[x4], %[x1]") // xmm1 = x1 y1 z1 w1
TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [pv] "r" (pv), [n] "r" (n)
);
return x0;
}
float check_triplet3d_v2n(const vector3d_t *v1, const vector3d_t *v2, const vector3d_t *n)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v1]), %[x1]") // xmm1 = x1 y1 z1 w1
__ASM_EMIT("movups (%[v2]), %[x2]") // xmm2 = x2 y2 z2 w2
__ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [v1] "r" (v1), [v2] "r" (v2), [n] "r" (n)
);
return x0;
}
float check_triplet3d_vvn(const vector3d_t *v, const vector3d_t *n)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[v]), %[x1]") // xmm1 = x1 y1 z1 w1
__ASM_EMIT("movups 0x10(%[v]), %[x2]") // xmm2 = x2 y2 z2 w2
__ASM_EMIT("movups (%[n]), %[x0]") // xmm0 = nx ny nz nw
TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [v] "r" (v), [n] "r" (n)
);
return x0;
}
float check_triplet3d_vv(const vector3d_t *v)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[v]), %[x1]") // xmm1 = x1 y1 z1 w1
__ASM_EMIT("movups 0x10(%[v]), %[x2]") // xmm2 = x2 y2 z2 w2
__ASM_EMIT("movups 0x20(%[v]), %[x0]") // xmm0 = nx ny nz nw
TRIPLET_CHECK("x0", "x1", "x2", "x3", "x4")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3), [x4] "=&x" (x4)
: [v] "r" (v)
);
return x0;
}
size_t longest_edge3d_p3(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3)
{
size_t res;
float x0, x1, x2, x3;
ARCH_X86_ASM
(
// Calc vectors
__ASM_EMIT("movups (%[p1]), %[x2]") // xmm2 = x0 y0 z0 w0
__ASM_EMIT("movups (%[p2]), %[x0]") // xmm0 = x1 y1 z1 w1
__ASM_EMIT("movups (%[p3]), %[x1]") // xmm1 = x2 y2 z2 w2
__ASM_EMIT("movaps %[x2], %[x3]") // xmm3 = x0 y0 z0 w0
__ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x0-x2 y0-y2 z0-z2 w0-w2 = dx2 dy2 dz2 0
__ASM_EMIT("subps %[x0], %[x1]") // xmm1 = x2-x1 y2-y1 z2-z1 w2-w1 = dx1 dy1 dz1 0
__ASM_EMIT("subps %[x3], %[x0]") // xmm0 = x1-x0 y1-y0 z1-z0 w1-w0 = dx0 dy0 dz0 0
// Calc scalar multiplication
__ASM_EMIT("mulps %[x2], %[x2]") // xmm2 = cx2 cy2 cz2 0
__ASM_EMIT("mulps %[x1], %[x1]") // xmm1 = bx2 by2 bz2 0
__ASM_EMIT("mulps %[x0], %[x0]") // xmm0 = ax2 ay2 az2 0
MAT3_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x0]") // xmm0 = A2 B2 C2 ?
__ASM_EMIT("xor %[res], %[res]") // res = 0
__ASM_EMIT("movhlps %[x0], %[x2]") // xmm2 = C2 ? ? ?
__ASM_EMIT("unpcklps %[x0], %[x0]") // xmm0 = A2 A2 B2 B2
__ASM_EMIT("movhlps %[x0], %[x1]") // xmm1 = B2 B2 ? ?
__ASM_EMIT("ucomiss %[x1], %[x0]") // A2 > B2
__ASM_EMIT("jae 1f")
// A2 < B2
__ASM_EMIT("ucomiss %[x1], %[x2]") // C2 > B2
__ASM_EMIT("seta %%al") // res = (B2 >= C2) ? 0 : 1
__ASM_EMIT("add $1, %[res]") // res = (B2 >= C2) ? 1 : 2
__ASM_EMIT("jmp 2f")
__ASM_EMIT("1:")
// A2 >= B2
__ASM_EMIT("ucomiss %[x2], %[x0]") // A2 > C2
__ASM_EMIT("jae 2f")
__ASM_EMIT("mov $2, %[res]") // res = (A2 >= C2) ? 0 : 2
__ASM_EMIT("2:")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[res] "=a" (res)
: [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3)
);
return res;
}
size_t longest_edge3d_pv(const point3d_t *p)
{
size_t res;
float x0, x1, x2, x3;
ARCH_X86_ASM
(
// Calc vectors
__ASM_EMIT("movups 0x00(%[p]), %[x2]") // xmm2 = x0 y0 z0 w0
__ASM_EMIT("movups 0x10(%[p]), %[x0]") // xmm0 = x1 y1 z1 w1
__ASM_EMIT("movups 0x20(%[p]), %[x1]") // xmm1 = x2 y2 z2 w2
__ASM_EMIT("movaps %[x2], %[x3]") // xmm3 = x0 y0 z0 w0
__ASM_EMIT("subps %[x1], %[x2]") // xmm2 = x0-x2 y0-y2 z0-z2 w0-w2 = dx2 dy2 dz2 dw2
__ASM_EMIT("subps %[x0], %[x1]") // xmm1 = x2-x1 y2-y1 z2-z1 w2-w1 = dx1 dy1 dz1 dw1
__ASM_EMIT("subps %[x3], %[x0]") // xmm0 = x1-x0 y1-y0 z1-z0 w1-w0 = dx0 dy0 dz0 dw0
// Calc scalar multiplication
__ASM_EMIT("mulps %[x2], %[x2]") // xmm2 = sx2 sy2 sz2 sw2
__ASM_EMIT("mulps %[x1], %[x1]") // xmm1 = sx2 sy2 sz2 sw2
__ASM_EMIT("mulps %[x0], %[x0]") // xmm0 = sx2 sy2 sz2 sw2
MAT3_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]")
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x0]") // xmm0 = S0 S1 S2 ?
__ASM_EMIT("xor %[res], %[res]")
__ASM_EMIT("movhlps %[x0], %[x2]") // xmm2 = S2 ? ? ? ?
__ASM_EMIT("unpcklps %[x0], %[x0]") // xmm0 = S0 S0 S1 S1
__ASM_EMIT("movhlps %[x0], %[x1]") // xmm1 = S1 ? ? ? ?
__ASM_EMIT("ucomiss %[x1], %[x0]") // A2 > B2
__ASM_EMIT("jae 1f")
// A2 < B2
__ASM_EMIT("ucomiss %[x1], %[x2]") // C2 > B2
__ASM_EMIT("seta %%al") // res = (B2 >= C2) ? 0 : 1
__ASM_EMIT("add $1, %[res]") // res = (B2 >= C2) ? 1 : 2
__ASM_EMIT("jmp 2f")
__ASM_EMIT("1:")
// A2 >= B2
__ASM_EMIT("ucomiss %[x2], %[x0]") // A2 > C2
__ASM_EMIT("jae 2f")
__ASM_EMIT("mov $2, %[res]") // res = (A2 >= C2) ? 0 : 2
__ASM_EMIT("2:")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[res] "=a" (res)
: [p] "r" (p)
);
return res;
}
float check_point3d_on_triangle_pvp(const point3d_t *pv, const point3d_t *p)
{
float x0, x1, x2, x3, x4, x5, x6, x7;
uint32_t mask;
ARCH_X86_ASM
(
/* Load vectors */
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz 1 */
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 1 */
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 1 */
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 1 */
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = v0 = dx0 dy0 dz0 0 */
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = v1 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = v2 = dx2 dy2 dz2 0 */
/* 3x vector multiplications */
VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
/* 3x scalar multiplications */
/* xmm0 = m0 = v0 cross v1 */
/* xmm1 = m1 = v1 cross v2 */
/* xmm2 = m2 = v2 cross v0 */
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
/* Compare with zeros */
/* xmm0 = r0 = m0 dot m1 */
/* xmm1 = r1 = m1 dot m2 */
/* xmm2 = r2 = m2 dot m0 */
__ASM_EMIT("unpcklps %[x1], %[x0]") /* xmm0 = r0 r1 ? ? */
__ASM_EMIT("movlhps %[x2], %[x0]") /* xmm0 = r0 r1 r2 ? */
__ASM_EMIT("xorps %[x4], %[x4]") /* xmm4 = 0 */
__ASM_EMIT("cmpps $6, %[x0], %[x4]") /* xmm4 = r0 < 0 r1 < 0 r2 < 0 */
__ASM_EMIT("movmskps %[x4], %[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
__ASM_EMIT("testl $0x07, %k[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
__ASM_EMIT("jnz 100f")
__ASM_EMIT("mulss %[x1], %[x0]") /* x0 = r0 * r1 */
__ASM_EMIT("mulss %[x2], %[x0]") /* x0 = r0 * r1 * r2 */
__ASM_EMIT("jmp 110f")
__ASM_EMIT("100:")
__ASM_EMIT("movss %[NEG], %[x0]") /* x0 = negative value */
/* End */
__ASM_EMIT("110:")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7),
[mask] "=&r"(mask)
: [pv] "r" (pv), [p] "r" (p),
[NEG] "m" (X_3D_MTOLERANCE)
: "cc"
);
return x0;
}
float check_point3d_on_triangle_p3p(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const point3d_t *p)
{
float x0, x1, x2, x3, x4, x5, x6, x7;
uint32_t mask;
ARCH_X86_ASM
(
/* Load vectors */
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz 1 */
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 1 */
__ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 1 */
__ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 1 */
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = v0 = dx0 dy0 dz0 0 */
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = v1 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = v2 = dx2 dy2 dz2 0 */
/* 3x vector multiplications */
VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
/* 3x scalar multiplications */
/* xmm0 = m0 = v0 cross v1 */
/* xmm1 = m1 = v1 cross v2 */
/* xmm2 = m2 = v2 cross v0 */
SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
/* Compare with zeros */
/* xmm0 = r0 = m0 dot m1 */
/* xmm1 = r1 = m1 dot m2 */
/* xmm2 = r2 = m2 dot m0 */
__ASM_EMIT("unpcklps %[x1], %[x0]") /* xmm0 = r0 r1 ? ? */
__ASM_EMIT("movlhps %[x2], %[x0]") /* xmm0 = r0 r1 r2 ? */
__ASM_EMIT("xorps %[x4], %[x4]") /* xmm4 = 0 */
__ASM_EMIT("cmpps $6, %[x0], %[x4]") /* xmm4 = r0 < 0 r1 < 0 r2 < 0 */
__ASM_EMIT("movmskps %[x4], %[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
__ASM_EMIT("testl $0x07, %k[mask]") /* mask = r0 < 0 || r1 < 0 || r2 < 0 */
__ASM_EMIT("jnz 100f")
__ASM_EMIT("mulss %[x1], %[x0]") /* x0 = r0 * r1 */
__ASM_EMIT("mulss %[x2], %[x0]") /* x0 = r0 * r1 * r2 */
__ASM_EMIT("jmp 110f")
__ASM_EMIT("100:")
__ASM_EMIT("movss %[NEG], %[x0]") /* x0 = negative value */
/* End */
__ASM_EMIT("110:")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7),
[mask] "=&r"(mask)
: [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [p] "r" (p),
[NEG] "m" (X_3D_MTOLERANCE)
: "cc"
);
return x0;
}
void calc_normal3d_v2(vector3d_t *n, const vector3d_t *v1, const vector3d_t *v2)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[v1]), %[x0]") /* xmm0 = dx1 dy1 dz1 dw1 */
__ASM_EMIT("movups (%[v2]), %[x1]") /* xmm1 = dx2 dy2 dz2 dw2 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
__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 */
__ASM_EMIT("movups %[x1], (%[n])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [n] "r" (n), [v1] "r" (v1), [v2] "r" (v2)
: "memory"
);
}
void calc_normal3d_vv(vector3d_t *n, const vector3d_t *vv)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[vv]), %[x0]") /* xmm0 = dx1 dy1 dz1 dw1 */
__ASM_EMIT("movups 0x10(%[vv]), %[x1]") /* xmm1 = dx2 dy2 dz2 dw2 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
__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 */
__ASM_EMIT("movups %[x1], (%[n])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [n] "r" (n), [vv] "r" (vv)
: "memory"
);
}
void calc_normal3d_p3(vector3d_t *n, const point3d_t *p1, const point3d_t *p2, const point3d_t *p3)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p1]), %[x2]") /* xmm0 = x1 y1 z1 w1 */
__ASM_EMIT("movups (%[p2]), %[x0]") /* xmm1 = x2 y2 z2 w2 */
__ASM_EMIT("movups (%[p3]), %[x1]") /* xmm2 = x3 y3 z3 w3 */
__ASM_EMIT("subps %[x0], %[x1]") /* xmm0 = dx1 dy1 dz1 dw1 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm1 = dx2 dy2 dz2 dw2 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
__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 */
__ASM_EMIT("movups %[x1], (%[n])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [n] "r" (n), [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3)
: "memory"
);
}
void calc_normal3d_pv(vector3d_t *n, const point3d_t *pv)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm0 = x1 y1 z1 w1 */
__ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm1 = x2 y2 z2 w2 */
__ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm2 = x3 y3 z3 w3 */
__ASM_EMIT("subps %[x0], %[x1]") /* xmm0 = dx1 dy1 dz1 dw1 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm1 = dx2 dy2 dz2 dw2 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0xc9, %[x1], %[x1]") /* xmm1 = NX NY NZ 0 */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(W2) = W */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W 0/W */
__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 */
__ASM_EMIT("movups %[x1], (%[n])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [n] "r" (n), [pv] "r" (pv)
: "memory"
);
}
float calc_area_p3(const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p0]), %[x2]") /* xmm0 = x0 y0 z0 w0 */
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm1 = x1 y1 z1 w1 */
__ASM_EMIT("movups (%[p2]), %[x1]") /* xmm2 = x2 y2 z2 w2 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(NX*NX + NY*NY + NZ*NZ) */
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2)
: "memory"
);
return x0;
}
float calc_area_pv(const point3d_t *pv)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm0 = x0 y0 z0 w0 */
__ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm1 = x1 y1 z1 w1 */
__ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm2 = x2 y2 z2 w2 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY NW */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ */
__ASM_EMIT("sqrtss %[x0], %[x0]") /* xmm0 = sqrtf(NX*NX + NY*NY + NZ*NZ) */
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [pv] "r" (pv)
: "memory"
);
return x0;
}
float calc_plane_p3(vector3d_t *v, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p0]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
__ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x2 y2 z2 w2 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
__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 */
__ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
__ASM_EMIT("movups (%[p0]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
__ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz nz */
VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
__ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
__ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
__ASM_EMIT("movups %[x2], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [v] "r" (v), [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2),
[X_ISIGN] "m" (X_SMASK0111)
: "memory"
);
return x0;
}
float calc_plane_pv(vector3d_t *v, const point3d_t *pv)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[pv]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
__ASM_EMIT("movups 0x10(%[pv]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
__ASM_EMIT("movups 0x20(%[pv]), %[x1]") /* xmm1 = x2 y2 z2 w2 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
__ASM_EMIT("subps %[x2], %[x1]") /* xmm1 = p2 - p0 = dx2 dy2 dz2 0 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
__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 */
__ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
__ASM_EMIT("movups 0x00(%[pv]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
__ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz -nz */
VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
__ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
__ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
__ASM_EMIT("movups %[x2], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [v] "r" (v), [pv] "r" (pv),
[X_ISIGN] "m" (X_SMASK0111)
: "memory"
);
return x0;
}
float calc_plane_v1p2(vector3d_t *v, const vector3d_t *v0, const point3d_t *p0, const point3d_t *p1)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p0]), %[x2]") /* xmm2 = x0 y0 z0 w0 */
__ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x1 y1 z1 w1 */
__ASM_EMIT("movups (%[v0]), %[x1]") /* xmm1 = v = dx2 dy2 dz2 0 */
__ASM_EMIT("subps %[x2], %[x0]") /* xmm0 = p1 - p0 = dx1 dy1 dz1 0 */
VECTOR_MUL("[x0]", "[x1]", "[x2]", "[x3]") /* xmm0 = NZ NX NY 0 */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = NZ NX NY 0 */
VECTOR_DPPS3("[x0]", "[x0]", "[x2]") /* xmm0 = NX*NX + NY*NY + NZ*NZ = W2 */
__ASM_EMIT("shufps $0x09, %[x1], %[x1]") /* xmm1 = NX NY NZ NZ */
__ASM_EMIT("shufps $0x00, %[x0], %[x0]") /* xmm0 = W2 W2 W2 W2 */
__ASM_EMIT("xorps %[x3], %[x3]") /* xmm3 = 0 */
__ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(W2) sqrtf(W2) sqrtf(W2) sqrtf(W2) = W W W W */
__ASM_EMIT("cmpps $4, %[x0], %[x3]") /* xmm3 = W != 0 */
__ASM_EMIT("divps %[x0], %[x1]") /* xmm1 = NX/W NY/W NZ/W NZ/W */
__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 */
__ASM_EMIT("movaps %[x1], %[x2]") /* xmm2 = nx ny nz nz */
__ASM_EMIT("movups (%[p0]), %[x3]") /* xmm3 = x0 y0 z0 w0 */
__ASM_EMIT("xorps %[X_ISIGN], %[x1]") /* xmm1 = -nx -ny -nz -nz */
VECTOR_DPPS3("[x1]", "[x3]", "[x3]") /* xmm1 = -(nx*x0 + ny*y0 + nz*z0) = dw ? */
__ASM_EMIT("shufps $0xf0, %[x2], %[x1]") /* xmm1 = dw dw nz nz */
__ASM_EMIT("shufps $0x24, %[x1], %[x2]") /* xmm2 = nx ny nz dw */
__ASM_EMIT("movups %[x2], (%[v])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [v] "r" (v), [v0] "r" (v0), [p0] "r" (p0), [p1] "r" (p1),
[X_ISIGN] "m" (X_SMASK0111)
: "memory"
);
return x0;
}
void calc_split_point_p2v1(point3d_t *sp, const point3d_t *l0, const point3d_t *l1, const vector3d_t *pl)
{
float x0, x1, x2, x3, x4, x5, x6;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[l0]), %[x1]") /* xmm1 = l0 = lx0 ly0 lz0 ? */
__ASM_EMIT("movups (%[l1]), %[x0]") /* xmm0 = l1 = lx1 ly1 lz1 ? */
__ASM_EMIT("movups (%[pl]), %[x2]") /* xmm2 = pl = nx ny nz nw */
__ASM_EMIT("subps %[x1], %[x0]") /* xmm0 = d = l1 - l0 = dx dy dz ? */
__ASM_EMIT("movaps %[x2], %[x3]") /* xmm3 = pl */
__ASM_EMIT("movaps %[x2], %[x4]") /* xmm4 = pl */
__ASM_EMIT("mulps %[x1], %[x2]") /* xmm2 = l0 * pl = lx0*nx ly0*ny lz0*nz ? */
__ASM_EMIT("mulps %[x0], %[x3]") /* xmm3 = d * pl = dx*nx dy*ny dz*nz ? */
__ASM_EMIT("movhlps %[x2], %[x5]") /* xmm5 = lz0*nz ? */
__ASM_EMIT("shufps $0xff, %[x4], %[x4]") /* xmm4 = nw nw nw nw */
__ASM_EMIT("addss %[x2], %[x5]") /* xmm5 = lz0*nz + lx0*nx */
__ASM_EMIT("movhlps %[x3], %[x6]") /* xmm6 = dz*nz ? */
__ASM_EMIT("shufps $0x55, %[x2], %[x2]") /* xmm2 = ly0*ny ly0*ny ly0*ny ly0*ny */
__ASM_EMIT("addss %[x3], %[x6]") /* xmm6 = dz*nz + dx*nx */
__ASM_EMIT("addss %[x5], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz */
__ASM_EMIT("shufps $0x55, %[x3], %[x3]") /* xmm3 = dy*ny dy*ny dy*ny dy*ny */
__ASM_EMIT("addss %[x4], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz + nw */
__ASM_EMIT("addss %[x6], %[x3]") /* xmm3 = dx*nx + dy*ny + dz*nz */
__ASM_EMIT("divss %[x3], %[x2]") /* xmm2 = t = (lx0*nx + ly0*ny + lz0*nz + nw) / (dx*nx + dy*ny + dz*nz) */
__ASM_EMIT("shufps $0x00, %[x2], %[x2]") /* xmm2 = t t t t */
__ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = dx*t dy*t dz*t ? */
__ASM_EMIT("subps %[x0], %[x1]") /* xmm1 = l0 - d*t */
__ASM_EMIT("andps %[X_3DMASK], %[x1]")
__ASM_EMIT("orps %[X_3DPOINT], %[x1]")
__ASM_EMIT("movups %[x1], (%[sp])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6)
: [sp] "r" (sp), [l0] "r" (l0), [l1] "r" (l1), [pl] "r" (pl),
[X_3DMASK] "m" (X_3DMASK),
[X_3DPOINT] "m" (X_3DPOINT)
: "memory"
);
}
void calc_split_point_pvv1(point3d_t *sp, const point3d_t *lv, const vector3d_t *pl)
{
float x0, x1, x2, x3, x4, x5, x6;
ARCH_X86_ASM
(
__ASM_EMIT("movups 0x00(%[lv]), %[x1]") /* xmm1 = l0 = lx0 ly0 lz0 ? */
__ASM_EMIT("movups 0x10(%[lv]), %[x0]") /* xmm0 = l1 = lx1 ly1 lz1 ? */
__ASM_EMIT("movups (%[pl]), %[x2]") /* xmm2 = pl = nx ny nz nw */
__ASM_EMIT("subps %[x1], %[x0]") /* xmm0 = d = l1 - l0 = dx dy dz ? */
__ASM_EMIT("movaps %[x2], %[x3]") /* xmm3 = pl */
__ASM_EMIT("movaps %[x2], %[x4]") /* xmm4 = pl */
__ASM_EMIT("mulps %[x1], %[x2]") /* xmm2 = l0 * pl = lx0*nx ly0*ny lz0*nz ? */
__ASM_EMIT("mulps %[x0], %[x3]") /* xmm3 = d * pl = dx*nx dy*ny dz*nz ? */
__ASM_EMIT("movhlps %[x2], %[x5]") /* xmm5 = lz0*nz ? */
__ASM_EMIT("shufps $0xff, %[x4], %[x4]") /* xmm4 = nw nw nw nw */
__ASM_EMIT("addss %[x2], %[x5]") /* xmm5 = lz0*nz + lx0*nx */
__ASM_EMIT("movhlps %[x3], %[x6]") /* xmm6 = dz*nz ? */
__ASM_EMIT("shufps $0x55, %[x2], %[x2]") /* xmm2 = ly0*ny ly0*ny ly0*ny ly0*ny */
__ASM_EMIT("addss %[x3], %[x6]") /* xmm6 = dz*nz + dx*nx */
__ASM_EMIT("addss %[x5], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz */
__ASM_EMIT("shufps $0x55, %[x3], %[x3]") /* xmm3 = dy*ny dy*ny dy*ny dy*ny */
__ASM_EMIT("addss %[x4], %[x2]") /* xmm2 = lx0*nx + ly0*ny + lz0*nz + nw */
__ASM_EMIT("addss %[x6], %[x3]") /* xmm3 = dx*nx + dy*ny + dz*nz */
__ASM_EMIT("divss %[x3], %[x2]") /* xmm2 = t = (lx0*nx + ly0*ny + lz0*nz + nw) / (dx*nx + dy*ny + dz*nz) */
__ASM_EMIT("shufps $0x00, %[x2], %[x2]") /* xmm2 = t t t t */
__ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = dx*t dy*t dz*t ? */
__ASM_EMIT("subps %[x0], %[x1]") /* xmm1 = l0 - d*t */
__ASM_EMIT("andps %[X_3DMASK], %[x1]")
__ASM_EMIT("orps %[X_3DPOINT], %[x1]")
__ASM_EMIT("movups %[x1], (%[sp])")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6)
: [sp] "r" (sp), [lv] "r" (lv), [pl] "r" (pl),
[X_3DMASK] "m" (X_3DMASK),
[X_3DPOINT] "m" (X_3DPOINT)
: "memory"
);
}
float calc_min_distance_p3(const point3d_t *sp, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[sp]), %[x3]") /* xmm3 = pl */
__ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = d0 */
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = d1 */
__ASM_EMIT("mulps %[x0], %[x0]") /* xmm0 = d0 * d0 */
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = d2 */
__ASM_EMIT("mulps %[x1], %[x1]") /* xmm1 = d1 * d1 */
__ASM_EMIT("mulps %[x2], %[x2]") /* xmm2 = d2 * d2 */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k1) sqrtf(k2) ? */
__ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = sqrtf(k2) */
__ASM_EMIT("unpcklps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k0) sqrtf(k1) sqrtf(k1) */
__ASM_EMIT("movhlps %[x0], %[x2]") /* xmm1 = sqrtf(k2) */
__ASM_EMIT("minss %[x1], %[x0]")
__ASM_EMIT("minss %[x2], %[x0]")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [sp] "r" (sp), [p0] "r" (p0), [p1] "r" (p1), [p2] "r" (p2)
:
);
return x0;
}
float calc_min_distance_pv(const point3d_t *sp, const point3d_t *pv)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[sp]), %[x3]") /* xmm3 = pl */
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */
__ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = d0 */
__ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = d1 */
__ASM_EMIT("mulps %[x0], %[x0]") /* xmm0 = d0 * d0 */
__ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = d2 */
__ASM_EMIT("mulps %[x1], %[x1]") /* xmm1 = d1 * d1 */
__ASM_EMIT("mulps %[x2], %[x2]") /* xmm2 = d2 * d2 */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("sqrtps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k1) sqrtf(k2) ? */
__ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = sqrtf(k2) */
__ASM_EMIT("unpcklps %[x0], %[x0]") /* xmm0 = sqrtf(k0) sqrtf(k0) sqrtf(k1) sqrtf(k1) */
__ASM_EMIT("movhlps %[x0], %[x2]") /* xmm1 = sqrtf(k2) */
__ASM_EMIT("minss %[x1], %[x0]")
__ASM_EMIT("minss %[x2], %[x0]")
: [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [sp] "r" (sp), [pv] "r" (pv)
:
);
return x0;
}
size_t colocation_x2_v1p2(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[pl]), %[x2]") /* xmm3 = pl */
__ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = x0*px y0*py z0*pz w0*pw */
__ASM_EMIT("mulps %[x2], %[x1]") /* xmm1 = x1*px y1*py z1*pz w1*pw */
__ASM_EMIT("movaps %[x0], %[x3]") /* xmm3 = x0*px y0*py z0*pz w0*pw */
__ASM_EMIT("movlhps %[x1], %[x0]") /* xmm0 = x0*px y0*py x1*px y1*py */
__ASM_EMIT("movhlps %[x3], %[x1]") /* xmm1 = z0*pz w0*pw z1*pz w1*pw */
__ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+z0*pz y0*py+w0*pw x1*px+z1*pz y1*py+w1*pw */
__ASM_EMIT("shufps $0xd8, %[x0], %[x0]") /* xmm0 = x0*px+z0*pz x1*px+z1*pz y0*py+w0*pw y1*py+w1*pw */
__ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = y0*py+w0*pw y1*py+w1*pw ? ? */
__ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+y0*py+z0*pz+w0*pw x1*px+y1*py+z1*pz+w1*pw ? ? = k0 k1 ? ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 ? ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL ? ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL ? ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] ? ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] ? ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p0]")
__ASM_EMIT64("cvttss2si %[x0], %k[p0]")
__ASM_EMIT("shufps $0x55, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p1]")
__ASM_EMIT64("cvttss2si %[x0], %k[p1]")
__ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]")
: [p0] "+r" (p0), [p1] "+r" (p1),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [pl] "r" (pl),
[PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return size_t(p0);
}
size_t colocation_x2_v1pv(const vector3d_t *pl, const point3d_t *pv)
{
float x0, x1, x2, x3;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[pl]), %[x2]") /* xmm3 = pl */
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("mulps %[x2], %[x0]") /* xmm0 = x0*px y0*py z0*pz w0*pw */
__ASM_EMIT("mulps %[x2], %[x1]") /* xmm1 = x1*px y1*py z1*pz w1*pw */
__ASM_EMIT("movaps %[x0], %[x3]") /* xmm3 = x0*px y0*py z0*pz w0*pw */
__ASM_EMIT("movlhps %[x1], %[x0]") /* xmm0 = x0*px y0*py x1*px y1*py */
__ASM_EMIT("movhlps %[x3], %[x1]") /* xmm1 = z0*pz w0*pw z1*pz w1*pw */
__ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+z0*pz y0*py+w0*pw x1*px+z1*pz y1*py+w1*pw */
__ASM_EMIT("shufps $0xd8, %[x0], %[x0]") /* xmm0 = x0*px+z0*pz x1*px+z1*pz y0*py+w0*pw y1*py+w1*pw */
__ASM_EMIT("movhlps %[x0], %[x1]") /* xmm1 = y0*py+w0*pw y1*py+w1*pw ? ? */
__ASM_EMIT("addps %[x1], %[x0]") /* xmm0 = x0*px+y0*py+z0*pz+w0*pw x1*px+y1*py+z1*pz+w1*pw ? ? = k0 k1 ? ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 ? ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL ? ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL ? ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] ? ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] ? ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pl]")
__ASM_EMIT64("cvttss2si %[x0], %k[pl]")
__ASM_EMIT("shufps $0x55, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pv]")
__ASM_EMIT64("cvttss2si %[x0], %k[pv]")
__ASM_EMIT("lea (%[pl], %[pv], 4), %[pl]")
: [pl] "+r" (pl), [pv] "+r" (pv),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3)
: [PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return size_t(pl);
}
size_t colocation_x3_v1p3(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
__ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */
__ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = p0 * pl */
__ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = p1 * pl */
__ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = p2 * pl */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p0]")
__ASM_EMIT64("cvttss2si %[x0], %k[p0]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p1]")
__ASM_EMIT64("cvttss2si %[x0], %k[p1]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p2]")
__ASM_EMIT64("cvttss2si %[x0], %k[p2]")
__ASM_EMIT("lea (%[p1], %[p2], 4), %[p1]")
__ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]")
: [p0] "+r" (p0), [p1] "+r" (p1), [p2] "+r" (p2),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [pl] "r" (pl),
[PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return size_t(p0);
}
size_t colocation_x3_v1pv(const vector3d_t *pl, const point3d_t *pv)
{
float x0, x1, x2, x3, x4;
size_t pt;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
__ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
__ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
__ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */
__ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = p0 * pl */
__ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = p1 * pl */
__ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = p2 * pl */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pl]")
__ASM_EMIT64("cvttss2si %[x0], %k[pl]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pv]")
__ASM_EMIT64("cvttss2si %[x0], %k[pv]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pt]")
__ASM_EMIT64("cvttss2si %[x0], %k[pt]")
__ASM_EMIT("lea (%[pv], %[pt], 4), %[pt]")
__ASM_EMIT("lea (%[pl], %[pt], 4), %[pt]")
: [pl] "+r" (pl), [pv] "+r" (pv), [pt] "=&r" (pt),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return pt;
}
size_t colocation_x3_v3p1(const vector3d_t *v0, const vector3d_t *v1, const vector3d_t *v2, const point3d_t *p)
{
float x0, x1, x2, x3, x4;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = p */
__ASM_EMIT("movups (%[v0]), %[x0]") /* xmm0 = v0 */
__ASM_EMIT("movups (%[v1]), %[x1]") /* xmm1 = v1 */
__ASM_EMIT("movups (%[v2]), %[x2]") /* xmm2 = v2 */
__ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = v0 * p */
__ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = v1 * p */
__ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = v2 * p */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[v0]")
__ASM_EMIT64("cvttss2si %[x0], %k[v0]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[v1]")
__ASM_EMIT64("cvttss2si %[x0], %k[v1]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[v2]")
__ASM_EMIT64("cvttss2si %[x0], %k[v2]")
__ASM_EMIT("lea (%[v1], %[v2], 4), %[v1]")
__ASM_EMIT("lea (%[v0], %[v1], 4), %[v0]")
: [v0] "+r" (v0), [v1] "+r" (v1), [v2] "+r" (v2),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [p] "r" (p),
[PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return size_t(v0);
}
size_t colocation_x3_vvp1(const vector3d_t *vv, const point3d_t *p)
{
float x0, x1, x2, x3, x4;
size_t pt;
ARCH_X86_ASM
(
__ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = p */
__ASM_EMIT("movups 0x00(%[vv]), %[x0]") /* xmm0 = v0 */
__ASM_EMIT("movups 0x10(%[vv]), %[x1]") /* xmm1 = v1 */
__ASM_EMIT("movups 0x20(%[vv]), %[x2]") /* xmm2 = v2 */
__ASM_EMIT("mulps %[x3], %[x0]") /* xmm0 = v0 * p */
__ASM_EMIT("mulps %[x3], %[x1]") /* xmm1 = v1 * p */
__ASM_EMIT("mulps %[x3], %[x2]") /* xmm2 = v2 * p */
MAT4_TRANSPOSE("[x0]", "[x1]", "[x2]", "[x3]", "[x4]")
/* xmm0 = x0 x1 x2 ? */
/* xmm1 = y0 y1 y2 ? */
/* xmm2 = z0 z1 z2 ? */
/* xmm3 = w0 w1 w2 ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT("addps %[x3], %[x2]")
__ASM_EMIT("addps %[x2], %[x0]") /* xmm0 = k0 k1 k2 ? */
__ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 ? */
__ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
__ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
__ASM_EMIT("andps %[ONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
__ASM_EMIT("andps %[ONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
__ASM_EMIT("addps %[x1], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[vv]")
__ASM_EMIT64("cvttss2si %[x0], %k[vv]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[p]")
__ASM_EMIT64("cvttss2si %[x0], %k[p]")
__ASM_EMIT("shufps $0x39, %[x0], %[x0]")
__ASM_EMIT32("cvttss2si %[x0], %[pt]")
__ASM_EMIT64("cvttss2si %[x0], %k[pt]")
__ASM_EMIT("lea (%[p], %[pt], 4), %[pt]")
__ASM_EMIT("lea (%[vv], %[pt], 4), %[pt]")
: [vv] "+r" (vv), [p] "+r" (p), [pt] "=&r" (pt),
[x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
[x4] "=&x" (x4)
: [PTOL] "m" (X_3D_TOLERANCE),
[MTOL] "m" (X_3D_MTOLERANCE),
[ONE] "m" (ONE)
: "cc"
);
return pt;
}
} /* namespace sse */
} /* namespace lsp */
#undef SCALAR_MUL
#undef MAT3_TRANSPOSE
#undef MAT4_TRANSPOSE
#undef MATRIX_LOAD
#undef MATRIX_STORE
#undef NORMALIZE
#undef TRIPLET_CHECK
#undef SCALAR_MUL3
#undef VECTOR_MUL3
#undef VECTOR_MUL
#undef VECTOR_XCHG
#endif /* PRIVATE_DSP_ARCH_X86_SSE_3DMATH_H_ */