Circle vs rectangle corner collision

//top left cornet		if (getDistance(ball_x, ball_y, rect_x-rect_w/2, rect_y-rect_h/2) < ball_r)		{			//change color to indicate collision			ball.setColor(Color.blue);			//stupid variables for the dot product			double pt1 = ball_vx;			double pt2 = ball_vy;			double pt3 = (rect_x - rect_w/2) - ball_x; //this is the collision normal, a line form the middle of the circle to the corner			double pt4 = (rect_y - rect_h/2) - ball_y;						//dot product to find angle between ball's velocity vector and collision normal			double ai = ((pt1*pt3) + (pt2*pt4)) / ((Math.sqrt(pt1*pt1+pt2*pt2)) * (Math.sqrt(pt3*pt3+pt4*pt4)));			double i = Math.acos(ai);			//this is the velocity vector for the ball projected on the normal			double bcv = ball_vec * Math.cos(i);			//use dot product to find the angle between the rect's velocity vector and the normal			pt1 = rect_vx;			pt2 = rect_vy;			double air = ((pt1*pt3) + (pt2*pt4)) / ((Math.sqrt(pt1*pt1+pt2*pt2)) * (Math.sqrt(pt3*pt3+pt4*pt4)));			double ir = Math.acos(air);			//the rect's velocity vector projected on the normal			double rcv = rect_vec * Math.cos(ir);			//calculate the velocities after collision			double ball_final = bcv * ((ball_m-rect_m)/(ball_m+rect_m)) + rcv * ((2*rect_m)/(ball_m+rect_m));			double rect_final = bcv * ((2*ball_m)/(ball_m+rect_m)) - rcv * ((ball_m-rect_m)/(ball_m+rect_m));			//now we have the two final vectors on the normal. these need to be used together with the remaining components from the initial velocity vectors			//to get the final velocity vectors This should be correct in theory, I hope, but I fear the code bellow is messed up.			//first the ball			double remaining_component = Math.sin(i) * ball_vec; //get the remaining component from the initial velocity vector (i.e. the one not colliding)			double ball_f = Math.sqrt(remaining_component*remaining_component + ball_final*ball_final); //the final velocity vector			//calculate the new angle between the new velocity vector and the normal			double new_i = Math.atan(remaining_component / ball_final); //OBSOBS switched places for these two guys			//calculate angle between normal and x-axis			double norm_a = Math.atan(pt4/pt3);			//angle between velocity vector and x-axis			double final_a = new_i - norm_a; //OBS swiched places			//set the speeds			ball.setVelX(ball_f * Math.cos(final_a));			ball.setVelY(ball_f * Math.sin(final_a));			//now the rectangle			remaining_component = Math.sin(ir) * rect_vec; //get the remaining component from the initial velocity vector (i.e. the one not colliding)			double rect_f = Math.sqrt(remaining_component*remaining_component + rect_final*rect_final); //the final velocity vector			//calculate the new angle between the new velocity vector and the normal			new_i = Math.atan(remaining_component / rect_final); //Switched places			//angle between velocity vector and x-axis			final_a = new_i -  norm_a;			//set the speeds			rect.setVelX(rect_f * Math.cos(final_a));			rect.setVelY(rect_f * Math.sin(final_a));		}

// Find the closest point on the box surface to a pointVector ClosestPointOnBox(const Vector& Point, const Vector& Centre, const Vector& Extent, bool& inside){    Vector Delta = (Point - Centre);    inside = true;       if (fabs(Delta.x) > Extent.x)      {        inside = false;        Delta.x = Extent.x * sign(Delta.x);    }    if (fabs(Delta.y) > Extent.y)      {        inside = false;        Delta.y = Extent.y * sign(Delta.y);    }    if (fabs(Delta.z) > Extent.z)      {        inside = false;        Delta.z = Extent.z * sign(Delta.z);    }    // point was found outside. all good.    if(!inside)        return Centre + Delta;    // find the MTD (Minimum Translation Distance).    Vector MTD;    // calculate the distance of the point form one face along each axes.    MTD.x = (Extent.x - fabs(Delta.x)) * sgn(Delta.x);    MTD.y = (Extent.y - fabs(Delta.y)) * sgn(Delta.y);    MTD.z = (Extent.z - fabs(Delta.z)) * sgn(Delta.z);    // Find the minimum of the three.    if (fabs(MTD.x) < fabs(MTD.y))    {        MTD.y = 0.0f;        if (fabs(MTD.z) < fabs(MTD.x))            MTD.x = 0.0f;        else             MTD.z = 0.0f;    }    else     {        MTD.x = 0.0f;        if (fabs(MTD.z) < fabs(MTD.y))            MTD.y = 0.0f;        else             MTD.z = 0.0f;    }    // point on surface    return Point + MTD;}

// Resolve a collision, using the conservation of momentumbool ResolveCollision(const Vector& N, Vector& Va, float ima, Vector& Vb, float imb, float CoR=0.8f, float CoF=0.05f){    // relative velocities    Vector V = (Va - Vb);    float vn = V.DotProduct(N); // velocity along normal, or impact velocity    if (vn > 0.0f) return false; // objects moving away from each other.     //-------------------------------------------------    // Collision Impulse    //-------------------------------------------------    float i = (-(1.0f + CoR) * vn / (ima + imb)); // collision impulse.    Vector Ir = N * i; // vector collision impulse    Va += Ir * ima;     Vb -= Ir * imb;    //-------------------------------------------------    // Friction Impulse    //-------------------------------------------------    Vector Vt = V - (vn * N); // velocity projected in plane of collision    Vector If = Vt * (-CoF / (ima + imb)); // friction impulse    Va += If * ima;    Vb -= If * imb;    return true;}

// Solve the intersection between two embedded objectsbool ResolveIntersection(const Vector& N, float depth, Vector& Ca, float ima, Vector& Cb, float imb, float relaxation=0.5f){    Ca += N * depth * (ima / (ima + imb)) * relaxation;    Cb -= N * depth * (imb / (ima + imb)) * relaxation;    return true;}

// Compute the collision between a box and a sphere// 1) Detect intersection// 2) Solve intersection// 3) Solve collision impulsebool CollideSphereBox(C_Sphere& S, C_Box& B){    // Find the closest point on box to the sphere centre    bool inside;    Vector Pclosest = ClosestPointOnBox(S.m_Centre, B.m_Centre, B.m_HalfSize, inside);    // Check if closest point is inside the sphere    Vector Delta = (S.m_Centre - Pclosest);    float delta_length_squared = Delta.DotProduct(Delta);    // Sphere and box dont intersect    if(delta_length_squared > S.m_Radius * S.m_Radius) return false;    // Generate the collision plane information    float delta_length = sqrt(delta_length_squared);    Vector Ncoll;    float dcoll;    // Ball centre was inside the box.     // The intersection is deep.    if(inside)        delta_length *= -1.0f;    Ncoll = Delta / delta_length;    dcoll = S.m_Radius - delta_length;        // resolve the intersection    ResolveIntersection(Ncoll, dcoll, S.m_Centre, S.m_InverseMass, B.m_Centre, B.m_InverseMass);    // resolve the collision    ResolveCollision(Ncoll, S.m_Velocity, S.m_InverseMass, B.m_Velocity, B.m_InverseMass);    return true;}

/*------------------------------------------------------------------File: main.cppStarted: 21/03/2007 22:33:14  $Header: $$Revision: $$Locker: $$Date: $  Author: Olivier renault------------------------------------------------------------------Module: Description: ------------------------------------------------------------------$History: $------------------------------------------------------------------*/#include <GL/glfw.h>#include <stdio.h>#include <stdlib.h>#include <math.h>#pragma comment( lib, "opengl32.lib")#pragma comment( lib, "glu32.lib")#pragma comment( lib, "glfw.lib")#pragma comment( lib, "glfwdll.lib")// crappy non-ansi MSDEV compiler compliance#define for if(0); else forextern int Update(double deltaTime);extern int Init();extern int Shutdown();int screen_width = 800;int screen_height = 600;	int Run() {	static double time = 0.0f;	const double desiredTime = 1.0f / 60.0f;	double oldTime = time;	while (glfwGetTime() - time < desiredTime) 	{}	time = glfwGetTime();	double deltaTime = time - oldTime;	return Update(deltaTime);}int main( void ){	int running = GL_TRUE;	glfwInit();	Init();	if ( !glfwOpenWindow( screen_width, screen_height, 0, 0, 0, 0, 0, 0, GLFW_WINDOW ) )	{		glfwTerminate();		return -1;	}	while( running )	{		running = Run();		if (glfwGetKey( GLFW_KEY_SPACE )) 			Init();		running &= (!glfwGetKey( GLFW_KEY_ESC ) && glfwGetWindowParam( GLFW_OPENED ));	}		Shutdown();	glfwTerminate();	return 0;}inline float fsign(float x)				{ return (x < 0.0f)? -1.0f : 1.0f; }inline float frand(float x=1.0f)		{ return (rand() / (float) RAND_MAX) * x; }inline float frand(float a, float b)	{ return a + frand(b- a); }inline void  fswap(float& a, float& b)	{ float c = a; a = b; b = c; }inline float pi   (void)				{ static const float pi = (float) atan(1.0f) * 4.0f; return pi; }class Vector{public:	float x, y, z;	inline Vector(void)	{}	Vector(const Vector& V)	: x(V.x)	, y(V.y)	, z(V.z)	{}	Vector(float ix,float iy, float iz=0.0f)	: x(ix)	, y(iy)	, z(iz)	{}	inline Vector &operator /=(const float scalar)	{ x /= scalar; y /= scalar;	z /= scalar; 	return *this; }	inline Vector &operator *=(const float scalar)	{ x *= scalar; y *= scalar;	z *= scalar; 	return *this; }		inline Vector &operator +=(const Vector &Other) { x += Other.x;	y += Other.y; z += Other.z;	return *this; }	inline Vector &operator -=(const Vector &Other) { x -= Other.x;	y -= Other.y;	return *this;	}	inline float operator * (const Vector &V)	const	{	return (x*V.x) + (y*V.y) + (z*V.z); } // dot product	inline Vector operator * (float  s)			const	{	Vector T(*this); return T *= s; }		inline Vector operator / (float  s)			const	{	Vector T(*this); return T /= s; }		inline Vector operator + (const Vector &V)	const	{	Vector T(*this); return T += V; }			inline Vector operator - (const Vector &V)	const	{	Vector T(*this); return T -= V; }	friend Vector operator * (float k, const Vector& V) {	return (V * k); }	inline Vector operator -(void) const { return Vector(-x, -y, -z); }		inline float DotProduct(const Vector& V) { return (*this) * V; }	inline float LengthSquared(void) const { return (*this) * (*this); }	inline Vector Scale(const Vector& V) { return Vector(x*V.x, y*V.y, z*V.z); }	inline float Length(void) const { return (float) sqrt(LengthSquared()); }	float Normalise(void) 	{			float fLength = Length();					if (fLength == 0.0f) 			return 0.0f; 				(*this) *= (1.0f / fLength); 			return fLength;		}	Vector Normalised(void) const	{			Vector T(*this);		T.Normalise();		return T;		}};class C_Sphere{public:	C_Sphere() {}	C_Sphere(const Vector& Centre, float radius, float mass=1.0f)	: m_Centre(Centre)	, m_Velocity(0, 0, 0)	, m_Radius(radius)	, m_InverseMass(mass > 0.00001f? 1.0f / mass : 0.0f)	{}	void Update(float dt)	{		m_Centre += m_Velocity * dt;	}	void Render()	{		glColor3f(1.0f, 1.0f, 1.0f);		glBegin(GL_LINE_LOOP);		float a = 0.0f;		float da = pi() / 16.0f;				for(int i = 0; i < 32; i ++, a += da)		{			Vector P = m_Centre + Vector((float)cos(a), (float)sin(a), 0.0f) * m_Radius;			glVertex2f(P.x, P.y);		}		glEnd();		}	Vector m_Centre;	Vector m_Velocity;	float  m_Radius;	float  m_InverseMass;};class C_Box{public:	C_Box() {}	C_Box(const Vector& Centre, const Vector& HalfSize, float mass=1.0f)	: m_Centre(Centre)	, m_Velocity(0, 0, 0)	, m_HalfSize(HalfSize)	, m_InverseMass(mass > 0.00001f? 1.0f / mass : 0.0f)	{}	void Update(float dt)	{		m_Centre += m_Velocity * dt;		}	void Render()	{		glColor3f(1.0f, 1.0f, 1.0f);		glBegin(GL_LINE_LOOP);		glVertex2f(m_Centre.x - m_HalfSize.x, m_Centre.y - m_HalfSize.y);		glVertex2f(m_Centre.x - m_HalfSize.x, m_Centre.y + m_HalfSize.y);		glVertex2f(m_Centre.x + m_HalfSize.x, m_Centre.y + m_HalfSize.y);		glVertex2f(m_Centre.x + m_HalfSize.x, m_Centre.y - m_HalfSize.y);		glEnd();	}	Vector m_Centre;	Vector m_Velocity;	Vector m_HalfSize;	float  m_InverseMass;};C_Box Walls[4];C_Sphere Ball;C_Box Paddle;int Init(){	Vector P[4] = { Vector(-1.0f, 0.0f, 10.0f), Vector(1.0f, 0.0f, 10.0f), Vector(0.0f, -1.0f, 10.0f), Vector(0.0f, 1.0f, 10.0f) };	Vector E[4] = { Vector(0.2f,  1.0f, 10.0f), Vector(0.2f, 1.0f, 10.0f), Vector(1.0f,  0.2f, 10.0f), Vector(1.0f, 0.2f, 10.0f) };	Vector L(screen_width * 0.5f, screen_height * 0.5f, 100.0f);	for(int i = 0; i < 4; i ++)	{		Vector C = L + P.Scale(L);		Vector H = E.Scale(L);		Walls = C_Box(C, H, 0.0f);	}	Paddle = C_Box(L, L.Scale(Vector(0.1f, 0.05f, 5.0f)), 1000.0f);	Ball = C_Sphere(L + L * 0.5f, L.Length() * 0.05f, 1.0f);	Paddle.m_Centre.z = Ball.m_Centre.z = 0.0f;	return 1;}int Shutdown(){	return 1;}int Render(){	glViewport( 0, 0, screen_width, screen_height );	glClearColor( 0.0f, 0.0f, 0.0f, 0.1f );	glClear( GL_COLOR_BUFFER_BIT );	glMatrixMode(GL_PROJECTION);	glLoadIdentity();	gluOrtho2D(0.0f, screen_width, screen_height, 0.0f);		glMatrixMode( GL_MODELVIEW );    glLoadIdentity();	for(int i = 0; i < 4; i ++)	{		Walls.Render();	}	Paddle.Render();	Ball.Render();	glfwSwapBuffers();	//printf("delta : %f ms, %f fps.\n", deltaTime * 1000.0f, 1.0f / deltaTime);	return 1;}int Update(double deltaTime){	int x, y;	glfwGetMousePos(&x, &y);	Vector Delta = Vector(x, y) - Paddle.m_Centre;	Paddle.m_Velocity = Delta / deltaTime;	Ball.m_Velocity += Vector(0.0f, 500.0f * deltaTime, 0.0f);	Paddle.Update(deltaTime);	Ball.Update(deltaTime);	extern bool CollideSphereBox(C_Sphere& S, C_Box& B);	for(int i = 0; i < 4; i ++)	{		CollideSphereBox(Ball, Walls);	}	CollideSphereBox(Ball, Paddle);	Render();	return 1;}// Find the closest point on the box surface to a pointVector ClosestPointOnBox(const Vector& Point, const Vector& Centre, const Vector& Extent, bool& inside){    Vector Delta = (Point - Centre);    inside = true;       if (fabs(Delta.x) > Extent.x)      {        inside = false;        Delta.x = Extent.x * fsign(Delta.x);    }    if (fabs(Delta.y) > Extent.y)      {        inside = false;        Delta.y = Extent.y * fsign(Delta.y);    }    if (fabs(Delta.z) > Extent.z)      {        inside = false;        Delta.z = Extent.z * fsign(Delta.z);    }    // point was found outside. all good.    if(!inside)        return Centre + Delta;    // find the MTD (Minimum Translation Distance).    Vector MTD;    // calculate the distance of the point form one face along each axes.    MTD.x = (Extent.x - fabs(Delta.x)) * fsign(Delta.x);    MTD.y = (Extent.y - fabs(Delta.y)) * fsign(Delta.y);    MTD.z = (Extent.z - fabs(Delta.z)) * fsign(Delta.z);    // Find the minimum of the three.    if (fabs(MTD.x) < fabs(MTD.y))    {        MTD.y = 0.0f;        if (fabs(MTD.z) < fabs(MTD.x))            MTD.x = 0.0f;        else             MTD.z = 0.0f;    }    else     {        MTD.x = 0.0f;        if (fabs(MTD.z) < fabs(MTD.y))            MTD.y = 0.0f;        else             MTD.z = 0.0f;    }    // point on surface    return Point + MTD;}// Resolve a collision, using the conservation of momentumbool ResolveCollision(const Vector& N, Vector& Va, float ima, Vector& Vb, float imb, float CoR=0.8f, float CoF=0.15f){    // relative velocities    Vector V = (Va - Vb);    float vn = V.DotProduct(N); // velocity along normal, or impact velocity    if (vn > 0.0f) return false; // objects moving away from each other.     //-------------------------------------------------    // Collision Impulse    //-------------------------------------------------    float i = (-(1.0f + CoR) * vn / (ima + imb)); // collision impulse.    Vector Ir = N * i; // vector collision impulse    Va += Ir * ima;     Vb -= Ir * imb;    //-------------------------------------------------    // Friction Impulse    //-------------------------------------------------    Vector Vt = V - (vn * N); // velocity projected in plane of collision    Vector If = Vt * (-CoF / (ima + imb)); // friction impulse    Va += If * ima;    Vb -= If * imb;    return true;}// Solve the intersection between two embedded objectsbool ResolveIntersection(const Vector& N, float depth, Vector& Ca, float ima, Vector& Cb, float imb, float relaxation=0.5f){    Ca += N * depth * (ima / (ima + imb)) * relaxation;    Cb -= N * depth * (imb / (ima + imb)) * relaxation;    return true;}// Compute the collision between a box and a sphere// 1) Detect intersection// 2) Solve intersection// 3) Solve collision impulsebool CollideSphereBox(C_Sphere& S, C_Box& B){    // Find the closest point on box to the sphere centre    bool inside;    Vector Pclosest = ClosestPointOnBox(S.m_Centre, B.m_Centre, B.m_HalfSize, inside);    // Check if closest point is inside the sphere    Vector Delta = (S.m_Centre - Pclosest);    float delta_length_squared = Delta.DotProduct(Delta);    // Sphere and box dont intersect    if(delta_length_squared > S.m_Radius * S.m_Radius) return false;    // Generate the collision plane information    float delta_length = sqrt(delta_length_squared);    Vector Ncoll;    float dcoll;    // Ball centre was inside the box.     // The intersection is deep.    if(inside)        delta_length *= -1.0f;    Ncoll = Delta / delta_length;    dcoll = S.m_Radius - delta_length;        // resolve the intersection    ResolveIntersection(Ncoll, dcoll, S.m_Centre, S.m_InverseMass, B.m_Centre, B.m_InverseMass);    // resolve the collision    ResolveCollision(Ncoll, S.m_Velocity, S.m_InverseMass, B.m_Velocity, B.m_InverseMass);    return true;}

    //-------------------------------------------------    // Collision Impulse    //-------------------------------------------------    float i = (-(1.0f + CoR) * vn / (ima + imb)); // collision impulse.    Vector Ir = N * i; // vector collision impulse    Va += Ir * ima;     Vb -= Ir * imb;    //-------------------------------------------------    // Friction Impulse    //-------------------------------------------------    Vector Vt = V - (vn * N); // velocity projected in plane of collision    Vector If = Vt * (-CoF / (ima + imb)); // friction impulse    Va += If * ima;    Vb -= If * imb;    

/*------------------------------------------------------------------File: main.cppStarted: 21/03/2007 22:33:14  $Header: $$Revision: $$Locker: $$Date: $  Author: Olivier renault------------------------------------------------------------------Module: Description: ------------------------------------------------------------------$History: $------------------------------------------------------------------*/#include <GL/glfw.h>#include <stdio.h>#include <stdlib.h>#include <math.h>#pragma comment( lib, "opengl32.lib")#pragma comment( lib, "glu32.lib")#pragma comment( lib, "glfw.lib")#pragma comment( lib, "glfwdll.lib")// crappy non-ansi MSDEV compiler compliance#define for if(0); else forextern int Update(double time, double deltaTime);extern int Init();extern int Shutdown();int screen_width = 800;int screen_height = 600;int Run() {	static double time = 0.0;	const double desiredTime = 1.0 / 60.0;	double oldTime = time;	while (glfwGetTime() - time < desiredTime) 	{}	time = glfwGetTime();	double deltaTime = time - oldTime;	return Update(time, deltaTime);}int main( void ){	int running = GL_TRUE;	glfwInit();	Init();	if ( !glfwOpenWindow( screen_width, screen_height, 0, 0, 0, 0, 0, 0, GLFW_WINDOW ) )	{		glfwTerminate();		return -1;	}	while( running )	{		running = Run();		if (glfwGetKey( GLFW_KEY_SPACE )) 			Init();		running &= (!glfwGetKey( GLFW_KEY_ESC ) && glfwGetWindowParam( GLFW_OPENED ));	}		Shutdown();	glfwTerminate();	return 0;}inline double fsign(double x)			{ return (x < 0.0)? -1.0 : 1.0; }inline double frand(double x=1.0)		{ return (rand() / (double) RAND_MAX) * x; }inline double frand(double a, double b)	{ return a + frand(b- a); }inline void  fswap(double& a, double& b){ double c = a; a = b; b = c; }inline double pi   (void)				{ static const double pi = atan(1.0) * 4.0; return pi; }class Vector{public:	union 	{		double a[2];		struct		{			double x, y;		};	};	inline Vector(void)	{}	Vector(const Vector& V)	: x(V.x)	, y(V.y)	{}	Vector(double ix,double iy)	: x(ix)	, y(iy)	{}	inline Vector &operator /=(const double scalar)	{ x /= scalar; y /= scalar;	return *this; }	inline Vector &operator *=(const double scalar)	{ x *= scalar; y *= scalar;	return *this; }		inline Vector &operator +=(const Vector &Other) { x += Other.x;	y += Other.y; return *this; }	inline Vector &operator -=(const Vector &Other) { x -= Other.x;	y -= Other.y;	return *this;	}	inline double operator * (const Vector &V)	const	{	return (x*V.x) + (y*V.y); } // dot product	inline Vector operator * (double  s)			const	{	Vector T(*this); return T *= s; }		inline Vector operator / (double  s)			const	{	Vector T(*this); return T /= s; }		inline Vector operator + (const Vector &V)	const	{	Vector T(*this); return T += V; }			inline Vector operator - (const Vector &V)	const	{	Vector T(*this); return T -= V; }	friend Vector operator * (double k, const Vector& V) {	return (V * k); }	inline Vector operator -(void) const { return Vector(-x, -y); }	inline Vector Perp() { return Vector(-y, x); }	inline double CrossProduct(const Vector& V) const { return x*V.y - y*V.x; }	inline double DotProduct(const Vector& V) const { return (*this) * V; }	inline double LengthSquared(void) const { return (*this) * (*this); }	inline Vector Scale(const Vector& V) { return Vector(x*V.x, y*V.y); }	inline double Length(void) const { return sqrt(LengthSquared()); }	// [] accessor	inline double& operator[](int i) { return a; }	inline const double operator[](int i) const { return a; }	// multiply by a transposed matrix defined by two vectors (a frame of reference)	inline Vector MultipliedByTransposeMatrix(const Vector m[]) const	{		Vector T(x*m[0][0] + y*m[0][1], x*m[1][0]+y*m[1][1]);		return T;	}   		inline Vector MultipliedByMatrix(const Vector m[]) const	{		// multiply by a matrix defined by two vectors  (a frame of reference)		Vector T(x*m[0][0] + y*m[1][0], x*m[0][1]+y*m[1][1]);		return T;	}		double Normalise(void) 	{			double fLength = Length();					if (fLength == 0.0) 			return 0.0; 				(*this) *= (1.0 / fLength); 			return fLength;		}	Vector Normalised(void) const	{			Vector T(*this);		T.Normalise();		return T;		}};class C_Body{public:	C_Body() 	{}	C_Body(const Vector& Position, double mass, double inertia) 	: m_Position(Position)	, m_LinearVelocity(0.0, 0.0)	, m_InverseMass(mass > 0.00001f? 1.0 / mass : 0.0) 	, m_Orientation(0.0)	, m_AngularVelocity(0.0)	, m_InverseInertia((inertia > 0.0001f)? 1.0 / inertia : 0.0)	{		UpdateAxes();	}	virtual void Render() 	{		glBegin(GL_LINES);		glVertex2d(m_Position.x, m_Position.y);		glVertex2d(m_Position.x + m_Axes[0].x * 20, m_Position.y + m_Axes[0].y * 20);		glVertex2d(m_Position.x, m_Position.y);		glVertex2d(m_Position.x + m_Axes[1].x * 20, m_Position.y + m_Axes[1].y * 20);		glEnd();	}	virtual void Update(double t, double dt) 	{		m_Position		 += m_LinearVelocity * dt;		m_Orientation	 += m_AngularVelocity * dt;				UpdateAxes();	}	// Calculate the major axes of the box	void UpdateAxes()	{		double co = cos(m_Orientation);		double si = sin(m_Orientation);		m_Axes[0] = Vector(co, si);		m_Axes[1] = Vector(-si, co);	}	void AddGravity(const Vector& Gravity, double dt)	{		m_LinearVelocity += Gravity * dt;	}		void AddImpulse(const Vector& P, const Vector& J)	{		// Add an impulse (change angular and linear momentums). 		m_LinearVelocity += J * m_InverseMass;		m_AngularVelocity += (P - m_Position).CrossProduct(J) * m_InverseInertia;	}		// Move bodies away from each other (distance of intersection is vector between contact points).	static bool SeparateBodies(C_Body& A, C_Body& B, const Vector& Pa, const Vector& Pb, const Vector& N, double relax=0.8)	{		Vector D = (Pa - Pb);		double d = (D.DotProduct(N));		if(d < 0.0) return false;			// Move each bodies directly, based on weight ratios		A.m_Position += D * A.m_InverseMass / (A.m_InverseMass + B.m_InverseMass);		B.m_Position -= D * B.m_InverseMass / (A.m_InverseMass + B.m_InverseMass);		return true;	}	//------------------------------------------------------------------------------------------------------	// compute impulse (frction and restitution).	// ------------------------------------------	//	//									-(1+Cor)(Vel.norm)	//			j =  ------------------------------------------------------------	//			     [1/Ma + 1/Mb] + [Ia' * (ra x norm)²] + [Ib' * (rb x norm)²]	//------------------------------------------------------------------------------------------------------	static bool CollideBodies(C_Body& A, C_Body& B, const Vector& Pa, const Vector& Pb, const Vector& N, double CoR = 0.75, double CoF = 0.15)	{		// collision points, relative to the body centre of mass		Vector Ra  = (Pa - A.m_Position);		Vector Rb  = (Pb - B.m_Position);				// velocities at the contact point		Vector Va = A.m_LinearVelocity + Ra.Perp() * A.m_AngularVelocity;		Vector Vb = B.m_LinearVelocity + Rb.Perp() * B.m_AngularVelocity;	    // relative velocities		Vector V = (Va - Vb);		double vn = V.DotProduct(N); // velocity along normal, or impact velocity		if (vn > 0.0) return false;    // objects moving away from each other. 		//-------------------------------------------------		// Collision Impulse		//-------------------------------------------------		double numer = -(1.0 + CoR) * vn;		double denom0 = (A.m_InverseMass + B.m_InverseMass);		double denom1 = A.m_InverseInertia * Ra.CrossProduct(N) * Ra.CrossProduct(N); 		double denom2 = B.m_InverseInertia * Rb.CrossProduct(N) * Rb.CrossProduct(N);		double i = numer / (denom0 + denom1 + denom2);		Vector Ir = N * i; // vector collision impulse		//-------------------------------------------------		// Friction Impulse		//-------------------------------------------------		Vector Vt = V - (vn * N); // velocity projected in plane of collision		Vector If = Vt * (-CoF / (A.m_InverseMass + B.m_InverseMass)); // friction impulse				//-------------------------------------------------		// Add total collision impulse		//-------------------------------------------------		Vector I = (Ir + If);				A.AddImpulse(Pa,  I);		B.AddImpulse(Pb, -I);		return true;	}public:	Vector m_Position;	Vector m_LinearVelocity;	double m_InverseMass;		double m_Orientation;	double m_AngularVelocity;	double m_InverseInertia;	Vector m_Axes[2];};class C_Sphere: public C_Body{public:	C_Sphere() {}	C_Sphere(const Vector& Centre, double radius, double mass=1.0)	: C_Body(Centre, mass,GetInertiaMoment(radius) * mass)	, m_Radius(radius)	{}	void Render()	{		glColor3d(1.0, 1.0, 1.0);					glBegin(GL_LINE_LOOP);		double a = 0.0;		double da = pi() / 16.0;				for(int i = 0; i < 32; i ++, a += da)		{			Vector P = m_Position + Vector(cos(a), sin(a)) * m_Radius;			glVertex2d(P.x, P.y);		}		glEnd();		C_Body::Render();	}	// Moment of inertia of a sphere	static double GetInertiaMoment(double r)	{		return 0.5 * (r*r);	}	double m_Radius;};class C_Box: public C_Body{public:	C_Box() {}	C_Box(const Vector& Centre, const Vector& HalfSize, double mass=1.0)	: C_Body(Centre, mass, GetInertiaMoment(HalfSize.x, HalfSize.y) * mass)	, m_HalfSize(HalfSize)	{}	void Render()	{		glPushMatrix();		glTranslated(m_Position.x, m_Position.y, 0.0);		glRotated(m_Orientation * 180.0f / pi(), 0.0, 0.0, 1.0);		glScaled(m_HalfSize.x, m_HalfSize.y, 1.0);		glColor3f(1.0, 1.0, 1.0);		glBegin(GL_LINE_LOOP);		glVertex2d(-1, -1);		glVertex2d(-1,  1);		glVertex2d( 1,  1);		glVertex2d( 1, -1);		glEnd();		glPopMatrix();		C_Body::Render();	}	// Moment of inertia of a box	static double GetInertiaMoment(double dx, double dy)	{		return (1.0f / 12.0f) * (dx*dx + dy*dy);	}	Vector m_HalfSize;};C_Box Walls[4];C_Sphere Ball;C_Box Paddle;int Init(){	Vector P[4] = { Vector(-1.0, 0.0), Vector(1.0, 0.0), Vector(0.0, -1.0), Vector(0.0, 1.0) };	Vector E[4] = { Vector(0.2,  1.0), Vector(0.2, 1.0), Vector(1.0,  0.2), Vector(1.0, 0.2) };	Vector L(screen_width * 0.5, screen_height * 0.5);	for(int i = 0; i < 4; i ++)	{		Vector C = L + P.Scale(L);		Vector H = E.Scale(L);		Walls = C_Box(C, H, 0.0);	}	Paddle = C_Box(L, L.Scale(Vector(0.1f, 0.05f)), 100.0);	Ball = C_Sphere(L + L * 0.5, L.Length() * 0.05, 3.0);		return 1;}int Shutdown(){	return 1;}int Render(){	glfwGetWindowSize(&screen_width, &screen_height);	glViewport( 0, 0, screen_width, screen_height );	glClearColor( 0.0, 0.0, 0.0, 0.1f );	glClear( GL_COLOR_BUFFER_BIT );	glMatrixMode(GL_PROJECTION);	glLoadIdentity();	gluOrtho2D(0.0, screen_width, 0.0, screen_height);		glMatrixMode( GL_MODELVIEW );    glLoadIdentity();	for(int i = 0; i < 4; i ++)	{		Walls.Render();	}	Paddle.Render();	Ball.Render();	glfwSwapBuffers();	//printf("delta : %f ms, %f fps.\n", deltaTime * 1000.0, 1.0 / deltaTime);	return 1;}int Update(double time, double deltaTime){	// compute the paddle velocity (based on mouse position and position the last frame)	glfwGetWindowSize(&screen_width, &screen_height);	int x, y;	glfwGetMousePos(&x, &y);	Vector Delta = Vector(x, screen_height-y) - Paddle.m_Position;	Paddle.m_LinearVelocity = Delta / deltaTime;	// Add gravity to the ball	Ball.AddGravity(Vector(0.0, -500.0), deltaTime);	Paddle.Update(time, deltaTime);	Ball.Update(time, deltaTime);	extern bool CollideSphereBox(C_Sphere& S, C_Box& B);	// collide sphere against walls	for(int i = 0; i < 4; i ++)	{		CollideSphereBox(Ball, Walls);	}	// collide sphere against paddle	CollideSphereBox(Ball, Paddle);	// render stuff	Render();	return 1;}// Find the closest point on the box surface (with an orientation) to a pointVector ClosestPointOnBox(const Vector& Point, const Vector& Centre, const Vector& Extent, const Vector* OrientationMatrix, bool& inside){	Vector WorldDelta = (Point - Centre);    inside = true;	// convert to box'local space	Vector Delta = WorldDelta.MultipliedByTransposeMatrix(OrientationMatrix);   	    if (fabs(Delta.x) > Extent.x)      {        inside = false;        Delta.x = Extent.x * fsign(Delta.x);    }    if (fabs(Delta.y) > Extent.y)      {        inside = false;        Delta.y = Extent.y * fsign(Delta.y);    }    	// point was found outside. all good. (converted to world coordinates).    if(!inside)	{        return Centre + Delta.MultipliedByMatrix(OrientationMatrix);	}    // find the MTD (Minimum Translation Distance).    Vector MTD;    // calculate the distance of the point form one face along each axes.    MTD.x = (Extent.x - fabs(Delta.x)) * fsign(Delta.x);    MTD.y = (Extent.y - fabs(Delta.y)) * fsign(Delta.y);        // Find the minimum of the three.    if (fabs(MTD.x) < fabs(MTD.y))    {        MTD.y = 0.0;    }    else     {        MTD.x = 0.0;    }    // point on surface (converted to world coordinates).    return Point + MTD.MultipliedByMatrix(OrientationMatrix);}// Compute the collision between a box and a sphere// 1) Detect intersection// 2) Solve intersection// 3) Solve collision impulsebool CollideSphereBox(C_Sphere& S, C_Box& B){    // Find the closest point on box to the sphere centre    bool inside;    Vector Pclosest = ClosestPointOnBox(S.m_Position, B.m_Position, B.m_HalfSize, B.m_Axes, inside);    // Check if closest point is inside the sphere    Vector Delta = (S.m_Position - Pclosest);    double delta_length_squared = Delta.DotProduct(Delta);    // Sphere and box dont intersect    if(delta_length_squared > S.m_Radius * S.m_Radius) return false;    // Generate the collision plane information    double delta_length = sqrt(delta_length_squared);        // Ball centre was inside the box.     // The intersection is deep.    if(inside)        delta_length *= -1.0;	// collision plane    Vector N = Delta / delta_length;    double d = S.m_Radius - delta_length;	// Points of contact    Vector Pb = Pclosest; // point of colllision on the box	Vector Ps = Pb + N * d; // point of collision on the sphere.    	// Separate bodies    C_Body::SeparateBodies(S, B, Ps, Pb, N);		// Apply collision impulse    C_Body::CollideBodies(S, B, Ps, Pb, N);    return true;}