public void InetersectsRectangleF(float x, float y, float width, float height, bool expected)
{
var rect = new RectangleF(x, y, width, height);
var sut = new AxisAlignedBoundingBox(new Vector3(-2), new Vector3(2));
Assert.Equal(expected, sut.Intersects(rect));
}
public void IntersectsPlane(float x, float y, float z, float d, PlaneIntersectionType expected)
{
var plane = new Plane(x, y, z, d);
var sut = new AxisAlignedBoundingBox(new Vector3(-2), new Vector3(2));
Assert.Equal(expected, sut.Intersects(plane));
}
bool IsBlockPlacementSafe(IndexPosition blockPos, IndexPosition chunkPos)
{
Vector3 pos = Chunk.ChunkBlockToWorldCoords(chunkPos, blockPos);
Vector3 halfSize = Block.CUBE_3D_SIZE / 2f;
AxisAlignedBoundingBox aabb = new AxisAlignedBoundingBox(pos - halfSize, pos + halfSize);
return(!aabb.Intersects(ownerPlayerPhysicsBody.GetCollider()));
}
public void IntersectsAxisAlignedBoundingBox(Vector3 min, Vector3 max, bool expected)
{
var sut = new AxisAlignedBoundingBox(new Vector3(-2), new Vector3(2));
var other = new AxisAlignedBoundingBox(min, max);
Assert.Equal(expected, sut.Intersects(other));
}
bool CanIntersect(bool p1Static, AxisAlignedBoundingBox c1, AxisAlignedBoundingBox b1,
bool p2Static, AxisAlignedBoundingBox c2, AxisAlignedBoundingBox b2)
{
if (p1Static && p2Static)
{
return(false);
}
else if (p1Static && !p2Static)
{
return(b2.Intersects(c1));
}
else if (!p1Static && p2Static)
{
return(b1.Intersects(c2));
}
else
{
return(b1.Intersects(b2));
}
}
/// <summary>
/// WARNING: Not finalized!
/// </summary>
public bool AABBIntersectsTerrain(AxisAlignedBoundingBox aabb, out float highestY)
{
highestY = float.MinValue;
bool intersects = false;
for (int x = -2; x <= 2; x++)
{
for (int y = -2; y <= 2; y++)
{
for (int z = -2; z <= 2; z++)
{
Vector3 off = new Vector3(Block.CUBE_SIZE * x, Block.CUBE_SIZE * y, Block.CUBE_SIZE * z);
IndexPosition bpos = Chunk.WorldToBlockCoords(aabb.Center + off);
IndexPosition cpos = Terrain.WorldToChunkCoords(aabb.Center + off);
bpos = Chunk.BlockToChunkBlockCoords(cpos, bpos);
Chunk chunk;
if (Terrain.Chunks.TryGetValue(cpos, out chunk))
{
if (chunk.Blocks[bpos.Z, bpos.Y, bpos.X].HasCollision())
{
Vector3 cubeWorldPos = chunk.Position + (bpos * Block.CUBE_3D_SIZE) - Block.HALF_CUBE_3D_SIZE;
AxisAlignedBoundingBox AABoundingBox =
new AxisAlignedBoundingBox(cubeWorldPos, cubeWorldPos + Block.CUBE_3D_SIZE);
if (aabb.Intersects(AABoundingBox))
{
intersects = true;
highestY = Math.Max(highestY, AABoundingBox.Max.Y);
}
}
}
}
}
}
return(intersects);
}
public IEnumerable <PhysicsBodyComponent> GetBroadphaseIntersections(AxisAlignedBoundingBox broad)
{
_terrainBlockCache.Clear();
// Convert the broad AABB to an IndexPosition AABB
IndexPosition min = new IndexPosition(
Maths.NegativeRound(broad.Min.X / Block.CUBE_SIZE),
Maths.NegativeRound(broad.Min.Y / Block.CUBE_SIZE),
Maths.NegativeRound(broad.Min.Z / Block.CUBE_SIZE));
IndexPosition max = new IndexPosition(
(int)Math.Ceiling(broad.Max.X / Block.CUBE_SIZE),
(int)Math.Ceiling(broad.Max.Y / Block.CUBE_SIZE),
(int)Math.Ceiling(broad.Max.Z / Block.CUBE_SIZE));
// Calculate the chunk index to use as reference
IndexPosition chunkIndex = Terrain.WorldToChunkCoords(broad.Center);
// Try each block
for (int x = min.X; x <= max.X; x++)
{
for (int y = min.Y; y <= max.Y; y++)
{
for (int z = min.Z; z <= max.Z; z++)
{
// Calculate the index positions for the current block
IndexPosition blockIndexWorld = new IndexPosition(x, y, z);
IndexPosition blockChunkIndex = Chunk.BlockToChunkBlockCoords(chunkIndex, blockIndexWorld);
// Find the block
Chunk chunk;
int fx, fy, fz;
Block block = Terrain.FindBlock(chunkIndex, blockChunkIndex.X, blockChunkIndex.Y, blockChunkIndex.Z,
out fx, out fy, out fz, out chunk);
// If this block has collision, process it
if (block.HasCollision())
{
IndexPosition blockIPos = new IndexPosition(fx, fy, fz);
// Calculate the blocks world position and create a PhyicsBlock from it
Vector3 blockWorldPosition = Chunk.ChunkBlockToWorldCoords(chunk.Position, blockIPos);
PhysicsBlock physBlock = GetNewPhysicsBlock(block, blockWorldPosition, blockIPos, chunk);
// Grab its collider
PhysicsBodyComponent physicsBody = physBlock.GetComponent <PhysicsBodyComponent>();
AxisAlignedBoundingBox physBlockCollider = physicsBody.GetCollider();
//DebugAABBs.Add(physBlockCollider as AABoundingBox);
// Check if the block intersects the broad,
// if it does this block is valid for collision response
// TODO: Might be able to remove the intersect check
if (broad.Intersects(physBlockCollider))
{
_terrainBlockStorage.Add(physBlock);
_terrainBlockCache.Add(physicsBody);
}
else
{
unusedPhysBlocks.Enqueue(physBlock);
}
}
}
}
}
return(_terrainBlockCache);
}
void CollisionResponse(float deltaTime)
{
for (int i = 0; i < intersections.Length; i++)
{
Intersection intersection = intersections[i];
PhysicsBodyComponent p1 = intersection.Object1;
PhysicsBodyComponent p2 = intersection.Object2;
AxisAlignedBoundingBox p1Collider = p1.GetCollider();
AxisAlignedBoundingBox p1DeltaCollider = p1.GetColliderAt(p1.Delta.FinalPosition);
AxisAlignedBoundingBox p2Collider = p2.GetCollider();
AxisAlignedBoundingBox p2DeltaCollider = p2.GetColliderAt(p2.Delta.FinalPosition);
if (intersection.Type == IntersectionType.Rigid)
{
// Update the intersection data, so that previous updates for
// these objects are applied
intersection.UpdateFromDelta();
// If both entry times are invalid, the objects aren't intersecting anymore,
// so ignore the collision.
if (intersection.Object1EntryTime == 1 && intersection.Object2EntryTime == 1)
{
continue;
}
// Handle the collision based on static states
if (intersection.Object2.IsStatic)
{
// If we are past the first sweep pass for this object,
// and this intersection is no longer valid, then ignore it.
if (p1.Delta.DeltaPass > 0)
{
if (!p1DeltaCollider.Intersects(p2Collider))
{
continue;
}
}
// Gather intersection data
Vector3 surfaceNormal = intersection.Object2Normal;
float collisionTime = intersection.Object1EntryTime;
float remainingTime = 1f - collisionTime;
float stepDist = intersection.Resolver.StepDistance(p1DeltaCollider, p2Collider);
//if (surfaceNormal.Y == 0)
// Diagnostics.DashCMD.WriteLine("{0} | {1}", stepDist.ToString(), Math.Min(p1.Delta.MaxStep, p1.MaxStep));
//Diagnostics.DashCMD.WriteLine(surfaceNormal);
// Try to step on object
if (p1.CanStep && p2.CanBeSteppedOn && surfaceNormal.Y == 0 &&
stepDist >= 0 && stepDist <= Math.Min(p1.Delta.MaxStep, p1.MaxStep))
{
// Step onto object
p1.Delta.FinalPosition.Y += stepDist + 0.001f;
p1.Delta.FinalVelocity.Y = 0;
p1.Delta.IsGrounded = p1.Delta.IsGrounded || true;
p1.Delta.Stepped = true;
}
else // Normal collision resolve
{
// Calculate the compensation in position from the collision
Vector3 compensation = surfaceNormal * (Maths.Abs(p1.Delta.FinalVelocity * deltaTime)) * remainingTime;
p1.Delta.FinalPosition += compensation;
// If this normal moved the object anywhere upward,
// that object is now considered grounded
if (surfaceNormal.Y > 0)
{
p1.Delta.IsGrounded = p1.Delta.IsGrounded || true;
}
// Fix the velocity
if (p1.BounceOnWallCollision || p1.BounceOnVerticalCollision)
{
if ((surfaceNormal.X != 0 || surfaceNormal.Z != 0) && p1.BounceOnWallCollision)
{
if (surfaceNormal.X != 0)
{
p1.Delta.FinalVelocity.X *= -(1f - p1.HorizontalBounceFalloff);
}
if (surfaceNormal.Z != 0)
{
p1.Delta.FinalVelocity.Z *= -(1f - p1.HorizontalBounceFalloff);
}
}
else if (surfaceNormal.Y != 0 && p1.BounceOnVerticalCollision)
{
p1.Delta.FinalVelocity.Y *= -(1f - p1.VerticalBounceFalloff);
p1.Delta.FinalVelocity.X *= p1.InverseFriction;
p1.Delta.FinalVelocity.Z *= p1.InverseFriction;
}
else
{
intersection.Resolver.FixVelocity(ref p1.Delta.FinalVelocity, surfaceNormal);
}
}
else
{
intersection.Resolver.FixVelocity(ref p1.Delta.FinalVelocity, surfaceNormal);
}
}
// Another delta pass has occured
p1.Delta.DeltaPass++;
p1.OnCollide(p2);
p2.OnCollide(p1);
}
else if (intersection.Object1.IsStatic)
{
// If we are past the first sweep pass for this object,
// and this intersection is no longer valid, then ignore it.
if (p2.Delta.DeltaPass > 0)
{
if (!p2DeltaCollider.Intersects(p1Collider))
{
continue;
}
}
// Gather intersection data
Vector3 surfaceNormal = intersection.Object1Normal;
float collisionTime = intersection.Object2EntryTime;
float remainingTime = 1f - collisionTime;
float stepDist = intersection.Resolver.StepDistance(p2DeltaCollider, p1Collider);
// Try to step on object
if (p2.CanStep && p1.CanBeSteppedOn && surfaceNormal.Y == 0 &&
stepDist >= 0 && stepDist <= Math.Min(p2.Delta.MaxStep, p2.MaxStep))
{
// Step onto object
p2.Delta.FinalPosition.Y += stepDist + 0.001f;
p2.Delta.FinalVelocity.Y = 0;
p2.Delta.IsGrounded = p2.Delta.IsGrounded || true;
p2.Delta.Stepped = true;
}
else // Normal collision resolve
{
// Calculate the compensation in position from the collision
Vector3 compensation = surfaceNormal * (Maths.Abs(p2.Delta.FinalVelocity * deltaTime)) * remainingTime;
p2.Delta.FinalPosition += compensation;
// If this normal moved the object anywhere upward,
// that object is now considered grounded
if (surfaceNormal.Y > 0)
{
p2.Delta.IsGrounded = p2.Delta.IsGrounded || true;
}
// Fix the velocity
if (p2.BounceOnWallCollision || p2.BounceOnVerticalCollision)
{
if ((surfaceNormal.X != 0 || surfaceNormal.Z != 0) && p2.BounceOnWallCollision)
{
if (surfaceNormal.X != 0)
{
p2.Delta.FinalVelocity.X *= -(1f - p2.HorizontalBounceFalloff);
}
if (surfaceNormal.Z != 0)
{
p2.Delta.FinalVelocity.Z *= -(1f - p2.HorizontalBounceFalloff);
}
}
else if (surfaceNormal.Y != 0 && p2.BounceOnVerticalCollision)
{
p2.Delta.FinalVelocity.Y *= -(1f - p2.VerticalBounceFalloff);
p1.Delta.FinalVelocity.X *= p1.InverseFriction;
p1.Delta.FinalVelocity.Z *= p1.InverseFriction;
}
else
{
intersection.Resolver.FixVelocity(ref p2.Delta.FinalVelocity, surfaceNormal);
}
}
else
{
intersection.Resolver.FixVelocity(ref p2.Delta.FinalVelocity, surfaceNormal);
}
}
// Another delta pass has occured
p2.Delta.DeltaPass++;
p1.OnCollide(p2);
p2.OnCollide(p1);
}
}
else
{
// If we are past the first sweep pass for this object,
// and this intersection is no longer valid, then ignore it.
if ((!p1DeltaCollider.Intersects(p2Collider)) &&
(!p2DeltaCollider.Intersects(p1Collider)))
{
continue;
}
float p1DeltaMiddleX = p1.Delta.FinalPosition.X + p1.Size.X / 2f;
float p2DeltaMiddleX = p2.Delta.FinalPosition.X + p2.Size.X / 2f;
float p1DeltaMiddleZ = p1.Delta.FinalPosition.Z + p1.Size.Z / 2f;
float p2DeltaMiddleZ = p2.Delta.FinalPosition.Z + p2.Size.Z / 2f;
Vector3 p1Move = new Vector3((p2DeltaMiddleX - p1DeltaMiddleX), 0, (p2DeltaMiddleZ - p1DeltaMiddleZ));
Vector3 p2Move = new Vector3((p1DeltaMiddleX - p2DeltaMiddleX), 0, (p1DeltaMiddleZ - p2DeltaMiddleZ));
if (p1Move.X == p2Move.X)
{
p1Move.X += Maths.Random.Next((int)-p2.Size.X, (int)p2.Size.X) / deltaTime;
}
Vector2 masses = new Vector2((p2.Mass / p1.Mass), (p1.Mass / p2.Mass)) * 0.5f;
if (p1.CanBePushedBySoft)
{
p1.Delta.FinalVelocity -= p1Move * masses.X;
p1.Delta.FinalPosition = p1.Transform.Position + p1.Delta.FinalVelocity * deltaTime;
}
if (p2.CanBePushedBySoft)
{
p2.Delta.FinalVelocity -= p2Move * masses.Y;
p2.Delta.FinalPosition = p2.Transform.Position + p2.Delta.FinalVelocity * deltaTime;
}
p1.OnCollide(p2);
p2.OnCollide(p1);
}
}
}
