diff --git a/leg_mod/Data/Scripts/Bob/legCore.cs b/leg_mod/Data/Scripts/Bob/legCore.cs index a230550..11798b0 100644 --- a/leg_mod/Data/Scripts/Bob/legCore.cs +++ b/leg_mod/Data/Scripts/Bob/legCore.cs @@ -14,6 +14,8 @@ using VRageMath; using System.Linq; using SpaceEngineers.Game.Entities.Blocks; +using VRage.ModAPI; +using VRage; namespace MyMod @@ -108,20 +110,24 @@ public void Update() needToPlantFoot = true; - foreach (Leg leg in legs.ToList()) + for (int i = 0; i < legs.Count; i++) { + Leg leg = legs[i]; + + // Check if the leg's suspension is invalid or marked for removal if (leg.suspension == null || leg.suspension.MarkedForClose) { - legs.Remove(leg); + legs.RemoveAt(i); // Remove invalid leg + i--; // Adjust index to account for the removed element continue; } // Handle phase offset and foot planting if (Math.Abs(leg.GetPhaseOffset() - phase_leg_to_move) < PHASE_TOL) { - if (leg.isRooted) + if (leg.surface != null) { - phase_leg_to_move += 0.01; + phase_leg_to_move = legs[(i + 1) % legs.Count].GetPhaseOffset(); } leg.Update(false, true); // Plant foot only if necessary @@ -131,10 +137,14 @@ public void Update() leg.Update(false, false); // Move without planting } + // Display current phase and leg offset in a notification MyAPIGateway.Utilities.ShowNotification($"phase {phase_leg_to_move}, leg {leg.GetPhaseOffset()}", 16); - if (leg.isRooted) + // If the leg is on a surface, calculate and accumulate movement force + if (leg.surface != null) + { movementForce += leg.GetWalkForce(); + } } // TODO: fix workaround @@ -209,22 +219,160 @@ public void Draw() public class Segment { - IMyCubeBlock segment; + bool connected = false; + + const double spring_constant = 1000000; + IMyCubeGrid segment_grid; + IMyCubeGrid leg_grid; + Vector3D start_joint; + Vector3D end_joint; + + public Segment(string definition_string, Vector3D start_joint, Vector3D end_joint, Vector3D up, IMyCubeGrid leg_grid) + { + SpawnSegmentBlock(definition_string, start_joint, end_joint, up); + + this.start_joint = start_joint; + this.end_joint = end_joint; + this.leg_grid = leg_grid; + } + + public void Update(Vector3D start_joint, Vector3D end_joint) + { + var center = (start_joint - this.start_joint) / 2; - public Segment() + this.start_joint = start_joint; + this.end_joint = end_joint; + + /* + if (leg_grid as MyCubeGrid != null && segment_grid as MyCubeGrid != null && !connected) + { + try + { + Utilities.ConnectGrids(leg_grid as MyCubeGrid, segment_grid as MyCubeGrid); + if (MyAPIGateway.GridGroups.HasConnection(leg_grid, segment_grid, GridLinkTypeEnum.Logical)) + connected = true; + } + catch(Exception ex) + { + connected = false; + } + } + */ + + ApplyConnectingForces(); + } + + private void SpawnSegmentBlock(string definition_string, Vector3D start_joint, Vector3D end_joint, Vector3D up) { + // Get the definition for the block type of the main connector + MyDefinitionId id; + MyDefinitionId.TryParse(definition_string, out id); + + var blockDef = MyDefinitionManager.Static.GetCubeBlockDefinition(id); + if (blockDef == null) + return; + + // Get the position and orientation of the current connector + var _pos = (start_joint + end_joint) / 2; + var _fwd = Vector3D.Normalize(end_joint - start_joint); + var _up = Vector3D.Cross(_fwd, up); + + MatrixD worldMatrix = MatrixD.CreateWorld(_pos, _fwd, _up); + + // Create the object builder for the new connector + MyObjectBuilder_CubeBlock ob = (MyObjectBuilder_CubeBlock)MyObjectBuilderSerializer.CreateNewObject(blockDef.Id); + ob.EntityId = 0; // Allow the game to assign a new ID + ob.Min = Vector3I.Zero; + + // Create a new grid object builder for the detached grid + MyObjectBuilder_CubeGrid gridOB = MyObjectBuilderSerializer.CreateNewObject(); + gridOB.EntityId = 0; // Allow the game to assign a new ID + gridOB.DisplayName = ""; + gridOB.CreatePhysics = true; + gridOB.GridSizeEnum = blockDef.CubeSize; + gridOB.PositionAndOrientation = new MyPositionAndOrientation(_pos, worldMatrix.Forward, worldMatrix.Up); + gridOB.PersistentFlags = MyPersistentEntityFlags2.InScene; + gridOB.IsStatic = false; // This is a dynamic grid for the detached connector + gridOB.Editable = true; + gridOB.DestructibleBlocks = true; // Allow destruction of the grid + gridOB.IsRespawnGrid = false; + gridOB.CubeBlocks.Add(ob); + + IMyCubeGrid grid = null; + + // Spawn the new grid asynchronously + /* + MyAPIGateway.Entities.CreateFromObjectBuilderParallel(gridOB, true, gridEntity => + { + var spawnedGrid = gridEntity as MyCubeGrid; + MyAPIGateway.Utilities.ShowNotification($"grid : {spawnedGrid != null}", 10000); + if (spawnedGrid == null) + return; + // Set up the new grid (e.g., marking it as not a preview grid) + spawnedGrid.IsPreview = false; + spawnedGrid.Save = true; // Allow saving the new grid + + segment_grid = spawnedGrid as IMyCubeGrid; + });*/ + + IMyEntity ent = MyAPIGateway.Entities.CreateFromObjectBuilderAndAdd(gridOB); + ent.Save = true; + segment_grid = ent as IMyCubeGrid; } - public void ConnectSegment() + void MaintainOffsetVelocity(IMyCubeGrid targetGrid, IMyCubeGrid referenceGrid, Vector3D desiredOffset) { + // Ensure both grids have physics + var targetPhysics = targetGrid.Physics; + var referencePhysics = referenceGrid.Physics; + + if (targetPhysics == null || referencePhysics == null) + return; + + // Get current positions + Vector3D targetPosition = targetGrid.WorldMatrix.Translation; + Vector3D referencePosition = referenceGrid.WorldMatrix.Translation; + + // Calculate the current offset and the error + Vector3D currentOffset = targetPosition - referencePosition; + Vector3D offsetError = currentOffset - desiredOffset; + + // Calculate position correction velocity (Proportional Control) + const double correctionFactor = 2.0; // Adjust for responsiveness + Vector3D correctionVelocity = -offsetError * correctionFactor; + + // Get reference grid velocities + Vector3D referenceLinearVelocity = referencePhysics.LinearVelocity; + Vector3D referenceAngularVelocity = referencePhysics.AngularVelocity; + // Apply velocity adjustments + targetPhysics.LinearVelocity = referenceLinearVelocity + correctionVelocity; + targetPhysics.AngularVelocity = referenceAngularVelocity; + + // Optional: Log for debugging + MyAPIGateway.Utilities.ShowMessage("MaintainOffset", $"Offset Error: {offsetError}, Correction Velocity: {correctionVelocity}"); } public void ApplyConnectingForces() { + if (segment_grid?.Physics == null) // || !connected) + return; + /* + Vector3D block_start = segment_grid.WorldMatrix.Translation + segment_grid.WorldMatrix.Forward; + Vector3D block_end = segment_grid.WorldMatrix.Translation + segment_grid.WorldMatrix.Backward; + + segment_grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_FORCE, spring_constant * -(block_start - start_joint), block_start, null); + segment_grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_FORCE, spring_constant * -(block_end - end_joint), block_end, null); + + if(segment_grid.Physics.Gravity != null) + { + segment_grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_FORCE, (segment_grid as MyCubeGrid).GetCurrentMass() * -segment_grid.Physics.Gravity, null, null); + } + MaintainOffsetVelocity(segment_grid, leg_grid, (start_joint - this.start_joint) / 2 - leg_grid.WorldMatrix.Translation); + */ } } @@ -258,6 +406,37 @@ public static Vector3D Lerp(Vector3D start, Vector3D end, double t) return start + (end - start) * t; } + public static void ConnectGrids(MyCubeGrid a, MyCubeGrid b) + { + if (!a.IsInSameLogicalGroupAs(b)) + { + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Logical, a.EntityId, a, b); + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Physical, a.EntityId, a, b); + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Electrical, a.EntityId, a, b); + + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Logical, b.EntityId, a, b); + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Physical, b.EntityId, a, b); + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Electrical, b.EntityId, a, b); + } + // MyLog.Default.Info($"[Tether] ConnectGrids: grids {a.EntityId} and {b.EntityId} are now connected"); + + } + + public static void DisconnectGrids(MyCubeGrid a, MyCubeGrid b) + { + + if (a.IsInSameLogicalGroupAs(b)) + { + MyCubeGrid.BreakGridGroupLink(GridLinkTypeEnum.Logical, a.EntityId, a, b); + MyCubeGrid.CreateGridGroupLink(GridLinkTypeEnum.Physical, a.EntityId, a, b); + MyCubeGrid.BreakGridGroupLink(GridLinkTypeEnum.Electrical, a.EntityId, a, b); + + MyCubeGrid.BreakGridGroupLink(GridLinkTypeEnum.Logical, b.EntityId, a, b); + MyCubeGrid.BreakGridGroupLink(GridLinkTypeEnum.Physical, b.EntityId, a, b); + MyCubeGrid.BreakGridGroupLink(GridLinkTypeEnum.Electrical, b.EntityId, a, b); + } + } + // Function to draw a line between two vectors public static void DrawLineBetweenVectors(Vector3D start, Vector3D end, Color color) { @@ -304,42 +483,43 @@ public static bool BobRayCast(Vector3D start, Vector3D end, IMyCubeGrid ignoredG public class Leg { + private bool init = false; + public IMyMotorSuspension suspension; public IMyCubeGrid grid; public bool isRagdolling; - public bool isInStandingRange; - public bool inContact; - public bool isRooted; public bool isStepping; - public bool isMovingToSurface; - - IHitInfo surface; + public bool isRooted; - Vector3D upDirection; - Vector3D newFootLocation; - Vector3D curFootLocation; - Vector3D oldFootLocation; + Vector3D footLocation; + Vector3D targetLocation; - double heightAboveSurface = 0; + Vector3D up = Vector3D.Zero; + Vector3D direction = Vector3D.Zero; + Vector3D relativeSurfaceVelocity = Vector3D.Zero; + double suspension_height = double.MaxValue; - public double height => 10 * suspension.Height; + public double height => 15 * suspension.Height; public double StepLength => height / 2; - public double StepHeight => height / 3; + public double StepHeight => height / 5; + + const double MaxSquatStrength = 1e6; + const double MaxWalkStrength = 5000; + const double update = 0.01667; - double MaxSquatStrength = 1e6; - double MaxWalkStrength = 5000; + public Surface surface; List segments = new List(); List joints = new List - { - Vector3D.Zero, - Vector3D.Zero, - Vector3D.Zero, - }; + { + Vector3D.Zero, + Vector3D.Zero, + Vector3D.Zero, + }; - List segmentLengths = new List { 7.5, 7.5 }; + List segmentLengths = new List { 8, 8 }; public Leg(IMyMotorSuspension suspension, IMyCubeGrid grid) { @@ -357,199 +537,194 @@ public void Update(bool forceMoveFoot, bool canMoveFoot) suspension.Top.Close(); } - upDirection = Vector3D.Normalize(-grid.Physics.Gravity); + if (!init) + init_leg(); - UpdateState(forceMoveFoot, canMoveFoot); + UpdateFootLocation(forceMoveFoot, canMoveFoot); UpdateJoints(); DebugDrawLeg(); - - ApplyLegForces(1, Vector3D.Zero); // Assuming 4 legs + } - private void UpdateState(bool forceMoveFoot, bool canMoveFoot) + + public void UpdateFootLocation(bool forceMoveFoot, bool canMoveFoot) { + suspension_height = GetSuspensionHeight(); + targetLocation = GetTargetLocation(); + relativeSurfaceVelocity = GetRelativeSurfaceVelocity(); - double dist = Vector3D.Distance(curFootLocation, -upDirection * height); + if (suspension_height > 2 * height) + return; - if (!suspension.IsFunctional || !suspension.Enabled) - { - TransitionToRagdoll(); - } - else if (!isStepping && !isMovingToSurface && (forceMoveFoot || (canMoveFoot && dist > height * 0.5) || dist > height)) - { - MyAPIGateway.Utilities.ShowNotification($"is{!isStepping} && istm {!isMovingToSurface} && cmf {canMoveFoot} && dist {Vector3D.Distance(curFootLocation, -upDirection * height) > height * 0.5}", 32 ); - TransitionToStepping(); - } - else if (isStepping) + ApplyVelocityLimit(); + + if (surface == null) { - CheckIfSteppingComplete(); + if (!isOverextended(canMoveFoot)) + GetSurface(); + footLocation += Vector3D.Normalize(targetLocation - footLocation + up * (Vector3D.Distance(targetLocation, footLocation) - StepHeight)) * update * StepLength * (1 + relativeSurfaceVelocity.Length() / 5); + //DebugState($"dist {Vector3D.Distance(MyAPIGateway.Session.Camera.WorldMatrix.Translation, footLocation + suspension.GetPosition())}", Color.Black); } - else if (isMovingToSurface) + + if (surface != null) { - CheckIfReachedSurface(); + footLocation = surface.Position - suspension.GetPosition(); + ApplyLegForces(1); + } - else if (isRooted) + + if(isOverextended(canMoveFoot) || forceMoveFoot) { - TransitionToRooted(); + surface = null; } } - private void TransitionToMovingToSurface() + public void init_leg() { - isStepping = false; - isMovingToSurface = true; + up = GetUpDirection(); - DebugState("transition to moving to surf", Color.Gray); - } + footLocation = -up * height; - private void TransitionToRagdoll() - { - isRagdolling = true; - isRooted = false; - isStepping = false; - isMovingToSurface = false; - isInStandingRange = false; + joints[0] = suspension.GetPosition(); + FABRIKSolver.SolveFABRIK(joints, segmentLengths, footLocation + suspension.GetPosition()); - newFootLocation = -upDirection * height; - curFootLocation = (3 * curFootLocation + newFootLocation) / 4; + for (int i = 0; i < joints.Count - 1; i++) + { + segments.Add(new Segment("MyObjectBuilder_UpgradeModule/LargeProductivityModule", joints[i], joints[i+1], up, grid)); + } - DebugState("transition to ragdoll", Color.Gray); + init = true; } - private void TransitionToStanding() + public bool isOverextended(bool canMoveFoot) { - isRagdolling = false; - isInStandingRange = true; + Utilities.DrawLineBetweenVectors(footLocation + suspension.GetPosition(), targetLocation + suspension.GetPosition(), Color.Green); - DebugState("transition to standing", Color.Gray); + return (canMoveFoot && Vector3D.Distance(footLocation, targetLocation) > StepLength) || footLocation.Length() > height; } - private void TransitionToStepping() + public void UpdateJoints() { - isRooted = false; - isMovingToSurface = false; - isStepping = true; - oldFootLocation = curFootLocation; + joints[0] = suspension.GetPosition(); + FABRIKSolver.SolveFABRIK(joints, segmentLengths, footLocation + suspension.GetPosition()); - DebugState("transition to stepping", Color.Yellow); + for (int i = 0; i < joints.Count - 1; i++) + { + segments[i].Update(joints[i], joints[i + 1]); + } } - private void CheckIfSteppingComplete() + public void DebugDrawLeg() { - newFootLocation = (newFootLocation + Vector3D.Normalize(grid.Physics.LinearVelocity) * StepLength - upDirection * (height - StepHeight)) / 2; - - Utilities.DrawLineBetweenVectors(suspension.GetPosition(), newFootLocation + suspension.GetPosition(), Color.Blue); - - curFootLocation = (3 * curFootLocation + newFootLocation) / 4; - - MyAPIGateway.Utilities.ShowNotification($"dist {Vector3D.Distance(newFootLocation, curFootLocation)}, height {StepHeight * 2}", 16); - - if (Vector3D.Distance(newFootLocation, curFootLocation) < StepHeight * 2) + for (int i = 0; i < joints.Count - 1; i++) { - FindSurfaceLocation(Vector3D.Zero); + Utilities.DrawLineBetweenVectors(joints[i], joints[i + 1], Color.Red); } - - DebugState("transition to checking", Color.Yellow); } - private void CheckIfReachedSurface() + public void ApplyLegForces(int legs) { - curFootLocation = (9 * curFootLocation + newFootLocation) / 10; + if (grid.Physics == null) + return; - Utilities.DrawLineBetweenVectors(suspension.GetPosition(), newFootLocation + suspension.GetPosition(), Color.Blue); - if (Vector3D.Distance(newFootLocation, curFootLocation) < StepHeight) + Vector3D netForce = Vector3D.Zero; + + if (suspension_height < 1.5 * height && suspension_height > height) { - TransitionToRooted(); + // TransitionToStanding(); } + else + { + netForce += up * GetSquatForce(up, legs, suspension_height); + + // TransitionToStanding(); + } + + netForce += CalculateFrictionForceVector(grid.Physics.Mass / legs * grid.Physics.Gravity, 1.0, 1.0); + + DebugState($"netforce {netForce.Length()}", Color.Black); - DebugState("transition to moving to surface", Color.Cyan); + grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_FORCE, netForce, null, null); } - private void TransitionToRooted() + public Vector3D GetTravelDirection() { - curFootLocation = surface.Position - suspension.GetPosition(); - isRooted = true; - isMovingToSurface = false; - - DebugState("transition to rooted", Color.Green); + if(relativeSurfaceVelocity.Length() > 5) + return Vector3D.Normalize(relativeSurfaceVelocity); + else + return Vector3D.Zero; } - public void UpdateJoints() + public Vector3D GetRelativeSurfaceVelocity() { - joints[0] = suspension.GetPosition(); - FABRIKSolver.SolveFABRIK(joints, segmentLengths, curFootLocation + suspension.GetPosition()); + if(grid.Physics == null) + return Vector3D.Zero; + + if (surface == null) + return grid.LinearVelocity; + + Vector3D relativeVelocity = grid.LinearVelocity - surface.Velocity; + return relativeVelocity - Vector3D.Dot(relativeVelocity, surface.Normal) * surface.Normal; } - public void DebugDrawLeg() + public double GetSuspensionHeight() { - for (int i = 0; i < joints.Count - 1; i++) + IHitInfo validHit; + if (!Utilities.BobRayCast(suspension.GetPosition(), suspension.GetPosition() - up * 2 * height, grid, out validHit)) { - Utilities.DrawLineBetweenVectors(joints[i], joints[i + 1], Color.Red); + return double.MaxValue; } + + return Vector3D.Distance(validHit.Position, suspension.GetPosition()); } - public void FindSurfaceLocation(Vector3D surfaceVelocity) + public Vector3D GetTargetLocation() { IHitInfo hit; - Vector3D start = curFootLocation + upDirection * height + suspension.GetPosition(); - Vector3D end = start - upDirection * 4 * height; + Vector3D start = GetTravelDirection() * StepLength + suspension.GetPosition(); + Vector3D end = start - up * 2 * height; + + Utilities.DrawLineBetweenVectors(start, end, Color.Blue); if (Utilities.BobRayCast(start, end, grid, out hit)) { - TransitionToMovingToSurface(); - newFootLocation = hit.Position; - surface = hit; - DebugState("fd srf", Color.Green); + return hit.Position - suspension.GetPosition(); } - Utilities.DrawLineBetweenVectors(start, end, Color.Black); - DebugState($"checking for surf {hit == null}", Color.Green); + return start - up * height - suspension.GetPosition(); } - public void ApplyLegForces(int legs, Vector3D surfaceVelocity) + public Surface GetSurface() { - IHitInfo validHit; - if (!Utilities.BobRayCast(suspension.GetPosition(), suspension.GetPosition() - upDirection * 2 * height, grid, out validHit)) - { - TransitionToRagdoll(); - return; - } + IHitInfo hit; + Vector3D start = footLocation + 0.1 * up + suspension.GetPosition(); + Vector3D end = suspension.GetPosition(); - Vector3D netForce = Vector3D.Zero; - double distanceFromGround = Vector3D.Distance(validHit.Position, suspension.GetPosition()); + Utilities.DrawLineBetweenVectors(start, end, Color.Black); - if (distanceFromGround < 1.5 * height && distanceFromGround > height) - { - TransitionToStanding(); - } - else + if (Utilities.BobRayCast(start, end, grid, out hit)) { - netForce += upDirection * GetSquatForce(upDirection, legs, distanceFromGround); - TransitionToStanding(); + surface = new Surface(hit); + + return surface; } - netForce += CalculateFrictionForceVector(validHit.Normal, Vector3D.Zero, grid.Physics.Mass / legs * grid.Physics.Gravity, 1.0, 1.0); - grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_FORCE, netForce, null, null); + DebugState($"checking for surf {hit == null}", Color.Green); + + return null; } - /// - /// Calculates the surface-relative velocity of the grid. - /// - /// The normal vector of the surface. - /// The velocity of the surface. - /// A velocity vector relative to the surface. - public Vector3D CalculateSurfaceVelocity(Vector3D surfaceNormal, Vector3D surfaceVelocity) + public Vector3D GetUpDirection() { - Vector3D relativeVelocity = grid.LinearVelocity - surfaceVelocity; - return relativeVelocity - Vector3D.Dot(relativeVelocity, surfaceNormal) * surfaceNormal; + if (grid.Physics?.Gravity != null) + return -Vector3D.Normalize(grid.Physics.Gravity); + else if (surface != null) + return surface.Normal; + else + return suspension.WorldMatrix.Up; } - /// - /// Calculates the maximum extension of the leg based on segment lengths and a fudge factor. - /// - /// A multiplier to slightly increase the maximum extension. - /// The maximum extension length of the leg. public double GetMaxExtension(double fudge) { double maxExtension = 0; @@ -560,22 +735,27 @@ public double GetMaxExtension(double fudge) return maxExtension; } - /// - /// Gets the phase offset of the leg, used for determining gait timing. - /// - /// A normalized value between 0 and 1 representing the phase offset. public double GetPhaseOffset() { return suspension.MaxSteerAngle / Math.PI; } - /// - /// Calculates the squat force applied by the leg based on height difference and surface normal. - /// - /// The normal vector of the surface. - /// The total number of legs contributing to the force. - /// The height difference from the target position. - /// The magnitude of the squat force. + public void ApplyVelocityLimit() + { + if (grid.Physics == null) + return; + + double speedTowardsSurface = -Vector3D.Dot(grid.Physics.LinearVelocity - relativeSurfaceVelocity, up); + + if (suspension_height >= 0.75 * height) // airshock region + { + if (speedTowardsSurface < 1) + { + grid.Physics.LinearVelocity += up * (speedTowardsSurface - 1) * 0.5; + } + } + } + public double GetSquatForce(Vector3D surfaceNormal, int legs, double distanceFromGround) { double speedTowardsSurface = -Vector3D.Dot(grid.Physics.LinearVelocity, Vector3D.Normalize(surfaceNormal)); @@ -592,10 +772,6 @@ public double GetSquatForce(Vector3D surfaceNormal, int legs, double distanceFro else { squatForce = 1.0 * (grid.Physics.Mass * grid.Physics.Gravity / legs).Length(); - if (speedTowardsSurface < 1) - { - grid.Physics.LinearVelocity += Vector3D.Normalize(surfaceNormal) * (speedTowardsSurface - 1) * 0.5; - } } //MyAPIGateway.Utilities.ShowNotification($"sf {(int)squatForce}", 16); @@ -605,21 +781,11 @@ public double GetSquatForce(Vector3D surfaceNormal, int legs, double distanceFro return Math.Min(squatForce, MaxSquatStrength); } - /// - /// Gets the walking force based on suspension friction. - /// - /// The calculated walking force. public double GetWalkForce() { return MaxWalkStrength * suspension.Friction; } - /// - /// Calculates the normal force vector from a given surface normal and force vector. - /// - /// The normal vector of the surface. - /// The total force applied on the surface. - /// The normal force vector. public Vector3D CalculateNormalForceVector(Vector3D surfaceNormal, Vector3D force) { surfaceNormal = Vector3D.Normalize(surfaceNormal); @@ -627,30 +793,19 @@ public Vector3D CalculateNormalForceVector(Vector3D surfaceNormal, Vector3D forc return Math.Abs(dotProduct) * surfaceNormal; } - /// - /// Calculates the friction force vector based on surface conditions and the force applied. - /// - /// The normal vector of the surface. - /// The velocity of the surface relative to the leg. - /// The normal force acting on the surface. - /// The coefficient of static friction. - /// The coefficient of dynamic friction. - /// The calculated friction force vector. - public Vector3D CalculateFrictionForceVector(Vector3D surfaceNormal, Vector3D surfaceVelocity, Vector3D normalForce, double staticFrictionCoefficient, double dynamicFrictionCoefficient) + public Vector3D CalculateFrictionForceVector(Vector3D normalForce, double staticFrictionCoefficient, double dynamicFrictionCoefficient) { - if (!isRooted) + if (surface == null) return Vector3D.Zero; - Vector3D velocityPerpendicular = CalculateSurfaceVelocity(surfaceNormal, surfaceVelocity); - - if (velocityPerpendicular.LengthSquared() > 1) + if (relativeSurfaceVelocity.LengthSquared() > 1) { - return dynamicFrictionCoefficient * -Vector3D.Normalize(velocityPerpendicular) * - (normalForce.Length() + 100 * velocityPerpendicular.LengthSquared()) * suspension.Friction / 100; + return dynamicFrictionCoefficient * -Vector3D.Normalize(relativeSurfaceVelocity) * + (normalForce.Length() + 100 * relativeSurfaceVelocity.LengthSquared()) * suspension.Friction / 100; } else { - return staticFrictionCoefficient * -Vector3D.Normalize(velocityPerpendicular) * + return staticFrictionCoefficient * -Vector3D.Normalize(relativeSurfaceVelocity) * normalForce.Length() * suspension.Friction / 100; } } @@ -658,10 +813,8 @@ public Vector3D CalculateFrictionForceVector(Vector3D surfaceNormal, Vector3D su private void DebugState(string message, Color color) { MyAPIGateway.Utilities.ShowNotification(message, 16); - //Utilities.DrawLineBetweenVectors(suspension.GetPosition(), curFootLocation + suspension.GetPosition(), color); } - // Other helper methods remain unchanged (GetSquatForce, CalculateFrictionForceVector, etc.) } [MyEntityComponentDescriptor(typeof(MyObjectBuilder_MotorSuspension), true)] public class LegSuspensionGameLogic : MyGameLogicComponent @@ -693,6 +846,68 @@ public override void Init(MyObjectBuilder_EntityBase objectBuilder) } } + public class Surface + { + public IMyEntity Entity { get; private set; } + private Vector3D initialNormal; + private Vector3D initialPosition; + public float Friction { get; private set; } + + public Surface(IHitInfo hit, float fric = 1f) + { + initialNormal = hit.Normal; + Entity = hit.HitEntity; + initialPosition = hit.Position; + Friction = fric; + } + + // Getter for position in world space + public Vector3D Position + { + get + { + if (Entity != null) + { + // Update position based on the entity's current transformation + return Vector3D.Transform(initialPosition - Entity.GetPosition(), Entity.WorldMatrix); + } + return initialPosition; + } + } + + // Getter for velocity in world space + public Vector3D Velocity + { + get + { + if (Entity != null && Entity.Physics != null) + { + // Include both linear and rotational contributions + var angularVelocity = Entity.Physics.AngularVelocity; + var relativePosition = Position - Entity.GetPosition(); + var rotationalVelocity = Vector3D.Cross(angularVelocity, relativePosition); + + return Entity.Physics.LinearVelocity + rotationalVelocity; + } + return Vector3D.Zero; + } + } + + // Getter for normal relative to the entity's orientation + public Vector3D Normal + { + get + { + if (Entity != null) + { + // Transform the initial normal vector into the entity's local space + return Vector3D.TransformNormal(initialNormal, MatrixD.Transpose(Entity.WorldMatrix)); + } + return initialNormal; + } + } + } + [MySessionComponentDescriptor(MyUpdateOrder.BeforeSimulation)] // No continuous updates needed for this component public class LegSessionComponent : MySessionComponentBase