/*
vv.exe MSDOS program

Copyright (C) 2000 Robert Walker

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

Makes VRML Viewpoint from coords + coords of point to look at + tilt.

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.
*/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
double view_to[3];
double view_from[3];
double view_tilt;
int precision=7;//number of significant places to show in result
#define PI		((double)3.14159265358979323851)
#define TWO_PI ((double)(2.0*PI))
#define RESOLUTION 1e-5
#define AEZERO(x) (fabs(x)<RESOLUTION) 
#define AEZERO2(x) (fabs(x)<RESOLUTION) 
#define FastSetInRange(theta,min,increment)\
			(\
				(double)((min)+(increment)*(((theta)-(min))/(increment)-floor(((theta)-(min))/(increment))))\
			)

void MakeViewpoint(void);

main(int argc,char *argv[])
{

	if(argc>1)
	{
		sscanf(argv[1],"%d",&precision);
		printf("will show %d significant places\n",precision);
	}
	for(;;)
	{
		char string[5];
		printf("Enter coords of viewpoint: ");
		scanf("%lg %lg %lg", &(view_from[0]), &(view_from[1]), &(view_from[2]));
		printf("Enter look at point: ");
		scanf("%lg %lg %lg", &(view_to[0]), &(view_to[1]), &(view_to[2]));
		printf("Enter tilt: ");
		scanf("%lg", &view_tilt);
		view_tilt=view_tilt*TWO_PI/360;//convert to radians from degrees
		MakeViewpoint();
		printf("Find another? (Y/N)\n");
		scanf("%2s", &string);
		if(string[0]=='n'||string[0]=='N')
			break;
		fflush(stdin);
	}
	getchar();
	return 0;
}

void MakeMatrixForRotation(double (*Mat)[3],double theta, double ax,double by,double cz)
{
	double a=ax,b=by,c=cz;
	double cos_theta=cos(theta);
	double sin_theta=sin(theta);
	Mat[0][0]=a*a*(1-cos_theta)+cos_theta;
	Mat[0][1]=a*b*(1-cos_theta)-c*sin_theta;
	Mat[0][2]=a*c*(1-cos_theta)+b*sin_theta;
	Mat[1][0]=a*b*(1-cos_theta)+c*sin_theta;
	Mat[1][1]=b*b*(1-cos_theta)+cos_theta;
	Mat[1][2]=b*c*(1-cos_theta)-a*sin_theta;
	Mat[2][0]=a*c*(1-cos_theta)-b*sin_theta;
	Mat[2][1]=b*c*(1-cos_theta)+a*sin_theta;
	Mat[2][2]=c*c*(1-cos_theta)+cos_theta;
}

void MatMult(double (*Mat1)[3],double (*Mat2)[3],double (*Result)[3])
{
	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];
	}	
}

void FindLengthAndAngleFrom1stAxisAsdouble(double *ptheta,double *pradius,double x,double y)
{
	double ratio=x/(*pradius);
	ratio=min(1,max(-1,ratio));//in case rounding error takes its abs value over 1
	*pradius=(double)max(1e-10,(float)sqrt(x*x+y*y));
	*ptheta=(double)acos((double)x/(*pradius));
	if(*pradius==0)
		*ptheta=0;
	if(y<0)*ptheta=-*ptheta;
	*ptheta=FastSetInRange(*ptheta,-PI,TWO_PI);
}

char is_solution(double (*a)[3],double *x)
{
 int i,j;
 for(i=0;i<3;i++)
 {
  double sum=0;
  for(j=0;j<3;j++)
   sum+=a[i][j]*x[j];
  if((fabs(sum)>=RESOLUTION))
   return 0;
 }
 return 1;
}

char solve_sim_eqs_for_non_zero_sol(double (*a)[3],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
{
 int i,j,row_to_subtract,ith,jth;
 double A[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(fabs(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(fabs(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(fabs(A[j2][i2])>=RESOLUTION)
    {
     x[i1]=1;
     x[i2]=-A[j2][i1]/A[j2][i2];
    }
    else 
     if(fabs(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(fabs(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;
}

void MakeViewpoint(void)
{
	double Mat1[3][3],Mat2[3][3],Mat3[3][3];
	double Result[3][3];
	double r[3][3];
	double new_axis[3];
	double theta=0;
	double diff[3];
	int i;
	double th_about_z,th_about_y,th_about_x;
	double radius;
	double diffn[3],ldiffn=0;
	char 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];
	if(ldiffn<1e-15)
		//view_to and view_from are identical - use default
	{ldiffn=1;diff[0]=diff[1]=0;diff[1]=1;}
	for(i=0;i<3;i++)
	{
		diffn[i]=diff[i]/(double)sqrt(ldiffn);
  diffn[i]=min(1,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)asin(diffn[1]);
	th_about_x=FastSetInRange((double)th_about_x,-PI,TWO_PI);
	//angle about y is angle about vertical (y)
	FindLengthAndAngleFrom1stAxisAsdouble(&th_about_y,&radius,-diffn[2],-diffn[0]);
	if(AEZERO2(radius))
		th_about_y=0;
 if(th_about_z!=0)//non zero view_tilt
  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 eigenvector for (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
	//r[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);
	//now need to find the rotation about that 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)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++)
			if(fabs(new_axis[i])<RESOLUTION)
				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(AEZERO2(1-c*c))
			//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=min(1,max(-1,arg));//in case rounding error takes its abs value over 1
			theta=(double)acos(arg);
			theta=FastSetInRange(theta,-PI,TWO_PI);
			{
				double cos_theta=cos(theta),sin_theta=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(
						!AEZERO2(t1+diffn[0])
						||!AEZERO2(t2+diffn[1])
						||!AEZERO2(t3+diffn[2])
						)
						theta=TWO_PI-theta;
					theta=FastSetInRange(theta,-PI,TWO_PI);
				}
			}
		}
	}
	if(fabs(theta)<RESOLUTION)
		theta=0;
	printf("orientation %.*g %.*g %.*g %.*g\n"
			                 ,precision,new_axis[0],precision,new_axis[1],precision,new_axis[2]
																				,precision,theta
			      );
	if(ok_sol==0)
		printf("no solution found for rotation axis - using default\n");
}