Отрисовывание пирамидки с использованием graphics.h

#include <winbgim.h>
#include <stdio.h>
#include <math.h>
#include <dos.h>
#define defcolour 0
#define PI 3.14159265
#define BLUE 9
#define GREEN 10
#define CYAN 11
#define RED 12
#define MAGENTA 13
#define YELLOW 14
#define WHITE 15
#define BLACK 0
 
int K = 10000, koef = 1667, center = 250;
float p = 0, q = 0, r = -1/250;
 
drawfig(float PyramidTrue[3][5], int AngleX, int AngleY, int AngleZ, int figcolor)
{
    int flag = 0, i, j, k, t;
    float res, XSU, YSU, XSD, YSD, xtemp, xleft, xright, XC, YC, ZC;
    float MatrixProj[4][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {p, q, r, 1}};
    float MatrixRotX[3][3] = {{1, 0, 0}, {0, cos(AngleX*PI/180), -sin(AngleX*PI/180)}, {0, sin(AngleX*PI/180), cos(AngleX*PI/180)}};
    float MartixRotY[3][3] = {{cos(AngleY*PI/180), 0, sin(AngleY*PI/180)}, {0, 1, 0}, {-sin(AngleY*PI/180), 0, cos(AngleY*PI/180)}};
    float MartixRotZ[3][3] = {{cos(AngleZ*PI/180), -sin(AngleZ*PI/180), 0}, {sin(AngleZ*PI/180), cos(AngleZ*PI/180), 0}, {0, 0, 1}};
    float Proj[4][5];
 
    for (XC = 0, i = 0; i < 5; ++i)
        XC += PyramidTrue[0][i];
    XC /= 5;
    for (YC = 0, i = 0; i < 5; ++i)
        YC += PyramidTrue[1][i];
    YC /= 5;
    for (ZC = 0, i = 0; i < 5; ++i)
        ZC += PyramidTrue[2][i];
    ZC /= 5;
    for (i = 0; i < 5; ++i)
        PyramidTrue[0][i] -= XC;
    for (i = 0; i < 5; ++i)
        PyramidTrue[1][i] -= YC;
    for (i = 0; i < 5; ++i)
        PyramidTrue[2][i] -= ZC;
    for (k = 0; k < 5; ++k)
    {
        for (i = 0; i < 4; ++i)
        {
            res = 0;
            for (j = 0; j < 4; ++j)
            {
                res += (MatrixProj[i][j] * PyramidTrue[j][k]);
            }
            Proj[i][k] = res;
        }
        for (i = 0; i < 3; ++i)
        {
            Proj[i][k] /= Proj[3][k];
        }
    }
    for (i = 0; i < 5; ++i)
        PyramidTrue[0][i] += XC;
    for (i = 0; i < 5; ++i)
        PyramidTrue[1][i] += YC;
    for (i = 0; i < 5; ++i)
        PyramidTrue[2][i] += ZC;
 
    for (k = 0; k < 5; ++k)
    {
        for (i = 0; i < 3; ++i)
        {
            res = 0;
            for (j = 0; j < 3; ++j)
            {
                res += (MatrixRotX[i][j] * Proj[j][k]);
            }
            Proj[i][k] = res;
        }
    }
    for (k = 0; k < 5; ++k)
    {
        for (i = 0; i < 3; ++i)
        {
            res = 0;
            for (j = 0; j < 3; ++j)
            {
                res += (MartixRotY[i][j] * Proj[j][k]);
            }
            Proj[i][k] = res;
        }
    }
    for (k = 0; k < 5; ++k)
    {
        for (i = 0; i < 3; ++i)
        {
            res = 0;
            for (j = 0; j < 3; ++j)
            {
                res += (MartixRotZ[i][j] * Proj[j][k]);
            }
            Proj[i][k] = res;
        }
    }
    for (i = 0; i < 5; ++i)
        Proj[0][i] += XC;
    for (i = 0; i < 5; ++i)
        Proj[1][i] += YC;
    for (i = 0; i < 5; ++i)
        Proj[2][i] += ZC;
 
 
    setcolor(BLUE);
    for (i = 0, j = 1, k = 0; k < 8; ++k, ++i, ++j)
    {
        if (i == 5)
            --i;
        if (j == 4)
            j = 0;
        if (k == 4)
            --j;
        line (Proj[0][i], Proj[1][i], Proj[0][j], Proj[1][j]);
    }
    setcolor(WHITE);
    circle(XC,YC,5);
}
 
main(int argc, char *argv[])
{
    int i, j, angleX = 0, angleY = 0, angleZ = 0;
    char ch,excolor;
    float Pyramid[4][5] = {{10, 50, 50, 10, 30}, {10, 10, 50, 50, 30}, {10, 10, 10, 10, 50}, {1, 1, 1, 1, 1}};
    initwindow(500, 500);
    setbkcolor(defcolour);
    excolor = BLUE;
    printf("WASD - up, left, down, right\nRF - rotation X cw, ccw\nTG - rotation Y cw, ccw\nYH - rotation Z cw, ccw\nQE - scale in, scale out\n");
    while(1)
    {
        cleardevice();
        drawfig(Pyramid, angleX, angleY, angleZ, CYAN);
        ch = getch();
        if(ch == 'r')
        {
            angleX++;
        }
        if(ch == 'f')
        {
            angleX--;
        }
        if(ch == 't')
        {
            angleY++;
        }
        if(ch == 'g')
        {
            angleY--;
        }
        if(ch == 'y')
        {
            angleZ++;
        }
        if(ch == 'h')
        {
            angleZ--;
        }
        if (ch == 'q')
        {
            for (i = 0; i < 3; ++i)
            {
                for (j = 0; j < 5; ++j)
                    Pyramid[i][j] *= 1.01;
            }
        }
        if (ch == 'e')
        {
            for (i = 0; i < 3; ++i)
            {
                for (j = 0; j < 5; ++j)
                    Pyramid[i][j] *= 1/1.01;
            }
        }
        if(ch == 'd')
        {
            for (i = 0; i < 5; ++i)
            {
                ++Pyramid[0][i];
            }
        }
        if(ch == 'a')
        {
            for (i = 0; i < 5; ++i)
            {
                --Pyramid[0][i];
            }
        }
        if(ch == 's')
        {
            for (i = 0; i < 5; ++i)
            {
                ++Pyramid[1][i];
            }
        }
        if(ch == 'w')
        {
            for (i = 0; i < 5; ++i)
            {
                --Pyramid[1][i];
            }
        }
 
    }
}