448 lines
14 KiB
C#
448 lines
14 KiB
C#
using OpenTK.Mathematics;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace Voxel
|
||
{
|
||
public class Entity
|
||
{
|
||
public Vector3 Position;
|
||
public Vector3 Velocity;
|
||
public bool OnGround;
|
||
public float Rotation;
|
||
|
||
public float Width;
|
||
public float Height;
|
||
|
||
private float _gravity = 0.08f;
|
||
private float _terminalVelocity = -3.92f;
|
||
private float _airMultiplier = 0.91f;
|
||
private float _yMultiplier = 0.98f;
|
||
private float _groundMultiplier = 0.6f;
|
||
|
||
private const float COLLISION_EPSILON = 0.001f;
|
||
|
||
protected World _world;
|
||
|
||
// Helper methods for consistent block coordinate conversion
|
||
private static int BlockCoordMin(float coord) => (int)MathF.Floor(coord - 1e-6f);
|
||
private static int BlockCoordMax(float coord) => (int)MathF.Floor(coord + 1e-6f);
|
||
|
||
public Entity(Vector3 position, float width, float height, World world)
|
||
{
|
||
Position = position;
|
||
Width = width;
|
||
Height = height;
|
||
_world = world;
|
||
}
|
||
|
||
public void Tick()
|
||
{
|
||
Vector3 desiredMovement = Velocity;
|
||
Vector3 newPosition = Position;
|
||
|
||
CheckAndResolveCollisions(ref newPosition, desiredMovement);
|
||
|
||
Position = newPosition;
|
||
|
||
if (!OnGround)
|
||
{
|
||
Vector3 acceleration = new Vector3(0, -_gravity, 0);
|
||
Velocity += acceleration;
|
||
Velocity.X *= _airMultiplier;
|
||
Velocity.Z *= _airMultiplier;
|
||
Velocity.Y *= _yMultiplier;
|
||
|
||
if (Velocity.Y < _terminalVelocity)
|
||
{
|
||
Velocity.Y = _terminalVelocity;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Velocity = new Vector3(Velocity.X * _groundMultiplier, 0f, Velocity.Z * _groundMultiplier);
|
||
}
|
||
|
||
UpdateOnGround();
|
||
}
|
||
|
||
public void CheckAndResolveCollisions(ref Vector3 position, Vector3 movement)
|
||
{
|
||
Vector3 originalPosition = position;
|
||
|
||
// Try full movement first
|
||
AABB futureBox = GetBoundingBoxAt(originalPosition + movement);
|
||
|
||
if (!HasCollision(futureBox))
|
||
{
|
||
// No collision, apply full movement
|
||
position = originalPosition + movement;
|
||
return;
|
||
}
|
||
|
||
// Collision detected, resolve each axis independently
|
||
position = originalPosition;
|
||
|
||
// Resolve Y collision first
|
||
ResolveYCollisionIndependent(ref position, movement.Y);
|
||
|
||
// Resolve X collision
|
||
ResolveXCollisionIndependent(ref position, movement.X);
|
||
|
||
// Resolve Z collision
|
||
ResolveZCollisionIndependent(ref position, movement.Z);
|
||
|
||
// After moving horizontally, re‑resolve Y to handle any new vertical collisions
|
||
// (e.g., sliding into a ledge)
|
||
ResolveYCollisionIndependent(ref position, 0);
|
||
}
|
||
|
||
private void ResolveYCollisionIndependent(ref Vector3 position, float velocityY)
|
||
{
|
||
// First, handle if we're already inside a block (shouldn't happen normally)
|
||
AABB currentBox = GetBoundingBoxAt(position);
|
||
if (HasCollision(currentBox))
|
||
{
|
||
// Try to push upward to escape
|
||
float step = 0.05f;
|
||
for (int i = 0; i < 10; i++)
|
||
{
|
||
position.Y += step;
|
||
if (!HasCollision(GetBoundingBoxAt(position)))
|
||
break;
|
||
}
|
||
Velocity.Y = 0;
|
||
OnGround = false;
|
||
return;
|
||
}
|
||
|
||
if (velocityY == 0) return;
|
||
|
||
Vector3 testPos = new Vector3(position.X, position.Y + velocityY, position.Z);
|
||
AABB testBox = GetBoundingBoxAt(testPos);
|
||
|
||
if (HasCollision(testBox))
|
||
{
|
||
if (velocityY > 0) // Hitting ceiling
|
||
{
|
||
float ceilingY = GetCeilingHeight(testBox);
|
||
position.Y = ceilingY - (Height / 2);
|
||
Velocity.Y = 0;
|
||
}
|
||
else // Hitting floor
|
||
{
|
||
float floorY = GetFloorHeight(testBox);
|
||
position.Y = floorY + (Height / 2);
|
||
Velocity.Y = 0;
|
||
OnGround = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
position.Y += velocityY;
|
||
}
|
||
}
|
||
|
||
private void ResolveXCollisionIndependent(ref Vector3 position, float velocityX)
|
||
{
|
||
if (velocityX == 0) return;
|
||
|
||
float halfWidth = Width / 2;
|
||
float direction = Math.Sign(velocityX);
|
||
float targetX = position.X + velocityX;
|
||
|
||
// Sweep the bounding box to the target X position
|
||
Vector3 testPos = new Vector3(targetX, position.Y, position.Z);
|
||
AABB testBox = GetBoundingBoxAt(testPos);
|
||
|
||
if (HasCollision(testBox))
|
||
{
|
||
// Find the wall we hit
|
||
float wallX;
|
||
if (direction > 0) // Moving right
|
||
{
|
||
wallX = GetRightWallPosition(testBox) - halfWidth - COLLISION_EPSILON;
|
||
}
|
||
else // Moving left
|
||
{
|
||
wallX = GetLeftWallPosition(testBox) + halfWidth + COLLISION_EPSILON;
|
||
}
|
||
position.X = wallX;
|
||
Velocity.X = 0;
|
||
}
|
||
else
|
||
{
|
||
position.X = targetX;
|
||
}
|
||
}
|
||
|
||
private void ResolveZCollisionIndependent(ref Vector3 position, float velocityZ)
|
||
{
|
||
if (velocityZ == 0) return;
|
||
|
||
float halfWidth = Width / 2;
|
||
float direction = Math.Sign(velocityZ);
|
||
float targetZ = position.Z + velocityZ;
|
||
|
||
// Sweep the bounding box to the target Z position
|
||
Vector3 testPos = new Vector3(position.X, position.Y, targetZ);
|
||
AABB testBox = GetBoundingBoxAt(testPos);
|
||
|
||
if (HasCollision(testBox))
|
||
{
|
||
// Find the wall we hit
|
||
float wallZ;
|
||
if (direction > 0) // Moving forward (+Z)
|
||
{
|
||
wallZ = GetFrontWallPosition(testBox) - halfWidth - COLLISION_EPSILON;
|
||
}
|
||
else // Moving backward (-Z)
|
||
{
|
||
wallZ = GetBackWallPosition(testBox) + halfWidth + COLLISION_EPSILON;
|
||
}
|
||
position.Z = wallZ;
|
||
Velocity.Z = 0;
|
||
}
|
||
else
|
||
{
|
||
position.Z = targetZ;
|
||
}
|
||
}
|
||
|
||
private float GetFloorHeight(AABB box)
|
||
{
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
int checkY = BlockCoordMin(box.Min.Y);
|
||
|
||
float highestFloor = float.MinValue;
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, checkY, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
highestFloor = Math.Max(highestFloor, checkY + 1); // Top of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return highestFloor != float.MinValue ? highestFloor : box.Min.Y;
|
||
}
|
||
|
||
private float GetCeilingHeight(AABB box)
|
||
{
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
int checkY = BlockCoordMax(box.Max.Y); // Fix: use the block above the entity
|
||
|
||
float lowestCeiling = float.MaxValue;
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, checkY, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
lowestCeiling = Math.Min(lowestCeiling, checkY); // Bottom of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return lowestCeiling != float.MaxValue ? lowestCeiling : box.Max.Y;
|
||
}
|
||
|
||
private float GetLeftWallPosition(AABB box)
|
||
{
|
||
int minY = BlockCoordMin(box.Min.Y);
|
||
int maxY = BlockCoordMax(box.Max.Y);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
int checkX = BlockCoordMin(box.Min.X); // leftmost block the entity overlaps
|
||
|
||
float rightmostWall = float.MinValue;
|
||
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(checkX, y, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
rightmostWall = Math.Max(rightmostWall, checkX + 1); // Right side of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return rightmostWall != float.MinValue ? rightmostWall : box.Min.X;
|
||
}
|
||
|
||
private float GetRightWallPosition(AABB box)
|
||
{
|
||
int minY = BlockCoordMin(box.Min.Y);
|
||
int maxY = BlockCoordMax(box.Max.Y);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
int checkX = BlockCoordMax(box.Max.X); // rightmost block the entity overlaps
|
||
|
||
float leftmostWall = float.MaxValue;
|
||
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(checkX, y, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
leftmostWall = Math.Min(leftmostWall, checkX); // Left side of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return leftmostWall != float.MaxValue ? leftmostWall : box.Max.X;
|
||
}
|
||
|
||
private float GetBackWallPosition(AABB box)
|
||
{
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minY = BlockCoordMin(box.Min.Y);
|
||
int maxY = BlockCoordMax(box.Max.Y);
|
||
int checkZ = BlockCoordMin(box.Min.Z); // backmost block the entity overlaps
|
||
|
||
float frontmostWall = float.MinValue;
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, y, checkZ);
|
||
if (block != Blocks.Air)
|
||
{
|
||
frontmostWall = Math.Max(frontmostWall, checkZ + 1); // Front side of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return frontmostWall != float.MinValue ? frontmostWall : box.Min.Z;
|
||
}
|
||
|
||
private float GetFrontWallPosition(AABB box)
|
||
{
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minY = BlockCoordMin(box.Min.Y);
|
||
int maxY = BlockCoordMax(box.Max.Y);
|
||
int checkZ = BlockCoordMax(box.Max.Z); // frontmost block the entity overlaps
|
||
|
||
float backmostWall = float.MaxValue;
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, y, checkZ);
|
||
if (block != Blocks.Air)
|
||
{
|
||
backmostWall = Math.Min(backmostWall, checkZ); // Back side of the block
|
||
}
|
||
}
|
||
}
|
||
|
||
return backmostWall != float.MaxValue ? backmostWall : box.Max.Z;
|
||
}
|
||
|
||
public bool HasCollision(AABB box)
|
||
{
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minY = BlockCoordMin(box.Min.Y);
|
||
int maxY = BlockCoordMax(box.Max.Y);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, y, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
public void ApplyImpulse(Vector3 force)
|
||
{
|
||
Velocity += force;
|
||
}
|
||
|
||
public AABB GetBoundingBox()
|
||
{
|
||
return GetBoundingBoxAt(Position);
|
||
}
|
||
|
||
public AABB GetBoundingBoxAt(Vector3 position)
|
||
{
|
||
float halfWidth = Width / 2;
|
||
float halfHeight = Height / 2;
|
||
|
||
Vector3 min = new Vector3(
|
||
position.X - halfWidth,
|
||
position.Y - halfHeight,
|
||
position.Z - halfWidth
|
||
);
|
||
|
||
Vector3 max = new Vector3(
|
||
position.X + halfWidth,
|
||
position.Y + halfHeight,
|
||
position.Z + halfWidth
|
||
);
|
||
|
||
return new AABB(min, max);
|
||
}
|
||
|
||
public void UpdateOnGround()
|
||
{
|
||
AABB box = GetBoundingBox();
|
||
float yCheck = box.Min.Y - COLLISION_EPSILON;
|
||
|
||
int minX = BlockCoordMin(box.Min.X);
|
||
int maxX = BlockCoordMax(box.Max.X);
|
||
int minZ = BlockCoordMin(box.Min.Z);
|
||
int maxZ = BlockCoordMax(box.Max.Z);
|
||
int y = BlockCoordMin(yCheck);
|
||
|
||
OnGround = false;
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int z = minZ; z <= maxZ; z++)
|
||
{
|
||
Blocks block = _world.GetBlock(x, y, z);
|
||
if (block != Blocks.Air)
|
||
{
|
||
OnGround = true;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
} |