wheel rotating on a plane (sunanda madam's assignment)

FILE : rrc.cpp

/*
 *      rrc.cpp
 *
 *      Copyright 2009 Rooparam Choudhary <rooparambhakar@gmail.com>
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 *      MA 02110-1301, USA.
 */
#include <graphics.h>
#include <math.h>

typedef  float   matrix3x3[3][3];
typedef  float   vector[3];


/* -------- Public Interface ------------ */
static int _WIDTH = 640;     static int _HEIGHT = 480;
static int _CX = 320;        static int _CY = 240;         // CENTER OF COORDINATES
static int _COLOR = BLACK;
static vector    _DOT;
static matrix3x3    _M;
static int _TRANS = 0;                    // whether to use transformation matrix in pixel drawing
                                          // 0: false   1: true

static void SetToIdentity(matrix3x3 X);                // Converts X to indentity matrix
static void Transform(matrix3x3 X, vector P);          // P = X.P
static void mul(matrix3x3 Left, matrix3x3 right);      // [right] = [Left].[right]
static void Translate(matrix3x3 X, int tx, int ty);    // X = [Tranlation matrix].X
static void Rotate(matrix3x3 X, float theta);          // X = [Rotation Matrix].X

static void setPixel(int x, int y);                // (x, y) are in screen coordinates
// to optimize, shift transformation operations from setPixel to line_r.
static void line_r(int x0, int y0, int x1, int y1, const int color = -1);      
static void circle_r(int xc, int yc, const int radius, const int color = -1);
static void ellipse_r(int xc, int yc, int rx, int ry, const int color = -1);

/* -------------------------------------- */

static void Transform(matrix3x3 X, vector P) {
       vector T;
       for(int i=0; i<3; ++i) {
               T[i] = 0.0f;
               for(int j = 0; j<3; ++j) {
                       T[i] += X[i][j]*P[j];
               }
       }
       P[0] = T[0];
       P[1] = T[1];
       P[2] = T[2];
}

static void SetToIdentity(matrix3x3 X) {
       for(int i=0; i<3; ++i) {
             for(int j=0; j<3; ++j) {
                     X[i][j] = (float)(i==j);
             }
       }
}


static void mul(matrix3x3 Left, matrix3x3 right) {      // right's content will be changed
       matrix3x3 T;        // new matrix
       for(int i=0; i<3; ++i) {
               for(int j=0; j<3; ++j) {
                       T[i][j] = 0.0f;
                       for(int k=0; k<3; ++k)
                               T[i][j] += Left[i][k] * right[k][j];
               }
       }
     
       // copy T to right
       for(int i=0; i < 3; ++i)
        for(int j=0; j < 3; ++j)
                right[i][j] = T[i][j];
}

/* [Tranlation matrix].X */
static void Translate(matrix3x3 X, int tx, int ty) {
       matrix3x3 A1;
       SetToIdentity(A1);
     
       A1[0][2] = tx;
       A1[1][2] = ty;
     
       mul(A1, X);
}

/* [Rotation Matrix].X */
static void Rotate(matrix3x3 X, float theta) {
       matrix3x3 A1;          // rotation matrix
       SetToIdentity(A1);
     
       A1[0][0] = A1[1][1] = cos(theta);
       A1[0][1] = -sin(theta);
       A1[1][0] = sin(theta);
     
       mul(A1, X);  
}

static void setPixel(int x, int y) {
       if(_TRANS) {
              _DOT[0] = (float)(x - _CX);
              _DOT[1] = (float)(-y + _CY);
              _DOT[2] = 1.0f;
 
              Transform(_M, _DOT);
              x = int(_DOT[0] + _CX);   y = int(_CY - _DOT[1]);
       }
       putpixel(x, y, _COLOR);
}

/*
 * line drawing function on the screen
 * implements : Bresenhem's Line Drawing Algorithm
 * parameters :
 *              x0, y0      -   one end of the line
 *              x1, y1      -   another end of the line
 *              color   -   color of line, default value -1 indicates
 *                          we will use last defined color
 */
static void line_r(int x0, int y0, int x1, int y1, const int color)
{
    if(color != -1)
        _COLOR = color;
       
    int isTRANS = 0;
    if(_TRANS) {
              _DOT[0] = (float)x0;
              _DOT[1] = (float)y0;
              _DOT[2] = 1.0f;
 
              Transform(_M, _DOT);
              x0 = (int)_DOT[0];   y0 = (int)_DOT[1];
             
              _DOT[0] = (float)x1;
              _DOT[1] = (float)y1;
              _DOT[2] = 1.0f;
 
              Transform(_M, _DOT);
              x1 = (int)_DOT[0];   y1 = (int)_DOT[1];
             
              isTRANS = 1;
              _TRANS = 0;
    }

    int inverse =    0; // 0 : false      1 : true
    int sign = 0;       // 0 : y = x        1 : y = -x

    /* ---------------------------------------------------- */
    inverse = 0;
    sign = 0;
   float m = 0;
   int t;
   int infinity = (x1 == x0);

   if(!infinity)
      m = (y1-y0) / (float)(x1-x0);
    if(m < 0)
        sign = 1;

   if(infinity || fabs(m) > 1) {
      inverse = 1;

        if(sign)
            { t = -x0;  x0 = -y0;   y0 = t;
              t = -x1;  x1 = -y1;   y1 = t; }
        else
            { t = x0;   x0 = y0;    y0 = t;
              t = x1;   x1 = y1;    y1 = t; }
   }

   if(x1<x0)
    {
        t = x1;   x1 = x0;    x0 = t;
        t = y1;   y1 = y0;    y0 = t;
    }
    /* ---------------------------------------------------- */

    /* ---------------------------------------------------- */
    int twoDY = 2*(y1-y0), twoDX = 2*(x1-x0),   DX = x1-x0;
   int incY = ((twoDY < 0) ? -1 : 1);
   twoDY = abs(twoDY);
   int p = twoDY - DX;  int x = x0, y = y0;

   /* put a pixel at (x, y) */
   if(inverse){
            if(sign)
              setPixel(_CX - y, _CY + x);
            else
                setPixel(_CX + y, _CY - x);
   } else
      setPixel(_CX + x, _CY - y);
   /* --------------------- */

   while(x < x1) {
      ++x;
      if(!(p<0)) {
         y += incY;
         p -= twoDX;
      }
      p += twoDY;
        /* put a pixel at (x, y) */
        if(inverse){
            if(sign)
              setPixel(_CX - y, _CY + x);
            else
                setPixel(_CX + y, _CY - x);
        } else
            setPixel(_CX + x, _CY - y);
        /* --------------------- */
   }
    /* ---------------------------------------------------- */
    if(isTRANS)
        _TRANS = 1;
}

/*
 * helper for 'circle' function
 * it uses the symmetry of Circle to draw points in other octants
 */
 static inline void _draw(int xc, int yc, int x, int y) {
     setPixel(_CX + xc+x, _CY - (yc+y) );      // Ist      Octant
     setPixel(_CX + xc+y, _CY - (yc+x) );      // IInd     Octant
     setPixel(_CX + xc-y, _CY - (yc+x) );      // IIIrd    Octant
     setPixel(_CX + xc-x, _CY - (yc+y) );      // IVth     Octant
     setPixel(_CX + xc-x, _CY - (yc-y) );      // Vth      Octant
     setPixel(_CX + xc-y, _CY - (yc-x) );      // VIth     Octant
     setPixel(_CX + xc+y, _CY - (yc-x) );      // VIIth    Octant
     setPixel(_CX + xc+x, _CY - (yc-y) );      // VIIIth   Octant
}


/*
 * circle drawing function on the screen
 * implements : Midpoint Circle Algorithm
 * parameters :
 *              xc, yc  -   center of the circle
 *              radius  -   radius of the circle
 *              color   -   color of line, default value -1 indicates
 *                          we will use last defined color
 */
static void circle_r(int xc, int yc, const int radius, const int color)
{
    if(color != -1)
        _COLOR = color;

    int p = 1 - radius;
     int x = 0,       y = radius;
     do{
           _draw(xc, yc, x, y);
           ++x;
           if (p < 0)
              p += 1 + (x<<1);
           else{
                --y;
                p += 1 - ((y-x) << 1) ;
           }
     }while(y >= x);
}

/*
 * helper for 'ellipse' function
 * it uses the symmetry of Ellipse to draw points in other quadrants
 */
static inline void draw_E(int xc, int yc, int x, int y)
{
     setPixel(_CX + xc+x, _CY - (yc+y));      // Ist      Quadrant
     setPixel(_CX + xc-x, _CY - (yc+y));      // IInd     Quadrant
     setPixel(_CX + xc-x, _CY - (yc-y));      // IIIrd    Quadrant
     setPixel(_CX + xc+x, _CY - (yc-y));      // IVth     Quadrant
}
/*
 * ellipse drawing function on the screen
 * implements : Midpoint Ellipse Algorithm
 * parameters :
 *              xc, yc  -   center of the ellipse
 *              rx, ry  -   axes of the ellipse
 *              color   -   color of line, default value -1 indicates
 *                          we will use last defined color
 */
static void ellipse_r(int xc, int yc, int rx, int ry, const int color)
{
       if(color != -1)
        _COLOR = color;
       
     long long int rx_2 = rx*rx,   ry_2 = ry*ry;
     long long int p = ry_2 - rx_2*ry + (ry_2>>2);
     int x = 0,       y = ry;
     long long int two_ry_2_x = 0, two_rx_2_y = (rx_2<<1)*y;
     draw_E(xc, yc, x, y);
     while(two_rx_2_y >= two_ry_2_x){
           ++x;
           two_ry_2_x += (ry_2<<1);
         
           p +=  two_ry_2_x + ry_2;
             
           if(p >= 0){
                --y;
                two_rx_2_y -= (rx_2<<1);
               
                p -= two_rx_2_y ;
           }
           draw_E(xc, yc, x, y);
     }
   
     p = (long long int)(ry_2*(x+1/2.0)*(x+1/2.0) + rx_2*(y-1)*(y-1) - rx_2*ry_2);
     while (y>=0) {
           p += rx_2;
           --y;
           two_rx_2_y -= (rx_2<<1);
           p -= two_rx_2_y;
         
           if(p <= 0) {
                ++x;
                two_ry_2_x += (ry_2<<1);
                p += two_ry_2_x;
           }
           draw_E(xc, yc, x, y);
     }
}

FILE : wheel.cpp

/*
 *      wheel.cpp
 *
 *      Copyright 2009 Rooparam Choudhary <rooparambhakar@gmail.com>
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 *      MA 02110-1301, USA.
 */
 #include <iostream>
 #include <stdio.h>
 #include <math.h>
 #include "rrc.cpp"

 #define PI        3.14159f
 #define ee        0.85f     // collision constant
 #define CLOCK     0.2f     // tick increment
 // #define _PEL_DO
 
 static void draw_wheel(int xc, int yc, const int radius, const float theta)
 {
        int temp = _TRANS;
        _TRANS = 1;
     SetToIdentity(_M);
     Rotate(_M, theta);
     //     Translate(_M, xc, yc);
     _M[0][2] = xc;       _M[1][2] = yc;
     
     int x0, y0, x1, y1;    
     float rim = 4.0f/5.0f;  
     
     // first spike of wheel
     x0 = int(-radius*rim);  y0 = 0;
     x1 = int(radius*rim);   y1 = 0;
     line_r(x0, y0, x1, y1, RED);
     _M[1][2] -= 1;
     line_r(x0+1, y0, x1-1, y1, RED);
     _M[1][2] += 2;
     line_r(x0+1, y0, x1-1, y1, RED);
     _M[1][2] -= 1;
     
     // second spike of wheel
     x0 = int(-0.5f*radius*rim);     y0 = int(-0.866f*radius*rim);
     x1 = int(0.5f*radius*rim);      y1 = int(0.866f*radius*rim);
     line_r(x0, y0, x1, y1, GREEN);
     _M[0][2] += 1;
     line_r(x0, y0, x1-1, y1-1, GREEN);
     _M[0][2] -= 2;
     line_r(x0-1, y0-1, x1, y1, GREEN);
     _M[0][2] += 1;
     
     // third spike of wheel
     x0 = int(-0.5f*radius*rim);       y0 = int(0.866f*radius*rim);
     x1 = int(0.5f*radius*rim);        y1 = int(-0.866f*radius*rim);
     line_r(x0, y0, x1, y1, BLUE);
     _M[0][2] += 1;
     line_r(x0, y0, x1-1, y1+1, BLUE);
     _M[0][2] -= 2;
     line_r(x0+1, y0-1, x1, y1, BLUE);
     _M[0][2] += 1;
     
     // drawing tyre
     _TRANS = 0;
     circle_r(xc, yc, int(radius*rim), BLACK);
     circle_r(xc, yc, int(radius*rim-1), BLACK);
     int my_r = (int)(radius*rim+4.0f);
     do{
         circle_r(xc, yc, my_r, BLACK);
         my_r += 4;
     } while(my_r < (radius-5));
     circle_r(xc, yc, radius, BLACK);
     circle_r(xc, yc, radius-1, BLACK);
     circle_r(xc, yc, radius-2, BLACK);
     
     _TRANS = temp;
 }

 static void simulate(float angle, float radius, float g)
 {
     char *author = "Coder:- Rooparam Choudhary";

     float ix, iy;     // (ix, iy) : upper end of wedge

     setbkcolor(WHITE);

     if(tan(angle) <= (float)_CY/(_WIDTH - _CX) ) {
         ix = (float)(_WIDTH - _CX);
         iy = ix * tan(angle);
     } else {
         iy = (float)_CY;
         ix = iy / tan(angle);
     }

     // (x, y) : wheel center
     float x = ix - radius*(cos(angle) + sin(angle));
     float y = iy + radius*(cos(angle) - sin(angle));
     
     float C1 = g*sin(angle)/4.0f;
     float C2 = C1/radius;
     float tick;     // discrete time, increment on every iteration of while loop
     float twoT;

     float h, h1, t, t1, energy, speed, MAX;
     h = y-radius/cos(angle);
     energy = g*h;
     MAX = energy;
     
     float theta = 0.0f;        // rotation angle of wheel (in radians)
     while(energy > 0.01f*MAX)
     {
                  // rotating down the slope from upper right.
        t = 2.0f/sin(angle)*sqrtf(h/g);
        tick = 0.0f;
         do
         {
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#ifdef _PEL_DO
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#endif            
            twoT = (CLOCK + 2*tick)*CLOCK;
            theta += C2*twoT;
            while(theta >= 2.0f*PI) { theta -= 2.0f*PI; }
            x -= C1*cos(angle)*twoT;
            y -= C1*sin(angle)*twoT;
            tick += CLOCK;
         } while(tick <= t);
         
                      // climbing up the slope from center towards upper left.
           h1 = ee*ee*h;
           t1 = 2.0f/sin(angle)*sqrtf(h1/g);
           tick = 0.0f;
           x = 0.0f;   y = radius/cos(angle);
           speed = sqrtf(g*h1);
         do
         {
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#ifdef _PEL_DO            
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#endif            
            twoT = (CLOCK+2*tick)*CLOCK;
            theta += speed/radius*CLOCK - C2*twoT;
            while(theta >= 2.0f*PI) { theta -= 2.0f*PI; }
            x -= (speed*CLOCK - C1*twoT)*cos(angle);
            y += (speed*CLOCK - C1*twoT)*sin(angle);
            tick += CLOCK;
         } while(tick <= t1);
         
                      // rotating down the slope from upper left
           tick = 0.0f;
           float br, kr;
           kr = h1+radius/cos(angle) - radius*(cos(angle) - sin(angle));
           br = kr/tan(angle);
           x = -br + radius*(cos(angle) + sin(angle));     y = h1+radius/cos(angle);
           speed = 0;
         do
         {
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#ifdef _PEL_DO            
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#endif            
            twoT = (CLOCK+2*tick)*CLOCK;
            theta -= C2*twoT;
            while(theta <= -2.0f*PI) { theta += 2.0f*PI; }
            x += C1*twoT*cos(angle);
            y -= C1*twoT*sin(angle);
            tick += CLOCK;
         } while(tick <= t1);
         
                         // climbing up the slope towards upper right
           h = ee*ee*h1;
           t = 2.0f/sin(angle)*sqrtf(h/g);
           tick = 0.0f;
           x = 0.0f;   y = radius/cos(angle);
           speed = sqrtf(g*h);
         do
         {
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#ifdef _PEL_DO            
            cleardevice();
            setcolor(WHITE);
            setbkcolor(GREEN);
            outtextxy(getmaxx()-4 -textwidth(author), getmaxy()-20, author);
            setbkcolor(WHITE);
   
            // draw wedge
            line_r(0, 0, (int)ix, (int)iy, MAGENTA);
            /*
            line_r(1, 0, (int)ix+1, (int)iy, MAGENTA);
            line_r(2, 0, (int)ix+2, (int)iy, MAGENTA);
            line_r(3, 0, (int)ix+3, (int)iy, MAGENTA);
            line_r(4, 0, (int)ix+4, (int)iy, MAGENTA);
            */
            line_r(5, 0, (int)ix+5, (int)iy, MAGENTA);
            line_r(0, 0, (int)-ix, (int)iy, MAGENTA);
            /*
            line_r(-1, 0, (int)-ix-1, (int)iy, MAGENTA);
            line_r(-2, 0, (int)-ix-2, (int)iy, MAGENTA);
            line_r(-3, 0, (int)-ix-3, (int)iy, MAGENTA);
            line_r(-4, 0, (int)-ix-4, (int)iy, MAGENTA);
            */
            line_r(-5, 0, (int)-ix-5, (int)iy, MAGENTA);
           
            draw_wheel((int)x, (int)y, (int)radius, theta);
            swapbuffers();
#endif            
            twoT = (CLOCK+2*tick)*CLOCK;
            theta -= speed/radius*CLOCK - C2*twoT;
            while(theta <= -2.0f*PI) { theta += 2.0f*PI; }
            x += (speed*CLOCK - C1*twoT)*cos(angle);
            y += (speed*CLOCK - C1*twoT)*sin(angle);
            tick += CLOCK;
         } while(tick <= t);
         
           energy = g*h;
           kr = h+radius/cos(angle) - radius*(cos(angle) - sin(angle));
           br = kr / tan(angle);
           x = br - radius*(cos(angle) + sin(angle));    y = h+radius/cos(angle);
           speed = 0.0f;
     }
 }
#define _WHEEL
int main(void)
{
    std::cout << "Enter Screen Size [Width, Height] : ";
    std::cin >> _WIDTH >> _HEIGHT;
    std::cout << "Enter Center Coordinates [CX, CY] : ";
    std::cin >> _CX >> _CY;
    initwindow(_WIDTH, _HEIGHT, ".:: WHEEL ROLLING ON AN INCLINED PLANE ::.", 100, 100, true);
    setbkcolor(WHITE);
    cleardevice();
    swapbuffers();
    cleardevice();
    swapbuffers();
   
#ifdef _TEST_TRANSLATE
       int x0, y0, x1, y1, tx, ty;
       
       line_r(-_CX, 0, _WIDTH-_CX, 0, MAGENTA);
       line_r(0, _CY, 0, -_HEIGHT+_CY, MAGENTA);
       swapbuffers();
       line_r(-_CX, 0, _WIDTH-_CX, 0, MAGENTA);
       line_r(0, _CY, 0, -_HEIGHT+_CY, MAGENTA);
       swapbuffers();
       _TRANS = 1;
       std::cout << "Enter (x0, y0, x1, y1, tx, ty) [5000 to exit]: ";
       std::cin >> x0;
       while(x0 != 5000) {
                std::cin >> y0 >> x1 >> y1 >> tx >> ty;
                SetToIdentity(_M);
                Translate(_M, tx, ty);
                line_r(x0, y0, x1, y1, BLACK);
                swapbuffers();
                line_r(x0, y0, x1, y1, BLACK);
                swapbuffers();
                std::cout << "Enter (x0, y0, x1, y1, tx, ty) [5000 to exit]: ";
                std::cin >> x0;
       }
#endif          // _TEST_TRANSLATE
   
#ifdef _TEST_WHEEL
       int xc, yc, r; float theta, radian;
       
       line_r(-_CX, 0, _WIDTH-_CX, 0, MAGENTA);
       line_r(0, _CY, 0, -_HEIGHT+_CY, MAGENTA);
       swapbuffers();
       line_r(-_CX, 0, _WIDTH-_CX, 0, MAGENTA);
       line_r(0, _CY, 0, -_HEIGHT+_CY, MAGENTA);
       swapbuffers();
       
       std::cout << "Enter (xc, yc, r, theta)[5000 to exit]: ";
       std::cin >> xc;
       while(xc != 5000) {
                std::cin  >> yc >> r >> theta;
                radian = theta / 180.0f * 3.14159f;
                draw_wheel(xc, yc, r, radian);
                swapbuffers();
                draw_wheel(xc, yc, r, radian);
                swapbuffers();
                std::cout << "Enter (xc, yc, r, theta)[5000 to exit]: ";
                std::cin >> xc;
       }
       
#endif          // _TEST_WHEEL
   
#ifdef _WHEEL
    float theta, gravity;
    int radius;
   
    std::cout << "Enter radius of wheel [5000 to exit]: ";
    std::cin >> radius;
    while(radius != 5000) {
                 std::cout << "Enter inclination angle of plane: ";
                 std::cin >> theta;
                 std::cout << "Enter gravity constant of planet (980 for earth)[unit is cm/s2]: ";
                 std::cin >> gravity;
   
                 simulate(theta / 180.0f * 3.14159f, radius, gravity);
                 
                 std::cout << "Enter radius of wheel [5000 to exit]: ";
                 std::cin >> radius;
    }
#endif          // _WHEEL  

    closegraph();
    restorecrtmode();
}