import java.awt.*;
import java.awt.event.*;
import java.util.*;
import java.text.*;
import java.applet.*;



public class Epi extends Applet implements ActionListener{

private TextField txtRotY;
private Button btMovePoint;
private Button btClose;
private Button btChangeView;
private Button btShowTrace;

private int   showTrace = 0;  // don't show epipolar plane traced

private float RotY = 0.0f;
private float Old_RotY = 0.0f;

private int       maxStep = 15;
private float[][] uvflist = new float[maxStep][3];
private int       uvf_idx = 0;

private float[][] img1 = new float[4][3];
private float[][] img2 = new float[4][3];
private float[]   cam1pos = new float[3];
private float[]   cam2pos = new float[3];
private float[]   norm1   = new float[3];
private float[]   norm2   = new float[3];
private float[]   uv1  = new float[2];
private float[]   uv2  = new float[2];

private float[][] Pt3D = new float[maxStep][3];
private float[]   Orig_Pt3D = new float[3];

private float focal = 5.0f;

private int stepNo = 0;

private float disp_scale   = 1.2f;
private float disp_offx    = 200.0f;
private float disp_offy    = 320.0f;

private float view_x = 0.0f;
private float view_y = -10.0f;
private float view_z = 0.f;



private float[][] view_rot = { {1.0f,  0.0f,  0.0f},
                               {0.0f,  0.0f,  -1.0f},
                               {0.0f,  1.0f,  0.0f}
                             };  


/***
private float[][] view_rot = { {1.0f,  0.0f,  0.0f},
                               {0.0f,  1.0f,  0.0f},
                               {0.0f,  0.0f,  1.0f}
                             };  
***/


private int view_rot_no = 0;

 
public static void main(String[] args)
{
Frame f = new MyFrameWithExitHandling("Epipolar Geometry");
f.setLayout(new BorderLayout());

Epi epi = new Epi();

f.add("Center", epi);
f.setSize(500,500);

epi.init();
f.setVisible(true);

f.show();
}



public void init()
{
txtRotY = new TextField("0", 3);
btClose = new Button("Close");
btMovePoint = new Button("MovePoint");
btChangeView = new Button("ChangeView");
btShowTrace  = new Button("ShowTrace");

setBackground(Color.white);
setForeground(Color.black);

Panel p = new Panel();

p.setLayout(new GridLayout(1,1));

p.add(new Label("Rotate Right Plane by (angle)"));
p.add(txtRotY);

setLayout(new FlowLayout());
add(p);

add(btClose);
btClose.addActionListener(this);

add(btMovePoint);
btMovePoint.addActionListener(this);

add(btChangeView);
btChangeView.addActionListener(this);

add(btShowTrace);
btShowTrace.addActionListener(this);

// define the default image planes
img1[0][0] = -3.0f;
img1[0][1] = -3.0f;
img1[0][2] =  0.0f;
img1[1][0] =  3.0f;
img1[1][1] = -3.0f;
img1[1][2] =  0.0f;
img1[2][0] =  3.0f;
img1[2][1] =  3.0f;
img1[2][2] =  0.0f;
img1[3][0] = -3.0f;
img1[3][1] =  3.0f;
img1[3][2] =  0.0f;

img2[0][0] = -3.0f;
img2[0][1] = -3.0f;
img2[0][2] =  0.0f;
img2[1][0] =  3.0f;
img2[1][1] = -3.0f;
img2[1][2] =  0.0f;
img2[2][0] =  3.0f;
img2[2][1] =  3.0f;
img2[2][2] =  0.0f;
img2[3][0] = -3.0f;
img2[3][1] =  3.0f;
img2[3][2] =  0.0f;

cam1pos[0] = -6.0f;
cam1pos[1] =  0.0f;
cam1pos[2] =  0.0f;

cam2pos[0] =  6.0f;
cam2pos[1] =  0.0f;
cam2pos[2] =  0.0f;

int i;

for (i=0; i<4; i++) {
   img1[i][0] += cam1pos[0];
   img1[i][1] += cam1pos[1];
   img1[i][2] += (cam1pos[2] + focal);
   img2[i][0] += cam2pos[0];
   img2[i][1] += cam2pos[1];
   img2[i][2] += (cam2pos[2] + focal);
} 


Pt3D[0][0] = 10.0f;
Pt3D[0][1] = -15.0f;
Pt3D[0][2] = 40.0f;

Orig_Pt3D[0] = Pt3D[0][0];
Orig_Pt3D[1] = Pt3D[0][1];
Orig_Pt3D[2] = Pt3D[0][2];

}



public void actionPerformed(ActionEvent e)
{
String actionCommand = e.getActionCommand();
if (e.getSource() instanceof Button)
   if (actionCommand.equals("Close"))
      System.exit(0);
   else if (actionCommand.equals("MovePoint"))
      MovePoint();
   else if (actionCommand.equals("ChangeView"))
      ChangeView();
   else if (actionCommand.equals("ShowTrace")) {
      if (showTrace == 0)
         showTrace = 1;
      else
         showTrace = 0;
   }

RotY = (float)Integer.parseInt(txtRotY.getText().trim());

RotY = -RotY;

if ((int)RotY != (int)Old_RotY) {
   //   System.out.println("new angle = " + (-RotY));
   Old_RotY = RotY;
   stepNo = 0;
   uvf_idx = 0;
   //Pt3D[0] = Orig_Pt3D[0];
   //Pt3D[1] = Orig_Pt3D[1];
   //Pt3D[2] = Orig_Pt3D[2];

   //repaint();

   double rad = (double)RotY * Math.PI / 180.0;
   double[] temp = new double[3];

   for (int i=0; i<4; i++) {
      temp[0] = Math.cos(rad)*(img2[i][0]-cam2pos[0]) + Math.sin(rad)*(img2[i][2]-cam2pos[2]);
      temp[1] = img2[i][1]-cam2pos[1];
      temp[2] = -Math.sin(rad)*(img2[i][0]-cam2pos[0]) + Math.cos(rad)*(img2[i][2]-cam2pos[2]); 

      img2[i][0] = (float)temp[0] + cam2pos[0];
      img2[i][1] = (float)temp[1] + cam2pos[1];
      img2[i][2] = (float)temp[2] + cam2pos[2];
   }



}

}



public void MovePoint()
{

if (stepNo < maxStep-1) {
   stepNo++;
   uvf_idx++;
   Pt3D[stepNo][0] = Pt3D[stepNo-1][0] + (cam1pos[0] - Pt3D[0][0]) * 0.06f;
   Pt3D[stepNo][1] = Pt3D[stepNo-1][1] + (cam1pos[1] - Pt3D[0][1]) * 0.06f;
   Pt3D[stepNo][2] = Pt3D[stepNo-1][2] + (cam1pos[2] - Pt3D[0][2]) * 0.06f;
   
} else {
   Pt3D[0][0] = Orig_Pt3D[0];
   Pt3D[0][1] = Orig_Pt3D[1];
   Pt3D[0][2] = Orig_Pt3D[2];
   stepNo  = 1;
   uvf_idx = 1;
}


repaint();
}


public void ChangeView()
{
if (view_rot_no > 1)
   view_rot_no = 0;

if (view_rot_no == 0) {
   view_rot[0][0] = 1.0f; 
   view_rot[0][1] = 0.0f;
   view_rot[0][2] = 0.0f;
   view_rot[1][0] = 0.0f; 
   view_rot[1][1] =  (float)Math.cos(-0.0*Math.PI/180.0);
   view_rot[1][2] = -(float)Math.sin(-0.0*Math.PI/180.0);
   view_rot[2][0] = 0.0f; 
   view_rot[2][1] =  (float)Math.sin(-0.0*Math.PI/180.0);
   view_rot[2][2] =  (float)Math.cos(-0.0*Math.PI/180.0);
 
   view_x = 0.0f;
   view_y = -3.0f;
   view_z = -5.0f;
  
   disp_scale   = 2.2f;
   disp_offx    = 150.0f;
   disp_offy    = 300.0f;
} 
else if (view_rot_no == 1) {
   view_rot[0][0] = 1.0f; 
   view_rot[0][1] = 0.0f;
   view_rot[0][2] = 0.0f;
   view_rot[1][0] = 0.0f; 
   view_rot[1][1] = 0.0f;
   view_rot[1][2] = -1.0f;
   view_rot[2][0] = 0.0f; 
   view_rot[2][1] = 1.0f;
   view_rot[2][2] = 0.0f;
 
   view_x = 0.0f;
   view_y = -10.0f;
   view_z = 0.0f;

   disp_scale   = 1.2f;
   disp_offx    = 200.0f;
   disp_offy    = 320.0f;
}

view_rot_no++;

repaint();

}







public void paint(Graphics g)
{
// rotate the second image plane w.r.t. Y axis
int i;
double[] temp = new double[3];

 
// calculte the image plane normal vectors
float[] p12 = new float[3];
float[] p32 = new float[3];
   
   // image plane 1 

   /***********
   p12[0] = img1[0][0] - img1[1][0];
   p12[1] = img1[0][1] - img1[1][1];
   p12[2] = img1[0][2] - img1[1][2];
   p32[0] = img1[2][0] - img1[1][0];
   p32[1] = img1[2][1] - img1[1][1];
   p32[2] = img1[2][2] - img1[1][2];
   norm1[0] = p12[1]*p32[2] - p12[2]*p32[1];
   norm1[1] = p12[2]*p32[0] - p12[0]*p32[2];
   norm1[2] = p12[0]*p32[1] - p12[1]*p32[0];     
   float mag = (float)Math.sqrt(norm1[0]*norm1[0] + norm1[1]*norm1[1]+ norm1[2]*norm1[2]);

   norm1[0] = norm1[0] / mag;
   norm1[1] = norm1[1] / mag;
   norm1[2] = norm1[2] / mag;
   *************/

   float meanx = 0.0f;
   float meany = 0.0f;
   float meanz = 0.0f;
   for (i=0; i<4; i++) {
      meanx += img1[i][0];
      meany += img1[i][1];
      meanz += img1[i][2];
   }
   meanx /= 4;
   meany /= 4;
   meanz /= 4;
   
   norm1[0] = (meanx - cam1pos[0])/focal;
   norm1[1] = (meany - cam1pos[1])/focal;
   norm1[2] = (meanz - cam1pos[2])/focal;
   
 



 
   // image plane 2

   /***************
   p12[0] = img2[0][0] - img2[1][0];
   p12[1] = img2[0][1] - img2[1][1];
   p12[2] = img2[0][2] - img2[1][2];
   p32[0] = img2[2][0] - img2[1][0];
   p32[1] = img2[2][1] - img2[1][1];
   p32[2] = img2[2][2] - img2[1][2];
   norm2[0] = p12[1]*p32[2] - p12[2]*p32[1];
   norm2[1] = p12[2]*p32[0] - p12[0]*p32[2];
   norm2[2] = p12[0]*p32[1] - p12[1]*p32[0];     
   mag = (float)Math.sqrt(norm2[0]*norm2[0] + norm2[1]*norm2[1]+ norm2[2]*norm2[2]);

   norm2[0] = norm2[0] / mag;
   norm2[1] = norm2[1] / mag;
   norm2[2] = norm2[2] / mag;
   *****************/


   meanx = 0.0f;
   meany = 0.0f;
   meanz = 0.0f;
   for (i=0; i<4; i++) {
      meanx += img2[i][0];
      meany += img2[i][1];
      meanz += img2[i][2];
   }
   meanx /= 4;
   meany /= 4;
   meanz /= 4;
   
   norm2[0] = (meanx - cam2pos[0])/focal;
   norm2[1] = (meany - cam2pos[1])/focal;
   norm2[2] = (meanz - cam2pos[2])/focal;

 




// draw the first image plane
g.setColor(Color.gray);
Polygon poly1 = new Polygon();

float[] tempuv;

for (i=0; i<4; i++) {
   tempuv = Perspective(img1[i][0], img1[i][1], img1[i][2]);
   poly1.addPoint((int)tempuv[0], 
                  (int)tempuv[1]);
}
g.drawPolygon(poly1);

// draw the second image plane
Polygon poly2 = new Polygon();
for (i=0; i<4; i++) {
   tempuv = Perspective(img2[i][0], img2[i][1], img2[i][2]);
   poly2.addPoint((int)tempuv[0], 
                  (int)tempuv[1]);
}
g.drawPolygon(poly2);


// project 3D point onto image plane
float lambda;

  // image plane 1

lambda = focal / ( norm1[0]*(Pt3D[stepNo][0] - cam1pos[0]) +
                   norm1[1]*(Pt3D[stepNo][1] - cam1pos[1]) + 
                   norm1[2]*(Pt3D[stepNo][2] - cam1pos[2]));

float[] pttemp = new float[3];
 
pttemp[0] = cam1pos[0] + lambda * (Pt3D[stepNo][0] - cam1pos[0]);
pttemp[1] = cam1pos[1] + lambda * (Pt3D[stepNo][1] - cam1pos[1]);
pttemp[2] = cam1pos[2] + lambda * (Pt3D[stepNo][2] - cam1pos[2]);
 

//System.out.println("3D location on image plane1 = " + pttemp[0] + " " + pttemp[1] + " " + //pttemp[2]);

tempuv = Perspective(pttemp[0], pttemp[1], pttemp[2]);

uv1[0] = tempuv[0];
uv1[1] = tempuv[1];


  // image plane 2

lambda = focal / ( norm2[0]*(Pt3D[stepNo][0] - cam2pos[0]) +
                   norm2[1]*(Pt3D[stepNo][1] - cam2pos[1]) + 
                   norm2[2]*(Pt3D[stepNo][2] - cam2pos[2]));
 
pttemp[0] = cam2pos[0] + lambda * (Pt3D[stepNo][0] - cam2pos[0]);
pttemp[1] = cam2pos[1] + lambda * (Pt3D[stepNo][1] - cam2pos[1]);
pttemp[2] = cam2pos[2] + lambda * (Pt3D[stepNo][2] - cam2pos[2]);

//System.out.println("3D location on image plane2 = " + pttemp[0] + " " + pttemp[1] + " " + //pttemp[2]);


uvflist[uvf_idx][0] = pttemp[0];
uvflist[uvf_idx][1] = pttemp[1];
uvflist[uvf_idx][2] = pttemp[2];

uvflist[0][0] = uvflist[1][0];
uvflist[0][1] = uvflist[1][1];
uvflist[0][2] = uvflist[1][2];

//draw the locus of image point on image plane 2
float[] old_tempuv = new float[2];
old_tempuv = Perspective(uvflist[0][0], uvflist[0][1], uvflist[0][2]);
for (i=1; i<=uvf_idx; i++) {
   tempuv = Perspective(uvflist[i][0], uvflist[i][1], uvflist[i][2]);
   g.setColor(Color.blue);
   g.drawLine((int)old_tempuv[0], (int)old_tempuv[1], 
              (int)tempuv[0],     (int)tempuv[1]);
   old_tempuv[0] = tempuv[0];
   old_tempuv[1] = tempuv[1];
}



tempuv = Perspective(pttemp[0], pttemp[1], pttemp[2]);

uv2[0] = tempuv[0];
uv2[1] = tempuv[1];


g.setColor(Color.blue);
g.drawOval((int)uv1[0]-3,
           (int)uv1[1]-3,
           5,5);

g.drawOval(350,300,5,5);
g.drawString("plane 1 img pt", 370,305);

g.setColor(Color.black);
g.drawOval((int)uv2[0]-3,
           (int)uv2[1]-3,
           5,5);

g.drawOval(350,320,5,5);
g.drawString("plane 2 img pt", 370,325);

// draw the 3D point
float[] tempuv_Pt3D = Perspective(Pt3D[stepNo][0], Pt3D[stepNo][1], Pt3D[stepNo][2]);
g.setColor(Color.red);
g.drawOval((int)tempuv_Pt3D[0]-3,
           (int)tempuv_Pt3D[1]-3,
           5, 5);


// draw the line joining the 3D point to first camera center
float[] tempuv_cam1pos = Perspective(cam1pos[0], cam1pos[1], cam1pos[2]);
g.setColor(Color.black);
g.drawString("Pin hole 1", (int)tempuv_cam1pos[0], (int)tempuv_cam1pos[1]-5);
g.drawLine((int)tempuv_Pt3D[0],
           (int)tempuv_Pt3D[1],
           (int)tempuv_cam1pos[0],
           (int)tempuv_cam1pos[1]);


// draw the line joint the 3D point to second camera center
float[] tempuv_cam2pos = Perspective(cam2pos[0], cam2pos[1], cam2pos[2]);
g.drawString("Pin hole 2", (int)tempuv_cam2pos[0], (int)tempuv_cam2pos[1]-5);

if (showTrace == 1) {
   for (i = 1; i<=stepNo; i++) {
      tempuv_Pt3D = Perspective(Pt3D[i][0], Pt3D[i][1], Pt3D[i][2]);
      g.drawLine((int)tempuv_Pt3D[0],
                 (int)tempuv_Pt3D[1],
                 (int)tempuv_cam2pos[0],
                 (int)tempuv_cam2pos[1]);
   }
   g.drawString("3D Point", (int)tempuv_Pt3D[0]-15, (int)tempuv_Pt3D[1]-10);
} 
else {
   tempuv_Pt3D = Perspective(Pt3D[stepNo][0], Pt3D[stepNo][1], Pt3D[stepNo][2]);
   g.drawString("3D Point", (int)tempuv_Pt3D[0]-15, (int)tempuv_Pt3D[1]-10);

   g.drawLine((int)tempuv_Pt3D[0],
              (int)tempuv_Pt3D[1],
              (int)tempuv_cam2pos[0],
              (int)tempuv_cam2pos[1]);
   
}





/*****
if (stepNo == 1) {
  g.setColor(Color.yellow);
  g.drawRect(100, 100, 300, 300);
  }
else if (stepNo == 2) {
  g.setColor(Color.blue);
  g.drawLine(130,150, 250, 280);
  }
else if (stepNo == 3) {
  g.setColor(Color.red); 
  g.drawOval(100,100, 200, 200);
  stepNo = 0;
}
*****/



//System.out.println("uv1 = " + uv1[0] + " " + uv1[1]);
//System.out.println("uv2 = " + uv2[0] + " " + uv2[1]);

}





public float[] Perspective(float x, float y, float z) {

float tempx, tempy, tempz;
float num;
float den;
float[] uv = new float[2];

tempx = (x - view_x);
tempy = (y - view_y);
tempz = (z - view_z);

num = view_rot[0][0]*tempx + view_rot[0][1]*tempy + view_rot[0][2]*tempz;
den = view_rot[2][0]*tempx + view_rot[2][1]*tempy + view_rot[2][2]*tempz;







den = 1.0f;   // orthography






// note that below uses focal as focal length, actually, this focal length
// can be different from focal

uv[0] = focal * num / den;

uv[0] = uv[0]*disp_scale + disp_offx;

num = view_rot[1][0]*tempx + view_rot[1][1]*tempy + view_rot[1][2]*tempz;

uv[1] = focal * num / den;

uv[1] = uv[1]*disp_scale + disp_offy;

return uv;

}



}