// 2d Physics Library Version 1.0
// Written by Harvey Cotton

// An easy to use API which allows programmers to
// add realistic dynamics to their games with
// a basic knowledge of physics.

// ***************************************************************** Libraries

#include <iostream.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>
#include <dos.h>
#include <graphics.h>
#include <time.h>

// ***************************************************************** Reference

// Velcity (v) = ds/dt
// Acceleration (a) = dv/dt
// Force (f) = m*a
// Momentum (p) = m*v
// Conservation of momentum (in a perfectly elastic collision)
// 	m1*v1 + m2*v2 (before) = m1*v1 + m2*v2 (after)
// Kinetic energy (ke) = 0.5*m*v^2 = Joules

// ******************************************************************* Defines

#define GRAVITY_EARTH		9.8
#define GRAVITY_MOON		5.9
#define GRAVITY_ZERO		0.0

#define BOUNDARY_NOCLIP		0
#define BOUNDARY_CLIP		1
#define BOUNDARY_BOUNCE		2

#define PAL_25FPS		25
#define PAL_50FPS		50
#define NTSC_30FPS		30
#define NTSC_60FPS		60

#define SetPosX(_object,_x)		(_object->x=(_x)<<12)
#define SetPosY(_object,_y)		(_object->y=(_y)<<12)

#define GetPosX(_object)		(_object->x>>12)
#define GetPosY(_object)		(_object->y>>12)

#define SetVelX(_world,_object,_vx)	(_object->vx=(_vx)*_world->frame)
#define SetVelY(_world,_object,_vy)	(_object->vy=(_vy)*_world->frame)

// **************************************************************** Structures

typedef struct
{
	// Time
	unsigned long time;		// Incremental counter (1 unit = 1 frame)
	unsigned long frame;		// Time elapsed per frame (4096/framerate)

	// External forces
	long	gravity_x;		// Gravity in x direction (1 pixel/s = 4096)
	long	gravity_y;		// Gravity in y direction (1 pixel/s = 4096)

	// Boundaries
	long	x1,y1;			// Top-left boundary (1 pixel = 4096)
	long	x2,y2;			// Bottom-right boundary (1 pixel = 4096)
	char	collision;		// Collision flag (0=noclip,1=clip,2=bounce)
} PhysicsWorld;

typedef struct
{
	long	x,y;			// Centre of zone (1 pixel = 4096)
	long	w,h;			// Width & height of zone (1 pixel = 4096)
} PhysicsZone;

typedef struct
{
	long	x1,y1;			// First point (1 pixel = 4096)
	long	x2,y2;			// Second point (1 pixel = 4096)
	long	nx,ny;			// Normal of barrier (1 pixel = 4096)
	long	x,y;			// Centre point (1 pixel = 4096)
	long	w,h;			// Half width & height (1 pixel = 4096)
} PhysicsBarrier;

typedef struct
{
	long	x,y;			// Field position (1 pixel = 4096)
	long	radius;			// Field radius (1 pixel = 4096)
	long	gravity;		// Local acceleration (1 pixel/s = 4096)
} PhysicsForceField;

typedef struct
{
	long	x,y;			// Object position (1 pixel = 4096)
	long	vx,vy;			// Object velocity (1 pixel/s = 4096)
	long	acceleration;		// Object acceleration (1 pixel/s = 4096)
	long	mass;			// Object mass (1 unit = 4096)
	long	radius;			// Object radius (1 pixel = 4096)
	long	restitution;		// The portion of energy lost in collisions (0=100% loss, 4096=0%loss)
} PhysicsBody;

// *********************************************************** World Functions

void InitPhysicsWorld (PhysicsWorld *world,float grav_x,float grav_y,long fps,long x1,long y1,long x2,long y2,char collision)
{
	world->time=0;
	world->frame=4096/fps;
	world->gravity_x=grav_x*world->frame;
	world->gravity_y=grav_y*world->frame;
	world->x1=x1<<12;
	world->y1=y1<<12;
	world->x2=x2<<12;
	world->y2=y2<<12;
	world->collision=collision;
}

void UpdatePhysicsWorld (PhysicsWorld *w)
{
	w->time++;
}

void SetPhysicsWorldGravity (PhysicsWorld *world,float gravity,long x,long y)
{
	register long tmp;

	// Compute vector length
	world->gravity_x=labs(x);
	world->gravity_y=labs(y);
	tmp=world->gravity_x<world->gravity_y ? world->gravity_x : world->gravity_y;
	tmp=world->gravity_x+world->gravity_y-(tmp>>1)-(tmp>>2)+(tmp>>3)+(tmp>>4)-(tmp>>5);
	if (tmp<1) tmp=1;

	// Generate gravity based on direction and acceleration
	world->gravity_x=(x*gravity*world->frame)/tmp;
	world->gravity_y=(y*gravity*world->frame)/tmp;
}

// ************************************************************ Zone Functions

void InitPhysicsZone (PhysicsZone *zone,long x,long y,long w,long h)
{
	zone->x=x<<12;
	zone->y=y<<12;
	zone->w=w<<11;
	zone->h=h<<11;
}

long PhysicsZoneCollision (PhysicsZone *zone,PhysicsBody *body)
{
	register long dx,dy;

	dx=(zone->w+body->radius)-labs(zone->x-body->x);
	dy=(zone->h+body->radius)-labs(zone->y-body->y);

	if (dx>0 && dy>0)
	{
		if (dx<dy)
		{
		       body->vx=-((body->vx*body->restitution)>>12);
		       if (body->x<zone->x) body->x=zone->x-zone->w-body->radius;
		       else body->x=zone->x+zone->w+body->radius;
		}
		else
		{
		       body->vy=-((body->vy*body->restitution)>>12);
		       if (body->y<zone->y) body->y=zone->y-zone->h-body->radius;
		       else body->y=zone->y+zone->h+body->radius;
		}

		return 1;
	}

	return 0;
}

// ********************************************************* Barrier Functions

void InitPhysicsBarrier (PhysicsBarrier *barrier,long x1,long y1,long x2,long y2)
{
	register long nx,ny,tmp;

	// Calculate vector length
	nx=labs(x1-x2);
	ny=labs(y1-y2);
	tmp=nx<ny ? nx : ny;
	tmp=nx+ny-(tmp>>1)-(tmp>>2)+(tmp>>3)+(tmp>>4)-(tmp>>5);
	if (tmp<1) tmp=1;

	// Set fixed point positions
	x1<<=12;
	y1<<=12;
	x2<<=12;
	y2<<=12;
	barrier->x1=x1;
	barrier->y1=y1;
	barrier->x2=x2;
	barrier->y2=y2;
	barrier->x=(x1+x2)>>1;
	barrier->y=(y1+y2)>>1;
	barrier->w=labs(x1-x2)>>1;
	barrier->h=labs(y1-y2)>>1;

	// Calculate normal vector
	barrier->nx=(y1-y2)/tmp;
	barrier->ny=(x2-x1)/tmp;
}

long PhysicsBarrierCollision (PhysicsBarrier *barrier,PhysicsBody *body)
{
	register long x1,y1,x2,y2,x3,y3,x4,y4,tmp;
	long r,s;

	r=body->radius+(labs(body->vx)>>1);
	s=body->radius+(labs(body->vy)>>1);

	// Collision test 1 - do bounding boxes overlap?
	if ((r+barrier->w)>labs(body->x-barrier->x) && (s+barrier->h)>labs(body->y-barrier->y))
	{
		x1=barrier->x1>>4;
		y1=barrier->y1>>4;
		x2=barrier->x2>>4;
		y2=barrier->y2>>4;
		x3=body->x>>4;
		y3=body->y>>4;
		x4=(body->x+body->vx+(body->vx<1 ? -body->radius : body->radius))>>4;
		y4=(body->y+body->vy+(body->vy<1 ? -body->radius : body->radius))>>4;

		// Compute demoninator
		tmp=((x2-x1)*(y4-y3)-(y2-y1)*(x4-x3))>>8;
		tmp=tmp==0 ? 1 : tmp;

		// Compute numerators
		r=((y1-y3)*(x4-x3)-(x1-x3)*(y4-y3))/tmp;
		s=((y1-y3)*(x2-x1)-(x1-x3)*(y2-y1))/tmp;

		// Collision test 3 - do lines intersect?
		if (r>=0 && r<=256 && s>=0 && s<=256)
		{
			// Calculate vector reflection
			tmp=-(((body->vx*barrier->nx)>>12)+((body->vy*barrier->ny)>>12));
			tmp=(tmp*body->restitution)>>12;
			body->vx+=(tmp*barrier->nx)>>11;
			body->vy+=(tmp*barrier->ny)>>11;

			// Clip body to surface
			body->x=(x1+(((x2-x1)*r)>>8))<<4;
			body->x+=((body->vx<0 ? -1 : 1)*barrier->nx*body->radius)>>11;
			body->y=(y1+(((y2-y1)*r)>>8))<<4;
			body->y+=((body->vy<0 ? -1 : 1)*barrier->ny*body->radius)>>11;

			/*
			r=((y2-y1)<<8)/(x2-x1);
			s=((y4-y3)<<8)/(x4-x3);

			if (r!=s)
			{
				body->x=(-r/(s-r))*x2+r*(y3-y1)+x1;
				body->y=(r*y1-s*y3+x3-x1)/(r-s);

				body->x=((-r*x2)/(s-r))+((r*(y3-y1))>>8)+x1;
				body->x<<=4;

				body->y=((((r*y1)>>8)-((s*y3)>>8)+x3-x1)<<8)/(r-s);
				body->y<<=4;

				body->x+=((body->vx<0 ? -1 : 1)*barrier->nx*body->radius)>>12;
				body->y+=((body->vy<0 ? -1 : 1)*barrier->ny*body->radius)>>12;

			}
			*/
			return 1;
		}
	}

	return 0;
}

// ***************************************************** Force Field Functions

void InitPhysicsForceField (PhysicsWorld *world,PhysicsForceField *field,long x,long y,long radius,long gravity)
{
	field->x=x<<12;
	field->y=y<<12;
	field->radius=radius<<12;
	field->gravity=gravity*world->frame;
}

long PhysicsForceFieldCollision (PhysicsForceField *field,PhysicsBody *body)
{
	register long x,y,nx,ny,tmp;

	x=labs(field->x-body->x);
	y=labs(field->y-body->y);
	tmp=x<y ? x : y;
	tmp=x+y-(tmp>>1)-(tmp>>2)+(tmp>>3)+(tmp>>4)-(tmp>>5);
	if (tmp<1) tmp=1;

	if (field->radius+body->radius>tmp)
	{
		// Find the normal between body & field
		nx=((field->x-body->x)<<12)/tmp;
		ny=((field->y-body->y)<<12)/tmp;

		// Calculate the acceleration based on the normal and the gravity
		x=(nx*field->gravity)>>12;
		y=(ny*field->gravity)>>12;

		// Update velocity of body
		body->vx+=x;
		body->vy+=y;

		return 1;
	}

	return 0;
}

// ************************************************************ Body Functions

void InitPhysicsBody (PhysicsWorld *world,PhysicsBody *body,long x,long y,long vx,long vy,long accel,long mass,long radius,float restitution)
{
	body->x=x<<12;
	body->y=y<<12;
	body->vx=vx*world->frame;
	body->vy=vy*world->frame;
	body->acceleration=accel*world->frame;
	body->mass=mass;
	body->radius=radius<<12;
	body->restitution=restitution*4096;
}

void UpdatePhysicsBody (PhysicsWorld *world,PhysicsBody *body)
{
	body->vx+=body->acceleration+world->gravity_x;
	body->vy+=body->acceleration+world->gravity_y;

	body->x+=body->vx;
	body->y+=body->vy;

	switch(world->collision)
	{
		case BOUNDARY_CLIP :
			if (body->x<world->x1+body->radius)
			{
				body->x=world->x1+body->radius;
			}
			if (body->x>world->x2-body->radius)
			{
				body->x=world->x2-body->radius;
			}
			if (body->y<world->y1+body->radius)
			{
				body->y=world->y1+body->radius;
			}
			if (body->y>world->y2-body->radius)
			{
				body->y=world->y2-body->radius;
			}
		break;

		case BOUNDARY_BOUNCE :
			if (body->x<world->x1+body->radius)
			{
				body->x=world->x1+body->radius;
				body->vx=-((body->vx*body->restitution)>>12);
			}
			if (body->x>world->x2-body->radius)
			{
				body->x=world->x2-body->radius;
				body->vx=-((body->vx*body->restitution)>>12);
			}
			if (body->y<world->y1+body->radius)
			{
				body->y=world->y1+body->radius;
				body->vy=-((body->vy*body->restitution)>>12);
			}
			if (body->y>world->y2-body->radius)
			{
				body->y=world->y2-body->radius;
				body->vy=-((body->vy*body->restitution)>>12);
			}
		break;
	}
}

long PhysicsBodyCollision (PhysicsBody *body1,PhysicsBody *body2)
{
	register long nx,ny,tx,ty;
	long vait,vain,vbit,vbin;
	long vx1,vy1,vx2,vy2;
	long tmp;

	nx=labs(body1->x-body2->x);
	ny=labs(body1->y-body2->y);
	tmp=nx<ny ? nx : ny;
	tmp=nx+ny-(tmp>>1)-(tmp>>2)+(tmp>>3)+(tmp>>4)-(tmp>>5);
	if (tmp<1) tmp=1;

	if (body1->radius+body2->radius>tmp)
	{
		// Calculate the normal from one body to another
		nx=((body2->x-body1->x)<<12)/tmp;
		ny=((body2->y-body1->y)<<12)/tmp;

		// Calculate tangent (which is just 90 degrees from normal)
		tx=-ny;
		ty=nx;

		// Compute initial & final velocities of both bodies
		vait=((body1->vx*tx)>>12)+((body1->vy*ty)>>12);
		vain=((body1->vx*nx)>>12)+((body1->vy*ny)>>12);
		vbit=((body2->vx*tx)>>12)+((body2->vy*ty)>>12);
		vbin=((body2->vx*nx)>>12)+((body2->vy*ny)>>12);

		// Compute final velocity of first body
		vx1=(body2->mass*vbin)>>12;
		vx1=(vx1*(body2->restitution+4096))>>12;
		vx1+=(vain*(body1->mass-((body2->restitution*body2->mass)>>12)))>>12;
		vx1=(vx1<<12)/(body1->mass+body2->mass);
		vy1=vait;

		// Compute final velocity of second body
		vx2=(body1->mass*vain)>>12;
		vx2=(vx2*(body1->restitution+4096))>>12;
		vx2-=(vbin*(body1->mass-((body1->restitution*body2->mass)>>12)))>>12;
		vx2=(vx2<<12)/(body1->mass+body2->mass);
		vy2=vbit;

		// Compute sum of x,y components relative to n,t axes
		body1->vx=((vx1*nx)>>12)+((vy1*tx)>>12);
		body1->vy=((vx1*ny)>>12)+((vy1*ty)>>12);
		body2->vx=((vx2*nx)>>12)+((vy2*tx)>>12);
		body2->vy=((vx2*ny)>>12)+((vy2*ty)>>12);

		// Prevent bodies from sticking together by pushing them away slightly
		tmp=(tmp-(body1->radius+body2->radius))>>1;
		body1->x+=(tmp*nx)>>12;
		body1->y+=(tmp*ny)>>12;
		body2->x+=(tmp*-nx)>>12;
		body2->y+=(tmp*-ny)>>12;

		return 1;
	}

	return 0;
}

// ****************************************************************** Graphics

void InitialiseGraphics (int gdriver,int gmode)
{
	// Load & initiate graphics mode
	initgraph(&gdriver, &gmode, "");
	if (graphresult()!=grOk) exit(1);
	sleep(2);
	setbkcolor(BLACK);
	cleardevice();
}

int GetKey ()
{
	if (kbhit())
	{
		int key=getch();
		if (key==0) key=getch()+128;
		return key;
	}
	else return 0;
}

void double_buffer ()
{
	static int PAGE=0;
	setvisualpage(PAGE);
	setactivepage(PAGE=1-PAGE);
	while(inp(0x3DA) & 0x08);
	while(!(inp(0x3DA) & 0x08));
	cleardevice();
}

// ************************************************************* Mouse Support

long MOUSE_X=0,MOUSE_Y=0,MOUSE_CLICK=0;

void mouse_reset () // Initiate mouse
{
	union REGS inregs,outregs;
	inregs.x.ax=0x00;
	int86(0x33, &inregs,&outregs);
	MOUSE_CLICK=outregs.x.bx;
}

void mouse_pointer (int flag) // Show arrow (flag=1) or hide arrow (flag=0)
{
	union REGS inregs,outregs;
	if (flag) { inregs.x.ax=0x01; int86(0x33,&inregs,&outregs); }
	else { inregs.x.ax=0x02; int86(0x33,&inregs,&outregs); }
}

void mouse_arrow () // Alternative pointer
{
	register int arrow[16]={MOUSE_X,MOUSE_Y,MOUSE_X+10,MOUSE_Y+4,MOUSE_X+8,MOUSE_Y+6,MOUSE_X+14,MOUSE_Y+12,
				MOUSE_X+12,MOUSE_Y+14,MOUSE_X+6,MOUSE_Y+8,MOUSE_X+4,MOUSE_Y+10,MOUSE_X,MOUSE_Y};
	setcolor(LIGHTGRAY);
	setfillstyle(SOLID_FILL,WHITE);
	fillpoly(8,arrow);
}

void mouse_read () // Read mouse into globals
{
	union REGS inregs,outregs;
	inregs.x.ax=0x03;
	int86(0x33,&inregs,&outregs);
	MOUSE_X    =outregs.x.cx;
	MOUSE_Y    =outregs.x.dx;
	MOUSE_CLICK=outregs.x.bx;
}

void mouse_sensitivity (int x,int y,int step) // Sets sensitivity of mouse
{
	union REGS inregs,outregs;
	inregs.x.bx=x;
	inregs.x.cx=y;
	inregs.x.dx=step;
	inregs.x.ax=0x1A;
	int86(0x33,&inregs,&outregs);
}

// ************************************************************* Demonstration

#define BALLS	1
#define K	0.8

PhysicsWorld		world;
PhysicsZone		zone[2];
PhysicsBarrier		barrier;
PhysicsForceField	field;
PhysicsBody		ball[BALLS];

void main ()
{
	InitialiseGraphics(VGA,VGAMED);

	mouse_reset();

	int i,j,input=0;

	randomize();

	/***** Initialise environment & bodies *****/

	InitPhysicsWorld(&world, 0,GRAVITY_EARTH, 70, 0,0,639,349, BOUNDARY_BOUNCE);

	InitPhysicsZone(&zone[0],320,349,100,50);

	InitPhysicsZone(&zone[1],580,349,20,200);

	InitPhysicsBarrier(&barrier,0,175,200,175);

	InitPhysicsForceField(&world,&field,320,349,50,-50);

	for(i=0; i<BALLS; i++)
		InitPhysicsBody(&world,&ball[i],rand()%640,rand()%350,(rand()%400)-200,(rand()%400)-200,0, (i%10)+5,(i%10)+5, K);

	/***** Animation loop *****/

	while(input!=27)
	{
		/***** User input *****/

		mouse_read();

		input=GetKey();

		if (input==32)
			for(i=0; i<BALLS; i++)
				InitPhysicsBody(&world,&ball[i],rand()%640,rand()%350,(rand()%200)-100,(rand()%200)-100,0, (i%10)+5,(i%10)+5, K);

		/***** Update world *****/

		UpdatePhysicsWorld(&world);

		/***** Update force field *****/

		field.x=MOUSE_X<<12;
		field.y=MOUSE_Y<<12;

		/***** Collision detection *****/

		for(i=0; i<BALLS; i++)
		{
			UpdatePhysicsBody(&world,&ball[i]);

			PhysicsForceFieldCollision(&field,&ball[i]);

			for(j=i+1; j<BALLS; j++)
				if (i!=j) PhysicsBodyCollision(&ball[i],&ball[j]);

			PhysicsZoneCollision(&zone[0],&ball[i]);

			PhysicsZoneCollision(&zone[1],&ball[i]);

			PhysicsBarrierCollision(&barrier,&ball[i]);
		}


		/***** Draw everything *****/

		double_buffer();

		setcolor((world.time%15)+1);
		circle(field.x>>12,field.y>>12,field.radius>>12);
		if (field.gravity<1) circle(field.x>>12,field.y>>12,world.time%((field.radius>>12)));
		else circle(field.x>>12,field.y>>12,((field.radius>>12)-(world.time%((field.radius>>12)))));

		for(i=0; i<BALLS; i++)
		{
			setcolor((i%15)+1);
			circle(ball[i].x>>12,ball[i].y>>12,ball[i].radius>>12);
		}

		setcolor(WHITE);
		rectangle((zone[0].x-zone[0].w)>>12,(zone[0].y-zone[0].h)>>12,(zone[0].x+zone[0].w)>>12,(zone[0].y+zone[0].h)>>12);
		rectangle((zone[1].x-zone[1].w)>>12,(zone[1].y-zone[1].h)>>12,(zone[1].x+zone[1].w)>>12,(zone[1].y+zone[1].h)>>12);
		line(barrier.x1>>12,barrier.y1>>12,barrier.x2>>12,barrier.y2>>12);
		rectangle(0,0,639,349);
		outtextxy(10,10,"2d Physics Library Demonstration - By Harvey.C");
		outtextxy(10,20,"Press SPACE to reset or ESC to exit");
	}

	closegraph();
}