internal static bool CircleBoxTest(ref CircleShape A, ref JVector PA, ref BoxShape B, ref JVector PB, ref JMatrix OB)
{
// find vertex closest to circles center
// move circle into boxes space
var pa = JVector.TransposedTransform(PA - PB, OB);
// find normal from box to circles center
var axis = pa;
JVector closestVertex;
// find closest vertex
B.SupportMapping(ref axis, out closestVertex);
// do a SAT test on that axis
// axis to test
JVector T = pa - closestVertex;
float TL = Math.Abs(T * axis);
float a = Math.Abs(pa * axis);
float b = Math.Abs(closestVertex * axis);
if (TL > (a + b + A.Radius))
{
return(false);
}
return(true);
}
/// <summary>
/// Gets up to two supports points if the given direction is within a tolerance of being parallel to the normal of the edge formed by the supports. Otherwise it just returns a single support.
/// </summary>
internal int FindSupportPoints(ref JVector direction, ref JVector PA, ref JMatrix OA, out JVector[] S)
{
// init S
S = new JVector[2];
// transform the normal into object space
JVector N = JVector.TransposedTransform(direction, OA);
// find dots
float a = this.GetCorner(0) * N;
float b = this.GetCorner(1) * N;
float c = this.GetCorner(2) * N;
float d = this.GetCorner(3) * N;
// find min
float min = JMath.Min(a, b);
min = JMath.Min(c, min);
min = JMath.Min(d, min);
// now min should be right
int Snum = 0;
const float threshold = 1.0E-3f;
if (a < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(0), OA) + PA;
}
if (b < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(1), OA) + PA;
}
if (c < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(2), OA) + PA;
}
if (d < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(3), OA) + PA;
}
return(Snum);
}
// this method is extremely brute force, only use for debugging!
public void DebugDraw(IDebugDrawer drawer)
{
if (this.shape.type == ShapeType.Box)
{
BoxShape box = this.shape as BoxShape;
// get corners
JVector a = box.GetCorner(0);
JVector b = box.GetCorner(1);
JVector c = box.GetCorner(2);
JVector d = box.GetCorner(3);
// transform points
JMatrix xform = JMatrix.CreateRotationZ(this.orientation);
JVector.Transform(ref a, ref xform, out a);
JVector.Transform(ref b, ref xform, out b);
JVector.Transform(ref c, ref xform, out c);
JVector.Transform(ref d, ref xform, out d);
a += this.position;
b += this.position;
c += this.position;
d += this.position;
if (isStatic)
{
drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
}
else if (isActive)
{
drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
}
else
{
drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
}
drawer.DrawTriangle(a, c, b);
drawer.DrawTriangle(c, a, d);
// draw outline
drawer.SetColor(0, 0, 0, 1);
drawer.DrawLine(a, b);
drawer.DrawLine(b, c);
drawer.DrawLine(c, d);
drawer.DrawLine(d, a);
}
else if (this.shape.type == ShapeType.Circle)
{
JMatrix o1 = JMatrix.CreateRotationZ(orientation);
JVector dir = JVector.Up;
JVector u = JVector.Zero;
JVector a;
for (int i = -1; i <= 36; i++)
{
JVector.TransposedTransform(ref dir, ref o1, out a);
// get the support in the given direction
JVector s; this.shape.SupportMapping(ref a, out s);
// transform the support into world space
a = JVector.Transform(s, o1) + position;
dir = JVector.Transform(dir, JMatrix.CreateRotationZ(0.0174532925f * 10f));
if (i >= 0)
{
if (isStatic)
{
drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
}
else if (isActive)
{
drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
}
else
{
drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
}
drawer.DrawTriangle(a, u, this.position);
drawer.SetColor(0, 0, 0, 1);
drawer.DrawLine(a, u);
}
u = a;
}
}
//JMatrix o1 = JMatrix.CreateRotationZ(orientation);
//JVector dir = JVector.Up;
//JVector u = JVector.Zero;
//JVector a;
//for (int i = -1; i <= 36; i++)
//{
// JVector.TransposedTransform(ref dir, ref o1, out a);
// // get the support in the given direction
// JVector s; this.shape.SupportMapping(ref a, out s);
// // transform the support into world space
// a = JVector.Transform(s, o1) + position;
// dir = JVector.Transform(dir, JMatrix.CreateRotationZ(0.0174532925f * 10f));
// if (i >= 0)
// {
// if (isStatic)
// drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
// else if (isActive)
// drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
// else
// drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
// drawer.DrawTriangle(a, u, this.position);
// drawer.SetColor(0,0,0, 1);
// drawer.DrawLine(a, u);
// }
// u = a;
//}
//JMatrix xForm = JMatrix.CreateRotationZ(orientation);
//drawer.SetColor(1, 0, 0, 1);
//drawer.DrawLine(position + JVector.Transform(JVector.Left * 0.25f, xForm), position + JVector.Transform(JVector.Zero, xForm));
//drawer.SetColor(0, 1, 0, 1);
//drawer.DrawLine(position + JVector.Transform(JVector.Up * 0.25f, xForm), position + JVector.Transform(JVector.Zero, xForm));
}