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