#include "system.h"

#include "prototyp.h"

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <sys/time.h>

#define deg22pt5 256

#define deg45 512

#define deg90 1024

#define Cosine45 46341           /* 46340.95001 cosine(45deg)*/

extern int16_t ArcCosTable[];

extern int16_t ArcSineTable[];

extern int16_t ArcTanTable[];

static int oneoversin[4096];

int min(int a, int b) { return a < b ? a : b; }

int max(int a, int b) { return a < b ? b : a; }

/* Fixed Point Divide - returns a / b */

int DIV_FIXED(const int a, const int b)

{

    return (((int64_t)a << 16) / b);

}

/*

 Fixed Point Multiply.

 16.16 * 16.16 -> 16.16

 or

 16.16 * 0.32 -> 0.32

 A proper version of this function ought to read

 16.16 * 16.16 -> 32.16

 but this would require a long long result

 Algorithm:

 Take the mid 32 bits of the 64 bit result

*/

int MUL_FIXED(const int a, const int b)

{

    return (((int64_t)a * (int64_t)b) >> 16);

}

void ConstructOneOverSinTable() 

{

    int a = 0;

    for (; a < 4096; a++) 

    {

        int sin = GetSin(a);

        if (sin != 0) 

        {

            oneoversin[a] = DIV_FIXED(ONE_FIXED, sin);

        }

        else

        {

            sin = 100;

            oneoversin[a] = DIV_FIXED(ONE_FIXED, sin);

        }

    }

}

int GetOneOverSin(int a) 

{

    return oneoversin[a & wrap360];

}

int DotProduct(const VECTORCH *vptr1, const VECTORCH *vptr2)

{

    return MUL_FIXED(vptr1->vx, vptr2->vx) + MUL_FIXED(vptr1->vy, vptr2->vy)+ MUL_FIXED(vptr1->vz, vptr2->vz);

}

int Magnitude(const VECTORCH *v)

{

    float x = v->vx;

    float y = v->vy;

    float z = v->vz;

    return sqrt((x * x) + (y * y) + (z * z));

}

int VectorDistance(const VECTORCH *v1, const VECTORCH *v2)

{

    float x = v1->vx - v2->vx;

    float y = v1->vy - v2->vy;

    float z = v1->vz - v2->vz;

        return sqrt((x * x) + (y * y) + (z * z));

}

/*

 Create Matrix from Euler Angles

 It requires a pointer to some euler angles and a pointer to a matrix

 Construct the matrix elements using the following formula

 Formula for ZXY Matrix

 m11 = cy*cz + sx*sy*sz    m12 = -cy*sz + sx*sy*cz   m13 = cx*sy

 m21 = cx*sz               m22 = cx*cz               m23 = -sx

 m31 = -sy*cz + sx*cy*sz   m32 = sy*sz + sx*cy*cz    m33 = cx*cy

*/

void CreateEulerMatrix(EULER *e, MATRIXCH *m1)

{

    int sx = GetSin(e->EulerX);

    int sy = GetSin(e->EulerY);

    int sz = GetSin(e->EulerZ);

    int cx = GetCos(e->EulerX);

    int cy = GetCos(e->EulerY);

    int cz = GetCos(e->EulerZ);

    int sx_sy = MUL_FIXED(sx, sy);

    int sx_cy = MUL_FIXED(sx, cy);

    /* m11 = cy*cz + sx*sy*sz */

    m1->mat11 = MUL_FIXED(cy, cz) + MUL_FIXED(sx_sy, sz);

    /* m12 = -cy*sz + sx*sy*cz */

    m1->mat12 = MUL_FIXED(-cy, sz) + MUL_FIXED(sx_sy, cz);

    /* m13 = cx*sy */

    m1->mat13 = MUL_FIXED(cx, sy);

    /* m21 = cx*sz */

    m1->mat21 = MUL_FIXED(cx, sz);

    /* m22 = cx*cz */

    m1->mat22 = MUL_FIXED(cx, cz);

    /* m23 = -sx */

    m1->mat23 = -sx;

    /* m31 = -sy*cz + sx*cy*sz */

    m1->mat31 = MUL_FIXED(-sy, cz) + MUL_FIXED(sx_cy, sz);

    /* m32 = sy*sz + sx*cy*cz */

    m1->mat32 = MUL_FIXED(sy,sz) + MUL_FIXED(sx_cy, cz);

    /* m33 = cx*cy */

    m1->mat33 = MUL_FIXED(cx,cy);

}

/*

 Matrix Multiply Function

 A 3x3 Matrix is represented here as

 m11 m12 m13

 m21 m22 m23

 m31 m32 m33

 Row #1 (r1) of the matrix is m11 m12 m13

 Column #1 (c1) of the matrix is m11 m32 m31

 Under multiplication

 m'' = m x m'

 where

 m11'' = c1.r1'

 m12'' = c2.r1'

 m13'' = c3.r1'

 m21'' = c1.r2'

 m22'' = c2.r2'

 m23'' = c3.r2'

 m31'' = c1.r3'

 m32'' = c2.r3'

 m33'' = c3.r3'

*/

void MatrixMultiply(const MATRIXCH *m1, const MATRIXCH *m2,  MATRIXCH *m3)

{

    int mat11 = MUL_FIXED(m1->mat11, m2->mat11) + MUL_FIXED(m1->mat21, m2->mat12) + MUL_FIXED(m1->mat31, m2->mat13);

    int mat12 = MUL_FIXED(m1->mat12, m2->mat11) + MUL_FIXED(m1->mat22, m2->mat12) + MUL_FIXED(m1->mat32, m2->mat13);

    int mat13 = MUL_FIXED(m1->mat13, m2->mat11) + MUL_FIXED(m1->mat23, m2->mat12) + MUL_FIXED(m1->mat33, m2->mat13);

    int mat21 = MUL_FIXED(m1->mat11, m2->mat21) + MUL_FIXED(m1->mat21, m2->mat22) + MUL_FIXED(m1->mat31, m2->mat23);

    int mat22 = MUL_FIXED(m1->mat12, m2->mat21) + MUL_FIXED(m1->mat22, m2->mat22) + MUL_FIXED(m1->mat32, m2->mat23);

    int mat23 = MUL_FIXED(m1->mat13, m2->mat21) + MUL_FIXED(m1->mat23, m2->mat22) + MUL_FIXED(m1->mat33, m2->mat23);

    int mat31 = MUL_FIXED(m1->mat11, m2->mat31) + MUL_FIXED(m1->mat21, m2->mat32) + MUL_FIXED(m1->mat31, m2->mat33);

    int mat32 = MUL_FIXED(m1->mat12, m2->mat31) + MUL_FIXED(m1->mat22, m2->mat32) + MUL_FIXED(m1->mat32, m2->mat33);

    int mat33 = MUL_FIXED(m1->mat13, m2->mat31) + MUL_FIXED(m1->mat23, m2->mat32) + MUL_FIXED(m1->mat33, m2->mat33);

    m3->mat11 = mat11;

    m3->mat12 = mat12;

    m3->mat13 = mat13;

    m3->mat21 = mat21;

    m3->mat22 = mat22;

    m3->mat23 = mat23;

    m3->mat31 = mat31;

    m3->mat32 = mat32;

    m3->mat33 = mat33;

}

void TransposeMatrixCH(MATRIXCH *m1)

{

    int temp = m1->mat12;

    m1->mat12 = m1->mat21;

    m1->mat21 = temp;

    temp = m1->mat13;

    m1->mat13 = m1->mat31;

    m1->mat31 = temp;

    temp = m1->mat23;

    m1->mat23 = m1->mat32;

    m1->mat32 = temp;

}

/*

 Matrix Rotatation of a Vector

 Overwrite the Source Vector with the Rotated Vector

 x' = v.c1

 y' = v.c2

 z' = v.c3

*/

void RotateVector(VECTORCH *v, const MATRIXCH*  m)

{

    int x = MUL_FIXED(m->mat11, v->vx) + MUL_FIXED(m->mat21, v->vy) + MUL_FIXED(m->mat31, v->vz);

    int y = MUL_FIXED(m->mat12, v->vx) + MUL_FIXED(m->mat22, v->vy) + MUL_FIXED(m->mat32, v->vz);

    int z = MUL_FIXED(m->mat13, v->vx) + MUL_FIXED(m->mat23, v->vy) + MUL_FIXED(m->mat33, v->vz);

    v->vx = x;

    v->vy = y;

    v->vz = z;

}

/*

 Matrix Rotation of a Source Vector using a Matrix

 Copying to a Destination Vector

 x' = v.c1

 y' = v.c2

 z' = v.c3

*/

void RotateAndCopyVector(const VECTORCH *v1, VECTORCH *v2, const MATRIXCH *m)

{

    v2->vx = MUL_FIXED(m->mat11, v1->vx) + MUL_FIXED(m->mat21, v1->vy) + MUL_FIXED(m->mat31, v1->vz);

    v2->vy = MUL_FIXED(m->mat12, v1->vx) + MUL_FIXED(m->mat22, v1->vy) + MUL_FIXED(m->mat32, v1->vz);

    v2->vz = MUL_FIXED(m->mat13, v1->vx) + MUL_FIXED(m->mat23, v1->vy) + MUL_FIXED(m->mat33, v1->vz);

}

/*

 Matrix to Euler Angles

 Maths overflow is a real problem for this function. To prevent overflows

 the matrix Sines and Cosines are calculated using values scaled down by 4.

 sinx    =    -M23

 cosx    =    sqr ( 1 - sinx^2 )

 siny    =    M13 / cosx

 cosy    =    M33 / cosx

 sinz    =    M21 / cosx

 cosz    =    M22 / cosx

*/

int SqRoot32(int A)

{

    return sqrt( (float)A );

}

/* This function performs a Widening Multiply followed by a Narrowing Divide.  */

int WideMulNarrowDiv(const int a, const int b, const int c)

{

    return (int64_t)a * (int64_t)b /c;

}

#define m2e_scale 2

#define ONE_FIXED_S ((ONE_FIXED >> m2e_scale) - 1)

#define m2e_shift 14

#define j_and_r_change 1 

void MatrixToEuler(const MATRIXCH *m, EULER *e)

{

    int x, siny,cosy, abs_cosy;

    if(m->mat32 > -65500 && m->mat32 < 65500)

    {

        int abs_cosx,  abs_cosz, sinx, cosx, sinz, cosz;

        int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;

        int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;

        /* Yaw */

        /* Pitch */

        #if j_and_r_change

        SineMatrixPitch = -m->mat32;

        #else

        SineMatrixPitch = -m->mat23;

        #endif

        SineMatrixPitch >>= m2e_scale;

        CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;

        CosMatrixPitch >>= m2e_shift;

        CosMatrixPitch = -CosMatrixPitch;

        CosMatrixPitch += ONE_FIXED_S;

        CosMatrixPitch *= ONE_FIXED_S;

        CosMatrixPitch = SqRoot32(CosMatrixPitch);

        if(CosMatrixPitch) 

        {

            if(CosMatrixPitch > ONE_FIXED_S) 

                CosMatrixPitch = ONE_FIXED_S;

            else if(CosMatrixPitch < -ONE_FIXED_S)

                CosMatrixPitch = -ONE_FIXED_S;

        }

        else

            CosMatrixPitch = 1;

        SineMatrixYaw = WideMulNarrowDiv(

            #if j_and_r_change

            m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

            #else

            m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

            #endif

        CosMatrixYaw = WideMulNarrowDiv( m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

        /* Roll */

        SineMatrixRoll = WideMulNarrowDiv(

            #if j_and_r_change

            m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

            #else

            m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

            #endif

        CosMatrixRoll = WideMulNarrowDiv( m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);

        /* Tables are for values +- 2^16 */

        sinx = SineMatrixPitch << m2e_scale;

        siny = SineMatrixYaw   << m2e_scale;

        sinz = SineMatrixRoll  << m2e_scale;

        cosx = CosMatrixPitch << m2e_scale;

        cosy = CosMatrixYaw   << m2e_scale;

        cosz = CosMatrixRoll  << m2e_scale;

        /* Absolute Cosines */

        abs_cosx = abs(cosx);

        abs_cosy = abs(cosy);

        abs_cosz = abs(cosz);

        /* Euler X */

        if(abs_cosx > Cosine45) 

        {

            x = ArcSin(sinx);

            if(cosx < 0)

            {

                x =  -x;

                x += deg180;

                x &= wrap360;

            }

        }

        else

        {

            x = ArcCos(cosx);

            if(sinx < 0)

            {

                x =  -x;

                x &= wrap360;            

            }

        }

        #if ( 0 == j_and_r_change )

        x = -x;

        x &= wrap360;

        #endif

        e->EulerX = x;

        /* Euler Y */

        if(abs_cosy > Cosine45)

        {

            x = ArcSin(siny);

            if(cosy < 0)

            {

                x =  -x;

                x += deg180;

                x &= wrap360;

            }

        }

        else 

        {

            x = ArcCos(cosy);

            if(siny < 0)

            {

                x =  -x;

                x &= wrap360;            

            }

        }

        #if ( 0 == j_and_r_change )

        x = -x;

        x &= wrap360;

        #endif

        e->EulerY = x;

        /* Euler Z */

        if(abs_cosz > Cosine45) 

        {

            x = ArcSin(sinz);

            if(cosz < 0) 

            {

                x =  -x;

                x += deg180;

                x &= wrap360;

            }

        }

        else 

        {

            x = ArcCos(cosz);

            if(sinz < 0)

            {

                x =  -x;

                x &= wrap360;            

            }

        }

        #if ( 0 == j_and_r_change )

        x =  -x;

        x &= wrap360;

        #endif

        e->EulerZ = x;

    }

    else //singularity case

    {

        e->EulerX = (m->mat32 > 0) ? 3072 : 1024;

        e->EulerZ = 0;

        /* Yaw */

        siny = -m->mat13 ;

        cosy =     m->mat11 ;

        abs_cosy = abs(cosy);

        if(abs_cosy > Cosine45) 

        {

            x = ArcSin(siny);

            if(cosy < 0) 

            {

                x =  -x;

                x += deg180;

                x &= wrap360;

            }

        }

        else

        {

            x = ArcCos(cosy);

            if(siny < 0) 

            {

                x =  -x;

                x &= wrap360;            

            }

        }

        #if ( 0 == j_and_r_change )

        x = -x;

        x &= wrap360;

        #endif

        e->EulerY = x;

    }

}

/*

 Normalise a Matrix

 Dot the three vectors together (XY, XZ, YZ) and take the two nearest to

 90ř from each other. Cross them to create a new third vector, then cross

 the first and third to create a new second.

*/

void MNormalise(MATRIXCH *m)

{

    VECTORCH *x = (VECTORCH *) &m->mat11;

    VECTORCH *y = (VECTORCH *) &m->mat21;

    VECTORCH *z = (VECTORCH *) &m->mat31;

    int dotxy = DotProduct(x, y);

    int dotxz = DotProduct(x, z);

    int dotyz = DotProduct(y, z);

    VECTORCH *s;

    VECTORCH *t;

    VECTORCH u;

    VECTORCH v;

    VECTORCH zero = {0, 0, 0};

    /* Find the two vectors nearest 90ř */

    if(dotxy > dotxz && dotxy > dotyz) 

    {

        /* xy are the closest to 90ř */

        /*printf("xy\n");*/

        s = x;

        t = y;

        MakeNormal(&zero, s, t, &u);        /* Cross them for a new 3rd vector */

        MakeNormal(&zero, s, &u, &v);        /* Cross 1st & 3rd for a new 2nd */

        v.vx = -v.vx;

        v.vy = -v.vy;

        v.vz = -v.vz;

        *z = u;

        *y = v;

    }

    else if(dotxz > dotxy && dotxz > dotyz) 

    {

        /* xz are the closest to 90ř */

        /*printf("xz\n");*/

        s = x;

        t = z;

        MakeNormal(&zero, s, t, &u);        /* Cross them for a new 3rd vector */

        u.vx = -u.vx;

        u.vy = -u.vy;

        u.vz = -u.vz;

        MakeNormal(&zero, s, &u, &v);        /* Cross 1st & 3rd for a new 2nd */

        *y = u;

        *z = v;

    }

    else

    {

        /* yz are the closest to 90ř */

        /*printf("yz\n");*/

        s = y;

        t = z;

        MakeNormal(&zero, s, t, &u);        /* Cross them for a new 3rd vector */

        MakeNormal(&zero, s, &u, &v);        /* Cross 1st & 3rd for a new 2nd */

        v.vx = -v.vx;

        v.vy = -v.vy;

        v.vz = -v.vz;

        *x = u;

        *z = v;

    }

}

/*

 ArcCos

 In:  COS value as -65,536 -> +65,536.

 Out: Angle in 0 -> 4095 form.

 Notes:

 The angle returned is in the range 0 -> 2,047 since the sign of SIN

 is not known.

 ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )

 ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)

 -65,536 = 180 Degrees

 0      = 90 Degrees

 +65,536 = 0 Degrees

 The table has 4,096 entries.

*/

int ArcCos(int c)

{

    if(c < (-(ONE_FIXED - 1)))

        c = -(ONE_FIXED - 1);

    else if(c > (ONE_FIXED - 1))

        c = ONE_FIXED - 1;

    int16_t acos = ArcCosTable[(c >> 5) + 2048];

return (acos & wrap360);

}

/*

 ArcSin

 In:  SIN value in ax as -65,536 -> +65,536.

 Out: Angle in 0 -> 4095 form in ax.

 Notes:

 The angle returned is in the range -1,024 -> 1,023 since the sign of COS

 is not known.

 ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )

 ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)

 -65,536 = 270 Degrees

 0      = 0 Degrees

 +65,536 = 90 Degrees

 The table has 4,096 entries.

*/

int ArcSin(int s)

{

    if(s < (-(ONE_FIXED - 1)))

        s = -(ONE_FIXED - 1);

    else if(s > (ONE_FIXED - 1))

        s = ONE_FIXED - 1;

    int16_t asin = ArcSineTable[(s >> 5) + 2048];

return (asin & wrap360);

}

/*

 ArcTan

 Pass (x,z)

 And ATN(x/z) is returned such that:

 000ř is Map North

 090ř is Map East

 180ř is Map South

 270ř is Map West

*/

int ArcTan(int height_x, int width_z)

{

    int abs_height_x, abs_width_z, angle, signsame = 0;

    int sign = 0;

    if((height_x < 0 && width_z < 0) || (height_x >=0 && width_z >= 0))

        signsame = 1;

    abs_height_x = abs(height_x);

    abs_width_z = abs(width_z);

    if(!width_z)

        angle = -deg90;

    else if(abs_width_z == abs_height_x)

        angle = deg45;

    else 

    {

        if(abs_width_z > abs_height_x)

        {

            int temp = abs_width_z;

            abs_width_z = abs_height_x;

            abs_height_x = temp;

            sign = -1;

        }

        if(abs_height_x != 0)

            angle = (abs_width_z << 8)/ abs_height_x;

        else

            angle = deg22pt5;

        angle = ArcTanTable[angle];

        if(sign >= 0) 

        {

            angle = -angle;

            angle += deg90;

        }

    }

    if(!signsame)

        angle = -angle;

    if(width_z <= 0)

        angle += deg180;

    angle &= wrap360;

return angle;

}

static int CMP_LL(int64_t *a, int64_t *b)

{

    return (a < b) ? -1 : (a > b);

}

/* 

"Yes" if "point" is inside "polygon"

 **************************************************

 WARNING!! Point and Polygon Data are OVERWRITTEN!!

 **************************************************

 The function requires point to be an integer array containing a single

 XY pair. The number of points must be passed too.

 Pass the size of the polygon point e.g. A Gouraud polygon has points X,Y,I

 so its point size would be 3.

 Item                                Polygon Point Size

 ----                                ------------------

 I_Polygon            2

 I_GouraudPolygon        3

 I_2dTexturedPolygon        4

 I_3dTexturedPolygon,        5

 I_Gouraud2dTexturedPolygon    5

 I_Polygon_ZBuffer        3

 I_GouraudPolygon_ZBuffer    4

 PASS ONLY POSITIVE COORDINATES!

*/

int PointInPolygon(int* point, int* polygon, int c, int ppsize)

{

  /* Tim's New Point In Polygon test-- hopefully much faster, */

  /* certainly much smaller. */

  /* Uses Half-Line test for point-in-2D-polygon test */

  /* Tests the half-line going from the point in the direction of positive z */

  int  x, z;        /* point */

  int  sx, sz;      /* vertex 1 */

  int *polyp;       /* vertex 2 pointer */

  int  t;

  int  dx = 0;      /* ABS(vertex 2 - vertex 1) */

  int  sgnx;        /* going left or going right */

  int  intersects = 0;  /* number of intersections so far discovered */

  /* reject lines and points */

  if (c < 3) 

    return 0;

  x = point[ix];

  z = point[iy];  /* ! */

  /* get last point */

  polyp = polygon + ((c - 1) * ppsize);

  sx = polyp[0];

  sz = polyp[1];

  /* go back to first point */

  polyp = polygon;

  /* for each point */

  while (0 != c)

  {

    /* is this line straddling the x co-ordinate of the point? */

    /* if not it is not worth testing for intersection with the half-line */

    /* we must be careful to get the strict and non-stict inequalities */

    /* correct, or we may count intersections with vertices the wrong number */

    /* of times. */

    sgnx = 0;

    if (sx < x && x <= polyp[0])

    {

      /* going right */

      sgnx = 1;

      dx = polyp[0] - sx;

    }

    if (polyp[0] < x && x <= sx)

    {

      /* going left */

      sgnx = -1;

      dx   = sx - polyp[0];

    }

    /* if sgnx is zero then neither of the above conditions are true, */

    /* hence the line does not straddle the point in x */

    if (0 != sgnx)

    {

      /* next do trivial cases of line totally above or below point */

      if (z < sz && z < polyp[1])

      {

        /* line totally above point -- intersection */

        intersects++;

      }

      else if (z <= sz || z <= polyp[1])

      {

        /* line straddles point in both x and z -- we must do interpolation */

        /* get absolute differences between line end z co-ordinates */

        int dz = (sz < polyp[1]) ? (polyp[1] - sz) : (sz - polyp[1]);

        /* B504 is the square root of 7FFFFFFF */

        if (0xB504L < dx || 0xB504L < dz)

        { 

          /* LARGE line -- use 64-bit values */

          /* interpolate z */

    int64_t a_ll = (polyp[1] - sz) * (x - sx);

    int64_t b_ll = (polyp[0] - sx) * (z - sz);

          if(CMP_LL(&a_ll, &b_ll) == sgnx)

        intersects++;

        }

        else

        {

          /* small line -- use 32-bit values */

          /* interpolate z */

          t = (polyp[1] - sz) * (x - sx) - (polyp[0] - sx) * (z - sz);

          if ((t < 0 && sgnx < 0) || (0 < t && 0 < sgnx))

            intersects++;

        }

      } /* (if line straddles point in z) */

    } /* (if line straddles point in x) */

    /* get next line : */

    /* new vertex 1 is old vertex 2 */

    sx = polyp[0];

    sz = polyp[1];

    /* new vertex 2 is next point */

    polyp += ppsize;