/*
vrmlviewpoints.java applet

Copyright (C) 2000 Robert Walker

E-mail vrml_viewpoints_applet@rcwalker.freeserve.co.uk

Makes VRML Viewpoint from coords + coords of point to look at + tilt.
Tilt angle is in degrees.

Method is to find:

Direction cosines for vector in direction of view.

Angle to turn about x to move y coord to desired pos,
then, angle about y to move x, z to desired pos,
if tilt is non zero, add it as third rotation.

Matrices for each rotation, multiply together to get resulting rotation.

Axis as solution to (r-I).v==0 where r = rotation matrix, I = identity matrix, v = axis.

Finally, find angle of rotation about new axis using formula for matrix for a rotation by an 
arbitrary angle about that axis, and solve to find rotation that gives the matrix r.

My VRML home page is go.to/v.t

You are welcome to copy /modify this code and use it in your programs.
*/

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

class controlspanel2 extends Panel
{
 Button makeviewpointbutton;
 Label fill1,fill2;
 controlspanel2()
 {
  fill1=new Label();
  fill2=new Label();
  setLayout(new GridLayout(1,3));
  add(fill1);
  makeviewpointbutton = new Button("Make Viewpoint");
  add(makeviewpointbutton);
  add(fill2);
 }
}

class controlspanel1 extends Panel
{
 TextField descriptiontext,fromtext, totext, tilttext;
 Label descriptionlabel,tolabel,fromlabel,tiltlabel; 
 controlspanel2 panel2=new controlspanel2();
 controlspanel1()
 {
  setLayout(new GridLayout(10,1));
  descriptionlabel = new Label("View description", Label.CENTER);
  add(descriptionlabel);
  descriptiontext = new TextField(1024);
  add(descriptiontext);
  descriptiontext.setText("Viewpoint");
  fromlabel = new Label("View from", Label.CENTER);
  add(fromlabel);
  fromtext = new TextField(1024);
  add(fromtext);
  fromtext.setText("0 0 10");
  tolabel = new Label("View to", Label.CENTER);
  add(tolabel);
  totext = new TextField(1024);
  totext.setText("0 0 0");
  add(totext);
  tiltlabel = new Label("Tilt (in degrees)", Label.CENTER);
  add(tiltlabel);
  tilttext = new TextField(1024);
  add(tilttext);
  tilttext.setText("0");
  add(panel2);
 }
}
public class vrmlviewpoints extends Applet implements ActionListener
{
 TextArea viewpointtext;
 Label descriptionlabel,tolabel,fromlabel,tiltlabel; 
 double view_tilt=0;
 double view_field_of_view=((float)0.785398);
 double[] view_from = new double[3];//Viewpoint coords
 double[] view_to = new double[3];//point to look at
 controlspanel1 panel1;
 double PI=3.14159265358979323851;
 double TWO_PI=2*PI;
 double angles_mult=TWO_PI/360;//input angles in degrees - used for view_tilt
 String ViewpointName = new String("Viewpoint");
 double resolution=1e-4;
 public void init()
 {
  setLayout(new GridLayout(2,1));
  panel1=new controlspanel1();
  add(panel1);
  panel1.panel2.makeviewpointbutton.addActionListener(this);
  viewpointtext = new TextArea(1024,12);
  add(viewpointtext);
 }
 public void actionPerformed(ActionEvent e)
 {
  if(e.getSource() == panel1.panel2.makeviewpointbutton)
  {
   StringBuffer strbuffer = new StringBuffer(1024);
   StringTokenizer st = new StringTokenizer(panel1.fromtext.getText());
   int i=0;
   while (st.hasMoreTokens())
   {
    double d;
    String str=st.nextToken();
    d=new Double(str).doubleValue();;
    view_from[i]=d;
    i=i+1;
    if(i>=3)
     break; 
   }
   i=0;
   StringTokenizer st2 = new StringTokenizer(panel1.totext.getText());
   while (st2.hasMoreTokens())
   {
    double d;
    String str=st2.nextToken();
    d=new Double(str).doubleValue();
    view_to[i]=d;
    i=i+1;
    if(i>=3)
     break; 
   }
   StringTokenizer st3 = new StringTokenizer(panel1.tilttext.getText());
   i=0;
   while (st3.hasMoreTokens())
   {
    double d;
    String str=st3.nextToken();
    d=new Double(str).doubleValue();;
    view_tilt=d*angles_mult;
    break; 
   }
   ViewpointName=panel1.descriptiontext.getText();
   MakeViewpoint(strbuffer);
   viewpointtext.setText(strbuffer.toString());
  }
 }
 private boolean is_solution(double a[][],double x[])
 {
  int i,j;
  for(j=0;j<3;j++)
  {
   double sum=0;
   for(i=0;i<3;i++)
    sum+=a[j][i]*x[i];
   if((Math.abs(sum)>=resolution))
    return false;
  }
  return true;
 }
 private int solve_sim_eqs_for_non_zero_sol(double a[][],double x[])
                                           //coefficients, solution
 //solves system of 3 simultaneous equations
 //arguments should be 
 //double a[3][3]; ==coefficients
 //double x[3]; == solution
 //resolution is absolute value below which entries are to be regarded as zero
 //Each row of coefficients has the coefficients for one of the equations.
 //The equations are
 //a[j][0]*x[0] +a[j][1]*x[1]+a[j][2]*x[2]==0
 //looks for solution with at least one non zero value of x[].
 //method is, try eliminating 0th, then 1st, then 2nd coefficient. For each one, try eliminating
 //it using 1st, 2nd, or 3rd equation as the one to subtract
 //returns 1 for success
 {
  int i,j,row_to_subtract,ith,jth;
  double[][] A=new double[3][3];//scratch array for calcs - reset from a[][] each time
  //check to see if default axis is a solution - can happen e.g. if all entries in r are zero
  x[0]=x[2]=0;x[1]=1;
  if(is_solution(a,x))
   return 1;
  for(row_to_subtract=0;row_to_subtract<3;row_to_subtract++)
  for(ith=0;ith<3;ith++)
  {
   if(Math.abs(a[row_to_subtract][ith])>=resolution)
   {
    //reset scratch array ready to start new elimination
    for(i=0;i<3;i++)
    for(j=0;j<3;j++)
     A[j][i]=a[j][i];
    //eliminate ith coefficient from all other rows with ith coefficient non zero
    for(jth=0;jth<3;jth++)
    if(jth!=row_to_subtract)
    {
     //find multiplier 
     double mki=A[jth][ith]/A[row_to_subtract][ith];//so mki*A[row_to_subtract][ith]==A[jth][ith]
     //subtract row_to_subtract*multiplier from jth row
     if(mki!=0)
     for(i=0;i<3;i++)
     {
      A[jth][i]-=mki*A[row_to_subtract][i];
      if(Math.abs(A[jth][i])<resolution)
       A[jth][i]=0;
     }
    }
    //now A[j][] has ith coord zero for both of the j!=row_to_subtract
    //find non zero solution to transformed equations, if any
    {
     int i1=(ith+1)%3,i2=(ith+2)%3,i3=ith;                    
     int j1=(row_to_subtract+1)%3,j2=(row_to_subtract+2)%3,j3=row_to_subtract;
     //The i3==ith coefficients were eliminated from rows j1 and j2
     //so rows j1 and j2 have to be identical up to a factor for a non zero solution of the transformed equations
     //so can use either to find x[i1] and x[i2]
     //use A[j2][i1]*x[i1]+A[j2][i2]*x[i2]==0 to find x[i1] and x[i2]
     if(Math.abs(A[j2][i2])>=resolution)
     {
      x[i1]=1;
      x[i2]=-A[j2][i1]/A[j2][i2];
     }
     else 
      if(Math.abs(A[j2][i1])>=resolution)
      {
       x[i2]=1;
       x[i1]=-A[j2][i2]/A[j2][i1];
      }
      else
       //only solution to the transformed equations is x[i1]==x[i2]==x[i3]==0
       continue;
     //use A[j3][i1]*x[i1]+A[j3][i2]*x[i2]+A[j3][i3]*x[i3]==0 to find x[i3]
     if(Math.abs(A[j3][i3])>=resolution)
      x[i3]=-(A[j3][i1]*x[i1]+A[j3][i2]*x[i2])/A[j3][i3];
     else continue;//only solution to the transformed equations is x[i1]==x[i2]==x[i3]==0
     //check it is indeed a solution
     if(is_solution(a,x))
      return 1;
    }
   }
  }
  x[0]=x[2]=0;x[1]=1;//may as well return default axis - shouldn't happen
  return 0;
 }
 private double FastSetInRange(double theta,double min,double increment)
 {
  return (double)((min)+(increment)*(((theta)-(min))/(increment)-Math.floor(((theta)-(min))/(increment))));
 }
 private void MakeMatrixForRotation(double Mat[][],double theta, double ax,double by,double cz)
 {
  double a=ax,b=by,c=cz;
  Mat[0][0]=a*a*(1-Math.cos(theta))+Math.cos(theta);
  Mat[0][1]=a*b*(1-Math.cos(theta))-c*Math.sin(theta);
  Mat[0][2]=a*c*(1-Math.cos(theta))+b*Math.sin(theta);
  Mat[1][0]=a*b*(1-Math.cos(theta))+c*Math.sin(theta);
  Mat[1][1]=b*b*(1-Math.cos(theta))+Math.cos(theta);
  Mat[1][2]=b*c*(1-Math.cos(theta))-a*Math.sin(theta);
  Mat[2][0]=a*c*(1-Math.cos(theta))-b*Math.sin(theta);
  Mat[2][1]=b*c*(1-Math.cos(theta))+a*Math.sin(theta);
  Mat[2][2]=c*c*(1-Math.cos(theta))+Math.cos(theta);
 }
 private void MatMult(double Mat1[][],double Mat2[][],double Result[][])
 {
  int i,j;
  for(i=0;i<3;i++)
  for(j=0;j<3;j++)
  {
   int k;
   Result[i][j]=0;
   for(k=0;k<3;k++)
    Result[i][j]+=Mat1[i][k]*Mat2[k][j];
  } 
 }
 private void MakeViewpoint(StringBuffer stringbuffer)
 {
  double[][] Mat1 = new double[3][3],Mat2 = new double[3][3],Mat3 = new double[3][3];
  double[][] Result = new double[3][3];
  double[][] r = new double[3][3];
  double[] new_axis = new double[3];
  double theta=0;
  double[] diff= new double[3];
  int i;
  double th_about_z,th_about_y,th_about_x;
  double radius;
  double[] diffn= new double[3];
  double ldiffn=0;
  int ok_sol=0;
  //normalised diff - direction cosines
  for(i=0;i<3;i++)
   diff[i]=view_to[i]-view_from[i];
  for(i=0;i<3;i++)
   ldiffn+=diff[i]*diff[i];
  for(i=0;i<3;i++)
  {
   diffn[i]=diff[i]/(double)Math.sqrt(ldiffn);
   diffn[i]=Math.min(1,Math.max(-1,diffn[i]));//in case rounding error takes its abs value over 1
  }
  //need to apply tilt first, then angle about x, then finally, about y
  th_about_z=view_tilt;
  //angle about x is PI/2 + angle from vertical (y) (yields -ve Z axis if all angles 0)
  //have ndiff[1]==cos(angle from vertical)
  th_about_x=(double)Math.asin(diffn[1]);
  th_about_x=FastSetInRange((double)th_about_x,-PI,TWO_PI);
  //find th_about_y - angle about y axis (vertical)
  {
   double x=-diffn[2],y=-diffn[0];
   radius=(double)Math.max(1e-10,(float)Math.sqrt(x*x+y*y));
   double ratio=Math.min(1,Math.max(-1,x/radius));//in case rounding error takes its abs value over 1
   th_about_y=(double)Math.acos(ratio);
   if(radius==0)
    th_about_y=0;
   if(y<0)th_about_y=-th_about_y;
   th_about_y=FastSetInRange(th_about_y,-PI,TWO_PI);
  }
  if((Math.abs(radius)<1e-4))
   th_about_y=0;
  if(th_about_z!=0)
   MakeMatrixForRotation(Mat1,th_about_z,0,0,1);
  MakeMatrixForRotation(Mat2,th_about_x,1,0,0);
  MakeMatrixForRotation(Mat3,th_about_y,0,1,0);
  //Find Mat3.Mat2.Mat1 - leave out Mat1 if theta_about_z==0
  if(th_about_z==0)
   MatMult(Mat3,Mat2,r);
  else
  {
   MatMult(Mat2,Mat1,Result);
   MatMult(Mat3,Result,r);
  }
  //Now need to find axis of rotation for result
  //want x with (r-I)x=0
  //where r==rotation array
  r[0][0]-=1;
  r[1][1]-=1;
  r[2][2]-=1;
  //want to solve
  //r[0][0]*x[0]+r[0][1]*x[1]+r[0][2]*x[2]==0
  //Hr[1][0]*x[0]+r[1][1]*x[1]+r[1][2]*x[2]==0
  //r[2][0]*x[0]+r[2][1]*x[1]+r[2][2]*x[2]==0
  //where x==new_axis
  ok_sol=solve_sim_eqs_for_non_zero_sol(r,new_axis);
  if(ok_sol>0)
  {
   //convert new_axis to direction cosines
   double a=new_axis[0],b=new_axis[1],c=new_axis[2];
   double length=(double)Math.sqrt(a*a+b*b+c*c);
   a/=length;
   b/=length;
   c/=length;
   new_axis[0]=a;new_axis[1]=b;new_axis[2]=c;
   for(i=0;i<3;i++)
   {
    double d=new_axis[i];
    if(Math.abs(d)<1e-4)
     new_axis[i]=0;
   }
   //Now need new angle
   //Solve r.(0,0,-1)=diffn where r is rotation about new axis
   //so need to look at 3rd col of the rotation matrix for rotation
   //by theta about new axis
   //and want
   //a*c*(1-cos(theta))+b*sin(theta)==-diffn[0]
   //b*c*(1-cos(theta))-a*sin(theta)==-diffn[1]
   //c*c*(1-cos(theta))+cos(theta)==-diffn[2]
   //so
   //(1-c*c)cos(theta)==-diffn[2]-(c*c);
   if((Math.abs(1-c*c)<1e-4))
    //rotation about z axis
   {new_axis[0]=new_axis[1]=0;new_axis[2]=1;theta=FastSetInRange(view_tilt,-PI,TWO_PI);}
   else
   {
    double arg=(-diffn[2]-c*c)/(1-c*c);
    arg=Math.min(1,Math.max(-1,arg));//in case rounding error takes its abs value over 1
    theta=(double)Math.acos(arg);
    theta=FastSetInRange(theta,-PI,TWO_PI);
    {
     double cos_theta=Math.cos(theta),sin_theta=Math.sin(theta);
     double t1=a*c*(1-cos_theta)+b*sin_theta;//ti should be -Result2[i][2] to reconstruct original matrix using theta
     double t2=b*c*(1-cos_theta)-a*sin_theta;
     double t3=c*c*(1-cos_theta)+cos_theta;
     {
      if
      (
       !(Math.abs(t1+diffn[0])<1e-4)
       ||!(Math.abs(t2+diffn[1])<1e-4)
       ||!(Math.abs(t3+diffn[2])<1e-4)
       )
       theta=TWO_PI-theta;
      theta=FastSetInRange(theta,-PI,TWO_PI);
     }
    }
   }
  }
  DecimalFormat coord = new DecimalFormat("#0.######");
  stringbuffer.append("Viewpoint"+"\r\n"+"{"+"\r\n"+" description \""+ViewpointName
                     +"\""+"\r\n"+" position "+view_from[0]+" "+view_from[1]+" "+view_from[2]
                     +"\r\n"+" orientation "+coord.format(new_axis[0])+" "+coord.format(new_axis[1])+" "+coord.format(new_axis[2])+" "+coord.format(theta)
                     +"\r\n"+" fieldOfView "+coord.format(view_field_of_view)+"\r\n"+"}"+"\r\n"+"\r\n"
                     );
  if(ok_sol==0)
   stringbuffer.append("no solution found for rotation axis - using default");
 }
}