/src/Simd/src/Simd/SimdSse41WarpAffine.cpp
Line | Count | Source |
1 | | /* |
2 | | * Simd Library (http://ermig1979.github.io/Simd). |
3 | | * |
4 | | * Copyright (c) 2011-2024 Yermalayeu Ihar. |
5 | | * |
6 | | * Permission is hereby granted, free of charge, to any person obtaining a copy |
7 | | * of this software and associated documentation files (the "Software"), to deal |
8 | | * in the Software without restriction, including without limitation the rights |
9 | | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
10 | | * copies of the Software, and to permit persons to whom the Software is |
11 | | * furnished to do so, subject to the following conditions: |
12 | | * |
13 | | * The above copyright notice and this permission notice shall be included in |
14 | | * all copies or substantial portions of the Software. |
15 | | * |
16 | | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
17 | | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
18 | | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
19 | | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
20 | | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
21 | | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
22 | | * SOFTWARE. |
23 | | */ |
24 | | #include "Simd/SimdWarpAffine.h" |
25 | | #include "Simd/SimdWarpAffineCommon.h" |
26 | | #include "Simd/SimdCopy.h" |
27 | | #include "Simd/SimdUnpack.h" |
28 | | #include "Simd/SimdStore.h" |
29 | | |
30 | | #include "Simd/SimdPoint.hpp" |
31 | | |
32 | | namespace Simd |
33 | | { |
34 | | #ifdef SIMD_SSE41_ENABLE |
35 | | namespace Sse41 |
36 | | { |
37 | | template<int N> SIMD_INLINE void FillBorder(uint8_t* dst, int count, const __m128i & bv, const uint8_t * bs) |
38 | 0 | { |
39 | 0 | int i = 0, size = count * N, size16 = (int)AlignLo(size, 16); |
40 | 0 | for (; i < size16; i += 16) |
41 | 0 | _mm_storeu_si128((__m128i*)(dst + i), bv); |
42 | 0 | for (; i < size; i += N) |
43 | 0 | Base::CopyPixel<N>(bs, dst + i); |
44 | 0 | } Unexecuted instantiation: void Simd::Sse41::FillBorder<1>(unsigned char*, int, long long __vector(2) const&, unsigned char const*) Unexecuted instantiation: void Simd::Sse41::FillBorder<2>(unsigned char*, int, long long __vector(2) const&, unsigned char const*) Unexecuted instantiation: void Simd::Sse41::FillBorder<4>(unsigned char*, int, long long __vector(2) const&, unsigned char const*) |
45 | | |
46 | | template<> SIMD_INLINE void FillBorder<3>(uint8_t* dst, int count, const __m128i& bv, const uint8_t* bs) |
47 | 0 | { |
48 | 0 | int i = 0, size = count * 3, size3 = size - 3; |
49 | 0 | for (; i < size3; i += 3) |
50 | 0 | Base::CopyPixel<4>(bs, dst + i); |
51 | 0 | for (; i < size; i += 3) |
52 | 0 | Base::CopyPixel<3>(bs, dst + i); |
53 | 0 | } |
54 | | |
55 | | template<int N> SIMD_INLINE __m128i InitBorder(const uint8_t* border) |
56 | 0 | { |
57 | 0 | switch (N) |
58 | 0 | { |
59 | 0 | case 1: return _mm_set1_epi8(*border); |
60 | 0 | case 2: return _mm_set1_epi16(*(uint16_t*)border); |
61 | 0 | case 3: return _mm_setzero_si128(); |
62 | 0 | case 4: return _mm_set1_epi32(*(uint32_t*)border); |
63 | 0 | } |
64 | 0 | return _mm_setzero_si128(); |
65 | 0 | } Unexecuted instantiation: long long __vector(2) Simd::Sse41::InitBorder<1>(unsigned char const*) Unexecuted instantiation: long long __vector(2) Simd::Sse41::InitBorder<2>(unsigned char const*) Unexecuted instantiation: long long __vector(2) Simd::Sse41::InitBorder<3>(unsigned char const*) Unexecuted instantiation: long long __vector(2) Simd::Sse41::InitBorder<4>(unsigned char const*) |
66 | | |
67 | | //----------------------------------------------------------------------------------------- |
68 | | |
69 | | SIMD_INLINE __m128i NearestOffset(__m128 x, __m128 y, const __m128* m, __m128i w, const __m128i & h, const __m128i & n, const __m128i & s) |
70 | 0 | { |
71 | 0 | __m128 dx = _mm_add_ps(_mm_add_ps(_mm_mul_ps(x, m[0]), _mm_mul_ps(y, m[1])), m[2]); |
72 | 0 | __m128 dy = _mm_add_ps(_mm_add_ps(_mm_mul_ps(x, m[3]), _mm_mul_ps(y, m[4])), m[5]); |
73 | 0 | __m128i ix = _mm_min_epi32(_mm_max_epi32(_mm_cvtps_epi32(dx), _mm_setzero_si128()), w); |
74 | 0 | __m128i iy = _mm_min_epi32(_mm_max_epi32(_mm_cvtps_epi32(dy), _mm_setzero_si128()), h); |
75 | 0 | return _mm_add_epi32(_mm_mullo_epi32(ix, n), _mm_mullo_epi32(iy, s)); |
76 | 0 | } |
77 | | |
78 | | //----------------------------------------------------------------------------------------- |
79 | | |
80 | | template<int N> void NearestRun(const WarpAffParam& p, int yBeg, int yEnd, const int32_t* beg, const int32_t* end, const uint8_t* src, uint8_t* dst, uint32_t* buf) |
81 | 0 | { |
82 | 0 | bool fill = p.NeedFill(); |
83 | 0 | int width = (int)p.dstW, s = (int)p.srcS, w = (int)p.srcW - 1, h = (int)p.srcH - 1; |
84 | 0 | const __m128 _4 = _mm_set1_ps(4.0f); |
85 | 0 | static const __m128i _0123 = SIMD_MM_SETR_EPI32(0, 1, 2, 3); |
86 | 0 | __m128 _m[6]; |
87 | 0 | for (int i = 0; i < 6; ++i) |
88 | 0 | _m[i] = _mm_set1_ps(p.inv[i]); |
89 | 0 | __m128i _w = _mm_set1_epi32(w); |
90 | 0 | __m128i _h = _mm_set1_epi32(h); |
91 | 0 | __m128i _n = _mm_set1_epi32(N); |
92 | 0 | __m128i _s = _mm_set1_epi32(s); |
93 | 0 | __m128i _border = InitBorder<N>(p.border); |
94 | 0 | dst += yBeg * p.dstS; |
95 | 0 | for (int y = yBeg; y < yEnd; ++y) |
96 | 0 | { |
97 | 0 | int nose = beg[y], tail = end[y]; |
98 | 0 | { |
99 | 0 | int x = nose; |
100 | 0 | __m128 _y = _mm_cvtepi32_ps(_mm_set1_epi32(y)); |
101 | 0 | __m128 _x = _mm_cvtepi32_ps(_mm_add_epi32(_mm_set1_epi32(x), _0123)); |
102 | 0 | for (; x < tail; x += 4) |
103 | 0 | { |
104 | 0 | _mm_storeu_si128((__m128i*)(buf + x), NearestOffset(_x, _y, _m, _w, _h, _n, _s)); |
105 | 0 | _x = _mm_add_ps(_x, _4); |
106 | 0 | } |
107 | 0 | } |
108 | 0 | if (fill) |
109 | 0 | FillBorder<N>(dst, nose, _border, p.border); |
110 | 0 | Base::NearestGather<N>(src, buf + nose, tail - nose, dst + N * nose); |
111 | 0 | if (fill) |
112 | 0 | FillBorder<N>(dst + tail * N, width - tail, _border, p.border); |
113 | 0 | dst += p.dstS; |
114 | 0 | } |
115 | 0 | } Unexecuted instantiation: void Simd::Sse41::NearestRun<1>(Simd::WarpAffParam const&, int, int, int const*, int const*, unsigned char const*, unsigned char*, unsigned int*) Unexecuted instantiation: void Simd::Sse41::NearestRun<2>(Simd::WarpAffParam const&, int, int, int const*, int const*, unsigned char const*, unsigned char*, unsigned int*) Unexecuted instantiation: void Simd::Sse41::NearestRun<3>(Simd::WarpAffParam const&, int, int, int const*, int const*, unsigned char const*, unsigned char*, unsigned int*) Unexecuted instantiation: void Simd::Sse41::NearestRun<4>(Simd::WarpAffParam const&, int, int, int const*, int const*, unsigned char const*, unsigned char*, unsigned int*) |
116 | | |
117 | | //------------------------------------------------------------------------------------------------- |
118 | | |
119 | | WarpAffineNearest::WarpAffineNearest(const WarpAffParam& param) |
120 | 0 | : Base::WarpAffineNearest(param) |
121 | 0 | { |
122 | 0 | switch (_param.channels) |
123 | 0 | { |
124 | 0 | case 1: _run = NearestRun<1>; break; |
125 | 0 | case 2: _run = NearestRun<2>; break; |
126 | 0 | case 3: _run = NearestRun<3>; break; |
127 | 0 | case 4: _run = NearestRun<4>; break; |
128 | 0 | } |
129 | 0 | } |
130 | | |
131 | | void WarpAffineNearest::SetRange(const Base::Point* points) |
132 | 0 | { |
133 | 0 | const WarpAffParam& p = _param; |
134 | 0 | int w = (int)p.dstW, h = (int)p.dstH, h4 = (int)AlignLo(h, 4); |
135 | 0 | static const __m128i _0123 = SIMD_MM_SETR_EPI32(0, 1, 2, 3); |
136 | 0 | __m128i _w = _mm_set1_epi32(w), _1 = _mm_set1_epi32(1); |
137 | 0 | int y = 0; |
138 | 0 | for (; y < h4; y += 4) |
139 | 0 | { |
140 | 0 | _mm_storeu_si128((__m128i*)(_beg.data + y), _w); |
141 | 0 | _mm_storeu_si128((__m128i*)(_end.data + y), _mm_setzero_si128()); |
142 | 0 | } |
143 | 0 | for (; y < h; ++y) |
144 | 0 | { |
145 | 0 | _beg[y] = w; |
146 | 0 | _end[y] = 0; |
147 | 0 | } |
148 | 0 | for (int v = 0; v < 4; ++v) |
149 | 0 | { |
150 | 0 | const Base::Point& curr = points[v]; |
151 | 0 | const Base::Point& next = points[(v + 1) & 3]; |
152 | 0 | float yMin = Simd::Max(Simd::Min(curr.y, next.y), 0.0f); |
153 | 0 | float yMax = Simd::Min(Simd::Max(curr.y, next.y), (float)p.dstH); |
154 | 0 | int yBeg = Round(yMin); |
155 | 0 | int yEnd = Round(yMax); |
156 | 0 | int yEnd4 = (int)AlignLo(yEnd - yBeg, 4) + yBeg; |
157 | 0 | if (next.y == curr.y) |
158 | 0 | continue; |
159 | 0 | float a = (next.x - curr.x) / (next.y - curr.y); |
160 | 0 | float b = curr.x - curr.y * a; |
161 | 0 | __m128 _a = _mm_set1_ps(a); |
162 | 0 | __m128 _b = _mm_set1_ps(b); |
163 | 0 | if (abs(a) <= 1.0f) |
164 | 0 | { |
165 | 0 | int y = yBeg; |
166 | 0 | for (; y < yEnd4; y += 4) |
167 | 0 | { |
168 | 0 | __m128 _y = _mm_cvtepi32_ps(_mm_add_epi32(_mm_set1_epi32(y), _0123)); |
169 | 0 | __m128i _x = _mm_cvtps_epi32(_mm_add_ps(_mm_mul_ps(_y, _a), _b)); |
170 | 0 | __m128i xBeg = _mm_loadu_si128((__m128i*)(_beg.data + y)); |
171 | 0 | __m128i xEnd = _mm_loadu_si128((__m128i*)(_end.data + y)); |
172 | 0 | xBeg = _mm_min_epi32(xBeg, _mm_max_epi32(_x, _mm_setzero_si128())); |
173 | 0 | xEnd = _mm_max_epi32(xEnd, _mm_min_epi32(_mm_add_epi32(_x, _1), _w)); |
174 | 0 | _mm_storeu_si128((__m128i*)(_beg.data + y), xBeg); |
175 | 0 | _mm_storeu_si128((__m128i*)(_end.data + y), xEnd); |
176 | 0 | } |
177 | 0 | for (; y < yEnd; ++y) |
178 | 0 | { |
179 | 0 | int x = Round(y * a + b); |
180 | 0 | _beg[y] = Simd::Min(_beg[y], Simd::Max(x, 0)); |
181 | 0 | _end[y] = Simd::Max(_end[y], Simd::Min(x + 1, w)); |
182 | 0 | } |
183 | 0 | } |
184 | 0 | else |
185 | 0 | { |
186 | 0 | int y = yBeg; |
187 | 0 | __m128 _05 = _mm_set1_ps(0.5f); |
188 | 0 | __m128 _yMin = _mm_set1_ps(yMin); |
189 | 0 | __m128 _yMax = _mm_set1_ps(yMax); |
190 | 0 | for (; y < yEnd4; y += 4) |
191 | 0 | { |
192 | 0 | __m128 _y = _mm_cvtepi32_ps(_mm_add_epi32(_mm_set1_epi32(y), _0123)); |
193 | 0 | __m128 yM = _mm_min_ps(_mm_max_ps(_mm_sub_ps(_y, _05), _yMin), _yMax); |
194 | 0 | __m128 yP = _mm_min_ps(_mm_max_ps(_mm_add_ps(_y, _05), _yMin), _yMax); |
195 | 0 | __m128 xM = _mm_add_ps(_mm_mul_ps(yM, _a), _b); |
196 | 0 | __m128 xP = _mm_add_ps(_mm_mul_ps(yP, _a), _b); |
197 | 0 | __m128i xBeg = _mm_loadu_si128((__m128i*)(_beg.data + y)); |
198 | 0 | __m128i xEnd = _mm_loadu_si128((__m128i*)(_end.data + y)); |
199 | 0 | xBeg = _mm_min_epi32(xBeg, _mm_max_epi32(_mm_cvtps_epi32(_mm_min_ps(xM, xP)), _mm_setzero_si128())); |
200 | 0 | xEnd = _mm_max_epi32(xEnd, _mm_min_epi32(_mm_add_epi32(_mm_cvtps_epi32(_mm_max_ps(xM, xP)), _1), _w)); |
201 | 0 | _mm_storeu_si128((__m128i*)(_beg.data + y), xBeg); |
202 | 0 | _mm_storeu_si128((__m128i*)(_end.data + y), xEnd); |
203 | 0 | } |
204 | 0 | for (; y < yEnd; ++y) |
205 | 0 | { |
206 | 0 | float xM = b + Simd::RestrictRange(float(y) - 0.5f, yMin, yMax) * a; |
207 | 0 | float xP = b + Simd::RestrictRange(float(y) + 0.5f, yMin, yMax) * a; |
208 | 0 | int xBeg = Round(Simd::Min(xM, xP)); |
209 | 0 | int xEnd = Round(Simd::Max(xM, xP)); |
210 | 0 | _beg[y] = Simd::Min(_beg[y], Simd::Max(xBeg, 0)); |
211 | 0 | _end[y] = Simd::Max(_end[y], Simd::Min(xEnd + 1, w)); |
212 | 0 | } |
213 | 0 | } |
214 | 0 | } |
215 | 0 | } |
216 | | |
217 | | //------------------------------------------------------------------------------------------------- |
218 | | |
219 | | const __m128i K32_WA_FRACTION_RANGE = SIMD_MM_SET1_EPI32(Base::WA_FRACTION_RANGE); |
220 | | |
221 | | SIMD_INLINE void ByteBilinearPrepMain4(__m128 x, __m128 y, const __m128* m, __m128i n, const __m128i & s, uint32_t* offs, uint8_t* fx, uint16_t* fy) |
222 | 0 | { |
223 | 0 | __m128 dx = _mm_add_ps(_mm_add_ps(_mm_mul_ps(x, m[0]), _mm_mul_ps(y, m[1])), m[2]); |
224 | 0 | __m128 dy = _mm_add_ps(_mm_add_ps(_mm_mul_ps(x, m[3]), _mm_mul_ps(y, m[4])), m[5]); |
225 | 0 | __m128 ix = _mm_floor_ps(dx); |
226 | 0 | __m128 iy = _mm_floor_ps(dy); |
227 | 0 | __m128 range = _mm_cvtepi32_ps(K32_WA_FRACTION_RANGE); |
228 | 0 | __m128i _fx = _mm_cvtps_epi32(_mm_mul_ps(_mm_sub_ps(dx, ix), range)); |
229 | 0 | __m128i _fy = _mm_cvtps_epi32(_mm_mul_ps(_mm_sub_ps(dy, iy), range)); |
230 | 0 | _mm_storeu_si128((__m128i*)offs, _mm_add_epi32(_mm_mullo_epi32(_mm_cvtps_epi32(ix), n), _mm_mullo_epi32(_mm_cvtps_epi32(iy), s))); |
231 | 0 | _fx = _mm_or_si128(_mm_sub_epi32(K32_WA_FRACTION_RANGE, _fx), _mm_slli_epi32(_fx, 16)); |
232 | 0 | _fy = _mm_or_si128(_mm_sub_epi32(K32_WA_FRACTION_RANGE, _fy), _mm_slli_epi32(_fy, 16)); |
233 | 0 | _mm_storel_epi64((__m128i*)fx, _mm_packus_epi16(_fx, _mm_setzero_si128())); |
234 | 0 | _mm_storeu_si128((__m128i*)fy, _fy); |
235 | 0 | } |
236 | | |
237 | | //------------------------------------------------------------------------------------------------- |
238 | | |
239 | | const __m128i K32_WA_BILINEAR_ROUND_TERM = SIMD_MM_SET1_EPI32(Base::WA_BILINEAR_ROUND_TERM); |
240 | | |
241 | | template<int N> void ByteBilinearInterpMainN(const uint8_t* src0, const uint8_t* src1, const uint8_t* fx, const uint16_t* fy, uint8_t* dst); |
242 | | |
243 | | template<> SIMD_INLINE void ByteBilinearInterpMainN<1>(const uint8_t* src0, const uint8_t* src1, const uint8_t* fx, const uint16_t* fy, uint8_t* dst) |
244 | 0 | { |
245 | 0 | __m128i fx0 = _mm_loadu_si128((__m128i*)fx + 0); |
246 | 0 | __m128i fx1 = _mm_loadu_si128((__m128i*)fx + 1); |
247 | 0 | __m128i r00 = _mm_maddubs_epi16(_mm_loadu_si128((__m128i*)src0 + 0), fx0); |
248 | 0 | __m128i r01 = _mm_maddubs_epi16(_mm_loadu_si128((__m128i*)src0 + 1), fx1); |
249 | 0 | __m128i r10 = _mm_maddubs_epi16(_mm_loadu_si128((__m128i*)src1 + 0), fx0); |
250 | 0 | __m128i r11 = _mm_maddubs_epi16(_mm_loadu_si128((__m128i*)src1 + 1), fx1); |
251 | |
|
252 | 0 | __m128i s0 = _mm_madd_epi16(UnpackU16<0>(r00, r10), _mm_loadu_si128((__m128i*)fy + 0)); |
253 | 0 | __m128i d0 = _mm_srli_epi32(_mm_add_epi32(s0, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
254 | |
|
255 | 0 | __m128i s1 = _mm_madd_epi16(UnpackU16<1>(r00, r10), _mm_loadu_si128((__m128i*)fy + 1)); |
256 | 0 | __m128i d1 = _mm_srli_epi32(_mm_add_epi32(s1, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
257 | |
|
258 | 0 | __m128i s2 = _mm_madd_epi16(UnpackU16<0>(r01, r11), _mm_loadu_si128((__m128i*)fy + 2)); |
259 | 0 | __m128i d2 = _mm_srli_epi32(_mm_add_epi32(s2, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
260 | |
|
261 | 0 | __m128i s3 = _mm_madd_epi16(UnpackU16<1>(r01, r11), _mm_loadu_si128((__m128i*)fy + 3)); |
262 | 0 | __m128i d3 = _mm_srli_epi32(_mm_add_epi32(s3, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
263 | |
|
264 | 0 | _mm_storeu_si128((__m128i*)dst, _mm_packus_epi16(_mm_packus_epi32(d0, d1), _mm_packus_epi32(d2, d3))); |
265 | 0 | } |
266 | | |
267 | | template<> SIMD_INLINE void ByteBilinearInterpMainN<2>(const uint8_t* src0, const uint8_t* src1, const uint8_t* fx, const uint16_t* fy, uint8_t* dst) |
268 | 0 | { |
269 | 0 | static const __m128i SHUFFLE = SIMD_MM_SETR_EPI8(0x0, 0x2, 0x1, 0x3, 0x4, 0x6, 0x5, 0x7, 0x8, 0xA, 0x9, 0xB, 0xC, 0xE, 0xD, 0xF); |
270 | |
|
271 | 0 | __m128i _fx = _mm_loadu_si128((__m128i*)fx); |
272 | 0 | __m128i fx0 = UnpackU16<0>(_fx, _fx); |
273 | 0 | __m128i fx1 = UnpackU16<1>(_fx, _fx); |
274 | 0 | __m128i r00 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 0), SHUFFLE), fx0); |
275 | 0 | __m128i r01 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 1), SHUFFLE), fx1); |
276 | 0 | __m128i r10 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 0), SHUFFLE), fx0); |
277 | 0 | __m128i r11 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 1), SHUFFLE), fx1); |
278 | |
|
279 | 0 | __m128i fy0 = _mm_loadu_si128((__m128i*)fy + 0); |
280 | 0 | __m128i s0 = _mm_madd_epi16(UnpackU16<0>(r00, r10), UnpackU32<0>(fy0, fy0)); |
281 | 0 | __m128i d0 = _mm_srli_epi32(_mm_add_epi32(s0, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
282 | |
|
283 | 0 | __m128i s1 = _mm_madd_epi16(UnpackU16<1>(r00, r10), UnpackU32<1>(fy0, fy0)); |
284 | 0 | __m128i d1 = _mm_srli_epi32(_mm_add_epi32(s1, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
285 | |
|
286 | 0 | __m128i fy1 = _mm_loadu_si128((__m128i*)fy + 1); |
287 | 0 | __m128i s2 = _mm_madd_epi16(UnpackU16<0>(r01, r11), UnpackU32<0>(fy1, fy1)); |
288 | 0 | __m128i d2 = _mm_srli_epi32(_mm_add_epi32(s2, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
289 | |
|
290 | 0 | __m128i s3 = _mm_madd_epi16(UnpackU16<1>(r01, r11), UnpackU32<1>(fy1, fy1)); |
291 | 0 | __m128i d3 = _mm_srli_epi32(_mm_add_epi32(s3, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
292 | |
|
293 | 0 | _mm_storeu_si128((__m128i*)dst, _mm_packus_epi16(_mm_packus_epi32(d0, d1), _mm_packus_epi32(d2, d3))); |
294 | 0 | } |
295 | | |
296 | | template<> SIMD_INLINE void ByteBilinearInterpMainN<3>(const uint8_t* src0, const uint8_t* src1, const uint8_t* fx, const uint16_t* fy, uint8_t* dst) |
297 | 0 | { |
298 | 0 | static const __m128i SRC_SHUFFLE = SIMD_MM_SETR_EPI8(0x0, 0x3, 0x1, 0x4, 0x2, 0x5, -1, -1, 0x8, 0xB, 0x9, 0xC, 0xA, 0xD, -1, -1); |
299 | 0 | static const __m128i DST_SHUFFLE = SIMD_MM_SETR_EPI8(0x0, 0x1, 0x2, 0x4, 0x5, 0x6, 0x8, 0x9, 0xA, 0xC, 0xD, 0xE, -1, -1, -1, -1); |
300 | |
|
301 | 0 | __m128i _fx = _mm_loadu_si128((__m128i*)fx); |
302 | 0 | _fx = UnpackU16<0>(_fx, _fx); |
303 | 0 | __m128i fx0 = UnpackU16<0>(_fx, _fx); |
304 | 0 | __m128i fx1 = UnpackU16<1>(_fx, _fx); |
305 | 0 | __m128i r00 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 0), SRC_SHUFFLE), fx0); |
306 | 0 | __m128i r01 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 1), SRC_SHUFFLE), fx1); |
307 | 0 | __m128i r10 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 0), SRC_SHUFFLE), fx0); |
308 | 0 | __m128i r11 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 1), SRC_SHUFFLE), fx1); |
309 | |
|
310 | 0 | __m128i _fy = _mm_loadu_si128((__m128i*)fy); |
311 | 0 | __m128i fy0 = UnpackU32<0>(_fy, _fy); |
312 | 0 | __m128i s0 = _mm_madd_epi16(UnpackU16<0>(r00, r10), UnpackU32<0>(fy0, fy0)); |
313 | 0 | __m128i d0 = _mm_srli_epi32(_mm_add_epi32(s0, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
314 | |
|
315 | 0 | __m128i s1 = _mm_madd_epi16(UnpackU16<1>(r00, r10), UnpackU32<1>(fy0, fy0)); |
316 | 0 | __m128i d1 = _mm_srli_epi32(_mm_add_epi32(s1, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
317 | |
|
318 | 0 | __m128i fy1 = UnpackU32<1>(_fy, _fy); |
319 | 0 | __m128i s2 = _mm_madd_epi16(UnpackU16<0>(r01, r11), UnpackU32<0>(fy1, fy1)); |
320 | 0 | __m128i d2 = _mm_srli_epi32(_mm_add_epi32(s2, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
321 | |
|
322 | 0 | __m128i s3 = _mm_madd_epi16(UnpackU16<1>(r01, r11), UnpackU32<1>(fy1, fy1)); |
323 | 0 | __m128i d3 = _mm_srli_epi32(_mm_add_epi32(s3, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
324 | |
|
325 | 0 | Store12(dst, _mm_shuffle_epi8(_mm_packus_epi16(_mm_packus_epi32(d0, d1), _mm_packus_epi32(d2, d3)), DST_SHUFFLE)); |
326 | 0 | } |
327 | | |
328 | | template<> SIMD_INLINE void ByteBilinearInterpMainN<4>(const uint8_t* src0, const uint8_t* src1, const uint8_t* fx, const uint16_t* fy, uint8_t* dst) |
329 | 0 | { |
330 | 0 | static const __m128i SHUFFLE = SIMD_MM_SETR_EPI8(0x0, 0x4, 0x1, 0x5, 0x2, 0x6, 0x3, 0x7, 0x8, 0xC, 0x9, 0xD, 0xA, 0xE, 0xB, 0xF); |
331 | |
|
332 | 0 | __m128i _fx = _mm_loadu_si128((__m128i*)fx); |
333 | 0 | _fx = UnpackU16<0>(_fx, _fx); |
334 | 0 | __m128i fx0 = UnpackU16<0>(_fx, _fx); |
335 | 0 | __m128i fx1 = UnpackU16<1>(_fx, _fx); |
336 | 0 | __m128i r00 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 0), SHUFFLE), fx0); |
337 | 0 | __m128i r01 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src0 + 1), SHUFFLE), fx1); |
338 | 0 | __m128i r10 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 0), SHUFFLE), fx0); |
339 | 0 | __m128i r11 = _mm_maddubs_epi16(_mm_shuffle_epi8(_mm_loadu_si128((__m128i*)src1 + 1), SHUFFLE), fx1); |
340 | |
|
341 | 0 | __m128i _fy = _mm_loadu_si128((__m128i*)fy); |
342 | 0 | __m128i fy0 = UnpackU32<0>(_fy, _fy); |
343 | 0 | __m128i s0 = _mm_madd_epi16(UnpackU16<0>(r00, r10), UnpackU32<0>(fy0, fy0)); |
344 | 0 | __m128i d0 = _mm_srli_epi32(_mm_add_epi32(s0, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
345 | |
|
346 | 0 | __m128i s1 = _mm_madd_epi16(UnpackU16<1>(r00, r10), UnpackU32<1>(fy0, fy0)); |
347 | 0 | __m128i d1 = _mm_srli_epi32(_mm_add_epi32(s1, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
348 | |
|
349 | 0 | __m128i fy1 = UnpackU32<1>(_fy, _fy); |
350 | 0 | __m128i s2 = _mm_madd_epi16(UnpackU16<0>(r01, r11), UnpackU32<0>(fy1, fy1)); |
351 | 0 | __m128i d2 = _mm_srli_epi32(_mm_add_epi32(s2, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
352 | |
|
353 | 0 | __m128i s3 = _mm_madd_epi16(UnpackU16<1>(r01, r11), UnpackU32<1>(fy1, fy1)); |
354 | 0 | __m128i d3 = _mm_srli_epi32(_mm_add_epi32(s3, K32_WA_BILINEAR_ROUND_TERM), Base::WA_BILINEAR_SHIFT); |
355 | |
|
356 | 0 | _mm_storeu_si128((__m128i*)dst, _mm_packus_epi16(_mm_packus_epi32(d0, d1), _mm_packus_epi32(d2, d3))); |
357 | 0 | } |
358 | | |
359 | | //------------------------------------------------------------------------------------------------- |
360 | | |
361 | | template<int N> void ByteBilinearRun(const WarpAffParam& p, int yBeg, int yEnd, const int* ib, const int* ie, const int* ob, const int* oe, const uint8_t* src, uint8_t* dst, uint8_t* buf) |
362 | 0 | { |
363 | 0 | constexpr int M = (N == 3 ? 4 : N); |
364 | 0 | bool fill = p.NeedFill(); |
365 | 0 | int width = (int)p.dstW, s = (int)p.srcS, w = (int)p.srcW - 2, h = (int)p.srcH - 2, n = A / M; |
366 | 0 | size_t wa = AlignHi(p.dstW, p.align) + p.align; |
367 | 0 | uint32_t* offs = (uint32_t*)buf; |
368 | 0 | uint8_t* fx = (uint8_t *)(offs + wa); |
369 | 0 | uint16_t* fy = (uint16_t*)(fx + wa * 2); |
370 | 0 | uint8_t* rb0 = (uint8_t*)(fy + wa * 2); |
371 | 0 | uint8_t* rb1 = (uint8_t*)(rb0 + wa * M * 2); |
372 | 0 | const __m128 _4 = _mm_set1_ps(4.0f); |
373 | 0 | static const __m128i _0123 = SIMD_MM_SETR_EPI32(0, 1, 2, 3); |
374 | 0 | __m128 _m[6]; |
375 | 0 | for (int i = 0; i < 6; ++i) |
376 | 0 | _m[i] = _mm_set1_ps(p.inv[i]); |
377 | 0 | __m128i _n = _mm_set1_epi32(N); |
378 | 0 | __m128i _s = _mm_set1_epi32(s); |
379 | 0 | __m128i _border = InitBorder<N>(p.border); |
380 | 0 | dst += yBeg * p.dstS; |
381 | 0 | for (int y = yBeg; y < yEnd; ++y) |
382 | 0 | { |
383 | 0 | int iB = ib[y], iE = ie[y], oB = ob[y], oE = oe[y]; |
384 | 0 | if (fill) |
385 | 0 | { |
386 | 0 | FillBorder<N>(dst, oB, _border, p.border); |
387 | 0 | for (int x = oB; x < iB; ++x) |
388 | 0 | Base::ByteBilinearInterpEdge<N>(x, y, p.inv, w, h, s, src, p.border, dst + x * N); |
389 | 0 | } |
390 | 0 | else |
391 | 0 | { |
392 | 0 | for (int x = oB; x < iB; ++x) |
393 | 0 | Base::ByteBilinearInterpEdge<N>(x, y, p.inv, w, h, s, src, dst + x * N, dst + x * N); |
394 | 0 | } |
395 | 0 | { |
396 | 0 | int x = iB, iEn = (int)AlignLo(iE - iB, n) + iB; |
397 | 0 | __m128 _y = _mm_cvtepi32_ps(_mm_set1_epi32(y)); |
398 | 0 | __m128 _x = _mm_cvtepi32_ps(_mm_add_epi32(_mm_set1_epi32(x), _0123)); |
399 | 0 | for (; x < iE; x += 4) |
400 | 0 | { |
401 | 0 | ByteBilinearPrepMain4(_x, _y, _m, _n, _s, offs + x, fx + 2 * x, fy + 2 * x); |
402 | 0 | _x = _mm_add_ps(_x, _4); |
403 | 0 | } |
404 | 0 | Base::ByteBilinearGather<M>(src, src + s, offs + iB, iE - iB, rb0 + 2 * M * iB, rb1 + 2 * M * iB); |
405 | 0 | for (x = iB; x < iEn; x += n) |
406 | 0 | ByteBilinearInterpMainN<N>(rb0 + x * M * 2, rb1 + x * M * 2, fx + 2 * x, fy + 2 * x, dst + x * N); |
407 | 0 | for (; x < iE; ++x) |
408 | 0 | Base::ByteBilinearInterpMain<N>(rb0 + x * M * 2, rb1 + x * M * 2, fx + 2 * x, fy + 2 * x, dst + x * N); |
409 | 0 | } |
410 | 0 | if (fill) |
411 | 0 | { |
412 | 0 | for (int x = iE; x < oE; ++x) |
413 | 0 | Base::ByteBilinearInterpEdge<N>(x, y, p.inv, w, h, s, src, p.border, dst + x * N); |
414 | 0 | FillBorder<N>(dst + oE * N, width - oE, _border, p.border); |
415 | 0 | } |
416 | 0 | else |
417 | 0 | { |
418 | 0 | for (int x = iE; x < oE; ++x) |
419 | 0 | Base::ByteBilinearInterpEdge<N>(x, y, p.inv, w, h, s, src, dst + x * N, dst + x * N); |
420 | 0 | } |
421 | 0 | dst += p.dstS; |
422 | 0 | } |
423 | 0 | } Unexecuted instantiation: void Simd::Sse41::ByteBilinearRun<1>(Simd::WarpAffParam const&, int, int, int const*, int const*, int const*, int const*, unsigned char const*, unsigned char*, unsigned char*) Unexecuted instantiation: void Simd::Sse41::ByteBilinearRun<2>(Simd::WarpAffParam const&, int, int, int const*, int const*, int const*, int const*, unsigned char const*, unsigned char*, unsigned char*) Unexecuted instantiation: void Simd::Sse41::ByteBilinearRun<3>(Simd::WarpAffParam const&, int, int, int const*, int const*, int const*, int const*, unsigned char const*, unsigned char*, unsigned char*) Unexecuted instantiation: void Simd::Sse41::ByteBilinearRun<4>(Simd::WarpAffParam const&, int, int, int const*, int const*, int const*, int const*, unsigned char const*, unsigned char*, unsigned char*) |
424 | | |
425 | | //------------------------------------------------------------------------------------------------- |
426 | | |
427 | | WarpAffineByteBilinear::WarpAffineByteBilinear(const WarpAffParam& param) |
428 | 0 | : Base::WarpAffineByteBilinear(param) |
429 | 0 | { |
430 | 0 | switch (_param.channels) |
431 | 0 | { |
432 | 0 | case 1: _run = ByteBilinearRun<1>; break; |
433 | 0 | case 2: _run = ByteBilinearRun<2>; break; |
434 | 0 | case 3: _run = ByteBilinearRun<3>; break; |
435 | 0 | case 4: _run = ByteBilinearRun<4>; break; |
436 | 0 | } |
437 | 0 | } |
438 | | |
439 | | void WarpAffineByteBilinear::SetRange(const Base::Point* rect, int* beg, int* end, const int* lo, const int* hi) |
440 | 0 | { |
441 | 0 | const WarpAffParam& p = _param; |
442 | 0 | float* min = (float*)_buf.data; |
443 | 0 | float* max = min + p.dstH; |
444 | 0 | float w = (float)p.dstW, h = (float)p.dstH, z = 0.0f; |
445 | 0 | static const __m128i _0123 = SIMD_MM_SETR_EPI32(0, 1, 2, 3); |
446 | 0 | __m128 _w = _mm_set1_ps(w), _z = _mm_set1_ps(z); |
447 | 0 | int y = 0, dH = (int)p.dstH, dH4 = (int)AlignLo(dH, 4); |
448 | 0 | for (; y < dH4; y += 4) |
449 | 0 | { |
450 | 0 | _mm_storeu_ps(min + y, _w); |
451 | 0 | _mm_storeu_ps(max + y, _mm_setzero_ps()); |
452 | 0 | } |
453 | 0 | for (; y < dH; ++y) |
454 | 0 | { |
455 | 0 | min[y] = w; |
456 | 0 | max[y] = 0; |
457 | 0 | } |
458 | 0 | for (int v = 0; v < 4; ++v) |
459 | 0 | { |
460 | 0 | const Base::Point& curr = rect[v]; |
461 | 0 | const Base::Point& next = rect[(v + 1) & 3]; |
462 | 0 | if (next.y == curr.y) |
463 | 0 | continue; |
464 | 0 | float yMin = Simd::Max(Simd::Min(curr.y, next.y), z); |
465 | 0 | float yMax = Simd::Min(Simd::Max(curr.y, next.y), h); |
466 | 0 | int yBeg = (int)ceil(yMin); |
467 | 0 | int yEnd = (int)ceil(yMax); |
468 | 0 | int yEnd4 = (int)AlignLo(yEnd - yBeg, 4) + yBeg; |
469 | 0 | float a = (next.x - curr.x) / (next.y - curr.y); |
470 | 0 | float b = curr.x - curr.y * a; |
471 | 0 | __m128 _a = _mm_set1_ps(a); |
472 | 0 | __m128 _b = _mm_set1_ps(b); |
473 | 0 | __m128 _yMin = _mm_set1_ps(yMin); |
474 | 0 | __m128 _yMax = _mm_set1_ps(yMax); |
475 | 0 | for (y = yBeg; y < yEnd4; y += 4) |
476 | 0 | { |
477 | 0 | __m128 _y = _mm_cvtepi32_ps(_mm_add_epi32(_mm_set1_epi32(y), _0123)); |
478 | 0 | _y = _mm_min_ps(_yMax, _mm_max_ps(_y, _yMin)); |
479 | 0 | __m128 _x = _mm_add_ps(_mm_mul_ps(_y, _a), _b); |
480 | 0 | _mm_storeu_ps(min + y, _mm_min_ps(_mm_loadu_ps(min + y), _mm_max_ps(_x, _z))); |
481 | 0 | _mm_storeu_ps(max + y, _mm_max_ps(_mm_loadu_ps(max + y), _mm_min_ps(_x, _w))); |
482 | 0 | } |
483 | 0 | for (; y < yEnd; ++y) |
484 | 0 | { |
485 | 0 | float x = Simd::RestrictRange(float(y), yMin, yMax) * a + b; |
486 | 0 | min[y] = Simd::Min(min[y], Simd::Max(x, z)); |
487 | 0 | max[y] = Simd::Max(max[y], Simd::Min(x, w)); |
488 | 0 | } |
489 | 0 | } |
490 | 0 | for (y = 0; y < dH4; y += 4) |
491 | 0 | { |
492 | 0 | __m128i _beg = _mm_cvtps_epi32(_mm_ceil_ps(_mm_loadu_ps(min + y))); |
493 | 0 | __m128i _end = _mm_cvtps_epi32(_mm_ceil_ps(_mm_loadu_ps(max + y))); |
494 | 0 | _mm_storeu_si128((__m128i*)(beg + y), _beg); |
495 | 0 | _mm_storeu_si128((__m128i*)(end + y), _mm_max_epi32(_beg, _end)); |
496 | 0 | } |
497 | 0 | for (; y < dH; ++y) |
498 | 0 | { |
499 | 0 | beg[y] = (int)ceil(min[y]); |
500 | 0 | end[y] = (int)ceil(max[y]); |
501 | 0 | end[y] = Simd::Max(beg[y], end[y]); |
502 | 0 | } |
503 | 0 | if (hi) |
504 | 0 | { |
505 | 0 | for (y = 0; y < dH4; y += 4) |
506 | 0 | { |
507 | 0 | __m128i _hi = _mm_loadu_si128((__m128i*)(hi + y)); |
508 | 0 | _mm_storeu_si128((__m128i*)(beg + y), _mm_min_epi32(_mm_loadu_si128((__m128i*)(beg + y)), _hi)); |
509 | 0 | _mm_storeu_si128((__m128i*)(end + y), _mm_min_epi32(_mm_loadu_si128((__m128i*)(end + y)), _hi)); |
510 | 0 | } |
511 | 0 | for (; y < dH; ++y) |
512 | 0 | { |
513 | 0 | beg[y] = Simd::Min(beg[y], hi[y]); |
514 | 0 | end[y] = Simd::Min(end[y], hi[y]); |
515 | 0 | } |
516 | 0 | } |
517 | 0 | } |
518 | | |
519 | | //------------------------------------------------------------------------------------------------- |
520 | | |
521 | | void* WarpAffineInit(size_t srcW, size_t srcH, size_t srcS, size_t dstW, size_t dstH, size_t dstS, size_t channels, const float* mat, SimdWarpAffineFlags flags, const uint8_t* border) |
522 | 0 | { |
523 | 0 | WarpAffParam param(srcW, srcH, srcS, dstW, dstH, dstS, channels, mat, flags, border, A); |
524 | 0 | if (!param.Valid()) |
525 | 0 | return NULL; |
526 | 0 | if (param.IsNearest()) |
527 | 0 | return new WarpAffineNearest(param); |
528 | 0 | else if (param.IsByteBilinear()) |
529 | 0 | return new WarpAffineByteBilinear(param); |
530 | 0 | else |
531 | 0 | return NULL; |
532 | 0 | } |
533 | | } |
534 | | #endif |
535 | | } |