diff --git a/Game.Server/BaseEntity.cs b/Game.Server/BaseEntity.cs index d5e2273d..3f04bb11 100644 --- a/Game.Server/BaseEntity.cs +++ b/Game.Server/BaseEntity.cs @@ -1407,11 +1407,251 @@ public virtual void VPhysicsUpdate(IPhysicsObject physics) { PhysicsRelinkChildren(gpGlobals.FrameTime); break; } + + case Source.MoveType.Push: + VPhysicsUpdatePusher(physics); + break; + default: break; } } + static BaseEntity? FindPhysicsBlocker(IPhysicsObject physics, PhysicsPushList list, in Vector3 pushVel) { + IPhysicsFrictionSnapshot snapshot = physics.CreateFrictionSnapshot(); + BaseEntity? blocker = null; + float maxForce = 0; + while (snapshot.IsValid()) { + IPhysicsObject other = snapshot.GetObject(1)!; + BaseEntity? otherEntity = other.GetGameData() as BaseEntity; + bool inList = false; + for (int i = 0; i < list.PushedCount; i++) { + if (otherEntity == list.PushedEnts[i].Get()) { + inList = true; + break; + } + } + + snapshot.GetSurfaceNormal(out Vector3 normal); + float dot = Vector3.Dot(pushVel, snapshot.GetNormalForce() * normal); + if (blocker == null || (!inList && dot > maxForce)) { + blocker = otherEntity; + if (!inList) + maxForce = dot; + } + + snapshot.NextFrictionData(); + } + physics.DestroyFrictionSnapshot(snapshot); + + return blocker; + } + + struct PushBlock + { + public PhysicsPushList? List; + public BaseEntity RootParent; + public BaseEntity? BlockedEntity; + public float MoveBackFraction; + public float MoveTime; + } + + static void ComputePushStartMatrix(out Matrix3x4 start, BaseEntity entity, in PushBlock pushBlock) { + Vector3 localOrigin; + QAngle localAngles; + if (pushBlock.List != null) { + localOrigin = pushBlock.List.LocalOrigin; + localAngles = pushBlock.List.LocalAngles; + } + else { + localOrigin = pushBlock.RootParent.GetAbsOrigin() - pushBlock.RootParent.GetAbsVelocity() * pushBlock.MoveTime; + localAngles = pushBlock.RootParent.GetAbsAngles() - pushBlock.RootParent.GetLocalAngularVelocity() * pushBlock.MoveTime; + } + MathLib.AngleMatrix(localAngles, localOrigin, out Matrix3x4 xform); + + MathLib.MatrixInvert(pushBlock.RootParent.EntityToWorldTransform(), out Matrix3x4 srcInv); + MathLib.ConcatTransforms(xform, srcInv, out Matrix3x4 delta); + MathLib.ConcatTransforms(delta, entity.EntityToWorldTransform(), out start); + } + + static void CheckPushedEntity(BaseEntity entity, ref PushBlock pushBlock) { + IPhysicsObject? physics = entity.VPhysicsGetObject(); + if (physics == null) + return; + + Assert(physics.IsMoveable()); + IPhysicsShadowController? shadow = physics.GetShadowController(); + if (!physics.IsMoveable() || shadow == null) + return; + + bool checkrot = true; + bool checkmove = true; + physics.GetShadowPosition(out Vector3 origin, out QAngle angles); + float fraction = -1.0f; + + if (entity == pushBlock.RootParent) { + if (entity.GetLocalAngularVelocity() == vec3_angle) + checkrot = false; + if (entity.GetLocalVelocity() == vec3_origin) + checkmove = false; + } + + if (checkmove) { + Vector3 dir = entity.GetAbsVelocity(); + float speed = MathLib.VectorNormalize(ref dir); + shadow.GetTargetPosition(out Vector3 targetPos, out _); + float targetAmount = Vector3.Dot(targetPos, dir); + float currentAmount = Vector3.Dot(origin, dir); + float entityAmount = Vector3.Dot(entity.GetAbsOrigin(), dir); + + if ((targetAmount - entityAmount) > 1) + entity.UpdatePhysicsShadowToCurrentPosition(0); + else { + float dist = targetAmount - currentAmount; + if (dist > 1) { + float movementAmount = targetAmount - (speed * pushBlock.MoveTime); + if (entity == pushBlock.RootParent) { + if (pushBlock.List != null) { + Vector3 localVel = entity.GetLocalVelocity(); + MathLib.VectorNormalize(ref localVel); + float localTargetAmt = Vector3.Dot(entity.GetLocalOrigin(), localVel); + movementAmount = targetAmount + Vector3.Dot(pushBlock.List.LocalOrigin, localVel) - localTargetAmt; + } + } + else { + ComputePushStartMatrix(out Matrix3x4 start, entity, pushBlock); + MathLib.MatrixPosition(start, out Vector3 startPos); + movementAmount = Vector3.Dot(startPos, dir); + } + float expectedDist = targetAmount - movementAmount; + if (expectedDist <= 0) + fraction = 1; + else { + fraction = dist / expectedDist; + fraction = Math.Clamp(fraction, 0.0f, 1.0f); + } + } + } + } + + if (checkrot) { + MathLib.RotationDeltaAxisAngle(angles, entity.GetAbsAngles(), out Vector3 axis, out float deltaAngle); + if (MathF.Abs(deltaAngle) > 0.5f) { + shadow.GetTargetPosition(out _, out QAngle targetRot); + MathLib.RotationDeltaAxisAngle(angles, targetRot, out _, out float deltaTargetAngle); + if (MathF.Abs(deltaTargetAngle) > 0.01f) { + float expectedDist = deltaAngle; + if (entity == pushBlock.RootParent) + expectedDist = entity.GetLocalAngularVelocity().Length() * pushBlock.MoveTime; + else { + ComputePushStartMatrix(out Matrix3x4 start, entity, pushBlock); + MathLib.MatrixAngles(start, out QAngle startAngles, out _); + MathLib.RotationDeltaAxisAngle(startAngles, entity.GetAbsAngles(), out Vector3 startAxis, out float startAngle); + expectedDist = startAngle * Vector3.Dot(startAxis, axis); + } + + float t = expectedDist != 0.0f ? MathF.Abs(deltaAngle / expectedDist) : 1.0f; + t = Math.Clamp(t, 0.0f, 1.0f); + fraction = Math.Max(fraction, t); + } + else + entity.UpdatePhysicsShadowToCurrentPosition(0); + } + } + + if (fraction >= pushBlock.MoveBackFraction) { + pushBlock.MoveBackFraction = fraction; + pushBlock.BlockedEntity = entity; + } + } + + public void VPhysicsUpdatePusher(IPhysicsObject physics) { + float movetime = (float)(LocalTime - VPhysicsUpdateLocalTime); + if (movetime <= 0) + return; + + if (!PhysIsFinalTick()) + return; + + if (GetSolid() != SolidType.BSP && physics.GetShadowPosition(out _, out _) != 0) { + List list = []; + Hierarchy.GetAllInHierarchy(this, list); + + PhysicsPushList? pushList = null; + if (HasDataObjectType(DataObjectType.PhysicsPushList)) { + pushList = GetDataObject(DataObjectType.PhysicsPushList); + Assert(pushList != null); + } + bool checkrot = GetLocalAngularVelocity() != vec3_angle; + bool checkmove = GetLocalVelocity() != vec3_origin; + + PushBlock pushBlock = new() { + RootParent = this, + List = pushList, + BlockedEntity = null, + MoveBackFraction = 0.0f, + MoveTime = movetime + }; + for (int i = 0; i < list.Count; i++) { + if (list[i].IsSolid()) + CheckPushedEntity(list[i], ref pushBlock); + } + + TimeUnit_t physLocalTime = LocalTime; + if (pushBlock.BlockedEntity != null) { + float moveback = movetime * pushBlock.MoveBackFraction; + if (moveback > 0) { + physLocalTime = LocalTime - moveback; + if (physLocalTime <= (VPhysicsUpdateLocalTime + movetime * 0.99f)) { + BaseEntity? blocked = null; + + if (pushList != null) { + SetLocalOrigin(pushList.LocalOrigin); + SetLocalAngles(pushList.LocalAngles); + physLocalTime = pushList.LocalMoveTime; + for (int i = 0; i < pushList.PushedCount; i++) { + BaseEntity? pushedEntity = pushList.PushedEnts[i].Get(); + if (pushedEntity == null) + continue; + + pushedEntity.SetAbsOrigin(pushedEntity.GetAbsOrigin() - pushList.PushVec[i]); + } + BaseEntity? physicsBlocker = FindPhysicsBlocker(VPhysicsGetObject()!, pushList, pushList.PushVec[0]); + if (physicsBlocker != null) + blocked = physicsBlocker; + } + else { + Vector3 origin = GetLocalOrigin(); + QAngle angles = GetLocalAngles(); + + if (checkmove) + origin -= GetLocalVelocity() * moveback; + if (checkrot) + angles -= GetLocalAngularVelocity() * moveback; + + SetLocalOrigin(origin); + SetLocalAngles(angles); + } + + if (blocked != null) + Blocked(blocked); + + LocalTime = physLocalTime; + } + } + } + } + + if (HasDataObjectType(DataObjectType.PhysicsPushList)) + DestroyDataObject(DataObjectType.PhysicsPushList); + + VPhysicsUpdateLocalTime = LocalTime; + if (MoveDoneTime <= LocalTime && MoveDoneTime > 0) { + SetMoveDoneTime(-1); + MoveDone(); + } + } + public void PhysicsRelinkChildren(TimeUnit_t dt) { BaseEntity? child; diff --git a/Game.Server/Physics.cs b/Game.Server/Physics.cs index 8710bad3..71e0a5bb 100644 --- a/Game.Server/Physics.cs +++ b/Game.Server/Physics.cs @@ -258,6 +258,7 @@ public static bool PhysIsInCallback(){ public static void PhysAddShadow(BaseEntity entity) => g_ShadowEntities!.AddEntity(entity); public static void PhysRemoveShadow(BaseEntity entity) => g_ShadowEntities!.DeleteEntity(entity); + public static bool PhysIsFinalTick() => g_PhysicsHook.IsFinalTick; public static void PhysGetListOfPenetratingEntities(BaseEntity search, List list) => g_Collisions.GetListOfPenetratingEntities(search, list); public static void PhysOnCleanupDeleteList() { @@ -472,8 +473,18 @@ public override void LevelShutdownPostEntity() { } public override void FrameUpdatePostEntityThink() { base.FrameUpdatePostEntityThink(); - PhysFrame(gpGlobals.FrameTime); + + TimeUnit_t interval = gpGlobals.FrameTime > 0.0 ? TICK_INTERVAL : 0.0; + + if (BaseEntity.IsSimulatingOnAlternateTicks()) { + IsFinalTick = false; + PhysFrame(interval); + } + IsFinalTick = true; + PhysFrame(interval); } + + public bool IsFinalTick = true; public override void PreClientUpdate() { ImpactSoundTime += gpGlobals.FrameTime; if (ImpactSoundTime > 0.05) { diff --git a/Game.Server/PhysicsMain.cs b/Game.Server/PhysicsMain.cs index 188f27d2..0a6205ef 100644 --- a/Game.Server/PhysicsMain.cs +++ b/Game.Server/PhysicsMain.cs @@ -551,8 +551,12 @@ void PerformPush(TimeUnit_t movetime) { } if (VPhysicsGetObject() != null) { - if (movetime > 0 && Blocker.Get() == null && GetSolid() == SolidType.VPhysics && Physics.PushedEntities.CountMovedEntities() > 0) - throw new NotImplementedException(); + if (movetime > 0 && Blocker.Get() == null && GetSolid() == SolidType.VPhysics && Physics.PushedEntities.CountMovedEntities() > 0) { + Assert(!HasDataObjectType(DataObjectType.PhysicsPushList)); + PhysicsPushList list = CreateDataObject(DataObjectType.PhysicsPushList); + if (list != null) + Physics.PushedEntities.StoreMovedEntities(list); + } } else { if (MoveDoneTime <= LocalTime && MoveDoneTime > 0) { diff --git a/Game.Server/PushEntity.cs b/Game.Server/PushEntity.cs index 8135eca4..44b98a7b 100644 --- a/Game.Server/PushEntity.cs +++ b/Game.Server/PushEntity.cs @@ -47,6 +47,23 @@ protected struct PhysicsPushedInfo public int CountMovedEntities() => Moved.Count; + public void StoreMovedEntities(PhysicsPushList list) { + list.LocalMoveTime = RootPusherStartLocaltime; + list.LocalOrigin = RootPusherStartLocalOrigin; + list.LocalAngles = RootPusherStartLocalAngles; + list.PushedCount = CountMovedEntities(); + Assert(list.PushedCount < PhysicsPushList.MAX_PUSHED_ENTITIES); + if (list.PushedCount > PhysicsPushList.MAX_PUSHED_ENTITIES) + list.PushedCount = PhysicsPushList.MAX_PUSHED_ENTITIES; + + for (int i = 0; i < list.PushedCount; i++) { + EHANDLE handle = new(); + handle.Set(Moved[i].Entity); + list.PushedEnts[i] = handle; + list.PushVec[i] = Moved[i].Entity.GetAbsOrigin() - Moved[i].StartAbsOrigin; + } + } + public void BeginPush(BaseEntity root) { Moved.Clear(); Pusher.Clear(); diff --git a/Game.Shared/PhysicsMainShared.cs b/Game.Shared/PhysicsMainShared.cs index 0248d0f0..43e86f5a 100644 --- a/Game.Shared/PhysicsMainShared.cs +++ b/Game.Shared/PhysicsMainShared.cs @@ -828,7 +828,7 @@ public override void Shutdown() { //PositionWatcher new EntityDataInstantiator(), //PhysicsPushList - null!, + new EntityDataInstantiator(), //VPhysicsUpdateAI new EntityDataInstantiator(), //VPhysicsWatcher diff --git a/Game.Shared/PhysicsPushList.cs b/Game.Shared/PhysicsPushList.cs new file mode 100644 index 00000000..ec810a38 --- /dev/null +++ b/Game.Shared/PhysicsPushList.cs @@ -0,0 +1,17 @@ +using Source.Common.Mathematics; + +using System.Numerics; + +namespace Game.Shared; + +public class PhysicsPushList +{ + public const int MAX_PUSHED_ENTITIES = 32; + + public TimeUnit_t LocalMoveTime; + public Vector3 LocalOrigin; + public QAngle LocalAngles; + public int PushedCount; + public readonly EHANDLE[] PushedEnts = new EHANDLE[MAX_PUSHED_ENTITIES]; + public readonly Vector3[] PushVec = new Vector3[MAX_PUSHED_ENTITIES]; +} diff --git a/Source.Common/Mathematics/MathLib.cs b/Source.Common/Mathematics/MathLib.cs index e7e1c9ea..773ab692 100644 --- a/Source.Common/Mathematics/MathLib.cs +++ b/Source.Common/Mathematics/MathLib.cs @@ -3132,6 +3132,25 @@ public static void VectorAngles(in Vector3 forward, in Vector3 pseudoup, out QAn } } + public static void QuaternionAxisAngle(in Quaternion q, out Vector3 axis, out float angle) { + angle = RAD2DEG(2 * MathF.Acos(q.W)); + if (angle > 180) + angle -= 360; + + axis = new(q.X, q.Y, q.Z); + VectorNormalize(ref axis); + } + + public static void RotationDeltaAxisAngle(in QAngle srcAngles, in QAngle destAngles, out Vector3 deltaAxis, out float deltaAngle) { + AngleQuaternion(srcAngles, out Quaternion srcQuat); + AngleQuaternion(destAngles, out Quaternion destQuat); + QuaternionScale(srcQuat, -1, out Quaternion srcQuatInv); + QuaternionMult(destQuat, srcQuatInv, out Quaternion result); + + QuaternionNormalize(ref result); + QuaternionAxisAngle(result, out deltaAxis, out deltaAngle); + } + public static void AxisAngleQuaternion(in Vector3 axis, float angle, out Quaternion q) { SinCos(DEG2RAD(angle) * 0.5f, out float sa, out float ca); q.X = axis.X * sa;