/*			*/
/*	幾何変換	*/
/*			*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "vision.h"

/*
	座標(x,y)における画素値
	
	画像 image[][]
	座標系：原点 = 格子点 [oi,oj] 
		格子点間隔 dx, dy
*/

double pixelvalue(double *image,
		  int width, int height,
		  int swidth, int sheight,
		  int oi, int oj, double dx, double dy,
		  double x, double y)
{
  int		i, j;
  double	sx, sy, rx, ry;
  double	c00, c10, c01, c11;
  double	v;

  sx = floor(x/dx);
  sy = floor(y/dy);

  /* [i,j] = (x,y) の含まれるブロックの左下の格子点 */
  i = sx + oi;	
  j = sy + oj;
  
  if ( i<0 || i>width-1 || j<0 || j>height-1 ) {
    return 0;
    /* 画像の範囲外の場合 */
  }
  
  rx = x - sx*dx;	/* x/dx  の余り */
  ry = y - sy*dy;	/* y/dy の余り */
  
  c00 = (dx-rx)*(dy-ry)/dx/dy;
  c10 =    (rx)*(dy-ry)/dx/dy;
  c01 = (dx-rx)*   (ry)/dx/dy;
  c11 =    (rx)*   (ry)/dx/dy;
  
  v = c00 * ELEMENT(image,swidth,sheight,i,    j)
    + c10 * ELEMENT(image,swidth,sheight,i+1,  j)
    + c01 * ELEMENT(image,swidth,sheight,i,  j+1)
    + c11 * ELEMENT(image,swidth,sheight,i+1,j+1);
  
  return v;
}

/*
	画像の並進移動
	
	画像 image[][]
	座標系：原点 = 格子点 [oi,oj] 
		格子点間隔 dx, dy

	x,y 方向に distx,disty 並進移動
*/

void imageTranslation(int width, int height,
		      int swidth, int sheight,
		      int oi, int oj, double dx, double dy,
		      double *input, double distx, double disty,
		      double *output)
{
  int		i, j;
  double	xnew, ynew, x, y;
  
  for (i=0; i<width; i++) {
    for (j=0; j<height; j++) {
      /* 画像 output[][] の格子点[i,j]に対応する座標 (xnew,ynew) */
      xnew = (i-oi)*dx;
      ynew = (j-oj)*dy;
      /* output[][]上の(xnew,ynew) と input[][]上の(x,y) 間の座標変換 */
      x = xnew - distx;
      y = ynew - disty;
      
      ELEMENT(output,swidth,sheight,i,j) =
	pixelvalue(input,width,height,swidth,sheight,oi,oj,dx,dy,x,y);
    }
  }
}

/*
	画像の回転移動
	
	画像 image[][]
	座標系：原点 = 格子点 [oi,oj] 
		格子点間隔 dx, dy

	原点周りに角度 a 回転
	余弦 cos(a) と正弦 sin(a) を指定する．
*/

void imageRotation(int width, int height,
		   int swidth, int sheight,
		   int oi, int oj, double dx, double dy,
		   double *input, double cosa, double sina,
		   double *output)
{
  int		i, j;
  double	xnew, ynew, x, y;
  
  for (i=0; i<width; i++) {
    for (j=0; j<height; j++) {
      /* 画像 output[][] の格子点[i,j]に対応する座標 (xnew,ynew) */
      xnew = (i-oi)*dx;
      ynew = (j-oj)*dy;
      /* output[][]上の(xnew,ynew) と input[][]上の(x,y) 間の座標変換 */
      x =  cosa*xnew + sina*ynew;
      y = -sina*xnew + cosa*ynew;
      
      ELEMENT(output,swidth,sheight,i,j) =
	pixelvalue(input,width,height,swidth,sheight,oi,oj,dx,dy,x,y);
    }
  }
}

/*
	画像の拡大/縮小
	
	画像 image[][]
	座標系：原点 = 格子点 [oi,oj] 
		格子点間隔 dx, dy

	原点を中心に拡大率 expand で拡大
*/

void imageExpansion(int width, int height,
		    int swidth, int sheight,
		    int oi, int oj, double dx, double dy,
		    double *input, double expand,
		    double *output)
{
  int		i, j;
  double	xnew, ynew, x, y;
  
  for (i=0; i<width; i++) {
    for (j=0; j<height; j++) {
      /* 画像 output[][] の格子点[i,j]に対応する座標 (xnew,ynew) */
      xnew = (i-oi)*dx;
      ynew = (j-oj)*dy;
      /* output[][]上の(xnew,ynew) と input[][]上の(x,y) 間の座標変換 */
      x = xnew/expand;
      y = ynew/expand;
      
      ELEMENT(output,swidth,sheight,i,j) =
	pixelvalue(input,width,height,swidth,sheight,oi,oj,dx,dy,x,y);
    }
  }
}