[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.