#include <graf.h>

static int UTL_SphereQuality=5,UTL_SphereInitDone=FALSE;

static FLOAT UTL_CosA[256];
static FLOAT UTL_SinA[256];
static FLOAT UTL_CosB[256];
static FLOAT UTL_SinB[256];

extern int USR_BackFace;

void utl_sphere_quality(int quality)
{
    int i,j,Division = 4*quality;
    FLOAT a,Pi=2*acos(0.0);

    UTL_SphereQuality = quality;

    for( i=0; i<=Division; i++ )
    {
        a = 2*Pi*i/(float)Division;
        UTL_CosA[i] = cos(a);
        UTL_SinA[i] = sin(a);
    }

    for( i=-Division/2,j=0; i<=Division/2; i+=2, j++ )
    {
        a = Pi*i/(float)Division;
        UTL_CosB[j] = cos(a);
        UTL_SinB[j] = sin(a);
    }

    UTL_SphereInitDone = TRUE;
}

void utl_sphere(FLOAT x,FLOAT y,FLOAT z,FLOAT r)
{
    int Division = 4*UTL_SphereQuality;
    int i,j;

    FLOAT v[3];
    FLOAT R[VERTEX_NCOMP];

    usr_backface(TRUE);

    trs_push_matrix();
    trs_translate(x,y,z);
    trs_scale(r,r,r);

    for( i=0; i<Division/2; i++ )
    for( j=0; j<Division; j++ )
    {
        usr_bgnpolygon();

        v[0] = UTL_CosA[j]*UTL_CosB[i];
        v[1] = UTL_SinA[j]*UTL_CosB[i];
        v[2] = UTL_SinB[i];
        usr_normal(v);
        usr_vertex(v);

        if ( i != 0  )
        {
            v[0] = UTL_CosA[j+1]*UTL_CosB[i];
            v[1] = UTL_SinA[j+1]*UTL_CosB[i];
            v[2] = UTL_SinB[i];
            usr_normal(v);
            usr_vertex(v);
        }

        v[0] = UTL_CosA[j+1]*UTL_CosB[i+1];
        v[1] = UTL_SinA[j+1]*UTL_CosB[i+1];
        v[2] = UTL_SinB[i+1];
        usr_normal(v);
        usr_vertex(v);

        if ( i+1  != Division )
        {
            v[0] = UTL_CosA[j]*UTL_CosB[i+1];
            v[1] = UTL_SinA[j]*UTL_CosB[i+1];
            v[2] = UTL_SinB[i+1];
            usr_normal(v);
            usr_vertex(v);
        }
        usr_endpolygon();
    }

    trs_pop_matrix();

    usr_backface(USR_BackFace);
}

void utl_vector_to_angles(FLOAT vx,FLOAT vy,FLOAT vz,FLOAT *px,FLOAT *py,FLOAT *pz)
{
     FLOAT s,c,l,t,alpha,beta;
     FLOAT r,x,y,z,AScale=90.0/asin(1.0);

     x = *px - vx;
     y = *py - vy;
     z = vz - *pz;

     r = sqrt(x*x+y*y+z*z);
     x /= r;
     y /= r;
     z /= r;

     beta = atan2(y,z);
     r = y*sin(beta) + z*cos(beta);
     alpha = atan2(r,x);

     *px = AScale*beta;
     *py = AScale*alpha;
     *pz = 0.0;
}

void utl_cylinder(FLOAT x0,FLOAT y0,FLOAT z0,FLOAT f0,FLOAT x1,FLOAT y1,FLOAT z1,FLOAT f1,FLOAT R)
{
    FLOAT Pi=2*acos(0.0);
    FLOAT a0,a1,v[3],n[3],ax,ay,az,r;

    int i,j;

    if ( !UTL_SphereInitDone ) utl_sphere_quality(UTL_SphereQuality);

    ax = x1;
    ay = y1;
    az = z1;

    x1 = ax - x0;
    y1 = ay - y0;
    z1 = az - z0;
    r = sqrt(x1*x1+y1*y1+z1*z1);

    utl_vector_to_angles(x0,y0,z0,&ax,&ay,&az);

    trs_push_matrix();

    trs_translate(x0,y0,z0);

    trs_rotate( ax,'x' );
    trs_rotate( ay,'y' );
    trs_scale( r,R,R );
    
    x0 = 0;
    x1 = 1;
    n[0] =  0;
    for( j=0; j<UTL_SphereQuality*4;j++ )
    {
        y0 = UTL_SinA[j];
        z0 = UTL_CosA[j];
        y1 = UTL_SinA[j+1];
        z1 = UTL_CosA[j+1];

        usr_bgnpolygon();

        v[0] = x0;
        v[1] = n[1] = y0;
        v[2] = n[2] = z0;
        usr_texture(usr_pack3f(f0,0.0,0.0));
        usr_normal(n);
        usr_vertex(v);

        v[0] = x1;
        usr_texture(usr_pack3f(f1,0.0,0.0));
        usr_vertex(v);

        v[1] = n[1] = y1;
        v[2] = n[2] = z1;
        usr_normal(n);
        usr_vertex(v);

        v[0] = x0;
        usr_texture(usr_pack3f(f0,0.0,0.0));
        usr_vertex(v);

        usr_endpolygon();
    }

    trs_pop_matrix();
}

void utl_cone(FLOAT x0,FLOAT y0,FLOAT z0,FLOAT f0,FLOAT R0,FLOAT x1,FLOAT y1,FLOAT z1,FLOAT f1,FLOAT R1)
{
    FLOAT Pi=2*acos(0.0);
    FLOAT a0,a1,v[3],n[3],ax,ay,az,r;

    int i,j;

    if ( !UTL_SphereInitDone ) utl_sphere_quality(UTL_SphereQuality);

    ax = x1;
    ay = y1;
    az = z1;

    x1 = ax - x0;
    y1 = ay - y0;
    z1 = az - z0;
    r = sqrt(x1*x1+y1*y1+z1*z1);

    utl_vector_to_angles(x0,y0,z0,&ax,&ay,&az);

    trs_push_matrix();

    trs_translate(x0,y0,z0);

    trs_rotate( ax,'x' );
    trs_rotate( ay,'y' );
    trs_scale( r,1.0,1.0 );
    
    x0 = 0;
    x1 = 1;
    n[0] = (R0-R1);
    for( j=0; j<UTL_SphereQuality*4;j++ )
    {
        usr_bgnpolygon();

        v[0] = x0;
        v[1] = R0*UTL_SinA[j];
        v[2] = R0*UTL_CosA[j];
        n[1] = UTL_SinA[j];
        n[2] = UTL_CosA[j];
        usr_texture(usr_pack3f(f0,0.0,0.0));
        usr_normal(n);
        usr_vertex(v);

        v[0] = x1;
        v[1] = R1*UTL_SinA[j];
        v[2] = R1*UTL_CosA[j];
        usr_texture(usr_pack3f(f1,0.0,0.0));
        usr_normal(n);
        usr_vertex(v);

        v[0] = x1;
        v[1] = R1*UTL_SinA[j+1];
        v[2] = R1*UTL_CosA[j+1];
        n[1] = UTL_SinA[j+1];
        n[2] = UTL_CosA[j+1];
        if ( R1 != 0.0 )
        {
            usr_normal(n);
            usr_vertex(v);
        }

        v[0] = x0;
        v[1] = R0*UTL_SinA[j+1];
        v[2] = R0*UTL_CosA[j+1];
        if ( R0 != 0.0 )
        {
            usr_texture(usr_pack3f(f0,0.0,0.0));
            usr_normal(n);
            usr_vertex(v);
        }

        usr_endpolygon();
    }

    trs_pop_matrix();
}
