[C++] Aiuto per implementare Separating Axis Theorem per un cubo

[-Ivan-]

Avrei bisogno di implementare questo algoritmo e su internet ho trovato diverse risorse al riguardo. Purtroppo tutti gli esempi di codice danno per scontato qualcosa (che non conosco) e non sono riuscito ancora ad implementarlo.

L'implementazione che sto provando si basa su queste 3 funzioni. Sto testando il programma su un cubo a cui non applico nessuna rotazione per semplicità.

Questa è la funzione principale che viene richiamata per eseguire il controllo tra due primitive (due cubi).

static bool CheckCollision(Primitive *A, Primitive *B)
{
		//
		Vector3D offset = B->Position() - A->Position();
		//
		float xLength = offset.Length();

		//
		Vector3D xVel = B->Velocity() - A->Velocity();
		//
		float depth[12];
		//
		Vector3D axis[12];

		int i = 0, j = 0;
		float t = 1.0f;

		/* ***************************************************************** *
		* Check between bounds to see if there is a separation axis.
		* If a separation line exists, return, as no collision can exist.
		* ****************************************************************** */
		
		for( i = 0; i < 6; i++ )
		{
			if( !intersectAt( *A, *B, A->Coords().normals[i], offset, xVel, t ) )
				return false;

			axis[j] = A->Coords().normals[i];
			depth[j] = t;
			j++;
		}

		for( i = 0; i < 6; i++ )
		{
			if( !intersectAt( *A, *B, B->Coords().normals[i], offset, xVel, t ) )
				return false;

			axis[j] = B->Coords().normals[i];
			depth[j] = t;
			j++;
		}

		return true;
}

static bool intersectAt(Primitive& A, Primitive& B, Vector3D N, const Vector3D& offset, const Vector3D& xVel, float& t)
{
		float min0, max0;	//define the interval for the first shape
		float min1, max1;	//define the interval for the second shape

		//project the bounds of the primitive onto the normal N and store the intervals
		GetInterval(A, N, min0, max0);
		GetInterval(B, N, min1, max1);

		//h helps to translate the primitives into each others local space
		float h = offset.Dot(N);
		min0 += h;
		max0 += h;

		float d0 = min0 - max1;	//if overlapped, d0 < 0
		float d1 = min1 - max0;	//if overlapped, d1 > 0

		//separated, test dynamic intervals
		float v;
		float t0, t1;
		if( d0 > 0.0f || d1 > 0.0f )
		{
			v = xVel.Dot(N);

			//small velocity, so only the overlap test will be relevant
			if( fabs(v) < 0.0000001f )
				return false;

			t0 = -d0 / v;	//time of impact to d0 reaches 0
			t1 = d1 / v;	//time of impact to d0 reaches 1

			if( t0 > t1 )
			{
				std::swap(t0, t1);
			}

			( t0 > 0.0f ) ? t = t0 : t = t1;

			if( t < 0.0f || t > 1.0f )
				return false;

			return true;
		}
		else
		{
			//overlap, get the interval, as the smallest of |d0| ad |d1|
			//return negative number to mark it as an overlap
			( d0 > d1 ) ? t = d0 : t = d1;

			return true;
		}

		return false;
}

static void GetInterval(Primitive& primitive, Vector3D N, float& min0, float& max0)
{

		//setting min max for box1
		min0 = primitive.Coords().vertices[0].Dot(N);
		float min_dot_box1 = 1;
		float max_dot_box1 = 1;
 
		for (int j = 2; j < primitive.Coords().vertices.size(); j++)
		{
			float curr_proj1 = primitive.Coords().vertices[j].Dot(N);
			
			//select the maximum projection on axis to corresponding box corners
			if (min0 > curr_proj1)
			{
				min0 = curr_proj1;
				min_dot_box1 = j;
			}
			//select the minimum projection on axis to corresponding box corners
			if (curr_proj1> max0) 
			{
				max0 = curr_proj1;
				max0 = j;
			}
		}
}

Al momento mi restituisce quasi sempre true, quindi in sostanza è come se ci fosse sempre una collisione (che non c'è). Comunque il codice è un po' buttato giù alla buona e probabilmente ci sono errori di logica.