76 lines
3.8 KiB
C#
76 lines
3.8 KiB
C#
#if MODULE_ENTITIES
|
|
using Pathfinding.Drawing;
|
|
using Pathfinding.Util;
|
|
using Unity.Burst;
|
|
using Unity.Collections;
|
|
using Unity.Entities;
|
|
using Unity.Mathematics;
|
|
using Unity.Transforms;
|
|
using UnityEngine;
|
|
|
|
namespace Pathfinding.ECS {
|
|
[BurstCompile]
|
|
public partial struct JobApplyGravity : IJobEntity {
|
|
[ReadOnly]
|
|
public NativeArray<RaycastHit> raycastHits;
|
|
[ReadOnly]
|
|
public NativeArray<RaycastCommand> raycastCommands;
|
|
public CommandBuilder draw;
|
|
public float dt;
|
|
|
|
public static void UpdateMovementPlaneFromNormal (float3 normal, ref AgentMovementPlane movementPlane) {
|
|
// Calculate a new movement plane that is perpendicular to the surface normal
|
|
// and is as similar to the previous movement plane as possible.
|
|
var forward = math.normalizesafe(math.mul(movementPlane.value.rotation, new float3(0, 0, 1)));
|
|
normal = math.normalizesafe(normal);
|
|
// TODO: This doesn't guarantee an orthogonal basis? forward and normal may not be perpendicular
|
|
movementPlane.value = new NativeMovementPlane(new quaternion(new float3x3(
|
|
math.cross(normal, forward),
|
|
normal,
|
|
forward
|
|
)));
|
|
}
|
|
|
|
void ResolveGravity (RaycastHit hit, bool grounded, ref LocalTransform transform, in AgentMovementPlane movementPlane, ref GravityState gravityState) {
|
|
var localPosition = movementPlane.value.ToPlane(transform.Position, out var currentElevation);
|
|
if (grounded) {
|
|
// Grounded
|
|
// Make the vertical velocity fall off exponentially. This is reasonable from a physical standpoint as characters
|
|
// are not completely stiff and touching the ground will not immediately negate all velocity downwards. The AI will
|
|
// stop moving completely due to the raycast penetration test but it will still *try* to move downwards. This helps
|
|
// significantly when moving down along slopes, because if the vertical velocity would be set to zero when the character
|
|
// was grounded it would lead to a kind of 'bouncing' behavior (try it, it's hard to explain). Ideally this should
|
|
// use a more physically correct formula but this is a good approximation and is much more performant. The constant
|
|
// CONVERGENCE_SPEED in the expression below determines how quickly it converges but high values can lead to too much noise.
|
|
const float CONVERGENCE_SPEED = 5f;
|
|
gravityState.verticalVelocity *= math.max(0, 1 - CONVERGENCE_SPEED * dt);
|
|
|
|
movementPlane.value.ToPlane(hit.point, out var hitElevation);
|
|
var elevationDelta = gravityState.verticalVelocity * dt;
|
|
const float VERTICAL_COLLISION_ADJUSTMENT_SPEED = 6f;
|
|
if (hitElevation > currentElevation) {
|
|
// Already below ground, only allow upwards movement
|
|
currentElevation = Mathf.MoveTowards(currentElevation, hitElevation, VERTICAL_COLLISION_ADJUSTMENT_SPEED * math.sqrt(math.abs(hitElevation - currentElevation)) * dt);
|
|
} else {
|
|
// Was above the ground, allow downwards movement until we are at the ground
|
|
currentElevation = math.max(hitElevation, currentElevation + elevationDelta);
|
|
}
|
|
} else {
|
|
var elevationDelta = gravityState.verticalVelocity * dt;
|
|
currentElevation += elevationDelta;
|
|
}
|
|
transform.Position = movementPlane.value.ToWorld(localPosition, currentElevation);
|
|
}
|
|
|
|
public void Execute (ref LocalTransform transform, in MovementSettings movementSettings, ref AgentMovementPlane movementPlane, ref GravityState gravityState, in AgentMovementPlaneSource movementPlaneSource, [Unity.Entities.EntityIndexInQuery] int entityIndexInQuery) {
|
|
var hit = raycastHits[entityIndexInQuery];
|
|
var hitAnything = math.any((float3)hit.normal != 0f);
|
|
if (hitAnything && movementPlaneSource.value == MovementPlaneSource.Raycast) {
|
|
UpdateMovementPlaneFromNormal(hit.normal, ref movementPlane);
|
|
}
|
|
ResolveGravity(hit, hitAnything, ref transform, in movementPlane, ref gravityState);
|
|
}
|
|
}
|
|
}
|
|
#endif
|