Debian Codegraph @

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

raw
1 /*
2 * Copyright (C) 2026 Linux Studio Plugins Project <https://lsp-plug.in/>
3 * (C) 2026 Vladimir Sadovnikov <[email protected]>
4 *
5 * This file is part of lsp-dsp-lib
6 * Created on: 7 авг. 2026 г.
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_AVX_3DMATH_3DMATH_H_
23 #define PRIVATE_DSP_ARCH_X86_AVX_3DMATH_3DMATH_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_AVX_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_AVX_IMPL */
28
29 #include <private/dsp/arch/x86/avx/3dmath/common.h>
30
31 namespace lsp
32 {
33 namespace avx
34 {
35 using namespace dsp;
36
37 void init_matrix3d(matrix3d_t *dst, const matrix3d_t *src)
38 {
39 ARCH_X86_ASM
40 (
41 __ASM_EMIT("vmovups 0x00(%[s]), %%ymm0")
42 __ASM_EMIT("vmovups 0x20(%[s]), %%ymm1")
43 __ASM_EMIT("vmovups %%ymm0, 0x00(%[d])")
44 __ASM_EMIT("vmovups %%ymm1, 0x20(%[d])")
45 :
46 : [s] "r" (src), [d] "r" (dst)
47 : "memory",
48 "%xmm0", "%xmm1"
49 );
50 }
51
52 void init_matrix3d_one(matrix3d_t *m)
53 {
54 ARCH_X86_ASM
55 (
56 __ASM_EMIT("vmovaps %[one], %%ymm0")
57 __ASM_EMIT("vmovups %%ymm0, 0x00(%[m])")
58 __ASM_EMIT("vmovups %%ymm0, 0x20(%[m])")
59 :
60 : [m] "r" (m),
61 [one] "m" (ONE)
62 : "memory",
63 "%xmm0"
64 );
65 }
66
67 void init_matrix3d_identity(matrix3d_t *m)
68 {
69 ARCH_X86_ASM
70 (
71 __ASM_EMIT("vmovaps 0x00 + %[id], %%ymm0")
72 __ASM_EMIT("vmovaps 0x20 + %[id], %%ymm1")
73 __ASM_EMIT("vmovups %%ymm0, 0x00(%[m])")
74 __ASM_EMIT("vmovups %%ymm1, 0x20(%[m])")
75 :
76 : [m] "r" (m),
77 [id] "o" (IDENTITY)
78 : "memory",
79 "%xmm0", "%xmm1"
80 );
81 }
82
83 void init_matrix3d_rotate_xyz(matrix3d_t *m, float x, float y, float z, float angle)
84 {
85 if (x == 0.0f)
86 {
87 if (y == 0.0f)
88 {
89 if (z > 0.0f)
90 init_matrix3d_rotate_z(m, angle);
91 else if (z < 0.0f)
92 init_matrix3d_rotate_z(m, -angle);
93 else // (z == 0.0)
94 init_matrix3d_identity(m);
95 return;
96 }
97 else if (z == 0.0f)
98 {
99 if (y > 0.0f)
100 init_matrix3d_rotate_y(m, angle);
101 else if (y < 0.0f)
102 init_matrix3d_rotate_y(m, -angle);
103 return;
104 }
105 }
106 else if ((y == 0.0) && (z == 0.0))
107 {
108 if (x > 0.0f)
109 init_matrix3d_rotate_x(m, angle);
110 else
111 init_matrix3d_rotate_x(m, -angle);
112 return;
113 }
114
115 const float s = sinf(angle);
116 const float c = cosf(angle);
117
118 float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
119 const float mag = sqrtf(x*x + y*y + z*z);
120
121 x /= mag;
122 y /= mag;
123 z /= mag;
124
125 xx = x * x;
126 yy = y * y;
127 zz = z * z;
128 xy = x * y;
129 yz = y * z;
130 zx = z * x;
131 xs = x * s;
132 ys = y * s;
133 zs = z * s;
134 one_c = 1.0f - c;
135
136 float *M = m->m;
137 M[0] = (one_c * xx) + c;
138 M[1] = (one_c * xy) + zs;
139 M[2] = (one_c * zx) - ys;
140 M[3] = 0.0f;
141 M[4] = (one_c * xy) - zs;
142 M[5] = (one_c * yy) + c;
143 M[6] = (one_c * yz) + xs;
144 M[7] = 0.0f;
145 M[8] = (one_c * zx) + ys;
146 M[9] = (one_c * yz) - xs;
147 M[10] = (one_c * zz) + c;
148 M[11] = 0.0f;
149 M[12] = 0.0f;
150 M[13] = 0.0f;
151 M[14] = 0.0f;
152 M[15] = 0.0f;
153 }
154
155 } /* namespace avx */
156 } /* namespace lsp */
157
158 #endif /* INCLUDE_PRIVATE_DSP_ARCH_X86_AVX_3DMATH_3DMATH_H_ */