/******************************************
*  3D move object
* 
*     FILE : robject.c
*  CREATED : 10.03.1997
*  
*  (c) Evgeny S. Borisov 
*  
*        OS : MS DOS 6.22
*  COMPILER : TopSpeed C
******************************************/

#include <graph.h>
#include <math.h>
#include <stdlib.h>
#include <window.h>
#include <stdio.h>
#include <conio.h>


float v11, v12, v13, v21, v22, v23, v31, v32, v33, v43, horfact, vertfact;
float screen_dist=15, c1=4.5, c2=3.5, x_max=10.0, y_max=7.0;
int X__max, Y__max;

void initgr() {
    _setvideomode( _VRES16COLOR );
    X__max = 640; 
    Y__max = 480;
    horfact = X__max/x_max; 
    vertfact = Y__max/y_max;
}

int IX(float x) { 
    return (int) (x * horfact + 0.5);
}

int IY(float y) {
    return (int) Y__max - (int)(y * vertfact + 0.5);
}

void move(float x, float y) {
    _moveto(IX(x), IY(y));
}

void draw(float x, float y) {
    _lineto(IX(x), IY(y));
}

void endgr(void) {
    _setvideomode( _DEFAULTMODE );
}

/*----------------------------------------------*/
void coeff( float rho, float theta, float phi ,float alpha) {
    float th, ph, c1, s1, c2, s2, s3, c3, factor;
    
    factor=atan(1.0)/45.0;
    
    th=theta*factor;
    ph=phi*factor;
    alpha=alpha*factor;
    
    c1=cos(th); s1=sin(th);
    c2=cos(ph); s2=sin(ph);
    c3=cos(alpha);s3=sin(alpha);
    
    v11=c1*c3+s1*s2*s3;   v12=-c1*s3+c3*s1*s2; v13=c2*s1;
    v21=c2*s3;            v22=c2*c3;           v23=-s2;
    v31=-c3*s1+c1*s2*s3;  v32=s1*s3+c1*c3*s2;  v33=c1*c2;
                                               v43=rho;
}

void perspective(float x, float y, float z, float *pX, float *pY) {
    float xe, ye, ze;
    
    xe = v11*x + v21*y + v31*z;
    ye = v12*x + v22*y + v32*z;
    ze = v13*x + v23*y + v33*z + v43;

    *pX = screen_dist*xe/ze + c1;
    *pY = screen_dist*ye/ze + c2;  
}

void mv(float x, float y, float z) { 
    float X, Y;
    perspective(x, y, z, &X, &Y);
    move(X, Y); 
}

void dw(float x, float y, float z) {
    float X, Y;
    perspective(x, y, z, &X, &Y);
    draw(X, Y); 
}

void  drawobject(short pst, short bkcolor) {
    float h=30.0,l=4;
    
    if(pst==1) { _setcolor(LightCyan); } 
    else { _setcolor(bkcolor); }

    mv(h, -h, -h*l); dw(h, h, -h*l);     /* AB */
    dw(-h, h, -h*l);                     /* BC */
    dw(-h, h, h*l);                      /* CG */
    dw(-h, -h, h*l);                     /* GH */
    dw(h, -h, h*l);                      /* HE */
    dw(h, -h, -h*l);                     /* EA */
    mv(h, h, -h*l); dw(h, h, h*l);       /* BF */
    dw(-h, h, h*l);                      /* FG */
    mv(h, h, h*l); dw(h, -h, h*l);       /* FE */
    mv(h, -h, -h*l); dw(-h, -h, -h*l);   /* AD */
    dw(-h, h, -h*l);                     /* DC */
    mv(-h, -h, -h*l); dw(-h, -h, h*l);   /* DH */
    dw(0,0,2*l*h);                     /* HM */
    dw(h,-h,h*l);                      /* ME */
    mv(h,h,h*l);dw(0,0,2*h*l);         /* FM */
    dw(-h,h,h*l);                      /* MG */

    if(pst==1) { _setcolor(LightGreen); }
    else { _setcolor(bkcolor); }

    mv(0,0,0); dw(-h*l+20,0,0); //OX
    mv(0,0,0); dw(0,h*l+20,0);  //OY
    mv(0,0,0); dw(0,0,2*h*l+20); //OZ

    if(pst==1) { _setcolor(LightRed); }
    else { _setcolor(bkcolor); }

    mv(0,h,-h);
    dw(0,h,h);
    dw(h/5,h,0);
    dw(-h/5,h,0);
    dw(0,h,h); 
}

void moveobject(float phi0, 
                float phi1, 
                float psi0, 
                float psi1,
                float theta0, 
                float theta1)
{
    float dphi,phi,psi,dpsi,dtheta,theta,r=1200;
    int b=100,i;
    short bkcol;
    
    bkcol=_getbkcolor();    
    phi=phi0;     dphi=(phi1-phi0)/b;    
    psi=psi0;     dpsi=(psi1-psi0)/b;    
    theta=theta0; dtheta=(theta1-theta0)/b;
    for(i=1;i<=b;i++) {
        coeff(r,phi,psi,theta);
        drawobject(1,bkcol);
        delay(20);
        drawobject(0,bkcol);
        phi=phi+dphi;
        psi=psi+dpsi;
        theta=theta+dtheta; 
    }
    coeff(r,phi1,psi1,theta1);
    drawobject(1,bkcol);
}

int main() {
    float theta=0, phi=0 ,psi=0,theta2=0, phi2=0 ,psi2=0;
    printf ("   phi: ");scanf("%f", &phi);
    printf ("  phi2: ");scanf("%f", &phi2);
    printf ("   psi: ");scanf("%f", &psi);
    printf ("  psi2: ");scanf("%f", &psi2);
    printf (" theta: ");scanf("%f", &theta);
    printf ("theta2: ");scanf("%f", &theta2);
    initgr();
    moveobject(phi,phi2,psi,psi2,theta,theta2);
    getch();
    endgr();
    return 0;
}