84 lines
1.8 KiB
C#
84 lines
1.8 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using Godot;
|
|
|
|
namespace Rokojori
|
|
{
|
|
|
|
public class SAT3
|
|
{
|
|
|
|
public static bool IsIntersecting(
|
|
List<Vector3> pointsA, List<Vector3> faceNormalsA, List<Vector3> edgeDirectionsA,
|
|
List<Vector3> pointsB, List<Vector3> faceNormalsB, List<Vector3> edgeDirectionsB
|
|
)
|
|
{
|
|
for ( var i = 0; i < faceNormalsA.Count; i++ )
|
|
{
|
|
var axis = faceNormalsA[ i ];
|
|
|
|
if ( ProjectionsHaveGap( axis, pointsA, pointsB ) )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
for ( var i = 0; i < faceNormalsB.Count; i++ )
|
|
{
|
|
var axis = faceNormalsB[ i ];
|
|
|
|
if ( ProjectionsHaveGap( axis, pointsA, pointsB ) )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
for ( var i = 0; i < edgeDirectionsA.Count; i++ )
|
|
{
|
|
var edgeA = edgeDirectionsA[ i ];
|
|
|
|
for ( var j = 0; j < edgeDirectionsB.Count; j++ )
|
|
{
|
|
var edgeB = edgeDirectionsB[ j ];
|
|
|
|
var axis = edgeA.Cross( edgeB );
|
|
|
|
if ( ProjectionsHaveGap( axis, pointsA, pointsB ) )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool ProjectionsHaveGap( Vector3 axis, List<Vector3> a, List<Vector3> b )
|
|
{
|
|
var projectionRangeA = Project( axis, a );
|
|
var projectionRangeB = Project( axis, b );
|
|
|
|
return ! projectionRangeA.Overlaps( projectionRangeB );
|
|
}
|
|
|
|
public static Range Project( Vector3 axis, List<Vector3> points )
|
|
{
|
|
var min = float.MaxValue;
|
|
var max = - float.MaxValue;
|
|
|
|
for ( var i = 0; i < points.Count; i++ )
|
|
{
|
|
var dot = Math3D.Dot( axis, points[ i ] );
|
|
|
|
min = Mathf.Min( min, dot );
|
|
max = Mathf.Max( max, dot );
|
|
}
|
|
|
|
return new Range( min, max );
|
|
}
|
|
|
|
}
|
|
}
|
|
|