/*
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2ContactManager.h"
#include "b2World.h"
#include "b2Body.h"

// This is a callback from the broadphase when two AABB proxies begin
// to overlap. We create a b2Contact to manage the narrow phase.
void* b2ContactManager::PairAdded(void* proxyUserData1, void* proxyUserData2)
{
	b2Shape* shape1 = (b2Shape*)proxyUserData1;
	b2Shape* shape2 = (b2Shape*)proxyUserData2;

	b2Body* body1 = shape1->GetBody();
	b2Body* body2 = shape2->GetBody();

	if (body1->IsStatic() && body2->IsStatic())
	{
		return &m_nullContact;
	}

	if (shape1->GetBody() == shape2->GetBody())
	{
		return &m_nullContact;
	}

	if (body2->IsConnected(body1))
	{
		return &m_nullContact;
	}

	if (m_world->m_contactFilter != NULL && m_world->m_contactFilter->ShouldCollide(shape1, shape2) == false)
	{
		return &m_nullContact;
	}

	// Call the factory.
	b2Contact* c = b2Contact::Create(shape1, shape2, &m_world->m_blockAllocator);

	if (c == NULL)
	{
		return &m_nullContact;
	}

	// Contact creation may swap shapes.
	shape1 = c->GetShape1();
	shape2 = c->GetShape2();
	body1 = shape1->GetBody();
	body2 = shape2->GetBody();

	// Insert into the world.
	c->m_prev = NULL;
	c->m_next = m_world->m_contactList;
	if (m_world->m_contactList != NULL)
	{
		m_world->m_contactList->m_prev = c;
	}
	m_world->m_contactList = c;

	// Connect to island graph.

	// Connect to body 1
	c->m_node1.contact = c;
	c->m_node1.other = body2;

	c->m_node1.prev = NULL;
	c->m_node1.next = body1->m_contactList;
	if (body1->m_contactList != NULL)
	{
		body1->m_contactList->prev = &c->m_node1;
	}
	body1->m_contactList = &c->m_node1;

	// Connect to body 2
	c->m_node2.contact = c;
	c->m_node2.other = body1;

	c->m_node2.prev = NULL;
	c->m_node2.next = body2->m_contactList;
	if (body2->m_contactList != NULL)
	{
		body2->m_contactList->prev = &c->m_node2;
	}
	body2->m_contactList = &c->m_node2;

	++m_world->m_contactCount;
	return c;
}

// This is a callback from the broadphase when two AABB proxies cease
// to overlap. We retire the b2Contact.
void b2ContactManager::PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData)
{
	B2_NOT_USED(proxyUserData1);
	B2_NOT_USED(proxyUserData2);

	if (pairUserData == NULL)
	{
		return;
	}

	b2Contact* c = (b2Contact*)pairUserData;
	if (c == &m_nullContact)
	{
		return;
	}

	// An attached body is being destroyed, we must destroy this contact
	// immediately to avoid orphaned shape pointers.
	Destroy(c);
}

void b2ContactManager::Destroy(b2Contact* c)
{
	b2Shape* shape1 = c->GetShape1();
	b2Shape* shape2 = c->GetShape2();
	b2Body* body1 = shape1->GetBody();
	b2Body* body2 = shape2->GetBody();

	b2ContactPoint cp;
	cp.shape1 = shape1;
	cp.shape2 = shape2;
	cp.friction = b2MixFriction(shape1->GetFriction(), shape2->GetFriction());
	cp.restitution = b2MixRestitution(shape1->GetRestitution(), shape2->GetRestitution());

	// Inform the user that this contact is ending.
	int32 manifoldCount = c->GetManifoldCount();
	if (manifoldCount > 0 && m_world->m_contactListener)
	{
		b2Manifold* manifolds = c->GetManifolds();

		for (int32 i = 0; i < manifoldCount; ++i)
		{
			b2Manifold* manifold = manifolds + i;
			cp.normal = manifold->normal;

			for (int32 j = 0; j < manifold->pointCount; ++j)
			{
				b2ManifoldPoint* mp = manifold->points + j;
				cp.position = body1->GetWorldPoint(mp->localPoint1);
				b2Vec2 v1 = body1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
				b2Vec2 v2 = body2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
				cp.velocity = v2 - v1;
				cp.separation = mp->separation;
				cp.id = mp->id;
				m_world->m_contactListener->Remove(&cp);
			}
		}
	}

	// Remove from the world.
	if (c->m_prev)
	{
		c->m_prev->m_next = c->m_next;
	}

	if (c->m_next)
	{
		c->m_next->m_prev = c->m_prev;
	}

	if (c == m_world->m_contactList)
	{
		m_world->m_contactList = c->m_next;
	}

	// Remove from body 1
	if (c->m_node1.prev)
	{
		c->m_node1.prev->next = c->m_node1.next;
	}

	if (c->m_node1.next)
	{
		c->m_node1.next->prev = c->m_node1.prev;
	}

	if (&c->m_node1 == body1->m_contactList)
	{
		body1->m_contactList = c->m_node1.next;
	}

	// Remove from body 2
	if (c->m_node2.prev)
	{
		c->m_node2.prev->next = c->m_node2.next;
	}

	if (c->m_node2.next)
	{
		c->m_node2.next->prev = c->m_node2.prev;
	}

	if (&c->m_node2 == body2->m_contactList)
	{
		body2->m_contactList = c->m_node2.next;
	}

	// Call the factory.
	b2Contact::Destroy(c, &m_world->m_blockAllocator);
	--m_world->m_contactCount;
}

// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
void b2ContactManager::Collide()
{
	// Update awake contacts.
	for (b2Contact* c = m_world->m_contactList; c; c = c->GetNext())
	{
		b2Body* body1 = c->GetShape1()->GetBody();
		b2Body* body2 = c->GetShape2()->GetBody();
		if (body1->IsSleeping() && body2->IsSleeping())
		{
			continue;
		}

		c->Update(m_world->m_contactListener);
	}
}