#include "physics.h"

Environ *Environ::mInstance = 0;

const unsigned int MAX_CONTACTS = 10;

void NearCallback(void *data, dGeomID g1, dGeomID g2)
{
	static dContact contact[MAX_CONTACTS];
	static unsigned int i;
	static Environ *environ = Environ::getInstance();

	dBodyID b1 = dGeomGetBody(g1), b2 = dGeomGetBody(g2);

	if (b1 && b2 && dAreConnected(b1, b2))
		return;

	for (i = 0; i < MAX_CONTACTS; ++i)
	{
		contact[i].surface.mode = dContactBounce | dContactApprox1;
		contact[i].surface.mu = dInfinity;
		contact[i].surface.bounce = 1;
		contact[i].surface.bounce_vel = .01;
	}

	unsigned int numContacts = dCollide(g1, g2, MAX_CONTACTS,
					    &contact[0].geom,
					    sizeof(dContact));
	if (numContacts)
	{
		bool bounce1 = false, bounce2 = false;

		CollisionData *a = (CollisionData *) dGeomGetData(g1),
			      *b = (CollisionData *) dGeomGetData(g2);
	
		Collidable *ca = (a ? ((Collidable *) a->ptr) : 0);
		Collidable *cb = (b ? ((Collidable *) b->ptr) : 0);

		bounce1 = (ca ? (ca->shouldCollide(cb)) : true);
		bounce2 = (cb ? (cb->shouldCollide(ca)) : true);

		if ((a && a->type == COLLISION_RAY) ||
		    (b && b->type == COLLISION_RAY))
		{
			if (ca && cb)
				ca->avoid(cb), cb->avoid(ca);
			return;
		}

		if (!(bounce1 && bounce2))
			return;

		dMatrix3 RI;
		dRSetIdentity(RI);
		for (i = 0; i < numContacts; ++i)
		{
			dJointID c = dJointCreateContact(environ->mWorld,
							 environ->mContactGroup,
							 contact+i);

			if (bounce1 && bounce2) dJointAttach(c, b1, b2);
			else if (bounce1)	dJointAttach(c, b1, 0);
			else			dJointAttach(c, 0, b2);
		}
	}
}