lib/multigrid.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <stdlib.h>
   4 #include <math.h>
   5 #include "darray.h"
   6 #include "colorize.h"
   7
   8 /* just process, but do not generate neighbor list and do not solve multigrid */
   9 //#define TEST_NO_RENDER
  10
  11 /* paste all color+luminance pyramids (from large to small) into result */
  12 //#define TEST_PYRAMID
  13
  14 /* test flow planes by drawing vectors into image
  15  * note: this only works in combination with TEST_PYRAMID
  16  */
  17 //#define TEST_FLOW
  18
  19 /* test makred planes as pyramid on the result
  20  * note: this only works in combination with TEST_PYRAMID
  21  */
  22 //#define TEST_MARKED
  23
  24 /* test shrinking of X and Y only */
  25 //#define TEST_SHRINK_XY
  26
  27 /* test shrinking of X, Y and Z */
  28 //#define TEST_SHRINK_Z
  29
  30 /* test expanding of X and Y only */
  31 //#define TEST_EXPAND_XY
  32
  33 /* test expanding of X, Y and Z */
  34 //#define TEST_EXPAND_Z
  35
  36 /* print error message b, if a is false */
  37 #define ASSERT(a, b) \
  38 { if (!(a)) { fprintf(stderr, "%s %s() Error in line %d: %s\n", __FILE__, __func__, __LINE__, (b)); abort(); } }
  39
  40 void ASSERT_FLOW(int w, int h, int k, const darray_t *flow_x, const darray_t *flow_y, const darray_t *flow_ix, const darray_t *flow_iy)
  41 {
  42         if (flow_x) {
  43                 ASSERT(darrayGetNumberOfDimensions(flow_x) == 3, "flow_x array must have 3 dimensions");
  44                 ASSERT(w == darrayGetDimensions(flow_x)[0], "x-sizes of flow_x vectors are different");
  45                 ASSERT(h == darrayGetDimensions(flow_x)[1], "y-sizes of flow_x vectors are different");
  46                 ASSERT(k-1 == darrayGetDimensions(flow_x)[2], "z-sizes of flow_x vectors are not k-1");
  47         }
  48         if (flow_y) {
  49                 ASSERT(darrayGetNumberOfDimensions(flow_y) == 3, "flow_y array must have 3 dimensions");
  50                 ASSERT(w == darrayGetDimensions(flow_y)[0], "x-sizes of flow_y vectors are different");
  51                 ASSERT(h == darrayGetDimensions(flow_y)[1], "y-sizes of flow_y vectors are different");
  52                 ASSERT(k-1 == darrayGetDimensions(flow_y)[2], "z-sizes of flow_y vectors are not k-1");
  53         }
  54         if (flow_ix) {
  55                 ASSERT(darrayGetNumberOfDimensions(flow_ix) == 3, "flow_ix array must have 3 dimensions");
  56                 ASSERT(w == darrayGetDimensions(flow_ix)[0], "x-sizes of flow_ix vectors are different");
  57                 ASSERT(h == darrayGetDimensions(flow_ix)[1], "y-sizes of flow_ix vectors are different");
  58                 ASSERT(k-1 == darrayGetDimensions(flow_ix)[2], "z-sizes of flow_ix vectors are not k-1");
  59         }
  60         if (flow_iy) {
  61                 ASSERT(darrayGetNumberOfDimensions(flow_iy) == 3, "flow_iy array must have 3 dimensions");
  62                 ASSERT(w == darrayGetDimensions(flow_iy)[0], "x-sizes of flow_iy vectors are different");
  63                 ASSERT(h == darrayGetDimensions(flow_iy)[1], "y-sizes of flow_iy vectors are different");
  64                 ASSERT(k-1 == darrayGetDimensions(flow_iy)[2], "z-sizes of flow_iy vectors are not k-1");
  65         }
  66 }
  67
  68 #define min(a, b) ((a < b) ? a : b)
  69 #define max(a, b) ((a > b) ? a : b)
  70
  71 #define NEIGHBORS_2D 9
  72 #define NEIGHBORS_3D 27
  73
  74 struct neighbors_2d {
  75         int num;                        /* # of neighbors */
  76         int index[NEIGHBORS_2D];        /* index of neighbors in an array */
  77         double weight[NEIGHBORS_2D];    /* calculated weight of neighbors */
  78 };
  79 struct neighbors_3d {
  80         int num;                        /* # of neighbors */
  81         int index[NEIGHBORS_3D];        /* index of neighbors in an array */
  82         double weight[NEIGHBORS_3D];    /* calculated weight of neighbors */
  83 };
  84
  85 static struct neigh_off {
  86         int x, y, z;
  87 } neigh_off[NEIGHBORS_3D] = {
  88         {  0,  0,  0 }, /* entry [0] always points to the same point */
  89         {  1,  0,  0 },
  90         {  0,  1,  0 },
  91         {  1,  1,  0 },
  92         {  0, -1,  0 },
  93         { -1,  0,  0 },
  94         { -1, -1,  0 },
  95         {  1, -1,  0 },
  96         { -1,  1,  0 },
  97         {  0,  0,  1 }, /* here begins the 3D part */
  98         {  1,  0,  1 },
  99         {  0,  1,  1 },
 100         {  1,  1,  1 },
 101         {  0, -1,  1 },
 102         { -1,  0,  1 },
 103         { -1, -1,  1 },
 104         {  1, -1,  1 },
 105         { -1,  1,  1 },
 106         {  0,  0, -1 },
 107         {  1,  0, -1 },
 108         {  0,  1, -1 },
 109         {  1,  1, -1 },
 110         {  0, -1, -1 },
 111         { -1,  0, -1 },
 112         { -1, -1, -1 },
 113         {  1, -1, -1 },
 114         { -1,  1, -1 },
 115 };
 116
 117 /* Shrink matrix by reducing z planes
 118  *
 119  * Every second (odd) plane is removed in the followin way:
 120  *  - The even plane is taken and the following odd plane is summed, using
 121  *    forward motion flow vectors. (If the motion vectors are not ouside the
 122  *    image).
 123  *  - The result is averaged.
 124  *
 125  * if the number of planes are odd, the last (even) plane is just copied.
 126  *
 127  * src plane 0 ---->_(+) dst plane 0
 128  *                  /|
 129  *                /
 130  *              / forward flow plane 0
 131  * src plane 1
 132  *
 133  *
 134  *
 135  * src plane 2 ---->_(+) dst plane 1
 136  *                  /|
 137  *                /
 138  *              / forward flow plane 2
 139  * src plane 3
 140  *
 141  *
 142  *
 143  * src plane 4 ------->  dst plane 2
 144  */
 145 static int shrink_z(darray_t *src, darray_t *dst, const darray_t *flow_x, const darray_t *flow_y)
 146 {
 147         int x, y, z, xx, yy, w, h, ks, kd;
 148         int i, pixles;
 149         double *src_ptr, *dst_ptr, *sum_ptr, *flow_x_ptr, *flow_y_ptr;
 150         darray_t *sum = NULL;
 151         int dst_move = 0;
 152
 153         ASSERT(darrayGetNumberOfDimensions(src) == 3, "src array must have 3 dimensions");
 154         w = darrayGetDimensions(src)[0];
 155         h = darrayGetDimensions(src)[1];
 156         ks = darrayGetDimensions(src)[2];
 157         kd = (ks+1)>>1;
 158
 159         /* create dst, if not given */
 160         if (!dst) {
 161                 int dimensions[3] = {w, h, kd};
 162
 163                 dst = darrayCreate(3, dimensions);
 164                 if (!dst)
 165                         goto error;
 166                 dst_move = 1;
 167         }
 168
 169         ASSERT(darrayGetNumberOfDimensions(dst) == 3, "dst array must have 3 dimensions");
 170         ASSERT(w == darrayGetDimensions(dst)[0], "x-size of source and destination must be equal");
 171         ASSERT(h == darrayGetDimensions(dst)[1], "y-size of source and destination must be equal");
 172         ASSERT(kd == ((ks+1)>>1), "z-size of destination must be half of the source (+an extra plane if source z-size is odd)");
 173         ASSERT_FLOW(w, h, ks, flow_x, flow_y, NULL, NULL);
 174
 175         sum = darrayCreate(3, darrayGetDimensions(dst));
 176         if (!sum)
 177                 goto error;
 178
 179         /* clear dst array with zeros, fill sum array with ones */
 180         pixles = w*h*kd;
 181         dst_ptr = darrayGetPr(dst);
 182         for (i = 0; i < pixles; i++)
 183                 *dst_ptr++ = 0.0;
 184         sum_ptr = darrayGetPr(sum);
 185         for (i = 0; i < pixles; i++)
 186                 *sum_ptr++ = 1.0;
 187
 188         /* first copy all even planes */
 189         for (z = 0; z < ks; z++) {
 190                 if ((z&1))
 191                         continue;
 192                 memcpy(darrayGetPr(dst) + (z/2)*w*h, darrayGetPr(src) + z*w*h, w*h*sizeof(double));
 193         }
 194
 195         /* project all odd planes */
 196         for (z = 0; z < ks; z++) {
 197                 if (!(z&1))
 198                         continue;
 199                 /* project up via forward flow */
 200                 src_ptr = darrayGetPr(src) + z*w*h;
 201                 dst_ptr = darrayGetPr(dst) + (z/2)*w*h;
 202                 sum_ptr = darrayGetPr(sum) + (z/2)*w*h;
 203                 flow_x_ptr = darrayGetPr(flow_x) + (z-1)*w*h;
 204                 flow_y_ptr = darrayGetPr(flow_y) + (z-1)*w*h;
 205                 for (y = 0; y < h; y++) {
 206                         for (x = 0; x < w; x++) {
 207                                 xx = x + *flow_x_ptr++;
 208                                 yy = y + *flow_y_ptr++;
 209                                 if (xx>=0 && yy>=0 && xx<w && yy<h) {
 210                                         *dst_ptr += src_ptr[xx + yy*w];
 211                                         (*sum_ptr) += 1.0;
 212                                 }
 213                                 dst_ptr++;
 214                                 sum_ptr++;
 215                         }
 216                 }
 217         }
 218
 219         /* average projected pixles */
 220         pixles = w*h*kd;
 221         dst_ptr = darrayGetPr(dst);
 222         sum_ptr = darrayGetPr(sum);
 223         for (i = 0; i < pixles; i++)
 224                 *dst_ptr++ /= *sum_ptr++;
 225
 226         darrayDestroy(sum);
 227         if (dst_move)
 228                 darrayMove(dst, src);
 229
 230         return 0;
 231
 232 error:
 233         if (dst_move)
 234                 darrayDestroy(dst);
 235         if (sum)
 236                 darrayDestroy(sum);
 237
 238         return -1;
 239 }
 240
 241 /* Expand matrix by expanding z planes
 242  *
 243  * The array is expanded by inserting new planes in the following way:
 244  *  - Forward flow vectors are used to project previous src plane to inserted
 245  *    odd destination plane. The weight is A.
 246  *  - Also forward flow vectors are used to project next src plane to inserted
 247  *    odd destination plane. The weight is B.
 248  *  - The result is averaged.
 249  *  - First border plane is just copied. (weighted 1.0)
 250  *  - Last border plane is projected using forward flow vector. (weighted 1.0)
 251  *  - Forward flow vectors are used to project previous source plane to even
 252  *    destination plane. The weight is B.
 253  *  - the source plane is copied to even destination plane. The weight is A.
 254  *  - All pixles pixles on the new plane result from two planes. Due
 255  *    to optical flow, none or many pixles are summed. All pixles on the
 256  *    new plane are summed and then averaged. At locations where no pixle
 257  *    was projected, the result is 0.
 258  *
 259  * If the number of planes in the result array is odd, it is just copied.
 260  *
 261  * The array (src) is scaled to array (dst) as shown:
 262  *
 263  * dst[0] = src[0]
 264  * dst[1] = src[0] * A + src[1] * B
 265  * dst[2] = src[1] * A + src[0] * B
 266  * dst[3] = src[1] * A + src[2] * B
 267  * dst[4] = src[2] * A + src[1] * B
 268  * dst[5] = src[2];
 269  *
 270  * The scaling shown in a different way:
 271  *
 272  * src plane 0 ------->  dst plane 0
 273  *
 274  * src plane 0
 275  *              \ *A
 276  *                \ dst forward flow plane 0
 277  *                 _\|
 278  *                  _(+) dst plane 1
 279  *                  /|
 280  *                / dst forward flow plane 1
 281  *              / *B
 282  * src plane 1
 283  *
 284  * src plane 0
 285  *            \ *B
 286  *              \ dst forward flow plane 0
 287  *                \ dst forward flow plane 1
 288  *                 _\|
 289  * src plane 1 -*A-->(+) dst plane 2
 290  *
 291  * src plane 1
 292  *              \ *A
 293  *                \ dst forward flow plane 2
 294  *                 _\|
 295  *                  _(+) dst plane 3
 296  *                  /|
 297  *                / dst forward flow plane 3
 298  *              / *B
 299  * src plane 2
 300  *
 301  * src plane 1
 302  *            \ *B
 303  *              \ dst forward flow plane 2
 304  *                \ dst forward flow plane 3
 305  *                 _\|
 306  * src plane 2 -*A-->(+) dst plane 4
 307  *
 308  * src plane 2
 309  *            \
 310  *              \ dst forward flow plane 4
 311  *                \
 312  *                 _\|
 313  *                       dst plane 5
 314  *
 315  * if odd number of dst planes:
 316  * src plane 3 ------->  dst plane 6
 317  *
 318  * A = 0.707
 319  * B = 0.293
 320  */
 321 static int expand_z(darray_t *src, const darray_t *dst, const darray_t *flow_x, const darray_t *flow_y)
 322 {
 323         int x, y, z, xx, yy, xxx, yyy, w, h, ks, kd, k_;
 324         double _x, _y;
 325         int i, pixles;
 326         double *src1_ptr, *src2_ptr, *dst1_ptr, *dst2_ptr, *sum1_ptr, *sum2_ptr, *flow_x1_ptr, *flow_y1_ptr, *flow_x2_ptr, *flow_y2_ptr;
 327         darray_t *sum = NULL;
 328         double A = sqrt(2) / 2;
 329         double B = 1.0 - A;
 330
 331         ASSERT(darrayGetNumberOfDimensions(src) == 3, "src array must have 3 dimensions");
 332         ASSERT(darrayGetNumberOfDimensions(dst) == 3, "dst array must have 3 dimensions");
 333         w = darrayGetDimensions(src)[0];
 334         h = darrayGetDimensions(src)[1];
 335         ks = darrayGetDimensions(src)[2];
 336         kd = darrayGetDimensions(dst)[2];
 337         ASSERT(w == darrayGetDimensions(dst)[0], "x-size of source and destination must be equal");
 338         ASSERT(h == darrayGetDimensions(dst)[1], "y-size of source and destination must be equal");
 339         ASSERT(ks == ((kd+1)>>1), "z-size of source must be half of the destination (+an extra plane if destination z-size is odd)");
 340         ASSERT_FLOW(w, h, kd, flow_x, flow_y, NULL, NULL);
 341
 342         sum = darrayCreate(3, darrayGetDimensions(dst));
 343         if (!sum)
 344                 goto error;
 345
 346         /* clear dst array with zeroes, keep sum with zeroes */
 347         pixles = w*h*kd;
 348         dst1_ptr = darrayGetPr(dst);
 349         for (i = 0; i < pixles; i++)
 350                 *dst1_ptr++ = 0.0;
 351
 352         z = 0;
 353         k_ = kd&(~1); /* even number of planes */
 354         if (k_) {
 355                 /* copy first plane with weight 1.0 */
 356                 memcpy(darrayGetPr(dst) + z*w*h, darrayGetPr(src) + (z/2)*w*h, w*h*sizeof(double));
 357                 /* fill sum array with ones */
 358                 pixles = w*h;
 359                 sum1_ptr = darrayGetPr(sum) + z*w*h;
 360                 for (i = 0; i < pixles; i++)
 361                         *sum1_ptr++ = 1.0;
 362                 z++;
 363
 364                 /* project planes in between */
 365                 for (z = 0; z < k_-2; z+=2) {
 366                         dst1_ptr = darrayGetPr(dst) + z*w*h;
 367                         dst2_ptr = dst1_ptr + w*h;
 368                         sum1_ptr = darrayGetPr(sum) + z*w*h;
 369                         sum2_ptr = sum1_ptr + w*h;
 370                         src1_ptr = darrayGetPr(src) + (z/2)*w*h;
 371                         src2_ptr = src1_ptr + w*h;
 372                         flow_x1_ptr = darrayGetPr(flow_x) + z*w*h;
 373                         flow_y1_ptr = darrayGetPr(flow_y) + z*w*h;
 374                         flow_x2_ptr = flow_x1_ptr + w*h;
 375                         flow_y2_ptr = flow_y1_ptr + w*h;
 376                         for (y = 0; y < h; y++) {
 377                                 for (x = 0; x < w; x++) {
 378                                         /* even plane (weight A) */
 379                                         dst2_ptr[x + y*w] += src2_ptr[x + y*w] * A;
 380                                         sum2_ptr[x + y*w] += A;
 381                                         /* get position via firt plane */
 382                                         _x = *flow_x1_ptr++;
 383                                         _y = *flow_y1_ptr++;
 384                                         xx = x + round(_x);
 385                                         yy = y + round(_y);
 386                                         if (xx<0 || yy<0 || xx>=w || yy>=h)
 387                                                 continue;
 388                                         /* odd plane (weight A)*/
 389                                         dst1_ptr[xx + yy*w] += src1_ptr[x + y*w] * A;
 390                                         sum1_ptr[xx + yy*w] += A;
 391                                         /* follow position via second plane */
 392                                         _x += flow_x2_ptr[xx + yy*w];
 393                                         _y += flow_y2_ptr[xx + yy*w];
 394                                         xxx = x + round(_x);
 395                                         yyy = y + round(_y);
 396                                         if (xxx<0 || yyy<0 || xxx>=w || yyy>=h)
 397                                                 continue;
 398                                         /* even plane (weight B) */
 399                                         dst2_ptr[xxx + yyy*w] += src1_ptr[x + y*w] * B;
 400                                         sum2_ptr[xxx + yyy*w] += B;
 401                                         /* odd plane (weigth B) */
 402                                         dst1_ptr[xx + yy*w] += src2_ptr[xxx + yyy*w] * B;
 403                                         sum1_ptr[xx + yy*w] += B;
 404                                 }
 405                         }
 406                 }
 407
 408                 /* project last plane with weight 1.0 */
 409                 dst1_ptr = darrayGetPr(dst) + z*w*h;
 410                 sum1_ptr = darrayGetPr(sum) + z*w*h;
 411                 src1_ptr = darrayGetPr(src) + (z/2)*w*h;
 412                 flow_x1_ptr = darrayGetPr(flow_x) + z*w*h;
 413                 flow_y1_ptr = darrayGetPr(flow_y) + z*w*h;
 414                 for (y = 0; y < h; y++) {
 415                         for (x = 0; x < w; x++) {
 416                                 /* get position via firt plane */
 417                                 _x = *flow_x1_ptr++;
 418                                 _y = *flow_y1_ptr++;
 419                                 xx = x + round(_x);
 420                                 yy = y + round(_y);
 421                                 if (xx<0 || yy<0 || xx>=w || yy>=h)
 422                                         continue;
 423                                 /* border (odd) plane (weight 1.0)*/
 424                                 dst1_ptr[xx + yy*w] += src1_ptr[x + y*w];
 425                                 sum1_ptr[xx + yy*w] += 1.0;
 426                         }
 427                 }
 428                 z++;
 429         }
 430
 431         /* copy extra plane, if destiantion size is odd */
 432         if (z < kd) {
 433                 memcpy(darrayGetPr(dst) + z*w*h, darrayGetPr(src) + (z/2)*w*h, w*h*sizeof(double));
 434                 /* fill sum array with ones */
 435                 pixles = w*h;
 436                 sum1_ptr = darrayGetPr(sum) + z*w*h;
 437                 for (i = 0; i < pixles; i++)
 438                         *sum1_ptr++ = 1.0;
 439         }
 440
 441         /* average projected pixles */
 442         pixles = w*h*kd;
 443         dst1_ptr = darrayGetPr(dst);
 444         sum1_ptr = darrayGetPr(sum);
 445         for (i = 0; i < pixles; i++) {
 446                 /* only if there is a sum */
 447                 if (*sum1_ptr > 0.1)
 448                         *dst1_ptr++ /= *sum1_ptr;
 449                 else
 450                         *dst1_ptr++ = 0.0;
 451                 sum1_ptr++;
 452         }
 453
 454         darrayDestroy(sum);
 455
 456         return 0;
 457
 458 error:
 459         if (sum)
 460                 darrayDestroy(sum);
 461
 462         return -1;
 463 }
 464
 465 /* Shrink flow planes
 466  *
 467  * Each pair of flow planes is shrinked by combining (tracing) two planes
 468  * in forward direction.
 469  *
 470  * If the number of flow planes is odd, the extra plane is removed.
 471  */
 472 static int shrink_flow_z(darray_t *src_x, darray_t *src_y, darray_t *dst_x, darray_t *dst_y)
 473 {
 474         int w, h, ks, kd, x, y, z, xx, yy;
 475         double _x, _y;
 476         double *src_x1_ptr, *src_x2_ptr, *src_y1_ptr, *src_y2_ptr, *dst_x_ptr, *dst_y_ptr;
 477         int dst_x_move = 0, dst_y_move = 0;
 478
 479         ASSERT(darrayGetNumberOfDimensions(src_x) == 3, "src_x array must have 3 dimensions");
 480         ASSERT(darrayGetNumberOfDimensions(src_y) == 3, "src_y array must have 3 dimensions");
 481         w = darrayGetDimensions(src_x)[0];
 482         h = darrayGetDimensions(src_x)[1];
 483         ks = darrayGetDimensions(src_x)[2];
 484         kd = ks>>1;
 485         ASSERT(w == darrayGetDimensions(src_y)[0], "x-size must be equal");
 486         ASSERT(h == darrayGetDimensions(src_y)[1], "y-size must be equal");
 487         ASSERT(ks == darrayGetDimensions(src_y)[2], "z-size must be equal");
 488
 489         if (dst_x == NULL) {
 490                 int dimensions[3] = {w, h, kd};
 491
 492                 dst_x = darrayCreate(3, dimensions);
 493                 if (!dst_x)
 494                         goto error;
 495                 dst_x_move = 1;
 496         }
 497         if (dst_y == NULL) {
 498                 int dimensions[3] = {w, h, kd};
 499
 500                 dst_y = darrayCreate(3, dimensions);
 501                 if (!dst_y)
 502                         goto error;
 503                 dst_y_move = 1;
 504         }
 505
 506         ASSERT(darrayGetNumberOfDimensions(dst_x) == 3, "flow_x array must have 3 dimensions");
 507         ASSERT(darrayGetNumberOfDimensions(dst_y) == 3, "flow_y array must have 3 dimensions");
 508         ASSERT(w == darrayGetDimensions(dst_x)[0], "x-size must be equal");
 509         ASSERT(h == darrayGetDimensions(dst_x)[1], "y-size must be equal");
 510         ASSERT(kd == darrayGetDimensions(dst_x)[2], "z-size of dest must be half of the planes + optional odd plane");
 511         ASSERT(w == darrayGetDimensions(dst_y)[0], "x-size must be equal");
 512         ASSERT(h == darrayGetDimensions(dst_y)[1], "y-size must be equal");
 513         ASSERT(kd == darrayGetDimensions(dst_y)[2], "z-size of dest must be half of the planes + optional odd plane");
 514
 515         for (z = 0; z < kd; z++) {
 516                 src_x1_ptr = darrayGetPr(src_x) + (z*2)*w*h;
 517                 src_y1_ptr = darrayGetPr(src_y) + (z*2)*w*h;
 518                 dst_x_ptr = darrayGetPr(dst_x) + z*w*h;
 519                 dst_y_ptr = darrayGetPr(dst_y) + z*w*h;
 520                 src_x2_ptr = darrayGetPr(src_x) + (z*2+1)*w*h;
 521                 src_y2_ptr = darrayGetPr(src_y) + (z*2+1)*w*h;
 522                 for (y = 0; y < h; y++) {
 523                         for (x = 0; x < w; x++) {
 524                                 /* get position via first plane */
 525                                 _x = *src_x1_ptr++;
 526                                 _y = *src_y1_ptr++;
 527                                 xx = x + round(_x);
 528                                 yy = y + round(_y);
 529                                 if (xx<0 || yy<0 || xx>=w || yy>=h) {
 530                                         /* use an outside value when outside in the first plane */
 531                                         *dst_x_ptr++ = _x;
 532                                         *dst_y_ptr++ = _y;
 533                                         continue;
 534                                 }
 535                                 /* follow position via second plane */
 536                                 _x += src_x2_ptr[xx + yy*w];
 537                                 _y += src_y2_ptr[xx + yy*w];
 538                                 /* store displacement result */
 539                                 *dst_x_ptr++ = _x;
 540                                 *dst_y_ptr++ = _y;
 541                         }
 542                 }
 543         }
 544
 545         if (dst_x_move)
 546                 darrayMove(dst_x, src_x);
 547         if (dst_y_move)
 548                 darrayMove(dst_y, src_y);
 549
 550         return 0;
 551
 552 error:
 553         if (dst_x_move)
 554                 darrayDestroy(dst_x);
 555         if (dst_y_move)
 556                 darrayDestroy(dst_y);
 557
 558         return -1;
 559 }
 560
 561 /* Shrink source matrix to half width and height and store into destinaction
 562  * matrix
 563  *
 564  * The destination matrix must be half in width and heit of the source matrix.
 565  * The z-depth is not changed, so the destination matrix has same z-dimension.
 566  *
 567  * If a dimension of a source matrix is odd, the end of the dimension is just
 568  * copied and not scaled.
 569  *
 570  * The points in a plane are shrinked from 2x2x2 to 1x1x1 by calculating the mean
 571  * of all eight values.
 572  */
 573 static int shrink_xy(darray_t *src, darray_t *dst)
 574 {
 575         int x, xx, y, z, ws, wd, w_, hs, hd, h_, k;
 576         double *s1, *s2, *d;
 577         int dst_move = 0;
 578
 579         ASSERT(darrayGetNumberOfDimensions(src) == 3, "Source array must have 3 dimensions");
 580         ws = darrayGetDimensions(src)[0];
 581         w_ = ws>>1;
 582         hs = darrayGetDimensions(src)[1];
 583         h_ = hs>>1;
 584         k = darrayGetDimensions(src)[2];
 585
 586         /* create dst, if not given */
 587         if (!dst) {
 588                 int dimensions[3] = {(ws+1)>>1, (hs+1)>>1, k};
 589
 590                 dst = darrayCreate(3, dimensions);
 591                 if (!dst)
 592                         goto error;
 593                 dst_move = 1;
 594         }
 595
 596         ASSERT(darrayGetNumberOfDimensions(dst) == 3, "Destination array must have 3 dimensions");
 597         wd = darrayGetDimensions(dst)[0];
 598         hd = darrayGetDimensions(dst)[1];
 599         ASSERT((ws&1) || ws == wd*2, "x-size of source must be twice the size of destination (this is even source size)");
 600         ASSERT(!(ws&1) || ws-1 == (wd-1)*2, "x-size of source-1 must be twice the size of destination-1 (this odd source size)");
 601         ASSERT((hs&1) || hs == hd*2, "y-size of source must be twice the size of destination (this is even source size)");
 602         ASSERT(!(hs&1) || hs-1 == (hd-1)*2, "y-size of source-1 must be twice the size of destination-1 (this odd source size)");
 603         ASSERT(k == darrayGetDimensions(dst)[2], "z-size must be equal");
 604
 605         for (z = 0; z < k; z++) {
 606                 for (y = 0; y < h_; y++) {
 607                         /* mean of 2x2x1 */
 608                         s1 = darrayGetPr(src) + y*2*ws + z*ws*hs;
 609                         s2 = s1 + ws;
 610                         d = darrayGetPr(dst) + y*wd + z*wd*hd;
 611                         for (x = 0, xx = 0; x < w_; x++, xx+=2)
 612                                 d[x] = (s1[xx] + s1[xx+1] + s2[xx] + s2[xx+1]) * 0.25;
 613                         /* mean of 1x2x1 */
 614                         if (x < wd)
 615                                 d[x] = (s1[xx] + s2[xx]) * 0.5;
 616                 }
 617                 if (y < hd) {
 618                         /* mean of 2x1x1 */
 619                         s1 = darrayGetPr(src) + y*2*ws + z*ws*hs;
 620                         d = darrayGetPr(dst) + y*wd + z*wd*hd;
 621                         for (x = 0, xx = 0; x < w_; x++, xx+=2)
 622                                 d[x] = (s1[xx] + s1[xx+1]) * 0.5;
 623                         /* mean of 1x1x1 */
 624                         if (x < wd)
 625                                 d[x] = s1[xx];
 626                 }
 627         }
 628
 629         if (dst_move)
 630                 darrayMove(dst, src);
 631
 632         return 0;
 633
 634 error:
 635         if (dst_move)
 636                 darrayDestroy(dst);
 637
 638         return -1;
 639 }
 640
 641 /* Expand X and Y using a simple linear filter
 642  *
 643  * The array (src) is scaled to array (dst) as shown:
 644  *
 645  * dst[0] = src[0]
 646  * dst[1] = src[0] * A + src[1] * B
 647  * dst[2] = src[1] * A + src[0] * B
 648  * dst[3] = src[1] * A + src[2] * B
 649  * dst[4] = src[2] * A + src[1] * B
 650  * dst[5] = src[2] * A + src[3] * B
 651  * dst[6] = src[3] * A + src[2] * B
 652  * dst[7] = src[3];
 653  *
 654  * Optional if the source grid has odd dimension:
 655  *
 656  * dst[8] = src[4];
 657  *
 658  * First the x-dimension is scaled, then the y-dimension is scaled
 659  * the same way.
 660  *
 661  * A = 0.707
 662  * B = 0.293
 663  */
 664 static void expand_xy(darray_t *src, const darray_t *dst)
 665 {
 666         int x, y, z, ws, wd, w_, hs, hd, h_, k;
 667         double *s, *s1, *s2, *d, *d1, *d2;
 668         double A = sqrt(2) / 2;
 669         double B = 1.0 - A;
 670
 671         ASSERT(darrayGetNumberOfDimensions(dst) == 3, "nvalue array must have 3 dimensions");
 672         ASSERT(darrayGetNumberOfDimensions(src) == 3, "value array must have 3 dimensions");
 673         ws = darrayGetDimensions(src)[0];
 674         wd = darrayGetDimensions(dst)[0];
 675         w_ = wd>>1;
 676         hs = darrayGetDimensions(src)[1];
 677         hd = darrayGetDimensions(dst)[1];
 678         h_ = hd>>1;
 679         k = darrayGetDimensions(src)[2];
 680         ASSERT((wd&1) || wd == ws*2, "x-size of destination must be twice the size of source (this is even destination size)");
 681         ASSERT(!(wd&1) || wd-1 == (ws-1)*2, "x-size of destination-1 must be twice the size of source-1 (this odd destination size)");
 682         ASSERT((hd&1) || hd == hs*2, "y-size of destination must be twice the size of source (this is even destination size)");
 683         ASSERT(!(hd&1) || hd-1 == (hs-1)*2, "y-size of destination-1 must be twice the size of source-1 (this odd destination size)");
 684         ASSERT(k == darrayGetDimensions(dst)[2], "z-size of source and destination must be equal");
 685
 686         /* Expand x-dimension
 687          * src is scaled in x-direction and stored in dst. In y-
 688          * direction it is stored in the upper half */
 689         for (z = 0; z < k; z++) {
 690                 for (y = 0; y < hs; y++) {
 691                         s = darrayGetPr(src) + y*ws + z*ws*hs;
 692                         d = darrayGetPr(dst) + (y+h_)*wd + z*wd*hd;
 693                         *d++ = *s; /* x=0 plane */
 694                         for (x = 0; x < w_-1; x++) {
 695                                 *d++ = A*s[0] + B*s[1];
 696                                 *d++ = A*s[1] + B*s[0];
 697                                 s++;
 698                         }
 699                         *d++ = *s++; /* x=w-1 plane */
 700                         /* extra plane (odd fine grid) */
 701                         if ((wd&1))
 702                                 *d = *s;
 703                 }
 704         }
 705
 706         /* Expand y-dimension
 707          * upper half of dst is scaled in y-direction. */
 708         for (z = 0; z < k; z++) {
 709                 /* y=h-1 plane */
 710                 s = darrayGetPr(dst) + h_*wd + z*wd*hd;
 711                 d = darrayGetPr(dst) + z*wd*hd;
 712                 for (x = 0; x < wd; x++)
 713                         *d++ = *s++; /* y=0 plane */
 714                 for (y = 0; y < h_-1; y++) {
 715                         s1 = darrayGetPr(dst) + (y+h_)*wd + z*wd*hd;
 716                         s2 = s1 + wd;
 717                         d1 = darrayGetPr(dst) + (y*2+1)*wd + z*wd*hd;
 718                         d2 = d1 + wd;
 719                         for (x = 0; x < wd; x++) {
 720                                 *d1++ = A * *s1 + B * *s2;
 721                                 *d2++ = A * *s2 + B * *s1;
 722                                 s1++;
 723                                 s2++;
 724                         }
 725                 }
 726                 /* y=h-1 plane and extra plane (already there) */
 727         }
 728 }
 729
 730 /* Quantize the binary information (for marked arrays)
 731  *
 732  * If an element is greater theshold, the element is set to gt, otherwise le.
 733  */
 734 static void quantize(const darray_t *array, double threshold, double gt, double le)
 735 {
 736         int n = 1, i;
 737         double *ptr;
 738
 739         ASSERT(darrayGetNumberOfDimensions(array) == 3, "array must have 3 dimensions");
 740         n *= darrayGetDimensions(array)[0];
 741         n *= darrayGetDimensions(array)[1];
 742         n *= darrayGetDimensions(array)[2];
 743         ptr = darrayGetPr(array);
 744
 745         for (i = 0; i < n; i++, ptr++) {
 746                 if (*ptr > threshold)
 747                         *ptr = gt;
 748                 else
 749                         *ptr = le;
 750         }
 751 }
 752
 753 /* multiply the array content with a value
 754  */
 755 static void multiply(const darray_t *array, double factor)
 756 {
 757         int n = 1, i;
 758         double *ptr;
 759
 760         ASSERT(darrayGetNumberOfDimensions(array) == 3, "array must have 3 dimensions");
 761         n *= darrayGetDimensions(array)[0];
 762         n *= darrayGetDimensions(array)[1];
 763         n *= darrayGetDimensions(array)[2];
 764         ptr = darrayGetPr(array);
 765
 766         for (i = 0; i < n; i++, ptr++)
 767                 *ptr *= factor;
 768 }
 769
 770 /* create an array of neighbors and use flow vectors
 771  * flow_x and flow_x are the vectors that point to neighbors of the next z plane
 772  * flow_x and flow_x are the vectors that point to neighbors of the previous z plane
 773  */
 774 static unsigned char *gen_neighbor(int w, int h, int k, const darray_t *flow_x, const darray_t *flow_y, const darray_t *flow_ix, const darray_t *flow_iy)
 775 {
 776         int x, y, z, xx, yy, zz, i, j, n;
 777         double _x, _y;
 778         int index;
 779         double *flow_x_ptr, *flow_y_ptr, *flow_ix_ptr, *flow_iy_ptr;
 780         unsigned char *neighbors, *ret_neighbors;
 781         unsigned long nb_size;
 782         int nb_num;
 783         int *i_index;
 784
 785         ASSERT_FLOW(w, h, k, flow_x, flow_y, flow_ix, flow_iy);
 786         flow_x_ptr = darrayGetPr(flow_x);
 787         flow_y_ptr = darrayGetPr(flow_y);
 788         flow_ix_ptr = darrayGetPr(flow_ix);
 789         flow_iy_ptr = darrayGetPr(flow_iy);
 790
 791         nb_num = (k==1) ? NEIGHBORS_2D : NEIGHBORS_3D;
 792         nb_size = (k==1) ? sizeof(struct neighbors_2d) : sizeof(struct neighbors_3d);
 793         ret_neighbors = neighbors = calloc(w*h*k, nb_size);
 794 //      printf("alloc neighbor grid=%lu Mbytes\n", nb_size*w*h*k/1024/1024);
 795         if (!ret_neighbors) {
 796                 printf("failed to allocate neighbor grid\n");
 797                 return NULL;
 798         }
 799
 800         for (z = 0; z < k; z++) {
 801                 for (y = 0; y < h; y++) {
 802                         for (x = 0; x < w; x++) {
 803                                 if (k==1)
 804                                         i_index = ((struct neighbors_2d *)neighbors)->index;
 805                                 else
 806                                         i_index = ((struct neighbors_3d *)neighbors)->index;
 807                                 for (i = 0, n = 0; i < nb_num; i++) {
 808                                         /* xx, yy, zz is the neighbor of color source
 809                                          * The first neighbor is always the point itself!
 810                                          */
 811                                         xx = x+neigh_off[i].x;
 812                                         yy = y+neigh_off[i].y;
 813                                         zz = z+neigh_off[i].z;
 814                                         if (xx < 0 || yy < 0 || zz < 0)
 815                                                 continue;
 816                                         if (xx >= w || yy >= h || zz >= k)
 817                                                 continue;
 818                                         if (zz > z) {
 819                                                 _x = flow_x_ptr[xx + yy*w + (zz-1)*w*h];
 820                                                 _y = flow_y_ptr[xx + yy*w + (zz-1)*w*h];
 821                                                 xx = xx + round(_x);
 822                                                 yy = yy + round(_y);
 823                                         } else if (zz < z) {
 824                                                 _x = flow_ix_ptr[xx + yy*w + zz*w*h];
 825                                                 _y = flow_iy_ptr[xx + yy*w + zz*w*h];
 826                                                 xx = xx + round(_x);
 827                                                 yy = yy + round(_y);
 828                                         }
 829                                         if (xx < 0 || yy < 0)
 830                                                 continue;
 831                                         if (xx >= w || yy >= h)
 832                                                 continue;
 833                                         index = xx + yy*w + zz*w*h;
 834                                         /* eliminate double entries */
 835                                         for (j = 0; j < n; j++) {
 836                                                 if (i_index[j] == index)
 837                                                         break;
 838                                         }
 839                                         if (j < n)
 840                                                 continue;
 841                                         i_index[n++] = index;
 842                                 }
 843                                 if (k == 1)
 844                                         ((struct neighbors_2d *)neighbors)->num = n;
 845                                 else
 846                                         ((struct neighbors_3d *)neighbors)->num = n;
 847                                 neighbors += nb_size;
 848                         }
 849                 }
 850         }
 851
 852         return ret_neighbors;
 853 }
 854
 855 /* weighten neighbors */
 856 static void weighten(const darray_t *luminance, unsigned char *neighbors)
 857 {
 858         int x, y, z, i, w, h, k, num;
 859         double val, nval, mean, variance, sigma, sum, weight, min;
 860         double sqdiff[NEIGHBORS_3D];
 861         double log0_01 = log(0.01);
 862         double *lum_ptr;
 863         unsigned long nb_size;
 864         int *i_index; double *w_index;
 865
 866         ASSERT(darrayGetNumberOfDimensions(luminance) == 3, "luminance array must have 3 dimensions");
 867         w = darrayGetDimensions(luminance)[0];
 868         h = darrayGetDimensions(luminance)[1];
 869         k = darrayGetDimensions(luminance)[2];
 870         lum_ptr = darrayGetPr(luminance);
 871
 872         nb_size = (k==1) ? sizeof(struct neighbors_2d) : sizeof(struct neighbors_3d);
 873
 874         for (z = 0; z < k; z++) {
 875                 for (y = 0; y < h; y++) {
 876                         for (x = 0; x < w; x++, neighbors += nb_size) {
 877                                 if (k==1) {
 878                                         num = ((struct neighbors_2d *)neighbors)->num;
 879                                         i_index = ((struct neighbors_2d *)neighbors)->index;
 880                                         w_index = ((struct neighbors_2d *)neighbors)->weight;
 881                                 } else {
 882                                         num = ((struct neighbors_3d *)neighbors)->num;
 883                                         i_index = ((struct neighbors_3d *)neighbors)->index;
 884                                         w_index = ((struct neighbors_3d *)neighbors)->weight;
 885                                 }
 886                                 /* Be sure that we have at least 3 neighbors.
 887                                  * This applies for every corner in a plane.
 888                                  * The first point in the neighbor list iss the point itself.
 889                                  */
 890                                 ASSERT(num > 3, "Every point must have at least 3 neighbors");
 891                                 /* value of current pixle */
 892                                 val = lum_ptr[i_index[0]];
 893                                 mean = 0;
 894                                 variance = 0;
 895                                 for (i = 0; i < num; i++) {
 896                                         /* value of neighbor pixles, including current pixle */
 897                                         nval = lum_ptr[i_index[i]];
 898                                         /* squared diffs between current pixle and each neighbor */
 899                                         sqdiff[i] = (nval-val) * (nval-val);
 900                                         /* variance */
 901                                         variance += nval * nval;
 902                                         /* mean */
 903                                         mean += nval;
 904                                 }
 905                                 /* mean */
 906                                 mean /= num;
 907                                 /* variance */
 908                                 variance /= num;
 909                                 variance -= mean * mean;
 910                                 /* sigma */
 911                                 sigma = variance * 0.6;
 912                                 min = sqdiff[1];
 913                                 for (i = 2; i < num; i++) {
 914                                         if (sqdiff[i] < min)
 915                                                 min = sqdiff[i];
 916                                 }
 917                                 if (sigma < -min / log0_01)
 918                                         sigma = -min / log0_01;
 919                                 if (sigma < 0.000002)
 920                                         sigma = 0.000002;
 921                                 /* sum of weights */
 922                                 sum = 0;
 923                                 for (i = 0; i < num; i++) {
 924                                         sum += exp(-sqdiff[i]/sigma);
 925                                 }
 926                                 /* each weight (divided by sum) */
 927                                 for (i = 0; i < num; i++) {
 928                                         weight = exp(-sqdiff[i]/sigma) / sum;
 929                                         /* substract 1, if point is the point, not neighbor */
 930                                         if (i == 0)
 931                                                 weight -= 1;
 932                                         w_index[i] = weight;
 933                                 }
 934                         }
 935                 }
 936         }
 937 }
 938
 939 /* Smooth the value array
 940  *
 941  * If a pixle is marked (0 in mark array), the value at level 0 is taken from
 942  * the init array (color of marked pixle). At other level the color is not
 943  * changed.
 944  *
 945  * If a pixle is not marked, the value is calculated as below.
 946  */
 947 static void smooth(int level, darray_t *value, const darray_t *mark, const darray_t *init, unsigned char *neighbors)
 948 {
 949         int x, y, z, w, h, k, i, num;
 950         double *value_ptr, *value_line_ptr, *mark_line_ptr, *init_line_ptr;
 951         double val;
 952         unsigned long nb_size;
 953         int *i_index; double *w_index;
 954
 955         ASSERT(darrayGetNumberOfDimensions(value) == 3, "value array must have 3 dimensions");
 956         ASSERT(darrayGetNumberOfDimensions(mark) == 3, "mark array must have 3 dimensions");
 957         ASSERT(darrayGetNumberOfDimensions(init) == 3, "init array must have 3 dimensions");
 958         w = darrayGetDimensions(value)[0];
 959         h = darrayGetDimensions(value)[1];
 960         k = darrayGetDimensions(value)[2];
 961         ASSERT(w == darrayGetDimensions(mark)[0], "x-sizes of mark array is different");
 962         ASSERT(h == darrayGetDimensions(mark)[1], "y-sizes of mark array is different");
 963         ASSERT(k == darrayGetDimensions(mark)[2], "z-sizes of mark array is different");
 964         value_ptr = darrayGetPr(value);
 965
 966         nb_size = (k==1) ? sizeof(struct neighbors_2d) : sizeof(struct neighbors_3d);
 967
 968         for (z = 0; z < k; z++) {
 969                 for (y = 0; y < h; y++) {
 970                         value_line_ptr = darrayGetPr(value) + y*w + z*w*h;
 971                         mark_line_ptr = darrayGetPr(mark) + y*w + z*w*h;
 972                         init_line_ptr = darrayGetPr(init) + y*w + z*w*h;
 973                         for (x = 0; x < w; x++, neighbors += nb_size) {
 974                                 if (k==1) {
 975                                         num = ((struct neighbors_2d *)neighbors)->num;
 976                                         i_index = ((struct neighbors_2d *)neighbors)->index;
 977                                         w_index = ((struct neighbors_2d *)neighbors)->weight;
 978                                 } else {
 979                                         num = ((struct neighbors_3d *)neighbors)->num;
 980                                         i_index = ((struct neighbors_3d *)neighbors)->index;
 981                                         w_index = ((struct neighbors_3d *)neighbors)->weight;
 982                                 }
 983                                 /* if pixle is marked */
 984                                 if (mark_line_ptr[x] < 0.5) {
 985                                         /* always use initial color on marked plane, otherwise do not smooth */
 986                                         if (level == 0)
 987                                                 value_line_ptr[x] = init_line_ptr[x];
 988                                         continue;
 989                                 }
 990                                 /* Calculate value */
 991                                 val = 0;
 992                                 for (i = 1; i < num; i++) /* sum value without point itself */
 993                                         val += w_index[i] * value_ptr[i_index[i]];
 994                                 if (level == 0)
 995                                         value_line_ptr[x] = (init_line_ptr[x] - val) / w_index[0];
 996                                 else
 997                                         value_line_ptr[x] = (-val) / w_index[0];
 998                         }
 999                 }
1000         }
1001 }
1002
1003 /* Calculate residual
1004  */
1005 static double residual(int level, darray_t *value, const darray_t *mark, const darray_t *init, unsigned char *neighbors)
1006 {
1007         int x, y, z, w, h, k, i, num;
1008         double *value_ptr, *mark_line_ptr, *init_line_ptr;
1009         double val, residual = 0;
1010         unsigned long nb_size;
1011         int *i_index; double *w_index;
1012
1013         ASSERT(darrayGetNumberOfDimensions(value) == 3, "value array must have 3 dimensions");
1014         ASSERT(darrayGetNumberOfDimensions(mark) == 3, "mark array must have 3 dimensions");
1015         ASSERT(darrayGetNumberOfDimensions(init) == 3, "init array must have 3 dimensions");
1016         w = darrayGetDimensions(value)[0];
1017         h = darrayGetDimensions(value)[1];
1018         k = darrayGetDimensions(value)[2];
1019         ASSERT(w == darrayGetDimensions(mark)[0], "x-sizes of mark array is different");
1020         ASSERT(h == darrayGetDimensions(mark)[1], "y-sizes of mark array is different");
1021         ASSERT(k == darrayGetDimensions(mark)[2], "z-sizes of mark array is different");
1022         value_ptr = darrayGetPr(value);
1023
1024         nb_size = (k==1) ? sizeof(struct neighbors_2d) : sizeof(struct neighbors_3d);
1025
1026         for (z = 0; z < k; z++) {
1027                 for (y = 0; y < h; y++) {
1028                         init_line_ptr = darrayGetPr(init) + y*w + z*w*h;
1029                         mark_line_ptr = darrayGetPr(mark) + y*w + z*w*h;
1030                         for (x = 0; x < w; x++, neighbors += nb_size) {
1031                                 if (k==1) {
1032                                         num = ((struct neighbors_2d *)neighbors)->num;
1033                                         i_index = ((struct neighbors_2d *)neighbors)->index;
1034                                         w_index = ((struct neighbors_2d *)neighbors)->weight;
1035                                 } else {
1036                                         num = ((struct neighbors_3d *)neighbors)->num;
1037                                         i_index = ((struct neighbors_3d *)neighbors)->index;
1038                                         w_index = ((struct neighbors_3d *)neighbors)->weight;
1039                                 }
1040                                 /* if pixle is marked, no residual */
1041                                 if (mark_line_ptr[x] < 0.5)
1042                                         continue;
1043                                 /* Calculate value */
1044                                 val = 0;
1045                                 for (i = 0; i < num; i++) /* sum value including point itself */
1046                                         val += w_index[i] * value_ptr[i_index[i]];
1047                                 if (level == 0)
1048                                         val = init_line_ptr[x] - val;
1049                                 else
1050                                         val = -val;
1051                                 residual += val * val;
1052                         }
1053                 }
1054         }
1055
1056         return residual;
1057 }
1058
1059 /* Restrict
1060  *
1061  * Given are arrays for value and value of next level.
1062  *
1063  * If value and next value have not only different width/height, but also
1064  * different depth (z dimension), a temporary array (temp) is used to
1065  * perform the intermediate step of scaling x,y,z.
1066  *
1067  * Note that the flow vectors must be scaled xy already!
1068  */
1069 static int restrict(const darray_t *value, darray_t *nvalue, const darray_t *flow_x, const darray_t *flow_y)
1070 {
1071         int ks, wd, hd, kd;
1072         darray_t *temp = NULL;
1073         int rc = -1;
1074
1075         ASSERT(darrayGetNumberOfDimensions(value) == 3, "Source array must have 3 dimensions");
1076         ASSERT(darrayGetNumberOfDimensions(nvalue) == 3, "Destination array must have 3 dimensions");
1077         wd = darrayGetDimensions(nvalue)[0];
1078         hd = darrayGetDimensions(nvalue)[1];
1079         kd = darrayGetDimensions(nvalue)[2];
1080         ks = darrayGetDimensions(value)[2];
1081         if (ks != kd)
1082                 ASSERT(flow_x && flow_y, "when scaling z, flow vectors must be given");
1083
1084         if (ks != kd) {
1085                 int dimensions[3] = {wd, hd, ks};
1086
1087                 /* Cube that is only shrinked in x and y */
1088                 temp = darrayCreate(3, dimensions);
1089                 if (!temp)
1090                         goto error;
1091
1092                 rc = shrink_xy((darray_t *)value, temp);
1093                 if (rc < 0)
1094                         goto error;
1095                 rc = shrink_z(temp, nvalue, flow_x, flow_y);
1096                 if (rc < 0)
1097                         goto error;
1098         } else {
1099                 rc = shrink_xy((darray_t *)value, nvalue);
1100                 if (rc < 0)
1101                         goto error;
1102         }
1103
1104         rc = 0;
1105
1106 error:
1107         darrayDestroy(temp);
1108
1109         return rc;
1110 }
1111
1112 /* Prolong
1113  *
1114  * Given are arrays for value and value of next level.
1115  *
1116  * If value and next value have not only different width/height, but also
1117  * different depth (z dimension), a temporary array (temp3) is used to
1118  * perform the intermediate step of scaling x,y,z.
1119  *
1120  * Note that the flow vectors must be scaled xy already!
1121  *
1122  * First the value is shrinked to the size of the value of the next level.
1123  * The error is calculated by substracting the values of the next level
1124  * from the value. The error is then expanded. The expanded error is
1125  * substracted from the value. So the changes in the next level's value are
1126  * prolonged to the value.
1127  */
1128 static int prolong(darray_t *value, const darray_t *nvalue, const darray_t *flow_x, const darray_t *flow_y)
1129 {
1130         int ws, hs, ks, wd, hd, kd;
1131         int i, pixles;
1132         darray_t *temp1 = NULL, *temp2 = NULL, *temp3 = NULL;
1133         double *value_ptr, *nvalue_ptr, *temp1_ptr, *temp2_ptr;
1134         int rc = -1;
1135
1136         ASSERT(darrayGetNumberOfDimensions(value) == 3, "nvalue array must have 3 dimensions");
1137         ASSERT(darrayGetNumberOfDimensions(nvalue) == 3, "value array must have 3 dimensions");
1138         ws = darrayGetDimensions(nvalue)[0];
1139         hs = darrayGetDimensions(nvalue)[1];
1140         ks = darrayGetDimensions(nvalue)[2];
1141         wd = darrayGetDimensions(value)[0];
1142         hd = darrayGetDimensions(value)[1];
1143         kd = darrayGetDimensions(value)[2];
1144         if (ks != kd)
1145                 ASSERT(flow_x && flow_y, "when scaling z, flow vectors must be given");
1146
1147         /* generate temporary arrays */
1148         temp1 = darrayCreate(3, darrayGetDimensions(value));
1149         if (!temp1)
1150                 goto error;
1151         temp2 = darrayCreate(3, darrayGetDimensions(nvalue));
1152         if (!temp2)
1153                 goto error;
1154         if (ks != kd) {
1155                 int dimensions[3] = {ws, hs, kd};
1156
1157                 /* Cube that is only shrinked in x and y */
1158                 temp3 = darrayCreate(3, dimensions);
1159                 if (!temp3)
1160                         goto error;
1161         }
1162
1163         /* temp2 = shrink(value) */
1164         if (ks != kd) {
1165                 rc = shrink_xy(value, temp3);
1166                 if (rc < 0)
1167                         goto error;
1168                 rc = shrink_z(temp3, temp2, flow_x, flow_y);
1169                 if (rc < 0)
1170                         goto error;
1171         } else {
1172                 rc = shrink_xy(value, temp2);
1173                 if (rc < 0)
1174                         goto error;
1175         }
1176
1177         /* temp2 -= nvalue */
1178         temp2_ptr = darrayGetPr(temp2);
1179         nvalue_ptr = darrayGetPr(nvalue);
1180         pixles = ws*hs*ks;
1181         for (i = 0; i < pixles; i++)
1182                 *temp2_ptr++ -= *nvalue_ptr++;
1183
1184         /* Expand (temp2 -> temp1) */
1185         if (ks != kd) {
1186                 rc = expand_z(temp2, temp3, flow_x, flow_y);
1187                 if (rc < 0)
1188                         goto error;
1189                 expand_xy(temp3, temp1);
1190         } else
1191                 expand_xy(temp2, temp1);
1192
1193         /* value -= temp1 */
1194         temp1_ptr = darrayGetPr(temp1);
1195         value_ptr = darrayGetPr(value);
1196         pixles = wd*hd*kd;
1197         for (i = 0; i < pixles; i++)
1198                 *value_ptr++ -= *temp1_ptr++;
1199
1200         rc = 0;
1201
1202 error:
1203         /* destroy arrays */
1204         darrayDestroy(temp1);
1205         darrayDestroy(temp2);
1206         darrayDestroy(temp3);
1207
1208         return rc;
1209 }
1210
1211 /* Paste one array into anothter */
1212 void paste(const darray_t *src, darray_t *dst, int xoff, int yoff, int zoff)
1213 {
1214         int ws, hs, ks, wd, hd, /*kd,*/ x, y, z;
1215         double *src_ptr, *dst_ptr;
1216
1217         ws = darrayGetDimensions(src)[0];
1218         hs = darrayGetDimensions(src)[1];
1219         ks = darrayGetDimensions(src)[2];
1220         wd = darrayGetDimensions(dst)[0];
1221         hd = darrayGetDimensions(dst)[1];
1222         // kd = darrayGetDimensions(dst)[2];
1223         src_ptr = darrayGetPr(src);
1224         dst_ptr = darrayGetPr(dst);
1225
1226         for (z = 0; z < ks; z++) {
1227                 for (y = 0; y < hs; y++) {
1228                         for (x = 0; x < ws; x++) {
1229                                 dst_ptr[(x+xoff) + (y+yoff)*wd + (z+zoff)*wd*hd] = src_ptr[x + y*ws + z*ws*hs];
1230                         }
1231                 }
1232         }
1233 }
1234
1235 inline void draw_line(double *ptr, int w, int h, int x0, int y0, int x1, int y1, double color)
1236 {
1237         int dx, dy, sx, sy, err, e2;
1238
1239         dx = abs(x1 - x0);
1240         dy = abs(y1 - y0);
1241         if (x0 < x1)
1242                 sx = 1;
1243         else
1244                 sx = -1;
1245         if (y0 < y1)
1246                 sy = 1;
1247         else
1248                 sy = -1;
1249         err = dx - dy;
1250         while (1) {
1251                 if (x0 > 0 && x0 < w && y0 > 0 && y0 < h)
1252                         ptr[x0 + y0*w] = color;
1253                 if (x0 == x1 && y0 == y1)
1254                         break;
1255                 e2 = 2 * err;
1256                 if (e2 > -dy) {
1257                         err = err - dy;
1258                         x0 = x0 + sx;
1259                 }
1260                 if (e2 < dx) {
1261                         err = err + dx;
1262                         y0 = y0 + sy;
1263                 }
1264         }
1265 }
1266
1267 /* Draw motion flow vectors into dst
1268  *
1269  * The forward vectors are drawn white, the inverse vectors black.
1270  *
1271  * The flow vectors are drawn to the upper left corner of the dst, which
1272  * may be larger in size than the flow arrays.
1273  */
1274 void draw_flow(darray_t *dst, const darray_t *flow_x, const darray_t *flow_y, const darray_t *flow_ix, const darray_t *flow_iy)
1275 {
1276         int wd, hd, ws, hs, ks, x, y, z;
1277         double *flow_x_ptr, *flow_y_ptr, *flow_ix_ptr, *flow_iy_ptr;
1278         int step = 4;
1279
1280         wd = darrayGetDimensions(dst)[0];
1281         hd = darrayGetDimensions(dst)[1];
1282         ws = darrayGetDimensions(flow_x)[0];
1283         hs = darrayGetDimensions(flow_x)[1];
1284         ks = darrayGetDimensions(flow_x)[2];
1285         flow_x_ptr = darrayGetPr(flow_x);
1286         flow_y_ptr = darrayGetPr(flow_y);
1287         flow_ix_ptr = darrayGetPr(flow_ix);
1288         flow_iy_ptr = darrayGetPr(flow_iy);
1289
1290         for (z = 0; z <= ks; z++) {
1291                 for (y = step/2; y < hs-step/2+1; y+=step) {
1292                         for (x = step/2; x < ws-step/2+1; x+=step) {
1293                                 /* forward flow */
1294                                 if (z < ks)
1295                                         draw_line(darrayGetPr(dst) + z*wd*hd, wd, hd, x, y, x+round(flow_x_ptr[x + y*ws + z*ws*hs]), y+(int)(flow_y_ptr[x + y*ws + z*ws*hs]+0.5), 1.0);
1296                                 /* inverse flow */
1297                                 if (z > 0)
1298                                         draw_line(darrayGetPr(dst) + z*wd*hd, wd, hd, x, y, x+round(flow_ix_ptr[x + y*ws + (z-1)*ws*hs]), y+(int)(flow_iy_ptr[x + y*ws + (z-1)*ws*hs]+0.5), 0.0);
1299                         }
1300                 }
1301         }
1302 }
1303
1304 /* Solve multigrid
1305  *
1306  * Note that the flow vectors must be scaled xy already!
1307  */
1308 static int solve_mg(int nlevel, int iters, int ncycle, darray_t **values, darray_t **marks, const darray_t *init, unsigned char **nb_list, darray_t **flows_xy_x, darray_t **flows_xy_y, int scalexyz)
1309 {
1310         double curr_residual, prev_residual = 10000000000000.0;
1311         int cycle, level, i;
1312         int rc = -1;
1313
1314         for (cycle = 0; cycle < ncycle; cycle++) {
1315                 /* go down */
1316                 curr_residual = 0;
1317                 for (level = 0; level < nlevel-1; level++) {
1318                         for (i = 0; i < iters; i++)
1319                                 smooth(level, values[level], marks[level], init, nb_list[level]);
1320                         curr_residual += residual(level, values[level], marks[level], init, nb_list[level]);
1321                         if (level < scalexyz)
1322                                 rc = restrict(values[level], values[level+1], flows_xy_x[level], flows_xy_y[level]);
1323                         else
1324                                 rc = restrict(values[level], values[level+1], NULL, NULL);
1325                         if (rc < 0)
1326                                 goto error;
1327                 }
1328
1329                 /* end of going donw */
1330                 for (i = 0; i < iters*5; i++)
1331                         smooth(nlevel-1, values[nlevel-1], marks[nlevel-1], init, nb_list[nlevel-1]);
1332
1333                 /* go up */
1334                 for (level = nlevel-2; level >= 0; level--) {
1335                         if (level < scalexyz)
1336                                 rc = prolong(values[level], values[level+1], flows_xy_x[level], flows_xy_y[level]);
1337                         else
1338                                 rc = prolong(values[level], values[level+1], NULL, NULL);
1339                         if (rc < 0)
1340                                 goto error;
1341                         for (i = 0; i < iters; i++)
1342                                 smooth(level, values[level], marks[level], init, nb_list[level]);
1343                 }
1344
1345                 /* end if residual raises again */
1346 //              if (prev_residual < curr_residual)
1347 //                      break;
1348                 curr_residual = prev_residual;
1349         }
1350
1351         return 0;
1352
1353 error:
1354         return rc;
1355 }
1356
1357 /* colorize image array using image_mark
1358  *
1359  * image:
1360  *      first dimansion = width
1361  *      second dimension = height
1362  *      thrid dimension = components (first component is luminance, other components are features, like u, v)
1363  *      fourth dimension = time
1364  *
1365  * image_mark:
1366  *      first dimansion = width
1367  *      second dimension = height
1368  *      fourth dimension = time
1369  */
1370 darray_t *colorize(const darray_t *image, const darray_t *image_mark, const darray_t *flow_x, const darray_t *flow_y, const darray_t *flow_ix, const darray_t *flow_iy, int inner_iter, int outer_iter, int scalexyz)
1371 {
1372         unsigned char **nb_list = NULL;
1373         darray_t **values = NULL, **marks = NULL, *init = NULL, **flows_x = NULL, **flows_y = NULL, **flows_ix = NULL, **flows_iy = NULL, **flows_xy_x = NULL, **flows_xy_y = NULL;
1374         darray_t *result = NULL, *luminance = NULL;
1375         int dimensions[4];
1376         int w, h, k, ww, hh, kk, last_kk, comp, d, max_depth, temp, z, c;
1377         int rc = -1;
1378
1379         ASSERT(darrayGetNumberOfDimensions(image) == 4, "image array must have 4 dimensions");
1380         w = darrayGetDimensions(image)[0];
1381         h = darrayGetDimensions(image)[1];
1382         comp = darrayGetDimensions(image)[2];
1383         k = darrayGetDimensions(image)[3];
1384         ASSERT(comp > 0, "image array requires at least one component (Y-component)");
1385
1386         ASSERT(darrayGetNumberOfDimensions(image_mark) == 3, "image_mark array must have 3 dimensions");
1387         ASSERT(w == darrayGetDimensions(image_mark)[0], "image_mark and image must both have same x-size");
1388         ASSERT(h == darrayGetDimensions(image_mark)[1], "image_mark and image must both have same y-size");
1389         ASSERT(k == darrayGetDimensions(image_mark)[2], "image_mark and image must both have same z-size");
1390
1391         /* Calculate maximum depth
1392          * Scale down dimensions until at least 2x2 remains
1393          */
1394         temp = min(w, h);
1395         for (max_depth = 0; temp > 2; max_depth++)
1396                 temp=(temp+1)>>1;
1397         ASSERT(max_depth >= 2, "image too small");
1398
1399         nb_list = calloc(max_depth, sizeof(struct neighbors *));
1400         if (!nb_list)
1401                 goto error;
1402         marks = calloc(max_depth, sizeof(darray_t *));
1403         if (!marks)
1404                 goto error;
1405         ASSERT(marks, "no memory");
1406         values = calloc(max_depth, sizeof(darray_t *));
1407         if (!values)
1408                 goto error;
1409         ASSERT(values, "no memory");
1410         flows_x = calloc(max_depth, sizeof(darray_t *));
1411         if (!flows_x)
1412                 goto error;
1413         ASSERT(flows_x, "no memory");
1414         flows_y = calloc(max_depth, sizeof(darray_t *));
1415         if (!flows_y)
1416                 goto error;
1417         ASSERT(flows_y, "no memory");
1418         flows_ix = calloc(max_depth, sizeof(darray_t *));
1419         if (!flows_ix)
1420                 goto error;
1421         ASSERT(flows_ix, "no memory");
1422         flows_iy = calloc(max_depth, sizeof(darray_t *));
1423         if (!flows_iy)
1424                 goto error;
1425         ASSERT(flows_iy, "no memory");
1426         if (scalexyz) {
1427                 /* the flows_xy are scaled in xy in the current level */
1428                 flows_xy_x = calloc(max_depth, sizeof(darray_t *));
1429                 if (!flows_xy_x)
1430                         goto error;
1431                 flows_xy_y = calloc(max_depth, sizeof(darray_t *));
1432                 if (!flows_xy_y)
1433                         goto error;
1434         }
1435         dimensions[0] = w;
1436         dimensions[1] = h;
1437         dimensions[2] = k;
1438         luminance = darrayCreate(3, dimensions);
1439         if (!luminance)
1440                 goto error;
1441         /* generate sparse multigrid */
1442         last_kk = kk = k;
1443         printf("-> prepare sparse grid");
1444         for (d = 0, ww=w, hh=h; d < max_depth; d++, ww=(ww+1)>>1, hh=(hh+1)>>1) {
1445                 /* create temporary arrays
1446                  * copy fist array to level 0 or shrink previous level
1447                  */
1448                 printf(" #%d(w=%d h=%d k=%d)", d, ww, hh, kk); fflush(stdout);
1449                 dimensions[0] = ww;
1450                 dimensions[1] = hh;
1451                 dimensions[2] = last_kk;
1452                 marks[d] = darrayCreate(3, dimensions);
1453                 if (!marks[d])
1454                         goto error;
1455                 dimensions[0] = ww;
1456                 dimensions[1] = hh;
1457                 dimensions[2] = last_kk-1;
1458                 flows_x[d] = darrayCreate(3, dimensions);
1459                 if (!flows_x[d])
1460                         goto error;
1461                 flows_y[d] = darrayCreate(3, dimensions);
1462                 if (!flows_y[d])
1463                         goto error;
1464                 flows_ix[d] = darrayCreate(3, dimensions);
1465                 if (!flows_ix[d])
1466                         goto error;
1467                 flows_iy[d] = darrayCreate(3, dimensions);
1468                 if (!flows_iy[d])
1469                         goto error;
1470                 if (d < scalexyz) {
1471                         dimensions[0] = (ww+1)>>1;
1472                         dimensions[1] = (hh+1)>>1;
1473                         dimensions[2] = kk-1;
1474                         flows_xy_x[d] = darrayCreate(3, dimensions);
1475                         if (!flows_xy_x[d])
1476                                 goto error;
1477                         flows_xy_y[d] = darrayCreate(3, dimensions);
1478                         if (!flows_xy_y[d])
1479                                 goto error;
1480                 }
1481                 if (d == 0) {
1482                         for (z = 0; z < kk; z++)
1483                                 memcpy(darrayGetPr(luminance) + z*w*h, darrayGetPr(image) + z*w*h*comp, w*h*sizeof(double));
1484                         if (flow_x && flow_y && flow_ix && flow_iy) {
1485                                 memcpy(darrayGetPr(flows_x[d]), darrayGetPr(flow_x), w*h*(kk-1)*sizeof(double));
1486                                 memcpy(darrayGetPr(flows_y[d]), darrayGetPr(flow_y), w*h*(kk-1)*sizeof(double));
1487                                 memcpy(darrayGetPr(flows_ix[d]), darrayGetPr(flow_ix), w*h*(kk-1)*sizeof(double));
1488                                 memcpy(darrayGetPr(flows_iy[d]), darrayGetPr(flow_iy), w*h*(kk-1)*sizeof(double));
1489                         }
1490                         memcpy(darrayGetPr(marks[d]), darrayGetPr(image_mark), w*h*kk*sizeof(double));
1491                         /* invert, so 0 = marked */
1492                         quantize(marks[d], 0.5, 0, 1);
1493                 } else {
1494                         rc = shrink_xy(luminance, NULL);
1495                         if (rc < 0)
1496                                 goto error;
1497                         rc = shrink_xy(marks[d-1], marks[d]);
1498                         if (rc < 0)
1499                                 goto error;
1500                         rc = shrink_xy(flows_x[d-1], flows_x[d]);
1501                         if (rc < 0)
1502                                 goto error;
1503                         rc = shrink_xy(flows_y[d-1], flows_y[d]);
1504                         if (rc < 0)
1505                                 goto error;
1506                         rc = shrink_xy(flows_ix[d-1], flows_ix[d]);
1507                         if (rc < 0)
1508                                 goto error;
1509                         rc = shrink_xy(flows_iy[d-1], flows_iy[d]);
1510                         if (rc < 0)
1511                                 goto error;
1512                         /* shrink length of flow vector on smaller grid.
1513                          * This is not performed when comparing with
1514                          * the original algorithm. (it is a bug there)
1515                          */
1516                         multiply(flows_x[d], 0.5);
1517                         multiply(flows_y[d], 0.5);
1518                         multiply(flows_ix[d], 0.5);
1519                         multiply(flows_iy[d], 0.5);
1520                         if (d-1 < scalexyz) { /* process if previous d shall be shrinked in z direction */
1521                                 /* shrink time */
1522                                 rc = shrink_z(luminance, NULL, flows_x[d], flows_y[d]);
1523                                 if (rc < 0)
1524                                         goto error;
1525                                 rc = shrink_z(marks[d], NULL, flows_x[d], flows_y[d]);
1526                                 if (rc < 0)
1527                                         goto error;
1528                                 rc = shrink_flow_z(flows_x[d], flows_y[d], NULL, NULL);
1529                                 if (rc < 0)
1530                                         goto error;
1531                                 rc = shrink_flow_z(flows_ix[d], flows_iy[d], NULL, NULL);
1532                                 if (rc < 0)
1533                                         goto error;
1534                         }
1535                         /* If any of the restricted pixles is 0 (marked),
1536                          * the result must be 0 (marked) too.
1537                          */
1538                         quantize(marks[d], 0.9999, 1, 0);
1539                 }
1540                 if (d < scalexyz) {
1541                         /* create x and y shrinked flow vectors for restriction and prolonging process */
1542                         rc = shrink_xy(flows_x[d], flows_xy_x[d]);
1543                         if (rc < 0)
1544                                 goto error;
1545                         rc = shrink_xy(flows_y[d], flows_xy_y[d]);
1546                         if (rc < 0)
1547                                 goto error;
1548                         multiply(flows_xy_x[d], 0.5);
1549                         multiply(flows_xy_y[d], 0.5);
1550                 }
1551
1552                 /* generate values array */
1553                 dimensions[0] = ww;
1554                 dimensions[1] = hh;
1555                 dimensions[2] = kk;
1556                 values[d] = darrayCreate(3, dimensions);
1557                 if (!values[d])
1558                         goto error;
1559
1560                 /* generate array of neighbors */
1561 #ifndef TEST_NO_RENDER
1562                 nb_list[d] = gen_neighbor(ww, hh, kk, flows_x[d], flows_y[d], flows_ix[d], flows_iy[d]);
1563                 if (!nb_list[d])
1564                         goto error;
1565                 /* weighten */
1566                 weighten(luminance, nb_list[d]);
1567 #endif
1568
1569                 last_kk = kk;
1570                 if (d < scalexyz)
1571                         kk = (kk+1)>>1;
1572         }
1573         darrayDestroy(luminance);
1574         luminance = NULL;
1575         printf("\n");
1576
1577         dimensions[0] = w;
1578         dimensions[1] = h;
1579         dimensions[2] = k;
1580         init = darrayCreate(3, dimensions);
1581         if (!init)
1582                 goto error;
1583
1584         /* result array for all components */
1585         dimensions[0] = w;
1586         dimensions[1] = h;
1587         dimensions[2] = comp;
1588         dimensions[3] = k;
1589         result = darrayCreate(4, dimensions);
1590         if (!result)
1591                 goto error;
1592
1593 #ifdef TEST_PYRAMID
1594         dimensions[0] = w;
1595         dimensions[1] = h;
1596         dimensions[2] = k;
1597         luminance = darrayCreate(3, dimensions);
1598         for (z = 0; z < k; z++) {
1599                 memcpy(darrayGetPr(luminance) + z*w*h, darrayGetPr(image) + z*w*h*comp, w*h*sizeof(double));
1600                 memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(image) + z*w*h*comp, w*h*sizeof(double));
1601 #ifdef TEST_FLOW
1602                 draw_flow(values[0], flows_x[0], flows_y[0], flows_ix[0], flows_iy[0]);
1603 #endif
1604 #ifdef TEST_MARKED
1605                 paste(marks[0], values[0], 0, 0, 0);
1606 #endif
1607
1608         }
1609         for (d = 1; d < max_depth; d++) {
1610                 rc = shrink_xy(luminance, NULL);
1611                 if (rc < 0)
1612                         goto error;
1613                 if (d-1 < scalexyz)
1614                         shrink_z(luminance, NULL, flows_xy_x[d-1], flows_xy_y[d-1]);
1615                 paste(luminance, values[0], 0, 0, 0);
1616 #ifdef TEST_FLOW
1617                 draw_flow(values[0], flows_x[d], flows_y[d], flows_ix[d], flows_iy[d]);
1618 #endif
1619 #ifdef TEST_MARKED
1620                 paste(marks[d], values[0], 0, 0, 0);
1621 #endif
1622         }
1623         darrayDestroy(luminance);
1624         luminance = NULL;
1625         /* copy test pyramid image as Y-component */
1626         for (z = 0; z < k; z++)
1627                 memcpy(darrayGetPr(result) + z*w*h*comp, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1628 #else
1629         /* copy Y-component */
1630         for (z = 0; z < k; z++)
1631                 memcpy(darrayGetPr(result) + z*w*h*comp, darrayGetPr(image) + z*w*h*comp, w*h*sizeof(double));
1632 #endif
1633
1634         printf("-> apply component to grid and solve");
1635         for (c = 1; c < comp; c++) {
1636                 int pixles, j;
1637                 double *value_ptr, *init_ptr, *mark_ptr;
1638
1639                 printf(" #%d", c); fflush(stdout);
1640                 /* apply component
1641                  * - copy component into value array (level 0), otherwise 0
1642                  * - use maked colors for init array, otherwise use 0
1643                  */
1644                 for (z = 0; z < k; z++)
1645                         memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(image) + c*w*h + z*w*h*comp, w*h*sizeof(double));
1646                 for (d = 1, ww=(w+1)>>1, hh=(h+1)>>1, kk=k; d < max_depth; d++, ww=(ww+1)>>1, hh=(hh+1)>>1) {
1647                         if (d-1 < scalexyz) /* scale kk, if last array was scaled */
1648                                 kk=(kk+1)>>1;
1649                         value_ptr = darrayGetPr(values[d]);
1650                         pixles = ww*hh*kk;
1651                         for (j = 0; j < pixles; j++)
1652                                 *value_ptr++ = 0.0;
1653                 }
1654                 value_ptr = darrayGetPr(values[0]);
1655                 init_ptr = darrayGetPr(init);
1656                 mark_ptr = darrayGetPr(image_mark);
1657                 pixles = w*h*k;
1658                 for (j = 0; j < pixles; j++) {
1659                         if (*mark_ptr++ > 0.5)
1660                                 *init_ptr++ = *value_ptr;
1661                         else
1662                                 *init_ptr++ = 0.0;
1663                         value_ptr++;
1664                 }
1665
1666 #ifndef TEST_NO_RENDER
1667                 /* colorize component */
1668                 int i;
1669                 for (i = 0; i < outer_iter; i++) {
1670                         rc = solve_mg(max_depth, inner_iter, 2, values, marks, init, nb_list, flows_xy_x, flows_xy_y, scalexyz);
1671                         if (rc < 0)
1672                                 goto error;
1673                         rc = solve_mg((max_depth+1) / 2, inner_iter, 2, values, marks, init, nb_list, flows_xy_x, flows_xy_y, scalexyz);
1674                         if (rc < 0)
1675                                 goto error;
1676                         rc = solve_mg(2, inner_iter, 2, values, marks, init, nb_list, flows_xy_x, flows_xy_y, scalexyz);
1677                         if (rc < 0)
1678                                 goto error;
1679                         rc = solve_mg(1, inner_iter, 4, values, marks, init, nb_list, flows_xy_x, flows_xy_y, scalexyz);
1680                         if (rc < 0)
1681                                 goto error;
1682                 }
1683 #endif
1684
1685 #ifdef TEST_PYRAMID
1686                 for (d = 1; d < max_depth; d++)
1687                         paste(values[d], values[0], 0, 0, 0);
1688 #endif
1689
1690                 /* apply colorized component */
1691                 for (z = 0; z < k; z++)
1692                         memcpy(darrayGetPr(result) + z*w*h*comp + c*w*h, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1693         }
1694         printf("\n");
1695
1696 #ifdef TEST_SHRINK_XY
1697         for (c = 0; c < comp; c++) {
1698                 darray_t *temp;
1699                 int dimensions[3] = {w, h, k};
1700                 for (z = 0; z < k; z++)
1701                         memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(result) + z*w*h*comp + c*w*h, w*h*sizeof(double));
1702                 dimensions[0] = (w+1)>>1;
1703                 dimensions[1] = (h+1)>>1;
1704                 temp = darrayCreate(3, dimensions);
1705                 ASSERT(temp, "no memory");
1706                 rc = shrink_xy(values[0], temp);
1707                 ASSERT(rc >= 0, "no memory");
1708                 paste(temp, values[0], 0, 0, 0);
1709                 darrayDestroy(temp);
1710                 for (z = 0; z < k; z++)
1711                         memcpy(darrayGetPr(result) + z*w*h*comp + c*w*h, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1712         }
1713 #endif
1714
1715 #ifdef TEST_SHRINK_Z
1716         for (c = 0; c < comp; c++) {
1717                 darray_t *temp;
1718                 int dimensions[3] = {w, h, k};
1719                 for (z = 0; z < k; z++)
1720                         memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(result) + z*w*h*comp + c*w*h, w*h*sizeof(double));
1721                 dimensions[2] = (k+1)>>1;
1722                 temp = darrayCreate(3, dimensions);
1723                 ASSERT(temp, "no memory");
1724                 rc = shrink_z(values[0], temp, flows_x[0], flows_y[0]);
1725                 ASSERT(rc >= 0, "no memory");
1726                 paste(temp, values[0], 0, 0, 0);
1727                 darrayDestroy(temp);
1728                 for (z = 0; z < k; z++)
1729                         memcpy(darrayGetPr(result) + z*w*h*comp + c*w*h, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1730         }
1731 #endif
1732
1733 #ifdef TEST_EXPAND_XY
1734         for (c = 0; c < comp; c++) {
1735                 darray_t *temp;
1736                 int dimensions[3] = {w, h, k};
1737                 for (z = 0; z < k; z++)
1738                         memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(result) + z*w*h*comp + c*w*h, w*h*sizeof(double));
1739                 dimensions[0] = (w+1)>>1;
1740                 dimensions[1] = (h+1)>>1;
1741                 temp = darrayCreate(3, dimensions);
1742                 ASSERT(temp, "no memory");
1743                 rc = shrink_xy(values[0], temp);
1744                 ASSERT(rc >= 0, "no memory");
1745                 expand_xy(temp, values[0]);
1746                 darrayDestroy(temp);
1747                 for (z = 0; z < k; z++)
1748                         memcpy(darrayGetPr(result) + z*w*h*comp + c*w*h, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1749         }
1750 #endif
1751
1752 #ifdef TEST_EXPAND_Z
1753         for (c = 0; c < comp; c++) {
1754                 darray_t *temp;
1755                 int dimensions[3] = {w, h, k};
1756                 for (z = 0; z < k; z++)
1757                         memcpy(darrayGetPr(values[0]) + z*w*h, darrayGetPr(result) + z*w*h*comp + c*w*h, w*h*sizeof(double));
1758                 dimensions[2] = (k+1)>>1;
1759                 temp = darrayCreate(3, dimensions);
1760                 ASSERT(temp, "no memory");
1761                 rc = shrink_z(values[0], temp, flows_x[0], flows_y[0]);
1762                 ASSERT(rc >= 0, "no memory");
1763                 rc = expand_z(temp, values[0], flows_x[0], flows_y[0]);
1764                 ASSERT(rc >= 0, "no memory");
1765                 darrayDestroy(temp);
1766                 for (z = 0; z < k; z++)
1767                         memcpy(darrayGetPr(result) + z*w*h*comp + c*w*h, darrayGetPr(values[0]) + z*w*h, w*h*sizeof(double));
1768         }
1769 #endif
1770
1771         rc = 0;
1772
1773 error:
1774         /* free memory */
1775         for (d = 0; d < max_depth; d++) {
1776                 if (nb_list)
1777                         free(nb_list[d]);
1778                 if (marks)
1779                         darrayDestroy(marks[d]);
1780                 if (values)
1781                         darrayDestroy(values[d]);
1782                 if (flows_xy_x)
1783                         darrayDestroy(flows_xy_x[d]);
1784                 if (flows_xy_y)
1785                         darrayDestroy(flows_xy_y[d]);
1786                 if (flows_x)
1787                         darrayDestroy(flows_x[d]);
1788                 if (flows_y)
1789                         darrayDestroy(flows_y[d]);
1790                 if (flows_ix)
1791                         darrayDestroy(flows_ix[d]);
1792                 if (flows_iy)
1793                         darrayDestroy(flows_iy[d]);
1794         }
1795         free(nb_list);
1796         free(marks);
1797         free(values);
1798         free(flows_xy_x);
1799         free(flows_xy_y);
1800         free(flows_x);
1801         free(flows_y);
1802         free(flows_ix);
1803         free(flows_iy);
1804         darrayDestroy(init);
1805         darrayDestroy(luminance);
1806         if (rc < 0)
1807                 darrayDestroy(result);
1808
1809         return (rc < 0) ? NULL : result;
1810 }