public static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = Complex.Divide(ref _axis, ref xfA.q);
Vector2 axisB = -Complex.Divide(ref _axis, ref xfB.q);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 axisB = -Complex.Divide(ref normal, ref xfB.q);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 axisA = -Complex.Divide(ref normal, ref xfA.q);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
public float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointA = _proxyA.GetVertex(indexA);
Vector2 localPointB = _proxyB.GetVertex(indexB);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.GetVertex(indexB);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.GetVertex(indexA);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
public static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
var axisA = MathUtils.MulT(ref xfA.Q, _axis);
var axisB = MathUtils.MulT(ref xfB.Q, -_axis);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
var localPointA = _proxyA.vertices[indexA];
var localPointB = _proxyB.vertices[indexB];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
var normal = MathUtils.Mul(ref xfA.Q, _axis);
var pointA = MathUtils.Mul(ref xfA, _localPoint);
var axisB = MathUtils.MulT(ref xfB.Q, -normal);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
var localPointB = _proxyB.vertices[indexB];
var pointB = MathUtils.Mul(ref xfB, localPointB);
var separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
var normal = MathUtils.Mul(ref xfB.Q, _axis);
var pointB = MathUtils.Mul(ref xfB, _localPoint);
var axisA = MathUtils.MulT(ref xfA.Q, -normal);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
var localPointA = _proxyA.vertices[indexA];
var pointA = MathUtils.Mul(ref xfA, localPointA);
var separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}