File size: 7,996 Bytes
dffe123
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "ndsrvp_hal.hpp"
#include "opencv2/imgproc/hal/interface.h"
#include "cvutils.hpp"

namespace cv {

namespace ndsrvp {

int remap32f(int src_type, const uchar* src_data, size_t src_step, int src_width, int src_height,
    uchar* dst_data, size_t dst_step, int dst_width, int dst_height, float* mapx, size_t mapx_step,
    float* mapy, size_t mapy_step, int interpolation, int border_type, const double border_value[4])
{
    const bool isRelative = ((interpolation & CV_HAL_WARP_RELATIVE_MAP) != 0);
    interpolation &= ~CV_HAL_WARP_RELATIVE_MAP;

    if( interpolation == CV_HAL_INTER_AREA )
        interpolation = CV_HAL_INTER_LINEAR;

    if( interpolation != CV_HAL_INTER_NEAREST )
        return CV_HAL_ERROR_NOT_IMPLEMENTED;

    // only CV_8U
    if( (src_type & CV_MAT_DEPTH_MASK) != CV_8U )
        return CV_HAL_ERROR_NOT_IMPLEMENTED;

    int cn = CV_MAT_CN(src_type);

    src_step /= sizeof(uchar);
    dst_step /= sizeof(uchar);

    // mapping CV_32FC1
    mapx_step /= sizeof(float);
    mapy_step /= sizeof(float);

    // border
    uchar border_const[CV_CN_MAX];
    for( int k = 0; k < CV_CN_MAX; k++ )
        border_const[k] = saturate_cast<uchar>(border_value[k & 3]);

    // divide into blocks
    const int BLOCK_SIZE = 1024;
    int x, y, x1, y1;
    std::array<short, BLOCK_SIZE * BLOCK_SIZE * 2> aXY;
    short* XY = aXY.data();
    size_t XY_step = BLOCK_SIZE * 2;

    // vectorize
    const int32x2_t src_wh = {src_width, src_height};
    const int32x2_t arr_index = {cn, (int)src_step};

    for (y = 0; y < dst_height; y += BLOCK_SIZE)
    {
        int dy = std::min(BLOCK_SIZE, dst_height - y);
        for (x = 0; x < dst_width; x += BLOCK_SIZE)
        {
            const int off_y = isRelative ? y : 0;
            const int off_x = isRelative ? x : 0;
            const int32x2_t voff = {off_x, off_y};

            int dx = std::min(BLOCK_SIZE, dst_width - x);
            // prepare mapping data XY
            for (y1 = 0; y1 < dy; y1++)
            {
                short* rXY = XY + y1 * XY_step;
                const float* sX = mapx + (y + y1) * mapx_step + x;
                const float* sY = mapy + (y + y1) * mapy_step + x;
                for (x1 = 0; x1 < dx; x1++)
                {
                    rXY[x1 * 2] = saturate_cast<short>(sX[x1]);
                    rXY[x1 * 2 + 1] = saturate_cast<short>(sY[x1]);
                }
            }

            // precalulate offset
            if(isRelative)
            {
                int16x8_t voff_x;
                int16x8_t voff_y = {0, 0, 1, 0, 2, 0, 3, 0};
                int16x8_t vones_x = {4, 0, 4, 0, 4, 0, 4, 0};
                int16x8_t vones_y = {0, 1, 0, 1, 0, 1, 0, 1};
                for(y1 = 0; y1 < BLOCK_SIZE; y1++, voff_y += vones_y)
                {
                    int16x8_t* vrXY = (int16x8_t*)(XY + y1 * XY_step);
                    for(x1 = 0, voff_x = voff_y; x1 < BLOCK_SIZE; x1 += 4, vrXY++, voff_x += vones_x)
                    {
                        *vrXY += voff_x;
                    }
                }
            }

            // process the block
            for( y1 = 0; y1 < dy; y1++ )
            {
                uchar* dst_row = dst_data + (y + y1) * dst_step + x * cn;
                const short* rXY = XY + y1 * XY_step;
                if( cn == 1 )
                {
                    for( x1 = 0; x1 < dx; x1++ )
                    {
                        int32x2_t vsxy = (int32x2_t){rXY[x1 * 2], rXY[x1 * 2 + 1]} + voff;
                        if( (long)((uint32x2_t)vsxy < (uint32x2_t)src_wh) == -1 )
                            dst_row[x1] = src_data[__nds__v_smar64(0, vsxy, arr_index)];
                        else
                        {
                            if( border_type == CV_HAL_BORDER_REPLICATE )
                            {
                                vsxy = vclip(vsxy, (int32x2_t){0, 0}, src_wh);
                                dst_row[x1] = src_data[__nds__v_smar64(0, vsxy, arr_index)];
                            }
                            else if( border_type == CV_HAL_BORDER_CONSTANT )
                                dst_row[x1] = border_const[0];
                            else if( border_type != CV_HAL_BORDER_TRANSPARENT )
                            {
                                vsxy[0] = borderInterpolate(vsxy[0], src_width, border_type);
                                vsxy[1] = borderInterpolate(vsxy[1], src_height, border_type);
                                dst_row[x1] = src_data[__nds__v_smar64(0, vsxy, arr_index)];
                            }
                        }
                    }
                }
                else
                {
                    uchar* dst_ptr = dst_row;
                    for(x1 = 0; x1 < dx; x1++, dst_ptr += cn )
                    {
                        int32x2_t vsxy = (int32x2_t){rXY[x1 * 2], rXY[x1 * 2 + 1]} + voff;
                        const uchar *src_ptr;
                        if( (long)((uint32x2_t)vsxy < (uint32x2_t)src_wh) == -1 )
                        {
                            if( cn == 3 )
                            {
                                src_ptr = (uchar*)__nds__v_smar64((long)src_data, vsxy, arr_index);
                                dst_ptr[0] = src_ptr[0]; dst_ptr[1] = src_ptr[1]; dst_ptr[2] = src_ptr[2];
                                // performance loss, commented out
                                // *(unsigned*)dst_ptr = __nds__bpick(*(unsigned*)dst_ptr, *(unsigned*)src_ptr, 0xFF000000);
                            }
                            else if( cn == 4 )
                            {
                                src_ptr = (uchar*)__nds__v_smar64((long)src_data, vsxy, arr_index);
                                *(uint8x4_t*)dst_ptr = *(uint8x4_t*)src_ptr;
                            }
                            else
                            {
                                src_ptr = (uchar*)__nds__v_smar64((long)src_data, vsxy, arr_index);
                                int k = cn;
                                for(; k >= 8; k -= 8, dst_ptr += 8, src_ptr += 8)
                                    *(uint8x8_t*)dst_ptr = *(uint8x8_t*)src_ptr;
                                while( k-- )
                                    dst_ptr[k] = src_ptr[k];
                            }
                        }
                        else if( border_type != CV_HAL_BORDER_TRANSPARENT )
                        {
                            if( border_type == CV_HAL_BORDER_REPLICATE )
                            {
                                vsxy = vclip(vsxy, (int32x2_t){0, 0}, src_wh);
                                src_ptr = (uchar*)__nds__v_smar64((long)src_data, vsxy, arr_index);
                            }
                            else if( border_type == CV_HAL_BORDER_CONSTANT )
                                src_ptr = &border_const[0];
                            else
                            {
                                vsxy[0] = borderInterpolate(vsxy[0], src_width, border_type);
                                vsxy[1] = borderInterpolate(vsxy[1], src_height, border_type);
                                src_ptr = (uchar*)__nds__v_smar64((long)src_data, vsxy, arr_index);
                            }
                            int k = cn;
                            for(; k >= 8; k -= 8, dst_ptr += 8, src_ptr += 8)
                                *(uint8x8_t*)dst_ptr = *(uint8x8_t*)src_ptr;
                            while( k-- )
                                dst_ptr[k] = src_ptr[k];
                        }
                    }
                }
            }
        }
    }

    return CV_HAL_ERROR_OK;
}

} // namespace ndsrvp

} // namespace cv