public void Move()
{
int[] testArray = Enumerable.Range(0, 10).ToArray();
RawList<int> intList = new RawList<int>();
intList.AddRange(testArray);
intList.Move(0, 3, 1);
CollectionAssert.AreEqual(new int[] { 0, 0, 1, 2, 4, 5, 6, 7, 8, 9 }, intList);
intList.Clear();
intList.AddRange(testArray);
intList.Move(0, 3, 3);
CollectionAssert.AreEqual(new int[] { 0, 1, 2, 0, 1, 2, 6, 7, 8, 9 }, intList);
intList.Clear();
intList.AddRange(testArray);
intList.Move(0, 3, 5);
CollectionAssert.AreEqual(new int[] { 0, 1, 2, 3, 4, 0, 1, 2, 8, 9 }, intList);
intList.Clear();
intList.AddRange(testArray);
intList.Move(7, 3, -1);
CollectionAssert.AreEqual(new int[] { 0, 1, 2, 3, 4, 5, 7, 8, 9, 9 }, intList);
intList.Clear();
intList.AddRange(testArray);
intList.Move(7, 3, -3);
CollectionAssert.AreEqual(new int[] { 0, 1, 2, 3, 7, 8, 9, 7, 8, 9 }, intList);
intList.Clear();
intList.AddRange(testArray);
intList.Move(7, 3, -5);
CollectionAssert.AreEqual(new int[] { 0, 1, 7, 8, 9, 5, 6, 7, 8, 9 }, intList);
intList.Clear();
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<BroadPhaseEntry> list)
{
if (SubPoolBroadPhaseEntryList == null)
SubPoolBroadPhaseEntryList = new UnsafeResourcePool<RawList<BroadPhaseEntry>>();
list.Clear();
SubPoolBroadPhaseEntryList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<RayCastResult> list)
{
if (SubPoolRayCastResultList == null)
SubPoolRayCastResultList = new UnsafeResourcePool<RawList<RayCastResult>>();
list.Clear();
SubPoolRayCastResultList.GiveBack(list);
}
[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>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList <RayHit> list)
{
list.Clear();
SubPoolRayHitList.GiveBack(list);
}
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>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<Entity> list)
{
list.Clear();
SubPoolEntityRawList.GiveBack(list);
}
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();
}
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<CompoundChild> list)
{
list.Clear();
SubPoolCompoundChildList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<float> list)
{
list.Clear();
SubPoolFloatList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<Collidable> list)
{
if (SubPoolCollidableList == null)
SubPoolCollidableList = new UnsafeResourcePool<RawList<Collidable>>();
list.Clear();
SubPoolCollidableList.GiveBack(list);
}
public bool LoadFromFile(
GraphicsDevice gd,
ResourceFactory factory,
string filename,
VertexLayoutDescription layout,
ModelCreateInfo?createInfo,
PostProcessSteps flags = DefaultPostProcessSteps)
{
// Load file
AssimpContext assimpContext = new AssimpContext();
Scene pScene = assimpContext.ImportFile(filename, flags);
parts.Clear();
parts.Count = (uint)pScene.Meshes.Count;
Vector3 scale = new Vector3(1.0f);
Vector2 uvscale = new Vector2(1.0f);
Vector3 center = new Vector3(0.0f);
if (createInfo != null)
{
scale = createInfo.Value.Scale;
uvscale = createInfo.Value.UVScale;
center = createInfo.Value.Center;
}
RawList <float> vertices = new RawList <float>();
RawList <uint> indices = new RawList <uint>();
vertexCount = 0;
_indexCount = 0;
// Load meshes
for (int i = 0; i < pScene.Meshes.Count; i++)
{
var paiMesh = pScene.Meshes[i];
parts[i] = new ModelPart();
parts[i].vertexBase = vertexCount;
parts[i].indexBase = _indexCount;
vertexCount += (uint)paiMesh.VertexCount;
var pColor = pScene.Materials[paiMesh.MaterialIndex].ColorDiffuse;
Vector3D Zero3D = new Vector3D(0.0f, 0.0f, 0.0f);
for (int j = 0; j < paiMesh.VertexCount; j++)
{
Vector3D pPos = paiMesh.Vertices[j];
Vector3D pNormal = paiMesh.Normals[j];
Vector3D pTexCoord = paiMesh.HasTextureCoords(0) ? paiMesh.TextureCoordinateChannels[0][j] : Zero3D;
Vector3D pTangent = paiMesh.HasTangentBasis ? paiMesh.Tangents[j] : Zero3D;
Vector3D pBiTangent = paiMesh.HasTangentBasis ? paiMesh.BiTangents[j] : Zero3D;
foreach (var component in layout.Elements)
{
switch (component.Semantic)
{
case VertexElementSemantic.Position:
vertices.Add(pPos.X * scale.X + center.X);
vertices.Add(-pPos.Y * scale.Y + center.Y);
vertices.Add(pPos.Z * scale.Z + center.Z);
break;
case VertexElementSemantic.Normal:
vertices.Add(pNormal.X);
vertices.Add(-pNormal.Y);
vertices.Add(pNormal.Z);
break;
case VertexElementSemantic.TextureCoordinate:
vertices.Add(pTexCoord.X * uvscale.X);
vertices.Add(pTexCoord.Y * uvscale.Y);
break;
case VertexElementSemantic.Color:
vertices.Add(pColor.R);
vertices.Add(pColor.G);
vertices.Add(pColor.B);
break;
default: throw new System.NotImplementedException();
}
;
}
dim.Max.X = Math.Max(pPos.X, dim.Max.X);
dim.Max.Y = Math.Max(pPos.Y, dim.Max.Y);
dim.Max.Z = Math.Max(pPos.Z, dim.Max.Z);
dim.Min.X = Math.Min(pPos.X, dim.Min.X);
dim.Min.Y = Math.Min(pPos.Y, dim.Min.Y);
dim.Min.Z = Math.Min(pPos.Z, dim.Min.Z);
}
dim.Size = dim.Max - dim.Min;
parts[i].vertexCount = (uint)paiMesh.VertexCount;
uint indexBase = indices.Count;
for (uint j = 0; j < paiMesh.FaceCount; j++)
{
Face Face = paiMesh.Faces[(int)j];
if (Face.IndexCount != 3)
{
continue;
}
indices.Add(indexBase + (uint)Face.Indices[0]);
indices.Add(indexBase + (uint)Face.Indices[1]);
indices.Add(indexBase + (uint)Face.Indices[2]);
parts[i].indexCount += 3;
_indexCount += 3;
}
}
uint vBufferSize = (vertices.Count) * sizeof(float);
uint iBufferSize = (indices.Count) * sizeof(uint);
_vertexBuffer = factory.CreateBuffer(new BufferDescription(vBufferSize, BufferUsage.VertexBuffer));
_indexBuffer = factory.CreateBuffer(new BufferDescription(iBufferSize, BufferUsage.IndexBuffer));
gd.UpdateBuffer(_vertexBuffer, 0, ref vertices[0], vBufferSize);
gd.UpdateBuffer(_indexBuffer, 0, ref indices[0], iBufferSize);
return(true);
}
/// <summary>
/// Identifies the indices of points in a set which are on the outer convex hull of the set.
/// </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>
public static void GetConvexHull(RawList <Vector3> points, RawList <int> outputTriangleIndices)
{
if (points.Count == 0)
{
throw new ArgumentException("Point set must have volume.");
}
RawList <int> outsidePoints = CommonResources.GetIntList();
if (outsidePoints.Capacity < points.Count - 4)
{
outsidePoints.Capacity = points.Count - 4;
}
//Build the initial tetrahedron.
//It will also give us the location of a point which is guaranteed to be within the
//final convex hull. We can use this point to calibrate the winding of triangles.
//A set of outside point candidates (all points other than those composing the tetrahedron) will be returned in the outsidePoints list.
//That list will then be further pruned by the RemoveInsidePoints call.
Vector3 insidePoint;
ComputeInitialTetrahedron(points, outsidePoints, outputTriangleIndices, out insidePoint);
//Compute outside points.
RemoveInsidePoints(points, outputTriangleIndices, outsidePoints);
RawList <int> edges = CommonResources.GetIntList();
RawList <int> toRemove = CommonResources.GetIntList();
RawList <int> newTriangles = CommonResources.GetIntList();
//We're now ready to begin the main loop.
while (outsidePoints.Count > 0)
//While the convex hull is incomplete...
{
for (int k = 0; k < outputTriangleIndices.Count; k += 3)
{
//Find the normal of the triangle
Vector3 normal;
FindNormal(outputTriangleIndices, points, k, out normal);
//Get the furthest point in the direction of the normal.
int maxIndexInOutsideList = GetExtremePoint(ref normal, points, outsidePoints);
int maxIndex = outsidePoints.Elements[maxIndexInOutsideList];
Vector3 maximum = points.Elements[maxIndex];
//If the point is beyond the current triangle, continue.
Vector3 offset;
Vector3.Subtract(ref maximum, ref points.Elements[outputTriangleIndices.Elements[k]], out offset);
float dot;
Vector3.Dot(ref normal, ref offset, out dot);
if (dot > 0)
{
//It's been picked! Remove the maximum point from the outside.
outsidePoints.FastRemoveAt(maxIndexInOutsideList);
//Remove any triangles that can see the point, including itself!
edges.Clear();
toRemove.Clear();
for (int n = outputTriangleIndices.Count - 3; n >= 0; n -= 3)
//Go through each triangle, if it can be seen, delete it and use maintainEdge on its edges.
{
if (IsTriangleVisibleFromPoint(outputTriangleIndices, points, n, ref maximum))
{
//This triangle can see it!
//TODO: CONSIDER CONSISTENT WINDING HAPPYTIMES
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 1], edges);
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 2], edges);
MaintainEdge(outputTriangleIndices[n + 1], outputTriangleIndices[n + 2], edges);
//Because fast removals are being used, the order is very important.
//It's pulling indices in from the end of the list in order, and also ensuring
//that we never issue a removal order beyond the end of the list.
outputTriangleIndices.FastRemoveAt(n + 2);
outputTriangleIndices.FastRemoveAt(n + 1);
outputTriangleIndices.FastRemoveAt(n);
}
}
//Create new triangles.
for (int n = 0; n < edges.Count; n += 2)
{
//For each edge, create a triangle with the extreme point.
newTriangles.Add(edges[n]);
newTriangles.Add(edges[n + 1]);
newTriangles.Add(maxIndex);
}
//Only verify the windings of the new triangles.
VerifyWindings(newTriangles, points, ref insidePoint);
outputTriangleIndices.AddRange(newTriangles);
newTriangles.Clear();
//Remove all points from the outsidePoints if they are inside the polyhedron
RemoveInsidePoints(points, outputTriangleIndices, outsidePoints);
//The list has been significantly messed with, so restart the loop.
break;
}
}
}
CommonResources.GiveBack(outsidePoints);
CommonResources.GiveBack(edges);
CommonResources.GiveBack(toRemove);
CommonResources.GiveBack(newTriangles);
}
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);
}
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;
int nextIndex;
if (isLoop)
{
prevIndex = (i - 1 + currentChain.Count) % currentChain.Count;
nextIndex = (i + 1) % currentChain.Count;
}
else
{
prevIndex = Math.Max(i - 1, 0);
nextIndex = Math.Min(i + 1, currentChain.Count - 1);
}
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;
int nextIndex;
if (isLoop)
{
prevIndex = (i - 1 + currentChain.Count) % currentChain.Count;
nextIndex = (i + 1) % currentChain.Count;
}
else
{
prevIndex = Math.Max(i - 1, 0);
nextIndex = Math.Min(i + 1, currentChain.Count - 1);
}
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));
}
}
/// <summary>
/// Updates the collection of supporting contacts.
/// </summary>
public void UpdateSupports(ref Vector3 movementDirection)
{
bool hadTraction = HasTraction;
//Reset traction/support.
HasTraction = false;
HasSupport = false;
Vector3 downDirection = characterBody.orientationMatrix.Down;
Vector3 bodyPosition = characterBody.position;
//Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body.
//Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii.
Vector3 extremePoint;
var convexShape = characterBody.CollisionInformation.Shape as ConvexShape;
Debug.Assert(convexShape != null, "Character bodies must be convex.");
//Find the lowest point on the collision shape.
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint);
BottomDistance = -extremePoint.Y + convexShape.collisionMargin;
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint);
float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X);
//Vertically, the rays will start at the same height as the character's center.
//While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape!
tractionContacts.Clear();
supportContacts.Clear();
sideContacts.Clear();
headContacts.Clear();
foreach (var pair in characterBody.CollisionInformation.Pairs)
{
//Don't stand on things that aren't really colliding fully.
if (pair.CollisionRule != CollisionRule.Normal)
{
continue;
}
ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
}
HasSupport = supportContacts.Count > 0;
HasTraction = tractionContacts.Count > 0;
//Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction.
//(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable.
//Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope.
//If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall.
//The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it.
//Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps.
//If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction,
//the ray test won't recover traction. This situation just isn't very common.)
if (!HasSupport && hadTraction)
{
float supportRayLength = maximumAssistedDownStepHeight + BottomDistance;
SupportRayData = null;
//If the contacts aren't available to support the character, raycast down to find the ground.
if (!HasTraction)
{
//TODO: could also require that the character has a nonzero movement direction in order to use a ray cast. Questionable- would complicate the behavior on edges.
Ray ray = new Ray(bodyPosition, downDirection);
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
SupportRayData = data;
HasTraction = data.HasTraction;
HasSupport = true;
}
}
//If contacts and the center ray cast failed, try a ray offset in the movement direction.
bool tryingToMove = movementDirection.LengthSquared() > 0;
if (!HasTraction && tryingToMove)
{
Ray ray = new Ray(
characterBody.Position +
movementDirection * rayCastInnerRadius, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = characterBody.Position;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
//If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
Vector3 horizontalOffset;
Vector3.Cross(ref movementDirection, ref downDirection, out horizontalOffset);
Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
//If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
Vector3 horizontalOffset;
Vector3.Cross(ref downDirection, ref movementDirection, out horizontalOffset);
Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
}
UpdateSupportData(ref downDirection);
UpdateVerticalSupportData(ref downDirection, ref movementDirection);
}
void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation)
{
var indices = Resources.GetIntList();
surfaceVertices.Clear();
Toolbox.GetConvexHull(data.vertices, indices, surfaceVertices);
for (int i = 0; i < surfaceVertices.count; i++)
{
AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]);
}
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);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<CompoundChild> list)
{
if (SubPoolCompoundChildList == null)
SubPoolCompoundChildList = new UnsafeResourcePool<RawList<CompoundChild>>();
list.Clear();
SubPoolCompoundChildList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<RayHit> list)
{
list.Clear();
SubPoolRayHitList.GiveBack(list);
}
private void UpdateSurfaceVertices()
{
hullVertices.Clear();
if (Volume > 0)
{
ConvexHullHelper.GetConvexHull(triangleMesh.Data.vertices, hullVertices);
var transformableData = triangleMesh.Data as TransformableMeshData;
if (transformableData != null)
{
var transform = transformableData.worldTransform;
for (int i = 0; i < hullVertices.Count; i++)
{
AffineTransform.Transform(ref hullVertices.Elements[i], ref transform, out hullVertices.Elements[i]);
}
}
}
else
{
hullVertices.Clear();
//A mobile mesh is allowed to have zero volume, so long as it isn't solid.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < triangleMesh.Data.vertices.Length; i++)
{
Vector3 v;
triangleMesh.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.
hullVertices.Add(box.Min);
hullVertices.Add(box.Max);
hullVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList <int> list)
{
list.Clear();
SubPoolIntList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList <float> list)
{
list.Clear();
SubPoolFloatList.GiveBack(list);
}
public override void UpdateCollision(float 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>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<BroadPhaseEntry> list)
{
list.Clear();
SubPoolCollisionEntryList.GiveBack(list);
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;
//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 = true;
MeshBoundingBoxTreeData meshData = mesh.Shape.TriangleMesh.Data;
RigidTransform mobileTriangleTransform, detectorTriangleTransform;
mobileTriangleTransform.Orientation = Quaternion.Identity;
detectorTriangleTransform.Orientation = Quaternion.Identity;
for (int i = 0; i < meshData.Indices.Length; i += 3)
{
//Grab a triangle associated with the mobile mesh.
meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position,
out mobileTriangleTransform.Position);
Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position);
Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);
//Go through all the detector volume triangles which are near the mobile mesh triangle.
bool triangleTouching, triangleContaining;
BoundingBox mobileBoundingBox;
mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
for (int j = 0; j < overlaps.Count; j++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA,
out detectorTriangle.vB, out detectorTriangle.vC);
Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB,
out detectorTriangleTransform.Position);
Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position,
out detectorTriangleTransform.Position);
Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f,
out detectorTriangleTransform.Position);
Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position,
out detectorTriangle.vA);
Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position,
out detectorTriangle.vB);
Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position,
out detectorTriangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform,
ref mobileTriangleTransform))
{
triangleTouching = true;
//The convex can't be fully contained if it's still touching the surface.
triangleContaining = false;
overlaps.Clear();
goto finishTriangleTest;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
if ((!Touching || Containing) &&
DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
{
triangleTouching = true;
triangleContaining = true;
goto finishTriangleTest;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
triangleTouching = false;
triangleContaining = false;
finishTriangleTest:
//Analyze the results of the triangle test.
if (triangleTouching)
{
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 (!triangleContaining) //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;
}
}
//There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
//In this case, we should be Touching, but currently we are not.
if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
{
//To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
//are in the mobile mesh.
//This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
Vector3 vertex;
DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
Ray ray;
ray.Direction = Vector3.Up;
RayHit hit;
RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
Touching = true;
}
}
NotifyDetectorVolumeOfChanges();
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<int> list)
{
list.Clear();
SubPoolIntList.GiveBack(list);
}
protected internal override void CleanUpOverlappingTriangles()
{
overlappedTriangles.Clear();
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<Vector3> list)
{
list.Clear();
SubPoolVectorList.GiveBack(list);
}
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>
/// Returns a resource to the pool.
/// </summary>
/// <param name="triangleIndices">TriangleIndices list to return.</param>
public static void GiveBack(RawList<BEPUphysics.CollisionTests.Manifolds.TriangleMeshConvexContactManifold.TriangleIndices> triangleIndices)
{
triangleIndices.Clear();
SubPoolTriangleIndicesList.GiveBack(triangleIndices);
}
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);
}
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;
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList <Vector3> list)
{
list.Clear();
SubPoolVectorList.GiveBack(list);
}
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;
}
}
}
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]);
}
}
}
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);
}
/// <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>
/// Returns a resource to the pool.
/// </summary>
/// <param name="triangleIndices">TriangleIndices list to return.</param>
public static void GiveBack(RawList<TriangleMeshConvexContactManifold.TriangleIndices> triangleIndices)
{
if (SubPoolTriangleIndicesList == null)
SubPoolTriangleIndicesList = new UnsafeResourcePool<RawList<TriangleMeshConvexContactManifold.TriangleIndices>>();
triangleIndices.Clear();
SubPoolTriangleIndicesList.GiveBack(triangleIndices);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<Collidable> list)
{
list.Clear();
SubPoolCollidableList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<Entity> list)
{
if (SubPoolEntityRawList == null)
SubPoolEntityRawList = new UnsafeResourcePool<RawList<Entity>>();
list.Clear();
SubPoolEntityRawList.GiveBack(list);
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="triangleIndices">TriangleIndices list to return.</param>
public static void GiveBack(RawList<TriangleMeshConvexContactManifold.TriangleIndices> triangleIndices)
{
triangleIndices.Clear();
SubPoolTriangleIndicesList.GiveBack(triangleIndices);
}
[Test] public void RemoveResetsReferenceTypesToDefault()
{
RawList<string> list = new RawList<string>(Enumerable.Range(0, 10).Select(i => i.ToString()));
// Is the internal array empty if not assigned otherwise?
if (list.Capacity > list.Count)
Assert.AreSame(null, list.Data[list.Count]);
// Adjusting the count shouldn't affect the internal array, just as documented
list.Count = 0;
for (int i = 0; i < 10; i++)
{
Assert.AreNotSame(null, list.Data[i]);
}
list.Count = 10;
// Check various types of removal and make sure the internal array is reset properly
{
// Remove an element
list.Remove("1");
Assert.AreSame(null, list.Data[list.Count]);
list.RemoveAt(5);
Assert.AreSame(null, list.Data[list.Count]);
// Remove a range
list.RemoveRange(0, 5);
for (int i = list.Count; i < list.Data.Length; i++)
{
Assert.AreSame(null, list.Data[i]);
}
// Clear the list
list.Clear();
for (int i = list.Count; i < list.Data.Length; i++)
{
Assert.AreSame(null, list.Data[i]);
}
}
}
/// <summary>
/// Returns a resource to the pool.
/// </summary>
/// <param name="list">List to return.</param>
public static void GiveBack(RawList<RayCastResult> list)
{
list.Clear();
SubPoolRayCastResultList.GiveBack(list);
}
internal bool IsPointContained(ref Vector3 point, RawList<int> triangles)
{
Vector3 rayDirection;
//Point from the approximate center of the mesh outwards.
//This is a cheap way to reduce the number of unnecessary checks when objects are external to the mesh.
Vector3.Add(ref boundingBox.Max, ref boundingBox.Min, out rayDirection);
Vector3.Multiply(ref rayDirection, .5f, out rayDirection);
Vector3.Subtract(ref point, ref rayDirection, out rayDirection);
//If the point is right in the middle, we'll need a backup.
if (rayDirection.LengthSquared() < .01f)
rayDirection = Vector3.Up;
var ray = new Ray(point, rayDirection);
triangleMesh.Tree.GetOverlaps(ray, triangles);
float minimumT = float.MaxValue;
bool minimumIsClockwise = false;
for (int i = 0; i < triangles.Count; i++)
{
Vector3 a, b, c;
triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c);
RayHit hit;
bool hitClockwise;
if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit))
{
if (hit.T < minimumT)
{
minimumT = hit.T;
minimumIsClockwise = hitClockwise;
}
}
}
triangles.Clear();
//If the first hit is on the inner surface, then the ray started inside the mesh.
return minimumT < float.MaxValue && minimumIsClockwise == innerFacingIsClockwise;
}
/// <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((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)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:
Fix64 rayLength = 100;
RawList <Ray> rays = new RawList <Ray>();
for (int i = 0; i < numRuns; i++)
{
rays.Add(new Ray()
{
Position = new Vector3((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z),
Direction = Vector3.Normalize(new Vector3((Fix64)(rand.NextDouble() - .5), (Fix64)(rand.NextDouble() - .5), (Fix64)(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:
Fix64 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((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)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;
}
