/* affine_map_tri.c   
 * map a triangle with vertices x0,y0  x1,y1      x2,y2
 * onto a right regular triangle 0,0   1,0 (in a) 0,1 (in b)
 * and inverse map
 *
 *                   x2,y2
 *                     o***
 *                     *   ***
 *                    *       ***o x1,y1
 *                   *    ****
 *           x0,y0  o****
 *
 *       b
 *  0,1  o
 *       **
 *       *  *
 *       *   *
 *       *     *
 *  0,0  o*******o  a
 *              1,0
 */

#include <stdio.h>
#include <stdlib.h>
#undef abs
#define abs(a) ((a)<0?(-(a)):(a))


/*  the mapping from  a,b  to  x,y  is:
 * x = x0 + (x1-x0)*a + (x2-x0)*b   a in 0 .. 1
 * y = y0 + (y1-y0)*a + (y2-y0)*b   b in 0 .. 1-a
 *
 * This mapping: does not preserve area, does not preserve angle,
 *               is a full covering
 */

static void fabtoxy(double a, double b, double *x, double *y,
            double x0, double y0, double x1, double y1, double x2, double y2)
{
  *x = x0 + (x1-x0)*a + (x2-x0)*b;
  *y = y0 + (y1-y0)*a + (y2-y0)*b;
}

/* the mapping from x,y  to a,b  x,y in x0,y0 x1,y1 x2,y2
 *
 *     y *(x2-x0)-x *(y2-y0)+x0*y2-y0*x2
 * a = ---------------------------------
 *     y1*(x2-x0)-x1*(y2-y0)+x0*y2-y0*x2
 *
 *     x *(y1-y0)-y *(x1-x0)+x1*y0-y1*x0
 * b = ---------------------------------
 *     x2*(y1-y0)-y2*(x1-x0)+x1*y0-y1*x0
 */

static void fxytoab(double x, double y, double *a, double *b,
            double x0, double y0, double x1, double y1, double x2, double y2)
{
  *a = (y *(x2-x0)-x *(y2-y0)+x0*y2-y0*x2)/
       (y1*(x2-x0)-x1*(y2-y0)+x0*y2-y0*x2);
  *b = (x *(y1-y0)-y *(x1-x0)+x1*y0-y1*x0)/
       (x2*(y1-y0)-y2*(x1-x0)+x1*y0-y1*x0);
}

int main(int argc, char * argv[])
{
  double a, b, aa, bb, x, y, xx, yy;
  double xt[7], yt[7];
  int i, j, k;
  double x0 = 3.0;
  double y0 = 2.0;
  double x1 = 8.0;
  double y1 = 5.0;
  double x2 = 6.0;
  double y2 = 9.0;
  int n = 3;

  printf("test_affine_map.c running \n");
  printf("x0=%f,y0=%f  to  a=0,b=0 \n",   x0, y0);
  printf("x1=%f,y1=%f  to  a=1,b=0 \n",   x1, y1);
  printf("x2=%f,y2=%f  to  a=0,b=1 \n\n", x2, y2);

  for(i=0; i<=n; i++)
  {
    a = (double)i/n;
    for(j=0; j<=(n-i); j++)
    {
      b = (double)j/n;
      fabtoxy(a, b, &x, &y,
	      x0, y0, x1, y1, x2, y2);
      fxytoab(x, y, &aa, &bb,
              x0, y0, x1, y1, x2, y2);
      printf("fabtoxy %f,%f -> %f,%f \n", a, b, x, y);
      if(abs(a-aa)>0.00001 || abs(b-bb)>0.00001)
	printf("map error aa=%f, bb=%f \n\n", aa, bb);
    }
  }
  printf("\n");

  xt[0]=x0; yt[0]=y0;
  xt[1]=x1; yt[1]=y1;
  xt[2]=x2; yt[2]=y2;
  xt[3]=(x0+x1)/2.0; yt[3]=(y0+y1)/2.0;
  xt[4]=(x1+x2)/2.0; yt[4]=(y1+y2)/2.0;
  xt[5]=(x2+x0)/2.0; yt[5]=(y2+y0)/2.0;
  xt[6]=(x0+x1+x2)/3.0; yt[6]=(y0+y1+y2)/3.0;

  for(k=0; k<7; k++)
  {
    x = xt[k];
    y = yt[k];
    fxytoab(x, y, &a, &b,
            x0, y0, x1, y1, x2, y2);
    fabtoxy(a, b, &xx, &yy,
	    x0, y0, x1, y1, x2, y2);
    printf("fxytoab %f,%f -> %f,%f \n", x, y, a, b);
    if(abs(x-xx)>0.00001 || abs(y-yy)>0.00001)
       printf("map error xx=%f, yy=%f \n\n", xx, yy);
  }
  return 0;
} /* end test_affine_map.c */