void antigravity(uint8_t interval = 200 /* ms */) {
// move all of the LED's upwards (we can move them in other directions in the future)
for (uint8_t iteration = 0; iteration < 5; iteration++) {
mp_hal_delay_ms(interval);
bool wait = false;
for (uint8_t row = 1; row < 5 - iteration; row++) {
for (uint8_t col = 0; col < 5; col++) {
// move this bit down if possible
uint8_t val = GET_PIXEL(col, row);
if (val) {
// this is why the row for loop starts at one
if (!GET_PIXEL(col, row - 1)) {
SET_PIXEL(col, row, 0);
SET_PIXEL(col, row - 1, val);
wait = true;
}
} // we don't care if the LED is off
}
}
if (!wait) {
continue;
}
}
}
bool CCodecBuffer_RGBA16::ReadBlockRGBA(CMP_DWORD x, CMP_DWORD y, CMP_BYTE w, CMP_BYTE h, CMP_WORD wBlock[])
{
assert(x < GetWidth());
assert(y < GetHeight());
assert(x % w == 0);
assert(y % h == 0);
if(x >= GetWidth() || y >= GetHeight())
return false;
CMP_DWORD dwWidth = min(w, (GetWidth() - x));
CMP_DWORD i, j;
for(j = 0; j < h && (y + j) < GetHeight(); j++)
{
CMP_WORD* pData = (CMP_WORD*) (GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
for(i = 0; i < dwWidth; i++)
{
memcpy(GET_PIXEL(i, j), pData, nPixelSize);
pData += 4;
}
// Pad line with previous values if necessary
if(i < w)
PadLine(i, w, 4, &wBlock[j * w * 4]);
}
// Pad block with previous values if necessary
if(j < h)
PadBlock(j, w, h, 4, wBlock);
return true;
}
bool CCodecBuffer_R32F::ReadBlockRGBA(CMP_DWORD x, CMP_DWORD y, CMP_BYTE w, CMP_BYTE h, float block[])
{
assert(x < GetWidth());
assert(y < GetHeight());
assert(x % w == 0);
assert(y % h == 0);
if(x >= GetWidth() || y >= GetHeight())
return false;
CMP_DWORD dwWidth = min(w, (GetWidth() - x));
CMP_DWORD i, j;
for(j = 0; j < h && (y + j) < GetHeight(); j++)
{
float* pData = (float*) (GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
for(i = 0; i < dwWidth; i++)
{
float* pDest = GET_PIXEL(i, j);
*pDest++ = *pData++;
*pDest++ = 0.0f;
*pDest++ = 0.0f;
*pDest++ = 1.0f;
}
// Pad line with previous values if necessary
if(i < w)
PadLine(i, w, 4, &block[j * w * 4]);
}
// Pad block with previous values if necessary
if(j < h)
PadBlock(j, w, h, 4, block);
return true;
}
void samples2coefficients(FloatImage *inbuf, char dimension)
{
int ii, kk;
float z = sqrt(3.0) - 2.0;
float lambda = (1.0 - z) * (1.0 - 1.0/z);
float value;
// Apply overall gain
for (ii=0; ii<inbuf->size_x; ii++)
for (kk=0; kk<inbuf->size_y; kk++) {
value = GET_PIXEL(ii,kk) * lambda;
SET_PIXEL(ii,kk,value);
}
for (kk=0; kk<inbuf->size_y; kk++) {
// Causal initialization (first pixel of line)
}
// Causal recursion
// Anticausal initialization
// Anticausal recursion
}
/* Precalculate all three rows of an image and start the rendering. */
static void start_image(struct mb_display *disp, const struct mb_image *img)
{
int row, col;
for (row = 0; row < DISPLAY_ROWS; row++) {
disp->row[row] = 0;
for (col = 0; col < DISPLAY_COLS; col++) {
if (GET_PIXEL(img, map[row][col].x, map[row][col].y)) {
disp->row[row] |= BIT(LED_COL1_GPIO_PIN + col);
}
}
disp->row[row] = ~disp->row[row] & col_mask;
disp->row[row] |= BIT(LED_ROW1_GPIO_PIN + row);
}
disp->cur = 0;
if (disp->duration == K_FOREVER) {
disp->expiry = K_FOREVER;
} else {
disp->expiry = k_uptime_get() + disp->duration;
}
k_timer_start(&disp->timer, K_NO_WAIT, K_MSEC(4));
}
unsigned char *readppm(char *filename, int *width, int *height)
{
unsigned int x, y, ch, px;
unsigned char *img;
image image;
color_component *cp;
FILE *fp;
fopen_s(&fp, filename, "rb");
image = get_ppm(fp);
if (!image)
return NULL;
img = (unsigned char *)malloc(sizeof(unsigned char) * image->width * image->height * 3);
for (y = 0; y < image->height; ++y) {
for (x = 0; x < image->width; ++x) {
px = (y * image->width + x) * 3;
cp = GET_PIXEL(image, x, y);
for (ch = 0; ch < 3; ++ch) {
img[px + ch] = cp[ch];
}
}
}
*width = image->width;
*height = image->height;
return img;
}
/*
KITECH_TopologicalGlobalMapComp의 onInitialize()에서
bmp 파일의 비트맵 읽어 세선화 알고리즘을 수행하여 처리하기 때문에
수행속도가 오래 걸림.
bmp 비트맵의 크기와 형태에 따라 세선화 알고리즘의 수행속도가 틀려짐.
<환경 조건>
1. Tetra 로봇 플렛폼
2. Intel Core2Duo 2.1GHz
3. 950 x 850 크기의 map
<실험>
1. onInitialize() 수행 속도 측정
26047ms
26078ms
26093ms
26125ms
26031ms
26031ms
26094ms
26078ms
26125ms
26047ms
*/
ReturnType KITECH_TopologicalGlobalMapComp::onInitialize()
{
PrintMessage ("KITECH_TopologicalGlobalMapComp::onInitialize()\n");
#define GET_PIXEL(image, x, y) (*(image + mapWidth * y + x))
#define SET_PIXEL(image, x, y, value) (*(image + mapWidth * y + x) = value)
parameter.SetProperty(getPropertyMap());
if (!LoadProperty()) {
error = -1;
return OPROS_FIND_PROPERTY_ERROR;
}
gridBasedGlobalMap = new GridBasedGlobalMap (mapFileName.c_str(), mapResolution);
if (gridBasedGlobalMap == NULL) {
error = -1;
return OPROS_INTERNAL_FAULT;
}
gridBasedGlobalMap->ExpandObstacle (1, 1, 255);
gridBasedGlobalMap->ExpandObstacle (1, 255, 127);
gridBasedGlobalMap->ExpandObstacle (1, 127, 63);
gridBasedGlobalMap->ExpandObstacle (INTEGER(mobileSafeRadius/mapResolution), 1, 10);
int mapWidth = gridBasedGlobalMap->_width;
int mapHeight = gridBasedGlobalMap->_height;
BYTE *inBuff = new BYTE [mapWidth*mapHeight];
BYTE *outBuff = new BYTE [mapWidth*mapHeight];
int x, y;
for (y = 0; y < mapHeight; y++) {
for (x = 0; x < mapWidth; x++) {
BYTE pixel = gridBasedGlobalMap->GetPixelCELL (x, y);
SET_PIXEL (inBuff, x, y, ((0 < pixel) ? 0 : 4));
}
}
CThinning thinnig;
if (thinnig.MakeThinning (outBuff, inBuff, mapWidth, mapHeight) < 0) {
delete [] inBuff;
delete [] outBuff;
error = -1;
return OPROS_INTERNAL_FAULT;
}
for (y = 0; y < mapHeight; y++) {
for (x = 0; x < mapWidth; x++) {
BYTE pixel = GET_PIXEL (outBuff, x, y);
BYTE pixel2 = gridBasedGlobalMap->GetPixelCELL (x, y);
gridBasedGlobalMap->SetPixelCELL (x, y, pixel | pixel2);
}
}
delete [] inBuff;
delete [] outBuff;
error = 0;
return OPROS_SUCCESS;
}
/* draw raw hline */
static void framebuffer_draw_raw_hline(rt_uint8_t *pixels, int x1, int x2, int y)
{
rt_uint8_t *dst;
dst = GET_PIXEL(rtgui_graphic_get_device(), x1, y, rt_uint8_t);
rt_memcpy(dst, pixels, (x2 - x1) * (rtgui_graphic_get_device()->bits_per_pixel/8));
}
static void _rgb565p_get_pixel(rtgui_color_t *c, int x, int y)
{
rt_uint16_t pixel;
pixel = *GET_PIXEL(rtgui_graphic_get_device(), x, y, rt_uint16_t);
/* get pixel from color */
*c = rtgui_color_from_565p(pixel);
}
/* draw raw hline */
static void framebuffer_draw_raw_hline(rt_uint8_t *pixels, int x1, int x2, int y)
{
struct rtgui_graphic_driver *drv;
rt_uint8_t *dst;
drv = rtgui_graphic_get_device();
dst = GET_PIXEL(drv, x1, y, rt_uint8_t);
memcpy(dst, pixels,
(x2 - x1) * _UI_BITBYTES(drv->bits_per_pixel));
}
bool CCodecBuffer_RGB9995EF::ReadBlockRGBA(CMP_DWORD x, CMP_DWORD y, CMP_BYTE w, CMP_BYTE h, float block[])
{
assert(x < GetWidth());
assert(y < GetHeight());
assert(x % w == 0);
assert(y % h == 0);
if(x >= GetWidth() || y >= GetHeight())
return false;
CMP_DWORD dwWidth = min(w, (GetWidth() - x));
union { float f; int32_t i; } fi;
float Scale = 0.0f;
CMP_DWORD i, j;
for(j = 0; j < h && (y + j) < GetHeight(); j++)
{
DWORD* pData = (DWORD*)(GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
R9G9B9E5 pTemp;
pTemp.rm = ((*pData) & 0x000001ff);
pTemp.gm = ((*pData) & 0x0003fe00) >> 9;
pTemp.bm = ((*pData) & 0x07fc0000) >> 18;
pTemp.e = ((*pData) & 0xf8000000) >> 27;
fi.i = 0x33800000 + (pTemp.e << 23);
Scale = fi.f;
for (i = 0; i < dwWidth; i++)
{
float* pDest = GET_PIXEL(i, j);
*pDest++ = Scale * float(pTemp.rm);
*pDest++ = Scale * float(pTemp.gm);
*pDest++ = Scale * float(pTemp.bm);
*pDest++ = 1.0f;
//pData += nChannelCount;
}
//float* pData = (float*) (GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
//for(i = 0; i < dwWidth; i++)
//{
// float* pDest = GET_PIXEL(i, j);
// *pDest++ = *pData++;
// *pDest++ = 0.0f;
// *pDest++ = 0.0f;
// *pDest++ = 1.0f;
//}
// Pad line with previous values if necessary
if (i < w)
PadLine(i, w, 1, &block[j * w]);
}
// Pad block with previous values if necessary
if (j < h)
PadBlock(j, w, h, 1, block);
return true;
}
static void
paint_centered(int minor, char *data, int width, int height)
{
u8_t pixel[3];
u32_t i, min_x, min_y, max_x, max_y, x_painted = 0, rows = 0;
int r, bytespp;
struct device dev;
struct fb_var_screeninfo fbvs;
/* Put display in a known state to simplify positioning code below */
if ((r = arch_get_varscreeninfo(minor, &fbvs)) != OK) {
printf("fb: unable to get screen info: %d\n", r);
}
fbvs.yoffset = 0;
if ((r = arch_pan_display(minor, &fbvs)) != OK) {
printf("fb: unable to pan display: %d\n", r);
}
arch_get_device(minor, &dev);
/* Paint on a white canvas */
bytespp = fbvs.bits_per_pixel / 8;
for (i = 0; i < fbvs.xres * fbvs.yres * bytespp; i+= bytespp)
*((u32_t *)((u32_t) dev.dv_base + i)) = 0x00FFFFFF;
/* First seek to start */
min_x = fbvs.xres / 2 - width / 2;
max_x = fbvs.xres / 2 + width / 2;
min_y = fbvs.yres / 2 - height / 2;
max_y = fbvs.yres / 2 + height / 2;
i = min_x * fbvs.xres + min_y;
/* Add the image data */
for (i = ((min_y * fbvs.xres) + min_x) * bytespp; rows < height;) {
GET_PIXEL(data, pixel);
((unsigned char *)((u32_t) dev.dv_base + i))[0] = pixel[2];
((unsigned char *)((u32_t) dev.dv_base + i))[1] = pixel[1];
((unsigned char *)((u32_t) dev.dv_base + i))[2] = pixel[0];
((unsigned char *)((u32_t) dev.dv_base + i))[3] = 0;
x_painted++;/* Keep tab of how many row pixels we've painted */
if (x_painted == width) {
/* We've reached the end of the row, carriage return
* and go to next line.
*/
x_painted = 0;
rows++;
i = (((min_y + rows) * fbvs.xres) + min_x) * 4;
} else {
i += 4;
}
}
}
BOOL GetColorForPoint(int colorDiff, BYTE *p, int width,
int x0, int y0, int x1, int y1, int x2, int y2, BOOL *foundBkg, BYTE colors[][3])
{
BYTE *px1 = GET_PIXEL(p, x0, y0);
BYTE *px2 = GET_PIXEL(p, x1, y1);
BYTE *px3 = GET_PIXEL(p, x2, y2);
// If any of the corners have transparency, forget about it
// Not using != 255 because some MSN bmps have 254 in some positions
if (px1[3] < 253 || px2[3] < 253 || px3[3] < 253)
return FALSE;
// See if is the same color
if (ColorsAreTheSame(colorDiff, px1, px2) && ColorsAreTheSame(colorDiff, px3, px2)) {
*foundBkg = TRUE;
memmove(colors, px1, 3);
}
else *foundBkg = FALSE;
return TRUE;
}
static void _argb888_draw_hline(rtgui_color_t *c, int x1, int x2, int y)
{
int index;
rtgui_color_t *pixel_ptr;
/* get pixel pointer in framebuffer */
pixel_ptr = GET_PIXEL(rtgui_graphic_get_device(), x1, y, rtgui_color_t);
for (index = x1; index < x2; index++)
{
*pixel_ptr = *c;
pixel_ptr++;
}
}
static void _argb888_draw_vline(rtgui_color_t *c, int x, int y1, int y2)
{
struct rtgui_graphic_driver *drv;
rtgui_color_t *dst;
int index;
drv = rtgui_graphic_get_device();
dst = GET_PIXEL(drv, x, y1, rtgui_color_t);
for (index = y1; index < y2; index++)
{
*dst = *c;
dst += drv->width;
}
}
static void _rgb565p_draw_vline(rtgui_color_t *c, int x , int y1, int y2)
{
rt_uint8_t *dst;
rt_uint16_t pixel;
rt_ubase_t index;
pixel = rtgui_color_to_565p(*c);
dst = GET_PIXEL(rtgui_graphic_get_device(), x, y1, rt_uint8_t);
for (index = y1; index < y2; index ++)
{
*(rt_uint16_t*)dst = pixel;
dst += rtgui_graphic_get_device()->pitch;
}
}
static void _rgb565p_draw_vline(rtgui_color_t *c, int x , int y1, int y2)
{
struct rtgui_graphic_driver *drv;
rt_uint8_t *dst;
rt_uint16_t pixel;
int index;
drv = rtgui_graphic_get_device();
pixel = rtgui_color_to_565p(*c);
dst = GET_PIXEL(drv, x, y1, rt_uint8_t);
for (index = y1; index < y2; index ++)
{
*(rt_uint16_t *)dst = pixel;
dst += drv->pitch;
}
}
static void _rgb565p_draw_hline(rtgui_color_t *c, int x1, int x2, int y)
{
int index;
rt_uint16_t pixel;
rt_uint16_t *pixel_ptr;
/* get pixel from color */
pixel = rtgui_color_to_565p(*c);
/* get pixel pointer in framebuffer */
pixel_ptr = GET_PIXEL(rtgui_graphic_get_device(), x1, y, rt_uint16_t);
for (index = x1; index < x2; index ++)
{
*pixel_ptr = pixel;
pixel_ptr ++;
}
}
bool CCodecBuffer_RGBA32F::WriteBlockRGBA(CMP_DWORD x, CMP_DWORD y, CMP_BYTE w, CMP_BYTE h, float block[])
{
assert(x < GetWidth());
assert(y < GetHeight());
assert(x % w == 0);
assert(y % h == 0);
if(x >= GetWidth() || y >= GetHeight())
return false;
CMP_DWORD dwWidth = min(w, (GetWidth() - x));
for(CMP_DWORD j = 0; j < h && (y + j) < GetHeight(); j++)
{
float* pData = (float*) (GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
memcpy(pData, GET_PIXEL(0, j), dwWidth * nPixelSize);
}
return true;
}
BOOL MakeGrayscale(HBITMAP *hBitmap)
{
BYTE *p = NULL;
BYTE *p1;
DWORD dwLen;
int width, height, x, y;
BITMAP bmp;
GetObject(*hBitmap, sizeof(bmp), &bmp);
width = bmp.bmWidth;
height = bmp.bmHeight;
dwLen = width * height * 4;
p = (BYTE *)malloc(dwLen);
if (p == NULL)
{
return FALSE;
}
if (bmp.bmBitsPixel != 32)
{
// Convert to 32 bpp
HBITMAP hBmpTmp = CopyBitmapTo32(*hBitmap);
DeleteObject(*hBitmap);
*hBitmap = hBmpTmp;
}
GetBitmapBits(*hBitmap, dwLen, p);
// Make grayscale
for (y = 0 ; y < height ; y++)
{
for (x = 0 ; x < width ; x++)
{
p1 = GET_PIXEL(p, x, y);
p1[0] = p1[1] = p1[2] = ( p1[0] + p1[1] + p1[2] ) / 3;
}
}
dwLen = SetBitmapBits(*hBitmap, dwLen, p);
free(p);
return TRUE;
}
bool CCodecBuffer_R8::WriteBlockRGBA(CMP_DWORD x, CMP_DWORD y, CMP_BYTE w, CMP_BYTE h, CMP_BYTE block[])
{
assert(x < GetWidth());
assert(y < GetHeight());
assert(x % 4 == 0);
assert(y % 4 == 0);
if(x >= GetWidth() || y >= GetHeight())
return false;
CMP_DWORD dwWidth = min(w, (GetWidth() - x));
for(CMP_DWORD j = 0; j < h && (y + j) < GetHeight(); j++)
{
CMP_BYTE* pData = (CMP_BYTE*) (GetData() + ((y + j) * m_dwPitch) + (x * nPixelSize));
for(CMP_DWORD i = 0; i < dwWidth; i++)
{
memcpy(pData, GET_PIXEL(i, j)+RGBA8888_CHANNEL_R, nPixelSize);
pData += nChannelCount;
}
}
return true;
}
static void _argb888_get_pixel(rtgui_color_t *c, int x, int y)
{
*c = (rtgui_color_t)*GET_PIXEL(rtgui_graphic_get_device(), x, y, rtgui_color_t);
}
Bool
VNCViewer::HandleHextileBPP (sgVNCViewer *ct, int rx, int ry, int rw, int rh)
{
CARDBPP bg, fg;
int i;
CARD8 *ptr;
int x, y, w, h;
int sx, sy, sw, sh;
CARD8 subencoding;
CARD8 nSubrects;
for (y = ry; y < ry+rh; y += 16) {
for (x = rx; x < rx+rw; x += 16) {
w = h = 16;
if (rx+rw - x < 16)
w = rx+rw - x;
if (ry+rh - y < 16)
h = ry+rh - y;
if (!ReadFromRFBServer((char *)&subencoding, 1))
return False;
if (subencoding & rfbHextileRaw) {
if (!ReadFromRFBServer(buffer, w * h * (BPP / 8)))
return False;
ct->CopyDataToScreen(buffer, x, y, w, h);
continue;
}
if (subencoding & rfbHextileBackgroundSpecified)
if (!ReadFromRFBServer((char *)&bg, sizeof(bg)))
return False;
ct->FillToScreen(bg, x, y, w, h);
if (subencoding & rfbHextileForegroundSpecified)
if (!ReadFromRFBServer((char *)&fg, sizeof(fg)))
return False;
if (!(subencoding & rfbHextileAnySubrects)) {
continue;
}
if (!ReadFromRFBServer((char *)&nSubrects, 1))
return False;
ptr = (CARD8 *)buffer;
if (subencoding & rfbHextileSubrectsColoured) {
if (!ReadFromRFBServer(buffer, nSubrects * (2 + (BPP / 8))))
return False;
for (i = 0; i < nSubrects; i++) {
GET_PIXEL(fg, ptr);
sx = rfbHextileExtractX(*ptr);
sy = rfbHextileExtractY(*ptr);
ptr++;
sw = rfbHextileExtractW(*ptr);
sh = rfbHextileExtractH(*ptr);
ptr++;
ct->FillToScreen(fg, x+sx, y+sy, sw, sh);
}
} else {
if (!ReadFromRFBServer(buffer, nSubrects * 2))
return False;
for (i = 0; i < nSubrects; i++) {
sx = rfbHextileExtractX(*ptr);
sy = rfbHextileExtractY(*ptr);
ptr++;
sw = rfbHextileExtractW(*ptr);
sh = rfbHextileExtractH(*ptr);
ptr++;
ct->FillToScreen(fg, x+sx, y+sy, sw, sh);
}
}
}
}
return True;
}
GdkPixmap *
gdk_bitmap_create_from_data (GdkDrawable *drawable,
const gchar *data,
gint width,
gint height)
{
GdkPixmap *pixmap;
gi_gc_ptr_t gc;
g_return_val_if_fail (drawable == NULL || GDK_IS_DRAWABLE (drawable), NULL);
g_return_val_if_fail (data != NULL, NULL);
g_return_val_if_fail (width > 0 && height > 0, NULL);
GDK_NOTE (MISC, g_print ("gdk_bitmap_create_from_data: %dx%d\n",
width, height));
pixmap = gdk_pixmap_new (drawable, width, height, 1);
#define GET_PIXEL(data,pixel) \
((data[(pixel / 8)] & (0x1 << ((pixel) % 8))) >> ((pixel) % 8))
if (pixmap)
{
guchar *dst;
gint pitch;
gc = gi_create_gc(GDK_DRAWABLE_IMPL_GIX (GDK_PIXMAP_OBJECT (pixmap)->impl)->window_id,NULL);
#if 1
dst = bitmap_create_from_data((const uint8_t *)data, width,height,TRUE, &pitch);
if(dst){
gi_image_t img;
img.rgba = (gi_color_t*)dst;
img.format = GI_RENDER_a8;
img.w = width;
img.h = height;
img.pitch = pitch;
gi_draw_image(gc,&img,0,0);
free(dst);
}
#else
gint i, j;
surface = gi_create_image_depth(width,height,GI_RENDER_a8);
dst = surface->rgba;
pitch = surface->pitch;
for (i = 0; i < height; i++)
{
for (j = 0; j < width; j++){
dst[j] = GET_PIXEL (data, j) * 255;
}
data += (width + 7) / 8;
dst += pitch;
}
gi_draw_image(gc,surface,0,0);
gi_destroy_image(surface);
#endif
gi_destroy_gc(gc);
}
#undef GET_PIXEL
return pixmap;
}
static Bool
HandleHextileBPP (int rx, int ry, int rw, int rh)
{
CARDBPP bg, fg;
/* XGCValues gcv; */
int i;
CARD8 *ptr;
int x, y, w, h;
int sx, sy, sw, sh;
CARD8 subencoding;
CARD8 nSubrects;
for (y = ry; y < ry+rh; y += 16) {
for (x = rx; x < rx+rw; x += 16) {
w = h = 16;
if (rx+rw - x < 16)
w = rx+rw - x;
if (ry+rh - y < 16)
h = ry+rh - y;
if (!ReadFromRFBServer((char *)&subencoding, 1))
return False;
if (subencoding & rfbHextileRaw) {
if (!ReadFromRFBServer(buffer, w * h * (BPP / 8)))
return False;
/* CopyDataToScreen(buffer, x, y, w, h); */
continue;
}
if (subencoding & rfbHextileBackgroundSpecified)
if (!ReadFromRFBServer((char *)&bg, sizeof(bg)))
return False;
#if (BPP == 8)
/* if (appData.useBGR233)
gcv.foreground = BGR233ToPixel[bg];
else */
#endif
/* gcv.foreground = bg;
XChangeGC(dpy, gc, GCForeground, &gcv);
XFillRectangle(dpy, desktopWin, gc, x, y, w, h); */
if (subencoding & rfbHextileForegroundSpecified)
if (!ReadFromRFBServer((char *)&fg, sizeof(fg)))
return False;
if (!(subencoding & rfbHextileAnySubrects)) {
continue;
}
if (!ReadFromRFBServer((char *)&nSubrects, 1))
return False;
ptr = (CARD8 *)buffer;
if (subencoding & rfbHextileSubrectsColoured) {
if (!ReadFromRFBServer(buffer, nSubrects * (2 + (BPP / 8))))
return False;
for (i = 0; i < nSubrects; i++) {
GET_PIXEL(fg, ptr);
sx = rfbHextileExtractX(*ptr);
sy = rfbHextileExtractY(*ptr);
ptr++;
sw = rfbHextileExtractW(*ptr);
sh = rfbHextileExtractH(*ptr);
ptr++;
#if (BPP == 8)
/* if (appData.useBGR233)
gcv.foreground = BGR233ToPixel[fg];
else */
#endif
/* gcv.foreground = fg;
XChangeGC(dpy, gc, GCForeground, &gcv);
XFillRectangle(dpy, desktopWin, gc, x+sx, y+sy, sw, sh); */
}
} else {
if (!ReadFromRFBServer(buffer, nSubrects * 2))
return False;
#if (BPP == 8)
/* if (appData.useBGR233)
gcv.foreground = BGR233ToPixel[fg];
else */
#endif
/* gcv.foreground = fg;
XChangeGC(dpy, gc, GCForeground, &gcv); */
for (i = 0; i < nSubrects; i++) {
sx = rfbHextileExtractX(*ptr);
sy = rfbHextileExtractY(*ptr);
ptr++;
sw = rfbHextileExtractW(*ptr);
sh = rfbHextileExtractH(*ptr);
ptr++;
/* XFillRectangle(dpy, desktopWin, gc, x+sx, y+sy, sw, sh); */
}
}
}
}
return True;
}
inline void _ffill_rgbcolor(image img, rgb_color_p tc, int px, int py)
{
tc->red = GET_PIXEL(img, px, py)[0];
tc->green = GET_PIXEL(img, px, py)[1];
tc->blue = GET_PIXEL(img, px, py)[2];
}
/*
* See if finds a transparent background in image, and set its transparency
* Return TRUE if found a transparent background
*/
BOOL MakeTransparentBkg(MCONTACT hContact, HBITMAP *hBitmap)
{
int i, j;
BITMAP bmp;
GetObject(*hBitmap, sizeof(bmp), &bmp);
int width = bmp.bmWidth;
int height = bmp.bmHeight;
int colorDiff = db_get_w(hContact, "ContactPhoto", "TranspBkgColorDiff", db_get_w(0, AVS_MODULE, "TranspBkgColorDiff", 10));
// Min 5x5 to easy things in loop
if (width <= 4 || height <= 4)
return FALSE;
DWORD dwLen = width * height * 4;
BYTE *p = (BYTE *)malloc(dwLen);
if (p == nullptr)
return FALSE;
HBITMAP hBmpTmp;
if (bmp.bmBitsPixel == 32)
hBmpTmp = *hBitmap;
else // Convert to 32 bpp
hBmpTmp = CopyBitmapTo32(*hBitmap);
GetBitmapBits(hBmpTmp, dwLen, p);
// **** Get corner colors
// Top left
BYTE colors[8][3];
BOOL foundBkg[8];
if (!GetColorForPoint(colorDiff, p, width, 0, 0, 0, 1, 1, 0, &foundBkg[0], &colors[0])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Top center
if (!GetColorForPoint(colorDiff, p, width, width / 2, 0, width / 2 - 1, 0, width / 2 + 1, 0, &foundBkg[1], &colors[1])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Top Right
if (!GetColorForPoint(colorDiff, p, width,
width - 1, 0, width - 1, 1, width - 2, 0, &foundBkg[2], &colors[2])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Center left
if (!GetColorForPoint(colorDiff, p, width, 0, height / 2, 0, height / 2 - 1, 0, height / 2 + 1, &foundBkg[3], &colors[3])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Center left
if (!GetColorForPoint(colorDiff, p, width, width - 1, height / 2, width - 1, height / 2 - 1, width - 1, height / 2 + 1, &foundBkg[4], &colors[4])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Bottom left
if (!GetColorForPoint(colorDiff, p, width, 0, height - 1, 0, height - 2, 1, height - 1, &foundBkg[5], &colors[5])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Bottom center
if (!GetColorForPoint(colorDiff, p, width, width / 2, height - 1, width / 2 - 1, height - 1, width / 2 + 1, height - 1, &foundBkg[6], &colors[6])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Bottom Right
if (!GetColorForPoint(colorDiff, p, width, width - 1, height - 1, width - 1, height - 2, width - 2, height - 1, &foundBkg[7], &colors[7])) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// **** X corners have to have the same color
int count = 0;
for (i = 0; i < 8; i++)
if (foundBkg[i])
count++;
if (count < db_get_w(hContact, "ContactPhoto", "TranspBkgNumPoints", db_get_w(0, AVS_MODULE, "TranspBkgNumPoints", 5))) {
if (hBmpTmp != *hBitmap)
DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Ok, X corners at least have a color, lets compare then
int maxCount = 0, selectedColor = 0;
for (i = 0; i < 8; i++) {
if (foundBkg[i]) {
count = 0;
for (j = 0; j < 8; j++) {
if (foundBkg[j] && ColorsAreTheSame(colorDiff, (BYTE *)&colors[i], (BYTE *)&colors[j]))
count++;
}
if (count > maxCount) {
maxCount = count;
selectedColor = i;
}
}
}
if (maxCount < db_get_w(hContact, "ContactPhoto", "TranspBkgNumPoints",
db_get_w(0, AVS_MODULE, "TranspBkgNumPoints", 5))) {
// Not enought corners with the same color
if (hBmpTmp != *hBitmap) DeleteObject(hBmpTmp);
free(p);
return FALSE;
}
// Get bkg color as mean of colors
{
int bkgColor[3];
bkgColor[0] = 0;
bkgColor[1] = 0;
bkgColor[2] = 0;
for (i = 0; i < 8; i++) {
if (foundBkg[i] && ColorsAreTheSame(colorDiff, (BYTE *)&colors[i], (BYTE *)&colors[selectedColor])) {
bkgColor[0] += colors[i][0];
bkgColor[1] += colors[i][1];
bkgColor[2] += colors[i][2];
}
}
bkgColor[0] /= maxCount;
bkgColor[1] /= maxCount;
bkgColor[2] /= maxCount;
colors[selectedColor][0] = bkgColor[0];
colors[selectedColor][1] = bkgColor[1];
colors[selectedColor][2] = bkgColor[2];
}
// **** Set alpha for the background color, from the borders
if (hBmpTmp != *hBitmap) {
DeleteObject(*hBitmap);
*hBitmap = hBmpTmp;
GetObject(*hBitmap, sizeof(bmp), &bmp);
GetBitmapBits(*hBitmap, dwLen, p);
}
// Set alpha from borders
bool transpProportional = (db_get_b(NULL, AVS_MODULE, "MakeTransparencyProportionalToColorDiff", 0) != 0);
int *stack = (int *)malloc(width * height * 2 * sizeof(int));
if (stack == nullptr) {
free(p);
return FALSE;
}
// Put four corners
int topPos = 0;
AddToStack(stack, &topPos, 0, 0);
AddToStack(stack, &topPos, width / 2, 0);
AddToStack(stack, &topPos, width - 1, 0);
AddToStack(stack, &topPos, 0, height / 2);
AddToStack(stack, &topPos, width - 1, height / 2);
AddToStack(stack, &topPos, 0, height - 1);
AddToStack(stack, &topPos, width / 2, height - 1);
AddToStack(stack, &topPos, width - 1, height - 1);
int curPos = 0;
while (curPos < topPos) {
// Get pos
int x = stack[curPos]; curPos++;
int y = stack[curPos]; curPos++;
// Get pixel
BYTE *px1 = GET_PIXEL(p, x, y);
// It won't change the transparency if one exists
// (This avoid an endless loop too)
// Not using == 255 because some MSN bmps have 254 in some positions
if (px1[3] >= 253) {
if (ColorsAreTheSame(colorDiff, px1, (BYTE *)&colors[selectedColor])) {
px1[3] = (transpProportional) ? min(252,
(abs(px1[0] - colors[selectedColor][0])
+ abs(px1[1] - colors[selectedColor][1])
+ abs(px1[2] - colors[selectedColor][2])) / 3) : 0;
// Add 4 neighbours
if (x + 1 < width)
AddToStack(stack, &topPos, x + 1, y);
if (x - 1 >= 0)
AddToStack(stack, &topPos, x - 1, y);
if (y + 1 < height)
AddToStack(stack, &topPos, x, y + 1);
if (y - 1 >= 0)
AddToStack(stack, &topPos, x, y - 1);
}
}
}
free(stack);
SetBitmapBits(*hBitmap, dwLen, p);
free(p);
return TRUE;
}
// This function is buggy, since it doesn't fully implement a super sampling
// Still being worked on...
int camWarpingSuperSampling(CamImage *source, CamImage *dest, CamWarpingParams *params)
{
int x,y,width,height;
int xl,yl,xr,yr; // Position on the left and right side (source image)
int incxl,incyl,incxr,incyr; // Increment on the left and right sides (source image)
int dxl,dyl,dxr,dyr; // Delta for the left and right sides
int xp,yp; // Position of the current point
int xpp,ypp; // Position of the previous point
int incxp,incyp; // Increment for the current point
int xpl[CAM_MAX_SCANLINE+1],ypl[CAM_MAX_SCANLINE+1]; // (x,y) points for the previous scanline
CAM_PIXEL scanline[CAM_MAX_SCANLINE+1],valpix1,valpixp;
CAM_PIXEL *dstptr,*srcptr1,*cpdstptr;
int xptr1,yptr1=0xdead; // Special value. Generally inaccessible.
int result,xx,yy;
int perspective=params->perspective;
int zul,zbl,zur,zbr,cl,cr,zl,zr; // Projection parameters
int w1,w2,w3,w4,sum; // Weights for linear "square" interpolation
CamPoint I;
CAM_CHECK_ARGS(camWarpingSuperSampling,source->nChannels==1);
CAM_CHECK_ARGS(camWarpingSuperSampling,dest->nChannels==1);
CAM_CHECK_ARGS(camWarpingSuperSampling,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camWarpingSuperSampling,(source->depth&CAM_DEPTH_MASK)>=8);
CAM_CHECK_ARGS(camWarpingSuperSampling,(dest->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camWarpingSuperSampling,(dest->depth&CAM_DEPTH_MASK)>=8);
// ROI (Region Of Interest) management
if (dest->roi) {
dstptr=(CAM_PIXEL*)(dest->imageData+dest->roi->yOffset*dest->widthStep+dest->roi->xOffset*sizeof(CAM_PIXEL));
width=dest->roi->width;
height=dest->roi->height;
} else {
dstptr=(CAM_PIXEL*)dest->imageData;
width=dest->width;
height=dest->height;
}
xl=params->p[0].x;
yl=params->p[0].y;
xr=params->p[1].x;
yr=params->p[1].y;
dxl=params->p[3].x-xl;
dyl=params->p[3].y-yl;
dxr=params->p[2].x-xr;
dyr=params->p[2].y-yr;
// perspective warping or not?
if (perspective) {
if (!camIntersectionSegments(params->p,&I)) {
perspective=0;
} else {
// 32 bits fixed point
zul=(int)((((CAM_INT64)1)<<48)/(params->p[0].y-I.y));
zur=(int)((((CAM_INT64)1)<<48)/(params->p[1].y-I.y));
zbl=(int)((((CAM_INT64)1)<<48)/(params->p[3].y-I.y));
zbr=(int)((((CAM_INT64)1)<<48)/(params->p[2].y-I.y));
cl=(zul-zbl)/height; // 32 bits fixed point
cr=(zur-zbr)/height; // Should be positive valued
}
}
if (!perspective) {
incxl=dxl/height;
incyl=dyl/height;
incxr=dxr/height;
incyr=dyr/height;
} else {
incxl=(int)((((CAM_INT64)dxl)<<16)/dyl);
incxr=(int)((((CAM_INT64)dxr)<<16)/dyr);
}
// First scan-line
// Let's go across each horizontal pixel
xp=xl; yp=yl;
incxp=(xr-xl)/width;
incyp=(yr-yl)/width;
for (x=0;x<=width;x++,xp+=incxp,yp+=incyp) {
xpl[x]=xp;
ypl[x]=yp;
// Let's retrieve the pixel at that position in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
scanline[x]=valpix1;
}
// For all destination pixels
// Let's go across all the lines
for (y=0;y<height;y++) {
if (perspective) {
zl=zul-(int)(((CAM_INT64)y)*cl); // 32 bits fixed points
zr=zur-(int)(((CAM_INT64)y)*cr);
yl=I.y+(int)((((CAM_INT64)1)<<48)/zl);
yr=I.y+(int)((((CAM_INT64)1)<<48)/zr);
xl=params->p[0].x+(int)(((CAM_INT64)incxl)*(yl-params->p[0].y)>>16);
xr=params->p[1].x+(int)(((CAM_INT64)incxr)*(yr-params->p[1].y)>>16);
} else {
// Go to the next line
xl+=incxl;
xr+=incxr;
yl+=incyl;
yr+=incyr;
}
cpdstptr=dstptr;
xp=xl; yp=yl;
incxp=(xr-xl)/width;
incyp=(yr-yl)/width;
// Let's retrieve the value of the first pixel in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
// And then across each horizontal pixel
for (x=0;x<width;x++,dstptr++) {
// Manage the positions of pixels
xpp=xp;
ypp=yp;
xp+=incxp;
yp+=incyp;
// Manage the values of pixels
valpixp=valpix1;
// Let's retrieve the value of the current pixel in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
// OK. Now we do have our four source points
// These are pl[x], pl[x+1], pp and p
// (pl = previous line, pp = previous point, p = actual point)
// ((x,y) access trough xpl and ypl variables for pl)
// Their pixel values are respectively
// scanline[x], scanline[x+1], valpixp and valpix1
// We can compute the interpolation between them
switch (params->interpolation) {
case 1: // linear "square" interpolation (slow but nice for resampling)
w1=((0xffff-(xpl[x]&0xffff))>>8)*((0xffff-(ypl[x]&0xffff))>>8);
w2=((xpl[x+1]&0xffff)>>8)*((0xffff-(ypl[x+1]&0xffff))>>8);
w3=((0xffff-(xpp&0xffff))>>8)*((ypp&0xffff)>>8);
w4=((xp&0xffff)>>8)*((yp&0xffff)>>8);
sum=w1+w2+w3+w4;
if (sum) {
result=(((int)scanline[x])*w1+
((int)scanline[x+1])*w2+
((int)valpixp)*w3+
((int)valpix1)*w4)/(w1+w2+w3+w4);
break;
}
case 0: // Very crude (but fast) interpolation scheme
default:
result=((int)scanline[x]+
(int)scanline[x+1]+
(int)valpixp+
(int)valpix1)>>2;
break;
}
// Write the result to the destination image
*dstptr=(CAM_PIXEL)result;
// Go to the next point
// Copy to pl (previous line) in order to prepare for the next horizontal scan
xpl[x]=xpp;
ypl[x]=ypp;
// And copy the value of the previous pixel into the scanline buffer
scanline[x]=valpixp;
}
// Copy to pl (previous line) to prepare for the next horizontal scan
xpl[x]=xp;
ypl[x]=yp;
// And copy the value of the last pixel into the scanline buffer
scanline[x]=valpix1;
// Move the destination pointer
dstptr=(CAM_PIXEL*)(((char*)cpdstptr)+dest->widthStep);
}
static void _rgb565p_set_pixel(rtgui_color_t *c, int x, int y)
{
*GET_PIXEL(rtgui_graphic_get_device(), x, y, rt_uint16_t) = rtgui_color_to_565p(*c);
}
void
transform8_otherrgb_avx(ThreadInfo* t)
{
RS_IMAGE16 *input = t->input;
GdkPixbuf *output = t->output;
RS_MATRIX3 *matrix = t->matrix;
gint x,y;
gint width;
float mat_ps[4*4*3] __attribute__ ((aligned (16)));
for (x = 0; x < 4; x++ ) {
mat_ps[x] = matrix->coeff[0][0];
mat_ps[x+4] = matrix->coeff[0][1];
mat_ps[x+8] = matrix->coeff[0][2];
mat_ps[12+x] = matrix->coeff[1][0];
mat_ps[12+x+4] = matrix->coeff[1][1];
mat_ps[12+x+8] = matrix->coeff[1][2];
mat_ps[24+x] = matrix->coeff[2][0];
mat_ps[24+x+4] = matrix->coeff[2][1];
mat_ps[24+x+8] = matrix->coeff[2][2];
}
int start_x = t->start_x;
/* Always have aligned input and output adress */
if (start_x & 3)
start_x = ((start_x) / 4) * 4;
int complete_w = t->end_x - start_x;
/* If width is not multiple of 4, check if we can extend it a bit */
if (complete_w & 3)
{
if ((t->end_x+4) < input->w)
complete_w = ((complete_w+3) / 4 * 4);
}
__m128 gamma = _mm_set1_ps(t->output_gamma);
for(y=t->start_y ; y<t->end_y ; y++)
{
gushort *i = GET_PIXEL(input, start_x, y);
guchar *o = GET_PIXBUF_PIXEL(output, start_x, y);
gboolean aligned_write = !((guintptr)(o)&0xf);
width = complete_w >> 2;
while(width--)
{
/* Load and convert to float */
__m128i zero = _mm_setzero_si128();
__m128i in = _mm_load_si128((__m128i*)i); // Load two pixels
__m128i in2 = _mm_load_si128((__m128i*)i+1); // Load two pixels
_mm_prefetch(i + 64, _MM_HINT_NTA);
__m128i p1 =_mm_unpacklo_epi16(in, zero);
__m128i p2 =_mm_unpackhi_epi16(in, zero);
__m128i p3 =_mm_unpacklo_epi16(in2, zero);
__m128i p4 =_mm_unpackhi_epi16(in2, zero);
__m128 p1f = _mm_cvtepi32_ps(p1);
__m128 p2f = _mm_cvtepi32_ps(p2);
__m128 p3f = _mm_cvtepi32_ps(p3);
__m128 p4f = _mm_cvtepi32_ps(p4);
/* Convert to planar */
__m128 g1g0r1r0 = _mm_unpacklo_ps(p1f, p2f);
__m128 b1b0 = _mm_unpackhi_ps(p1f, p2f);
__m128 g3g2r3r2 = _mm_unpacklo_ps(p3f, p4f);
__m128 b3b2 = _mm_unpackhi_ps(p3f, p4f);
__m128 r = _mm_movelh_ps(g1g0r1r0, g3g2r3r2);
__m128 g = _mm_movehl_ps(g3g2r3r2, g1g0r1r0);
__m128 b = _mm_movelh_ps(b1b0, b3b2);
/* Apply matrix to convert to sRGB */
__m128 r2 = sse_matrix3_mul(mat_ps, r, g, b);
__m128 g2 = sse_matrix3_mul(&mat_ps[12], r, g, b);
__m128 b2 = sse_matrix3_mul(&mat_ps[24], r, g, b);
/* Normalize to 0->1 and clamp */
__m128 normalize = _mm_load_ps(_normalize);
__m128 max_val = _mm_load_ps(_ones_ps);
__m128 min_val = _mm_setzero_ps();
r = _mm_min_ps(max_val, _mm_max_ps(min_val, _mm_mul_ps(normalize, r2)));
g = _mm_min_ps(max_val, _mm_max_ps(min_val, _mm_mul_ps(normalize, g2)));
b = _mm_min_ps(max_val, _mm_max_ps(min_val, _mm_mul_ps(normalize, b2)));
/* Apply Gamma */
__m128 upscale = _mm_load_ps(_8bit);
r = _mm_mul_ps(upscale, _mm_fastpow_ps(r, gamma));
g = _mm_mul_ps(upscale, _mm_fastpow_ps(g, gamma));
b = _mm_mul_ps(upscale, _mm_fastpow_ps(b, gamma));
/* Convert to 8 bit unsigned and interleave*/
__m128i r_i = _mm_cvtps_epi32(r);
__m128i g_i = _mm_cvtps_epi32(g);
__m128i b_i = _mm_cvtps_epi32(b);
r_i = _mm_packs_epi32(r_i, r_i);
g_i = _mm_packs_epi32(g_i, g_i);
b_i = _mm_packs_epi32(b_i, b_i);
/* Set alpha value to 255 and store */
__m128i alpha_mask = _mm_load_si128((__m128i*)_alpha_mask);
__m128i rg_i = _mm_unpacklo_epi16(r_i, g_i);
__m128i bb_i = _mm_unpacklo_epi16(b_i, b_i);
p1 = _mm_unpacklo_epi32(rg_i, bb_i);
p2 = _mm_unpackhi_epi32(rg_i, bb_i);
p1 = _mm_or_si128(alpha_mask, _mm_packus_epi16(p1, p2));
if (aligned_write)
_mm_store_si128((__m128i*)o, p1);
else
_mm_storeu_si128((__m128i*)o, p1);
i += 16;
o += 16;
}
/* Process remaining pixels */
width = complete_w & 3;
while(width--)
{
__m128i zero = _mm_setzero_si128();
__m128i in = _mm_loadl_epi64((__m128i*)i); // Load two pixels
__m128i p1 =_mm_unpacklo_epi16(in, zero);
__m128 p1f = _mm_cvtepi32_ps(p1);
/* Splat r,g,b */
__m128 r = _mm_shuffle_ps(p1f, p1f, _MM_SHUFFLE(0,0,0,0));
__m128 g = _mm_shuffle_ps(p1f, p1f, _MM_SHUFFLE(1,1,1,1));
__m128 b = _mm_shuffle_ps(p1f, p1f, _MM_SHUFFLE(2,2,2,2));
__m128 r2 = sse_matrix3_mul(mat_ps, r, g, b);
__m128 g2 = sse_matrix3_mul(&mat_ps[12], r, g, b);
__m128 b2 = sse_matrix3_mul(&mat_ps[24], r, g, b);
r = _mm_unpacklo_ps(r2, g2); // GG RR GG RR
r = _mm_movelh_ps(r, b2); // BB BB GG RR
__m128 normalize = _mm_load_ps(_normalize);
__m128 max_val = _mm_load_ps(_ones_ps);
__m128 min_val = _mm_setzero_ps();
r = _mm_min_ps(max_val, _mm_max_ps(min_val, _mm_mul_ps(normalize, r)));
__m128 upscale = _mm_load_ps(_8bit);
r = _mm_mul_ps(upscale, _mm_fastpow_ps(r, gamma));
/* Convert to 8 bit unsigned */
zero = _mm_setzero_si128();
__m128i r_i = _mm_cvtps_epi32(r);
/* To 16 bit signed */
r_i = _mm_packs_epi32(r_i, zero);
/* To 8 bit unsigned - set alpha channel*/
__m128i alpha_mask = _mm_load_si128((__m128i*)_alpha_mask);
r_i = _mm_or_si128(alpha_mask, _mm_packus_epi16(r_i, zero));
*(int*)o = _mm_cvtsi128_si32(r_i);
i+=4;
o+=4;
}
}
}
