//--------------------------------------------------------------------------------------
// Copyright (C) 2008 http://shift.delblanco.es
//--------------------------------------------------------------------------------------
// SHIFT Videogame is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by the
// Free Software Foundation, either version 3 of the License, or later version.
//
// This program is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
//--------------------------------------------------------------------------------------
// 3DCollision.cpp
//--------------------------------------------------------------------------------------
#include "dxstdafx.h"
#include "DXUT.h"

#include "3DCollision.h"
#include "3DObject.h"
#include "3DObjVehicle.h"
#include "d3dx9math.h"

//--------------------------------------------------------------------------------------
// Espacio de nombres
//--------------------------------------------------------------------------------------
using namespace std;

//--------------------------------------------------------------------------------------
// Variables Globales
//--------------------------------------------------------------------------------------
// Estructura de datos para almacenar las colisiones de la escena
vector<C3DCollision *> g_aCollisions;

//--------------------------------------------------------------------------------------
// Constructor
//--------------------------------------------------------------------------------------
C3DCollision::C3DCollision()
{
    m_bInit = false;
    m_pObject = NULL;
	m_bIsVehicle = false;
}

//--------------------------------------------------------------------------------------
// Destructor
//--------------------------------------------------------------------------------------
C3DCollision::~C3DCollision()
{
    Release();
}

//--------------------------------------------------------------------------------------
// Inicializa el área de colisión
//--------------------------------------------------------------------------------------
bool C3DCollision::initialize(C3DObject *Object, float dmass)
{
    // Salimos si ya se ha inicializado
    if(m_bInit) return false;
    m_bInit = true;

    // Almacenamos el objeto asociado
    m_pObject = Object;
	m_mass = dmass;

    // Registramos el nuevo área de colisión
    g_aCollisions.push_back(this);

    return true;
}

//--------------------------------------------------------------------------------------
// Comprueba las colisiones del área de colisión con el resto de la escena
//--------------------------------------------------------------------------------------
void C3DCollision::update()
{
    // Salimos si no se ha inicializado
    if(!m_bInit) return;

    // Si el objeto es de tipo móvil,
	// entonces comprobamos colisiones con todos los objetos colisionables
	if (m_pObject->getTypeObject() == OBJECT_TYPE_MOBILE)
	{
		for (std::vector<C3DCollision *>::iterator itr = g_aCollisions.begin() ; itr != g_aCollisions.end() ; itr++)
		{
			// Si pasa los casos triviales y hay colisión de círculos
			// Evaluamos colisión de rectángulos (recolocando e intercambiando parámetros si hay colisión)
			if (CheckBasicCollision(*itr) && CheckQuickCollision(*itr))
				CheckFullCollision(*itr, true, true);
		}
	}
}

//--------------------------------------------------------------------------------------
// Libera el área de colisión
//--------------------------------------------------------------------------------------
void C3DCollision::Release()
{
    // Salimos si no se ha inicializado
    if(!m_bInit) return;
    m_bInit = false;

    // Des-registramos el área de colisión
    for (std::vector<C3DCollision *>::iterator itr = g_aCollisions.begin() ; itr != g_aCollisions.end() ; itr++)
    {
        if(*(itr) == this)
        {
            g_aCollisions.erase(itr);
            break;
        }
    }
}

//--------------------------------------------------------------------------------------
// Establece los cuatro vértices que determinan el área de colisión 2D (ejes X,Z) a partir de un vector dado
//--------------------------------------------------------------------------------------
void C3DCollision::setV(D3DXVECTOR4 *dv, bool IsVehicle)
{
	// Tomamos los puntos de colisión
	m_v[0] = dv[0];
	m_v[1] = dv[1];
	m_v[2] = dv[2];
	m_v[3] = dv[3];

	// Tomamos si es vehículo
	m_bIsVehicle = IsVehicle;

	// Tomamos el mayor radio aplicando antes el escalado del objeto
	D3DXVECTOR3 Scale;
	m_pObject->getScale(Scale);
	dv[0].x = dv[0].x * Scale.x; dv[0].z = dv[0].z * Scale.z;
	dv[1].x = dv[1].x * Scale.x; dv[1].z = dv[1].z * Scale.z;
	dv[2].x = dv[2].x * Scale.x; dv[2].z = dv[2].z * Scale.z;
	dv[3].x = dv[3].x * Scale.x; dv[3].z = dv[3].z * Scale.z;
	float radius_0 = sqrt((dv[0].x * dv[0].x) + (dv[0].z * dv[0].z));
	float radius_1 = sqrt((dv[1].x * dv[1].x) + (dv[1].z * dv[1].z));
	float radius_2 = sqrt((dv[2].x * dv[2].x) + (dv[2].z * dv[2].z));
	float radius_3 = sqrt((dv[3].x * dv[3].x) + (dv[3].z * dv[3].z));
	m_radius = max(radius_0, max(radius_1, max(radius_2, radius_3)));

	// Sobre el área escado calculamos el momento de inercia del objeto
	float Xmin = min(dv[0].x, min(dv[1].x, min(dv[2].x, dv[3].x)));
	float Xmax = max(dv[0].x, max(dv[1].x, max(dv[2].x, dv[3].x)));
	float Zmin = min(dv[0].z, min(dv[1].z, min(dv[2].z, dv[3].z)));
	float Zmax = max(dv[0].z, max(dv[1].z, max(dv[2].z, dv[3].z)));
	float width = Xmax - Xmin;
	float height = Zmax - Zmin;
	m_momentum = m_mass * (width*width + height*height) / 12.0f;
}

//--------------------------------------------------------------------------------------
// Establece los cuatro vértices que determinan el área de colisión 2D (ejes X,Z) a partir de una geometría
//--------------------------------------------------------------------------------------
void C3DCollision::setV(LPD3DXMESH pMesh, bool IsVehicle)
{
	D3DXVECTOR4 v[4];
	v[0] = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f);
	v[1] = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f);
	v[2] = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f);
	v[3] = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f);
	if (!pMesh) return;

	BYTE* pVertices = NULL;
	float Xmax = -FLT_MAX;	float Xmin = FLT_MAX;
	float Zmax = -FLT_MAX;	float Zmin = FLT_MAX;
	// Bloqueamos el buffer
	if (SUCCEEDED(pMesh->LockVertexBuffer(D3DLOCK_READONLY, (LPVOID*)&pVertices))) {
		// Tomamos el número de vértices
		unsigned numVerts = pMesh->GetNumVertices();
		// Tomamos el tamaño de vértice
		DWORD pSize = pMesh->GetNumBytesPerVertex()*sizeof(BYTE);
		// Recorremos el buffer
		for (unsigned i=0 ; i < numVerts ; i++)
		{
			// Tomamos la X (primer valor real) y la Z (tercer valor real)
			float X = *((float *)(pVertices + i*pSize + 0*sizeof(float)));
			float Z = *((float *)(pVertices + i*pSize + 2*sizeof(float)));
			// Actualizamos variables
			if (X > Xmax) Xmax = X; if (X < Xmin) Xmin = X;
			if (Z > Zmax) Zmax = Z; if (Z < Zmin) Zmin = Z;
		}
		// Desbloqueamos el buffer
		pMesh->UnlockVertexBuffer();
		// Tomamos el área de colisión rectangular
		v[0].x = Xmin;	v[0].z = Zmax;
		v[1].x = Xmax;	v[1].z = Zmax;
		v[2].x = Xmin;	v[2].z = Zmin;
		v[3].x = Xmax;	v[3].z = Zmin;
	}

	// Llamamos a la función sobrecargada
	setV((D3DXVECTOR4 *)v, IsVehicle);
}

//--------------------------------------------------------------------------------------
// Función para comprobar condiciones de colisión triviales (es un método directo)
//--------------------------------------------------------------------------------------
bool C3DCollision::CheckBasicCollision(C3DCollision *Collision)
{
	/*
		--------------------------------------------------------------------------------------
		TAREA 1
		--------------------------------------------------------------------------------------
		Evaluamos condiciones triviales de colisión entre THIS (1) y COLLISION (2)
		--------------------------------------------------------------------------------------
	*/

	/*
		No hay colisión si:
		--------------------------------------------------------------------------------------
		Son el mismo objeto
		Collision está desactivado
		Ninguno ha sido modificado tras la última iteración
	*/
	return
		(this != Collision) &&
		(Collision->m_pObject->getEnabled() == true) &&
		(this->m_pObject->getLastModified() || Collision->m_pObject->getLastModified());
}

//--------------------------------------------------------------------------------------
// Función para comprobar colisiones mediante esferas (es un método rápido pero aproximado)
//--------------------------------------------------------------------------------------
bool C3DCollision::CheckQuickCollision(C3DCollision *Collision)
{
	/*
		--------------------------------------------------------------------------------------
		TAREA 2
		--------------------------------------------------------------------------------------
		Evaluamos colisión de círculos entre THIS (1) y COLLISION (2)
		--------------------------------------------------------------------------------------
	*/

	/*
		Recopilamos variables
		--------------------------------------------------------------------------------------
	*/

	// Tomamos los centros de cada objeto en SRU
	D3DXVECTOR4 aux4;
	aux4 = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f); D3DXVec4Transform(&aux4, &aux4, this->m_pObject->getTransformation());
	D3DXVECTOR3 o1 = D3DXVECTOR3(aux4.x, 0.0f, aux4.z);
	aux4 = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f); D3DXVec4Transform(&aux4, &aux4, Collision->m_pObject->getTransformation());
	D3DXVECTOR3 o2 = D3DXVECTOR3(aux4.x, 0.0f, aux4.z);

	// Hayamos la distancia que separa los dos centros de los objetos en SRU
	D3DXVECTOR3 R = o2 - o1;

	// Si no hubo colisión, retornamos
	if (D3DXVec3Length(&R) > (this->m_radius + Collision->m_radius)) return false;

	// Si hubo colisión, hacemos llamada a los manejadores y retornamos si alguno no desea continuar
	if (this->m_pObject->onQuickCollision(Collision->m_pObject) == false ||
		Collision->m_pObject->onQuickCollision(this->m_pObject) == false)
		return false;

	// Retornamos
	return true;
}

//--------------------------------------------------------------------------------------
// Función para comprobar colisiones mediante rectángulos (es un método lento pero más detallado)
//--------------------------------------------------------------------------------------
bool C3DCollision::CheckFullCollision(C3DCollision *Collision, bool ReplaceElements, bool ExchangePhysics)
{
	/*
		--------------------------------------------------------------------------------------
		TAREA 3
		--------------------------------------------------------------------------------------
		Evaluamos colisión de rectángulos entre THIS (1) y COLLISION (2)
		--------------------------------------------------------------------------------------
	*/

	/*
		Recopilamos variables
		--------------------------------------------------------------------------------------
	*/

	// Variables auxiliares
	bool		hay_colision = false;					// Flag de colisión
	float		Xmax1,	Xmin1,	Zmax1,	Zmin1;			// Rangos en X e Y del área de colisión del objeto (1) en su SRO
	float		Xmax2,	Xmin2,	Zmax2,	Zmin2;			// Rangos en X e Y del área de colisión del objeto (2) en su SRO
	unsigned	type_coll;								// Indica el SRO en el que se detectó la colisión (COLLISION_TYPE_SRO1, COLLISION_TYPE_SRO2)
	unsigned	point_coll;								// Identifica el punto del área de colisión dentro del otro objeto (1, 2, 3, 4)
	D3DXMATRIX	Transformation_XtoY;					// Matriz auxiliar para aplicar la tranforación SRO(X) --> SRU --> SRO(Y)

	// Objetos
	C3DObject *obj_1 = this->m_pObject;					// Objeto (1)
	C3DObject *obj_2 = Collision->m_pObject;			// Objeto (2)

	// Áreas de colisión
	D3DXVECTOR4 *coll_1 = this->m_v;					// Área de colisión del objeto (1) respecto su SRO
	D3DXVECTOR4 *coll_2 = Collision->m_v;				// Área de colisión del objeto (2) respecto su SRO
	D3DXVECTOR4	coll_1r2[4];							// Área de colisión del objeto (1) respecto SRO(2)
	D3DXVECTOR4	coll_2r1[4];							// Área de colisión del objeto (2) respecto SRO(1)

	// Matrices de tranformación
	D3DXMATRIX *trans_1 = obj_1->getTransformation();	// Matriz de tranformación del objeto (1)
	D3DXMATRIX *trans_2 = obj_2->getTransformation();	// Matriz de tranformación del objeto (2)
	D3DXMATRIX *transInv_1 = &(this->m_transInv);		// Matriz inversa de tranformación del objeto (1)
	D3DXMATRIX *transInv_2 = &(Collision->m_transInv);	// Matriz inversa de tranformación del objeto (2)

	/*
		Comprobación de colisión entre los objetos (1) y (2)
		--------------------------------------------------------------------------------------
	*/

	// Comprobamos colisiones bajo SRO(1)

	// Hayamos la matriz de transformación SRO(2) --> SRU --> SRO(1)
	D3DXMatrixMultiply(&Transformation_XtoY, trans_2, transInv_1);

	// Transformamos cada punto de (2) a SRO(2) --> SRU --> SRO(1)
	D3DXVec4Transform(&(coll_2r1[0]), &(coll_2[0]), &Transformation_XtoY);
	D3DXVec4Transform(&(coll_2r1[1]), &(coll_2[1]), &Transformation_XtoY);
	D3DXVec4Transform(&(coll_2r1[2]), &(coll_2[2]), &Transformation_XtoY);
	D3DXVec4Transform(&(coll_2r1[3]), &(coll_2[3]), &Transformation_XtoY);

	// Hayamos los rangos en X e Y del área de colisión del objeto (1) centrado en su SRO
	Xmin1 = min(coll_1[0].x, min(coll_1[1].x, min(coll_1[2].x, coll_1[3].x)));
	Xmax1 = max(coll_1[0].x, max(coll_1[1].x, max(coll_1[2].x, coll_1[3].x)));
	Zmin1 = min(coll_1[0].z, min(coll_1[1].z, min(coll_1[2].z, coll_1[3].z)));
	Zmax1 = max(coll_1[0].z, max(coll_1[1].z, max(coll_1[2].z, coll_1[3].z)));

	// Por cada punto de (2)
	for (unsigned i=0 ; i<4 ; i++)
	{
		// Comprobamos si el punto de (2) está en el área de colisión de (1)
		if	(	Xmin1 < coll_2r1[i].x && coll_2r1[i].x < Xmax1 &&
				Zmin1 < coll_2r1[i].z && coll_2r1[i].z < Zmax1	)
		{
			hay_colision = true;
			type_coll = COLLISION_TYPE_SRO1;
			point_coll = i;
			break;
		}
	}

	// Si no hubo colisión bajo SRO(1) ...
	if (!hay_colision)
	{
		// Si el objeto (2) es de tipo móvil,
		// entonces hemos terminado, ya que este objeto comprobará colisiones respecto su SRO
//		if (obj_2->getTypeObject() == OBJECT_TYPE_MOBILE) return false;

		// En otro caso es necesario comprobar colisiones bajo SRO(2)

		// Hayamos la matriz de transformación SRO(1) --> SRU --> SRO(2)
		D3DXMatrixMultiply(&Transformation_XtoY, trans_1, transInv_2);

		// Transformamos cada punto de (1) a SRO(1) --> SRU --> SRO(2)
		D3DXVec4Transform(&(coll_1r2[0]), &(coll_1[0]), &Transformation_XtoY);
		D3DXVec4Transform(&(coll_1r2[1]), &(coll_1[1]), &Transformation_XtoY);
		D3DXVec4Transform(&(coll_1r2[2]), &(coll_1[2]), &Transformation_XtoY);
		D3DXVec4Transform(&(coll_1r2[3]), &(coll_1[3]), &Transformation_XtoY);

		// Hayamos los rangos en X e Y del área de colisión del objeto (2) centrado en su SRO
		Xmin2 = min(coll_2[0].x, min(coll_2[1].x, min(coll_2[2].x, coll_2[3].x)));
		Xmax2 = max(coll_2[0].x, max(coll_2[1].x, max(coll_2[2].x, coll_2[3].x)));
		Zmin2 = min(coll_2[0].z, min(coll_2[1].z, min(coll_2[2].z, coll_2[3].z)));
		Zmax2 = max(coll_2[0].z, max(coll_2[1].z, max(coll_2[2].z, coll_2[3].z)));

		// Por cada punto de (1)
		for (unsigned i=0 ; i<4 ; i++)
		{
			// Comprobamos si el punto de (1) está en el área de colisión de (2)
			if	(	Xmin2 < coll_1r2[i].x && coll_1r2[i].x < Xmax2 &&
					Zmin2 < coll_1r2[i].z && coll_1r2[i].z < Zmax2	)
			{
				hay_colision = true;
				type_coll = COLLISION_TYPE_SRO2;
				point_coll = i;
				break;
			}
		}
	}

	// Si no hubo colisión bajo SRO(1) ni bajo SRO(2), ya hemos terminado
	if (!hay_colision) return false;

	// Si hubo colisión, hacemos llamada a los manejadores y retornamos si alguno no desea continuar
	if (this->m_pObject->onFullCollision(Collision->m_pObject) == false ||
		Collision->m_pObject->onFullCollision(this->m_pObject) == false)
		return false;

	/*
		--------------------------------------------------------------------------------------
		TAREA 4
		--------------------------------------------------------------------------------------
		Recolocamos el objeto THIS (1) para que ambos dejen de estar solapados
		Siempre moveremos el objeto (1), ya que es el objeto móvil
		Hemos detectado la colisión en el SRO (X) y en cada caso mover (1) será diferente
		--------------------------------------------------------------------------------------
	*/

	/*
		Recopilamos variables
		--------------------------------------------------------------------------------------
	*/

	// Variables auxiliares
	float		Xmax2r1,Xmin2r1,Zmax2r1,Zmin2r1;		// Rangos en X e Y del área de colisión del objeto (2) en el SRO(1)
	float		Xmax1r2,Xmin1r2,Zmax1r2,Zmin1r2;		// Rangos en X e Y del área de colisión del objeto (1) en el SRO(2)
	float		Xneg,	Xpos,	Zneg,	Zpos;			// Rangos en la parte positiva y negativa de X e Y del área de colisión de (X)
	float		Xin,	Zin;							// Cuanto está solapado el objeto que mete la esquina por X y por Y
	float		movX = 0.0f;							// Cuanto habría que mover el objeto (1) si lo movemos en X
	float		movZ = 0.0f;							// Cuanto habría que mover el objeto (1) si lo movemos en Z
	D3DXVECTOR4 mov;									// Movimiento final que aplicaremos
	D3DXVECTOR3 scale, rotation;						// Escalado y rotación del objeto centrado en su SRO
	D3DXMATRIX	Transformation_aux;						// Tranformación auxiliar para tomar escalado y rotación anteriores
	D3DXMATRIX	Transformation_move1;					// Tranformación que aplicaremos al objeto (1) para evitar solapamiento
	D3DXVECTOR3 P;										// Punto exacto de la colisión en el SRU
	D3DXVECTOR3 n;										// Vector ortogonal al lado de choque apuntando hacia afuera del objeto

	// Físicas
	C3DPhysics *physic_1 = NULL;						// Física del objeto (1)
	if (obj_1->hasPhysics())
		physic_1 = obj_1->getPhysics();
	C3DPhysics *physic_2 = NULL;						// Física del objeto (2)
	if (obj_2->hasPhysics())
		physic_2 = obj_2->getPhysics();

	/*
		Sólo si realmente hay que hacerlo
		--------------------------------------------------------------------------------------
	*/
	if (ReplaceElements || ExchangePhysics)
	{

		/*
			Recolocación del objeto (1) si se detectó en el SRO(1)
			Significa que (2) mete en (1) la esquina (point_coll)
			--------------------------------------------------------------------------------------
		*/
		if (type_coll == COLLISION_TYPE_SRO1)
		{
			// Tomamos los rangos de las proyecciones de (2) respecto SRO(1)
			Xmin2r1 = min(coll_2r1[0].x, min(coll_2r1[1].x, min(coll_2r1[2].x, coll_2r1[3].x)));
			Xmax2r1 = max(coll_2r1[0].x, max(coll_2r1[1].x, max(coll_2r1[2].x, coll_2r1[3].x)));
			Zmin2r1 = min(coll_2r1[0].z, min(coll_2r1[1].z, min(coll_2r1[2].z, coll_2r1[3].z)));
			Zmax2r1 = max(coll_2r1[0].z, max(coll_2r1[1].z, max(coll_2r1[2].z, coll_2r1[3].z)));

			// Averiguamos por dónde entra el objeto (2) en X y en Z (por la positiva o negativa)
			Xneg = max(0.0f, Xmin1 - Xmin2r1);
			Xpos = max(0.0f, Xmax2r1 - Xmax1);
			Zneg = max(0.0f, Zmin1 - Zmin2r1);
			Zpos = max(0.0f, Zmax2r1 - Zmax1);

			// Averiguamos cuanto está solapado (2) en (1) en X y Z
			if (Xneg > Xpos)	Xin = Xmax2r1 - Xmin1;
			else				Xin = Xmax1 - Xmin2r1;
			if (Zneg > Zpos)	Zin = Zmax2r1 - Zmin1;
			else				Zin = Zmax1 - Zmin2r1;

			// Averiguamos lo que habría que mover (1) en X o Z para evitar el solapamiento
			if (Xneg > Xpos)	movX = coll_2r1[point_coll].x - Xmin1;
			else				movX = Xmax1 - coll_2r1[point_coll].x;
			if (Zneg > Zpos)	movZ = coll_2r1[point_coll].z - Zmin1;
			else				movZ = Zmax1 - coll_2r1[point_coll].z;

			// Movemos (1) en el eje dónde tengamos menor solapamiento (X o Z)
			// Además moveremos en el sentido contrario por dónde esté colocado (2)
			if (Xin < Zin)
			{
				movZ = 0.0f;
				if (Xneg < Xpos)	movX = - movX - COLLISION_EPSILON;
				else				movX = + movX + COLLISION_EPSILON;
			}
			else
			{
				movX = 0.0f;
				if (Zneg < Zpos)	movZ = - movZ - COLLISION_EPSILON;
				else				movZ = + movZ + COLLISION_EPSILON;
			}

			// Averiguamos el vector normalizado de choque en SRU
			D3DXVECTOR4 n_SROa (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 n_SROb (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 n_SRUa, n_SRUb, n_SRU;
			if (Xin < Zin)
			{
				if (Xneg < Xpos)	n_SROa.x = +1.0f;
				else				n_SROa.x = -1.0f;
			}
			else
			{
				if (Zneg < Zpos)	n_SROa.z = +1.0f;
				else				n_SROa.z = -1.0f;
			}
			D3DXVec4Transform(&n_SRUa, &n_SROa, trans_1);
			D3DXVec4Transform(&n_SRUb, &n_SROb, trans_1);
			n_SRU = n_SRUa - n_SRUb;
			D3DXVec4Normalize(&n_SRU, &n_SRU);
			n = D3DXVECTOR3(n_SRU.x, 0.0f, n_SRU.z);

			// Tenemos que aplicar a (1) una traslación opuesta e igual a la parte que tiene metida
			// Pero antes hay que aplicar a este movimiento el escalado y rotación del objeto (1)
			mov = D3DXVECTOR4(movX, 0.0f, movZ, 1.0f);
			D3DXMatrixIdentity(&Transformation_move1);
			obj_1->getScale(scale);
			D3DXMatrixScaling(&Transformation_aux, scale.x, scale.y, scale.z);
			D3DXMatrixMultiply(&Transformation_move1, &Transformation_move1, &Transformation_aux);
			obj_1->getRotation(rotation);
			D3DXMatrixRotationYawPitchRoll(&Transformation_aux, rotation.y, rotation.x, rotation.z);
			D3DXMatrixMultiply(&Transformation_move1, &Transformation_move1, &Transformation_aux);
			D3DXVec4Transform(&mov, &mov, &Transformation_move1);
			obj_1->addPosition(D3DXVECTOR3(mov.x, 0.0f, mov.z));

			// Tomo el punto exacto de colisión
			D3DXVECTOR4 P_SRO (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 P_SRU;
			P_SRO.x = coll_2r1[point_coll].x;
			P_SRO.z = coll_2r1[point_coll].z;
			D3DXVec4Transform(&P_SRU, &P_SRO, trans_1);
			P = D3DXVECTOR3(P_SRU.x, 0.0f, P_SRU.z);
		}

		/*
			Recolocación del objeto (1) si se detectó en el SRO(2)
			Significa que (1) mete en (2) la esquina (point_coll)
			--------------------------------------------------------------------------------------
		*/
		else
		{
			// Tomamos los rangos de las proyecciones de (1) respecto SRO(2)
			Xmin1r2 = min(coll_1r2[0].x, min(coll_1r2[1].x, min(coll_1r2[2].x, coll_1r2[3].x)));
			Xmax1r2 = max(coll_1r2[0].x, max(coll_1r2[1].x, max(coll_1r2[2].x, coll_1r2[3].x)));
			Zmin1r2 = min(coll_1r2[0].z, min(coll_1r2[1].z, min(coll_1r2[2].z, coll_1r2[3].z)));
			Zmax1r2 = max(coll_1r2[0].z, max(coll_1r2[1].z, max(coll_1r2[2].z, coll_1r2[3].z)));

			// Averiguamos por dónde entra el objeto (1) en X y en Z (por la positiva o negativa)
			Xneg = max(0.0f, Xmin2 - Xmin1r2);
			Xpos = max(0.0f, Xmax1r2 - Xmax2);
			Zneg = max(0.0f, Zmin2 - Zmin1r2);
			Zpos = max(0.0f, Zmax1r2 - Zmax2);

			// Averiguamos cuanto está solapado (1) en (2) en X y Z
			if (Xneg > Xpos)	Xin = Xmax1r2 - Xmin2;
			else				Xin = Xmax2 - Xmin1r2;
			if (Zneg > Zpos)	Zin = Zmax1r2 - Zmin2;
			else				Zin = Zmax2 - Zmin1r2;

			// Averiguamos lo que habría que mover (2) en X o Z para evitar el solapamiento
			// Si embargo moveremos (1) en sentido contrario, ya que es el objeto móvil
			if (Xneg > Xpos)	movX = coll_1r2[point_coll].x - Xmin2;
			else				movX = Xmax2 - coll_1r2[point_coll].x;
			if (Zneg > Zpos)	movZ = coll_1r2[point_coll].z - Zmin2;
			else				movZ = Zmax2 - coll_1r2[point_coll].z;

			// Movemos (1) en el eje dónde tengamos menor solapamiento (X o Z)
			// Además moveremos en el sentido contrario por dónde esté colocado (2)
			if (Xin < Zin)
			{
				movZ = 0.0f;
				if (Xneg < Xpos)	movX = - movX - COLLISION_EPSILON;
				else				movX = + movX + COLLISION_EPSILON;
			}
			else
			{
				movX = 0.0f;
				if (Zneg < Zpos)	movZ = - movZ - COLLISION_EPSILON;
				else				movZ = + movZ + COLLISION_EPSILON;
			}
			movX = -movX;
			movZ = -movZ;

			// Averiguamos el vector normalizado de choque en SRU
			D3DXVECTOR4 n_SROa (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 n_SROb (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 n_SRUa, n_SRUb, n_SRU;
			if (Xin < Zin)
			{
				if (Xneg < Xpos)	n_SROa.x = +1.0f;
				else				n_SROa.x = -1.0f;
			}
			else
			{
				if (Zneg < Zpos)	n_SROa.z = +1.0f;
				else				n_SROa.z = -1.0f;
			}
			D3DXVec4Transform(&n_SRUa, &n_SROa, trans_2);
			D3DXVec4Transform(&n_SRUb, &n_SROb, trans_2);
			n_SRU = n_SRUa - n_SRUb;
			D3DXVec4Normalize(&n_SRU, &n_SRU);
			n = D3DXVECTOR3(n_SRU.x, 0.0f, n_SRU.z);

			// Tenemos que aplicar a (1) una traslación opuesta e igual a la parte que ha metido
			// Pero antes hay que aplicar a este movimiento el escalado y rotación del objeto (2)
			mov = D3DXVECTOR4(movX, 0.0f, movZ, 1.0f);
			D3DXMatrixIdentity(&Transformation_move1);
			obj_2->getScale(scale);
			D3DXMatrixScaling(&Transformation_aux, scale.x, scale.y, scale.z);
			D3DXMatrixMultiply(&Transformation_move1, &Transformation_move1, &Transformation_aux);
			obj_2->getRotation(rotation);
			D3DXMatrixRotationYawPitchRoll(&Transformation_aux, rotation.y, rotation.x, rotation.z);
			D3DXMatrixMultiply(&Transformation_move1, &Transformation_move1, &Transformation_aux);
			D3DXVec4Transform(&mov, &mov, &Transformation_move1);
			obj_1->addPosition(D3DXVECTOR3(mov.x, 0.0f, mov.z));

			// Tomo el punto exacto de colisión
			D3DXVECTOR4 P_SRO (0.0f, 0.0f, 0.0f, 1.0f);
			D3DXVECTOR4 P_SRU;
			P_SRO.x = coll_1r2[point_coll].x;
			P_SRO.z = coll_1r2[point_coll].z;
			D3DXVec4Transform(&P_SRU, &P_SRO, trans_2);
			P = D3DXVECTOR3(P_SRU.x, 0.0f, P_SRU.z);
		}

	}

	/*
		--------------------------------------------------------------------------------------
		TAREA 5
		--------------------------------------------------------------------------------------
		Realizamos el intercambio de parámetros físicos entre THIS (1) y COLLISION (2)
		--------------------------------------------------------------------------------------
	*/

	/*
		Sólo si realmente hay que hacerlo
		--------------------------------------------------------------------------------------
	*/
	if (ExchangePhysics)
	{
		/*
			Calculamos variables pre-colisión
			--------------------------------------------------------------------------------------
		*/
		// Parámetros auxiliares
		D3DXVECTOR3 aux3;
		D3DXVECTOR4 aux4;

		// Tomamos los centros de cada objeto en SRU
		aux4 = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f); D3DXVec4Transform(&aux4, &aux4, trans_1);
		D3DXVECTOR3 o_a = D3DXVECTOR3(aux4.x, 0.0f, aux4.z);
		aux4 = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 1.0f); D3DXVec4Transform(&aux4, &aux4, trans_2);
		D3DXVECTOR3 o_b = D3DXVECTOR3(aux4.x, 0.0f, aux4.z);

		// Tomamos los vectores del radio que une el centro de cada objeto con el punto de colisión en SRU
		D3DXVECTOR3 r_ap = P - o_a; r_ap.y = 0.0f;
		D3DXVECTOR3 r_bp = P - o_b; r_bp.y = 0.0f;

		// Tomamos si son vehículos
		bool IsVehicle1 = this->m_bIsVehicle;
		bool IsVehicle2 = Collision->m_bIsVehicle;

		// Tomamos las velocidades lineales de cada objeto
		D3DXVECTOR3 v_a1 (0.0f, 0.0f, 0.0f);
		if (IsVehicle1) v_a1 = *(physic_1->getLDirection()) * physic_1->getLSpeed() +
							   *(static_cast<C3DPhyVehicle *>(physic_1)->getAditionalLDirection()) * static_cast<C3DPhyVehicle *>(physic_1)->getAditionalLSpeed();
		else if (physic_1) v_a1 = *(physic_1->getLDirection()) * physic_1->getLSpeed();

		D3DXVECTOR3 v_b1 (0.0f, 0.0f, 0.0f);
		if (IsVehicle2)	v_b1 = *(physic_2->getLDirection()) * physic_2->getLSpeed() +
							   *(static_cast<C3DPhyVehicle *>(physic_2)->getAditionalLDirection()) * static_cast<C3DPhyVehicle *>(physic_2)->getAditionalLSpeed();
		else if (physic_2) v_b1 = *(physic_2->getLDirection()) * physic_2->getLSpeed();

		// Tomamos las velocidades angulares de cada objeto
		float w_a1 = 0.0f;
		if (physic_1) { physic_1->getASpeed(aux3); w_a1 = aux3.y; }
		float w_b1 = 0.0f;
		if (physic_2) { physic_2->getASpeed(aux3); w_b1 = aux3.y; }
		D3DXVECTOR3 vector_w_a1 = D3DXVECTOR3(0.0f, w_a1, 0.0f);
		D3DXVECTOR3 vector_w_b1 = D3DXVECTOR3(0.0f, w_b1, 0.0f);

		// Tratamos si el primer objeto es un vehículo atrapado entre dos fijos
		bool locked = false;
		if( v_a1.x == 0.0f && v_a1.z == 0.0f &&
			v_b1.x == 0.0f && v_b1.z == 0.0f &&
			w_b1 == 0.0f && IsVehicle1 )
		{
			locked = true;
			if (D3DXVec3Dot(physic_1->getLDirection(), &r_ap) > 0.0f)
				v_a1 = *(physic_1->getLDirection()) * +5.0f;
			else
				v_a1 = *(physic_1->getLDirection()) * -5.0f;
		}

		// Tomamos las masas
		float m_a = this->m_mass;
		float m_b = Collision->m_mass;

		// Tomamos los momentos de inercia
		float I_a = this->m_momentum;
		float I_b = Collision->m_momentum;

		/*
			Calculamos variables intermedias
			--------------------------------------------------------------------------------------
		*/
		// Calculamos v_ab1
		D3DXVECTOR3 cross_w_a1 (w_a1 * r_ap.z, 0.0f, - w_a1 * r_ap.x);
		D3DXVECTOR3 cross_w_b1 (w_b1 * r_bp.z, 0.0f, - w_b1 * r_bp.x);
		D3DXVECTOR3 v_ab1 = v_a1 + cross_w_a1 - v_b1 - cross_w_b1;

		// Calculamos el impulso ante el caso [1-Móvil, 2-Fijo y sin física]
		float impulse;
		if (obj_2->getTypeObject() == OBJECT_TYPE_FIXED)
		{
			D3DXVECTOR3 cross_a; D3DXVec3Cross(&cross_a, &r_ap, &n);
			impulse = (-(1.0f + COLLISION_E_MF) * D3DXVec3Dot(&v_ab1, &n)) / ((1.0f / m_a) + D3DXVec3Dot(&cross_a, &cross_a) / I_a);
		}

		// Calculamos el impulso ante los casos [1-Móvil, 2-Móvil] y [1-Móvil, 2-Fijo y con física]
		else
		{
			D3DXVECTOR3 cross_a; D3DXVec3Cross(&cross_a, &r_ap, &n);
			D3DXVECTOR3 cross_b; D3DXVec3Cross(&cross_b, &r_bp, &n);
			impulse = (-(1.0f + COLLISION_E_MM) * D3DXVec3Dot(&v_ab1, &n)) / ((1.0f / m_a) + D3DXVec3Dot(&cross_a, &cross_a) / I_a + (1.0f / m_b) + D3DXVec3Dot(&cross_b, &cross_b) / I_b);
		}

		/*
			Calculamos variables post-colisión
			--------------------------------------------------------------------------------------
		*/
		aux3 = impulse * n;
		D3DXVECTOR3 v_a2 = v_a1 + aux3 / m_a;
		D3DXVECTOR3 v_b2 = v_b1 - aux3 / m_b;

		aux3 = impulse * n;
		D3DXVECTOR3 cross_w_a2; D3DXVec3Cross(&cross_w_a2, &r_ap, &aux3);
		D3DXVECTOR3 cross_w_b2; D3DXVec3Cross(&cross_w_b2, &r_bp, &aux3);
		D3DXVECTOR3 vector_w_a2 = vector_w_a1 + cross_w_a2 / I_a;
		D3DXVECTOR3 vector_w_b2 = vector_w_b1 - cross_w_b2 / I_b;
		float w_a2 = vector_w_a2.y;
		float w_b2 = vector_w_b2.y;

		/*
			Actualizamos las variables post-colisión
			--------------------------------------------------------------------------------------
		*/
		// Sólo actualizamos el objeto 1 si es móvil
		if (obj_1->getTypeObject() == OBJECT_TYPE_MOBILE)
		{
			// Sólo se actualiza la lineal del primer objeto si no es el caso
			// de un vehículo no se encuentra atrapado entre dos fijos
			if (!locked)
			{
				if (IsVehicle1) static_cast<C3DPhyVehicle *>(physic_1)->setAditionalL(v_a2);
				else physic_1->setLDirection(v_a2);
			}
			physic_1->getASpeed(aux3); aux3.y = w_a2; physic_1->setASpeed(aux3);
		}

		// Sólo actualizamos el objeto 2 si es móvil
		if (obj_2->getTypeObject() == OBJECT_TYPE_MOBILE)
		{
			if (IsVehicle2) static_cast<C3DPhyVehicle *>(physic_2)->setAditionalL(v_b2);
			else physic_2->setLDirection(v_b2);
			physic_2->getASpeed(aux3); aux3.y = w_b2; physic_2->setASpeed(aux3);
		}

	}

	// Retornamos
	return true;
}
