public bool HassSolidEntity(Location min, Location max)
{
// TODO: Better alg!
BoundingBox bb = new BoundingBox(min.ToBVector(), max.ToBVector());
List<BroadPhaseEntry> entries = new List<BroadPhaseEntry>();
PhysicsWorld.BroadPhase.QueryAccelerator.GetEntries(bb, entries);
if (entries.Count == 0)
{
return false;
}
Location center = (max + min) * 0.5;
Location rel = max - min;
BoxShape box = new BoxShape((double)rel.X, (double)rel.Y, (double)rel.Z);
RigidTransform start = new RigidTransform(center.ToBVector(), Quaternion.Identity);
Vector3 sweep = new Vector3(0, 0, 0.01f);
RayHit rh;
foreach (BroadPhaseEntry entry in entries)
{
if (entry is EntityCollidable && Collision.ShouldCollide(entry) &&
entry.CollisionRules.Group != CollisionUtil.Player &&
entry.ConvexCast(box, ref start, ref sweep, out rh))
{
return true;
}
}
return false;
}
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 } );
}
}
}
}
}
public void GetBoundingBox( ref Vector3 position, out BoundingBox boundingBox )
{
var size = new Vector3( CellWidth * VoxelSector.ZVOXELBLOCSIZE_X
, CellWidth * VoxelSector.ZVOXELBLOCSIZE_Y
, CellWidth * VoxelSector.ZVOXELBLOCSIZE_Z );
boundingBox.Min = position;
Vector3.Add( ref size, ref position, out boundingBox.Max );
}
/// <summary>
/// Gets the bounding box of an element in the data.
/// </summary>
/// <param name="triangleIndex">Index of the triangle in the data.</param>
/// <param name="boundingBox">Bounding box of the triangle.</param>
public void GetBoundingBox(int triangleIndex, out BoundingBox boundingBox)
{
Vector3 v1, v2, v3;
GetTriangle(triangleIndex, out v1, out v2, out v3);
Vector3.Min(ref v1, ref v2, out boundingBox.Min);
Vector3.Min(ref boundingBox.Min, ref v3, out boundingBox.Min);
Vector3.Max(ref v1, ref v2, out boundingBox.Max);
Vector3.Max(ref boundingBox.Max, ref v3, out boundingBox.Max);
}
public MobileChunkCollidable(MobileChunkShape shape)
{
ChunkShape = shape;
base.Shape = ChunkShape;
Vector3 max = new Vector3(shape.ChunkSize.X, shape.ChunkSize.Y, shape.ChunkSize.Z);
boundingBox = new BoundingBox(-max, max);
Events = new ContactEventManager<EntityCollidable>();
LocalPosition = -shape.Center;
}
/// <summary>
/// Determines if a bounding box intersects another bounding box.
/// </summary>
/// <param name="boundingBox">Bounding box to test against.</param>
/// <returns>Whether the bounding boxes intersected.</returns>
public bool Intersects(BoundingBox boundingBox)
{
if (boundingBox.Min.X > Max.X || boundingBox.Min.Y > Max.Y || boundingBox.Min.Z > Max.Z)
return false;
if (Min.X > boundingBox.Max.X || Min.Y > boundingBox.Max.Y || Min.Z > boundingBox.Max.Z)
return false;
return true;
}
public FullChunkObject(Vector3 pos, BlockInternal[] blocks)
{
ChunkShape = new FullChunkShape(blocks);
base.Shape = ChunkShape;
Position = pos;
boundingBox = new BoundingBox(Position, Position + new Vector3(30, 30, 30));
Events = new ContactEventManager<FullChunkObject>(this);
Material.Bounciness = 0.75f;
}
/// <summary>
/// Creates a bounding box from a bounding sphere.
/// </summary>
/// <param name="boundingSphere">Bounding sphere to be used to create the bounding box.</param>
/// <param name="boundingBox">Bounding box created from the bounding sphere.</param>
public static void CreateFromSphere(ref BoundingSphere boundingSphere, out BoundingBox boundingBox)
{
boundingBox.Min.X = boundingSphere.Center.X - boundingSphere.Radius;
boundingBox.Min.Y = boundingSphere.Center.Y - boundingSphere.Radius;
boundingBox.Min.Z = boundingSphere.Center.Z - boundingSphere.Radius;
boundingBox.Max.X = boundingSphere.Center.X + boundingSphere.Radius;
boundingBox.Max.Y = boundingSphere.Center.Y + boundingSphere.Radius;
boundingBox.Max.Z = boundingSphere.Center.Z + boundingSphere.Radius;
}
/// <summary>
/// Gets the triangles whose bounding boxes are overlapped by the query.
/// </summary>
/// <param name="boundingBox">Shape to query against the tree.</param>
/// <param name="outputOverlappedElements">Indices of triangles in the index buffer with bounding boxes which are overlapped by the query.</param>
/// <returns>Whether or not any elements were overlapped.</returns>
public bool GetOverlaps(BoundingBox boundingBox, IList<int> outputOverlappedElements)
{
if (root != null)
{
bool intersects;
root.BoundingBox.Intersects(ref boundingBox, out intersects);
if (intersects)
root.GetOverlaps(ref boundingBox, outputOverlappedElements);
}
return outputOverlappedElements.Count > 0;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
boundingBox.Min.X = shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X = shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public LotsOfBoxesTestDemo(DemosGame game)
: base(game)
{
var ground = new Box(new Vector3(0, -.5f, 0), 200, 1, 200);
Space.Add(ground);
var spawnVolume = new BoundingBox
{
Min = new Vector3(-25, 2, -25),
Max = new Vector3(25, 102, 25)
};
var span = spawnVolume.Max - spawnVolume.Min;
NarrowPhaseHelper.Factories.BoxBox.EnsureCount(30000);
var random = new Random(5);
for (int i = 0; i < 5000; ++i)
{
Vector3 position;
position.X = spawnVolume.Min.X + (float)random.NextDouble() * span.X;
position.Y = spawnVolume.Min.Y + (float)random.NextDouble() * span.Y;
position.Z = spawnVolume.Min.Z + (float)random.NextDouble() * span.Z;
var entity = new Box(position, 2, 2, 2, 10);
Space.Add(entity);
}
for (int i = 0; i < 100; ++i)
{
Space.Update();
}
Process.GetCurrentProcess().PriorityClass = ProcessPriorityClass.RealTime;
var start = Stopwatch.GetTimestamp();
for (int i = 0; i < 200; ++i)
{
Space.Update();
}
var end = Stopwatch.GetTimestamp();
var time = (end - start) / (double)Stopwatch.Frequency;
Console.WriteLine("Time: {0}", time);
game.Camera.Position = new Vector3(-10, 10, 10);
game.Camera.Yaw((float)Math.PI / -4f);
game.Camera.Pitch((float)Math.PI / 9f);
}
/// <summary>
/// Determines if a bounding box intersects another bounding box.
/// </summary>
/// <param name="boundingBox">Bounding box to test against.</param>
/// <param name="intersects">Whether the bounding boxes intersect.</param>
public void Intersects(ref BoundingBox boundingBox, out bool intersects)
{
if (boundingBox.Min.X > Max.X || boundingBox.Min.Y > Max.Y || boundingBox.Min.Z > Max.Z)
{
intersects = false;
return;
}
if (Min.X > boundingBox.Max.X || Min.Y > boundingBox.Max.Y || Min.Z > boundingBox.Max.Z)
{
intersects = false;
return;
}
intersects = true;
}
/// <summary>
/// Method defining the loading of the model in our space
/// </summary>
/// <param name="space">the current space</param>
/// <param name="game">the current game</param>
public void Load(Space space, Game game)
{
//Using the ModelDataExtractor in order to extract the bounding of the model and activate the collisions
ModelDataExtractor.GetVerticesAndIndicesFromModel(ModelLevel, out Vector3[] vertices, out int[] indices);
var meshLevel = new StaticMesh(vertices, indices, new AffineTransform(new Vector3(0, -40, 0)));
BoundingLevel = meshLevel.BoundingBox;
space.Add(meshLevel);
game.Components.Add(new StaticModel(ModelLevel, meshLevel.WorldTransform.Matrix, game));
ModelDataExtractor.GetVerticesAndIndicesFromModel(ModelArrival, out vertices, out indices);
var meshArrival = new StaticMesh(vertices, indices, new AffineTransform(new Vector3(0, -40, 0)));
BoundingArrive = meshArrival.BoundingBox;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
Vector3 left;
direction = new Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
Vector3 up;
direction = new Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
Vector3 down;
direction = new Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
Vector3 backward;
direction = new Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Vector3 forward;
direction = new Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
static void DrawBoundingBox( Display render, BoundingBox bb )
{
///Box box = e as Box;
// = e.CollisionInformation.BoundingBox;
//Vector3 corner = Matrix.TransformNormal( Vector3.One, e.WorldTransform );
Vector3[] corners = new Vector3[8];
//Vector3 half_size = new Vector3( box.HalfWidth, box.HalfHeight, box.HalfLength );
corners[0] = bb.Min;
corners[6] = bb.Max;
corners[1] = corners[0];
corners[1].X = corners[6].X;
corners[2] = corners[0];
corners[2].X = corners[6].X;
corners[2].Y = corners[6].Y;
corners[3] = corners[0];
corners[3].Y = corners[6].Y;
corners[4] = corners[0];
corners[4].Z = corners[6].Z;
corners[5] = corners[0];
corners[5].X = corners[6].X;
corners[5].Z = corners[6].Z;
corners[7] = corners[0];
corners[7].Y = corners[6].Y;
corners[7].Z = corners[6].Z;
Vector3 white = Vector3.One;
white.Y = 0;
white.Z = 0;
drawLine( render, ref corners[0], ref corners[1], ref white, ref white );
drawLine( render, ref corners[1], ref corners[2], ref white, ref white );
drawLine( render, ref corners[2], ref corners[3], ref white, ref white );
drawLine( render, ref corners[3], ref corners[0], ref white, ref white );
drawLine( render, ref corners[0], ref corners[4], ref white, ref white );
drawLine( render, ref corners[1], ref corners[5], ref white, ref white );
drawLine( render, ref corners[2], ref corners[6], ref white, ref white );
drawLine( render, ref corners[3], ref corners[7], ref white, ref white );
drawLine( render, ref corners[4], ref corners[5], ref white, ref white );
drawLine( render, ref corners[5], ref corners[6], ref white, ref white );
drawLine( render, ref corners[6], ref corners[7], ref white, ref white );
drawLine( render, ref corners[7], ref corners[4], ref white, ref white );
}
public static void Draw(this BEPUutilities.BoundingBox box)
{
if (RenderingDevice.HiDef)
{
effect.View = MathConverter.Convert(Renderer.Camera.ViewMatrix);
effect.Projection = MathConverter.Convert(Renderer.Camera.ProjectionMatrix);
BEPUutilities.Vector3[] corners = box.GetCorners();
for (int i = 0; i < 8; i++)
{
verts[i].Position = corners[i];
verts[i].Color = Color.Goldenrod;
}
foreach (EffectPass pass in effect.CurrentTechnique.Passes)
{
pass.Apply();
Renderer.GraphicsDevice.DrawUserIndexedPrimitives(PrimitiveType.LineList, verts, 0, 8, indices, 0, indices.Length / 2);
}
}
}
///<summary>
/// Computes the bounding box of the transformed mesh shape.
///</summary>
///<param name="transform">Transform to apply to the shape during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
public void ComputeBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
float minX = float.MaxValue;
float minY = float.MaxValue;
float minZ = float.MaxValue;
float maxX = -float.MaxValue;
float maxY = -float.MaxValue;
float maxZ = -float.MaxValue;
for (int i = 0; i < triangleMesh.Data.vertices.Length; i++)
{
System.Numerics.Vector3 vertex;
triangleMesh.Data.GetVertexPosition(i, out vertex);
Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex);
if (vertex.X < minX)
minX = vertex.X;
if (vertex.X > maxX)
maxX = vertex.X;
if (vertex.Y < minY)
minY = vertex.Y;
if (vertex.Y > maxY)
maxY = vertex.Y;
if (vertex.Z < minZ)
minZ = vertex.Z;
if (vertex.Z > maxZ)
maxZ = vertex.Z;
}
boundingBox.Min.X = transform.Translation.X + minX;
boundingBox.Min.Y = transform.Translation.Y + minY;
boundingBox.Min.Z = transform.Translation.Z + minZ;
boundingBox.Max.X = transform.Translation.X + maxX;
boundingBox.Max.Y = transform.Translation.Y + maxY;
boundingBox.Max.Z = transform.Translation.Z + maxZ;
}
public void GetEntries(BoundingBox boundingShape, IList<BroadPhaseEntry> overlaps)
{
//Compute the min and max of the bounding box.
//Loop through the cells and select bounding boxes which overlap the x axis.
Int2 min, max;
Grid2DSortAndSweep.ComputeCell(ref boundingShape.Min, out min);
Grid2DSortAndSweep.ComputeCell(ref boundingShape.Max, out max);
for (int i = min.Y; i <= max.Y; i++)
{
for (int j = min.Z; j <= max.Z; j++)
{
//Grab the cell that we are currently in.
Int2 cellIndex;
cellIndex.Y = i;
cellIndex.Z = j;
GridCell2D cell;
if (owner.cellSet.TryGetCell(ref cellIndex, out cell))
{
//To fully accelerate this, the entries list would need to contain both min and max interval markers.
//Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list.
//Consider some giant bounding box that spans the entire list.
for (int k = 0; k < cell.entries.Count
&& cell.entries.Elements[k].item.boundingBox.Min.X <= boundingShape.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test.
{
bool intersects;
var item = cell.entries.Elements[k].item;
boundingShape.Intersects(ref item.boundingBox, out intersects);
if (intersects && !overlaps.Contains(item))
{
overlaps.Add(item);
}
}
}
}
}
}
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>
/// Determines if and when the ray intersects the bounding box.
/// </summary>
/// <param name="boundingBox">Bounding box to test against.</param>
/// <param name="result">The length along the ray to the impact, or null if no impact is found.</param>
public void Intersects(ref BoundingBox boundingBox, out float? result)
{
if (System.Math.Abs(Direction.X) < Toolbox.Epsilon && (Position.X < boundingBox.Min.X || Position.X > boundingBox.Max.X))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it
//can't be intersecting.
result = null;
return;
}
float tmin = 0, tmax = float.MaxValue;
float inverseDirection = 1 / Direction.X;
float t1 = (boundingBox.Min.X - Position.X) * inverseDirection;
float t2 = (boundingBox.Max.X - Position.X) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = System.Math.Max(tmin, t1);
tmax = System.Math.Min(tmax, t2);
if (tmin > tmax)
{
result = null;
return;
}
if (System.Math.Abs(Direction.Y) < Toolbox.Epsilon && (Position.Y < boundingBox.Min.Y || Position.Y > boundingBox.Max.Y))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it
//can't be intersecting.
result = null;
return;
}
inverseDirection = 1 / Direction.Y;
t1 = (boundingBox.Min.Y - Position.Y) * inverseDirection;
t2 = (boundingBox.Max.Y - Position.Y) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = System.Math.Max(tmin, t1);
tmax = System.Math.Min(tmax, t2);
if (tmin > tmax)
{
result = null;
return;
}
if (System.Math.Abs(Direction.Z) < Toolbox.Epsilon && (Position.Z < boundingBox.Min.Z || Position.Z > boundingBox.Max.Z))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it
//can't be intersecting.
result = null;
return;
}
inverseDirection = 1 / Direction.Z;
t1 = (boundingBox.Min.Z - Position.Z) * inverseDirection;
t2 = (boundingBox.Max.Z - Position.Z) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = System.Math.Max(tmin, t1);
tmax = System.Math.Min(tmax, t2);
if (tmin > tmax)
{
result = null;
return;
}
result = tmin;
}
/// <summary>
/// Determines if and when the ray intersects the bounding box.
/// </summary>
/// <param name="boundingBox">Bounding box to test against.</param>
/// <returns>The length along the ray to the impact, or null if no impact is found.</returns>
public float? Intersects(BoundingBox boundingBox)
{
float? toReturn;
Intersects(ref boundingBox, out toReturn);
return toReturn;
}
/// <summary>
/// Computes a bounding box for the shape given the specified transform.
/// </summary>
/// <param name="transform">Transform to apply to the shape to compute the bounding box.</param>
/// <param name="boundingBox">Bounding box for the shape given the transform.</param>
public override void GetBoundingBox(ref RigidTransform transform, out BoundingBox boundingBox)
{
RigidTransform combinedTransform;
RigidTransform.Transform(ref shapes.Elements[0].LocalTransform, ref transform, out combinedTransform);
shapes.Elements[0].Shape.GetBoundingBox(ref combinedTransform, out boundingBox);
for (int i = 0; i < shapes.Count; i++)
{
RigidTransform.Transform(ref shapes.Elements[i].LocalTransform, ref transform, out combinedTransform);
BoundingBox childBoundingBox;
shapes.Elements[i].Shape.GetBoundingBox(ref combinedTransform, out childBoundingBox);
BoundingBox.CreateMerged(ref boundingBox, ref childBoundingBox, out boundingBox);
}
}
void GetSamplingOrigin(ref BoundingBox entityBoundingBox, out System.Numerics.Vector3 xSpacing, out System.Numerics.Vector3 zSpacing, out float perColumnArea, out System.Numerics.Vector3 origin)
{
//Compute spacing and increment informaiton.
float widthIncrement = (entityBoundingBox.Max.X - entityBoundingBox.Min.X) / samplePointsPerDimension;
float lengthIncrement = (entityBoundingBox.Max.Z - entityBoundingBox.Min.Z) / samplePointsPerDimension;
xSpacing = new System.Numerics.Vector3(widthIncrement, 0, 0);
zSpacing = new System.Numerics.Vector3(0, 0, lengthIncrement);
QuaternionEx.Transform(ref xSpacing, ref surfaceTransform.Orientation, out xSpacing);
QuaternionEx.Transform(ref zSpacing, ref surfaceTransform.Orientation, out zSpacing);
perColumnArea = widthIncrement * lengthIncrement;
//Compute the origin.
System.Numerics.Vector3 minimum;
RigidTransform.Transform(ref entityBoundingBox.Min, ref surfaceTransform, out minimum);
//Matrix3X3.TransformTranspose(ref entityBoundingBox.Min, ref surfaceOrientationTranspose, out minimum);
System.Numerics.Vector3 offset;
Vector3Ex.Multiply(ref xSpacing, .5f, out offset);
Vector3Ex.Add(ref minimum, ref offset, out origin);
Vector3Ex.Multiply(ref zSpacing, .5f, out offset);
Vector3Ex.Add(ref origin, ref offset, out origin);
//TODO: Could adjust the grid origin such that a ray always hits the deepest point.
//The below code is a prototype of the idea, but has bugs.
//var convexInfo = collidable as ConvexCollisionInformation;
//if (convexInfo != null)
//{
// System.Numerics.Vector3 dir;
// Vector3Ex.Negate(ref upVector, out dir);
// System.Numerics.Vector3 extremePoint;
// convexInfo.Shape.GetExtremePoint(dir, ref convexInfo.worldTransform, out extremePoint);
// //Use extreme point to snap to grid.
// Vector3Ex.Subtract(ref extremePoint, ref origin, out offset);
// float offsetX, offsetZ;
// Vector3Ex.Dot(ref offset, ref right, out offsetX);
// Vector3Ex.Dot(ref offset, ref backward, out offsetZ);
// offsetX %= widthIncrement;
// offsetZ %= lengthIncrement;
// if (offsetX > .5f * widthIncrement)
// {
// Vector3Ex.Multiply(ref right, 1 - offsetX, out offset);
// }
// else
// {
// Vector3Ex.Multiply(ref right, -offsetX, out offset);
// }
// if (offsetZ > .5f * lengthIncrement)
// {
// System.Numerics.Vector3 temp;
// Vector3Ex.Multiply(ref right, 1 - offsetZ, out temp);
// Vector3Ex.Add(ref temp, ref offset, out offset);
// }
// else
// {
// System.Numerics.Vector3 temp;
// Vector3Ex.Multiply(ref right, -offsetZ, out temp);
// Vector3Ex.Add(ref temp, ref offset, out offset);
// }
// Vector3Ex.Add(ref origin, ref offset, out origin);
//}
}
/// <summary>
/// Builds and spawns the body into the world.
/// </summary>
public virtual void SpawnBody()
{
if (Body != null)
{
DestroyBody();
}
Body = new BEPUphysics.Entities.Entity(Shape, Mass);
Body.CollisionInformation.CollisionRules.Group = CGroup;
InternalOffset = new Location(Body.Position);
Body.AngularVelocity = new Vector3((float)AVel.X, (float)AVel.Y, (float)AVel.Z);
Body.LinearVelocity = new Vector3((float)LVel.X, (float)LVel.Y, (float)LVel.Z);
Body.WorldTransform = WorldTransform; // TODO: Position + Quaternion
Body.PositionUpdateMode = BEPUphysics.PositionUpdating.PositionUpdateMode.Passive;
if (!CanRotate)
{
Body.AngularDamping = 1;
}
// TODO: Other settings
// TODO: Gravity
Body.Tag = this;
SetFriction(Friction);
SetBounciness(Bounciness);
TheRegion.PhysicsWorld.Add(Body);
for (int i = 0; i < Joints.Count; i++)
{
if (Joints[i] is BaseJoint)
{
BaseJoint joint = (BaseJoint)Joints[i];
joint.CurrentJoint = joint.GetBaseJoint();
TheRegion.PhysicsWorld.Add(joint.CurrentJoint);
}
}
ShadowCastShape = Shape.GetCollidableInstance().BoundingBox;
ShadowMainDupe = Shape.GetCollidableInstance().BoundingBox;
ShadowCenter = GetPosition();
}
/// <summary>
/// Computes the bounding box of three points.
/// </summary>
/// <param name="a">First vertex of the triangle.</param>
/// <param name="b">Second vertex of the triangle.</param>
/// <param name="c">Third vertex of the triangle.</param>
/// <param name="aabb">Bounding box of the triangle.</param>
public static void GetTriangleBoundingBox(ref Vector3 a, ref Vector3 b, ref Vector3 c, out BoundingBox aabb)
{
#if !WINDOWS
aabb = new BoundingBox();
#endif
//X axis
if (a.X > b.X && a.X > c.X)
{
//A is max
aabb.Max.X = a.X;
aabb.Min.X = b.X > c.X ? c.X : b.X;
}
else if (b.X > c.X)
{
//B is max
aabb.Max.X = b.X;
aabb.Min.X = a.X > c.X ? c.X : a.X;
}
else
{
//C is max
aabb.Max.X = c.X;
aabb.Min.X = a.X > b.X ? b.X : a.X;
}
//Y axis
if (a.Y > b.Y && a.Y > c.Y)
{
//A is max
aabb.Max.Y = a.Y;
aabb.Min.Y = b.Y > c.Y ? c.Y : b.Y;
}
else if (b.Y > c.Y)
{
//B is max
aabb.Max.Y = b.Y;
aabb.Min.Y = a.Y > c.Y ? c.Y : a.Y;
}
else
{
//C is max
aabb.Max.Y = c.Y;
aabb.Min.Y = a.Y > b.Y ? b.Y : a.Y;
}
//Z axis
if (a.Z > b.Z && a.Z > c.Z)
{
//A is max
aabb.Max.Z = a.Z;
aabb.Min.Z = b.Z > c.Z ? c.Z : b.Z;
}
else if (b.Z > c.Z)
{
//B is max
aabb.Max.Z = b.Z;
aabb.Min.Z = a.Z > c.Z ? c.Z : a.Z;
}
else
{
//C is max
aabb.Max.Z = c.Z;
aabb.Min.Z = a.Z > b.Z ? b.Z : a.Z;
}
}
int BuildPileSimulation(Space space)
{
Random rand = new Random(0);
#if WINDOWS
NarrowPhaseHelper.Factories.BoxBox.EnsureCount(30000);
BoundingBox box = new BoundingBox(new Vector3(-5, 10, -5), new Vector3(5, 300, 5));
for (int k = 0; k < 5000; k++)
#else
NarrowPhaseHelper.Factories.BoxBox.EnsureCount(1500);
BoundingBox box = new BoundingBox(new Vector3(-5, 10, -5), new Vector3(5, 20, 5));
for (int k = 0; k < 250; k++)
#endif
{
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));
var toAdd = new Box(position, 1, 1, 1, 1);
toAdd.ActivityInformation.IsAlwaysActive = true;
space.Add(toAdd);
}
Box ground = new Box(new Vector3(0, 0, 0), 300, 10, 300);
space.Add(ground);
#if WINDOWS
return 700;
#else
return 350;
#endif
}
public static void Validate(this BoundingBox b)
{
b.Min.Validate();
b.Max.Validate();
}
/// <summary>
/// Expands a bounding box by the given sweep.
/// </summary>
/// <param name="boundingBox">Bounding box to expand.</param>
/// <param name="sweep">Sweep to expand the bounding box with.</param>
public static void ExpandBoundingBox(ref BoundingBox boundingBox, ref Vector3 sweep)
{
if (sweep.X > 0)
boundingBox.Max.X += sweep.X;
else
boundingBox.Min.X += sweep.X;
if (sweep.Y > 0)
boundingBox.Max.Y += sweep.Y;
else
boundingBox.Min.Y += sweep.Y;
if (sweep.Z > 0)
boundingBox.Max.Z += sweep.Z;
else
boundingBox.Min.Z += sweep.Z;
}
Location SkyMod(Location pos, Location norm, float light)
{
if (light > 0 && TheClient.CVars.r_treeshadows.ValueB)
{
BoundingBox bb = new BoundingBox(pos.ToBVector(), (pos + new Location(1, 1, 300)).ToBVector());
if (GenShadowCasters != null)
{
for (int i = 0; i < GenShadowCasters.Length; i++)
{
PhysicsEntity pe = GenShadowCasters[i];
if (pe.GenBlockShadows && pe.ShadowCenter.DistanceSquared_Flat(pos) < pe.ShadowRadiusSquaredXY)
{
light -= 0.05f;
if (pe.ShadowMainDupe.Intersects(bb))
{
light = 0;
break;
}
if (pe.ShadowCastShape.Intersects(bb))
{
light -= 0.1f;
}
if (light <= 0)
{
light = 0;
break;
}
}
}
}
}
return Math.Max(norm.Dot(SunLightPathNegative), 0.5) * new Location(light) * SkyLightMod;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var right = new System.Numerics.Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);
var up = new System.Numerics.Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);
var backward = new System.Numerics.Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 3 required values directly.
System.Numerics.Vector3 offset;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out offset);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
Vector3Ex.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max);
Vector3Ex.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min);
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
System.Numerics.Vector3 a, b, c;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
Matrix3x3.Transform(ref vA, ref o, out a);
Matrix3x3.Transform(ref vB, ref o, out b);
Matrix3x3.Transform(ref vC, ref o, out c);
Vector3Ex.Min(ref a, ref b, out boundingBox.Min);
Vector3Ex.Min(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3Ex.Max(ref a, ref b, out boundingBox.Max);
Vector3Ex.Max(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
/// <summary>
/// Creates the smallest bounding box which contains two other bounding boxes.
/// </summary>
/// <param name="a">First bounding box to be contained.</param>
/// <param name="b">Second bounding box to be contained.</param>
/// <param name="merged">Smallest bounding box which contains the two input bounding boxes.</param>
public static void CreateMerged(ref BoundingBox a, ref BoundingBox b, out BoundingBox merged)
{
if (a.Min.X < b.Min.X)
merged.Min.X = a.Min.X;
else
merged.Min.X = b.Min.X;
if (a.Min.Y < b.Min.Y)
merged.Min.Y = a.Min.Y;
else
merged.Min.Y = b.Min.Y;
if (a.Min.Z < b.Min.Z)
merged.Min.Z = a.Min.Z;
else
merged.Min.Z = b.Min.Z;
if (a.Max.X > b.Max.X)
merged.Max.X = a.Max.X;
else
merged.Max.X = b.Max.X;
if (a.Max.Y > b.Max.Y)
merged.Max.Y = a.Max.Y;
else
merged.Max.Y = b.Max.Y;
if (a.Max.Z > b.Max.Z)
merged.Max.Z = a.Max.Z;
else
merged.Max.Z = b.Max.Z;
}
/// <summary>
/// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles.
/// </summary>
public void RecalculateBoundingBox()
{
var points = CommonResources.GetVectorList();
foreach (var tri in SurfaceTriangles)
{
points.Add(tri[0]);
points.Add(tri[1]);
points.Add(tri[2]);
points.Add(tri[0] - upVector * MaxDepth);
points.Add(tri[1] - upVector * MaxDepth);
points.Add(tri[2] - upVector * MaxDepth);
}
boundingBox = BoundingBox.CreateFromPoints(points);
CommonResources.GiveBack(points);
//Compute the transforms used to pull objects into fluid local space.
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation);
Matrix3x3.CreateFromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix);
surfaceTransform.Position = surfaceTriangles[0][0];
}
