mirror of
https://github.com/ClassiCube/ClassiCube.git
synced 2025-10-05 12:07:25 -04:00
331 lines
14 KiB
C#
331 lines
14 KiB
C#
// ClassicalSharp copyright 2014-2016 UnknownShadow200 | Licensed under MIT
|
|
using System;
|
|
using OpenTK;
|
|
|
|
namespace ClassicalSharp {
|
|
|
|
/// <summary> Entity component that performs collision detection. </summary>
|
|
public sealed class PhysicsComponent {
|
|
|
|
Game game;
|
|
Entity entity;
|
|
BlockInfo info;
|
|
public PhysicsComponent( Game game, Entity entity ) {
|
|
this.game = game;
|
|
this.entity = entity;
|
|
info = game.BlockInfo;
|
|
}
|
|
|
|
internal bool hitYMax, collideX, collideY, collideZ;
|
|
|
|
/// <summary> Constant offset used to avoid floating point roundoff errors. </summary>
|
|
public const float Adjustment = 0.001f;
|
|
|
|
public byte GetPhysicsBlockId( int x, int y, int z ) {
|
|
if( x < 0 || x >= game.Map.Width || z < 0 ||
|
|
z >= game.Map.Length || y < 0 ) return (byte)Block.Bedrock;
|
|
|
|
if( y >= game.Map.Height ) return (byte)Block.Air;
|
|
return game.Map.GetBlock( x, y, z );
|
|
}
|
|
|
|
bool GetBoundingBox( byte block, int x, int y, int z, ref BoundingBox box ) {
|
|
if( info.CollideType[block] != BlockCollideType.Solid ) return false;
|
|
Add( x, y, z, ref info.MinBB[block], ref box.Min );
|
|
Add( x, y, z, ref info.MaxBB[block], ref box.Max );
|
|
return true;
|
|
}
|
|
|
|
static void Add( int x, int y, int z, ref Vector3 offset, ref Vector3 target ) {
|
|
target.X = x + offset.X;
|
|
target.Y = y + offset.Y;
|
|
target.Z = z + offset.Z;
|
|
}
|
|
|
|
|
|
// TODO: test for corner cases, and refactor this.
|
|
internal void MoveAndWallSlide() {
|
|
if( entity.Velocity == Vector3.Zero ) return;
|
|
Vector3 size = entity.CollisionSize;
|
|
BoundingBox entityBB, entityExtentBB;
|
|
int count = 0;
|
|
FindReachableBlocks( ref count, ref size, out entityBB, out entityExtentBB );
|
|
CollideWithReachableBlocks( count, ref size, ref entityBB, ref entityExtentBB );
|
|
}
|
|
|
|
void FindReachableBlocks( ref int count, ref Vector3 size,
|
|
out BoundingBox entityBB, out BoundingBox entityExtentBB ) {
|
|
Vector3 vel = entity.Velocity;
|
|
Vector3 pos = entity.Position;
|
|
entityBB = new BoundingBox(
|
|
pos.X - size.X / 2, pos.Y, pos.Z - size.Z / 2,
|
|
pos.X + size.X / 2, pos.Y + size.Y, pos.Z + size.Z / 2
|
|
);
|
|
|
|
// Exact maximum extent the entity can reach, and the equivalent map coordinates.
|
|
entityExtentBB = new BoundingBox(
|
|
vel.X < 0 ? entityBB.Min.X + vel.X : entityBB.Min.X,
|
|
vel.Y < 0 ? entityBB.Min.Y + vel.Y : entityBB.Min.Y,
|
|
vel.Z < 0 ? entityBB.Min.Z + vel.Z : entityBB.Min.Z,
|
|
vel.X > 0 ? entityBB.Max.X + vel.X : entityBB.Max.X,
|
|
vel.Y > 0 ? entityBB.Max.Y + vel.Y : entityBB.Max.Y,
|
|
vel.Z > 0 ? entityBB.Max.Z + vel.Z : entityBB.Max.Z
|
|
);
|
|
Vector3I min = Vector3I.Floor( entityExtentBB.Min );
|
|
Vector3I max = Vector3I.Floor( entityExtentBB.Max );
|
|
|
|
int elements = (max.X + 1 - min.X) * (max.Y + 1 - min.Y) * (max.Z + 1 - min.Z);
|
|
if( elements > stateCache.Length ) {
|
|
stateCache = new State[elements];
|
|
}
|
|
|
|
BoundingBox blockBB = default( BoundingBox );
|
|
// Order loops so that we minimise cache misses
|
|
for( int y = min.Y; y <= max.Y; y++ )
|
|
for( int z = min.Z; z <= max.Z; z++ )
|
|
for( int x = min.X; x <= max.X; x++ )
|
|
{
|
|
byte blockId = GetPhysicsBlockId( x, y, z );
|
|
if( !GetBoundingBox( blockId, x, y, z, ref blockBB ) ) continue;
|
|
if( !entityExtentBB.Intersects( blockBB ) ) continue; // necessary for non whole blocks. (slabs)
|
|
|
|
float tx = 0, ty = 0, tz = 0;
|
|
CalcTime( ref vel, ref entityBB, ref blockBB, out tx, out ty, out tz );
|
|
if( tx > 1 || ty > 1 || tz > 1 ) continue;
|
|
float tSquared = tx * tx + ty * ty + tz * tz;
|
|
stateCache[count++] = new State( x, y, z, blockId, tSquared );
|
|
}
|
|
}
|
|
|
|
void CollideWithReachableBlocks( int count, ref Vector3 size,
|
|
ref BoundingBox entityBB, ref BoundingBox entityExtentBB ) {
|
|
bool wasOn = entity.onGround;
|
|
entity.onGround = false;
|
|
if( count > 0 )
|
|
QuickSort( stateCache, 0, count - 1 );
|
|
collideX = false; collideY = false; collideZ = false;
|
|
BoundingBox blockBB = default(BoundingBox);
|
|
|
|
for( int i = 0; i < count; i++ ) {
|
|
State state = stateCache[i];
|
|
Vector3 blockPos = new Vector3( state.X >> 3, state.Y >> 3, state.Z >> 3 );
|
|
int block = (state.X & 0x7) | (state.Y & 0x7) << 3 | (state.Z & 0x7) << 6;
|
|
blockBB.Min = blockPos + info.MinBB[block];
|
|
blockBB.Max = blockPos + info.MaxBB[block];
|
|
if( !entityExtentBB.Intersects( blockBB ) ) continue;
|
|
|
|
float tx = 0, ty = 0, tz = 0;
|
|
CalcTime( ref entity.Velocity, ref entityBB, ref blockBB, out tx, out ty, out tz );
|
|
if( tx > 1 || ty > 1 || tz > 1 )
|
|
Utils.LogDebug( "t > 1 in physics calculation.. this shouldn't have happened." );
|
|
BoundingBox finalBB = entityBB.Offset( entity.Velocity * new Vector3( tx, ty, tz ) );
|
|
|
|
// if we have hit the bottom of a block, we need to change the axis we test first.
|
|
if( hitYMax ) {
|
|
if( finalBB.Min.Y + Adjustment >= blockBB.Max.Y )
|
|
ClipYMax( ref blockBB, ref entityBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.Y - Adjustment <= blockBB.Min.Y )
|
|
ClipYMin( ref blockBB, ref entityBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Min.X + Adjustment >= blockBB.Max.X )
|
|
ClipXMax( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.X - Adjustment <= blockBB.Min.X )
|
|
ClipXMin( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Min.Z + Adjustment >= blockBB.Max.Z )
|
|
ClipZMax( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.Z - Adjustment <= blockBB.Min.Z )
|
|
ClipZMin( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
continue;
|
|
}
|
|
|
|
// if flying or falling, test the horizontal axes first.
|
|
if( finalBB.Min.X + Adjustment >= blockBB.Max.X )
|
|
ClipXMax( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.X - Adjustment <= blockBB.Min.X )
|
|
ClipXMin( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Min.Z + Adjustment >= blockBB.Max.Z )
|
|
ClipZMax( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.Z - Adjustment <= blockBB.Min.Z )
|
|
ClipZMin( ref blockBB, ref entityBB, wasOn, finalBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Min.Y + Adjustment >= blockBB.Max.Y )
|
|
ClipYMax( ref blockBB, ref entityBB, ref entityExtentBB, ref size );
|
|
else if( finalBB.Max.Y - Adjustment <= blockBB.Min.Y )
|
|
ClipYMin( ref blockBB, ref entityBB, ref entityExtentBB, ref size );
|
|
}
|
|
}
|
|
|
|
void ClipXMin( ref BoundingBox blockBB, ref BoundingBox entityBB, bool wasOn,
|
|
BoundingBox finalBB, ref BoundingBox entityExtentBB, ref Vector3 size ) {
|
|
if( !wasOn || !DidSlide( blockBB, ref size, finalBB, ref entityBB, ref entityExtentBB ) ) {
|
|
entity.Position.X = blockBB.Min.X - size.X / 2 - Adjustment;
|
|
ClipX( ref size, ref entityBB, ref entityExtentBB );
|
|
}
|
|
}
|
|
|
|
void ClipXMax( ref BoundingBox blockBB, ref BoundingBox entityBB, bool wasOn,
|
|
BoundingBox finalBB, ref BoundingBox entityExtentBB, ref Vector3 size ) {
|
|
if( !wasOn || !DidSlide( blockBB, ref size, finalBB, ref entityBB, ref entityExtentBB ) ) {
|
|
entity.Position.X = blockBB.Max.X + size.X / 2 + Adjustment;
|
|
ClipX( ref size, ref entityBB, ref entityExtentBB );
|
|
}
|
|
}
|
|
|
|
void ClipZMax( ref BoundingBox blockBB, ref BoundingBox entityBB, bool wasOn,
|
|
BoundingBox finalBB, ref BoundingBox entityExtentBB, ref Vector3 size ) {
|
|
if( !wasOn || !DidSlide( blockBB, ref size, finalBB, ref entityBB, ref entityExtentBB ) ) {
|
|
entity.Position.Z = blockBB.Max.Z + size.Z / 2 + Adjustment;
|
|
ClipZ( ref size, ref entityBB, ref entityExtentBB );
|
|
}
|
|
}
|
|
|
|
void ClipZMin( ref BoundingBox blockBB, ref BoundingBox entityBB, bool wasOn,
|
|
BoundingBox finalBB, ref BoundingBox extentBB, ref Vector3 size ) {
|
|
if( !wasOn || !DidSlide( blockBB, ref size, finalBB, ref entityBB, ref extentBB ) ) {
|
|
entity.Position.Z = blockBB.Min.Z - size.Z / 2 - Adjustment;
|
|
ClipZ( ref size, ref entityBB, ref extentBB );
|
|
}
|
|
}
|
|
|
|
void ClipYMin( ref BoundingBox blockBB, ref BoundingBox entityBB,
|
|
ref BoundingBox extentBB, ref Vector3 size ) {
|
|
entity.Position.Y = blockBB.Min.Y - size.Y - Adjustment;
|
|
ClipY( ref size, ref entityBB, ref extentBB );
|
|
hitYMax = false;
|
|
}
|
|
|
|
void ClipYMax( ref BoundingBox blockBB, ref BoundingBox entityBB,
|
|
ref BoundingBox extentBB, ref Vector3 size ) {
|
|
entity.Position.Y = blockBB.Max.Y + Adjustment;
|
|
entity.onGround = true;
|
|
ClipY( ref size, ref entityBB, ref extentBB );
|
|
hitYMax = true;
|
|
}
|
|
|
|
bool DidSlide( BoundingBox blockBB, ref Vector3 size, BoundingBox finalBB,
|
|
ref BoundingBox entityBB, ref BoundingBox entityExtentBB ) {
|
|
float yDist = blockBB.Max.Y - entityBB.Min.Y;
|
|
if( yDist > 0 && yDist <= entity.StepSize + 0.01f ) {
|
|
float blockXMin = blockBB.Min.X, blockZMin = blockBB.Min.Z;
|
|
blockBB.Min.X = Math.Max( blockBB.Min.X, blockBB.Max.X - size.X / 2 );
|
|
blockBB.Max.X = Math.Min( blockBB.Max.X, blockXMin + size.X / 2 );
|
|
blockBB.Min.Z = Math.Max( blockBB.Min.Z, blockBB.Max.Z - size.Z / 2 );
|
|
blockBB.Max.Z = Math.Min( blockBB.Max.Z, blockZMin + size.Z / 2 );
|
|
|
|
BoundingBox adjBB = finalBB;
|
|
adjBB.Min.X = Math.Min( finalBB.Min.X, blockBB.Min.X + Adjustment );
|
|
adjBB.Max.X = Math.Max( finalBB.Max.X, blockBB.Max.X - Adjustment );
|
|
adjBB.Min.Y = blockBB.Max.Y + Adjustment;
|
|
adjBB.Max.Y = adjBB.Min.Y + size.Y;
|
|
adjBB.Min.Z = Math.Min( finalBB.Min.Z, blockBB.Min.Z + Adjustment );
|
|
adjBB.Max.Z = Math.Max( finalBB.Max.Z, blockBB.Max.Z - Adjustment );
|
|
|
|
if( !CanSlideThrough( ref adjBB ) )
|
|
return false;
|
|
entity.Position.Y = blockBB.Max.Y + Adjustment;
|
|
entity.onGround = true;
|
|
ClipY( ref size, ref entityBB, ref entityExtentBB );
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool CanSlideThrough( ref BoundingBox adjFinalBB ) {
|
|
Vector3I bbMin = Vector3I.Floor( adjFinalBB.Min );
|
|
Vector3I bbMax = Vector3I.Floor( adjFinalBB.Max );
|
|
|
|
for( int y = bbMin.Y; y <= bbMax.Y; y++ )
|
|
for( int z = bbMin.Z; z <= bbMax.Z; z++ )
|
|
for( int x = bbMin.X; x <= bbMax.X; x++ )
|
|
{
|
|
byte block = GetPhysicsBlockId( x, y, z );
|
|
Vector3 min = new Vector3( x, y, z ) + info.MinBB[block];
|
|
Vector3 max = new Vector3( x, y, z ) + info.MaxBB[block];
|
|
|
|
BoundingBox blockBB = new BoundingBox( min, max );
|
|
if( !blockBB.Intersects( adjFinalBB ) )
|
|
continue;
|
|
if( info.CollideType[GetPhysicsBlockId( x, y, z )] == BlockCollideType.Solid )
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void ClipX( ref Vector3 size, ref BoundingBox entityBB, ref BoundingBox entityExtentBB ) {
|
|
entity.Velocity.X = 0;
|
|
entityBB.Min.X = entityExtentBB.Min.X = entity.Position.X - size.X / 2;
|
|
entityBB.Max.X = entityExtentBB.Max.X = entity.Position.X + size.X / 2;
|
|
collideX = true;
|
|
}
|
|
|
|
void ClipY( ref Vector3 size, ref BoundingBox entityBB, ref BoundingBox entityExtentBB ) {
|
|
entity.Velocity.Y = 0;
|
|
entityBB.Min.Y = entityExtentBB.Min.Y = entity.Position.Y;
|
|
entityBB.Max.Y = entityExtentBB.Max.Y = entity.Position.Y + size.Y;
|
|
collideY = true;
|
|
}
|
|
|
|
void ClipZ( ref Vector3 size, ref BoundingBox entityBB, ref BoundingBox entityExtentBB ) {
|
|
entity.Velocity.Z = 0;
|
|
entityBB.Min.Z = entityExtentBB.Min.Z = entity.Position.Z - size.Z / 2;
|
|
entityBB.Max.Z = entityExtentBB.Max.Z = entity.Position.Z + size.Z / 2;
|
|
collideZ = true;
|
|
}
|
|
|
|
static void CalcTime( ref Vector3 vel, ref BoundingBox entityBB, ref BoundingBox blockBB,
|
|
out float tx, out float ty, out float tz ) {
|
|
float dx = vel.X > 0 ? blockBB.Min.X - entityBB.Max.X : entityBB.Min.X - blockBB.Max.X;
|
|
float dy = vel.Y > 0 ? blockBB.Min.Y - entityBB.Max.Y : entityBB.Min.Y - blockBB.Max.Y;
|
|
float dz = vel.Z > 0 ? blockBB.Min.Z - entityBB.Max.Z : entityBB.Min.Z - blockBB.Max.Z;
|
|
|
|
tx = vel.X == 0 ? float.PositiveInfinity : Math.Abs( dx / vel.X );
|
|
ty = vel.Y == 0 ? float.PositiveInfinity : Math.Abs( dy / vel.Y );
|
|
tz = vel.Z == 0 ? float.PositiveInfinity : Math.Abs( dz / vel.Z );
|
|
|
|
if( entityBB.XIntersects( blockBB ) ) tx = 0;
|
|
if( entityBB.YIntersects( blockBB ) ) ty = 0;
|
|
if( entityBB.ZIntersects( blockBB ) ) tz = 0;
|
|
}
|
|
|
|
|
|
struct State {
|
|
public int X, Y, Z;
|
|
public float tSquared;
|
|
|
|
public State( int x, int y, int z, byte block, float tSquared ) {
|
|
X = x << 3; Y = y << 3; Z = z << 3;
|
|
X |= (block & 0x07);
|
|
Y |= (block & 0x38) >> 3;
|
|
Z |= (block & 0xC0) >> 6;
|
|
this.tSquared = tSquared;
|
|
}
|
|
}
|
|
static State[] stateCache = new State[0];
|
|
|
|
static void QuickSort( State[] keys, int left, int right ) {
|
|
while( left < right ) {
|
|
int i = left, j = right;
|
|
float pivot = keys[(i + j) / 2].tSquared;
|
|
// partition the list
|
|
while( i <= j ) {
|
|
while( pivot > keys[i].tSquared ) i++;
|
|
while( pivot < keys[j].tSquared ) j--;
|
|
|
|
if( i <= j ) {
|
|
State key = keys[i]; keys[i] = keys[j]; keys[j] = key;
|
|
i++; j--;
|
|
}
|
|
}
|
|
|
|
// recurse into the smaller subset
|
|
if( j - left <= right - i ) {
|
|
if( left < j )
|
|
QuickSort( keys, left, j );
|
|
left = i;
|
|
} else {
|
|
if( i < right )
|
|
QuickSort( keys, i, right );
|
|
right = j;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |