/// <summary>
/// Adds entities associated with the solver item to the involved entities list.
/// Ensure that sortInvolvedEntities() is called at the end of the function.
/// This allows the non-batched multithreading system to lock properly.
/// </summary>
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
if (connectionA != null && connectionA != WorldEntity)
outputInvolvedEntities.Add(connectionA);
if (connectionB != null && connectionB != WorldEntity)
outputInvolvedEntities.Add(connectionB);
}
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
//The default implementation for solver groups looks at every single subconstraint.
//That's not necessary for these special constraints.
if (entityA != null)
outputInvolvedEntities.Add(entityA);
if (entityB != null)
outputInvolvedEntities.Add(entityB);
}
/// <summary>
/// Refreshes the contact manifold, removing any out of date contacts
/// and updating others.
/// </summary>
public static void ContactRefresh(RawList<Contact> contacts, RawValueList<ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList<int> toRemove)
{
//TODO: Could also refresh normals with some trickery.
//Would also need to refresh depth using new normals, and would require some extra information.
for (int k = 0; k < contacts.Count; k++)
{
contacts.Elements[k].Validate();
ContactSupplementData data = supplementData.Elements[k];
System.Numerics.Vector3 newPosA, newPosB;
RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA);
RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB);
//ab - (ab*n)*n
//Compute the horizontal offset.
System.Numerics.Vector3 ab;
Vector3Ex.Subtract(ref newPosB, ref newPosA, out ab);
float dot;
Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
System.Numerics.Vector3 temp;
Vector3Ex.Multiply(ref contacts.Elements[k].Normal, dot, out temp);
Vector3Ex.Subtract(ref ab, ref temp, out temp);
dot = temp.LengthSquared();
if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared)
{
toRemove.Add(k);
}
else
{
//Depth refresh:
//Find deviation ((Ra-Rb)*N) and add to base depth.
Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot;
if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance)
toRemove.Add(k);
else
{
//Refresh position and ra/rb.
System.Numerics.Vector3 newPos;
Vector3Ex.Add(ref newPosB, ref newPosA, out newPos);
Vector3Ex.Multiply(ref newPos, .5f, out newPos);
contacts.Elements[k].Position = newPos;
//This is an interesting idea, but has very little effect one way or the other.
//data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth;
//RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA);
//RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB);
}
contacts.Elements[k].Validate();
}
}
}
public static void GetShapeMeshData(EntityCollidable collidable, List<VertexPositionNormalTexture> vertices, List<ushort> indices)
{
var shape = collidable.Shape as ConvexShape;
if (shape == null)
throw new ArgumentException("Wrong shape type for this helper.");
var vertexPositions = new BEPUutilities.Vector3[SampleDirections.Length];
for (int i = 0; i < SampleDirections.Length; ++i)
{
shape.GetLocalExtremePoint(SampleDirections[i], out vertexPositions[i]);
}
var hullIndices = new RawList<int>();
ConvexHullHelper.GetConvexHull(vertexPositions, hullIndices);
var hullTriangleVertices = new RawList<BEPUutilities.Vector3>();
foreach (int i in hullIndices)
{
hullTriangleVertices.Add(vertexPositions[i]);
}
for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
{
Vector3 normal = MathConverter.Convert(BEPUutilities.Vector3.Normalize(BEPUutilities.Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i])));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i]), normal, new Vector2(0, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 1]), normal, new Vector2(1, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 2]), normal, new Vector2(0, 1)));
indices.Add(i);
indices.Add((ushort)(i + 1));
indices.Add((ushort)(i + 2));
}
}
[Test] public void Basics()
{
RawList<int> intList = new RawList<int>();
intList.Add(10);
intList.AddRange(new int[] { 17, 42, 94 });
Assert.AreEqual(4, intList.Count);
Assert.IsTrue(intList.Contains(42));
Assert.AreEqual(2, intList.IndexOf(42));
CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList.Data.Take(4));
intList.ShrinkToFit();
Assert.AreEqual(intList.Count, intList.Capacity);
intList.Remove(42);
Assert.AreEqual(3, intList.Count);
Assert.IsTrue(!intList.Contains(42));
Assert.AreEqual(-1, intList.IndexOf(42));
CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList.Data.Take(3));
intList.Insert(1, 100);
CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList.Data.Take(4));
intList.InsertRange(2, new int[] { 150, 200, 250, 300 });
CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList.Data.Take(8));
intList.Clear();
Assert.AreEqual(0, intList.Count);
Assert.IsTrue(!intList.Contains(94));
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BroadPhaseRemovalTestDemo(DemosGame game)
: base(game)
{
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-500, -500, -500), new Vector3(500, 500, 500));
DynamicHierarchy dh = new DynamicHierarchy();
Random rand = new Random(0);
RawList<Entity> entities = new RawList<Entity>();
for (int k = 0; k < 1000; k++)
{
Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
toAdd = new Box(MathConverter.Convert(position), 1, 1, 1, 1);
entities.Add(toAdd);
}
testResults = new double[2];
int runCount = 10;
for (int k = 0; k < runCount; k++)
{
for (int i = 0; i < entities.Count; i++)
{
dh.Add(entities[i].CollisionInformation);
}
long start = Stopwatch.GetTimestamp();
for (int i = 0; i < entities.Count; i++)
{
//dh.RemoveFast(entities[i].CollisionInformation);
}
long end = Stopwatch.GetTimestamp();
testResults[0] += (end - start) / (double)Stopwatch.Frequency;
for (int i = 0; i < entities.Count; i++)
{
dh.Add(entities[i].CollisionInformation);
}
start = Stopwatch.GetTimestamp();
for (int i = 0; i < entities.Count; i++)
{
//dh.RemoveBrute(entities[i].CollisionInformation);
}
end = Stopwatch.GetTimestamp();
testResults[1] += (end - start) / (double)Stopwatch.Frequency;
}
testResults[0] /= runCount;
testResults[1] /= runCount;
}
/// <summary>
/// Collects the entities which are affected by the solver group and updates the internal listing.
/// </summary>
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
foreach (EntitySolverUpdateable item in solverUpdateables)
{
for (int i = 0; i < item.involvedEntities.count; i++)
{
if (!outputInvolvedEntities.Contains(item.involvedEntities.Elements[i]))
{
outputInvolvedEntities.Add(item.involvedEntities.Elements[i]);
}
}
}
}
float upStepMargin = .1f; //There's a little extra space above the maximum step height to start the obstruction and downcast test rays. Helps when a step is very close to the max step height.
void FindUpStepCandidates(RawList <ContactData> outputStepCandidates)
{
foreach (var c in character.SupportFinder.sideContacts)
{
//A 6DOF character will need to have a 3d movement direction. It will replace this graduation of a 2d vector.
Vector3 movementDirection = new Vector3()
{
X = character.HorizontalMotionConstraint.MovementDirection.X,
Z = character.HorizontalMotionConstraint.MovementDirection.Y
};
//Check to see if the contact is sufficiently aligned with the movement direction to be considered for stepping.
//TODO: This could behave a bit odd when encountering steps or slopes near the base of rounded collision margin.
var contact = c.Contact;
float dot;
Vector3.Dot(ref contact.Normal, ref movementDirection, out dot);
if (dot > 0)
{
//It is! But is it low enough?
dot = Vector3.Dot(character.Body.OrientationMatrix.Down, c.Contact.Position - character.Body.Position);
//It must be between the bottom of the character and the maximum step height.
if (dot < character.Body.Height * .5f && dot > character.Body.Height * .5f - maximumStepHeight - upStepMargin)
{
//It's a candidate!
//But wait, there's more! Do we already have a candidate that covers this direction?
bool shouldAdd = true;
for (int i = 0; i < outputStepCandidates.Count; i++)
{
Vector3.Dot(ref outputStepCandidates.Elements[i].Normal, ref contact.Normal, out dot);
if (dot > .99f)
{
shouldAdd = false; //Woops! This direction is already covered. Don't bother.
break;
}
}
if (shouldAdd)
{
outputStepCandidates.Add(contact);
}
}
}
}
}
private static void ScanObject(float rayIncrement, float maxLength, ref Vector3 increment1, ref Vector3 increment2, ref Ray ray, ref RayHit startHit, ref RayHit endHit, RawList <Vector3> pointContributions, out float volume)
{
Vector3 cell;
Vector3.Multiply(ref ray.Direction, rayIncrement, out cell);
Vector3.Add(ref increment1, ref cell, out cell);
Vector3.Add(ref increment2, ref cell, out cell);
float perCellVolume = cell.X * cell.Y * cell.Z;
volume = 0;
for (int i = (int)(startHit.T / rayIncrement); i <= (int)((maxLength - endHit.T) / rayIncrement); i++)
{
Vector3 position;
Vector3.Multiply(ref ray.Direction, (i + .5f) * rayIncrement, out position);
Vector3.Add(ref position, ref ray.Position, out position);
pointContributions.Add(position);
volume += perCellVolume;
}
}
///<summary>
/// Adds a solver updateable to the solver.
///</summary>
///<param name="item">Updateable to add.</param>
///<exception cref="ArgumentException">Thrown when the item already belongs to a solver.</exception>
public void Add(SolverUpdateable item)
{
if (item.Solver == null)
{
item.Solver = this;
bool taken = false;
addRemoveLocker.Enter(ref taken);
//addRemoveLocker.Enter();
item.solverIndex = solverUpdateables.Count;
solverUpdateables.Add(item);
addRemoveLocker.Exit();
DeactivationManager.Add(item.simulationIslandConnection);
item.OnAdditionToSolver(this);
}
else
{
throw new ArgumentException("Solver updateable already belongs to something; it can't be added.", "item");
}
}
/// <summary>
/// Adds a solver updateable to the group.
/// </summary>
/// <param name="solverUpdateable">Solver updateable to add.</param>
/// <exception cref="InvalidOperationException">Thrown when the SolverUpdateable to add to the SolverGroup already belongs to another SolverGroup or to a Space.</exception>
protected void Add(EntitySolverUpdateable solverUpdateable)
{
if (solverUpdateable.solver == null)
{
if (solverUpdateable.SolverGroup == null)
{
solverUpdateables.Add(solverUpdateable);
solverUpdateable.SolverGroup = this;
solverUpdateable.Solver = solver;
OnInvolvedEntitiesChanged();
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup.");
}
}
else
{
throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver.");
}
}
///<summary>
/// Constructs a compound collidable using additional information about the shapes in the compound.
///</summary>
///<param name="children">Data representing the children of the compound collidable.</param>
///<param name="center">Location computed to be the center of the compound object.</param>
public CompoundCollidable(IList <CompoundChildData> children, out Vector3 center)
{
Events = new CompoundEventManager();
RawList <CompoundShapeEntry> shapeList = new RawList <CompoundShapeEntry>();
//Create the shape first.
for (int i = 0; i < children.Count; i++)
{
shapeList.Add(children[i].Entry);
}
base.Shape = new CompoundShape(shapeList, out center);
//Now create the actual child objects.
for (int i = 0; i < children.Count; i++)
{
this.children.Add(GetChild(children[i], i));
}
hierarchy = new CompoundHierarchy(this);
}
///<summary>
/// Adds a simulation island member to the manager.
///</summary>
///<param name="simulationIslandMember">Member to add.</param>
///<exception cref="Exception">Thrown if the member already belongs to a manager.</exception>
public void Add(SimulationIslandMember simulationIslandMember)
{
if (simulationIslandMember.DeactivationManager == null)
{
simulationIslandMember.Activate();
simulationIslandMember.DeactivationManager = this;
simulationIslandMembers.Add(simulationIslandMember);
if (simulationIslandMember.IsDynamic)
{
AddSimulationIslandToMember(simulationIslandMember);
}
else
{
RemoveSimulationIslandFromMember(simulationIslandMember);
}
}
else
{
throw new ArgumentException("Cannot add that member to this DeactivationManager; it already belongs to a manager.");
}
}
internal void ReadAnimatedTiles(string path)
{
using (Stream s = FileOp.Open(path, FileAccessMode.Read))
using (DeflateStream deflate = new DeflateStream(s, CompressionMode.Decompress))
using (BinaryReader r = new BinaryReader(deflate)) {
int count = r.ReadInt32();
animatedTiles = new RawList <AnimatedTile>(count);
for (int i = 0; i < count; i++)
{
ushort frameCount = r.ReadUInt16();
if (frameCount == 0)
{
continue;
}
ushort[] frames = new ushort[frameCount];
byte[] flags = new byte[frameCount];
for (int j = 0; j < frameCount; j++)
{
frames[j] = r.ReadUInt16();
flags[j] = r.ReadByte();
}
byte speed = r.ReadByte();
ushort delay = r.ReadUInt16();
ushort delayJitter = r.ReadUInt16();
byte pingPong = r.ReadByte();
ushort pingPongDelay = r.ReadUInt16();
// ToDo: Adjust FPS in Import
speed = (byte)(speed * 14 / 10);
animatedTiles.Add(new AnimatedTile(tileset, frames, flags, speed,
delay, delayJitter, (pingPong > 0), pingPongDelay));
}
}
}
public AnimatedTile(TileSet tileset, ushort[] tileIDs, byte[] tileFlags, int fps, int delay, int delayJitter, bool pingPong, int pingPongDelay)
{
this.frameRate = fps;
this.delay = delay;
// ToDo: DelayJitter is not used...
//this.delayJitter = delayJitter;
this.pingPong = pingPong;
this.pingPongDelay = pingPongDelay;
tiles = new RawList <LayerTile>();
for (int i = 0; i < tileIDs.Length; i++)
{
LayerTile tile = tileset.GetDefaultTile(tileIDs[i]);
byte tileModifier = (byte)(tileFlags[i] >> 4);
if (tileModifier == 1 /*Translucent*/)
{
tile.MaterialAlpha = /*127*/ 140;
}
else if (tileModifier == 2 /*Invisible*/)
{
tile.MaterialAlpha = 0;
}
else
{
tile.MaterialAlpha = 255;
}
tile.SuspendType = SuspendType.None;
tiles.Add(tile);
}
if (fps > 0)
{
frameDuration = 70f / fps;
framesLeft = frameDuration;
}
}
public static unsafe void Initialize()
{
if (_static_infos != null)
{
return;
}
_static_infos = new RawList <static_animation_info>();
UOFile file = AnimDataLoader.Instance.AnimDataFile;
if (file == null)
{
return;
}
long startAddr = file.StartAddress.ToInt64();
uint lastaddr = (uint)(startAddr + file.Length - sizeof(AnimDataFrame2));
for (int i = 0; i < TileDataLoader.Instance.StaticData.Length; i++)
{
if (TileDataLoader.Instance.StaticData[i]
.IsAnimated)
{
uint addr = (uint)(i * 68 + 4 * (i / 8 + 1));
uint offset = (uint)(startAddr + addr);
if (offset <= lastaddr)
{
_static_infos.Add
(
new static_animation_info
{
index = (ushort)i,
is_field = StaticFilters.IsField((ushort)i)
}
);
}
}
}
}
///<summary>
/// Updates the pair handler's contacts.
///</summary>
///<param name="dt">Timestep duration.</param>
protected virtual void UpdateContacts(float dt)
{
UpdateContainedPairs(dt);
//Eliminate old pairs.
foreach (TriangleEntry pair in subPairs.Keys)
{
if (!containedPairs.Contains(pair))
{
pairsToRemove.Add(pair);
}
}
for (int i = 0; i < pairsToRemove.Count; i++)
{
MobileMeshPairHandler toReturn = subPairs[pairsToRemove.Elements[i]];
subPairs.Remove(pairsToRemove.Elements[i]);
//The contained pairs list pulled TriangleCollidables from a pool to create the opposing collidables.
//Clean those up now.
//CollidableA is used without checking, because MobileMeshPairHandlers always put the convex in slot A.
CleanUpCollidable((TriangleCollidable)toReturn.CollidableA);
toReturn.CleanUp();
toReturn.Factory.GiveBack(toReturn);
}
containedPairs.Clear();
pairsToRemove.Clear();
foreach (KeyValuePair <TriangleEntry, MobileMeshPairHandler> pair in subPairs)
{
if (pair.Value.BroadPhaseOverlap.collisionRule < CollisionRule.NoNarrowPhaseUpdate
) //Don't test if the collision rules say don't.
{
ConfigureCollidable(pair.Key, dt);
//Update the contact count using our (the parent) contact count so that the child can avoid costly solidity testing.
pair.Value.MeshManifold.parentContactCount = contactCount;
pair.Value.UpdateCollision(dt);
}
}
}
[Test] public void Basics()
{
RawList <int> intList = new RawList <int>();
intList.Add(10);
intList.AddRange(new int[] { 17, 42, 94 });
Assert.AreEqual(4, intList.Count);
Assert.IsTrue(intList.Contains(42));
Assert.AreEqual(2, intList.IndexOf(42));
CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList.Data.Take(intList.Count));
intList.ShrinkToFit();
Assert.AreEqual(intList.Count, intList.Capacity);
intList.Remove(42);
Assert.AreEqual(3, intList.Count);
Assert.IsTrue(!intList.Contains(42));
Assert.AreEqual(-1, intList.IndexOf(42));
CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList.Data.Take(intList.Count));
intList.Insert(1, 100);
CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList.Data.Take(intList.Count));
intList.InsertRange(2, new int[] { 150, 200, 250, 300 });
CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList.Data.Take(intList.Count));
intList.RemoveAt(1);
CollectionAssert.AreEqual(new int[] { 10, 150, 200, 250, 300, 17, 94 }, intList);
CollectionAssert.AreEqual(new int[] { 10, 150, 200, 250, 300, 17, 94 }, intList.Data.Take(intList.Count));
intList.Clear();
Assert.AreEqual(0, intList.Count);
Assert.IsTrue(!intList.Contains(94));
}
public static void GetShapeMeshData(EntityCollidable collidable, List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
var shape = collidable.Shape as ConvexShape;
if (shape == null)
{
throw new ArgumentException("Wrong shape type for this helper.");
}
var vertexPositions = new BEPUutilities.Vector3[SampleDirections.Length];
for (int i = 0; i < SampleDirections.Length; ++i)
{
shape.GetLocalExtremePoint(SampleDirections[i], out vertexPositions[i]);
}
var hullIndices = new RawList <int>();
ConvexHullHelper.GetConvexHull(vertexPositions, hullIndices);
var hullTriangleVertices = new RawList <BEPUutilities.Vector3>();
foreach (int i in hullIndices)
{
hullTriangleVertices.Add(vertexPositions[i]);
}
for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
{
Vector3 normal = MathConverter.Convert(BEPUutilities.Vector3.Normalize(BEPUutilities.Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i])));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i]), normal, new Vector2(0, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 1]), normal, new Vector2(1, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 2]), normal, new Vector2(0, 1)));
indices.Add(i);
indices.Add((ushort)(i + 1));
indices.Add((ushort)(i + 2));
}
}
[Test] public void RentReset()
{
RawListPool <int> intPool = new RawListPool <int>();
List <RawList <int> > previouslyRented = new List <RawList <int> >();
// Rent a few lists with varying capacities, and do so a few times
for (int n = 0; n < 5; n++)
{
int[] capacities = new int[] { 0, 7, 3, 10, 5, 19 };
for (int i = 0; i < capacities.Length; i++)
{
RawList <int> list = intPool.Rent(capacities[i]);
previouslyRented.Add(list);
// Asset that they're empty, but match the required min capacity
Assert.AreEqual(0, list.Count);
Assert.GreaterOrEqual(list.Capacity, capacities[i]);
// Add a few elements
for (int k = 0; k < capacities[i]; k++)
{
list.Add(k + 1);
}
}
// Reset the pool. We haven't returned any lists so far.
intPool.Reset();
// Assert that, after the reset, all previously rented lists
// have been cleared, but still have their required min capacity.
for (int i = 0; i < capacities.Length; i++)
{
RawList <int> list = previouslyRented[i];
Assert.AreEqual(0, list.Count);
Assert.GreaterOrEqual(list.Capacity, capacities[i]);
}
}
}
internal void ReadAnimatedTiles(Stream s)
{
using (BinaryReader r = new BinaryReader(s)) {
int count = r.ReadInt32();
animatedTiles = new RawList <AnimatedTile>(count);
for (int i = 0; i < count; i++)
{
ushort frameCount = r.ReadUInt16();
if (frameCount == 0)
{
continue;
}
ushort[] frames = new ushort[frameCount];
byte[] flags = new byte[frameCount];
for (int j = 0; j < frameCount; j++)
{
frames[j] = r.ReadUInt16();
flags[j] = r.ReadByte();
}
byte speed = r.ReadByte();
ushort delay = r.ReadUInt16();
ushort delayJitter = r.ReadUInt16();
byte pingPong = r.ReadByte();
ushort pingPongDelay = r.ReadUInt16();
// ToDo: Adjust FPS in Import
speed = (byte)(speed * 14 / 10);
animatedTiles.Add(new AnimatedTile(tileset, frames, flags, speed,
delay, delayJitter, (pingPong > 0), pingPongDelay));
}
}
}
Fix64 upStepMargin = F64.C0p1; //There's a little extra space above the maximum step height to start the obstruction and downcast test rays. Helps when a step is very close to the max step height.
void FindUpStepCandidates(RawList <ContactData> outputStepCandidates, ref Vector3 down)
{
Vector3 movementDirection = HorizontalMotionConstraint.MovementDirection3d;
foreach (var c in SupportFinder.SideContacts)
{
//Check to see if the contact is sufficiently aligned with the movement direction to be considered for stepping.
//TODO: This could behave a bit odd when encountering steps or slopes near the base of rounded collision margin.
var contact = c.Contact;
Fix64 dot;
Vector3.Dot(ref contact.Normal, ref movementDirection, out dot);
if (dot > F64.C0)
{
//It is! But is it low enough?
dot = Vector3.Dot(down, c.Contact.Position - characterBody.Position);
//It must be between the bottom of the character and the maximum step height.
if (dot < characterBody.Height * F64.C0p5 && dot > characterBody.Height * F64.C0p5 - maximumStepHeight - upStepMargin)
{
//It's a candidate!
//But wait, there's more! Do we already have a candidate that covers this direction?
bool shouldAdd = true;
for (int i = 0; i < outputStepCandidates.Count; i++)
{
Vector3.Dot(ref outputStepCandidates.Elements[i].Normal, ref contact.Normal, out dot);
if (dot > F64.C0p99)
{
shouldAdd = false; //Woops! This direction is already covered. Don't bother.
break;
}
}
if (shouldAdd)
{
outputStepCandidates.Add(contact);
}
}
}
}
}
public override void Update(float dt)
{
for (int i = removedEntities.Count - 1; i >= 0; --i)
{
if (random.NextDouble() < 0.2)
{
var entity = removedEntities[i];
addedEntities.Add(entity);
Space.Add(entity);
removedEntities.FastRemoveAt(i);
}
}
for (int i = addedEntities.Count - 1; i >= 0; --i)
{
if (random.NextDouble() < 0.02)
{
var entity = addedEntities[i];
removedEntities.Add(entity);
Space.Remove(entity);
addedEntities.FastRemoveAt(i);
}
}
if (Game.MouseInput.MiddleButton != Microsoft.Xna.Framework.Input.ButtonState.Pressed)
{
for (int i = 0; i < 20; i++)
{
var entity = addedEntities[random.Next(addedEntities.Count)];
entity.Position = new Vector3(
(float)(random.NextDouble() - 0.5f) * width,
(float)(random.NextDouble() - 0.5f) * height,
(float)(random.NextDouble() - 0.5f) * length);
}
}
base.Update(dt);
}
private static void RemoveInsidePoints(RawList <Vector3> points, RawList <int> triangleIndices,
RawList <int> outsidePoints)
{
RawList <int> insidePoints = CommonResources.GetIntList();
//We're going to remove points from this list as we go to prune it down to the truly inner points.
insidePoints.AddRange(outsidePoints);
outsidePoints.Clear();
for (int i = 0; i < triangleIndices.Count && insidePoints.Count > 0; i += 3)
{
//Compute the triangle's plane in point-normal representation to test other points against.
Vector3 normal;
FindNormal(triangleIndices, points, i, out normal);
Vector3 p = points.Elements[triangleIndices.Elements[i]];
for (int j = insidePoints.Count - 1; j >= 0; --j)
{
//Offset from the triangle to the current point, tested against the normal, determines if the current point is visible
//from the triangle face.
Vector3 offset;
Vector3.Subtract(ref points.Elements[insidePoints.Elements[j]], ref p, out offset);
float dot;
Vector3.Dot(ref offset, ref normal, out dot);
//If it's visible, then it's outside!
if (dot > 0)
{
//This point is known to be on the outside; put it on the outside!
outsidePoints.Add(insidePoints.Elements[j]);
insidePoints.FastRemoveAt(j);
}
}
}
CommonResources.GiveBack(insidePoints);
}
//This works in the specific case of 4 contacts and 1 contact candidate.
///<summary>
/// Reduces a 4-contact manifold and contact candidate to 4 total contacts.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidate">Contact candidate to include in the reduction process.</param>
///<param name="toRemove">Contacts that need to be removed to reduce the manifold.</param>
///<param name="addCandidate">Whether or not to add the contact candidate to reach the reduced manifold.</param>
///<exception cref="ArgumentException">Thrown when the contact manifold being reduced doesn't have 4 contacts.</exception>
public static void ReduceContacts(RawList<Contact> contacts, ref ContactData contactCandidate, RawList<int> toRemove, out bool addCandidate)
{
if (contacts.Count != 4)
throw new ArgumentException("Can only use this method to reduce contact lists with four contacts and a contact candidate.");
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
for (int i = 0; i < 4; i++)
{
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
if (contactCandidate.PenetrationDepth > maximumDepth)
{
deepestIndex = 4;
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < 4)
deepestPosition = contacts.Elements[deepestIndex].Position;
else
deepestPosition = contactCandidate.Position;
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < 4; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
Vector3.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestIndex = 4;
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
furthestPosition = contacts.Elements[furthestIndex].Position;
else
furthestPosition = contactCandidate.Position;
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref contacts.Elements[0].Normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
float dot;
for (int i = 0; i < 4; i++)
{
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
Vector3.Dot(ref contactCandidate.Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = 4;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = 4;
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
if (4 == deepestIndex || 4 == furthestIndex || 4 == minYAxisIndex || 4 == maxYAxisIndex)
{
addCandidate = true;
//Only reduce when we are going to add a new contact, and only get rid of one.
for (int i = 0; i < 4; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
toRemove.Add(i);
break;
}
}
}
else
addCandidate = false;
}
internal override void RetrieveNodes(RawList <LeafNode> leafNodes)
{
Refit();
leafNodes.Add(this);
}
///<summary>
/// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape.
///</summary>
///<param name="localBoundingBox">Bounding box in the local space of the terrain shape.</param>
///<param name="overlappedElements">Indices of elements whose bounding boxes overlap the input bounding box.</param>
public bool GetOverlaps(BoundingBox localBoundingBox, RawList<int> overlappedElements)
{
int width = heights.GetLength(0);
int minX = Math.Max((int)localBoundingBox.Min.X, 0);
int minY = Math.Max((int)localBoundingBox.Min.Z, 0);
int maxX = Math.Min((int)localBoundingBox.Max.X, width - 2);
int maxY = Math.Min((int)localBoundingBox.Max.Z, heights.GetLength(1) - 2);
for (int i = minX; i <= maxX; i++)
{
for (int j = minY; j <= maxY; j++)
{
//Before adding a triangle to the list, make sure the object isn't too high or low from the quad.
float highest, lowest;
float y1 = heights[i, j];
float y2 = heights[i + 1, j];
float y3 = heights[i, j + 1];
float y4 = heights[i + 1, j + 1];
highest = y1;
lowest = y1;
if (y2 > highest)
highest = y2;
else if (y2 < lowest)
lowest = y2;
if (y3 > highest)
highest = y3;
else if (y3 < lowest)
lowest = y3;
if (y4 > highest)
highest = y4;
else if (y4 < lowest)
lowest = y4;
if (localBoundingBox.Max.Y < lowest ||
localBoundingBox.Min.Y > highest)
continue;
//Now the local bounding box is very likely intersecting those of the triangles.
//Add the triangles to the list.
int quadIndex = (i + j * width) * 2;
overlappedElements.Add(quadIndex);
overlappedElements.Add(quadIndex + 1);
}
}
return overlappedElements.Count > 0;
}
public void Draw(Effect effect, Space space)
{
if (space.Entities.Count > 0)
{
BoundingBox box;
foreach (var entity in space.Entities)
{
var island = entity.ActivityInformation.SimulationIsland;
if (island != null)
{
if (islandBoundingBoxes.TryGetValue(island, out box))
{
box = BoundingBox.CreateMerged(entity.CollisionInformation.BoundingBox, box);
islandBoundingBoxes[island] = box;
}
else
{
islandBoundingBoxes.Add(island, entity.CollisionInformation.BoundingBox);
}
}
}
foreach (var islandBoundingBox in islandBoundingBoxes)
{
Color colorToUse = islandBoundingBox.Key.IsActive ? Color.DarkGoldenrod : Color.DarkGray;
Vector3[] boundingBoxCorners = islandBoundingBox.Value.GetCorners();
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[0], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[1], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[0], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[3], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[0], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[4], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[1], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[2], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[1], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[5], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[2], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[3], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[2], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[6], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[3], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[7], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[4], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[5], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[4], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[7], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[5], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[6], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[6], colorToUse));
boundingBoxLines.Add(new VertexPositionColor(boundingBoxCorners[7], colorToUse));
}
if (space.DeactivationManager.SimulationIslands.Count > 0)
{
foreach (var pass in effect.CurrentTechnique.Passes)
{
pass.Apply();
game.GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, boundingBoxLines.Elements, 0, islandBoundingBoxes.Count * 12);
}
}
islandBoundingBoxes.Clear();
boundingBoxLines.Clear();
}
}
void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation)
{
//Compute the surface vertices of the shape.
surfaceVertices.Clear();
try
{
ConvexHullHelper.GetConvexHull(data.vertices, surfaceVertices);
for (int i = 0; i < surfaceVertices.count; i++)
{
AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]);
}
}
catch
{
surfaceVertices.Clear();
//If the convex hull failed, then the point set has no volume. A mobile mesh is allowed to have zero volume, however.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < data.vertices.Length; i++)
{
Vector3 v;
data.GetVertexPosition(i, out v);
if (v.X > box.Max.X)
{
box.Max.X = v.X;
}
if (v.X < box.Min.X)
{
box.Min.X = v.X;
}
if (v.Y > box.Max.Y)
{
box.Max.Y = v.Y;
}
if (v.Y < box.Min.Y)
{
box.Min.Y = v.Y;
}
if (v.Z > box.Max.Z)
{
box.Max.Z = v.Z;
}
if (v.Z < box.Min.Z)
{
box.Min.Z = v.Z;
}
}
//Add the corners. This will overestimate the size of the surface a bit.
surfaceVertices.Add(box.Min);
surfaceVertices.Add(box.Max);
surfaceVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
shapeInformation.Center = new Vector3();
if (solidity == MobileMeshSolidity.Solid)
{
//The following inertia tensor calculation assumes a closed mesh.
shapeInformation.Volume = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
shapeInformation.Volume += tetrahedronVolume;
shapeInformation.Center += tetrahedronVolume * (v2 + v3 + v4);
}
shapeInformation.Center /= shapeInformation.Volume * 4;
shapeInformation.Volume /= 6;
shapeInformation.Volume = Math.Abs(shapeInformation.Volume);
data.worldTransform.Translation -= shapeInformation.Center;
//Source: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates
//http://www.scipub.org/fulltext/jms2/jms2118-11.pdf
//x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3
//Looking to find inertia tensor matrix of the form
// [ a -b' -c' ]
// [ -b' b -a' ]
// [ -c' -a' c ]
float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0;
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
totalWeight += tetrahedronVolume;
a += tetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
b += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
c += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y);
ao += tetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z);
bo += tetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z);
co += tetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y);
}
float density = 1 / totalWeight;
float diagonalFactor = density / 10;
float offFactor = -density / 20;
a *= diagonalFactor;
b *= diagonalFactor;
c *= diagonalFactor;
ao *= offFactor;
bo *= offFactor;
co *= offFactor;
shapeInformation.VolumeDistribution = new Matrix3X3(a, bo, co,
bo, b, ao,
co, ao, c);
}
else
{
shapeInformation.Center = new Vector3();
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
shapeInformation.Center += weight * (vA + vB + vC) / 3;
}
shapeInformation.Center /= totalWeight;
shapeInformation.Volume = 0;
data.worldTransform.Translation -= shapeInformation.Center;
shapeInformation.VolumeDistribution = new Matrix3X3();
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
Matrix3X3 innerProduct;
Matrix3X3.CreateScale(vA.LengthSquared(), out innerProduct);
Matrix3X3 outerProduct;
Matrix3X3.CreateOuterProduct(ref vA, ref vA, out outerProduct);
Matrix3X3 contribution;
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3X3.CreateScale(vB.LengthSquared(), out innerProduct);
Matrix3X3.CreateOuterProduct(ref vB, ref vB, out outerProduct);
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out outerProduct);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3X3.CreateScale(vC.LengthSquared(), out innerProduct);
Matrix3X3.CreateOuterProduct(ref vC, ref vC, out outerProduct);
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
}
Matrix3X3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (6 * totalWeight), out shapeInformation.VolumeDistribution);
}
////Configure the inertia tensor to be local.
//Vector3 finalOffset = shapeInformation.Center;
//Matrix3X3 finalInnerProduct;
//Matrix3X3.CreateScale(finalOffset.LengthSquared(), out finalInnerProduct);
//Matrix3X3 finalOuterProduct;
//Matrix3X3.CreateOuterProduct(ref finalOffset, ref finalOffset, out finalOuterProduct);
//Matrix3X3 finalContribution;
//Matrix3X3.Subtract(ref finalInnerProduct, ref finalOuterProduct, out finalContribution);
//Matrix3X3.Subtract(ref shapeInformation.VolumeDistribution, ref finalContribution, out shapeInformation.VolumeDistribution);
}
//This works in the general case where there can be any number of contacts and candidates. Could specialize it as an optimization to single-contact added incremental manifolds.
///<summary>
/// Reduces the contact manifold to a good subset.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidates">Contact candidates to include in the reduction process.</param>
///<param name="contactsToRemove">Contacts that need to removed to reach the reduced state.</param>
///<param name="toAdd">Contact candidates that should be added to reach the reduced state.</param>
///<exception cref="InvalidOperationException">Thrown when the set being reduced is empty.</exception>
public static void ReduceContacts(RawList <Contact> contacts, RawValueList <ContactData> contactCandidates, RawList <int> contactsToRemove, RawValueList <ContactData> toAdd)
{
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
Vector3 normal = Toolbox.ZeroVector;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.Add(ref normal, ref contacts.Elements[i].Normal, out normal);
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal);
if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = contacts.Count + i;
maximumDepth = contactCandidates.Elements[i].PenetrationDepth;
}
}
//If the normals oppose each other, this can happen. It doesn't need to be normalized, but having SOME normal is necessary.
if (normal.LengthSquared() < Toolbox.Epsilon)
{
if (contacts.Count > 0)
{
normal = contacts.Elements[0].Normal;
}
else if (contactCandidates.Count > 0)
{
normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty.
}
else //This method should not have been called at all if it gets here.
{
throw new ArgumentException("Cannot reduce an empty contact set.");
}
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < contacts.Count)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = contacts.Count + i;
}
}
if (furthestIndex == -1)
{
//Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location.
if (contacts.Count > 0)
{
for (int i = 1; i < contacts.Count; i++)
{
contactsToRemove.Add(i);
}
return;
}
if (contactCandidates.Count > 0)
{
toAdd.Add(ref contactCandidates.Elements[0]);
return;
}
throw new ArgumentException("Cannot reduce an empty contact set.");
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position;
}
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
float dot;
Vector3.Dot(ref contactCandidates.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i + contacts.Count;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i + contacts.Count;
maxYAxisDot = dot;
}
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
for (int i = 0; i < contactCandidates.Count; i++)
{
int totalIndex = i + contacts.Count;
if (totalIndex == deepestIndex || totalIndex == furthestIndex || totalIndex == minYAxisIndex || totalIndex == maxYAxisIndex)
{
//This contact is present in the new manifold. Add it.
toAdd.Add(ref contactCandidates.Elements[i]);
}
}
for (int i = 0; i < contacts.Count; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
contactsToRemove.Add(i);
}
}
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MutableStaticGroupTestDemo(DemosGame game)
: base(game)
{
//Creating a bunch of separate StaticMeshes or kinematic Entity objects for an environment can pollute the broad phase.
//This is because the broad phase implementation doesn't have guarantees about what elements can collide, so it has to
//traverse the acceleration structure all the way down to pairs to figure it out. That can get expensive!
//Individual objects, like StaticMeshes, can have very complicated geometry without hurting the broad phase because the broad phase
//has no knowledge of the thousands of triangles in the mesh. The StaticMesh itself knows that the triangles within the mesh
//never need to collide, so it never needs to test them against each other.
//Similarly, the StaticGroup can be given a bunch of separate collidables. The broad phase doesn't directly know about these child collidables-
//it only sees the StaticGroup. The StaticGroup knows that the things inside it can't ever collide with each other, so no tests are needed.
//This avoids the performance problem!
//To demonstrate, we'll be creating a set of static objects and giving them to a group to manage.
var collidables = new List<Collidable>();
//Start with a whole bunch of boxes. These are entity collidables, but without entities!
float xSpacing = 6;
float ySpacing = 6;
float zSpacing = 6;
//NOTE: You might notice this demo takes a while to start, especially on the Xbox360. Do not fear! That's due to the creation of the graphics data, not the physics.
//The physics can handle over 100,000 static objects pretty easily. The graphics, not so much :)
//Try disabling the game.ModelDrawer.Add() lines and increasing the number of static objects.
int xCount = 15;
int yCount = 7;
int zCount = 15;
var random = new Random(5);
for (int i = 0; i < xCount; i++)
{
for (int j = 0; j < yCount; j++)
{
for (int k = 0; k < zCount; k++)
{
//Create a transform and the instance of the mesh.
var collidable = new ConvexCollidable<BoxShape>(new BoxShape((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f));
//This EntityCollidable isn't associated with an entity, so we must manually tell it where to sit by setting the WorldTransform.
//This also updates its bounding box.
collidable.WorldTransform = new RigidTransform(
new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 3, k * zSpacing - zCount * zSpacing * .5f),
Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100));
collidables.Add(collidable);
}
}
}
//Now create a bunch of instanced meshes too.
xSpacing = 6;
ySpacing = 6;
zSpacing = 6;
xCount = 10;
yCount = 2;
zCount = 10;
Vector3[] vertices;
int[] indices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("fish"), out vertices, out indices);
var meshShape = new InstancedMeshShape(vertices, indices);
for (int i = 0; i < xCount; i++)
{
for (int j = 0; j < yCount; j++)
{
for (int k = 0; k < zCount; k++)
{
//Create a transform and the instance of the mesh.
var transform = new AffineTransform(
new Vector3((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f),
Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100),
new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 50, k * zSpacing - zCount * zSpacing * .5f));
var mesh = new InstancedMesh(meshShape, transform);
//Making the triangles one-sided makes collision detection a bit more robust, since the backsides of triangles won't try to collide with things
//and 'pull' them back into the mesh.
mesh.Sidedness = TriangleSidedness.Counterclockwise;
collidables.Add(mesh);
}
}
}
var ground = new ConvexCollidable<BoxShape>(new BoxShape(200, 1, 200));
ground.WorldTransform = new RigidTransform(new Vector3(0, -3, 0), Quaternion.Identity);
collidables.Add(ground);
var group = new StaticGroup(collidables);
var removed = new RawList<Collidable>();
var contained = new RawList<Collidable>();
group.Shape.CollidableTree.CollectLeaves(contained);
for (int i = 0; i < 100000; ++i)
{
for (int collidableIndex = contained.Count - 1; collidableIndex >= 0; --collidableIndex)
{
if (random.NextDouble() < 0.6)
{
group.Shape.Remove(contained[collidableIndex]);
removed.Add(contained[collidableIndex]);
contained.FastRemoveAt(collidableIndex);
}
}
for (int collidableIndex = removed.Count - 1; collidableIndex >= 0; --collidableIndex)
{
if (random.NextDouble() < 0.4)
{
group.Shape.Add(removed[collidableIndex]);
contained.Add(removed[collidableIndex]);
removed.FastRemoveAt(collidableIndex);
}
}
}
for (int i = 0; i < contained.Count; ++i)
{
game.ModelDrawer.Add(contained[i]);
}
Space.Add(group);
//Create a bunch of dynamic boxes to drop on the staticswarm.
xCount = 8;
yCount = 3;
zCount = 8;
xSpacing = 3f;
ySpacing = 5f;
zSpacing = 3f;
for (int i = 0; i < xCount; i++)
for (int j = 0; j < zCount; j++)
for (int k = 0; k < yCount; k++)
{
Space.Add(new Box(new Vector3(
xSpacing * i - (xCount - 1) * xSpacing / 2f,
100 + k * (ySpacing),
2 + zSpacing * j - (zCount - 1) * zSpacing / 2f),
1, 1, 1, 10));
}
game.Camera.Position = new Vector3(0, 60, 90);
}
internal bool IsHitUnique(RawList<RayHit> hits, ref RayHit hit)
{
for (int i = 0; i < hits.count; i++)
{
if (Math.Abs(hits.Elements[i].T - hit.T) < MeshHitUniquenessThreshold)
return false;
}
hits.Add(hit);
return true;
}
void FindUpStepCandidates(RawList<ContactData> outputStepCandidates, ref Vector3 down)
{
Vector3 movementDirection = HorizontalMotionConstraint.MovementDirection3d;
foreach (var c in SupportFinder.SideContacts)
{
//Check to see if the contact is sufficiently aligned with the movement direction to be considered for stepping.
//TODO: This could behave a bit odd when encountering steps or slopes near the base of rounded collision margin.
var contact = c.Contact;
float dot;
Vector3.Dot(ref contact.Normal, ref movementDirection, out dot);
if (dot > 0)
{
//It is! But is it low enough?
dot = Vector3.Dot(down, c.Contact.Position - characterBody.Position);
//It must be between the bottom of the character and the maximum step height.
if (dot < characterBody.Height * .5f && dot > characterBody.Height * .5f - maximumStepHeight - upStepMargin)
{
//It's a candidate!
//But wait, there's more! Do we already have a candidate that covers this direction?
bool shouldAdd = true;
for (int i = 0; i < outputStepCandidates.Count; i++)
{
Vector3.Dot(ref outputStepCandidates.Elements[i].Normal, ref contact.Normal, out dot);
if (dot > .99f)
{
shouldAdd = false; //Woops! This direction is already covered. Don't bother.
break;
}
}
if (shouldAdd)
outputStepCandidates.Add(contact);
}
}
}
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BroadPhasesTestDemo(DemosGame game)
: base(game)
{
Space.Solver.IterationLimit = 0;
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-50, -50, -50), new Vector3(50, 50, 50));
//DynamicHierarchyOld dhOld = new DynamicHierarchyOld(Space.ThreadManager);
DynamicHierarchy dh = new DynamicHierarchy(Space.ParallelLooper);
SortAndSweep1D sas1d = new SortAndSweep1D(Space.ParallelLooper);
Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep(Space.ParallelLooper);
//DynamicHierarchy dh = new DynamicHierarchy();
//DynamicHierarchy4 dh4 = new DynamicHierarchy4();
//SortAndSweep1D sas1d = new SortAndSweep1D();
//Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep();
//DynamicHierarchy2 dh2 = new DynamicHierarchy2();
//DynamicHierarchy3 dh3 = new DynamicHierarchy3();
//SortAndSweep3D sap3d = new SortAndSweep3D();
RawList<Entity> entities = new RawList<Entity>();
for (int k = 0; k < 100; k++)
{
Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
toAdd = new Box(position, 1, 1, 1, 1);
toAdd.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhasePair;
toAdd.CollisionInformation.UpdateBoundingBox(0);
//Space.Add(toAdd);
//dhOld.Add(toAdd.CollisionInformation);
dh.Add(toAdd.CollisionInformation);
sas1d.Add(toAdd.CollisionInformation);
grid2DSAS.Add(toAdd.CollisionInformation);
entities.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
int numRuns = 10000;
//Prime the system.
grid2DSAS.Update();
sas1d.Update();
//dhOld.Update();
dh.Update();
var testType = Test.Update;
double startTime, endTime;
switch (testType)
{
#region Update Timing
case Test.Update:
for (int i = 0; i < numRuns; i++)
{
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dhOld.Update();
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime += endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
dh.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime += endTime - startTime;
//SAP1D
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
sas1d.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
SAS1Dtime += endTime - startTime;
//Grid2D SOS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAS.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime += endTime - startTime;
//if (sap1d.Overlaps.Count != dh.Overlaps.Count)
// Debug.WriteLine("SAP1D Failure");
//if (grid2DSOS.Overlaps.Count != dh.Overlaps.Count)
// Debug.WriteLine("grid2DSOS Failure");
//for (int j = 0; j < dh2.Overlaps.Count; j++)
//{
// if (!grid2DSOS.Overlaps.Contains(dh2.Overlaps[j]))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!dh2.Overlaps.Contains(grid2DSOS.Overlaps[j]))
// break;
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!dh4.Overlaps.Contains(grid2DSOS.Overlaps[j]))
// break;
//}
//for (int j = 0; j < dh.Overlaps.Count; j++)
//{
// if (!dh.Overlaps[j].EntryA.BoundingBox.Intersects(dh.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < sap1d.Overlaps.Count; j++)
//{
// if (!sap1d.Overlaps[j].EntryA.BoundingBox.Intersects(sap1d.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!grid2DSOS.Overlaps[j].EntryA.BoundingBox.Intersects(grid2DSOS.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Ray cast timing
case Test.RayCast:
float rayLength = 100;
RawList<Ray> rays = new RawList<Ray>();
for (int i = 0; i < numRuns; i++)
{
rays.Add(new Ray()
{
Position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)),
Direction = Vector3.Normalize(new Vector3((float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5)))
});
}
RawList<BroadPhaseEntry> outputIntersections = new RawList<BroadPhaseEntry>();
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//for (int i = 0; i < numRuns; i++)
//{
// dhOld.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
// outputIntersections.Clear();
//}
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime = endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime = endTime - startTime;
//Grid2DSAS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
grid2DSAS.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime = endTime - startTime;
break;
#endregion
#region Bounding box query timing
case Test.BoundingBoxQuery:
float boundingBoxSize = 10;
var boundingBoxes = new RawList<BoundingBox>();
Vector3 offset = new Vector3(boundingBoxSize / 2, boundingBoxSize / 2, boundingBoxSize / 2);
for (int i = 0; i < numRuns; i++)
{
Vector3 center = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
boundingBoxes.Add(new BoundingBox()
{
Min = center - offset,
Max = center + offset
});
}
outputIntersections = new RawList<BroadPhaseEntry>();
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//for (int i = 0; i < numRuns; i++)
//{
// dhOld.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
// outputIntersections.Clear();
//}
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime = endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime = endTime - startTime;
//Grid2DSAS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
grid2DSAS.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime = endTime - startTime;
break;
#endregion
}
DHOldTime /= numRuns;
DH2time /= numRuns;
DH3time /= numRuns;
DHtime /= numRuns;
SAS1Dtime /= numRuns;
grid2DSAStime /= numRuns;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BooleanConvexTestDemo(DemosGame game)
: base(game)
{
var random = new Random();
int numberOfConfigurations = 1000;
int numberOfTestsPerConfiguration = 10000;
float size = 2;
var aPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
size = 1;
var aShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
int pointsInA = 10;
int pointsInB = 10;
RawList<Vector3> points = new RawList<Vector3>();
long accumulatedMPR = 0;
long accumulatedGJK = 0;
long accumulatedGJKSeparatingAxis = 0;
for (int i = 0; i < numberOfConfigurations; i++)
{
//Create two convex hull shapes.
for (int j = 0; j < pointsInA; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref aShapeBounds, out point);
points.Add(point);
}
var a = new ConvexHullShape(points);
points.Clear();
for (int j = 0; j < pointsInB; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref bShapeBounds, out point);
points.Add(point);
}
var b = new ConvexHullShape(points);
points.Clear();
//Generate some random tranforms for the shapes.
RigidTransform aTransform;
var axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
var angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out aTransform.Orientation);
GetRandomPointInBoundingBox(random, ref aPositionBounds, out aTransform.Position);
RigidTransform bTransform;
axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out bTransform.Orientation);
GetRandomPointInBoundingBox(random, ref bPositionBounds, out bTransform.Position);
//Perform MPR tests.
//Warm up the cache a bit.
MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform);
long start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform))
overlapsMPR++;
}
long end = Stopwatch.GetTimestamp();
accumulatedMPR += end - start;
//Perform GJK tests.
//Warm up the cache a bit.
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform))
overlapsGJK++;
}
end = Stopwatch.GetTimestamp();
accumulatedGJK += end - start;
//Perform GJK Separating Axis tests.
//Warm up the cache a bit.
Vector3 localSeparatingAxis = Vector3.Up;
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis))
overlapsGJKSeparatingAxis++;
}
end = Stopwatch.GetTimestamp();
accumulatedGJKSeparatingAxis += end - start;
}
//Compute the actual time per test.
long denominator = Stopwatch.Frequency * numberOfConfigurations * numberOfTestsPerConfiguration;
timeMPR = (double)accumulatedMPR / denominator;
timeGJK = (double)accumulatedGJK / denominator;
timeGJKSeparatingAxis = (double)accumulatedGJKSeparatingAxis / denominator;
}
private static void GetTileAreaOutlines(IReadOnlyGrid<bool> tileArea, Vector2 tileSize, ref List<Vector2[]> outlines)
{
// Initialize the container we'll put our outlines into
if (outlines == null)
outlines = new List<Vector2[]>();
else
outlines.Clear();
// Generate a data structure containing all visible edges
TileEdgeMap edgeMap = new TileEdgeMap(tileArea.Width + 1, tileArea.Height + 1);
for (int y = 0; y < edgeMap.Height; y++)
{
for (int x = 0; x < edgeMap.Width; x++)
{
// Determine highlight state of the four tiles around this node
bool topLeft = x > 0 && y > 0 && tileArea[x - 1, y - 1];
bool topRight = x < tileArea.Width && y > 0 && tileArea[x , y - 1];
bool bottomLeft = x > 0 && y < tileArea.Height && tileArea[x - 1, y ];
bool bottomRight = x < tileArea.Width && y < tileArea.Height && tileArea[x , y ];
// Determine which edges are visible
if (topLeft != topRight ) edgeMap.AddEdge(new Point2(x, y), new Point2(x , y - 1));
if (topRight != bottomRight) edgeMap.AddEdge(new Point2(x, y), new Point2(x + 1, y ));
if (bottomRight != bottomLeft ) edgeMap.AddEdge(new Point2(x, y), new Point2(x , y + 1));
if (bottomLeft != topLeft ) edgeMap.AddEdge(new Point2(x, y), new Point2(x - 1, y ));
}
}
// Traverse edges to form outlines until no more edges are left
RawList<Vector2> outlineBuilder = new RawList<Vector2>();
while (true)
{
// Find the beginning of an outline
Point2 current = edgeMap.FindNonEmpty();
if (current.X == -1 || current.Y == -1) break;
// Traverse it until no more edges are left
while (true)
{
Point2 next = edgeMap.GetClockwiseNextFrom(current);
if (next.X == -1 || next.Y == -1) break;
outlineBuilder.Add(next * tileSize);
edgeMap.RemoveEdge(current, next);
current = next;
}
// Close the loop by adding the first element again
if (outlineBuilder.Count > 0)
outlineBuilder.Add(outlineBuilder[0]);
// If we have enough vertices, keep the outline for drawing
Vector2[] outline = new Vector2[outlineBuilder.Count];
outlineBuilder.CopyTo(outline, 0);
outlines.Add(outline);
// Reset the outline builder to an empty state
outlineBuilder.Clear();
}
}
public static void GetClosestPointOnTriangleToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 p, RawList<Vector3> subsimplex, out Vector3 closestPoint)
{
subsimplex.Clear();
float v, w;
Vector3 ab;
Vector3.Subtract(ref b, ref a, out ab);
Vector3 ac;
Vector3.Subtract(ref c, ref a, out ac);
//Vertex region A?
Vector3 ap;
Vector3.Subtract(ref p, ref a, out ap);
float d1;
Vector3.Dot(ref ab, ref ap, out d1);
float d2;
Vector3.Dot(ref ac, ref ap, out d2);
if (d1 <= 0 && d2 < 0)
{
subsimplex.Add(a);
closestPoint = a;
return;
}
//Vertex region B?
Vector3 bp;
Vector3.Subtract(ref p, ref b, out bp);
float d3;
Vector3.Dot(ref ab, ref bp, out d3);
float d4;
Vector3.Dot(ref ac, ref bp, out d4);
if (d3 >= 0 && d4 <= d3)
{
subsimplex.Add(b);
closestPoint = b;
return;
}
//Edge region AB?
float vc = d1 * d4 - d3 * d2;
if (vc <= 0 && d1 >= 0 && d3 <= 0)
{
subsimplex.Add(a);
subsimplex.Add(b);
v = d1 / (d1 - d3);
Vector3.Multiply(ref ab, v, out closestPoint);
Vector3.Add(ref closestPoint, ref a, out closestPoint);
return;
}
//Vertex region C?
Vector3 cp;
Vector3.Subtract(ref p, ref c, out cp);
float d5;
Vector3.Dot(ref ab, ref cp, out d5);
float d6;
Vector3.Dot(ref ac, ref cp, out d6);
if (d6 >= 0 && d5 <= d6)
{
subsimplex.Add(c);
closestPoint = c;
return;
}
//Edge region AC?
float vb = d5 * d2 - d1 * d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0)
{
subsimplex.Add(a);
subsimplex.Add(c);
w = d2 / (d2 - d6);
Vector3.Multiply(ref ac, w, out closestPoint);
Vector3.Add(ref closestPoint, ref a, out closestPoint);
return;
}
//Edge region BC?
float va = d3 * d6 - d5 * d4;
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0)
{
subsimplex.Add(b);
subsimplex.Add(c);
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
Vector3.Subtract(ref c, ref b, out closestPoint);
Vector3.Multiply(ref closestPoint, w, out closestPoint);
Vector3.Add(ref closestPoint, ref b, out closestPoint);
return;
}
//Inside triangle?
subsimplex.Add(a);
subsimplex.Add(b);
subsimplex.Add(c);
float denom = 1 / (va + vb + vc);
v = vb * denom;
w = vc * denom;
Vector3 abv;
Vector3.Multiply(ref ab, v, out abv);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref a, ref abv, out closestPoint);
Vector3.Add(ref closestPoint, ref acw, out closestPoint);
}
double RunTest(int splitOffset, IParallelLooper parallelLooper)
{
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-500, -500, -500), new Vector3(500, 500, 500));
int splitDepth = splitOffset + (int)Math.Ceiling(Math.Log(parallelLooper.ThreadCount, 2));
DynamicHierarchy dh = new DynamicHierarchy(parallelLooper);
Random rand = new Random(0);
RawList<Entity> entities = new RawList<Entity>();
for (int k = 0; k < 10000; k++)
{
Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
toAdd = new Box(position, 1, 1, 1, 1);
toAdd.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhasePair;
toAdd.CollisionInformation.UpdateBoundingBox(0);
dh.Add(toAdd.CollisionInformation);
entities.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
int numRuns = 3000;
//Prime the system.
dh.Update();
var testType = Test.Update;
BroadPhaseOverlap[] overlapBasis = new BroadPhaseOverlap[dh.Overlaps.Count];
dh.Overlaps.CopyTo(overlapBasis, 0);
double time = 0;
double startTime, endTime;
switch (testType)
{
#region Update Timing
case Test.Update:
for (int i = 0; i < numRuns; i++)
{
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.MultithreadedRefitPhase(splitDepth);
// dh.MultithreadedOverlapPhase(splitDepth);
// }
//}
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.SingleThreadedRefitPhase();
// dh.SingleThreadedOverlapPhase();
// }
//}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Update.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Update.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Refit Timing
case Test.Refit:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedRefitPhase(splitDepth);
//dh.SingleThreadedRefitPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//dh.SingleThreadedOverlapPhase();
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Refit.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Refit.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Overlap Timing
case Test.Overlap:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//dh.MultithreadedRefitPhase(splitDepth);
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedOverlapPhase(splitDepth);
//dh.SingleThreadedOverlapPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Overlap.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Overlap.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Ray cast timing
case Test.RayCast:
float rayLength = 100;
RawList<Ray> rays = new RawList<Ray>();
for (int i = 0; i < numRuns; i++)
{
rays.Add(new Ray()
{
Position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)),
Direction = Vector3.Normalize(new Vector3((float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5)))
});
}
RawList<BroadPhaseEntry> outputIntersections = new RawList<BroadPhaseEntry>();
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time = endTime - startTime;
break;
#endregion
#region Bounding box query timing
case Test.BoundingBoxQuery:
float boundingBoxSize = 10;
var boundingBoxes = new RawList<BoundingBox>();
Vector3 offset = new Vector3(boundingBoxSize / 2, boundingBoxSize / 2, boundingBoxSize / 2);
for (int i = 0; i < numRuns; i++)
{
Vector3 center = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
boundingBoxes.Add(new BoundingBox()
{
Min = center - offset,
Max = center + offset
});
}
outputIntersections = new RawList<BroadPhaseEntry>();
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time = endTime - startTime;
break;
#endregion
}
return time / numRuns;
}
public static void GetClosestPointOnTriangleToPoint(RawList<Vector3> q, int i, int j, int k, ref Vector3 p, RawList<int> subsimplex, RawList<float> baryCoords, out Vector3 closestPoint)
{
subsimplex.Clear();
baryCoords.Clear();
float v, w;
Vector3 a = q[i];
Vector3 b = q[j];
Vector3 c = q[k];
Vector3 ab;
Vector3.Subtract(ref b, ref a, out ab);
Vector3 ac;
Vector3.Subtract(ref c, ref a, out ac);
//Vertex region A?
Vector3 ap;
Vector3.Subtract(ref p, ref a, out ap);
float d1;
Vector3.Dot(ref ab, ref ap, out d1);
float d2;
Vector3.Dot(ref ac, ref ap, out d2);
if (d1 <= 0 && d2 < 0)
{
subsimplex.Add(i);
baryCoords.Add(1);
closestPoint = a;
return; //barycentric coordinates (1,0,0)
}
//Vertex region B?
Vector3 bp;
Vector3.Subtract(ref p, ref b, out bp);
float d3;
Vector3.Dot(ref ab, ref bp, out d3);
float d4;
Vector3.Dot(ref ac, ref bp, out d4);
if (d3 >= 0 && d4 <= d3)
{
subsimplex.Add(j);
baryCoords.Add(1);
closestPoint = b;
return; //barycentric coordinates (0,1,0)
}
//Edge region AB?
float vc = d1 * d4 - d3 * d2;
if (vc <= 0 && d1 >= 0 && d3 <= 0)
{
subsimplex.Add(i);
subsimplex.Add(j);
v = d1 / (d1 - d3);
baryCoords.Add(1 - v);
baryCoords.Add(v);
Vector3.Multiply(ref ab, v, out closestPoint);
Vector3.Add(ref closestPoint, ref a, out closestPoint);
return; //barycentric coordinates (1-v, v, 0)
}
//Vertex region C?
Vector3 cp;
Vector3.Subtract(ref p, ref c, out cp);
float d5;
Vector3.Dot(ref ab, ref cp, out d5);
float d6;
Vector3.Dot(ref ac, ref cp, out d6);
if (d6 >= 0 && d5 <= d6)
{
subsimplex.Add(k);
baryCoords.Add(1);
closestPoint = c;
return; //barycentric coordinates (0,0,1)
}
//Edge region AC?
float vb = d5 * d2 - d1 * d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0)
{
subsimplex.Add(i);
subsimplex.Add(k);
w = d2 / (d2 - d6);
baryCoords.Add(1 - w);
baryCoords.Add(w);
Vector3.Multiply(ref ac, w, out closestPoint);
Vector3.Add(ref closestPoint, ref a, out closestPoint);
return; //barycentric coordinates (1-w, 0, w)
}
//Edge region BC?
float va = d3 * d6 - d5 * d4;
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0)
{
subsimplex.Add(j);
subsimplex.Add(k);
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
baryCoords.Add(1 - w);
baryCoords.Add(w);
Vector3.Subtract(ref c, ref b, out closestPoint);
Vector3.Multiply(ref closestPoint, w, out closestPoint);
Vector3.Add(ref closestPoint, ref b, out closestPoint);
return; //barycentric coordinates (0, 1 - w ,w)
}
//Inside triangle?
subsimplex.Add(i);
subsimplex.Add(j);
subsimplex.Add(k);
float denom = 1 / (va + vb + vc);
v = vb * denom;
w = vc * denom;
baryCoords.Add(1 - v - w);
baryCoords.Add(v);
baryCoords.Add(w);
Vector3 abv;
Vector3.Multiply(ref ab, v, out abv);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref a, ref abv, out closestPoint);
Vector3.Add(ref closestPoint, ref acw, out closestPoint);
//return a + ab * v + ac * w; //barycentric coordinates (1 - v - w, v, w)
}
///<summary>
/// Constructs a compound collidable using additional information about the shapes in the compound.
///</summary>
///<param name="children">Data representing the children of the compound collidable.</param>
public CompoundCollidable(IList<CompoundChildData> children)
{
var shapeList = new RawList<CompoundShapeEntry>();
//Create the shape first.
for (int i = 0; i < children.Count; i++)
{
shapeList.Add(children[i].Entry);
}
base.Shape = new CompoundShape(shapeList);
//Now create the actual child objects.
for (int i = 0; i < children.Count; i++)
{
this.children.Add(GetChild(children[i], i));
}
hierarchy = new CompoundHierarchy(this);
}
//This works in the specific case of 4 contacts and 1 contact candidate.
///<summary>
/// Reduces a 4-contact manifold and contact candidate to 4 total contacts.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidate">Contact candidate to include in the reduction process.</param>
///<param name="toRemove">Contacts that need to be removed to reduce the manifold.</param>
///<param name="addCandidate">Whether or not to add the contact candidate to reach the reduced manifold.</param>
///<exception cref="ArgumentException">Thrown when the contact manifold being reduced doesn't have 4 contacts.</exception>
public static void ReduceContacts(RawList <Contact> contacts, ref ContactData contactCandidate, RawList <int> toRemove, out bool addCandidate)
{
if (contacts.Count != 4)
{
throw new ArgumentException("Can only use this method to reduce contact lists with four contacts and a contact candidate.");
}
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
for (int i = 0; i < 4; i++)
{
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
if (contactCandidate.PenetrationDepth > maximumDepth)
{
deepestIndex = 4;
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < 4)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidate.Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < 4; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
Vector3.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestIndex = 4;
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidate.Position;
}
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref contacts.Elements[0].Normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
float dot;
for (int i = 0; i < 4; i++)
{
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
Vector3.Dot(ref contactCandidate.Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = 4;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = 4;
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
if (4 == deepestIndex || 4 == furthestIndex || 4 == minYAxisIndex || 4 == maxYAxisIndex)
{
addCandidate = true;
//Only reduce when we are going to add a new contact, and only get rid of one.
for (int i = 0; i < 4; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
toRemove.Add(i);
break;
}
}
}
else
{
addCandidate = false;
}
}
///<summary>
/// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape.
///</summary>
///<param name="localSpaceBoundingBox">Bounding box in the local space of the terrain shape.</param>
///<param name="overlappedTriangles">Indices of triangles whose bounding boxes overlap the input bounding box.</param>
public bool GetOverlaps(BoundingBox localBoundingBox, RawList <int> overlappedElements)
{
int width = heights.GetLength(0);
int minX = Math.Max((int)localBoundingBox.Min.X, 0);
int minY = Math.Max((int)localBoundingBox.Min.Z, 0);
int maxX = Math.Min((int)localBoundingBox.Max.X, width - 2);
int maxY = Math.Min((int)localBoundingBox.Max.Z, heights.GetLength(1) - 2);
for (int i = minX; i <= maxX; i++)
{
for (int j = minY; j <= maxY; j++)
{
//Before adding a triangle to the list, make sure the object isn't too high or low from the quad.
float highest, lowest;
float y1 = heights[i, j];
float y2 = heights[i + 1, j];
float y3 = heights[i, j + 1];
float y4 = heights[i + 1, j + 1];
highest = y1;
lowest = y1;
if (y2 > highest)
{
highest = y2;
}
else if (y2 < lowest)
{
lowest = y2;
}
if (y3 > highest)
{
highest = y3;
}
else if (y3 < lowest)
{
lowest = y3;
}
if (y4 > highest)
{
highest = y4;
}
else if (y4 < lowest)
{
lowest = y4;
}
if (localBoundingBox.Max.Y < lowest ||
localBoundingBox.Min.Y > highest)
{
continue;
}
//Now the local bounding box is very likely intersecting those of the triangles.
//Add the triangles to the list.
int quadIndex = (i + j * width) * 2;
overlappedElements.Add(quadIndex);
overlappedElements.Add(quadIndex + 1);
}
}
return(overlappedElements.count > 0);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BroadPhasesTestDemo(DemosGame game)
: base(game)
{
Space.Solver.IterationLimit = 0;
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-50, -50, -50), new Vector3(50, 50, 50));
//DynamicHierarchyOld dhOld = new DynamicHierarchyOld(Space.ThreadManager);
DynamicHierarchy dh = new DynamicHierarchy(Space.ThreadManager);
SortAndSweep1D sas1d = new SortAndSweep1D(Space.ThreadManager);
Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep(Space.ThreadManager);
//DynamicHierarchy dh = new DynamicHierarchy();
//DynamicHierarchy4 dh4 = new DynamicHierarchy4();
//SortAndSweep1D sas1d = new SortAndSweep1D();
//Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep();
//DynamicHierarchy2 dh2 = new DynamicHierarchy2();
//DynamicHierarchy3 dh3 = new DynamicHierarchy3();
//SortAndSweep3D sap3d = new SortAndSweep3D();
RawList <Entity> entities = new RawList <Entity>();
for (int k = 0; k < 100; k++)
{
Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
toAdd = new Box(position, 1, 1, 1, 1);
toAdd.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhasePair;
toAdd.CollisionInformation.UpdateBoundingBox(0);
//Space.Add(toAdd);
//dhOld.Add(toAdd.CollisionInformation);
dh.Add(toAdd.CollisionInformation);
sas1d.Add(toAdd.CollisionInformation);
grid2DSAS.Add(toAdd.CollisionInformation);
entities.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
int numRuns = 10000;
//Prime the system.
grid2DSAS.Update();
sas1d.Update();
//dhOld.Update();
dh.Update();
var testType = Test.Update;
double startTime, endTime;
switch (testType)
{
#region Update Timing
case Test.Update:
for (int i = 0; i < numRuns; i++)
{
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dhOld.Update();
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime += endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
dh.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime += endTime - startTime;
//SAP1D
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
sas1d.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
SAS1Dtime += endTime - startTime;
//Grid2D SOS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAS.Update();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime += endTime - startTime;
//if (sap1d.Overlaps.Count != dh.Overlaps.Count)
// Debug.WriteLine("SAP1D Failure");
//if (grid2DSOS.Overlaps.Count != dh.Overlaps.Count)
// Debug.WriteLine("grid2DSOS Failure");
//for (int j = 0; j < dh2.Overlaps.Count; j++)
//{
// if (!grid2DSOS.Overlaps.Contains(dh2.Overlaps[j]))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!dh2.Overlaps.Contains(grid2DSOS.Overlaps[j]))
// break;
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!dh4.Overlaps.Contains(grid2DSOS.Overlaps[j]))
// break;
//}
//for (int j = 0; j < dh.Overlaps.Count; j++)
//{
// if (!dh.Overlaps[j].EntryA.BoundingBox.Intersects(dh.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < sap1d.Overlaps.Count; j++)
//{
// if (!sap1d.Overlaps[j].EntryA.BoundingBox.Intersects(sap1d.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//for (int j = 0; j < grid2DSOS.Overlaps.Count; j++)
//{
// if (!grid2DSOS.Overlaps[j].EntryA.BoundingBox.Intersects(grid2DSOS.Overlaps[j].EntryB.BoundingBox))
// Debug.WriteLine("Break.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Ray cast timing
case Test.RayCast:
float rayLength = 100;
RawList <Ray> rays = new RawList <Ray>();
for (int i = 0; i < numRuns; i++)
{
rays.Add(new Ray()
{
Position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)),
Direction = Vector3.Normalize(new Vector3((float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5)))
});
}
RawList <BroadPhaseEntry> outputIntersections = new RawList <BroadPhaseEntry>();
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//for (int i = 0; i < numRuns; i++)
//{
// dhOld.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
// outputIntersections.Clear();
//}
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime = endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime = endTime - startTime;
//Grid2DSAS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
grid2DSAS.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime = endTime - startTime;
break;
#endregion
#region Bounding box query timing
case Test.BoundingBoxQuery:
float boundingBoxSize = 10;
var boundingBoxes = new RawList <BoundingBox>();
Vector3 offset = new Vector3(boundingBoxSize / 2, boundingBoxSize / 2, boundingBoxSize / 2);
for (int i = 0; i < numRuns; i++)
{
Vector3 center = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
(float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
(float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
boundingBoxes.Add(new BoundingBox()
{
Min = center - offset,
Max = center + offset
});
}
outputIntersections = new RawList <BroadPhaseEntry>();
////DH
//startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//for (int i = 0; i < numRuns; i++)
//{
// dhOld.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
// outputIntersections.Clear();
//}
//endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//DHOldTime = endTime - startTime;
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
dh.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
DHtime = endTime - startTime;
//Grid2DSAS
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < numRuns; i++)
{
grid2DSAS.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections);
outputIntersections.Clear();
}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
grid2DSAStime = endTime - startTime;
break;
#endregion
}
DHOldTime /= numRuns;
DH2time /= numRuns;
DH3time /= numRuns;
DHtime /= numRuns;
SAS1Dtime /= numRuns;
grid2DSAStime /= numRuns;
}
private void UpdateComponents <T>(Action <T> updateAction) where T : class
{
Profile.TimeUpdateSceneComponents.BeginMeasure();
// Gather a list of updatable Components
RawList <Component> updatableComponents = new RawList <Component>(256);
RawList <UpdateEntry> updateMap = new RawList <UpdateEntry>();
foreach (var pair in this.componentsByType)
{
// Skip Component types that aren't updatable anyway
Component sampleComponent = pair.Value.FirstOrDefault();
if (!(sampleComponent is T))
{
continue;
}
int oldCount = updatableComponents.Count;
// Collect Components
updatableComponents.Reserve(updatableComponents.Count + pair.Value.Count);
for (int i = 0; i < pair.Value.Count; i++)
{
updatableComponents.Add(pair.Value[i]);
}
// Keep in mind how many Components of each type we have in what order
if (updatableComponents.Count - oldCount > 0)
{
updateMap.Add(new UpdateEntry
{
Type = pair.Key,
Count = updatableComponents.Count - oldCount,
Profiler = Profile.RequestCounter <TimeCounter>(Profile.TimeUpdateScene.FullName + @"\" + pair.Key.Name)
});
}
}
// Update all Components. They're still sorted by type.
{
int updateMapIndex = -1;
int updateMapBegin = -1;
TimeCounter activeProfiler = null;
Component[] data = updatableComponents.Data;
UpdateEntry[] updateData = updateMap.Data;
for (int i = 0; i < data.Length; i++)
{
if (i >= updatableComponents.Count)
{
break;
}
// Manage profilers per Component type
if (i == 0 || i - updateMapBegin >= updateData[updateMapIndex].Count)
{
// Note:
// Since we're doing this based on index-count ranges, this needs to be
// done before skipping inactive Components, so we don't run out of sync.
updateMapIndex++;
updateMapBegin = i;
if (activeProfiler != null)
{
activeProfiler.EndMeasure();
}
activeProfiler = updateData[updateMapIndex].Profiler;
activeProfiler.BeginMeasure();
}
// Skip inactive, disposed and detached Components
if (!data[i].Active)
{
continue;
}
// Invoke the Component's update action
updateAction(data[i] as T);
}
if (activeProfiler != null)
{
activeProfiler.EndMeasure();
}
}
Profile.TimeUpdateSceneComponents.EndMeasure();
}
/// <summary>
/// Adds entities associated with the solver item to the involved entities list.
/// Ensure that sortInvolvedEntities() is called at the end of the function.
/// This allows the non-batched multithreading system to lock properly.
/// </summary>
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
outputInvolvedEntities.Add(Body);
foreach (Wheel wheel in Wheels)
{
if (wheel.supportingEntity != null && !outputInvolvedEntities.Contains(wheel.supportingEntity))
outputInvolvedEntities.Add(wheel.supportingEntity);
}
}
public override void UpdateCollision(Fix64 dt)
{
WasContaining = Containing;
WasTouching = Touching;
//Gather current pairs.
UpdateContainedPairs();
//Eliminate old pairs.
foreach (var other in subPairs.Keys)
{
if (!containedPairs.Contains(other))
{
pairsToRemove.Add(other);
}
}
for (int i = 0; i < pairsToRemove.Count; i++)
{
var toReturn = subPairs[pairsToRemove.Elements[i]];
subPairs.Remove(pairsToRemove.Elements[i]);
toReturn.CleanUp();
toReturn.Factory.GiveBack(toReturn);
}
containedPairs.Clear();
pairsToRemove.Clear();
//Scan the pairs in sequence, updating the state as we go.
//Touching can be set to true by a single touching subpair.
Touching = false;
//Containing can be set to false by a single noncontaining or nontouching subpair.
Containing = subPairs.Count > 0;
foreach (var pair in subPairs.Values)
{
//For child convex pairs, we don't need to always perform containment checks.
//Only check if the containment state has not yet been invalidated or a touching state has not been identified.
var convexPair = pair as DetectorVolumeConvexPairHandler;
if (convexPair != null)
{
convexPair.CheckContainment = Containing || !Touching;
}
pair.UpdateCollision(dt);
if (pair.Touching)
{
Touching = true; //If one child is touching, then we are touching too.
}
else
{
Containing = false; //If one child isn't touching, then we aren't containing.
}
if (!pair.Containing) //If one child isn't containing, then we aren't containing.
{
Containing = false;
}
if (!Containing && Touching)
{
//If it's touching but not containing, no further pairs will change the state.
//Containment has been invalidated by something that either didn't touch or wasn't contained.
//Touching has been ensured by at least one object touching.
break;
}
}
NotifyDetectorVolumeOfChanges();
}
///<summary>
/// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape.
///</summary>
///<param name="localSpaceBoundingBox">Bounding box in the local space of the terrain shape.</param>
///<param name="overlappedTriangles">Triangles whose bounding boxes overlap the input bounding box.</param>
public bool GetOverlaps(BoundingBox localSpaceBoundingBox, RawList<TriangleMeshConvexContactManifold.TriangleIndices> overlappedTriangles)
{
int width = heights.GetLength(0);
int minX = Math.Max((int)localSpaceBoundingBox.Min.X, 0);
int minY = Math.Max((int)localSpaceBoundingBox.Min.Z, 0);
int maxX = Math.Min((int)localSpaceBoundingBox.Max.X, width - 2);
int maxY = Math.Min((int)localSpaceBoundingBox.Max.Z, heights.GetLength(1) - 2);
for (int i = minX; i <= maxX; i++)
{
for (int j = minY; j <= maxY; j++)
{
//Before adding a triangle to the list, make sure the object isn't too high or low from the quad.
float highest, lowest;
float y1 = heights[i, j];
float y2 = heights[i + 1, j];
float y3 = heights[i, j + 1];
float y4 = heights[i + 1, j + 1];
highest = y1;
lowest = y1;
if (y2 > highest)
highest = y2;
else if (y2 < lowest)
lowest = y2;
if (y3 > highest)
highest = y3;
else if (y3 < lowest)
lowest = y3;
if (y4 > highest)
highest = y4;
else if (y4 < lowest)
lowest = y4;
if (localSpaceBoundingBox.Max.Y < lowest ||
localSpaceBoundingBox.Min.Y > highest)
continue;
//Now the local bounding box is very likely intersecting those of the triangles.
//Add the triangles to the list.
var indices = new TriangleMeshConvexContactManifold.TriangleIndices();
//v3 v4
//v1 v2
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//v1 v2 v3
indices.A = i + j * width;
indices.B = i + 1 + j * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
//v2 v4 v3
indices.A = i + 1 + j * width;
indices.B = i + 1 + (j + 1) * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
}
else //Bottom right, Upper left
{
//v1 v2 v4
indices.A = i + j * width;
indices.B = i + 1 + j * width;
indices.C = i + 1 + (j + 1) * width;
overlappedTriangles.Add(indices);
//v1 v4 v3
indices.A = i + j * width;
indices.B = i + 1 + (j + 1) * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
}
}
}
return overlappedTriangles.Count > 0;
}
protected override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
var entityCollidable = supportData.SupportObject as EntityCollidable;
if (entityCollidable != null)
outputInvolvedEntities.Add(entityCollidable.Entity);
outputInvolvedEntities.Add(character.Body);
}
internal override void GetMultithreadedOverlaps(Node opposingNode, int splitDepth, int currentDepth, DynamicHierarchy owner, RawList <DynamicHierarchy.NodePair> multithreadingSourceOverlaps)
{
bool intersects;
if (currentDepth == splitDepth)
{
//We've reached the depth where our child comparisons will be multithreaded.
if (this == opposingNode)
{
//We are being compared against ourselves!
//Obviously we're an internal node, so spawn three children:
//A versus A:
if (!childA.IsLeaf) //This is performed in the child method usually by convention, but this saves some time.
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childA, b = childA
});
}
//B versus B:
if (!childB.IsLeaf) //This is performed in the child method usually by convention, but this saves some time.
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childB, b = childB
});
}
//A versus B (if they intersect):
childA.BoundingBox.Intersects(ref childB.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childA, b = childB
});
}
}
else
{
//Two different nodes. The other one may be a leaf.
if (opposingNode.IsLeaf)
{
//If it's a leaf, go deeper in our hierarchy, but not the opposition.
childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childA, b = opposingNode
});
}
childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childB, b = opposingNode
});
}
}
else
{
var opposingChildA = opposingNode.ChildA;
var opposingChildB = opposingNode.ChildB;
//If it's not a leaf, try to go deeper in both hierarchies.
childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childA, b = opposingChildA
});
}
childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childA, b = opposingChildB
});
}
childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childB, b = opposingChildA
});
}
childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects);
if (intersects)
{
multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair()
{
a = childB, b = opposingChildB
});
}
}
}
return;
}
if (this == opposingNode)
{
//We are being compared against ourselves!
//Obviously we're an internal node, so spawn three children:
//A versus A:
if (!childA.IsLeaf) //This is performed in the child method usually by convention, but this saves some time.
{
childA.GetMultithreadedOverlaps(childA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
//B versus B:
if (!childB.IsLeaf) //This is performed in the child method usually by convention, but this saves some time.
{
childB.GetMultithreadedOverlaps(childB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
//A versus B (if they intersect):
childA.BoundingBox.Intersects(ref childB.BoundingBox, out intersects);
if (intersects)
{
childA.GetMultithreadedOverlaps(childB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
}
else
{
//Two different nodes. The other one may be a leaf.
if (opposingNode.IsLeaf)
{
//If it's a leaf, go deeper in our hierarchy, but not the opposition.
childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects);
if (intersects)
{
childA.GetMultithreadedOverlaps(opposingNode, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects);
if (intersects)
{
childB.GetMultithreadedOverlaps(opposingNode, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
}
else
{
var opposingChildA = opposingNode.ChildA;
var opposingChildB = opposingNode.ChildB;
//If it's not a leaf, try to go deeper in both hierarchies.
childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects);
if (intersects)
{
childA.GetMultithreadedOverlaps(opposingChildA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects);
if (intersects)
{
childA.GetMultithreadedOverlaps(opposingChildB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects);
if (intersects)
{
childB.GetMultithreadedOverlaps(opposingChildA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects);
if (intersects)
{
childB.GetMultithreadedOverlaps(opposingChildB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps);
}
}
}
}
protected void UpdateLabel()
{
SetString(m_sInitialString, true);
if (m_fWidth > 0)
{
// Step 1: Make multiline
string str_whole = m_sString;
int stringLength = m_sString.Length;
var multiline_string = new StringBuilder(stringLength);
var last_word = new StringBuilder(stringLength);
int line = 1, i = 0;
bool start_line = false, start_word = false;
float startOfLine = -1, startOfWord = -1;
int skip = 0;
RawList<CCNode> children = m_pChildren;
for (int j = 0; j < children.count; j++)
{
CCSprite characterSprite;
while ((characterSprite = (CCSprite) GetChildByTag(j + skip)) == null)
{
skip++;
}
if (!characterSprite.Visible)
{
continue;
}
if (i >= stringLength)
{
break;
}
char character = str_whole[i];
if (!start_word)
{
startOfWord = GetLetterPosXLeft(characterSprite);
start_word = true;
}
if (!start_line)
{
startOfLine = startOfWord;
start_line = true;
}
// Newline.
if (character == '\n')
{
int len = last_word.Length;
while (len > 0 && Char.IsWhiteSpace(last_word[len - 1]))
{
len--;
last_word.Remove(len, 1);
}
multiline_string.Append(last_word);
multiline_string.Append('\n');
#if XBOX || XBOX360
last_word.Length = 0;
#else
last_word.Clear();
#endif
start_word = false;
start_line = false;
startOfWord = -1;
startOfLine = -1;
i++;
line++;
if (i >= stringLength)
break;
character = str_whole[i];
if (startOfWord == 0)
{
startOfWord = GetLetterPosXLeft(characterSprite);
start_word = true;
}
if (startOfLine == 0)
{
startOfLine = startOfWord;
start_line = true;
}
}
// Whitespace.
if (Char.IsWhiteSpace(character))
{
last_word.Append(character);
multiline_string.Append(last_word);
#if XBOX || XBOX360
last_word.Length = 0;
#else
last_word.Clear();
#endif
start_word = false;
startOfWord = -1;
i++;
continue;
}
// Out of bounds.
if (GetLetterPosXRight(characterSprite) - startOfLine > m_fWidth)
{
if (!m_bLineBreakWithoutSpaces)
{
last_word.Append(character);
int len = multiline_string.Length;
while (len > 0 && Char.IsWhiteSpace(multiline_string[len - 1]))
{
len--;
multiline_string.Remove(len, 1);
}
if (multiline_string.Length > 0)
{
multiline_string.Append('\n');
}
line++;
start_line = false;
startOfLine = -1;
i++;
}
else
{
int len = last_word.Length;
while (len > 0 && Char.IsWhiteSpace(last_word[len - 1]))
{
len--;
last_word.Remove(len, 1);
}
multiline_string.Append(last_word);
multiline_string.Append('\n');
#if XBOX || XBOX360
last_word.Length = 0;
#else
last_word.Clear();
#endif
start_word = false;
start_line = false;
startOfWord = -1;
startOfLine = -1;
line++;
if (i >= stringLength)
break;
if (startOfWord == 0)
{
startOfWord = GetLetterPosXLeft(characterSprite);
start_word = true;
}
if (startOfLine == 0)
{
startOfLine = startOfWord;
start_line = true;
}
j--;
}
continue;
}
else
{
// Character is normal.
last_word.Append(character);
i++;
continue;
}
}
multiline_string.Append(last_word);
m_sString = multiline_string.ToString();
UpdateString(true);
}
// Step 2: Make alignment
if (m_pAlignment != CCTextAlignment.CCTextAlignmentLeft)
{
int i = 0;
int lineNumber = 0;
int str_len = m_sString.Length;
var last_line = new RawList<char>();
for (int ctr = 0; ctr <= str_len; ++ctr)
{
if (ctr == str_len || m_sString[ctr] == '\n')
{
float lineWidth = 0.0f;
int line_length = last_line.Count;
// if last line is empty we must just increase lineNumber and work with next line
if (line_length == 0)
{
lineNumber++;
continue;
}
int index = i + line_length - 1 + lineNumber;
if (index < 0) continue;
var lastChar = (CCSprite) GetChildByTag(index);
if (lastChar == null)
continue;
lineWidth = lastChar.Position.X + lastChar.ContentSize.Width / 2.0f;
float shift = 0;
switch (m_pAlignment)
{
case CCTextAlignment.CCTextAlignmentCenter:
shift = ContentSize.Width / 2.0f - lineWidth / 2.0f;
break;
case CCTextAlignment.CCTextAlignmentRight:
shift = ContentSize.Width - lineWidth;
break;
default:
break;
}
if (shift != 0)
{
for (int j = 0; j < line_length; j++)
{
index = i + j + lineNumber;
if (index < 0) continue;
var characterSprite = (CCSprite) GetChildByTag(index);
characterSprite.Position = characterSprite.Position + new CCPoint(shift, 0.0f);
}
}
i += line_length;
lineNumber++;
last_line.Clear();
continue;
}
last_line.Add(m_sString[ctr]);
}
}
}
///<summary>
/// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape.
///</summary>
///<param name="localSpaceBoundingBox">Bounding box in the local space of the terrain shape.</param>
///<param name="overlappedTriangles">Triangles whose bounding boxes overlap the input bounding box.</param>
public bool GetOverlaps(BoundingBox localSpaceBoundingBox, RawList <TriangleMeshConvexContactManifold.TriangleIndices> overlappedTriangles)
{
int width = heights.GetLength(0);
int minX = Math.Max((int)localSpaceBoundingBox.Min.X, 0);
int minY = Math.Max((int)localSpaceBoundingBox.Min.Z, 0);
int maxX = Math.Min((int)localSpaceBoundingBox.Max.X, width - 2);
int maxY = Math.Min((int)localSpaceBoundingBox.Max.Z, heights.GetLength(1) - 2);
for (int i = minX; i <= maxX; i++)
{
for (int j = minY; j <= maxY; j++)
{
//Before adding a triangle to the list, make sure the object isn't too high or low from the quad.
float highest, lowest;
float y1 = heights[i, j];
float y2 = heights[i + 1, j];
float y3 = heights[i, j + 1];
float y4 = heights[i + 1, j + 1];
highest = y1;
lowest = y1;
if (y2 > highest)
{
highest = y2;
}
else if (y2 < lowest)
{
lowest = y2;
}
if (y3 > highest)
{
highest = y3;
}
else if (y3 < lowest)
{
lowest = y3;
}
if (y4 > highest)
{
highest = y4;
}
else if (y4 < lowest)
{
lowest = y4;
}
if (localSpaceBoundingBox.Max.Y < lowest ||
localSpaceBoundingBox.Min.Y > highest)
{
continue;
}
//Now the local bounding box is very likely intersecting those of the triangles.
//Add the triangles to the list.
var indices = new TriangleMeshConvexContactManifold.TriangleIndices();
//v3 v4
//v1 v2
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//v1 v2 v3
indices.A = i + j * width;
indices.B = i + 1 + j * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
//v2 v4 v3
indices.A = i + 1 + j * width;
indices.B = i + 1 + (j + 1) * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
}
else //Bottom right, Upper left
{
//v1 v2 v4
indices.A = i + j * width;
indices.B = i + 1 + j * width;
indices.C = i + 1 + (j + 1) * width;
overlappedTriangles.Add(indices);
//v1 v4 v3
indices.A = i + j * width;
indices.B = i + 1 + (j + 1) * width;
indices.C = i + (j + 1) * width;
overlappedTriangles.Add(indices);
}
}
}
return(overlappedTriangles.count > 0);
}
//This works in the general case where there can be any number of contacts and candidates. Could specialize it as an optimization to single-contact added incremental manifolds.
///<summary>
/// Reduces the contact manifold to a good subset.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidates">Contact candidates to include in the reduction process.</param>
///<param name="contactsToRemove">Contacts that need to removed to reach the reduced state.</param>
///<param name="toAdd">Contact candidates that should be added to reach the reduced state.</param>
///<exception cref="InvalidOperationException">Thrown when the set being reduced is empty.</exception>
public static void ReduceContacts(RawList<Contact> contacts, ref QuickList<ContactData> contactCandidates, RawList<int> contactsToRemove, ref QuickList<ContactData> toAdd)
{
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
Vector3 normal = Toolbox.ZeroVector;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.Add(ref normal, ref contacts.Elements[i].Normal, out normal);
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal);
if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = contacts.Count + i;
maximumDepth = contactCandidates.Elements[i].PenetrationDepth;
}
}
//If the normals oppose each other, this can happen. It doesn't need to be normalized, but having SOME normal is necessary.
if (normal.LengthSquared() < Toolbox.Epsilon)
if (contacts.Count > 0)
normal = contacts.Elements[0].Normal;
else if (contactCandidates.Count > 0)
normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty.
else //This method should not have been called at all if it gets here.
throw new ArgumentException("Cannot reduce an empty contact set.");
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
Vector3 deepestPosition;
if (deepestIndex < contacts.Count)
deepestPosition = contacts.Elements[deepestIndex].Position;
else
deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position;
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = contacts.Count + i;
}
}
if (furthestIndex == -1)
{
//Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location.
if (contacts.Count > 0)
{
for (int i = 1; i < contacts.Count; i++)
{
contactsToRemove.Add(i);
}
return;
}
if (contactCandidates.Count > 0)
{
toAdd.Add(ref contactCandidates.Elements[0]);
return;
}
throw new ArgumentException("Cannot reduce an empty contact set.");
}
Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
furthestPosition = contacts.Elements[furthestIndex].Position;
else
furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position;
Vector3 xAxis;
Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
Vector3 yAxis;
Vector3.Cross(ref xAxis, ref normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
float dot;
Vector3.Dot(ref contactCandidates.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i + contacts.Count;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i + contacts.Count;
maxYAxisDot = dot;
}
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
for (int i = 0; i < contactCandidates.Count; i++)
{
int totalIndex = i + contacts.Count;
if (totalIndex == deepestIndex || totalIndex == furthestIndex || totalIndex == minYAxisIndex || totalIndex == maxYAxisIndex)
{
//This contact is present in the new manifold. Add it.
toAdd.Add(ref contactCandidates.Elements[i]);
}
}
for (int i = 0; i < contacts.Count; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
contactsToRemove.Add(i);
}
}
}
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
//This should never really have to be called.
if (entityA != null)
outputInvolvedEntities.Add(entityA);
if (entityB != null)
outputInvolvedEntities.Add(entityB);
}
protected RawList<ImportInputAssignment> SelectImporter(AssetImportEnvironment env)
{
if (!env.IsPrepareStep) throw new ArgumentException(
"The specified import environment must be configured as a preparation environment.",
"env");
// Find an importer to handle some or all of the unhandled input files
RawList<ImportInputAssignment> candidateMapping = new RawList<ImportInputAssignment>();
foreach (IAssetImporter importer in AssetManager.Importers)
{
env.ResetAcquiredData();
try
{
importer.PrepareImport(env);
}
catch (Exception ex)
{
Log.Editor.WriteError("An error occurred in the preparation step of '{1}': {0}",
Log.Exception(ex),
Log.Type(importer.GetType()));
continue;
}
if (env.HandledInput.Any())
{
candidateMapping.Add(new ImportInputAssignment
{
Importer = importer,
HandledInput = env.HandledInput.ToArray(),
ExpectedOutput = env.Output.ToArray()
});
}
}
// Sort candidate mapping from most files to least files, so we can solve the biggest conflicts first
candidateMapping.Sort((a, b) => b.HandledInput.Length - a.HandledInput.Length);
// Determine if multiple importers intend to handle the same files and resolve conflicts
List<int> conflictingIndices = new List<int>();
List<string> conflictingFiles = new List<string>();
for (int mainIndex = 0; mainIndex < candidateMapping.Count; mainIndex++)
{
ImportInputAssignment assignment = candidateMapping[mainIndex];
// Find all conflicts related to this assignment
conflictingIndices.Clear();
conflictingFiles.Clear();
for (int secondIndex = 0; secondIndex < candidateMapping.Count; secondIndex++)
{
if (secondIndex == mainIndex) continue;
ImportInputAssignment conflictAssignment = candidateMapping[secondIndex];
IEnumerable<string> mainFiles = assignment.HandledInput.Select(item => item.Path);
IEnumerable<string> secondFiles = conflictAssignment.HandledInput.Select(item => item.Path);
string[] conflicts = mainFiles.Intersect(secondFiles).ToArray();
if (conflicts.Length > 0)
{
if (conflictingIndices.Count == 0) conflictingIndices.Add(mainIndex);
conflictingIndices.Add(secondIndex);
conflictingFiles.AddRange(conflicts);
}
}
// Resolve conflicts with this assignment
if (conflictingIndices.Count > 0)
{
// Determine which importer to prefer for this conflict
ImportInputAssignment[] conflictingAssignments = conflictingIndices.Select(i => candidateMapping[i]).ToArray();
int keepIndex = this.ResolveMappingConflict(conflictingAssignments);
// If we somehow decided that none of the options is viable, abort the operation
if (keepIndex == -1)
{
candidateMapping.Clear();
return candidateMapping;
}
// Sort indices to remove in declining order and remove their mappings
conflictingIndices.Remove(keepIndex);
conflictingIndices.Sort((a, b) => b - a);
foreach (int index in conflictingIndices)
{
candidateMapping.RemoveAt(index);
}
// Start over with the conflict search
mainIndex = -1;
continue;
}
}
return candidateMapping;
}
///<summary>
/// Adds a connection reference to the member.
///</summary>
///<param name="connection">Reference to add.</param>
///<returns>Index of the connection in the member's list.</returns>
internal int AddConnectionReference(SimulationIslandConnection connection)
{
connections.Add(connection);
return(connections.Count - 1);
}
public static void GetClosestPointOnTetrahedronToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, ref Vector3 p, RawList<Vector3> subsimplex, out Vector3 closestPoint)
{
// Start out assuming point inside all halfspaces, so closest to itself
subsimplex.Clear();
subsimplex.Add(a); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself.
subsimplex.Add(b);
subsimplex.Add(c);
subsimplex.Add(d);
closestPoint = p;
Vector3 pq;
Vector3 q;
float bestSqDist = float.MaxValue;
// If point outside face abc then compute closest point on abc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c))
{
GetClosestPointOnTriangleToPoint(ref a, ref b, ref c, ref p, subsimplex, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
}
}
// Repeat test for face acd
if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d))
{
GetClosestPointOnTriangleToPoint(ref a, ref c, ref d, ref p, subsimplex, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
}
}
// Repeat test for face adb
if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b))
{
GetClosestPointOnTriangleToPoint(ref a, ref d, ref b, ref p, subsimplex, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
}
}
// Repeat test for face bdc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c))
{
GetClosestPointOnTriangleToPoint(ref b, ref d, ref c, ref p, subsimplex, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
if (sqDist < bestSqDist)
{
closestPoint = q;
}
}
}
private static void GenerateCollisionShapes(TileEdgeMap edgeMap, Vector2 origin, Vector2 tileSize, bool roundedCorners, IList <ShapeInfo> shapeList)
{
// Traverse the edge map and gradually create chain / loop
// shapes until all edges have been used.
RawList <Point2> currentChain = new RawList <Point2>();
RawList <Vector2> vertexBuffer = new RawList <Vector2>();
while (true)
{
// Begin a new continuous chain of nodes
currentChain.Clear();
// Find a starting node for our current chain.
// If there is none, we found and handled all edges.
Point2 start = edgeMap.FindNonEmpty();
if (start == new Point2(-1, -1))
{
break;
}
// Traverse the current chain node-by-node from the start we found
Point2 current = start;
while (true)
{
// Add the current node to our continuous chain
currentChain.Add(current);
// Find the next node that connects to the current one.
// If there is none, our current chain is done.
Point2 next = edgeMap.GetClockwiseNextFrom(current);
if (next == new Point2(-1, -1))
{
break;
}
// Remove the edge we used to get to the next node
edgeMap.RemoveEdge(current, next);
// Use the next node as origin for traversing further
current = next;
}
// Generate a shape from the current chain
bool isLoop = (start == currentChain[currentChain.Count - 1]);
if (isLoop)
{
currentChain.RemoveAt(currentChain.Count - 1);
}
vertexBuffer.Clear();
// Rounded corners
if (roundedCorners && currentChain.Count >= 3)
{
vertexBuffer.Reserve(currentChain.Count * 2);
vertexBuffer.Count = 0;
for (int i = 0; i < currentChain.Count; i++)
{
int prevIndex = (i - 1 + currentChain.Count) % currentChain.Count;
int nextIndex = (i + 1) % currentChain.Count;
Vector2 currentVert = origin + tileSize * (Vector2)currentChain[i];
Vector2 prevVert = origin + tileSize * (Vector2)currentChain[prevIndex];
Vector2 nextVert = origin + tileSize * (Vector2)currentChain[nextIndex];
if (nextVert - currentVert != currentVert - prevVert)
{
if (!isLoop && (i == 0 || i == currentChain.Count - 1))
{
vertexBuffer.Add(currentVert);
}
else
{
vertexBuffer.Add(currentVert + (prevVert - currentVert).Normalized * tileSize * 0.2f);
vertexBuffer.Add(currentVert + (nextVert - currentVert).Normalized * tileSize * 0.2f);
}
}
}
}
// Sharp corners
else
{
vertexBuffer.Reserve(currentChain.Count);
vertexBuffer.Count = 0;
for (int i = 0; i < currentChain.Count; i++)
{
int prevIndex = (i - 1 + currentChain.Count) % currentChain.Count;
int nextIndex = (i + 1) % currentChain.Count;
Vector2 currentVert = origin + tileSize * (Vector2)currentChain[i];
Vector2 prevVert = origin + tileSize * (Vector2)currentChain[prevIndex];
Vector2 nextVert = origin + tileSize * (Vector2)currentChain[nextIndex];
if (nextVert - currentVert != currentVert - prevVert)
{
vertexBuffer.Add(currentVert);
}
}
}
Vector2[] vertices = new Vector2[vertexBuffer.Count];
vertexBuffer.CopyTo(vertices, 0);
shapeList.Add(isLoop ?
(ShapeInfo) new LoopShapeInfo(vertices) :
(ShapeInfo) new ChainShapeInfo(vertices));
}
}
public static void GetClosestPointOnTetrahedronToPoint(RawList<Vector3> tetrahedron, ref Vector3 p, RawList<int> subsimplex, RawList<float> baryCoords, out Vector3 closestPoint)
{
var subsimplexCandidate = CommonResources.GetIntList();
var baryCoordsCandidate = CommonResources.GetFloatList();
Vector3 a = tetrahedron[0];
Vector3 b = tetrahedron[1];
Vector3 c = tetrahedron[2];
Vector3 d = tetrahedron[3];
closestPoint = p;
Vector3 pq;
float bestSqDist = float.MaxValue;
subsimplex.Clear();
subsimplex.Add(0); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself.
subsimplex.Add(1);
subsimplex.Add(2);
subsimplex.Add(3);
baryCoords.Clear();
Vector3 q;
bool baryCoordsFound = false;
// If point outside face abc then compute closest point on abc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 1, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face acd
if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 2, 3, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face adb
if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 3, 1, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face bdc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 1, 3, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
if (!baryCoordsFound)
{
//subsimplex is the entire tetrahedron, can only occur when objects intersect! Determinants of each of the tetrahedrons based on triangles composing the sides and the point itself.
//This is basically computing the volume of parallelepipeds (triple scalar product).
//Could be quicker just to do it directly.
float abcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float pbcd = (new Matrix(p.X, p.Y, p.Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float apcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
p.X, p.Y, p.Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float abpd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
p.X, p.Y, p.Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
abcd = 1 / abcd;
baryCoords.Add(pbcd * abcd); //u
baryCoords.Add(apcd * abcd); //v
baryCoords.Add(abpd * abcd); //w
baryCoords.Add(1 - baryCoords[0] - baryCoords[1] - baryCoords[2]); //x = 1-u-v-w
}
CommonResources.GiveBack(subsimplexCandidate);
CommonResources.GiveBack(baryCoordsCandidate);
}
private static void ScanObject(float rayIncrement, float maxLength, ref Vector3 increment1, ref Vector3 increment2, ref Ray ray, ref RayHit startHit, ref RayHit endHit, RawList<Vector3> pointContributions, out float volume)
{
Vector3 cell;
Vector3.Multiply(ref ray.Direction, rayIncrement, out cell);
Vector3.Add(ref increment1, ref cell, out cell);
Vector3.Add(ref increment2, ref cell, out cell);
float perCellVolume = cell.X * cell.Y * cell.Z;
volume = 0;
for (int i = (int)(startHit.T / rayIncrement); i <= (int)((maxLength - endHit.T) / rayIncrement); i++)
{
Vector3 position;
Vector3.Multiply(ref ray.Direction, (i + .5f) * rayIncrement, out position);
Vector3.Add(ref position, ref ray.Position, out position);
pointContributions.Add(position);
volume += perCellVolume;
}
}
/// <summary>
/// Adds entities associated with the solver item to the involved entities list.
/// Ensure that sortInvolvedEntities() is called at the end of the function.
/// This allows the non-batched multithreading system to lock properly.
/// </summary>
protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
{
if (entity != null) //sometimes, the entity is set to null to 'deactivate' it. Don't add null to the involved entities list.
outputInvolvedEntities.Add(entity);
}