c******************************************************************************
c   This program is a 'remote' 3D-collision detector for two balls on linear
c   trajectories and returns, if applicable, the location of the collision for 
c   both balls as well as the new velocity vectors (assuming a partially elastic
c   collision as defined by the restitution coefficient).
c   The equations on which the code is based have been derived at
c   http://www.plasmaphysics.org.uk/collision3d.htm
c
c   All variables apart from 'error' are of Double Precision Floating Point type.
c
c   The Parameters are:
c 
c    R    (restitution coefficient)  between 0 and 1 (1=perfectly elastic collision)                                   
c    m1    (mass of ball 1)
c    m2    (mass of ball 2)
c    r1    (radius of ball 1)
c    r2    (radius of ball 2)
c  * x1    (x-coordinate of ball 1) 
c  * y1    (y-coordinate of ball 1)          
c  * z1    (z-coordinate of ball 1)  
c  * x2    (x-coordinate of ball 2)          
c  * y2    (y-coordinate of ball 2)         
c  * z2    (z-coordinate of ball 2)         
c  * vx1   (velocity x-component of ball 1) 
c  * vy1   (velocity y-component of ball 1)
c  * vz1   (velocity z-component of ball 1)          
c  * vx2   (velocity x-component of ball 2)         
c  * vy2   (velocity y-component of ball 2)
c  * vz2   (velocity z-component of ball 2)
c  * error (Integer*2) (0: no error 
c                      1: balls do not collide 
c                      2: initial positions impossible (balls overlap)
c
c   In the calling program, the arguments corresponding to the parameters
c   with an asterisk (*) will be updated (the positions and velocities however
c   only if 'error'=0).
c   All variables should have the same data types in the calling program
c   and all should be initialized before calling the subroutine.
c
c   This program is free to use for everybody. However, you use it at your own
c   risk and I do not accept any liability resulting from incorrect behaviour.
c   I have tested the program for numerous cases and I could not see anything 
c   wrong with it but I can not guarantee that it is bug-free under any 
c   circumstances. 
c   I do in general not recommend the use of single precision because for
c   iterative computations the rounding errors can accumulate and render the
c   result useless if there are not enough decimal places; however for uncritical
c   applications or if speed is of paramount importance, you can change the 
c   program to single precision by changing the variable declaration to REAL*4
c   and remove the 'D' in front of the mathematical function names.
c
c
c   I would appreciate if you could report any problems to me
c   (for contact details see  http://www.plasmaphysics.org.uk/feedback.htm ).
c
c   Thomas Smid   February 2004
c                 December 2005 (a few minor changes to improve speed)
c                 December 2009 (generalization to partially inelastic collisions)
c                 July     2011 (fix for possible rounding errors)
c*******************************************************************************
                       
                              
 
       SUBROUTINE collision3D(R,m1,m2,r1,r2,
     &   x1,y1,z1,x2,y2,z2,vx1,vy1,vz1,vx2,vy2,vz2,error)
   
       IMPLICIT REAL*8 (A-Z)
       INTEGER*2 error
       
c      **** initialize some variables ****
       pi=DACOS(-1.0D0)  
       error=0
       r12=r1+r2
       m21=m2/m1
       x21=x2-x1
       y21=y2-y1
       z21=z2-z1
       vx21=vx2-vx1
       vy21=vy2-vy1
       vz21=vz2-vz1

       vx_cm = (m1*vx1+m2*vx2)/(m1+m2) 
       vy_cm = (m1*vy1+m2*vy2)/(m1+m2) 
       vz_cm = (m1*vz1+m2*vz2)/(m1+m2) 
       
             
c      **** calculate relative distance and relative speed ***      
       d=DSQRT(x21*x21 +y21*y21 +z21*z21)
       v=DSQRT(vx21*vx21 +vy21*vy21 +vz21*vz21)
       
c      **** return if distance between balls smaller than sum of radii ****
       IF (d .LT. r12) THEN
       error=2
       RETURN
       END IF
       
c      **** return if relative speed = 0 ****
       IF (v .EQ. 0) THEN
       error=1
       RETURN
       END IF       
       

c      **** shift coordinate system so that ball 1 is at the origin ***       
       x2=x21
       y2=y21
       z2=z21
       
c      **** boost coordinate system so that ball 2 is resting ***    
       vx1=-vx21
       vy1=-vy21
       vz1=-vz21

c      **** find the polar coordinates of the location of ball 2 ***    
       theta2=DACOS(z2/d)
       IF (x2 .EQ. 0 .AND. y2 .EQ. 0) THEN
       phi2=0
       ELSE
       phi2=DATAN2(y2,x2)
       END IF   
       st=DSIN(theta2)							
       ct=DCOS(theta2)							
       sp=DSIN(phi2)							
       cp=DCOS(phi2) 							


c      **** express the velocity vector of ball 1 in a rotated coordinate
c           system where ball 2 lies on the z-axis ******   
       vx1r=ct*cp*vx1+ct*sp*vy1-st*vz1 					
       vy1r=cp*vy1-sp*vx1 				
       vz1r=st*cp*vx1+st*sp*vy1+ct*vz1
	   fvz1r=vz1r/v
c      **** fix for possible rounding errors ****
       IF (fvz1r .GT. 1) THEN
       fvz1r=1
       ELSE 
         IF (fvz1r .LT. -1) THEN
         fvz1r=-1
         END IF
       END IF		  
c      **** End 'fix for possible rounding errors' ****	   
       thetav=DACOS(fvz1r)
       IF (vx1r .EQ. 0 .AND. vy1r .EQ. 0) THEN
       phiv=0
       ELSE
       phiv=DATAN2(vy1r,vx1r)
       END IF   

        						
c      **** calculate the normalized impact parameter ***    
       dr=d*DSIN(thetav)/r12


c      **** return old positions and velocities if balls do not collide ***
       IF (thetav .GT. pi/2 .OR. DABS(dr) .GT. 1) THEN
       x2=x2+x1
       y2=y2+y1
       z2=z2+z1
       vx1=vx1+vx2
       vy1=vy1+vy2
       vz1=vz1+vz2
       error=1
       RETURN
       END IF
       
c      **** calculate impact angles if balls do collide ***      
       alpha=DASIN(-dr)
       beta=phiv
       sbeta=DSIN(beta)
       cbeta=DCOS(beta)
        
       
c      **** calculate time to collision ***      
       t=(d*DCOS(thetav) -r12*DSQRT(1-dr**2))/v
     
c      **** update positions and reverse the coordinate shift *** 
       x2=x2+vx2*t +x1
       y2=y2+vy2*t +y1
       z2=z2+vz2*t +z1
       x1=(vx1+vx2)*t +x1
       y1=(vy1+vy2)*t +y1
       z1=(vz1+vz2)*t +z1
        
 
       
c  ***  update velocities ***

       a=DTAN(thetav+alpha)

       dvz2=2*(vz1r+a*(cbeta*vx1r+sbeta*vy1r))/((1+a**2)*(1+m21))
       
       vz2r=dvz2
       vx2r=a*cbeta*dvz2
       vy2r=a*sbeta*dvz2
       vz1r=vz1r-m21*vz2r
       vx1r=vx1r-m21*vx2r
       vy1r=vy1r-m21*vy2r

       
c      **** rotate the velocity vectors back and add the initial velocity
c            vector of ball 2 to retrieve the original coordinate system ****
                     
       vx1=ct*cp*vx1r-sp*vy1r+st*cp*vz1r +vx2 						
       vy1=ct*sp*vx1r+cp*vy1r+st*sp*vz1r +vy2 					
       vz1=ct*vz1r-st*vx1r               +vz2
       vx2=ct*cp*vx2r-sp*vy2r+st*cp*vz2r +vx2 						
       vy2=ct*sp*vx2r+cp*vy2r+st*sp*vz2r +vy2 					
       vz2=ct*vz2r-st*vx2r               +vz2  
       

c     ***  velocity correction for inelastic collisions ***

       vx1=(vx1-vx_cm)*R + vx_cm;
       vy1=(vy1-vy_cm)*R + vy_cm;
       vz1=(vz1-vz_cm)*R + vz_cm;
       vx2=(vx2-vx_cm)*R + vx_cm;
       vy2=(vy2-vy_cm)*R + vy_cm;
       vz2=(vz2-vz_cm)*R + vz_cm;  
       
       RETURN
       END