Monday, October 6, 2008

Collision detection using SAT


Following is the SAT code for triangle - Box intersection , I couldn't find any c++ code in internet ( Muller's is there , it is cryptic for me as well as it is not c++)

template
<typename T>
Interfaces3D::InterSection CBBox<T>::IsInsideBox(
const Triangle<T>& refTri ) const
{

// If three points are inside the box, definitily the triangle is inside the ABBBox.
   if( IsInsideBox( refTri.m_Pt1 ) &&  IsInsideBox( refTri.m_Pt2 ) &&  IsInsideBox( refTri.m_Pt3 ) )
   return Interfaces3D::InterSectionIn;
   Math3D::vector<T> xAxis(1,0,0);
   Math3D::vector<T> yAxis(0,1,0);
   Math3D::vector<T> zAxis(0,0,1);

  //..............................................................
  // Intersection test using SAT (separating Axis test) algorithm.
  //..............................................................
  float fMin,fMax;
  float fBoxMin,fBoxMax;
  float fProjTri[3] = { 0,0,0 };
  float fProjBox[2] = { 0,0 };
   
 // First test is agnist the AABBox's cardianl axis.
 fProjTri[0] = refTri.m_Pt1.x;
 fProjTri[1] = refTri.m_Pt2.x;
 fProjTri[2] = refTri.m_Pt3.x; 
 FindMinMax( fProjTri,3,fMin,fMax );

 if( (fMin < m_XL && fMax < m_XL ) || (fMin > m_XR && fMax > m_XR ) )
     return Interfaces3D::InterSectionOut; // the triangle is outside.
 fProjTri[0] = refTri.m_Pt1.y;
 fProjTri[1] = refTri.m_Pt2.y;
 fProjTri[2] = refTri.m_Pt3.y;
 FindMinMax( fProjTri,3,fMin,fMax );

 if( (fMin < m_YB && fMax < m_YB ) || (fMin > m_YT && fMax > m_YT ) )
    return Interfaces3D::InterSectionOut; // the triangle is outside.
 fProjTri[0] = refTri.m_Pt1.z;
 fProjTri[1] = refTri.m_Pt2.z;
 fProjTri[2] = refTri.m_Pt3.z;

 FindMinMax( fProjTri,3,fMin,fMax );

 if( (fMin < m_ZF && fMax < m_ZF ) || (fMin > m_ZN && fMax > m_ZN ) )
    return Interfaces3D::InterSectionOut; // the triangle is outside.

 // Second test is agnist triangles normal. 
 Math3D::vector<T> vtNormal = refTri.m_Normal;
 // project the triangle coordinates on this normal as well as the AABB corners.
 // projecting Box's coordinates
 Math3D::vector<T> vtRadius = GetTopLeftNear() - GetPosition();

 float fExtent = vtNormal.Dot( vtRadius );
 float fCenterBox = vtNormal.Dot( GetPosition() );
 fProjBox[0] = fCenterBox + fExtent;
 fProjBox[1] = fCenterBox - fExtent; 
 FindMinMax( fProjBox, 2,fBoxMin,fBoxMax );

 // projecting triangle coordinates
 fProjTri[0] = vtNormal.Dot( refTri.m_Pt1 );
 fProjTri[1] = vtNormal.Dot( refTri.m_Pt2 );
 fProjTri[2] = vtNormal.Dot( refTri.m_Pt3 );

 FindMinMax( fProjTri, 3,fMin,fMax );

 // Check for overlapping between pronected coordinates.
 // if not overlapping , no intersection.
 if( (fMin < fBoxMin && fMax < fBoxMin ) || (fMin > fBoxMax && fMax > fBoxMax ) )
    return Interfaces3D::InterSectionOut; // the triangle is outside.
  
 // Next possible SAT axis are the Cross product of each tri edges with
 // box's cardinal axis.

 Math3D::vector<T> satAxis[9];
 Math3D::vector<T> vTmp = (refTri.m_Pt1 - refTri.m_Pt2);
 // edge 0 and X,Y,Z axis 
 satAxis[0] = vTmp.Cross( xAxis).normalize() ; 
 satAxis[1] = vTmp.Cross( yAxis).normalize() ;
 satAxis[2] = vTmp.Cross( zAxis).normalize() ;

 // edge 1 and X,Y,Z axis 
 vTmp = (refTri.m_Pt3 - refTri.m_Pt2);
 satAxis[3] = vTmp.Cross( xAxis).normalize() ;
 satAxis[4] = vTmp.Cross( yAxis).normalize() ;
 satAxis[5] = vTmp.Cross( zAxis).normalize() ;

 //  edge 2 and X,Y,Z axis
 vTmp = (refTri.m_Pt1 - refTri.m_Pt3);
 satAxis[6] = vTmp.Cross( xAxis).normalize() ;
 satAxis[7] = vTmp.Cross( yAxis).normalize() ;
 satAxis[8] = vTmp.Cross( zAxis).normalize() ;

 for( int i = 0; i < 9 ; i ++ )
  {
fExtent = satAxis[i].Dot( vtRadius );
fCenterBox = satAxis[i].Dot( GetPosition() );
fProjBox[0] = fCenterBox + fExtent;
fProjBox[1] = fCenterBox - fExtent;
FindMinMax( fProjBox, 2,fBoxMin,fBoxMax ); 
 // projecting triangle coordinates
            fProjTri[0] = satAxis[i].Dot( refTri.m_Pt1 );
            fProjTri[1] = satAxis[i].Dot( refTri.m_Pt2 );
            fProjTri[2] = satAxis[i].Dot( refTri.m_Pt3 );
            FindMinMax( fProjTri, 3,fMin,fMax );
           // check for overlapping on the SAT axis
           if( (fMin < fBoxMin && fMax < fBoxMin ) || (fMin > fBoxMax && fMax > fBoxMax ) )
               return Interfaces3D::InterSectionOut; // the triangle is outside.
  } 
return Interfaces3D::InterSectionIntersect;
}

No comments: