/** 3DGPL *************************************************\
 *  ()                                                    *
 *  3-D transformations of coordinates using fixed point. *
 *                                                        *
 *  Defines:                                              *
 *   T_init_math             Creating sin/cos tables;     *
 *                                                        *
 *   T_translation           Shifting of coordinates;     *
 *   T_scaling               Scaling coordinates;         *
 *                                                        *
 *   T_set_world_rotation    Viewer's rotation;           *
 *   T_world_rotation        Transforming coordinates;    *
 *   T_set_self_rotation     Object rotation;             *
 *   T_self_rotation         Transforming coordinates;    *
 *                                                        *
 *   T_perspective           Transform to perspective.    *
 *                                                        *
 *  Internals:                                            *
 *   TI_float2fixed          Converting float to fixed.   *
 *                                                        *
 *  (6/1995) By Sergei Savchenko. (savs@cs.mcgill.ca).    *
 *  Copyright (c) 1995 Sergei Savchenko.                  *
 *  THIS SOURCE CODE CAN'T BE USED FOR COMERCIAL PURPOSES *
 *  WITHOUT AUTHORISATION                                 *
\**********************************************************/

#include <math.h>                           /* sin & cos */
#include "../hardware/hardware.h"           /* 32bit int name */
#include "../trans/trans.h"                 /* 3D mathematics */

typedef signed_32_bit fixed;                /* better be 32bit machine */

#define T_RADS                  40.7436642  /* pseudo-grads into rads */
#define T_P                             14  /* fixed math precision */

fixed T_wx1,T_wx2,T_wx3,T_wy1,T_wy2,T_wy3,T_wz1,T_wz2,T_wz3;
fixed T_sx1,T_sx2,T_sx3,T_sy1,T_sy2,T_sy3,T_sz1,T_sz2,T_sz3;
fixed T_sin[256],T_cos[256];                /* precalculated */

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ 
 *  INTERNAL: Converting [-1,1] float into fixed.        *
 *  ---------                                            *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
fixed TI_float2fixed(float a)
{ 
 fixed res=0;
 int i,dv,sign;

 if((sign=a<0.0)==1) a=-a;

 for(i=1,dv=2;i<=T_P;i++,dv*=2)
  if(a>=1.0/dv) { a-=1.0/dv; res|=(0x1<<(T_P-i)); };

 if(sign==1) res=-res;

 return(res);
}

/**********************************************************\
 *  Initializing tables.                                  *
\**********************************************************/

void T_init_math(void)
{
 int i;

 for(i=0;i<256;i++)
 {
  T_sin[i]=TI_float2fixed(sin(i/T_RADS));
  T_cos[i]=TI_float2fixed(cos(i/T_RADS));
 }
} 

/**********************************************************\
 *  Translation of coordinates.                           *
\**********************************************************/

void T_translation(register int *from,register int *to,int length,
		   int addx,int addy,int addz
		  )
{
 register int i;

 for(i=0;i<length;i++)
 {
  (*to++)=(*from++)+addx;
  (*to++)=(*from++)+addy;
  (*to++)=(*from++)+addz;                   /* translation */
 }
}

/**********************************************************\
 *  Scaling coordinates. (takes in float parameters).     *
\**********************************************************/

void T_scaling(register int *from,register int *to,int length,
	       int mulx,int muly,int mulz
	      )
{
 register int i;

 for(i=0;i<length;i++)
 {
  (*to++)=(int)(*from++)*mulx;
  (*to++)=(int)(*from++)*muly;
  (*to++)=(int)(*from++)*mulz;
 }                                          /* scaling */
}

/**********************************************************\
 *  Constructing rotation matrix. (gam-bet-alp), rotation *
 *  then pitch then roll sequence. gam-rotation, bet-pitch*
 *                                 alp-roll.              *
 *                                                        *
 *          Y^    Z           x'=z*sin(gam)+x*cos(gam)    *
 *           |   /            y'=y                        *
 *           |  / alp         z'=z*cos(gam)-x*sin(gam)    *
 *          /|<---+                                       *
 *         | |/   |           x"=x'                       *
 *  -------|-+-----------> X  y"=y'*cos(bet)-z'*sin(bet)  *
 *     bet | |   __           z"=y'*sin(bet)+z'*cos(bet)  *
 *         V/|   /| gam                                   *
 *         /----+             x"'=y"*sin(alp)+x"*cos(alp) *
 *        /  |                y"'=y"*cos(alp)-x"*sin(alp) *
 *       /   |                z"'=z"                      *
 *           |                                            *
\**********************************************************/

void T_set_world_rotation(int alp,int bet,int gam)
{
 fixed cosalp,sinalp,cosbet,sinbet,cosgam,singam;

 cosalp=T_cos[alp];
 sinalp=T_sin[alp];
 cosbet=T_cos[bet];
 sinbet=T_sin[bet];
 cosgam=T_cos[gam];
 singam=T_sin[gam];                         /* initializing */

 T_wx1=((singam*((sinbet*sinalp)>>T_P))>>T_P) + ((cosgam*cosalp)>>T_P);
 T_wy1=((cosbet*sinalp)>>T_P);
 T_wz1=((singam*cosalp)>>T_P) - ((cosgam*((sinbet*sinalp)>>T_P))>>T_P);

 T_wx2=((singam*((sinbet*cosalp)>>T_P))>>T_P) - ((cosgam*sinalp)>>T_P);
 T_wy2=((cosbet*cosalp)>>T_P);
 T_wz2=-((cosgam*((sinbet*cosalp)>>T_P))>>T_P) - ((singam*sinalp)>>T_P);

 T_wx3=-((singam*cosbet)>>T_P);
 T_wy3=sinbet;
 T_wz3=((cosgam*cosbet)>>T_P);              /* calculating the matrix */
}

/**********************************************************\
 *  Rotating coordinates.                                 *
 *                                        |wx1 wx2 wx3|   *
 *  T'=T[W]  where:  [x' y' z'] = [x y z]*|wy1 wy2 wy3|   *
 *                                        |wz1 wz2 wz3|   *
\**********************************************************/

void T_world_rotation(int *from,register int *to,int length)
{
 register int i;
 register int xt,yt,zt;

 for(i=0;i<length;i++)
 {
  xt=*from++;
  yt=*from++;
  zt=*from++;

  *to++=((T_wx1*xt)>>T_P) + ((T_wy1*yt)>>T_P) + ((T_wz1*zt)>>T_P);
  *to++=((T_wx2*xt)>>T_P) + ((T_wy2*yt)>>T_P) + ((T_wz2*zt)>>T_P);
  *to++=((T_wx3*xt)>>T_P) + ((T_wy3*yt)>>T_P) + ((T_wz3*zt)>>T_P); 
 }
}

/**********************************************************\
 *  Constructing rotation matrix. (alp-bet-gam), roll     *
 *  then pitch then rotation sequence. gam-rotation,      *
 *                                     bet-pitch, alp-roll*
 *                                                        *
 *          Y^    Z           x'=y*sin(alp)+x*cos(alp)    *
 *           |   /            y'=y*cos(alp)-x*sin(alp)    *
 *           |  / alp         z'=z                        *
 *          /|<---+                                       *
 *         | |/   |           x"=x'                       *
 *  -------|-+-----------> X  y"=y'*cos(bet)-z'*sin(bet)  *
 *     bet | |   __           z"=y'*sin(bet)+z'*cos(bet)  *
 *         V/|   /| gam                                   *
 *         /----+             x"'=z"*sin(gam)+x"*cos(gam) *
 *        /  |                y"'=y"                      *
 *       /   |                z"'=z"*cos(gam)-x"*sin(gam) *
 *           |                                            *
\**********************************************************/

void T_set_self_rotation(int alp,int bet,int gam)
{ 
 fixed cosalp,sinalp,cosbet,sinbet,cosgam,singam;

 cosalp=T_cos[alp];
 sinalp=T_sin[alp];
 cosbet=T_cos[bet];
 sinbet=T_sin[bet];
 cosgam=T_cos[gam];
 singam=T_sin[gam];                         /* initializing */

 T_sx1=((cosalp*cosgam)>>T_P)-((sinalp*((sinbet*singam)>>T_P))>>T_P);
 T_sy1=((sinalp*cosgam)>>T_P)+((cosalp*((sinbet*singam)>>T_P))>>T_P);
 T_sz1=((cosbet*singam)>>T_P);

 T_sx2=-((sinalp*cosbet)>>T_P);
 T_sy2=((cosalp*cosbet)>>T_P);
 T_sz2=-sinbet;

 T_sx3=-((cosalp*singam)>>T_P)-((sinalp*((sinbet*cosgam)>>T_P))>>T_P);
 T_sy3=((cosalp*((sinbet*cosgam)>>T_P))>>T_P)-((sinalp*singam)>>T_P);
 T_sz3=((cosbet*cosgam)>>T_P);              /* calculating the matrix */
}

/**********************************************************\
 *  Rotating coordinates.                                 *
 *                                        |sx1 sx2 sx3|   *
 *  T'=T[S]  where:  [x' y' z'] = [x y z]*|sy1 sy2 sy3|   *
 *                                        |sz1 sz2 sz3|   *
\**********************************************************/

void T_self_rotation(int *from,register int *to,int length)
{
 register int i;
 register int xt,yt,zt;

 for(i=0;i<length;i++)
 {
  xt=*from++;
  yt=*from++;
  zt=*from++;

  *to++=((T_sx1*xt)>>T_P) + ((T_sy1*yt)>>T_P) + ((T_sz1*zt)>>T_P);
  *to++=((T_sx2*xt)>>T_P) + ((T_sy2*yt)>>T_P) + ((T_sz2*zt)>>T_P);
  *to++=((T_sx3*xt)>>T_P) + ((T_sy3*yt)>>T_P) + ((T_sz3*zt)>>T_P); 
 }
}

/**********************************************************\
 *  Transforming to perspective, coordinates passed ase   *
 *                        supposed to be both volume, and *
 *                *       Z-clipped, otherwise division   *
 *               /|X      by 0 or overflow can occur.     *
 *              / |                                       *
 *             /  |                                       *
 *            *   |                                       *
 *           /|X' |       X'      X                       *
 *          / |   |    ------- = ---                      *
 *         /  |   |     focus     Z                       *
 *        *---+---+                                       *
 *        0   ^   Z    X'= X*focus/Z                      *
 *            |                                           *
 *          focus                                         *
 *                                                        *
 *  ADDITIONAL FUNCTIONS: 1) changing formats:            *
 *  ---------------------                                 *
 *    source:       x, y ,z,a1,...,aN where N==dimension-3*
 *    destanation:  x',y',  a1,...,aN                     *   
 *                                                        *
 *  2) performs translation to the screen centre.         *
\**********************************************************/

void T_perspective(register int *from,register int *to,
		   int dimension,int length
		  )
{
 register int i;

 dimension-=3;                              /* other then X,Y */

 for(i=0;i<length;i++,from+=dimension,to+=dimension) 
 {                                          /* Z is not being changed */
  to[0]=((((long)from[0])<<T_LOG_FOCUS)/from[2])+HW_SCREEN_X_CENTRE;
  to[1]=((((long)from[1])<<T_LOG_FOCUS)/from[2])+HW_SCREEN_Y_CENTRE;
  HW_copy_int(from+=3,to+=2,dimension);
 }
}

/**********************************************************/