PROGRAM axis

c	Help analyze axis relating chains

	REAL	v1(3)/ 28.44778, 21.96756, 20.16742 /
	REAL	v2(3)/ 37.10480, 10.34954, 14.65247 /
	REAL	dv(3), mat(4,3), x(3), y(3)
	REAL	w(3) / 0.18032, 0.52823, -0.82973 /

	TYPE *, 'This is direction vector'
	WRITE (6, 1000) w
	TYPE *, 'Here are 2 points supposedly on axis'
	WRITE (6, 1000) v1
	WRITE (6, 1000) v2

c	First check that vector connecting 2 supposed points on axis
c	has same direction cosine as dv

	dd = 0.0
	DO i = 1,3
	  dv(i) = v1(i)-v2(i)
	  dd = dv(i)**2 + dd
	endDO
	dd = SQRT(dd)
	DO i = 1,3
	  dv(i) = dv(i)/dd
	endDO

	TYPE *, 'This should be same as direction vector'
	WRITE (6, 1000) dv
1000	FORMAT (4f10.6)

c	Since that did not work have to work with matrix

	OPEN (unit=1, file='rotalpha.mat',status='OLD',readonly)
	READ (1, 1000)
	READ (1, 1000) mat
	CLOSE (unit=1)

c	First check that rotation part working on direction cosines 
c	produce no effect

	DO i = 1,3
	  dv(i) = 0.0
	  DO j = 1,3
	    dv(i) = dv(i) + mat(j,i)*w(j)
	  endDO
	endDO

	TYPE *, 'Using only rotation on direction vector yields'
	WRITE (6, 1000) dv

c	Since that works now look at projection of translation onto direction
c	of axis.  This should yield translation.

	dd = 0.
	DO i = 1,3
	  dd = dd + mat(4,i)*dv(i)
	endDO

	TYPE *, 'This is translation part'
	WRITE (6, 1000) dd

c	That worked.  Now to yield a purely rotation matrix we need to
c	subtract the translation part and solve for a point on the axis

	DO i = 1,3
	  mat(4,i) = mat(4,i) - dd*dv(i)
	endDO

c	WRITE (6, 1000)
c	WRITE (6, 1001) mat
1001	FORMAT (4f15.6)
	DO i = 1,3
	  mat(i,i) = mat(i,i) - 1.
	  mat(4,i) = -mat(4,i)
	endDO

	DO i = 1,3
	  DO j = 4,1,-1
	    mat(j,i) = mat(j,i)/mat(1,i)
	  endDO
	endDO
c	WRITE (6, 1000)
c	WRITE (6, 1001) mat

	DO j = 1,4
	  mat(j,2) = mat(j,2)-mat(j,1)
	  mat(j,3) = mat(j,3)-mat(j,1)
	endDO	
c	WRITE (6, 1000)
c	WRITE (6, 1001) mat

	d2 = mat(2,1)/mat(2,2)
	d3 = mat(2,1)/mat(2,3)
	DO j = 1,4
	  mat(j,2) = mat(j,2)*d2
	  mat(j,3) = mat(j,3)*d3
	endDO
c	WRITE (6, 1000)
c	WRITE (6, 1001) mat

	DO j = 1,4
	  mat(j,1) = mat(j,1)-mat(j,2)
	  mat(j,3) = mat(j,3)-mat(j,2)
	endDO
c	WRITE (6, 1000)
c	WRITE (6, 1001) mat

	DO j = 4,1,-1
	  mat(j,2) = mat(j,2)/mat(2,2)
	endDO
c	WRITE (6, 1000)
c	WRITE (6, 1001) mat

	x(1) = mat(4,1)-v2(3)*mat(3,1)
	x(2) = mat(4,2)-v2(3)*mat(3,2)
	x(3) = v2(3)

	TYPE *, 'This is point on axis'
	WRITE (6, 1000) x

c	Check it

	OPEN (unit=1, file='rotalpha.mat',status='OLD',readonly)
	READ (1, 1000)
	READ (1, 1000) mat
	CLOSE (unit=1)

	DO i = 1,3
	  y(i) = mat(4,i)
	  DO j = 1,3
	    y(i) = y(i) + mat(j,i)*x(j)
	  endDO
	endDO

	TYPE *, 'Applying transformation point moves to'
	WRITE (6, 1000) y

	dd = 0.
	DO i = 1,3
	  dv(i) = x(i) - y(i)
	  dd = dd + dv(i)**2
	endDO
	dd = SQRT(dd)

	TYPE *, 'Distance between points'
	WRITE (6, 1000) dd

	DO i = 1,3
	  dv(i) = dv(i)/dd
	endDO

	TYPE *, 'Unit vector between points'
	WRITE (6, 1000) dv
c	
	end