//I make no claims to the validity or accuracy of this program. Use at your own risk.
//Don’t blame / sue me if it ends up causing you or your system damage in some way.
//
//*****************************************************************************************************
//Name  : Simple2DCollisionsWithFriction.c
//Author: Rahil Baber
//Date  : 3rd March 2011
//
//This program simulates a single 2D object colliding against the floor, with friction.
//Note that this program does not handle the object at rest properly.
//
//During the simulation press any key to restart the simulation.
// 
//The shape of the object which we are colliding and its properties are set in the function
//InitializeSimulation(). Modify the variables in this function to create your own simulations.
//
//This program was written for the website EuclideanSpace check out
//http://euclideanspace.com/physics/dynamics/collision/practical/RahilBaberCorrectionToBrianMirtich.pdf
//for a description of the algorithm.
//
//The program uses OpenGL to render the simulation and GLUT to create and manage the windows.
//
//To compile the program in Visual Studio you may need to add the libraries
//opengl32.lib glu32.lib glut32.lib
//to "Project>Properties>Configuration Properties>Linker>Input>Additional Dependencies".
//
//To compile the program using gcc type "gcc Simple2DCollisionsWithFriction.c -lglut -lGLU -lGL -lm"
//at the command line.
//
//If you've found this program useful or interesting or are creating your own physics simulation
//application based on any part of it, then drop me an e-mail (see the pdf for the address). 
//*****************************************************************************************************

#include <GL/glut.h>
#include <math.h>
#include <stdio.h>

#define PI 3.141592653589793

//The time in milliseconds between frames of the animation.
#define TIMERMILLISECS 33





//*** The global variables ***

//Used to measure time (in milliseconds) to create a consistent animation speed.
int prevTime;

//The width and height of the simulation.
//These values are set when the window is resized (see the Resize() function).
double simulationWidth;  //=(window width)/(window height)
double simulationHeight; //=1.0

//The scene is rendered as a line drawing, lineThickness indicates the thickness of those lines.
double lineThickness = 0.01;

//The remaining global variables describe the simulation and are initialized in InitializeSimulation().
double groundHeight; //The height of the ground.
double g;            //The acceleration due to gravity.
double cF;           //The coefficient of friction.
double cR;           //The coefficient of restitution.
double I;            //The moment of inertia of the object.
double cx, cy;       //The position of the object.
double vx, vy;       //The velocity of the object.
double a;            //The angle the object is orientated at.
double w;            //The angular velocity of the object.
double rx, ry;       //The relative position of the point of contact during a collision.

//The following variables describe the shape of the object. The shape is described by an array of
//coordinates (boundary[0][] to boundary[noOfPoints-1][]) which are joined by line segments. The point
//boundary[0][] is joined to boundary[1][], which in turn is joined to boundary[2][], etc. We take the
//boundary to be a closed loop and so we join boundary[noOfPoints-1][] to boundary[0][].
int    noOfPoints;        //The number of points used to describe the boundary of the object.
double boundary[1000][2]; //The coordinates of the boundary of the object.





//*** The function declarations ***

//This function initializes the simulation. Modify the variables in this function to create your own
//simulations.
void InitializeSimulation();

//These functions calculate the centre of mass and the moment of inertia of the object assuming the
//boundary describes a uniformly distributed solid or hollow object (respectively). They translate the
//coordinates of the boundary so that the centre of mass lies at the origin.
void CalculatePropertiesSolid();
void CalculatePropertiesHollow();

//Calculates the lowest point of the object in its current orientation.
//Used to determine if the object has collided with the ground and the relative position of the point
//of contact.
double yMin();

//Calculates the new linear and angular velocity of the object after a collision with the ground.
void ResolveCollision();

//Updates the simulation and calls Display() every "TIMERMILLISECS" milliseconds.
void Timer(int value);

//Used by GLUT to handle window resizing.
//Sets the simulationHeight to 1 and the simulationWidth to width/height. 
void Resize(int width, int height);

//Used by GLUT to handle keyboard input.
//Any keyboard input is interpreted as a command to reset the simulation.
void Keyboard(unsigned char key, int x, int y);

//Draws a line of thickness "lineThickness". Used by Display().
void DrawLine(double x1, double y1, double x2, double y2);

//Used by GLUT to draw graphics to the window.
void Display(void);





//*** The function definitions ***

//This function initializes the simulation. Modify the variables in this function to create your own
//simulations.
void InitializeSimulation()
{
    int i;

    //A few notes before we begin:
    //The object does not have to be convex.
    //We are assuming the mass of the object is 1 (this should make no difference to the simulation).
    //The lower left corner of the window is (0,0).
    //The upper right corner of the window is (simulationWidth, simulationHeight).
    //simulationHeight is always 1. (This should give you a rough idea of the scale of the simulation.)


    //Define the boundary of the object by setting noOfPoints and the boundary[][] array.
    //The centre of mass is assumed to be at the origin. We can use CalculatePropertiesSolid() and
    //CalculatePropertiesHollow() to calculate the centre of mass and translate the boundary
    //appropriately at a later time if we so wish.

    //Increase noOfPoints to get a circle, and change the 0.1 coefficients to get an ellipse.
    noOfPoints = 4;
    for(i=0;i<noOfPoints;i++)
    {
        boundary[i][0] = 0.1*cos((2*PI*i)/noOfPoints);
        boundary[i][1] = 0.1*sin((2*PI*i)/noOfPoints);
    }

    //Find the centre of mass and set the moment of inertia using CalculatePropertiesSolid().
    //We can set the moment of inertia "I" manually if we so wish.
    CalculatePropertiesSolid(); //We assume the object is solid and its mass is uniformly distributed.
    //CalculatePropertiesHollow(); //Uncomment this line if you wish to model the object as hollow.

    //The height of the ground.
    groundHeight = 0.1;    

    //The acceleration due to gravity.
    //Reduced Earth's gravity by 5 to make the simulation last longer.
    //Alternatively you can view this as saying 5 metres = 1 unit = the window's height. 
    g = -9.8/5;

    //The coefficient of friction.
    cF = 0.4;

    //The coefficient of restitution.
    cR = 0.5;//originally 0.9;

    //The position of the object.
    cx = -0.1;
    cy =  0.6;

    //The velocity of the object.
    vx = 0.9;
    vy = 0.0;

    //The orientation of the object (in radians).
    a = 0;

    //The angular velocity of the object (in radians per second).
    w = 0;

    return;
}





//This function calculates the centre of mass and the moment of inertia of the object assuming the
//boundary describes a uniformly distributed solid object. It translates the coordinates of the
//boundary so that the centre of mass lies at the origin.
void CalculatePropertiesSolid()
{
    int    i, j;
    double CM[2];
    double A, TotalA;

    //CM[] represents the coordinates of the centre of mass.
    //TotalA represents the total area enclosed by the boundary of the object. 

    //Initialize the variables.
    CM[0]  = 0;
    CM[1]  = 0;
    TotalA = 0;
    I      = 0; //"I" is the moment of inertia.

    //I won't go into the details of how the centre of mass and moment of inertia are calculated, but
    //an outline of the procedure is to decompose the object into a series of triangles (with one
    //vertex at the origin) and calculate these triangle's centre of mass and inertia, then sum their
    //contributions.
    for(i=0;i<noOfPoints;i++)
    {
        j = (i+1)%noOfPoints;

        A = (boundary[i][0]*boundary[j][1]-boundary[j][0]*boundary[i][1])/2;

        TotalA += A;

        CM[0] += A*(boundary[i][0]+boundary[j][0]);
        CM[1] += A*(boundary[i][1]+boundary[j][1]);

        I += A*(boundary[i][0]*boundary[i][0]
               +boundary[j][0]*boundary[j][0]
               +boundary[i][0]*boundary[j][0]
               +boundary[i][1]*boundary[i][1]
               +boundary[j][1]*boundary[j][1]
               +boundary[i][1]*boundary[j][1]);
    }

    CM[0] /= 3*TotalA;
    CM[1] /= 3*TotalA;
    I     /= 6*TotalA;

    //Apply the parallel axis theorem.
    I -= CM[0]*CM[0]+CM[1]*CM[1];

    //Translate the boundary so the centre of mass is at the origin.
    for(i=0;i<noOfPoints;i++)
    {
        boundary[i][0] -= CM[0];
        boundary[i][1] -= CM[1];
    }

    return;
}





//This function calculates the centre of mass and the moment of inertia of the object assuming the
//boundary describes a uniformly distributed hollow object. It translates the coordinates of the
//boundary so that the centre of mass lies at the origin.
void CalculatePropertiesHollow()
{
    int    i, j;
    double CM[2];
    double L, TotalL;

    //CM[] represents the coordinates of the centre of mass.
    //TotalL represents the total length of the boundary of the object. 

    //Initialize the variables.
    CM[0]  = 0;
    CM[1]  = 0;
    TotalL = 0;
    I      = 0; //"I" is the moment of inertia.

    //I won't go into the details of how the centre of mass and moment of inertia are calculated, but
    //an outline of the procedure is to calculate the centre of mass and inertia of each segment making
    //up the boundary, then sum their contributions.
    for(i=0;i<noOfPoints;i++)
    {
        j = (i+1)%noOfPoints;

        L = sqrt((boundary[i][0]-boundary[j][0])*(boundary[i][0]-boundary[j][0])
                +(boundary[i][1]-boundary[j][1])*(boundary[i][1]-boundary[j][1]));

        TotalL += L;

        CM[0] += L*(boundary[i][0]+boundary[j][0]);
        CM[1] += L*(boundary[i][1]+boundary[j][1]);

        I += L*(boundary[i][0]*boundary[i][0]
               +boundary[j][0]*boundary[j][0]
               +boundary[i][0]*boundary[j][0]
               +boundary[i][1]*boundary[i][1]
               +boundary[j][1]*boundary[j][1]
               +boundary[i][1]*boundary[j][1]);
    }

    CM[0] /= 2*TotalL;
    CM[1] /= 2*TotalL;
    I     /= 3*TotalL;

    //Apply the parallel axis theorem.
    I -= CM[0]*CM[0]+CM[1]*CM[1];

    //Translate the boundary so the centre of mass is at the origin.
    for(i=0;i<noOfPoints;i++)
    {
        boundary[i][0] -= CM[0];
        boundary[i][1] -= CM[1];
    }

    return;
}





//Calculates the lowest point of the object in its current orientation.
//Used to determine if the object has collided with the ground and the relative position of the point
//of contact.
double yMin()
{
    double x, y;
    int    t;

    //We'll store the position of the lowest point (relative to the objects centre of mass) in (rx,ry).

    //Initialize (rx,ry) with the coordinates of the first point on the boundary.
    rx = boundary[0][0]*cos(a)-boundary[0][1]*sin(a);
    ry = boundary[0][0]*sin(a)+boundary[0][1]*cos(a);

    //Compare the other points on the boundary to (rx,ry).
    for(t=1;t<noOfPoints;t++)
    {
        x = boundary[t][0]*cos(a)-boundary[t][1]*sin(a);
        y = boundary[t][0]*sin(a)+boundary[t][1]*cos(a);

        //Check if (x,y) is lower than (rx,ry)
        if(y<ry)
        {
            rx = x;
            ry = y;
        }
    }

    return ry;
}





//Calculates the new linear and angular velocity of the object after a collision with the ground.
void ResolveCollision()
{
    double px, py;
    double ux, uy;
    double u0x, u0y;
    double v0x, v0y;
    double w0;
    double Wc, Wd;
    double K11, K12, K21, K22;
    double invK11, invK12, invK21, invK22;
    double D;
    double kx, ky;
    double uOld, uOrigin;
    double tolerance = 0.000001;
    int    bSticking;
    int    bConverging;
    int    bDiverging;

    //I won't go into the details of how this algorithm works. The details can be found in the document
    //"A Correction To Brian Mirtich's Thesis" see the top of this file for a link.

    v0x = vx;
    v0y = vy;
    w0  = w;

    //Step 1. Rotate the space (is not necessary).



    //Step 2. Initialization.
    //Calculate the initial separation velocity. 
    u0x = v0x-w0*ry;
    u0y = v0y+w0*rx;

    if(u0y>0)
        return; //The object is not colliding.

    //Calculate the collision matrix K and its inverse invK.

    //We are assuming the mass of the object is 1.
    K11 = 1+ry*ry/I; K12 =  -rx*ry/I;
    K21 =  -rx*ry/I; K22 = 1+rx*rx/I;

    D = K11*K22-K12*K21; //The determinant of K.

    invK11 =  K22/D; invK12 = -K12/D;
    invK21 = -K21/D; invK22 =  K11/D;

    //Initialize (ux,uy), Wc, and Wd.
    ux = u0x;
    uy = u0y;
    Wc = 0;
    Wd = 0;



    //Step 3. Determining the type of ray we are on.
    bSticking   = 0;
    bConverging = 0;
    bDiverging  = 0;

    if(-tolerance<ux && ux<tolerance)
    {
        //Step 3a. Sticking has occurred.
        ux        = 0;
        bSticking = 1;
    }
    else
    if(ux>0)
    {
        //Step 3b.
        kx = -K11*cF+K12;
        ky = -K21*cF+K22;

        if(-tolerance<kx && kx<tolerance)
        {
            kx         = 0;
            ky         = 1/invK22;
            bDiverging = 1;
        }
        else
        {
            if(kx>0)
                bDiverging  = 1;
            else
                bConverging = 1;
        }
    }
    else
    {
        //Step 3c.
        kx = K11*cF+K12;
        ky = K21*cF+K22;

        if(-tolerance<kx && kx<tolerance)
        {
            kx         = 0;
            ky         = 1/invK22;
            bDiverging = 1;
        }
        else
        {
            if(kx>0)
                bConverging = 1;
            else
                bDiverging  = 1;
        }
    }



    //Step 4. We are on a converging ray.
    if(bConverging==1)
    {
        //Step 4a.
        uOrigin = uy-ky*ux/kx;

        if(uOrigin<=0)
        {
            //Step 4b.
            Wc = -ux*uy/kx+ky*ux*ux/(2*kx*kx);
            ux = 0;
            uy = uOrigin;

            bSticking = 1;
        }
        else
        if(0<uOrigin && uOrigin<-cR*uy)
        {
            //Step 4c.
            Wc = -uy*uy/(2*ky);
            Wd = uOrigin*uOrigin/(2*ky);
            ux = 0;
            uy = uOrigin;

            bSticking = 1;
        }
        else
        {
            //Step 4d.
            ux = ux-(1+cR)*kx*uy/ky;
            uy = -cR*uy;
        }
    }



    //Step 5. Sticking has occurred.
    if(bSticking==1)
    {
        ux = 0; //Just to be safe.

        if(-K12<=cF*K11 && K12<=cF*K11)
        {
            //Step 6. Stable sticking has occurred.
            kx         = 0;
            ky         = 1/invK22;
            bDiverging = 1;
        }
        else
        {
            //Step 7. Unstable sticking has occurred.
            kx = -K11*cF+K12;
            ky = -K21*cF+K22;

            if(kx<=tolerance)
            {
                kx = K11*cF+K12;
                ky = K21*cF+K22;

                if(kx>=-tolerance)
                {
                    //Just to be safe.
                    kx = 0;
                    ky = 1/invK22;
                }
            }

            bDiverging = 1;
        }
    }



    //Step 8. We are on a diverging/stationary ray (or stable sticking has occurred).
    if(bDiverging==1)
    {
        //Step 8a.
        uOld = uy;

        if(uy<0)
        {
            //Step 8b. We are in a compression phase.
            Wc += -uy*uy/(2*ky);
            uy = 0;
        }

        //Step 8c. We are in a decompression phase.
        uy = sqrt(2*ky*(-cR*cR*Wc-Wd)+uy*uy);
        ux = kx*(uy-uOld)/ky+ux;
    }



    //Step 9. Calculate the impulse and the new velocities.
    px = invK11*(ux-u0x)+invK12*(uy-u0y);
    py = invK21*(ux-u0x)+invK22*(uy-u0y);

    vx = v0x+px;
    vy = v0y+py;

    w  = w0+(rx*py-ry*px)/I;



    //Step 10. Rotate the space (is not necessary).

    return;
}





//Updates the simulation and calls Display() every "TIMERMILLISECS" milliseconds.
void Timer(int value)
{
    int    newTime;
    int    elapsedTime;
    double dt;

    //Ask GLUT to re-call this function after "TIMERMILLISECS" millieconds.
    glutTimerFunc(TIMERMILLISECS,Timer,0);

    //Work out how much time has passed since the last time we updated the simulation.
    newTime     = glutGet(GLUT_ELAPSED_TIME);
    elapsedTime = (newTime-prevTime);
    prevTime    = newTime;

    //Update the simulation in intervals of 6 milliseconds.        
    while(elapsedTime>=6)
    {
        dt           = 0.006;
        elapsedTime -= 6;

        //Update the position, orientation, and velocity of the object.
        cx += vx*dt;
        cy += vy*dt+g*dt*dt/2;
        vy += g*dt;
        a  += w*dt;

        //Check if a collision has occurred.
        if(cy+yMin()<groundHeight)
        {
            //Artifically push the object above the ground.
            cy = groundHeight-ry;

            //Work out the new velocities of the object.
            ResolveCollision();
        }
    }

    //If there is any elapsedTime left over we'll account for it next time. 
    prevTime -= elapsedTime;

    //Call Display().
    glutPostRedisplay();

    return;
}





//Used by GLUT to handle window resizing.
//Sets the simulationHeight to 1 and the simulationWidth to width/height.
void Resize(int width, int height)
{
    simulationWidth  = width/(double)height;
    simulationHeight = 1.0;

    //Set the new viewport dimensions, and create the 2D projection transformation.
    glViewport(0,0,width,height);
    glMatrixMode(GL_PROJECTION); 
    glLoadIdentity();
    glOrtho(0.0, simulationWidth, 0.0, simulationHeight, -1.0, 1.0);

    //Call Display().
    glutPostRedisplay();

    return;
}





//Used by GLUT to handle keyboard input.
//Any keyboard input is interpreted as a command to reset the simulation.
void Keyboard(unsigned char key, int x, int y)
{
    InitializeSimulation();
    glClear(GL_COLOR_BUFFER_BIT);
    return;
}





//Draws a line of thickness "lineThickness". Used by Display().
void DrawLine(double x1, double y1, double x2, double y2)
{
    int    i;
    int    segments = 10;
    double nx, ny, d;

    //Draw circles at the end points.
    for(i=0;i<segments;i++)
    {
        glVertex2d(x1,y1);
        glVertex2d(x1+lineThickness/2*cos(2*PI*i/segments),
                   y1+lineThickness/2*sin(2*PI*i/segments));
        glVertex2d(x1+lineThickness/2*cos(2*PI*(i+1)/segments),
                   y1+lineThickness/2*sin(2*PI*(i+1)/segments));

        glVertex2d(x2,y2);
        glVertex2d(x2+lineThickness/2*cos(2*PI*i/segments),
                   y2+lineThickness/2*sin(2*PI*i/segments));
        glVertex2d(x2+lineThickness/2*cos(2*PI*(i+1)/segments),
                   y2+lineThickness/2*sin(2*PI*(i+1)/segments));
    }

    //Calculate the normal of the line.
    nx = y1-y2;
    ny = x2-x1;
    d  = sqrt(nx*nx+ny*ny);

    if(d<0.00001)
        return;

    nx = nx/d*lineThickness/2;
    ny = ny/d*lineThickness/2;

    //Draw the main body of the line.
    glVertex2d(x1+nx,y1+ny);
    glVertex2d(x1-nx,y1-ny);
    glVertex2d(x2+nx,y2+ny);

    glVertex2d(x2+nx,y2+ny);
    glVertex2d(x2-nx,y2-ny);
    glVertex2d(x1-nx,y1-ny);

    return;
}





//Used by GLUT to draw graphics to the window.
void Display(void)
{
    int i, j;

    //Clear the back buffer.
    glClear(GL_COLOR_BUFFER_BIT);

    //Draw our object onto the back buffer.
    glBegin(GL_TRIANGLES);
    {
        //Draw the ground line.
        DrawLine(0.0,groundHeight-lineThickness/2,simulationWidth,groundHeight-lineThickness/2);

        //Draw the object.
        for(i=0;i<noOfPoints;i++)
        {
            j = (i+1)%noOfPoints;
            DrawLine(cx+boundary[i][0]*cos(a)-boundary[i][1]*sin(a),
                     cy+boundary[i][0]*sin(a)+boundary[i][1]*cos(a),
                     cx+boundary[j][0]*cos(a)-boundary[j][1]*sin(a),
                     cy+boundary[j][0]*sin(a)+boundary[j][1]*cos(a));
        }
    }
    glEnd();

    glFlush();         //Force OpenGL commands to be executed.
    glutSwapBuffers(); //Display the back buffer.

    return;
}





int main(int argc, char **argv)
{
    printf("A demo of rigid body collisions with friction\n");
    printf("made by Rahil Baber for EuclideanSpace.com.\n");
    printf("\n");
    printf("Press any key to restart the simulation.\n"); 
    printf("\n");

    //We use GLUT to create and manage the window.
    glutInit(&argc, argv);    
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
    glutInitWindowSize(600,400);
    glutInitWindowPosition(0,0);
    glutCreateWindow("Simple 2D Collisions With Friction");
   
    glutDisplayFunc(Display);
    glutReshapeFunc(Resize);
    glutKeyboardFunc(Keyboard);

    glClearColor(0.0, 0.0, 0.0, 0.0); //Background colour
    glColor3f(1.0,1.0,1.0);           //Foreground colour
    glShadeModel(GL_SMOOTH);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

    InitializeSimulation();
    
    //Start the timer.
    prevTime = glutGet(GLUT_ELAPSED_TIME);  
    glutTimerFunc(TIMERMILLISECS,Timer,0);

    glutMainLoop();
    
    return 0;    
}