/*

   File geometry.c

   Contains the geometry functions which need heavy optimisation
   currently compile with -opt 4

   Phil Atkin

   (c) Division Ltd. 1991

*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "pazpl5.h"

#define sqrt(f) sqrtf(f)
#define sin(f)  sinf(f)
#define cos(f)  cosf(f)

/*{{{  #define copy_row()*/
#define copy_row(s,d) \
sx=*s++; sy=*s++; sz=*s++; sw=*s++; \
*d++=sx; *d++=sy; *d++=sz; *d++=sw
/*}}}  */
/*{{{  #define cache_opt_matrix(m)*/
#define cache_opt_matrix(m)    \
register float m00=m[0][0];    \
register float m01=m[0][1];    \
register float m02=m[0][2];    \
register float m10=m[1][0];    \
register float m11=m[1][1];    \
register float m12=m[1][2];    \
register float m20=m[2][0];    \
register float m21=m[2][1];    \
register float m22=m[2][2];    \
register float m30=m[3][0];    \
register float m31=m[3][1];    \
register float m32=m[3][2];    \
register float m33=m[3][3];    \
register float sx;             \
register float sy;             \
register float sz;

/*}}}  */
/*{{{  #define cache_full_matrix(m)*/
#define cache_full_matrix(m)   \
register float m00=m[0][0];    \
register float m01=m[0][1];    \
register float m02=m[0][2];    \
register float m03=m[0][3];    \
register float m10=m[1][0];    \
register float m11=m[1][1];    \
register float m12=m[1][2];    \
register float m13=m[1][3];    \
register float m20=m[2][0];    \
register float m21=m[2][1];    \
register float m22=m[2][2];    \
register float m23=m[2][3];    \
register float m30=m[3][0];    \
register float m31=m[3][1];    \
register float m32=m[3][2];    \
register float m33=m[3][3];    \
register float sx;             \
register float sy;             \
register float sz;             \
register float sw;

/*}}}  */
/*{{{  #define cache_norm_matrix(m)*/
#define cache_norm_matrix(m)   \
register float m00=m[0][0];    \
register float m01=m[0][1];    \
register float m02=m[0][2];    \
register float m10=m[1][0];    \
register float m11=m[1][1];    \
register float m12=m[1][2];    \
register float m20=m[2][0];    \
register float m21=m[2][1];    \
register float m22=m[2][2];    \
register float sx;             \
register float sy;             \
register float sz;             \
register float dx;             \
register float dy;             \
register float dz;             \
register float hyp

/*}}}  */
/*{{{  #define xform_in_place(s)*/
#define xform_in_place(s)                         \
  sx   =*s++;                                     \
  sy   =*s++;                                     \
  sz   =*s++;                                     \
  *s-- = m33;           /* W frig ! */            \
  *s-- = (sx*m02) + (sy*m12) + (sz*m22) + m32;    \
  *s-- = (sx*m01) + (sy*m11) + (sz*m21) + m31;    \
  *s   = (sx*m00) + (sy*m10) + (sz*m20) + m30
/*}}}  */
/*{{{  #define xform(s,d)*/
#define xform(s,d)                              \
  sx =*s++;                                     \
  sy =*s++;                                     \
  sz =*s++;                                     \
  *d++ = (sx*m00) + (sy*m10) + (sz*m20) + m30;  \
  *d++ = (sx*m01) + (sy*m11) + (sz*m21) + m31;  \
  *d++ = (sx*m02) + (sy*m12) + (sz*m22) + m32;  \
  *d++ = m33
/*}}}  */
/*{{{  #define fullxform(s,d)*/
#define fullxform(s,d)                               \
  sx =*s++; sy =*s++; sz =*s++; sw=*s++;             \
  *d++ = (sx*m00) + (sy*m10) + (sz*m20) + (sw*m30);  \
  *d++ = (sx*m01) + (sy*m11) + (sz*m21) + (sw*m31);  \
  *d++ = (sx*m02) + (sy*m12) + (sz*m22) + (sw*m32);  \
  *d++ = (sx*m03) + (sy*m13) + (sz*m23) + (sw*m33)
/*}}}  */
/*{{{  #define nxform(s,d)*/
#define nxform(s,d) \
  sx =*s++;   \
  sy =*s++;   \
  sz =*s++;   \
  dx = (sx*m00) + (sy*m10) + (sz*m20);       \
  dy = (sx*m01) + (sy*m11) + (sz*m21);       \
  dz = (sx*m02) + (sy*m12) + (sz*m22);       \
  hyp= 1.0f/sqrt((dx*dx)+(dy*dy)+(dz*dz)); \
  *d++=dx*hyp; *d++=dy*hyp; *d++=dz*hyp
/*}}}  */
/*{{{  #define nxform_un_norm(s,d)*/
#define nxform_un_norm(s,d) \
  sx =*s++;   \
  sy =*s++;   \
  sz =*s++;   \
  *d++ = (sx*m00) + (sy*m10) + (sz*m20);       \
  *d++ = (sx*m01) + (sy*m11) + (sz*m21);       \
  *d++ = (sx*m02) + (sy*m12) + (sz*m22)
/*}}}  */
/*{{{  #define dot(d,a,b)*/
#define dot(d,a,b)                \
ax=*a++;                          \
bx=*b++;                          \
ay=*a++;                          \
by=*b++;                          \
az=*a++;                          \
bz=*b++;                          \
d=((ax*bx)+(ay*by)+(az*bz))
/*}}}  */
/*{{{  #define sdot(d,a,b)*/
#define sdot(d,a,b,cook)         \
ax=*a++;                         \
bx=*b++;                         \
ay=*a++;                         \
by=*b++;                         \
az=*a++;                         \
bz=*b++;                         \
d=cook*((ax*bx)+(ay*by)+(az*bz))
/*}}}  */

#define solve(point,plane) \
  (point[X]*plane[X])+(point[Y]*plane[Y])+(point[Z]*plane[Z])+plane[W]

/*{{{  static float dbl_plane_term ( double ay, double by, double cy,*/
static float dbl_plane_term ( double ay, double by, double cy,
                              double az, double bz, double cz )

{
  double r;

  r = (ay*(bz-cz))+(by*(cz-az))+(cy*(az-bz));

  return (float) r;

}
/*}}}  */
/*{{{  void plane_eqn ( POINT pp, POINT aa, POINT bb, POINT cc )*/
void plane_eqn ( dpl_POINT pp, dpl_POINT aa, dpl_POINT bb, dpl_POINT cc )
{
  /* extern float fn_plane_term ( float a, float b, float c, float d, float e, float f ); */
  register double ax=(double) aa[dpl_X];
  register double ay=(double) aa[dpl_Y];
  register double az=(double) aa[dpl_Z];

  register double bx=(double) bb[dpl_X];
  register double by=(double) bb[dpl_Y];
  register double bz=(double) bb[dpl_Z];

  register double cx=(double) cc[dpl_X];
  register double cy=(double) cc[dpl_Y];
  register double cz=(double) cc[dpl_Z];
  double t;

  pp[dpl_X] = (float) dbl_plane_term (ay,by,cy,az,bz,cz);
  pp[dpl_Y] = (float) dbl_plane_term (az,bz,cz,ax,bx,cx);
  pp[dpl_Z] = (float) dbl_plane_term (ax,bx,cx,ay,by,cy);

  /*
  t     = -((ax*(by*cz -  cy*bz)) +
            (bx*(cy*az -  ay*cz)) +
            (cx*(ay*bz -  by*az)));
  */
  /* NOTE - we do NOT save the correct plane equation, we negate the
     d term, so that our comparison becomes   ax + by + c > -d,
  */

  t     =  ((ax*(by*cz -  cy*bz)) +
            (bx*(cy*az -  ay*cz)) +
            (cx*(ay*bz -  by*az)));

  pp[dpl_W]=(float) t;
}
/*}}}  */

/*{{{  void for_polystrip_plane_eqns ( VSTRIP *v )*/
void for_polystrip_plane_eqns ( VSTRIP *v )
{
  VERTEX *v00, *v0, *v1, *vert;
  int n;

  v00=v->head;
  v0=v00;
  v1=v0->next;
  vert=v1->next;

  for (n=v->vertex_count-2; n>0; n-- ) {
    plane_eqn(v0->planeEqn, v00->position, v1->position, vert->position );
    v0=v1;
    v1=vert;
    vert=vert->next;
  }
}
/*}}}  */
/*{{{  static void for_tristrip_plane_eqns ( VSTRIP *v )*/
static void for_tristrip_plane_eqns ( VSTRIP *v )
{
  VERTEX *v0, *v1, *vert;
  int i, n;

  n   = v->vertex_count-2;
  v0  = v->head;
  v1  = v0->next;
  vert= v1->next;

  for (i=0; i<n; i++ ) {
    if (i&1)
      plane_eqn(v0->planeEqn, v0->position, vert->position, v1->position );
    else
      plane_eqn(v0->planeEqn, v0->position, v1->position, vert->position );

    v0  = v1;
    v1  = vert;
    vert= vert->next;
  }
}
/*}}}  */
/*{{{  int bbox_plane ( POINT *bound, POINT plane )*/
int bbox_plane ( dpl_POINT *b, dpl_POINT plane )
{
  register i, posses=0;

  for (i=0; i<8; i++) {

    posses+=(((*b)[0]*plane[0])+
             ((*b)[1]*plane[1])+
             ((*b)[2]*plane[2])+
             plane[3]) > 0.0f;

    b++;
  }
  return posses;
}
/*}}}  */


/*{{{  void compute_plane_eqns ( PATCH *p, int backwards )*/
void compute_plane_eqns ( PATCH *p, int backwards )
{
  VSTRIP *v;

  for (v=p->head; v!=NULL; v=v->next ) {
    if (v->strip_type == strip_type_tri)
      for_tristrip_plane_eqns(v);

    else if (v->strip_type == strip_type_poly)
      for_polystrip_plane_eqns(v);

  }
}
/*}}}  */
/*{{{  void compute_gnorms ( VSTRIP *v )*/
void compute_gnorms ( VSTRIP *v )
{
  register VERTEX *vert=v->head;
  register int n;
  register float x;
  register float y;
  register float z;
  register float hyp;
  register float *f;
  register float *g;

  /* needs fixing for supervision */

  if ((v->strip_shade & (strip_shade_smooth | strip_shade_coloured)) != 0)
    /* return if smooth OR coloured */
    return;

  for (n=v->vertex_count-2; n>0; n-- ) {

    x=vert->planeEqn[0];
    y=vert->planeEqn[1];
    z=vert->planeEqn[2];

    hyp= sqrt ((x*x)+(y*y)+(z*z));

    vert=vert->next;
    /* gnorm is in NEXT vertex remember */
    vert->normcol[0] = x / hyp;
    vert->normcol[1] = y / hyp;
    vert->normcol[2] = z / hyp;

  }
}
/*}}}  */
/*{{{  void compute_patch_gnorms ( PATCH *p )*/
void compute_patch_gnorms ( PATCH *p )
{
  VSTRIP *v;

  for (v=p->head; v!=NULL; v=v->next)
    compute_gnorms (v);
}
/*}}}  */

/*{{{  void compute_pmesh_plane_eqns ( PATCH *p, int backwards )*/
void compute_pmesh_plane_eqns ( PATCH *p, int backwards )
{
  /*
     this function computes plane equations, facet normals, and
     if necessary averages these normals into the pmesh vertex normals
  */
  extern void quick_renorm ( float *t, float a, float b, float c, float scale );

  VSTRIP *verts, *triangles;
  VERTEX *vert;
  int i, smooth=1;
  float x, y, z, hyp;

  verts=p->head;
  triangles=verts->next;

  if ((verts->strip_shade & (strip_shade_smooth | strip_shade_coloured)) != 0)
    smooth=0;
  else {
    verts->strip_shade |= strip_shade_smooth;
    verts->normals=1;
  }

  if (smooth) {
    for (i=verts->vertex_count, vert=verts->head; i; i--, vert++ ) {
      vert->normcol [0]=0.0f;
      vert->normcol [1]=0.0f;
      vert->normcol [2]=0.0f;
      vert->planeEqn[0]=0.0f;
    }
  }

  for (i=0, vert=triangles->head; vert; i++, vert=vert->next ) {
    int *ipos=(int *) vert->position;
    VERTEX *va, *vb, *vc;

    /* compute a plane equation and a facet normal for this triangle */

    va=(VERTEX *) (ipos[0]);
    vb=(VERTEX *) (ipos[1]);
    vc=(VERTEX *) (ipos[2]);

    plane_eqn(vert->planeEqn, va->position, vb->position, vc->position );

    if (smooth) {

      x=vert->planeEqn[0];
      y=vert->planeEqn[1];
      z=vert->planeEqn[2];

      quick_renorm ( vert->normcol, x, y, z, 1.0f );

      x=vert->normcol[0];
      y=vert->normcol[1];
      z=vert->normcol[2];

      va->normcol [0]+=x;
      va->normcol [1]+=y;
      va->normcol [2]+=z;
      va->planeEqn[0]+=1.0f;

      vb->normcol [0]+=x;
      vb->normcol [1]+=y;
      vb->normcol [2]+=z;
      vb->planeEqn[0]+=1.0f;

      vc->normcol [0]+=x;
      vc->normcol [1]+=y;
      vc->normcol [2]+=z;
      vc->planeEqn[0]+=1.0f;
    }
  }

  if (smooth) {
    VERTEX *orig;
    /* re-normalize normals into clone list */
    for (i=verts->vertex_count,
         vert=verts->head; i; i--, vert++ ) {

      x=vert->normcol[0];
      y=vert->normcol[1];
      z=vert->normcol[2];

      quick_renorm ( vert->normcol, x, y, z, 1.0f );
    }
  }
}
/*}}}  */

/*{{{  int morph_step ( OBJECT *srca, OBJECT *srcb, OBJECT *dst, float t )*/
static OBJECT *currentA=NULL,
              *currentB=NULL,
              *currentD=NULL;

static int allowmorph=0;

void morphize ( float *a, float *b, float *d, float t, int n )
{
  register float one_minus_t=1.0f-t;

  for (; n; n--) {
    *d++=t*(*b++) + (one_minus_t)*(*a++);
  }
}

int morph_step ( OBJECT *srca, OBJECT *srcb, OBJECT *dst, float t )
{
  extern void build_bound ( POINT *bound, POINT mini, POINT maxi );

  PATCH  *pa, *pb, *pd;
  VSTRIP *va, *vb, *vd;
  VERTEX *verta, *vertb, *vertd;
  register float one_minus_t=1.0f-t;
  register int n;

  /*{{{  check whether morphing is allowed, fix bounding box*/
  if ((currentA != srca) ||
      (currentB != srcb) ||
      (currentD != dst)) {
    POINT pta, ptb, ptd, mini, maxi;
  
    /* check topological consistency */
    allowmorph=1;
  
    currentA=srca;
    currentB=srca;
    currentD=dst;
  
    pa=srca->head, pb=srcb->head, pd=dst->head;
  
    while (pa) {
      if (pb == NULL) {
        allowmorph=0;
        return 0;
      }
      if (pd == NULL) {
        allowmorph=0;
        return 0;
      }
      /* now check each vstrip */
      va=pa->head, vb=pb->head, vd=pd->head;
      while (va) {
        if (pb == NULL) {
          allowmorph=0;
          return 0;
        }
        if (pd == NULL) {
          allowmorph=0;
          return 0;
        }
        if (va->vertex_count != vb->vertex_count) {
          allowmorph=0;
          return 0;
        }
        if (vb->vertex_count != vd->vertex_count) {
          allowmorph=0;
          return 0;
        }
        va=va->next;
        vb=vb->next;
        vd=vd->next;
      }
      /* now rebound dest patch according to srcA and src B */
      memcpy ( pta, &pa->bound[0], sizeof(POINT));
      memcpy ( ptb, &pb->bound[0], sizeof(POINT));
      if (pta[X] < ptb[X]) mini[X]=pta[X]; else mini[X]=ptb[X];
      if (pta[Y] < ptb[Y]) mini[Y]=pta[Y]; else mini[Y]=ptb[Y];
      if (pta[Z] < ptb[Z]) mini[Z]=pta[Z]; else mini[Z]=ptb[Z];
  
      memcpy ( pta, &pa->bound[7], sizeof(POINT));
      memcpy ( ptb, &pb->bound[7], sizeof(POINT));
      if (pta[X] > ptb[X]) maxi[X]=pta[X]; else maxi[X]=ptb[X];
      if (pta[Y] > ptb[Y]) maxi[Y]=pta[Y]; else maxi[Y]=ptb[Y];
      if (pta[Z] > ptb[Z]) maxi[Z]=pta[Z]; else maxi[Z]=ptb[Z];
  
      build_bound ( pd->bound, mini, maxi );
  
      pa=pa->next;
      pb=pb->next;
      pd=pd->next;
    }
  
    reboundObject ( dst );
  }
  /*}}}  */

  /*{{{  fix t*/
  if (t<0.0f) t=0.0f;
  if (t>1.0f) t=1.0f;
  /*}}}  */

  if (allowmorph) {
    /*{{{  morph each destination vertex*/
    for (pa=srca->head, pb=srcb->head, pd=dst->head;
         pa;
         pa=pa->next, pb=pb->next, pd=pd->next ) {
    
      /* morph this patch */
      for (va=pa->head, vb=pb->head, vd=pd->head;
           va;
           va=va->next, vb=vb->next, vd=vd->next ) {
    
    
        for (verta=va->head, vertb=vb->head, vertd=vd->head, n=va->vertex_count;
             n;
             n--, verta++, vertb++, vertd++ ) {
    
          register float *fd, *fa, *fb;
    
          fd=&vertd->position[0];
          fa=&verta->position[0];
          fb=&vertb->position[0];
    
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
    
          fd=&vertd->normcol[0];
          fa=&verta->normcol[0];
          fb=&vertb->normcol[0];
    
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
    
          fd=&vertd->texcoords[0];
          fa=&verta->texcoords[0];
          fb=&vertb->texcoords[0];
    
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
          *fd++=t*(*fb++) + (one_minus_t)*(*fa++);
    
        }
      }
      compute_plane_eqns   ( pd, 0 );
      compute_patch_gnorms ( pd );
    }
    /*}}}  */
  }

  return allowmorph;
}
/*}}}  */

/*{{{  void project_points ( float *p, VIEW *eye, int n_points, int stride )*/
void project_points ( float *p, cull_VIEW *eye, int n_points, int stride )
{
  /*
      all projections now go thru this, relying on the contiguity of vertices
  */
  extern float sub_pxl_correct;
  int n, i;
  float Czscale=1.04857588E+06f;
  register float john_rhoades=sub_pxl_correct;
  register float px, py, pz, invz, shx, shy, wx, wy, wz, zscale;

  wx=eye->screen_half_width;
  wy=eye->screen_half_height;

  shx=wx*(eye->shift_x+1.0f);
  shy=wy*(eye->shift_y+1.0f);

  /* is this totally bogus ? */
  /* need to compute near/z - then at z=near, z=1, at z=inf, z=0 */

  wz=eye->zscale;

  for ( n=n_points; n; n-- ) {
    invz=eye->d/p[2];

    px=((p[0]*wx*invz)+shx)+john_rhoades;
    py=((p[1]*wy*invz)+shy)+john_rhoades;
    pz=wz*invz*Czscale;

    p[0] = px-john_rhoades;
    p[1] = py-john_rhoades;
    p[2] = pz;

    p+=stride;
  }
}
/*}}}  */
/*{{{  void project_spheres ( float *p, VIEW *eye, int n_points, int stride )*/
void project_spheres ( float *p, cull_VIEW *eye, int n_points, int stride,
                       float scale_fac )
{
  /*
      all projections now go thru this, relying on the contiguity of vertices
  */
  extern float sub_pxl_correct;
  int n, i;
  float Czscale=1.04857588E+06f;
  register float john_rhoades=sub_pxl_correct;
  register float px, py, pz, pr, invz, shx, shy, wx, wy, wz, zscale;

  wx=eye->screen_half_width;
  wy=eye->screen_half_height;

  shx=wx*(eye->shift_x+1.0f);
  shy=wy*(eye->shift_y+1.0f);

  /* need to compute near/z - then at z=near, z=1, at z=inf, z=0 */

  wz=Czscale * eye->zscale;

  for ( n=n_points; n; n-- ) {
    /* r =  radius*xrat*zci; */
    /* rz = radius*hithp*dglob->zres/(ze*(ze-radius)); */
    pz=p[2];
    invz=eye->d/pz;
    pr=p[3];

    px=((p[0]*wx*invz)+shx)+john_rhoades;
    py=((p[1]*wy*invz)+shy)+john_rhoades;

    p[0] = px-john_rhoades;
    p[1] = py-john_rhoades;
    p[2] = wz*invz;
    p[3] = scale_fac*pr*wx*invz;

    /* planeEqn[3] = z-space screen radius */
    p[7] = (pr * wz) / (pz * (pz - pr));

    p+=stride;
  }
}
/*}}}  */