Skip to content

Instantly share code, notes, and snippets.

@Pizzaandy

Pizzaandy/AdvancedCharacterBody2D.cs

Created December 24, 2023 05:38
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save Pizzaandy/0e3a14be2e89c0aa4bd1ef3c095c6f29 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save Pizzaandy/0e3a14be2e89c0aa4bd1ef3c095c6f29 to your computer and use it in GitHub Desktop.
Download ZIP
Godot CharacterBody2D implemented in C#
Raw
AdvancedCharacterBody2D.cs
using Godot;
using System.Collections.Generic;
public enum MovementMode
{
Grounded,
Floating,
}
public partial class AdvancedCharacterBody2D : AnimatableBody2D
{
private struct CollisionResult
{
public bool Collided;
public Vector2 Normal;
public Vector2 Travel;
public Vector2 Remainder;
public GodotObject Collider;
public ulong ColliderId;
public Rid ColliderRid;
public Vector2 ColliderVelocity;
public float SafeFraction;
public float UnsafeFraction;
public readonly float GetAngleDegrees(Vector2 upDirection)
{
var radians = Mathf.Acos(Normal.Dot(upDirection));
return Mathf.RadToDeg(radians);
}
}
[Export]
public MovementMode Mode = MovementMode.Floating;
[Export]
public int MaxSlides = 6;
[Export]
public float SafeMargin = 0.08f;
[Export]
public float Speed = 300.0f;
[Export]
public float JumpVelocity = 100.0f;
[Export]
public Vector2 UpDirection = Vector2.Up;
[Export]
public float FloorMaxAngle = 60f;
[Export]
public bool FloorConstantSpeed = true;
[Export]
public float FloorSnapLength = 10f;
[Export(PropertyHint.Layers2DPhysics)]
public int PlatformFloorLayers;
[Export(PropertyHint.Layers2DPhysics)]
public int PlatformWallLayers;
public Vector2 Velocity;
private readonly List<CollisionResult> motionResults = new();
private Vector2 floorNormal;
private Vector2 lastMotion;
private Vector2 wallNormal;
private Vector2 previousPosition;
private bool onCeiling;
private bool onFloor;
private bool onWall;
bool floorStopOnSlope = true;
private readonly PhysicsTestMotionResult2D internalResult = new();
private static Rid cachedEmptyRid { get; } = new();
private bool IsOnWallOnly => onWall && !onFloor && !onCeiling;
private bool IsOnFloorOnly => onFloor && !onWall && !onCeiling;
private Rid platformRid;
private ulong platformObjectId;
private Vector2 platformVelocity;
private uint platformLayer;
private const float FloorAngleThreshold = 0.01f;
public float gravity = ProjectSettings.GetSetting("physics/2d/default_gravity").AsSingle();
public AdvancedCharacterBody2D()
{
SyncToPhysics = false;
}
public override void _PhysicsProcess(double delta)
{
Vector2 direction = Input.GetVector("move_left", "move_right", "move_up", "move_down");
// Add the gravity.
if (!onFloor)
{
Velocity.Y += gravity * (float)delta;
}
if (Input.IsActionJustPressed("move_up") && onFloor)
{
Velocity.Y = -JumpVelocity;
}
if (direction != Vector2.Zero)
{
Velocity.X = direction.X * Speed;
}
else
{
Velocity.X = 0f;
}
MoveAndSlide(delta);
}
public bool MoveAndSlide(double delta)
{
Vector2 currentPlatformVelocity = platformVelocity;
Transform2D gt = GlobalTransform;
previousPosition = gt.Origin;
if ((onFloor || onWall) && platformRid.IsValid)
{
bool excluded = false;
if (onFloor)
{
excluded = (PlatformFloorLayers & platformLayer) == 0;
}
else if (onWall)
{
excluded = (PlatformWallLayers & platformLayer) == 0;
}
if (!excluded)
{
// This approach makes sure there is less delay between the actual body velocity and the one we saved
if (PhysicsServer2D.BodyGetDirectState(platformRid) is PhysicsDirectBodyState2D bs)
{
Vector2 localPosition = gt.Origin - bs.Transform.Origin;
currentPlatformVelocity = bs.GetVelocityAtLocalPosition(localPosition);
}
else
{
// Body is removed or destroyed, invalidate floor.
currentPlatformVelocity = Vector2.Zero;
platformRid = cachedEmptyRid;
}
}
else
{
currentPlatformVelocity = Vector2.Zero;
}
}
motionResults.Clear();
lastMotion = Vector2.Zero;
bool wasOnFloor = onFloor;
onFloor = false;
onCeiling = false;
onWall = false;
if (!currentPlatformVelocity.IsZeroApprox())
{
var parameters = new PhysicsTestMotionParameters2D
{
From = gt,
Motion = currentPlatformVelocity * (float)delta,
Margin = SafeMargin,
RecoveryAsCollision = true
};
parameters.ExcludeBodies.Add(platformRid);
if (platformObjectId != 0)
{
parameters.ExcludeObjects.Add((int)platformObjectId);
}
if (MoveAndCollideInternal(parameters, out var result, false, false))
{
motionResults.Add(result);
SetCollisionDirection(result);
}
}
if (Mode is MovementMode.Grounded)
{
MoveAndSlideGrounded(delta, wasOnFloor);
}
else
{
MoveAndSlideFloating(delta);
}
if (!onFloor && !onWall)
{
Velocity += currentPlatformVelocity;
}
return motionResults.Count > 0;
}
private void MoveAndSlideFloating(double delta)
{
Vector2 motion = Velocity * (float)delta;
bool firstSlide = true;
platformRid = cachedEmptyRid;
platformObjectId = 0;
floorNormal = Vector2.Zero;
platformVelocity = Vector2.Zero;
for (var iteration = 0; iteration < MaxSlides; iteration++)
{
var parameters = new PhysicsTestMotionParameters2D
{
From = GlobalTransform,
Motion = motion,
Margin = SafeMargin,
RecoveryAsCollision = true
};
bool collided = MoveAndCollideInternal(parameters, out var result, false, false);
if (collided)
{
motionResults.Add(result);
if (result.Remainder.IsZeroApprox())
{
break;
}
if (firstSlide)
{
Vector2 motionSlideNorm = result.Remainder.Slide(result.Normal).Normalized();
motion = motionSlideNorm * (motion.Length() - result.Travel.Length());
}
else
{
motion = result.Remainder.Slide(result.Normal);
}
if (motion.Dot(Velocity) <= 0.0f)
{
motion = Vector2.Zero;
}
}
if (!collided || motion.IsZeroApprox())
{
break;
}
firstSlide = false;
}
}
private void MoveAndSlideGrounded(double delta, bool wasOnFloor)
{
Vector2 motion = Velocity * (float)delta;
Vector2 motionSlideUp = motion.Slide(UpDirection);
Vector2 prevFloorNormal = floorNormal;
platformRid = cachedEmptyRid;
floorNormal = Vector2.Zero;
platformVelocity = Vector2.Zero;
bool slidingEnabled = !floorStopOnSlope;
bool canApplyConstantSpeed = slidingEnabled;
bool applyCeilingVelocity = false;
bool firstSlide = true;
bool velDirFacingUp = Velocity.Dot(UpDirection) > 0;
bool slideOnCeiling = true;
bool floorBlockOnWall = true;
bool floorConstantSpeed = true;
Vector2 lastTravel = Vector2.Zero;
for (int iteration = 0; iteration < MaxSlides; ++iteration)
{
Vector2 prevPosition = GlobalPosition;
var parameters = new PhysicsTestMotionParameters2D
{
From = GlobalTransform,
Motion = motion,
Margin = SafeMargin,
RecoveryAsCollision = true
};
bool collided = MoveAndCollideInternal(
parameters,
out var result,
false,
!slidingEnabled
);
lastMotion = result.Travel;
if (collided)
{
motionResults.Add(result);
SetCollisionDirection(result);
// If we hit a ceiling platform, we set the vertical velocity to at least the platform one.
if (
onCeiling
&& result.ColliderVelocity != Vector2.Zero
&& result.ColliderVelocity.Dot(UpDirection) < 0
)
{
// If ceiling sliding is on, only apply when the ceiling is flat or when the motion is upward.
if (
!slideOnCeiling
|| motion.Dot(UpDirection) < 0
|| (result.Normal + UpDirection).Length() < 0.01f
)
{
applyCeilingVelocity = true;
Vector2 ceilingVerticalVelocity =
UpDirection * UpDirection.Dot(result.ColliderVelocity);
Vector2 motionVerticalVelocity = UpDirection * UpDirection.Dot(Velocity);
if (
motionVerticalVelocity.Dot(UpDirection) > 0
|| ceilingVerticalVelocity.LengthSquared()
> motionVerticalVelocity.LengthSquared()
)
{
Velocity = ceilingVerticalVelocity + Velocity.Slide(UpDirection);
}
}
}
if (
onFloor
&& floorStopOnSlope
&& (Velocity.Normalized() + UpDirection).Length() < 0.01f
)
{
if (result.Travel.Length() <= SafeMargin + Mathf.Epsilon)
{
GlobalTransform = GlobalTransform.Translated(-result.Travel);
}
Velocity = Vector2.Zero;
lastMotion = Vector2.Zero;
motion = Vector2.Zero;
break;
}
if (result.Remainder.IsZeroApprox())
{
motion = Vector2.Zero;
break;
}
// Move on floor only checks.
if (floorBlockOnWall && onWall && motionSlideUp.Dot(result.Normal) <= 0)
{
// Avoid moving forward on a wall if floor_block_on_wall is true.
if (wasOnFloor && !onFloor && !velDirFacingUp)
{
// If the movement is large the body can be prevented from reaching the walls.
if (result.Travel.Length() <= SafeMargin + Mathf.Epsilon)
{
// Cancels the motion.
GlobalTransform = GlobalTransform.Translated(-result.Travel);
}
// Determines if you are on the ground.
SnapOnFloor(true, false, true);
Velocity = Vector2.Zero;
lastMotion = Vector2.Zero;
motion = Vector2.Zero;
break;
}
// Prevents the body from being able to climb a slope when it moves forward against the wall.
else if (!onFloor)
{
motion = UpDirection * UpDirection.Dot(result.Remainder);
motion = motion.Slide(result.Normal);
}
else
{
motion = result.Remainder;
}
}
// Constant Speed when the slope is upward.
else if (
floorConstantSpeed
&& IsOnFloorOnly
&& canApplyConstantSpeed
&& wasOnFloor
&& motion.Dot(result.Normal) < 0
)
{
canApplyConstantSpeed = false;
Vector2 motionSlideNorm = result.Remainder.Slide(result.Normal).Normalized();
motion =
motionSlideNorm
* (
motionSlideUp.Length()
- result.Travel.Slide(UpDirection).Length()
- lastTravel.Slide(UpDirection).Length()
);
}
else if (
(slidingEnabled || !onFloor)
&& (!onCeiling || slideOnCeiling || !velDirFacingUp)
&& !applyCeilingVelocity
)
{
Vector2 slideMotion = result.Remainder.Slide(result.Normal);
if (slideMotion.Dot(Velocity) > 0.0)
{
motion = slideMotion;
}
else
{
motion = Vector2.Zero;
}
if (slideOnCeiling && onCeiling)
{
if (velDirFacingUp)
{
Velocity = Velocity.Slide(result.Normal);
}
else
{
Velocity = UpDirection * UpDirection.Dot(Velocity);
}
}
}
else
{
motion = result.Remainder;
if (onCeiling && !slideOnCeiling && velDirFacingUp)
{
Velocity = Velocity.Slide(UpDirection);
motion = motion.Slide(UpDirection);
}
}
lastTravel = result.Travel;
}
else if (
floorConstantSpeed && firstSlide && OnFloorIfSnapped(wasOnFloor, velDirFacingUp)
)
{
canApplyConstantSpeed = false;
slidingEnabled = true;
GlobalPosition = prevPosition;
Vector2 motionSlideNorm = motion.Slide(prevFloorNormal).Normalized();
motion = motionSlideNorm * (motionSlideUp.Length());
collided = true;
}
canApplyConstantSpeed = !canApplyConstantSpeed && !slidingEnabled;
slidingEnabled = true;
firstSlide = false;
if (!collided || motion.IsZeroApprox())
{
break;
}
}
SnapOnFloor(wasOnFloor, velDirFacingUp);
if (IsOnWallOnly && motionSlideUp.Dot(motionResults[0].Normal) < 0)
{
Vector2 slideMotion = Velocity.Slide(motionResults[0].Normal);
if (motionSlideUp.Dot(slideMotion) < 0)
{
Velocity = UpDirection * UpDirection.Dot(Velocity);
}
else
{
Velocity = UpDirection * UpDirection.Dot(Velocity) + slideMotion.Slide(UpDirection);
}
}
if (onFloor && !velDirFacingUp)
{
Velocity = Velocity.Slide(UpDirection);
}
}
private void SetCollisionDirection(CollisionResult result)
{
if (
Mode is MovementMode.Grounded
&& result.GetAngleDegrees(UpDirection) <= FloorMaxAngle + FloorAngleThreshold
)
{
onFloor = true;
floorNormal = result.Normal;
SetPlatformData(result);
}
else if (
Mode is MovementMode.Grounded
&& result.GetAngleDegrees(-UpDirection) <= FloorMaxAngle + FloorAngleThreshold
)
{
onCeiling = true;
}
else
{
onWall = true;
wallNormal = result.Normal;
// Don't apply wall velocity when the collider is a CharacterBody2D.
if (result.Collider is not AdvancedCharacterBody2D)
{
SetPlatformData(result);
}
}
}
private void SetPlatformData(CollisionResult result)
{
platformRid = result.ColliderRid;
platformObjectId = result.ColliderId;
platformVelocity = result.ColliderVelocity;
platformLayer = PhysicsServer2D.BodyGetCollisionLayer(platformRid);
}
private void SnapOnFloor(bool wasOnFloor, bool velDirFacingUp, bool wallAsFloor = false)
{
if (onFloor || !wasOnFloor || velDirFacingUp)
{
return;
}
ApplyFloorSnap(wallAsFloor);
}
private void ApplyFloorSnap(bool wallAsFloor)
{
if (onFloor)
{
return;
}
// Snap by at least collision margin to keep floor state consistent.
float length = Mathf.Max(FloorSnapLength, SafeMargin);
var parameters = new PhysicsTestMotionParameters2D
{
From = GlobalTransform,
Motion = -UpDirection * length,
Margin = SafeMargin,
RecoveryAsCollision = true,
CollideSeparationRay = true
};
if (MoveAndCollideInternal(parameters, out var result, true, false))
{
if (
(result.GetAngleDegrees(UpDirection) <= FloorMaxAngle + FloorAngleThreshold)
|| (
wallAsFloor
&& result.GetAngleDegrees(-UpDirection) > FloorMaxAngle + FloorAngleThreshold
)
)
{
onFloor = true;
floorNormal = result.Normal;
SetPlatformData(result);
if (floorStopOnSlope)
{
// move_and_collide may stray the object a bit because of pre un-stucking,
// so only ensure that motion happens on floor direction in this case.
if (result.Travel.Length() > SafeMargin)
{
result.Travel = UpDirection * UpDirection.Dot(result.Travel);
}
else
{
result.Travel = Vector2.Zero;
}
}
GlobalTransform = GlobalTransform.Translated(result.Travel);
}
}
}
private bool OnFloorIfSnapped(bool wasOnFloor, bool velDirFacingUp)
{
if (UpDirection == Vector2.Zero || onFloor || !wasOnFloor || velDirFacingUp)
{
return false;
}
float length = Mathf.Max(FloorSnapLength, SafeMargin);
var parameters = new PhysicsTestMotionParameters2D
{
From = GlobalTransform,
Motion = -UpDirection * length,
Margin = SafeMargin,
RecoveryAsCollision = true,
CollideSeparationRay = true
};
bool collided = MoveAndCollideInternal(parameters, out var result, true, false);
if (collided && result.GetAngleDegrees(UpDirection) <= FloorMaxAngle + FloorAngleThreshold)
{
return true;
}
return false;
}
private bool MoveAndCollideInternal(
PhysicsTestMotionParameters2D parameters,
out CollisionResult collisionResult,
bool testOnly,
bool cancelSliding
)
{
var result = internalResult;
bool colliding = PhysicsServer2D.BodyTestMotion(GetRid(), parameters, result);
// Restore direction of motion to be along the original motion,
// in order to avoid sliding due to recovery,
// but only if collision depth is low enough to avoid tunneling.
if (cancelSliding)
{
float motionLength = parameters.Motion.Length();
float precision = 0.001f;
if (colliding)
{
// Can't just use margin as a threshold because collision depth is calculated on unsafe motion,
// so even in normal resting cases, the depth can be a bit more than the margin.
precision +=
motionLength
* (result.GetCollisionUnsafeFraction() - result.GetCollisionSafeFraction());
if (result.GetCollisionDepth() > parameters.Margin + precision)
{
cancelSliding = false;
}
}
if (cancelSliding)
{
// When motion is null, recovery is the resulting motion.
Vector2 motionNormal = Vector2.Zero;
if (motionLength > Mathf.Epsilon)
{
motionNormal = parameters.Motion / motionLength;
}
// Check depth of recovery.
float projectedLength = result.GetTravel().Dot(motionNormal);
Vector2 recovery = result.GetTravel() - motionNormal * projectedLength;
float recoveryLength = recovery.Length();
// Fixes cases where canceling slide causes the motion to go too deep into the ground,
// because we're only taking rest information into account and not general recovery.
if (recoveryLength < parameters.Margin + precision)
{
// Apply adjustment to motion.
result.Set("travel", motionNormal * projectedLength);
result.Set("remainder", parameters.Motion - result.GetTravel());
}
}
}
if (!testOnly)
{
Transform2D gt = parameters.From;
gt.Origin += result.GetTravel();
GlobalTransform = gt;
}
if (result.GetCollider() == null)
{
collisionResult = new CollisionResult { Collided = false };
}
else
{
collisionResult = new CollisionResult
{
Collided = true,
Normal = result.GetCollisionNormal(),
Travel = result.GetTravel(),
Remainder = result.GetRemainder(),
Collider = result.GetCollider(),
ColliderId = result.GetColliderId(),
ColliderRid = result.GetColliderRid(),
ColliderVelocity = result.GetColliderVelocity(),
SafeFraction = result.GetCollisionSafeFraction(),
UnsafeFraction = result.GetCollisionUnsafeFraction()
};
}
return colliding;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment