/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRTestDemo(DemosGame game)
: base(game)
{
var shapeA = new BoxShape(1, 1, 1);
shapeA.CollisionMargin = 0;
var shapeB = new BoxShape(1, 1, 1);
shapeB.CollisionMargin = 0;
var transformA = new RigidTransform(new Vector3(0, 0, 0));
var transformB = new RigidTransform(new Vector3(.5m, .5m, 0));
Vector3 overlap;
bool overlapped = MPRToolbox.GetLocalOverlapPosition(shapeA, shapeB, ref transformB, out overlap);
Vector3 normal;
Fix64 depth;
Vector3 direction = new Vector3(0, -1, 0);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref transformB, ref direction, out depth, out normal);
ContactData contactData;
//bool overlappedOld = MPRToolboxOld.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
//Random rand = new Random(0);
//for (int i = 0; i < 10000000; i++)
//{
// transformA = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// transformB = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// overlapped = MPRTesting.GetOverlapPosition(shapeA, shapeB, ref transformA, ref transformB, out overlap);
// overlappedOld = MPRToolbox.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
// if (overlapped && !overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlappedOld && !overlapped &&
// (!MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlapped && overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
//}
//Do these tests with rotationally immobile objects.
CollisionDetectionSettings.DefaultMargin = 0;
groundWidth = 10;
groundHeight = .1m;
groundLength = 10;
//a = new Box(new Vector3(0, -5, 0), groundWidth, groundHeight, groundLength, 1);
//a = new TransformableEntity(new Vector3(0,0,0), new TriangleShape(new Vector3(-5, -5, -5), new Vector3(5, -5, -5), new Vector3(-5, -5, 5)), Matrix3x3.Identity);
a = new Triangle(new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), 1);
Space.Add(a);
Space.ForceUpdater.Gravity = new Vector3();
boxWidth = .25m;
boxHeight = .05m;
boxLength = 1;
b = new TransformableEntity(new Vector3(0, 2, 0), new BoxShape(boxWidth, boxHeight, boxLength), Matrix3x3.Identity, 1);
//b = new Cone(new Vector3(0, 2, 0), .2m, .1m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
b.LocalInertiaTensorInverse = new Matrix3x3();
CollisionRules.AddRule(b, a, CollisionRule.NoSolver);
b.ActivityInformation.IsAlwaysActive = true;
Space.Add(b);
//Space.Add(new TransformableEntity(new Vector3(0, 4, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Space.Add( new TransformableEntity(new Vector3(0, 6, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Vector3[] vertices = new Vector3[] { new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), new Vector3(0, -60, 5) };
//int[] indices = new int[] { 0, 1, 2 , 0, 2, 3 };
//StaticMesh mesh = new StaticMesh(vertices, indices);
//Space.Add(mesh);
//mesh.ImproveBoundaryBehavior = true;
//mesh.Sidedness = TriangleSidedness.Counterclockwise;
//game.ModelDrawer.Add(mesh);
//mesh.CollisionRules.Personal = CollisionRule.NoSolver;
}
public override void Update(Fix64 dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.Left))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Right))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Down))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Up))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Break.");
}
base.Update(dt);
RigidTransform localTransformB;
RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;
MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);
Vector3 position;
if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
{
//Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
Fix64 previousT;
Vector3 previousNormal;
Fix64 t;
Vector3 normal;
rayCastDirection = localTransformB.Position;
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
//Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fourthDirection = -secondDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fifthDirection = -thirdDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Correct the penetration depth.
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
contactDepth = t;
contactNormal = previousNormal;
////Converge to local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
// if (previousT - t <= Toolbox.BigEpsilon)
// break;
// previousT = t;
// previousNormal = normal;
//}
}
#region Box Box minkowski sum
////Construct explicit minkowski sum.
//Vector3[] aLines = new Vector3[8];
//aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
//aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);
//Vector3[] bLines = new Vector3[8];
//bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
//bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);
//for (int i = 0; i < 8; i++)
// aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);
//List<Vector3> vertices = new List<Vector3>();
//for (int i = 0; i < 8; i++)
//{
// for (int j = 0; j < 8; j++)
// {
// if (b.CollisionInformation.Pairs.Count > 0)
// {
// if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
// vertices.Add(aLines[i] - bLines[j]);
// else
// vertices.Add(bLines[i] - aLines[j]);
// }
// else
// {
// vertices.Add(bLines[i] - aLines[j]);
// }
// }
//}
//var indices = new List<int>();
//Toolbox.GetConvexHull(vertices, indices);
#endregion
#region Arbitrary minkowski sum
var vertices = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
int NumSamples = 16;
Fix64 angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
Fix64 phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = Fix64.Sin(phi);
var cosPhi = Fix64.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
Fix64 theta = j * angleChange;
direction.X = Fix64.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = Fix64.Sin(theta) * cosPhi;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);
vertices.Add(max);
}
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
vertices.Add(max);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
vertices.Add(max);
var indices = new List <int>();
ConvexHullHelper.GetConvexHull(vertices, indices);
#endregion
minkowskiLines.Clear();
for (int i = 0; i < indices.Count; i += 3)
{
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
}
}
