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