/* File: rang.c for The boomerang simulator
** Authors: John C. Orthoefer <jco@reef.cis.ufl.edu>
**	    Brian Bartholomew <bb@math.ufl.edu>
** Date: 10 Nov 1990
** History:  Converted from PC BASIC (10 Nov 1990)
** 
*/

#include <stdio.h>
#include <math.h>
#define INRANG
#include "rang.h"

initrang(mode)
int mode;		/* mode = 0 for only 3D perspeceve */
			/* mode = 1 for 3 flat view plus 3D view */
{
   Pi = 4.0*atan(1.0);
   Deg2Pi = Pi/180.0;

/* W is most likly a Rho from some diagram.  They all seem to be angles */
   W1 = aim*Deg2Pi;
   W2 = 0.0;
   W3 = tilt*Deg2Pi;
   W4 = LayDown*Deg2Pi;
   W5 = 0.0;
   W6 = W3*sin(W1);

/* K are Konstants.  Magic source (outside of program is unknown) */
   K1 = 0.06;
/*	K2 was unused in the BASIC version */
   K3 = 0.975;

/* E viewing angle in rad for the 3D stuff */
   E = Elevation*Deg2Pi;

/* What is a program with out X, Y, and Z */
/* These numbers are thought to be inital position of the 'rang (in */
/* meters) */
   X = 0.0;
   Y = 2.0;
   Z = 2.0;

/* V is the velocity of the rang */
   V = Vel;

/* Realy unknown right now. */
   B1 = 0.0;
   Bx = 0.0;
   By = 0.0;

   L = 1.0;

/* Time and G the acceleration of gravity on the surface of the planet */
   T = 0.1;   /* in seconds */
   G = 9.81001; 	/* why that many place when the orignal program */
			/* had Pi = 3.14, I don't know */

   switch (mode) {
      default: 	/* 3D mode only.  Why? because I want to pick one */
         M = 6.0 - 2.0*sin(E);
	 XRL = 160.0;
	 YRL = 175.0 - sin(E)*M*Z;
	 break;
      case 1:
	 M = 2.5;
	 X = 40.0;
	 XRL = 240.0;
	 YRL = 180.0;
	 break;
      }
}


AdvRang()
{
   S = V * T;
   Dx = S * cos(W2);
   Dy = S * sin(W2);
   X = X + Dx * cos(W1) * cos(W5) - Dy * cos(W3) * sin(W6);
   Y = Y + Dy * cos(W3) * cos(W6) - Dx * cos(W1) * sin(W5);
   Dz = Dy * sin(W3) + Dx * sin(W1);
   D = K1 * S;
   D1 = D * cos(W4);
   D2 = D * sin(W4);
   W7 = fnasn(Dz/S);
   D3 = (W7 - atan(tan(W7) - G * T/V/cos(W7))) * cos(B1);
   Dz = Dz - G * T*T / 2.0;
   Z = Z + Dz;
   V = sqrt(V * V - 2.0 * G * Dz) * K3;
   Da =  - L * D2 * sin(W2) - D3 * cos(W2);
   Dg = L * D2 * cos(W2) - D3 * sin(W2);
   W8 = fnasn(sin(W1)/cos(W3));
   W9 = fnasn(sin(W3)/cos(W1));
   W1 = W1 + Da * cos(W9);
   W3 = W3 + Dg * cos(W8);
   B0 = atan(sin(W3)/sin(W1));
   Q = sin(W1) * sin(W1) + sin(W3) * sin(W3);

   if (Q > 1.0) {
      W8 = fnasn(sin(W8 + D4));
      W9 = fnasn(sin(W9 + D5));
      W1 = fnasn(sin(W8) * cos(W3));
      W3 = fnasn(sin(W9) * cos(W1));
      Bk = B0 - atan(tan(B0) * cos(B1));
      B0 = atan(sin(W3)/sin(W1));
      Q = sin(W1) * sin(W1) + sin(W3) * sin(W3);
	
      if (L != 1.0) {
	 L = 1.0;
	 Bx = Bx - 2.0 * Bk + Pi;
	 By = By - 2.0 * Bk;
      } else {
	 L = -1.0;
	 Bx = Bx + 2.0 * Bk - Pi;
	 By = By + 2.0 * Bk;
      }
   }

   B1 = fnasn(sqrt(Q));
   W6 = B0 - atan(tan(B0) * cos(B1));
   W5 = fnasn(tan(W1) * tan(W3)) - W6;
   W6 = By + L * W6;
   W5 =  -Bx + L * W5;
   W2 = W2 + D1;
}

