/// <summary>
/// Identifies the points on the surface of hull.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputSurfacePoints">Unique points on the surface of the convex hull.</param>
public static void GetConvexHull(IList<Vector3> points, IList<Vector3> outputSurfacePoints)
{
var rawPoints = new QuickList<Vector3>(BufferPools<Vector3>.Locking, BufferPool.GetPoolIndex(points.Count));
rawPoints.AddRange(points);
GetConvexHull(ref rawPoints, outputSurfacePoints);
rawPoints.Dispose();
}
/// <summary>
/// Removes redundant points. Two points are redundant if they occupy the same hash grid cell.
/// </summary>
/// <param name="points">List of points to prune.</param>
/// <param name="cellSize">Size of cells to determine redundancy.</param>
public static void RemoveRedundantPoints(ref QuickList<Vector3> points, double cellSize)
{
var set = new QuickSet<Int3>(BufferPools<Int3>.Locking, BufferPools<int>.Locking, BufferPool.GetPoolIndex(points.Count));
for (int i = points.Count - 1; i >= 0; --i)
{
var element = points.Elements[i];
var cell = new Int3
{
X = (int)Math.Floor(element.X / cellSize),
Y = (int)Math.Floor(element.Y / cellSize),
Z = (int)Math.Floor(element.Z / cellSize)
};
if (set.Contains(cell))
{
points.FastRemoveAt(i);
}
else
{
set.Add(cell);
//TODO: Consider adding adjacent cells to guarantee that a point on the border between two cells will still detect the presence
//of a point on the opposite side of that border.
}
}
set.Dispose();
}
public void GetOverlaps( Vector3 gridPosition, BoundingBox boundingBox, ref QuickList<Int3> overlaps )
{
Vector3.Subtract( ref boundingBox.Min, ref gridPosition, out boundingBox.Min );
Vector3.Subtract( ref boundingBox.Max, ref gridPosition, out boundingBox.Max );
var inverseWidth = 1f / CellWidth;
var min = new Int3
{
X = Math.Max( 0, (uint)( boundingBox.Min.X * inverseWidth ) ),
Y = Math.Max( 0, (uint)( boundingBox.Min.Y * inverseWidth ) ),
Z = Math.Max( 0, (uint)( boundingBox.Min.Z * inverseWidth ) )
};
var max = new Int3
{
X = Math.Min( VoxelSector.ZVOXELBLOCSIZE_X - 1, (uint)( boundingBox.Max.X * inverseWidth ) ),
Y = Math.Min( VoxelSector.ZVOXELBLOCSIZE_Y - 1, (uint)( boundingBox.Max.Y * inverseWidth ) ),
Z = Math.Min( VoxelSector.ZVOXELBLOCSIZE_Z - 1, (uint)( boundingBox.Max.Z * inverseWidth ) )
};
for( uint i = min.X; i <= max.X; ++i )
{
for( uint j = min.Y; j <= max.Y; ++j )
{
for( uint k = min.Z; k <= max.Z; ++k )
{
uint offset = i * VoxelSector.ZVOXELBLOCSIZE_Y + j + k * VoxelSector.ZVOXELBLOCSIZE_X * VoxelSector.ZVOXELBLOCSIZE_Y;
if( Cells[offset] != VoxelShape.Empty )
{
overlaps.Add( new Int3 { X = i, Y = j, Z = k } );
}
}
}
}
}
bool IsDownStepObstructed(ref QuickList<CharacterContact> sideContacts)
{
//A contact is considered obstructive if its projected depth is deeper than any existing contact along the existing contacts' normals.
for (int i = 0; i < sideContacts.Count; i++)
{
if (IsObstructiveToDownStepping(ref sideContacts.Elements[i].Contact))
return true;
}
return false;
}
public unsafe void CollectSubtreesDirect(int nodeIndex, int maximumSubtrees, ref QuickList<int> subtrees, ref QuickQueue<int> internalNodes, out float treeletCost)
{
//TODO: doesn't work for non-node2.
Debug.Assert(ChildrenCapacity == 2, "If you're going to use >2ary trees, you need to correct the maximum depth. Probably better to use explicit recursion depth as input...");
var maximumDepth = BufferPool<int>.GetPoolIndex(maximumSubtrees) - 1;
Debug.Assert(maximumDepth > 0);
//Cost excludes the treelet root, since refinement can't change the treelet root's size. So don't bother including it in treeletCost.
CollectSubtreesForNodeDirect(nodeIndex, maximumDepth, ref subtrees, ref internalNodes, out treeletCost);
}
/// <summary>
/// Removes redundant points. Two points are redundant if they occupy the same hash grid cell.
/// </summary>
/// <param name="points">List of points to prune.</param>
/// <param name="cellSize">Size of cells to determine redundancy.</param>
public static void RemoveRedundantPoints(IList<Vector3> points, double cellSize)
{
var rawPoints = new QuickList<Vector3>(BufferPools<Vector3>.Locking, BufferPool.GetPoolIndex(points.Count));
rawPoints.AddRange(points);
RemoveRedundantPoints(ref rawPoints, cellSize);
points.Clear();
for (int i = 0; i < rawPoints.Count; ++i)
{
points.Add(rawPoints.Elements[i]);
}
rawPoints.Dispose();
}
/// <summary>
/// Identifies the points on the surface of hull.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
/// <param name="outputSurfacePoints">Unique points on the surface of the convex hull.</param>
public static void GetConvexHull(IList<Vector3> points, IList<int> outputTriangleIndices, IList<Vector3> outputSurfacePoints)
{
var rawPoints = new QuickList<Vector3>(BufferPools<Vector3>.Locking, BufferPool.GetPoolIndex(points.Count));
var rawIndices = new QuickList<int>(BufferPools<int>.Locking, BufferPool.GetPoolIndex(points.Count * 3));
rawPoints.AddRange(points);
GetConvexHull(ref rawPoints, ref rawIndices, outputSurfacePoints);
rawPoints.Dispose();
for (int i = 0; i < rawIndices.Count; i++)
{
outputTriangleIndices.Add(rawIndices[i]);
}
rawIndices.Dispose();
}
/// <summary>
/// Identifies the points on the surface of hull.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
/// <param name="outputSurfacePoints">Unique points on the surface of the convex hull.</param>
public static void GetConvexHull(ref QuickList <Vector3> points, ref QuickList <int> outputTriangleIndices, IList <Vector3> outputSurfacePoints)
{
GetConvexHull(ref points, ref outputTriangleIndices);
var alreadyContainedIndices = new QuickSet <int>(BufferPools <int> .Locking, BufferPools <int> .Locking);
for (int i = outputTriangleIndices.Count - 1; i >= 0; i--)
{
int index = outputTriangleIndices[i];
if (alreadyContainedIndices.Add(index))
{
outputSurfacePoints.Add(points[index]);
}
}
alreadyContainedIndices.Dispose();
}
private static void VerifyWindings(ref QuickList <int> newIndices, ref QuickList <Vector3> points, ref Vector3 centroid)
{
//Go through every triangle.
for (int k = 0; k < newIndices.Count; k += 3)
{
//Check if the triangle faces away or towards the centroid.
if (IsTriangleVisibleFromPoint(ref newIndices, ref points, k, ref centroid))
{
//If it's towards, flip the winding.
int temp = newIndices[k + 1];
newIndices[k + 1] = newIndices[k + 2];
newIndices[k + 2] = temp;
}
}
}
private static int GetExtremePoint(ref Vector3 direction, ref QuickList <Vector3> points, ref QuickList <int> outsidePoints)
{
float maximumDot = -float.MaxValue;
int extremeIndex = 0;
for (int i = 0; i < outsidePoints.Count; ++i)
{
float dot = Vector3.Dot(points.Elements[outsidePoints[i]], direction);
if (dot > maximumDot)
{
maximumDot = dot;
extremeIndex = i;
}
}
return(extremeIndex);
}
public void SequenceRemove()
{
var master = new QuickList <int>(20);
for (var i = 0; i < 256; i++)
{
master.Push(i);
}
master.Remove(5);
master.Remove(6);
Assert.AreEqual(7, master[5]);
Assert.AreEqual(8, master[6]);
Assert.AreEqual(254, master.Count);
}
public QuickList <T> Get <T>() where T : IPartitionObject
{
lock (tileLock) {
if (!PartitionObjects.TryGetValue(typeof(T), out QuickList <IPartitionObject> objects))
{
return(null);
}
QuickList <T> newList = new QuickList <T>();
foreach (IPartitionObject obj in objects)
{
newList.Add((T)obj);
}
return(newList);
}
}
static void FillTrashBuffers(Simulation simulation, Random random)
{
var pool = simulation.BufferPool;
const int bufferCount = 50;
var bufferList = new QuickList <Buffer <int> >(bufferCount, pool);
for (int trashBufferIndex = 0; trashBufferIndex < bufferCount; ++trashBufferIndex)
{
//Pull a buffer from the pool, fill it with trash data, and return it.
ref var buffer = ref bufferList.AllocateUnsafely();
pool.TakeAtLeast(1 << random.Next(18), out buffer);
for (int k = 0; k < buffer.Length; ++k)
{
buffer[k] = random.Next(int.MinValue, int.MaxValue);
}
}
bool IsObstructed(ref QuickList <CharacterContact> supportContacts, ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts)
{
if (IsObstructed(ref sideContacts, ref headContacts))
{
return(true);
}
for (int i = 0; i < supportContacts.Count; ++i)
{
if (IsSupportContactObstructive(ref supportContacts.Elements[i].Contact))
{
return(true);
}
}
return(false);
}
public void InsertBeforeBound()
{
var lst = new QuickList <int>(5);
for (var i = 0; i < 10; i++)
{
lst.Push(i);
}
lst.InsertBefore(4, 999);
lst.InsertBefore(999, 888);
lst.InsertBefore(888, 777);
Assert.AreEqual(777, lst[4]);
Assert.AreEqual(888, lst[5]);
Assert.AreEqual(999, lst[6]);
}
protected override void UpdateContainedPairs(float dt)
{
var overlappedElements = new QuickList <int>(BufferPools <int> .Thread);
BoundingBox localBoundingBox;
System.Numerics.Vector3 sweep;
Vector3Ex.Multiply(ref mobileMesh.entity.linearVelocity, dt, out sweep);
mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref mesh.worldTransform, ref sweep, out localBoundingBox);
mesh.Shape.GetOverlaps(localBoundingBox, ref overlappedElements);
for (int i = 0; i < overlappedElements.Count; i++)
{
TryToAdd(overlappedElements.Elements[i]);
}
overlappedElements.Dispose();
}
internal int Add(ref BodyDescription bodyDescription, int handle, int minimumConstraintCapacity, BufferPool pool)
{
var index = Count;
if (index == IndexToHandle.Length)
{
InternalResize(IndexToHandle.Length * 2, pool);
}
++Count;
IndexToHandle[index] = handle;
//Collidable's broad phase index is left unset. The Bodies collection is responsible for attaching that data.
QuickList <BodyConstraintReference, Buffer <BodyConstraintReference> > .Create(pool.SpecializeFor <BodyConstraintReference>(), minimumConstraintCapacity, out Constraints[index]);
ApplyDescriptionByIndex(index, ref bodyDescription);
return(index);
}
/// <summary>
/// Resizes the underlying buffer to the smallest size required to hold the given count and the current available id count.
/// </summary>
/// <param name="count">Number of elements to guarantee space for in the available ids queue.</param>
public void Resize <TPool>(int count, TPool pool) where TPool : IMemoryPool <int, TSpan>
{
if (!AvailableIds.Span.Allocated)
{
//If this was disposed, we must explicitly rehydrate it.
QuickList <int, TSpan> .Create(pool, count, out AvailableIds);
return;
}
var targetLength = BufferPool <int> .GetLowestContainingElementCount(Math.Max(count, AvailableIds.Count));
if (AvailableIds.Span.Length != targetLength)
{
AvailableIds.Resize(targetLength, pool);
}
}
CharacterContactPositionState TrySupportLocation(ref System.Numerics.Vector3 position, out float hintOffset,
ref QuickList <CharacterContact> tractionContacts, ref QuickList <CharacterContact> supportContacts, ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts)
{
hintOffset = 0;
PrepareQueryObject(standingQueryObject, ref position);
QueryManager.QueryContacts(standingQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
bool obstructed = IsObstructed(ref sideContacts, ref headContacts);
if (obstructed)
{
return(CharacterContactPositionState.Obstructed);
}
if (supportContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
var down = characterBody.orientationMatrix.Down;
//Note that traction is not tested for; it isn't important for the stance manager.
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//We need to have fairly good contacts after this process, so only push it up a bit.
hintOffset = Math.Min(0, Vector3Ex.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return(CharacterContactPositionState.Accepted);
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, Vector3Ex.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return(CharacterContactPositionState.TooDeep);
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3Ex.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.NoHit);
}
}
else //Not obstructed, but no support.
{
return(CharacterContactPositionState.NoHit);
}
}
bool IsObstructed(ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts)
{
//No head contacts can exist!
if (headContacts.Count > 0)
{
return(true);
}
//A contact is considered obstructive if its projected depth is deeper than any existing contact along the existing contacts' normals.
for (int i = 0; i < sideContacts.Count; i++)
{
if (IsObstructive(ref sideContacts.Elements[i].Contact))
{
return(true);
}
}
return(false);
}
private unsafe void TopDownAgglomerativeRefine(int nodeIndex, ref QuickList <int> spareNodes)
{
bool nodesInvalidated;
AgglomerativeRefine(nodeIndex, ref spareNodes, out nodesInvalidated);
//The root of the tree is guaranteed to stay in position, so nodeIndex is still valid.
//Node pointers can be invalidated, so don't hold a reference between executions.
for (int i = 0; i < nodes[nodeIndex].ChildCount; ++i)
{
var child = (&nodes[nodeIndex].ChildA)[i];
if (child >= 0)
{
TopDownAgglomerativeRefine(child, ref spareNodes);
}
}
}
public void NullElementShiftTest()
{
var master = new QuickList <object>(3);
var result = new List <object> {
null, 1, 2, null, 3
};
foreach (var r in result)
{
master.Push(r);
}
foreach (var r in result)
{
Assert.AreEqual(r, master.Shift());
}
}
public void GetRangeBlockTest()
{
var master = new QuickList <int>(20);
for (var i = 0; i < 256; i++)
{
master.Push(i);
}
var elements = master.GetRange(100, 200);
Assert.AreEqual(101, elements.Length);
for (var i = 100; i < 200; i++)
{
Assert.AreEqual(i, elements[i - 100]);
}
}
private void UpdateCollections()
{
PointsOfInterest.Clear();
QuickList.Clear();
foreach (RhitLocation location in Locations)
{
if (location.Type == LocationType.OnQuickList)
{
QuickList.Add(location);
PointsOfInterest.Add(location);
}
else if (location.Type == LocationType.PointOfInterest)
{
PointsOfInterest.Add(location);
}
}
}
public void SetListValueByIndex()
{
var master = new QuickList <int>(5);
for (var i = 0; i < 255; i++)
{
master.Push(i);
}
master[0] = 999;
master[128] = 999;
master[254] = 999;
Assert.AreEqual(999, master[0]);
Assert.AreEqual(999, master[128]);
Assert.AreEqual(999, master[254]);
}
public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchor = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchor, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, anchor, packedColor, 0);
}
public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var pose = bodies.Sets[setIndex].Poses[*bodyIndices];
Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset);
Vector3Wide.ReadFirst(prestepBundle.Target, out var target);
Quaternion.Transform(localOffset, pose.Orientation, out var worldOffset);
var anchor = pose.Position + worldOffset;
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(pose.Position, anchor, Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchor, target, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
}
CharacterContactPositionState TryDownStepPosition(ref System.Numerics.Vector3 position, ref System.Numerics.Vector3 down,
ref QuickList <CharacterContact> tractionContacts, ref QuickList <CharacterContact> supportContacts, ref QuickList <CharacterContact> sideContacts, ref QuickList <CharacterContact> headContacts,
out float hintOffset)
{
hintOffset = 0;
PrepareQueryObject(ref position);
QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
if (IsDownStepObstructed(ref sideContacts))
{
return(CharacterContactPositionState.Obstructed);
}
//Note the use of traction contacts. We only want to step down if there are traction contacts to support us.
if (tractionContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//The final state doesn't need to actually create contacts, so shove it up
//just barely to the surface.
hintOffset = -Vector3Ex.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.Accepted);
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, .001f - Vector3Ex.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth);
return(CharacterContactPositionState.TooDeep);
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3Ex.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return(CharacterContactPositionState.NoHit);
}
}
else
{
return(CharacterContactPositionState.NoHit);
}
}
public void NullElementPopTest()
{
var master = new QuickList <object>(3);
var result = new List <object> {
null, 1, 2, null, 3
};
foreach (var r in result)
{
master.Push(r);
}
result.Reverse();
foreach (var r in result)
{
Assert.AreEqual(r, master.Pop());
}
}
/// Init TweenMgr
public void Init(int nTimerPoolSize = DEFAULT_TIMER_POOL_SIZE)
{
Debugger.Assert(_timerPool == null, "Should never call AnimationMgr.Init more than once!");
_Time = 0;
_RealTime = 0;
__AliveTickableCount = 0;
// Init timer pool
_timerPool = new ObjectPool <Timer>(nTimerPoolSize, pool =>
{
Timer pTimer = new Timer();
pTimer._Type |= TickableType.Pooled;
return(pTimer);
});
_arrInQueueTickers = new QuickList <Tickable>();
}
public Input(NativeWindow window, BufferPool pool)
{
this.window = window;
this.window.KeyDown += KeyDown;
this.window.KeyUp += KeyUp;
this.window.MouseDown += MouseDown;
this.window.MouseUp += MouseUp;
this.window.MouseWheel += MouseWheel;
this.window.KeyPress += KeyPress;
this.pool = pool;
anyDownedButtons = new MouseButtonSet(8, pool);
downedButtons = new MouseButtonSet(8, pool);
previousDownedButtons = new MouseButtonSet(8, pool);
anyDownedKeys = new KeySet(8, pool);
downedKeys = new KeySet(8, pool);
previousDownedKeys = new KeySet(8, pool);
TypedCharacters = new QuickList <char>(32, pool);
}
public void GenerateRenderListsSingleThread(bool excludeUI)
{
QuickList <IPartitionObject> renderedSprites = VisibleSprites;
RenderContainer.Reset();
IPartitionObject[] spriteArray = renderedSprites.Array;
for (int i = 0; i < renderedSprites.Count; i++)
{
IRenderable renderObj = (IRenderable)spriteArray[i];
if (excludeUI && renderObj is IUISprite)
{
continue;
}
RenderContainer.Add(renderObj);
}
}
public void RemoveReturnNumTest()
{
var master = new QuickList <int>(8);
master.Push(111);
master.Push(111);
master.Push(111);
master.Push(222);
master.Push(333);
master.Push(111);
master.Push(111);
master.Push(444);
master.Push(333);
var removeNum = master.Remove(111);
Assert.AreEqual(5, removeNum);
}
public void GenerateRenderListsMultiThreaded(bool excludeUI)
{
QuickList <IPartitionObject> renderedSprites = VisibleSprites;
RenderContainer.Reset();
OrderablePartitioner <IPartitionObject> partitioner = Partitioner.Create(renderedSprites);
Parallel.ForEach(partitioner, (obj, loopstate) => {
IRenderable renderObj = (IRenderable)obj;
if (excludeUI && renderObj is IUISprite)
{
return;
}
RenderContainer.Add(renderObj, true);
});
}
public static void TestListResizing <TSpan, TPool>(TPool pool)
where TSpan : ISpan <int>
where TPool : IMemoryPool <int, TSpan>
{
Random random = new Random(5);
QuickList <int, TSpan> .Create(pool, 4, out var list);
List <int> controlList = new List <int>();
for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
list.Add(iterationIndex, pool);
controlList.Add(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
var indexToRemove = random.Next(list.Count);
list.RemoveAt(indexToRemove);
controlList.RemoveAt(indexToRemove);
}
if (iterationIndex % 1000 == 0)
{
list.EnsureCapacity(list.Count * 3, pool);
}
else if (iterationIndex % 7777 == 0)
{
list.Compact(pool);
}
}
Debug.Assert(list.Count == controlList.Count);
for (int i = 0; i < list.Count; ++i)
{
var a = list[i];
var b = controlList[i];
Debug.Assert(a == b);
Debug.Assert(list.Count == controlList.Count);
}
list.Dispose(pool);
}
unsafe void ReifyChildren(int internalNodeIndex, Node *stagingNodes, ref QuickList <int> subtrees, ref QuickList <int> treeletInternalNodes, ref int nextInternalNodeIndexToUse, ref QuickList <int> spareNodes, out bool nodesInvalidated)
{
nodesInvalidated = false;
var internalNode = nodes + internalNodeIndex;
var internalNodeChildren = &internalNode->ChildA;
for (int i = 0; i < internalNode->ChildCount; ++i)
{
if (internalNodeChildren[i] >= 0)
{
bool childNodesInvalidated;
internalNodeChildren[i] = ReifyStagingNode(internalNodeIndex, i, stagingNodes, internalNodeChildren[i],
ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out childNodesInvalidated);
if (childNodesInvalidated)
{
internalNode = nodes + internalNodeIndex;
internalNodeChildren = &internalNode->ChildA;
nodesInvalidated = true;
}
}
else
{
//It's a subtree. Update its pointers.
var subtreeIndex = subtrees.Elements[Encode(internalNodeChildren[i])];
internalNodeChildren[i] = subtreeIndex;
if (subtreeIndex >= 0)
{
Debug.Assert(subtreeIndex >= 0 && subtreeIndex < nodeCount);
//Subtree is an internal node. Update its parent pointers.
nodes[subtreeIndex].IndexInParent = i;
nodes[subtreeIndex].Parent = internalNodeIndex;
}
else
{
//Subtree is a leaf node. Update its parent pointers.
var leafIndex = Encode(subtreeIndex);
Debug.Assert(leafIndex >= 0 && leafIndex < LeafCount);
var leaf = leaves + leafIndex;
leaf->ChildIndex = i;
leaf->NodeIndex = internalNodeIndex;
}
}
}
}
unsafe int ReifyStagingNode(int parent, int indexInParent, Node *stagingNodes, int stagingNodeIndex,
ref QuickList <int> subtrees, ref QuickList <int> treeletInternalNodes, ref int nextInternalNodeIndexToUse, ref QuickList <int> spareNodes, out bool nodesInvalidated)
{
nodesInvalidated = false;
int internalNodeIndex;
if (nextInternalNodeIndexToUse < treeletInternalNodes.Count)
{
//There is an internal node that we can use.
//Note that we remove from the end to guarantee that the treelet root does not change location.
//The CollectSubtrees function guarantees that the treelet root is enqueued first.
internalNodeIndex = treeletInternalNodes.Elements[nextInternalNodeIndexToUse++];
}
else if (!spareNodes.TryPop(out internalNodeIndex))
{
//There was no pre-existing internal node that we could use. Apparently, the tree gained some internal nodes.
//Watch out, calls to AllocateNode potentially invalidate all extant node pointers.
//Fortunately, there are no pointers to invalidate... here. Propagate it up.
//Note that this is a locking operation to support multithreading. In practice, n-ary trees should hit this very rarely.
//In binary trees, this will never get hit at all.
bool addCausedInvalidation;
internalNodeIndex = AllocateNodeLocking(out addCausedInvalidation);
if (addCausedInvalidation)
{
nodesInvalidated = true;
}
}
//To make the staging node real, it requires an accurate parent pointer, index in parent, and child indices.
//Copy the staging node into the real tree.
//We take the staging node's child bounds, child indices, leaf counts, and child count.
//The parent and index in parent are provided by the caller.
var stagingNode = stagingNodes + stagingNodeIndex;
var internalNode = nodes + internalNodeIndex;
*internalNode = *stagingNode;
internalNode->RefineFlag = 0; //The staging node could have contained arbitrary refine flag data.
internalNode->Parent = parent;
internalNode->IndexInParent = indexInParent;
ReifyChildren(internalNodeIndex, stagingNodes, ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out nodesInvalidated);
return(internalNodeIndex);
}
public unsafe void Insert(Node* node, Node* nodes, ref QuickList<int> subtrees)
{
var children = &node->ChildA;
var bounds = &node->A;
for (int childIndex = 0; childIndex < node->ChildCount; ++childIndex)
{
if (children[childIndex] >= 0)
{
int index = Count;
var cost = Tree.ComputeBoundsMetric(ref bounds[childIndex]);// - node->PreviousMetric;
++Count;
//Sift up.
while (index > 0)
{
var parentIndex = (index - 1) >> 1;
var parent = Entries + parentIndex;
if (parent->Cost < cost)
{
//Pull the parent down.
Entries[index] = *parent;
index = parentIndex;
}
else
{
//Found the insertion spot.
break;
}
}
var entry = Entries + index;
entry->Index = children[childIndex];
entry->Cost = cost;
}
else
{
//Immediately add leaf nodes.
subtrees.Add(children[childIndex]);
}
}
}
unsafe void CollectSubtreesForNodeDirect(int nodeIndex, int remainingDepth, ref QuickList<int> subtrees, ref QuickQueue<int> internalNodes, out float treeletCost)
{
internalNodes.Enqueue(nodeIndex);
treeletCost = 0;
var node = nodes + nodeIndex;
var children = &node->ChildA;
var bounds = &node->A;
--remainingDepth;
if (remainingDepth >= 0)
{
for (int i = 0; i < node->ChildCount; ++i)
{
if (children[i] >= 0)
{
treeletCost += ComputeBoundsMetric(ref bounds[i]);
float childCost;
CollectSubtreesForNodeDirect(children[i], remainingDepth, ref subtrees, ref internalNodes, out childCost);
treeletCost += childCost;
}
else
{
//It's a leaf, immediately add it to subtrees.
subtrees.Add(children[i]);
}
}
}
else
{
//Recursion has bottomed out. Add every child.
//Once again, note that the treelet costs of these nodes are not considered, even if they are internal.
//That's because the subtree internal nodes cannot change size due to the refinement.
for (int i = 0; i < node->ChildCount; ++i)
{
subtrees.Add(children[i]);
}
}
}
public static void TestListResizing()
{
Random random = new Random(5);
UnsafeBufferPool<int> pool = new UnsafeBufferPool<int>();
QuickList<int> list = new QuickList<int>(pool, 2);
List<int> controlList = new List<int>();
for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex)
{
if (random.NextDouble() < 0.7)
{
list.Add(iterationIndex);
controlList.Add(iterationIndex);
}
if (random.NextDouble() < 0.2)
{
var indexToRemove = random.Next(list.Count);
list.RemoveAt(indexToRemove);
controlList.RemoveAt(indexToRemove);
}
if (iterationIndex % 1000 == 0)
{
list.EnsureCapacity(list.Count * 3);
}
else if (iterationIndex % 7777 == 0)
{
list.Compact();
}
}
Assert.IsTrue(list.Count == controlList.Count);
for (int i = 0; i < list.Count; ++i)
{
var a = list[i];
var b = controlList[i];
Assert.IsTrue(a == b);
Assert.IsTrue(list.Count == controlList.Count);
}
}
unsafe void ReifyChildren(int internalNodeIndex, Node* stagingNodes, ref QuickList<int> subtrees, ref QuickList<int> treeletInternalNodes, ref int nextInternalNodeIndexToUse, ref QuickList<int> spareNodes, out bool nodesInvalidated)
{
nodesInvalidated = false;
var internalNode = nodes + internalNodeIndex;
var internalNodeChildren = &internalNode->ChildA;
for (int i = 0; i < internalNode->ChildCount; ++i)
{
if (internalNodeChildren[i] >= 0)
{
bool childNodesInvalidated;
internalNodeChildren[i] = ReifyStagingNode(internalNodeIndex, i, stagingNodes, internalNodeChildren[i],
ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out childNodesInvalidated);
if (childNodesInvalidated)
{
internalNode = nodes + internalNodeIndex;
internalNodeChildren = &internalNode->ChildA;
nodesInvalidated = true;
}
}
else
{
//It's a subtree. Update its pointers.
var subtreeIndex = subtrees.Elements[Encode(internalNodeChildren[i])];
internalNodeChildren[i] = subtreeIndex;
if (subtreeIndex >= 0)
{
Debug.Assert(subtreeIndex >= 0 && subtreeIndex < nodeCount);
//Subtree is an internal node. Update its parent pointers.
nodes[subtreeIndex].IndexInParent = i;
nodes[subtreeIndex].Parent = internalNodeIndex;
}
else
{
//Subtree is a leaf node. Update its parent pointers.
var leafIndex = Encode(subtreeIndex);
Debug.Assert(leafIndex >= 0 && leafIndex < LeafCount);
var leaf = leaves + leafIndex;
leaf->ChildIndex = i;
leaf->NodeIndex = internalNodeIndex;
}
}
}
}
unsafe void ReifyStagingNodes(int treeletRootIndex, Node* stagingNodes, ref QuickList<int> subtrees, ref QuickList<int> treeletInternalNodes, ref int nextInternalNodeIndexToUse, ref QuickList<int> spareNodes, out bool nodesInvalidated)
{
nodesInvalidated = false;
//We take the staging node's child bounds, child indices, leaf counts, and child count.
//The parent and index in parent of the treelet root CANNOT BE TOUCHED.
//When running on multiple threads, another thread may modify the Parent and IndexInParent of the treelet root.
var internalNode = nodes + treeletRootIndex;
internalNode->ChildCount = stagingNodes->ChildCount;
var bounds = &internalNode->A;
var children = &internalNode->ChildA;
var leafCounts = &internalNode->LeafCountA;
var stagingBounds = &stagingNodes->A;
var stagingChildren = &stagingNodes->ChildA;
var stagingLeafCounts = &stagingNodes->LeafCountA;
for (int i = 0; i < internalNode->ChildCount; ++i)
{
bounds[i] = stagingBounds[i];
children[i] = stagingChildren[i];
leafCounts[i] = stagingLeafCounts[i];
}
ReifyChildren(treeletRootIndex, stagingNodes, ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out nodesInvalidated);
}
unsafe int ReifyStagingNode(int parent, int indexInParent, Node* stagingNodes, int stagingNodeIndex,
ref QuickList<int> subtrees, ref QuickList<int> treeletInternalNodes, ref int nextInternalNodeIndexToUse, ref QuickList<int> spareNodes, out bool nodesInvalidated)
{
nodesInvalidated = false;
int internalNodeIndex;
if (nextInternalNodeIndexToUse < treeletInternalNodes.Count)
{
//There is an internal node that we can use.
//Note that we remove from the end to guarantee that the treelet root does not change location.
//The CollectSubtrees function guarantees that the treelet root is enqueued first.
internalNodeIndex = treeletInternalNodes.Elements[nextInternalNodeIndexToUse++];
}
else if (!spareNodes.TryPop(out internalNodeIndex))
{
//There was no pre-existing internal node that we could use. Apparently, the tree gained some internal nodes.
//Watch out, calls to AllocateNode potentially invalidate all extant node pointers.
//Fortunately, there are no pointers to invalidate... here. Propagate it up.
//Note that this is a locking operation to support multithreading. In practice, n-ary trees should hit this very rarely.
//In binary trees, this will never get hit at all.
bool addCausedInvalidation;
internalNodeIndex = AllocateNodeLocking(out addCausedInvalidation);
if (addCausedInvalidation)
nodesInvalidated = true;
}
//To make the staging node real, it requires an accurate parent pointer, index in parent, and child indices.
//Copy the staging node into the real tree.
//We take the staging node's child bounds, child indices, leaf counts, and child count.
//The parent and index in parent are provided by the caller.
var stagingNode = stagingNodes + stagingNodeIndex;
var internalNode = nodes + internalNodeIndex;
*internalNode = *stagingNode;
internalNode->RefineFlag = 0; //The staging node could have contained arbitrary refine flag data.
internalNode->Parent = parent;
internalNode->IndexInParent = indexInParent;
ReifyChildren(internalNodeIndex, stagingNodes, ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out nodesInvalidated);
return internalNodeIndex;
}
private unsafe void TopDownAgglomerativeRefine(int nodeIndex, ref QuickList<int> spareNodes)
{
bool nodesInvalidated;
AgglomerativeRefine(nodeIndex, ref spareNodes, out nodesInvalidated);
//The root of the tree is guaranteed to stay in position, so nodeIndex is still valid.
//Node pointers can be invalidated, so don't hold a reference between executions.
for (int i = 0; i < nodes[nodeIndex].ChildCount; ++i)
{
var child = (&nodes[nodeIndex].ChildA)[i];
if (child >= 0)
{
TopDownAgglomerativeRefine(child, ref spareNodes);
}
}
}
public override void Update(double dt)
{
RigidTransform transform = new RigidTransform(mesh.Position);
RigidTransform convexTransform = convex.WorldTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convexTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
var overlaps = new QuickList<Vector3i>(BufferPools<Vector3i>.Thread);
mesh.ChunkShape.GetOverlaps(mesh.Position, convex.BoundingBox, ref overlaps);
var candidatesToAdd = new QuickList<ContactData>(BufferPools<ContactData>.Thread, BufferPool<int>.GetPoolIndex(overlaps.Count));
for (int i = 0; i < overlaps.Count; i++)
{
GeneralConvexPairTester manifold;
if (!ActivePairs.TryGetValue(overlaps.Elements[i], out manifold))
{
manifold = GetPair(ref overlaps.Elements[i]);
}
else
{
ActivePairs.FastRemove(overlaps.Elements[i]);
}
activePairsBackBuffer.Add(overlaps.Elements[i], manifold);
ContactData contactCandidate;
if (manifold.GenerateContactCandidate(out contactCandidate))
{
candidatesToAdd.Add(ref contactCandidate);
}
}
overlaps.Dispose();
for (int i = ActivePairs.Count - 1; i >= 0; i--)
{
ReturnPair(ActivePairs.Values[i]);
ActivePairs.FastRemove(ActivePairs.Keys[i]);
}
var temp = ActivePairs;
ActivePairs = activePairsBackBuffer;
activePairsBackBuffer = temp;
if (contacts.Count + candidatesToAdd.Count > 4)
{
var reducedCandidates = new QuickList<ContactData>(BufferPools<ContactData>.Thread, 3);
ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
reducedCandidates.Dispose();
}
else if (candidatesToAdd.Count > 0)
{
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
candidatesToAdd.Dispose();
}
/// <summary>
/// Finds contacts between the query object and any intersected objects within the character's bounding box.
/// </summary>
/// <param name="queryObject">Collidable to query for contacts with.</param>
/// <param name="tractionContacts">Output contacts that would provide traction.</param>
/// <param name="supportContacts">Output contacts that would provide support.</param>
/// <param name="sideContacts">Output contacts on the sides of the query object.</param>
/// <param name="headContacts">Output contacts on the head of the query object.</param>
public void QueryContacts(EntityCollidable queryObject,
ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts)
{
var downDirection = characterBody.orientationMatrix.Down;
tractionContacts.Clear();
supportContacts.Clear();
sideContacts.Clear();
headContacts.Clear();
foreach (var collidable in characterBody.CollisionInformation.OverlappedCollidables)
{
//The query object is assumed to have a valid bounding box.
if (collidable.BoundingBox.Intersects(queryObject.BoundingBox))
{
var pair = new CollidablePair(collidable, queryObject);
var pairHandler = NarrowPhaseHelper.GetPairHandler(ref pair);
if (pairHandler.CollisionRule == CollisionRule.Normal)
{
pairHandler.SuppressEvents = true;
pairHandler.UpdateCollision(0);
pairHandler.SuppressEvents = false;
contactCategorizer.CategorizeContacts(pairHandler, characterBody.CollisionInformation, ref downDirection,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
}
//TODO: It would be nice if this was a bit easier.
//Having to remember to clean up AND give it back is a bit weird, especially with the property-diving.
//No one would ever just guess this correctly.
//At least hide it behind a NarrowPhaseHelper function.
pairHandler.CleanUp();
pairHandler.Factory.GiveBack(pairHandler);
}
}
}
//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);
}
}
}
/// <summary>
/// Checks if a transition from the current stance to the target stance is possible given the current environment.
/// </summary>
/// <param name="targetStance">Stance to check for transition safety.</param>
/// <param name="newHeight">If the transition is safe, the new height of the character. Zero otherwise.</param>
/// <param name="newPosition">If the transition is safe, the new location of the character body if the transition occurred. Zero vector otherwise.</param>
/// <returns>True if the target stance is different than the current stance and the transition is valid, false otherwise.</returns>
public bool CheckTransition(Stance targetStance, out float newHeight, out Vector3 newPosition)
{
var currentPosition = characterBody.position;
var down = characterBody.orientationMatrix.Down;
newPosition = new Vector3();
newHeight = 0;
if (CurrentStance != targetStance)
{
float currentHeight;
switch (CurrentStance)
{
case Stance.Prone:
currentHeight = proneHeight;
break;
case Stance.Crouching:
currentHeight = crouchingHeight;
break;
default:
currentHeight = standingHeight;
break;
}
float targetHeight;
switch (targetStance)
{
case Stance.Prone:
targetHeight = proneHeight;
break;
case Stance.Crouching:
targetHeight = crouchingHeight;
break;
default:
targetHeight = standingHeight;
break;
}
if (currentHeight >= targetHeight)
{
//The character is getting smaller, so no validation queries are required.
if (SupportFinder.HasSupport)
{
//On the ground, so need to move the position down.
newPosition = currentPosition + down * ((currentHeight - targetHeight) * 0.5f);
}
else
{
//We're in the air, so we don't have to change the position at all- just change the height.
newPosition = currentPosition;
}
newHeight = targetHeight;
return true;
}
//The character is getting bigger, so validation is required.
ConvexCollidable<CylinderShape> queryObject;
switch (targetStance)
{
case Stance.Prone:
queryObject = proneQueryObject;
break;
case Stance.Crouching:
queryObject = crouchingQueryObject;
break;
default:
queryObject = standingQueryObject;
break;
}
var tractionContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var supportContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var sideContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var headContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
try
{
if (SupportFinder.HasSupport)
{
//Increasing in size requires a query to verify that the new state is safe.
//TODO: State queries can be expensive if the character is crouching beneath something really detailed.
//There are some situations where you may want to do an upwards-pointing ray cast first. If it hits something,
//there's no need to do the full query.
newPosition = currentPosition - down * ((targetHeight - currentHeight) * .5f);
PrepareQueryObject(queryObject, ref newPosition);
QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
if (IsObstructed(ref sideContacts, ref headContacts))
{
//Can't stand up if something is in the way!
return false;
}
newHeight = targetHeight;
return true;
}
else
{
//This is a complicated case. We must perform a semi-downstep query.
//It's different than a downstep because the head may be obstructed as well.
float highestBound = 0;
float lowestBound = (targetHeight - currentHeight) * .5f;
float currentOffset = lowestBound;
float maximum = lowestBound;
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
Vector3 candidatePosition = currentPosition + currentOffset * down;
float hintOffset;
switch (TrySupportLocation(queryObject, ref candidatePosition, out hintOffset, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > 0 && currentOffset < maximum)
{
newPosition = currentPosition + currentOffset * down;
newHeight = targetHeight;
return true;
}
else
{
return false;
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Couldn't find a hit. Go ahead and get bigger!
newPosition = currentPosition;
newHeight = targetHeight;
return true;
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
return false;
}
bool IsObstructed(ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts)
{
//No head contacts can exist!
if (headContacts.Count > 0)
return true;
//A contact is considered obstructive if its projected depth is deeper than any existing contact along the existing contacts' normals.
for (int i = 0; i < sideContacts.Count; i++)
{
if (IsObstructive(ref sideContacts.Elements[i].Contact))
return true;
}
return false;
}
CharacterContactPositionState TryUpStepPosition(ref Vector3 sideNormal, ref Vector3 position, ref Vector3 down,
ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts,
out float hintOffset)
{
hintOffset = 0;
PrepareQueryObject(ref position);
QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
if (headContacts.Count > 0)
{
//The head is obstructed. This will define a maximum bound.
//Find the deepest contact on the head and use it to provide a hint.
float dot;
Vector3.Dot(ref down, ref headContacts.Elements[0].Contact.Normal, out dot);
hintOffset = -dot * headContacts.Elements[0].Contact.PenetrationDepth;
for (int i = 1; i < headContacts.Count; i++)
{
Vector3.Dot(ref down, ref headContacts.Elements[i].Contact.Normal, out dot);
dot *= -headContacts.Elements[i].Contact.PenetrationDepth;
if (dot > hintOffset)
{
hintOffset = dot;
}
}
return CharacterContactPositionState.HeadObstructed;
}
bool obstructed = IsUpStepObstructedBySideContacts(ref sideNormal, ref sideContacts);
if (!obstructed && supportContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
if (supportState == CharacterContactPositionState.Accepted)
{
if (tractionContacts.Count > 0)
{
//We're done! The guess found a good spot to stand on.
//Unlike down stepping, upstepping DOES need good contacts in the final state.
//Push it up if necessary, but don't push it too far.
//Putting it into the middle of the allowed penetration makes it very likely that it will properly generate contacts.
//Choosing something smaller than allowed penetration ensures that the search makes meaningful progress forward when the sizes get really tiny;
//we wouldn't want it edging every closer to AllowedPenetration and then exit because too many queries were made.
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.Accepted;
}
else
{
//No traction... Before we give up and reject the step altogether, let's try one last thing. It's possible that the character is trying to step up onto the side of a ramp or something.
//In this scenario, the top-down ray cast detects a perfectly walkable slope. However, the contact queries will find a contact with a normal necessarily
//steeper than the one found by the ray cast because it is an edge contact. Not being able to step up in this scenario doesn't feel natural to the player
//even if it is technically consistent.
//So, let's try to ray cast down to the a point just barely beyond the contact (to ensure we don't land right on the edge, which would invite numerical issues).
//Note that this is NOT equivalent to the ray cast we performed earlier to test for an initial step height and surface normal.
//This one is based on the QUERY state and the QUERY's contact position.
//Find the down test ray's position.
Ray downRay;
downRay.Position = supportContact.Contact.Position + sideNormal * .001f;
float verticalOffset = Vector3.Dot(downRay.Position - position, down);
verticalOffset = characterBody.Height * .5f + verticalOffset;
downRay.Position -= verticalOffset * down;
downRay.Direction = down;
//First, we must ensure that the ray cast test origin is not obstructed. Starting very close to the very top of the character is safe because the process has already validated
//this location as accepted, just without traction.
Ray obstructionTestRay;
obstructionTestRay.Position = position - down * (characterBody.Height * .5f);
obstructionTestRay.Direction = downRay.Position - obstructionTestRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionTestRay, 1))
{
//Okay! it's safe to cast down, then.
RayHit hit;
if (QueryManager.RayCast(downRay, characterBody.Height, out hit))
{
//Got a hit!
if (characterBody.Height - maximumStepHeight < hit.T)
{
//It's in range!
float dot;
hit.Normal.Normalize();
Vector3.Dot(ref hit.Normal, ref down, out dot);
if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
{
//Slope is shallow enough to stand on!
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
//ONE MORE thing to check. The new position of the center ray must be able to touch the ground!
downRay.Position = position;
if (QueryManager.RayCast(downRay, characterBody.Height * .5f + maximumStepHeight, out hit))
{
//It hit.. almost there!
hit.Normal.Normalize();
Vector3.Dot(ref hit.Normal, ref down, out dot);
if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
{
//It has traction! We can step!
return CharacterContactPositionState.Accepted;
}
}
}
}
}
}
//If it didn't have traction, and this was the most valid location we could find, then there is no support.
return CharacterContactPositionState.Rejected;
}
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.TooDeep;
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return CharacterContactPositionState.NoHit;
}
}
else if (obstructed)
{
return CharacterContactPositionState.Obstructed;
}
else
{
return CharacterContactPositionState.NoHit;
}
}
public override void Update( float dt )
{
//Refresh the contact manifold for this frame.
var transform = new RigidTransform( voxelBlob.Position );
var convexTransform = convex.WorldTransform;
ContactRefresher.ContactRefresh( contacts, supplementData, ref convexTransform, ref transform, contactIndicesToRemove );
RemoveQueuedContacts();
//Collect the set of overlapped cell indices.
//Not the fastest way to do this, but it's relatively simple and easy.
var overlaps = new QuickList<Int3>( BufferPools<Int3>.Thread );
voxelBlob.Shape.GetOverlaps( voxelBlob.Position, convex.BoundingBox, ref overlaps );
var candidatesToAdd = new QuickList<ContactData>( BufferPools<ContactData>.Thread, BufferPool<int>.GetPoolIndex( overlaps.Count ) );
for( int i = 0; i < overlaps.Count; ++i )
{
GeneralConvexPairTester manifold;
if( !ActivePairs.TryGetValue( overlaps.Elements[i], out manifold ) )
{
//This manifold did not previously exist.
manifold = GetPair( ref overlaps.Elements[i] );
}
else
{
//It did previously exist.
ActivePairs.FastRemove( overlaps.Elements[i] );
}
activePairsBackBuffer.Add( overlaps.Elements[i], manifold );
ContactData contactCandidate;
if( manifold.GenerateContactCandidate( out contactCandidate ) )
{
candidatesToAdd.Add( ref contactCandidate );
}
}
overlaps.Dispose();
//Any pairs remaining in the activePairs set no longer exist. Clean them up.
for( int i = ActivePairs.Count - 1; i >= 0; --i )
{
ReturnPair( ActivePairs.Values[i] );
ActivePairs.FastRemove( ActivePairs.Keys[i] );
}
//Swap the pair sets.
var temp = ActivePairs;
ActivePairs = activePairsBackBuffer;
activePairsBackBuffer = temp;
//Check if adding the new contacts would overflow the manifold.
if( contacts.Count + candidatesToAdd.Count > 4 )
{
//Adding all the contacts would overflow the manifold. Reduce to the best subset.
var reducedCandidates = new QuickList<ContactData>( BufferPools<ContactData>.Thread, 3 );
ContactReducer.ReduceContacts( contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates );
RemoveQueuedContacts();
for( int i = reducedCandidates.Count - 1; i >= 0; i-- )
{
Add( ref reducedCandidates.Elements[i] );
reducedCandidates.RemoveAt( i );
}
reducedCandidates.Dispose();
}
else if( candidatesToAdd.Count > 0 )
{
//Won't overflow the manifold, so just toss it in.
for( int i = 0; i < candidatesToAdd.Count; i++ )
{
Add( ref candidatesToAdd.Elements[i] );
}
}
candidatesToAdd.Dispose();
}
bool TryToStepUsingContact(ref ContactData contact, ref Vector3 down, out Vector3 newPosition)
{
Vector3 position = characterBody.Position;
//The normal of the contact may not be facing perfectly out to the side.
//The detection process allows a bit of slop.
//Correct it by removing any component of the normal along the local up vector.
Vector3 normal = contact.Normal;
float dot;
Vector3.Dot(ref normal, ref down, out dot);
Vector3 error;
Vector3.Multiply(ref down, dot, out error);
Vector3.Subtract(ref normal, ref error, out normal);
normal.Normalize();
//Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
//Compute the ray origin location. Fire it out of the top of the character; if we're stepping, this must be a valid location.
//Putting it as high as possible helps to reject more invalid step geometry.
Ray ray;
float downRayLength = characterBody.Height;// MaximumStepHeight + upStepMargin;
Vector3.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
Vector3.Add(ref ray.Position, ref position, out ray.Position);
ray.Direction = normal;
//Include a little margin in the length.
//Technically, the character only needs to teleport horizontally by the complicated commented expression.
//That puts it just far enough to have traction on the new surface.
//In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
//which can cause side effects for the character.
float horizontalOffsetAmount = characterBody.CollisionInformation.Shape.CollisionMargin;// (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
float length = characterBody.Radius + horizontalOffsetAmount;// -contact.PenetrationDepth;
if (QueryManager.RayCastHitAnything(ray, length))
{
//The step is obstructed!
newPosition = new Vector3();
return false;
}
//The down-cast ray origin has been verified by the previous ray cast.
//Let's look for a support!
Vector3 horizontalOffset;
Vector3.Multiply(ref normal, length, out horizontalOffset);
Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
ray.Direction = down;
//Find the earliest hit, if any.
RayHit earliestHit;
if (!QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit.
earliestHit.T <= 0 || //Can't do anything if the hit was invalid.
earliestHit.T - downRayLength > -minimumUpStepHeight || //Don't bother doing anything if the step is too small.
earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall.
{
//No valid hit was detected.
newPosition = new Vector3();
return false;
}
//Ensure the candidate surface supports traction.
Vector3 supportNormal;
Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
//Calibrate the normal to face in the same direction as the down vector for consistency.
Vector3.Dot(ref supportNormal, ref down, out dot);
if (dot < 0)
{
Vector3.Negate(ref supportNormal, out supportNormal);
dot = -dot;
}
//If the new surface does not have traction, do not attempt to step up.
if (dot < ContactCategorizer.TractionThreshold)
{
newPosition = new Vector3();
return false;
}
//Since contact queries are frequently expensive compared to ray cast tests,
//do one more ray cast test. This time, starting from the same position, cast upwards.
//In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
Vector3.Negate(ref down, out ray.Direction);
//Find the earliest hit, if any.
//RayHit earliestHitUp = new RayHit();
//earliestHitUp.T = float.MaxValue;
float upLength = characterBody.Height - earliestHit.T;
//If the sum of the up and down distances is less than the height, the character can't fit.
if (QueryManager.RayCastHitAnything(ray, upLength))
{
newPosition = new Vector3();
return false;
}
//By now, a valid ray hit has been found. Now we need to validate it using contact queries.
//This process is very similar in concept to the down step verification, but it has some extra
//requirements.
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//The transform must be modified to position the query body at the right location.
//The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
Vector3 verticalOffset;
Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position);
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray downRay;
characterBody.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
downRay.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
Plane plane = new Plane(supportNormal, dot);
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = -maximumStepHeight;
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var supportContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var sideContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var headContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
{
hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
if (hitT < highestBound)
{
//Don't try a location known to be too high.
hitT = highestBound;
}
currentOffset = hitT;
if (currentOffset > lowestBound)
lowestBound = currentOffset;
candidatePosition = characterBody.Position + down * currentOffset + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return false;
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
} //TODO: If the ray cast doesn't hit, that could be used to early out... Then again, it pretty much can't happen.
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return false;
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
//Couldn't find a candidate.
newPosition = new Vector3();
return false;
}
CharacterContactPositionState TryDownStepPosition(ref Vector3 position, ref Vector3 down,
ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts,
out float hintOffset)
{
hintOffset = 0;
PrepareQueryObject(ref position);
QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
if (IsDownStepObstructed(ref sideContacts))
{
return CharacterContactPositionState.Obstructed;
}
//Note the use of traction contacts. We only want to step down if there are traction contacts to support us.
if (tractionContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//The final state doesn't need to actually create contacts, so shove it up
//just barely to the surface.
hintOffset = -Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return CharacterContactPositionState.Accepted;
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, .001f - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth);
return CharacterContactPositionState.TooDeep;
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return CharacterContactPositionState.NoHit;
}
}
else
{
return CharacterContactPositionState.NoHit;
}
}
/// <summary>
/// Determines if a down step is possible, and if so, computes the location to which the character should teleport.
/// </summary>
/// <param name="newPosition">New position the character should teleport to if the down step is accepted.</param>
/// <returns>Whether or not the character should attempt to step down.</returns>
public bool TryToStepDown(out Vector3 newPosition)
{
//Don't bother trying to step down if we already have a support contact or if the support ray doesn't have traction.
if (!(SupportFinder.Supports.Count == 0 && SupportFinder.SupportRayData != null && SupportFinder.SupportRayData.Value.HasTraction))
{
newPosition = new Vector3();
return false;
}
if (!(SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance > minimumDownStepHeight)) //Don't do expensive stuff if it's, at most, a super tiny step that gravity will take care of.
{
newPosition = new Vector3();
return false;
}
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
Vector3 normal = SupportFinder.SupportRayData.Value.HitData.Normal;
Vector3 down = characterBody.orientationMatrix.Down;
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray ray;
characterBody.CollisionInformation.Shape.GetExtremePoint(normal, ref transform, out ray.Position);
ray.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Plane plane = new Plane(normal, Vector3.Dot(SupportFinder.SupportRayData.Value.HitData.Location, normal));
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = 0;
//The lowest possible distance is the ray distance plus the collision margin because the ray could theoretically be on the outskirts of the collision margin
//where the shape would actually have to move more than the bottom distance difference would imply.
//(Could compute the true lowest bound analytically based on the horizontal position of the ray...)
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin + SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var supportContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var sideContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
var headContacts = new QuickList<CharacterContact>(BufferPools<CharacterContact>.Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref ray, ref plane, out hitT, out intersection))
{
currentOffset = hitT + CollisionDetectionSettings.AllowedPenetration * 0.5f;
candidatePosition = characterBody.Position + down * currentOffset;
switch (TryDownStepPosition(ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down;
switch (TryDownStepPosition(ref candidatePosition, ref down, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return true;
}
else
{
newPosition = new Vector3();
return false;
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Couldn't find a candidate.
newPosition = new Vector3();
return false;
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
/// <summary>
/// Collects a limited set of subtrees hanging from the specified node and performs a local treelet rebuild using a bottom-up agglomerative approach.
/// </summary>
/// <param name="nodeIndex">Root of the refinement treelet.</param>
/// <param name="nodesInvalidated">True if the refinement process invalidated node pointers, false otherwise.</param>
public unsafe void AgglomerativeRefine(int nodeIndex, ref QuickList<int> spareNodes, out bool nodesInvalidated)
{
var maximumSubtrees = ChildrenCapacity * ChildrenCapacity;
var poolIndex = BufferPool<int>.GetPoolIndex(maximumSubtrees);
var subtrees = new QuickList<int>(BufferPools<int>.Thread, poolIndex);
var treeletInternalNodes = new QuickList<int>(BufferPools<int>.Thread, poolIndex);
float originalTreeletCost;
var entries = stackalloc SubtreeHeapEntry[maximumSubtrees];
CollectSubtrees(nodeIndex, maximumSubtrees, entries, ref subtrees, ref treeletInternalNodes, out originalTreeletCost);
//We're going to create a little binary tree via agglomeration, and then we'll collapse it into an n-ary tree.
//Note the size: we first put every possible subtree in, so subtrees.Count.
//Then, we add up subtrees.Count - 1 internal nodes without removing earlier slots.
int tempNodesCapacity = subtrees.Count * 2 - 1;
var tempNodes = stackalloc TempNode[tempNodesCapacity];
int tempNodeCount = subtrees.Count;
int remainingNodesCapacity = subtrees.Count;
var remainingNodes = stackalloc int[remainingNodesCapacity];
int remainingNodesCount = subtrees.Count;
for (int i = 0; i < subtrees.Count; ++i)
{
var tempNode = tempNodes + i;
tempNode->A = Encode(i);
if (subtrees.Elements[i] >= 0)
{
//It's an internal node, so look at the parent.
var subtreeNode = nodes + subtrees.Elements[i];
tempNode->BoundingBox = (&nodes[subtreeNode->Parent].A)[subtreeNode->IndexInParent];
tempNode->LeafCount = (&nodes[subtreeNode->Parent].LeafCountA)[subtreeNode->IndexInParent];
}
else
{
//It's a leaf node, so grab the bounding box from the owning node.
var leafIndex = Encode(subtrees.Elements[i]);
var leaf = leaves + leafIndex;
var parentNode = nodes + leaf->NodeIndex;
tempNode->BoundingBox = (&parentNode->A)[leaf->ChildIndex];
tempNode->LeafCount = 1;
}
//Add a reference to the remaining list.
remainingNodes[i] = i;
}
while (remainingNodesCount >= 2)
{
//Determine which pair of subtrees has the smallest cost.
//(Smallest absolute cost is used instead of *increase* in cost because absolute tends to move bigger objects up the tree, which is desirable.)
float bestCost = float.MaxValue;
int bestA = 0, bestB = 0;
for (int i = 0; i < remainingNodesCount; ++i)
{
for (int j = i + 1; j < remainingNodesCount; ++j)
{
var nodeIndexA = remainingNodes[i];
var nodeIndexB = remainingNodes[j];
BoundingBox merged;
BoundingBox.Merge(ref tempNodes[nodeIndexA].BoundingBox, ref tempNodes[nodeIndexB].BoundingBox, out merged);
var cost = ComputeBoundsMetric(ref merged);
if (cost < bestCost)
{
bestCost = cost;
bestA = i;
bestB = j;
}
}
}
{
//Create a new temp node based on the best pair.
TempNode newTempNode;
newTempNode.A = remainingNodes[bestA];
newTempNode.B = remainingNodes[bestB];
//Remerging here may or may not be faster than repeatedly caching 'best' candidates from above. It is a really, really cheap operation, after all, apart from cache issues.
BoundingBox.Merge(ref tempNodes[newTempNode.A].BoundingBox, ref tempNodes[newTempNode.B].BoundingBox, out newTempNode.BoundingBox);
newTempNode.LeafCount = tempNodes[newTempNode.A].LeafCount + tempNodes[newTempNode.B].LeafCount;
//Remove the best options from the list.
//BestA is always lower than bestB, so remove bestB first to avoid corrupting bestA index.
TempNode.FastRemoveAt(bestB, remainingNodes, ref remainingNodesCount);
TempNode.FastRemoveAt(bestA, remainingNodes, ref remainingNodesCount);
//Add the reference to the new node.
var newIndex = TempNode.Add(ref newTempNode, tempNodes, ref tempNodeCount);
remainingNodes[remainingNodesCount++] = newIndex;
}
}
//The 2-ary proto-treelet is ready.
//Collapse it into an n-ary tree.
const int collapseCount = ChildrenCapacity == 32 ? 4 : ChildrenCapacity == 16 ? 3 : ChildrenCapacity == 8 ? 2 : ChildrenCapacity == 4 ? 1 : 0;
//Remember: All positive indices in the tempnodes array refer to other temp nodes: they are internal references. Encoded references point back to indices in the subtrees list.
Debug.Assert(remainingNodesCount == 1);
int parent = nodes[nodeIndex].Parent;
int indexInParent = nodes[nodeIndex].IndexInParent;
var stagingNodeCapacity = maximumSubtrees - 1;
var stagingNodes = stackalloc Node[maximumSubtrees - 1];
int stagingNodeCount = 0;
float newTreeletCost;
var stagingRootIndex = BuildStagingChild(parent, indexInParent, tempNodes, tempNodeCount - 1, collapseCount, stagingNodes, ref stagingNodeCount, out newTreeletCost);
Debug.Assert(stagingNodeCount < stagingNodeCapacity);
if (newTreeletCost < originalTreeletCost)
{
//The refinement is an actual improvement.
//Apply the staged nodes to real nodes!
int nextInternalNodeIndexToUse = 0;
ReifyStagingNodes(nodeIndex, stagingNodes, ref subtrees, ref treeletInternalNodes, ref nextInternalNodeIndexToUse, ref spareNodes, out nodesInvalidated);
//If any nodes are left over, put them into the spares list for later reuse.
for (int i = nextInternalNodeIndexToUse; i < treeletInternalNodes.Count; ++i)
{
spareNodes.Add(treeletInternalNodes.Elements[i]);
}
}
else
{
nodesInvalidated = false;
}
subtrees.Dispose();
treeletInternalNodes.Dispose();
}
CharacterContactPositionState TrySupportLocation(ConvexCollidable<CylinderShape> queryObject, ref Vector3 position, out float hintOffset,
ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts)
{
hintOffset = 0;
PrepareQueryObject(queryObject, ref position);
QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
bool obstructed = IsObstructed(ref sideContacts, ref headContacts);
if (obstructed)
{
return CharacterContactPositionState.Obstructed;
}
if (supportContacts.Count > 0)
{
CharacterContactPositionState supportState;
CharacterContact supportContact;
QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
var down = characterBody.orientationMatrix.Down;
//Note that traction is not tested for; it isn't important for the stance manager.
if (supportState == CharacterContactPositionState.Accepted)
{
//We're done! The guess found a good spot to stand on.
//We need to have fairly good contacts after this process, so only push it up a bit.
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.Accepted;
}
else if (supportState == CharacterContactPositionState.TooDeep)
{
//Looks like we have to keep trying, but at least we found a good hint.
hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
return CharacterContactPositionState.TooDeep;
}
else //if (supportState == SupportState.Separated)
{
//It's not obstructed, but the support isn't quite right.
//It's got a negative penetration depth.
//We can use that as a hint.
hintOffset = -.001f - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
return CharacterContactPositionState.NoHit;
}
}
else //Not obstructed, but no support.
{
return CharacterContactPositionState.NoHit;
}
}
public unsafe void TopDownAgglomerativeRefine()
{
var spareNodes = new QuickList<int>(BufferPools<int>.Thread, 8);
TopDownAgglomerativeRefine(0, ref spareNodes);
RemoveUnusedInternalNodes(ref spareNodes);
spareNodes.Dispose();
}
unsafe void TryToBottomUpAgglomerativeRefine(int[] refinementFlags, int nodeIndex, ref QuickList<int> spareInternalNodes)
{
if (++refinementFlags[nodeIndex] == nodes[nodeIndex].ChildCount)
{
bool nodesInvalidated;
AgglomerativeRefine(nodeIndex, ref spareInternalNodes, out nodesInvalidated);
var parent = nodes[nodeIndex].Parent;
if (parent != -1)
{
TryToBottomUpAgglomerativeRefine(refinementFlags, parent, ref spareInternalNodes);
}
}
}
/// <summary>
/// Analyzes the support state of the character based on the speculative input support and traction contacts.
/// </summary>
/// <param name="tractionContacts">Contacts providing the character with traction.</param>
/// <param name="supportContacts">Contacts providing the character with support.</param>
/// <param name="state">State of the contacts relative to the speculative character position.</param>
/// <param name="supportContact">Representative contact to use, if any.</param>
public void AnalyzeSupportState(ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts,
out CharacterContactPositionState state, out CharacterContact supportContact)
{
float maxDepth = -float.MaxValue;
int deepestIndex = -1;
if (tractionContacts.Count > 0)
{
//It has traction!
//But is it too deep?
//Find the deepest contact.
for (int i = 0; i < tractionContacts.Count; i++)
{
if (tractionContacts.Elements[i].Contact.PenetrationDepth > maxDepth)
{
maxDepth = tractionContacts.Elements[i].Contact.PenetrationDepth;
deepestIndex = i;
}
}
supportContact = tractionContacts.Elements[deepestIndex];
}
else if (supportContacts.Count > 0)
{
//It has support!
//But is it too deep?
//Find the deepest contact.
for (int i = 0; i < supportContacts.Count; i++)
{
if (supportContacts.Elements[i].Contact.PenetrationDepth > maxDepth)
{
maxDepth = supportContacts.Elements[i].Contact.PenetrationDepth;
deepestIndex = i;
}
}
supportContact = supportContacts.Elements[deepestIndex];
}
else
{
state = CharacterContactPositionState.NoHit;
supportContact = new CharacterContact();
return;
}
//Check the depth.
if (maxDepth > CollisionDetectionSettings.AllowedPenetration)
{
//It's too deep.
state = CharacterContactPositionState.TooDeep;
}
else if (maxDepth < 0)
{
//The depth is negative, meaning it's separated. This shouldn't happen with the initial implementation of the character controller,
//but this case could conceivably occur in other usages of a system like this (or in a future version of the character),
//so go ahead and handle it.
state = CharacterContactPositionState.NoHit;
}
else
{
//The deepest contact appears to be very nicely aligned with the ground!
//It's fully supported.
state = CharacterContactPositionState.Accepted;
}
}
protected override void ProcessCandidates(ref QuickList<ContactData> candidates)
{
if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid)
{
//If there's no new contacts on the mesh and it's supposed to be a solid,
//then we must check the convex for containment within the shell.
//We already know that it's not on the shell, meaning that the shape is either
//far enough away outside the shell that there's no contact (and we're done),
//or it's far enough inside the shell that the triangles cannot create contacts.
//To find out which it is, raycast against the shell.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation);
Ray ray;
Vector3Ex.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position, out ray.Position);
Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position);
//Cast from the current position back to the previous position.
Vector3Ex.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction);
float rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//The object may not have moved enough to normalize properly. If so, choose something arbitrary.
//Try the direction from the center of the object to the convex's position.
ray.Direction = ray.Position;
rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//This is unlikely; just pick something completely arbitrary then.
ray.Direction = Vector3Ex.Up;
rayDirectionLength = 1;
}
}
Vector3Ex.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction);
RayHit hit;
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
ContactData newContact = new ContactData { Id = 2 };
//Give it a special id so that we know that it came from the inside.
Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position);
Vector3Ex.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3Ex.Dot(ref ray.Direction, ref newContact.Normal, out factor);
newContact.PenetrationDepth = -factor * hit.T + convex.Shape.MinimumRadius;
Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal);
newContact.Validate();
//Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists.
bool addContact = true;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new System.Numerics.Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
addContact = false;
break;
}
}
if (addContact && contacts.Count == 0)
Add(ref newContact);
previousDepth = newContact.PenetrationDepth;
}
else
{
//It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion.
//If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding.
if (previousDepth > 0)
{
//We're not touching the mesh.
lastValidConvexPosition = ray.Position;
}
previousDepth = 0;
}
}
}
