From fbe9bce518a12a41b76a188789365f62c972dfcf Mon Sep 17 00:00:00 2001 From: Mattias Holm Date: Wed, 21 Oct 2026 19:57:30 +0200 Subject: [PATCH] Harpoons, landing points, step sizes --- Documentation/HarpoonAnchoring.md | 51 ++++ Documentation/LandingMechanics.md | 56 ++++ Sources/CelMek/FiniteBurnStateTargeting.swift | 17 +- Sources/CelMek/HarpoonAnchoring.swift | 105 +++++++ Sources/CelMek/LandingMechanics.swift | 264 ++++++++++++++++++ .../FiniteBurnControlStepTests.swift | 26 ++ Tests/CelMekTests/HarpoonTests.swift | 42 +++ .../LandingConservationTests.swift | 29 ++ Tests/CelMekTests/LandingMechanicsTests.swift | 76 +++++ 9 files changed, 660 insertions(+), 6 deletions(-) create mode 100644 Documentation/HarpoonAnchoring.md create mode 100644 Documentation/LandingMechanics.md create mode 100644 Sources/CelMek/HarpoonAnchoring.swift create mode 100644 Sources/CelMek/LandingMechanics.swift create mode 100644 Tests/CelMekTests/FiniteBurnControlStepTests.swift create mode 100644 Tests/CelMekTests/HarpoonTests.swift create mode 100644 Tests/CelMekTests/LandingConservationTests.swift create mode 100644 Tests/CelMekTests/LandingMechanicsTests.swift diff --git a/Documentation/HarpoonAnchoring.md b/Documentation/HarpoonAnchoring.md new file mode 100644 index 0000000..92d96bd --- /dev/null +++ b/Documentation/HarpoonAnchoring.md @@ -0,0 +1,51 @@ +# Harpoons and terrain tethers + +`HarpoonAnchoring.swift` extends the compliant landing mechanics with explicit +terrain anchoring and mass-conserving projectile launch. This is an engineering +model; supply measured material properties only when they are actually available. + +`MechanicalBody.launching` separates a small rigid component whose centre is +specified in the original body's axes. It subtracts component inertia and the +parallel-axis contributions, moves the retained centre of mass, and carries +rotational point velocity into both new bodies. Equal/opposite firing impulses +supply separation energy and produce recoil/mount torque. No mass is created or +counted twice. Component inertia is in the same body axes. Invalid resulting +inertia throws. Use `retainedCentreBodyMetres` to adjust every retained contact +and attachment coordinate. The projectile starts with the parent's orientation +and angular velocity; an impulse through its own centre adds no projectile spin. + +`HarpoonAnchor` declares the surface `Frame` identity. Firing is one-shot and +can explicitly misfire. Incoming impacts test incidence and normal kinetic +energy, using a constant penetration-resistance force: + + estimated depth = m * normalSpeed² / (2 * resistance) + +Attachment requires depth between the declared minimum and maximum supported +burial depths and an adequate incidence cosine. Too-hard, too-shallow, grazing +and overpenetrating impacts are rejected. This is not a soil mechanics, ice +fracture or fabric/rock interaction solver. Holding strength is a separate input; +it is not inferred from a visual texture, illumination or the penetration depth. +`surfacePointMetres` records the cable exit point on the terrain in surface axes; +penetration depth is recorded separately, not added as rendered surface relief. +The application supplies rejected-projectile rebound/contact behavior. + +`TerrainTether` evaluates the existing cable tension law against a prescribed +moving endpoint. Transform the surface-local cable exit point and its velocity +into the inertial integration frame at every stage. Apply the returned force at +the lander's body-local mount and record the equal/opposite reaction on the +prescribed celestial body. Do not integrate a fictitious anchor mass, aggregate +its inertia with the lander, or reparent the lander's physical motion to it. +The full celestial ephemeris is not perturbed by these ground reactions. + +`commitLoad` checks breaking and pullout at accepted substep boundaries. Cable +breakage takes precedence if both thresholds are exceeded. A broken or cut cable +leaves a successfully embedded anchor in place. Pullout instead releases the +projectile to independent dynamics while its cable can remain connected. Cutting +or changing an attachment state applies no hidden impulse or velocity reset to +the lander. The application transfers projectile stopping momentum into the +prescribed terrain, rather than removing that momentum from the lander twice. + +Winch payout/retraction is commanded mechanical work. Motor energy/force limits, +capstan friction, subsurface stress distribution, temperature-dependent strength, +foot screws and hold-down thrusters are outside this initial model. Event timing +is resolved to the chosen mechanical substep, not continuously certified. diff --git a/Documentation/LandingMechanics.md b/Documentation/LandingMechanics.md new file mode 100644 index 0000000..b01c998 --- /dev/null +++ b/Documentation/LandingMechanics.md @@ -0,0 +1,56 @@ +# Compliant landing mechanics + +`CelMek/LandingMechanics.swift` provides independent rigid bodies, cable edges, +spherical contact lobes/feet, local terrain planes and adiabatic gas cushions. +This is a compliant engineering model, not a rigid constraint solver or deformable +fabric model. All quantities use SI units. + +A `MechanicalSystem` declares one pseudo-inertial `Frame` and an `Epoch`. Body +positions and velocities are in that reference, orientations map body axes into +it, angular velocities and inertia are in body axes, and mount coordinates are +relative to each body's centre of mass. The connection graph is separate from +frame parentage. Multiple cables between the same pair of bodies are allowed. +Rigid assemblies can still use `MassComponent`; detach rigid components before +creating independent bodies and do not count their masses twice. + +Cable extension and attachment-point relative speed produce tension. Slack cables +exert no force, even when the endpoints are moving apart. Tension is clamped to +zero rather than allowing compression. Payout subtracts from extension rate and +advances rest length; retrieval can do positive mechanical work. Each load acts +at both mounts with equal and opposite reference forces and body-axis torques. +Cutting changes connectivity only. Breaking commits at accepted substep boundaries; +`cableLoads` is pure and never changes connectivity. A broken cable contributes no +load after the break boundary. Event time uncertainty is up to one substep. + +Contacts use a plane supplied by the terrain provider, normal spring support, +compression-only normal damping and speed-regularized Coulomb friction. Forces +act at the lobe's surface point and include angular point velocity. There is no +adhesion. Optional rolling resistance applies a dissipative torque proportional +to normal load and a resistance length, opposing angular motion tangent to the +plane. Its default is zero; a rigid sphere can roll without friction stopping it. +Regularized friction approaches zero with slip speed; this is not an +exact static-friction constraint. Supply local surface velocity explicitly; +for a rotating surface, evaluate a sufficiently local patch per contact. No +terrain queries depend on visual LOD. + +Gas pressure follows `P V^gamma = gasInventory`, reduced by ambient pressure. +Pressure support ramps with penetration over half a lobe radius, approximating +the growing contact footprint rather than applying full-area preload at first +touch. This footprint approximation is distinct from the gas compression volume. +Volume has a lower bound; normal contact stiffness supplies the bottom-out guard. +`vent(retaining:)` removes gas inventory monotonically. Creating a cushion at its +initial absolute pressure is an explicit inflation initialization, not a modeled +inflator or gas flow network. Geometry changes, gas vent rates and deployment +controllers belong to the application. Fabric stress, leaks, rupture, temperature +transport and inter-lobe plumbing are not included. + +`step` uses explicit midpoint integration and normalizes quaternions. External +loads are evaluated at the beginning and midpoint and must be pure functions of +sample epoch/state. Gravity, aerodynamic loads and controls belong there; do not +also apply the internal cable reactions. A failed step leaves the system unchanged. +Steps must be positive and at most 0.01 s, but that cap is **not** a stability +guarantee for arbitrary stiffness. Choose a smaller step relative to the highest +contact/cable natural frequency and check convergence. Open Orbit uses 0.002 s +for its declared low-mass landing demonstrations. No continuous collision search, +inter-body collision, rigid joints, line/cable terrain collision or long-duration +tether orbital integrator is provided. diff --git a/Sources/CelMek/FiniteBurnStateTargeting.swift b/Sources/CelMek/FiniteBurnStateTargeting.swift index f6deaac..f99ca02 100644 --- a/Sources/CelMek/FiniteBurnStateTargeting.swift +++ b/Sources/CelMek/FiniteBurnStateTargeting.swift @@ -4,7 +4,9 @@ import Math /// Two fixed-ignition inertial burns corrected against a full terminal state. /// Six duration-vector components are solved with finite differences and -/// pivoted elimination. Clearance and operational constraints remain caller checks. +/// pivoted elimination. `relativeControlStep` scales the duration-vector finite +/// differences (with a 1 ms floor); a smaller value can resolve long-burn +/// terminal encounters. Existing callers retain the 1e-4 default. Clearance and operational constraints remain caller checks. public struct FiniteBurnStateTargetingReport { public let best: FiniteTransferEvaluation public let evaluations: Int @@ -16,19 +18,22 @@ public enum FiniteBurnStateTargeting { positionToleranceMetres: Double = 10, velocityToleranceMetresPerSecond: Double = 0.01, minimumRemainingPropellantKilograms: Double = 0, maximumEvaluations: Int = 160, numericalOptions: NumericalPropagationOptions = .preset(for:.high), - exhaustVelocityForDirection: ((SIMD3) throws -> Double)? = nil) throws -> FiniteTransferEvaluation { + exhaustVelocityForDirection: ((SIMD3) throws -> Double)? = nil, + relativeControlStep: Double = 1e-4) throws -> FiniteTransferEvaluation { try refineWithReport(initialState:initialState,initialSchedule:initialSchedule,target:target,context:context, backgroundModels:backgroundModels,positionToleranceMetres:positionToleranceMetres, velocityToleranceMetresPerSecond:velocityToleranceMetresPerSecond,minimumRemainingPropellantKilograms:minimumRemainingPropellantKilograms, - maximumEvaluations:maximumEvaluations,numericalOptions:numericalOptions,exhaustVelocityForDirection:exhaustVelocityForDirection).best + maximumEvaluations:maximumEvaluations,numericalOptions:numericalOptions,exhaustVelocityForDirection:exhaustVelocityForDirection,relativeControlStep:relativeControlStep).best } public static func refineWithReport(initialState: SpacecraftState, initialSchedule: ScheduledThrust, target: CartesianState, context: ForceEvaluationContext, backgroundModels: [any ForceModel], positionToleranceMetres: Double = 10, velocityToleranceMetresPerSecond: Double = 0.01, minimumRemainingPropellantKilograms: Double = 0, maximumEvaluations: Int = 160, numericalOptions: NumericalPropagationOptions = .preset(for:.high), - exhaustVelocityForDirection: ((SIMD3) throws -> Double)? = nil) throws -> FiniteBurnStateTargetingReport { - guard initialSchedule.burns.count == 2, (14...10_000).contains(maximumEvaluations), + exhaustVelocityForDirection: ((SIMD3) throws -> Double)? = nil, + relativeControlStep: Double = 1e-4) throws -> FiniteBurnStateTargetingReport { + guard relativeControlStep.isFinite,relativeControlStep>0,relativeControlStep<=0.01, + initialSchedule.burns.count == 2, (14...10_000).contains(maximumEvaluations), initialSchedule.frame === initialState.frame, positionToleranceMetres.isFinite, velocityToleranceMetresPerSecond.isFinite, minimumRemainingPropellantKilograms.isFinite, @@ -78,7 +83,7 @@ public enum FiniteBurnStateTargeting { var matrix = Array(repeating:Array(repeating:0.0,count:7),count:6) let r = residual(best) for j in 0..<6 { - let h = max(0.001,abs(controls[j])*1e-4) + let h = max(0.001,abs(controls[j])*relativeControlStep) var a = controls, b = controls; a[j] += h; b[j] -= h let ea = try evaluate(a), eb = try evaluate(b) guard ea != nil || eb != nil else { throw FiniteTransferPlanningError.invalidInput } diff --git a/Sources/CelMek/HarpoonAnchoring.swift b/Sources/CelMek/HarpoonAnchoring.swift new file mode 100644 index 0000000..0b7d25d --- /dev/null +++ b/Sources/CelMek/HarpoonAnchoring.swift @@ -0,0 +1,105 @@ +// SPDX-License-Identifier: Apache-2.0 +// Copyright 2026 Mattias Holm +import Foundation +import Math + +public enum HarpoonPhase: String, Codable { case stowed, flying, anchored, rejected, pulledOut, misfire } +public enum AnchorCableState: String, Codable { case connected, cut, broken } +/// Explicit engineering assumptions, not an inference of subsurface strength from +/// the visible terrain. Normal kinetic energy is spent against constant resistance. +public struct HarpoonSubstrate { + public let penetrationResistanceNewtons, minimumDepthMetres, maximumDepthMetres, holdingForceNewtons, minimumIncidenceCosine: Double + public init(penetrationResistanceNewtons: Double, minimumDepthMetres: Double, maximumDepthMetres: Double, + holdingForceNewtons: Double, minimumIncidenceCosine: Double = 0.5) throws { + guard [penetrationResistanceNewtons,minimumDepthMetres,maximumDepthMetres,holdingForceNewtons].allSatisfy({$0.isFinite && $0 > 0}), + maximumDepthMetres >= minimumDepthMetres, minimumIncidenceCosine.isFinite, (0...1).contains(minimumIncidenceCosine) else { throw LandingMechanicsError.invalidParameter } + self.penetrationResistanceNewtons=penetrationResistanceNewtons; self.minimumDepthMetres=minimumDepthMetres + self.maximumDepthMetres=maximumDepthMetres; self.holdingForceNewtons=holdingForceNewtons; self.minimumIncidenceCosine=minimumIncidenceCosine + } +} +/// Terrain anchor state is in the declared rotating surface frame. Its tether +/// couples to an independent inertial body; it never reparents or grounds it. +public struct HarpoonAnchor { + public let id:String + public let surfaceFrame:Frame + public private(set) var phase:HarpoonPhase = .stowed + public private(set) var cableState:AnchorCableState = .connected + public private(set) var surfacePointMetres:SIMD3? + public private(set) var penetrationMetres=0.0 + public private(set) var holdingForceNewtons=0.0 + public init(id:String,surfaceFrame:Frame) throws { + guard !id.isEmpty else { throw LandingMechanicsError.invalidParameter } + self.id=id; self.surfaceFrame=surfaceFrame + } + public mutating func fire(succeeded:Bool = true) throws { + guard phase == .stowed else { throw LandingMechanicsError.invalidParameter } + phase=succeeded ? .flying : .misfire + } + /// Returns true only for a captured, incoming impact within the supported + /// burial layer. Rejected impacts remain dynamic and can bounce away. + @discardableResult public mutating func impact(pointMetres:SIMD3,in frame:Frame, + massKilograms:Double,normalSpeedMetresPerSecond:Double,incidenceCosine:Double,substrate:HarpoonSubstrate) throws -> Bool { + guard phase == .flying,frame === surfaceFrame,attitudeFinite(pointMetres),massKilograms.isFinite,massKilograms > 0, + normalSpeedMetresPerSecond.isFinite,normalSpeedMetresPerSecond > 0,incidenceCosine.isFinite,(0...1).contains(incidenceCosine) else { throw LandingMechanicsError.invalidParameter } + let depth=0.5*massKilograms*normalSpeedMetresPerSecond*normalSpeedMetresPerSecond/substrate.penetrationResistanceNewtons + guard depth.isFinite else { throw LandingMechanicsError.numericalRange } + penetrationMetres=depth + if incidenceCosine < substrate.minimumIncidenceCosine || depth < substrate.minimumDepthMetres || depth > substrate.maximumDepthMetres { + phase = .rejected; return false + } + surfacePointMetres=pointMetres; holdingForceNewtons=substrate.holdingForceNewtons;phase = .anchored;return true + } + /// Commit only at accepted integration boundaries. Break precedence is + /// explicit when both thresholds are crossed. No change to vehicle velocity. + public mutating func commitLoad(tensionNewtons:Double,breakingTensionNewtons:Double) throws { + guard tensionNewtons.isFinite,tensionNewtons >= 0,breakingTensionNewtons.isFinite,breakingTensionNewtons > 0 else { throw LandingMechanicsError.invalidParameter } + guard cableState == .connected else { return } + if tensionNewtons > breakingTensionNewtons { cableState = .broken } + else if phase == .anchored && tensionNewtons > holdingForceNewtons { phase = .pulledOut } + } + public mutating func cut() { cableState = .cut } +} + +/// A prescribed terrain endpoint uses the same tension law as two-body cables. +/// The equal and opposite force is a reaction on the prescribed celestial body, +/// not a force to apply a second time to the lander or an integrated dummy mass. +public struct TerrainTether { + public private(set) var cable:MechanicalTether + public init(id:String,body:String,mountMetres:SIMD3,lengthMetres:Double, + stiffnessNewtonsPerMetre:Double,dampingNewtonSecondsPerMetre:Double,breakingTensionNewtons:Double) throws { + cable=try MechanicalTether(id:id,firstBody:body,secondBody:"terrain:\(id)",firstMountMetres:mountMetres, + lengthMetres:lengthMetres,stiffnessNewtonsPerMetre:stiffnessNewtonsPerMetre, + dampingNewtonSecondsPerMetre:dampingNewtonSecondsPerMetre,breakingTensionNewtons:breakingTensionNewtons) + } + public mutating func setLength(metres:Double) throws { try cable.setLength(metres:metres) } + public func load(body:MechanicalBody,pointReferenceMetres:SIMD3,velocityReferenceMetresPerSecond:SIMD3,payoutMetresPerSecond:Double=0) throws -> TetherLoad { + let endpoint=try MechanicalBody(id:cable.secondBody,massKilograms:1,inertia:RigidBodyInertia(xx:1,yy:1,zz:1),positionMetres:pointReferenceMetres,velocityMetresPerSecond:velocityReferenceMetresPerSecond) + return try cable.load(first:body,second:endpoint,payoutMetresPerSecond:payoutMetresPerSecond) + } +} + +public struct MechanicalLaunch { + public let retained,projectile:MechanicalBody + public let retainedCentreBodyMetres:SIMD3 +} +extension MechanicalBody { + /// Separate a small rigid component, then apply an internal firing impulse + /// at the component centre. Mass, linear and angular momentum are conserved; + /// launch energy is supplied by the firing mechanism. Inertia is explicitly + /// removed and translated to the remaining body's new centre of mass. + public func launching(id projectileID:String,massKilograms m:Double,inertia projectileInertia:RigidBodyInertia, + centreBodyMetres r:SIMD3,impulseBodyNewtonSeconds j:SIMD3) throws -> MechanicalLaunch { + guard m.isFinite,m > 0,m < massKilograms,projectileID != id,attitudeFinite(r),attitudeFinite(j) else { throw LandingMechanicsError.invalidParameter } + let remaining=massKilograms-m,s = -r*(m/remaining) + func parallel(_ v:SIMD3,_ mass:Double,_ i:Int,_ k:Int)->Double { mass*((i == k ? (v*v).sum() : 0)-v[i]*v[k]) } + func entry(_ i:Int,_ k:Int)->Double { inertia.matrix[i,k]-projectileInertia.matrix[i,k]-parallel(r,m,i,k)-parallel(s,remaining,i,k) } + let retainedInertia=try RigidBodyInertia(xx:entry(0,0),yy:entry(1,1),zz:entry(2,2),xy:entry(0,1),xz:entry(0,2),yz:entry(1,2)) + let impulse=bodyToReference.matrix*j + let retained=try MechanicalBody(id:id,massKilograms:remaining,inertia:retainedInertia,positionMetres:point(s), + velocityMetresPerSecond:pointVelocity(s)-impulse/remaining,bodyToReference:bodyToReference, + angularVelocityBody:angularVelocityBody+retainedInertia.solve(attitudeCross(r-s,-j))) + let projectile=try MechanicalBody(id:projectileID,massKilograms:m,inertia:projectileInertia,positionMetres:point(r), + velocityMetresPerSecond:pointVelocity(r)+impulse/m,bodyToReference:bodyToReference,angularVelocityBody:angularVelocityBody) + return MechanicalLaunch(retained:retained,projectile:projectile,retainedCentreBodyMetres:s) + } +} diff --git a/Sources/CelMek/LandingMechanics.swift b/Sources/CelMek/LandingMechanics.swift new file mode 100644 index 0000000..3c3a40b --- /dev/null +++ b/Sources/CelMek/LandingMechanics.swift @@ -0,0 +1,264 @@ +// SPDX-License-Identifier: Apache-2.0 +// Copyright 2026 Mattias Holm +import Foundation +import Math + +public enum LandingMechanicsError: Error { case invalidParameter, invalidTopology, numericalRange } + +/// Centre-of-mass state, in the ensemble's shared inertial reference frame. +/// Connections are a graph; they do not change the frame tree or combine inertia. +public struct MechanicalBody { + public let id: String + public let massKilograms: Double + public let inertia: RigidBodyInertia + public private(set) var positionMetres: SIMD3 + public private(set) var velocityMetresPerSecond: SIMD3 + public private(set) var bodyToReference: Quaternion + public private(set) var angularVelocityBody: SIMD3 + public init(id: String, massKilograms: Double, inertia: RigidBodyInertia, + positionMetres: SIMD3, velocityMetresPerSecond: SIMD3 = .zero, + bodyToReference: Quaternion = .identity, angularVelocityBody: SIMD3 = .zero) throws { + guard !id.isEmpty, massKilograms.isFinite, massKilograms > 0, + attitudeFinite(positionMetres), attitudeFinite(velocityMetresPerSecond), attitudeFinite(angularVelocityBody) else { + throw LandingMechanicsError.invalidParameter + } + self.id=id; self.massKilograms=massKilograms; self.inertia=inertia + self.positionMetres=positionMetres; self.velocityMetresPerSecond=velocityMetresPerSecond + self.bodyToReference=try mechanicalUnit(bodyToReference); self.angularVelocityBody=angularVelocityBody + } + public func point(_ local: SIMD3) -> SIMD3 { positionMetres + bodyToReference.matrix*local } + public func pointVelocity(_ local: SIMD3) -> SIMD3 { + velocityMetresPerSecond + bodyToReference.matrix*attitudeCross(angularVelocityBody,local) + } +} + +public struct MechanicalWrench { + public var forceReferenceNewtons: SIMD3 = .zero + public var torqueBodyNewtonMetres: SIMD3 = .zero + public init(forceReferenceNewtons: SIMD3 = .zero, torqueBodyNewtonMetres: SIMD3 = .zero) { + self.forceReferenceNewtons=forceReferenceNewtons; self.torqueBodyNewtonMetres=torqueBodyNewtonMetres + } + public mutating func apply(force: SIMD3, at local: SIMD3, to body: MechanicalBody) { + forceReferenceNewtons += force + torqueBodyNewtonMetres += attitudeCross(local,body.bodyToReference.invMatrix*force) + } +} + +/// Tension-only Kelvin–Voigt cable. Payout is positive, retrieval negative. +/// The caller advances commanded length and commits break/cut events between steps. +public struct MechanicalTether { + public let id: String + public let firstBody: String + public let secondBody: String + public let firstMountMetres: SIMD3 + public let secondMountMetres: SIMD3 + public let stiffnessNewtonsPerMetre: Double + public let dampingNewtonSecondsPerMetre: Double + public let breakingTensionNewtons: Double + public private(set) var lengthMetres: Double + public private(set) var connected: Bool = true + public init(id: String, firstBody: String, secondBody: String, + firstMountMetres: SIMD3 = .zero, secondMountMetres: SIMD3 = .zero, + lengthMetres: Double, stiffnessNewtonsPerMetre: Double, dampingNewtonSecondsPerMetre: Double, + breakingTensionNewtons: Double = .greatestFiniteMagnitude) throws { + guard !id.isEmpty, !firstBody.isEmpty, !secondBody.isEmpty, firstBody != secondBody, + attitudeFinite(firstMountMetres), attitudeFinite(secondMountMetres), + [lengthMetres,stiffnessNewtonsPerMetre,breakingTensionNewtons].allSatisfy({$0.isFinite && $0 > 0}), + dampingNewtonSecondsPerMetre.isFinite, dampingNewtonSecondsPerMetre >= 0 else { throw LandingMechanicsError.invalidParameter } + self.id=id; self.firstBody=firstBody; self.secondBody=secondBody + self.firstMountMetres=firstMountMetres; self.secondMountMetres=secondMountMetres + self.lengthMetres=lengthMetres; self.stiffnessNewtonsPerMetre=stiffnessNewtonsPerMetre + self.dampingNewtonSecondsPerMetre=dampingNewtonSecondsPerMetre; self.breakingTensionNewtons=breakingTensionNewtons + } + public mutating func cut() { connected=false } + public mutating func setLength(metres: Double) throws { + guard metres.isFinite, metres > 0 else { throw LandingMechanicsError.invalidParameter } + lengthMetres=metres + } + public func load(first: MechanicalBody, second: MechanicalBody, payoutMetresPerSecond: Double = 0) throws -> TetherLoad { + guard first.id == firstBody, second.id == secondBody, payoutMetresPerSecond.isFinite else { throw LandingMechanicsError.invalidTopology } + let displacement=second.point(secondMountMetres)-first.point(firstMountMetres) + let distance=sqrt((displacement*displacement).sum()) + guard distance.isFinite else { throw LandingMechanicsError.numericalRange } + guard connected, distance > lengthMetres else { return TetherLoad(forceOnFirst: .zero, tensionNewtons:0, exceedsBreakingLoad:false) } + let direction=displacement/distance + let speed=((second.pointVelocity(secondMountMetres)-first.pointVelocity(firstMountMetres))*direction).sum()-payoutMetresPerSecond + let tension=max(0,stiffnessNewtonsPerMetre*(distance-lengthMetres)+dampingNewtonSecondsPerMetre*speed) + guard tension.isFinite else { throw LandingMechanicsError.numericalRange } + return TetherLoad(forceOnFirst:direction*tension,tensionNewtons:tension,exceedsBreakingLoad:tension > breakingTensionNewtons) + } +} +public struct TetherLoad { + public let forceOnFirst: SIMD3 + public let tensionNewtons: Double + public let exceedsBreakingLoad: Bool +} + +/// Adiabatic ideal-gas cushion, with atmospheric pressure subtracted. Inflation +/// and venting are explicit gas-inventory changes (pressure-volume^gamma units). +/// Effective area and uncompressed volume are engineering inputs, not a soft mesh. +public struct GasCushion { + public let uncompressedVolumeCubicMetres: Double + public let minimumVolumeFraction: Double + public let areaSquareMetres: Double + public let heatCapacityRatio: Double + public private(set) var gasInventory: Double + public init(volumeCubicMetres: Double, areaSquareMetres: Double, absolutePressurePascals: Double, + heatCapacityRatio: Double = 1.4, minimumVolumeFraction: Double = 0.15) throws { + guard [volumeCubicMetres,areaSquareMetres,absolutePressurePascals,heatCapacityRatio].allSatisfy({$0.isFinite && $0 > 0}), + heatCapacityRatio > 1, minimumVolumeFraction.isFinite, minimumVolumeFraction > 0, minimumVolumeFraction < 1 else { + throw LandingMechanicsError.invalidParameter + } + uncompressedVolumeCubicMetres=volumeCubicMetres; self.areaSquareMetres=areaSquareMetres + self.heatCapacityRatio=heatCapacityRatio; self.minimumVolumeFraction=minimumVolumeFraction + gasInventory=absolutePressurePascals*pow(volumeCubicMetres,heatCapacityRatio) + guard gasInventory.isFinite else { throw LandingMechanicsError.numericalRange } + } + /// Fraction of current gas retained. Venting never adds gas or stored energy. + public mutating func vent(retaining fraction: Double) throws { + guard fraction.isFinite, (0...1).contains(fraction) else { throw LandingMechanicsError.invalidParameter } + gasInventory *= fraction + } + public func forceNewtons(compressionMetres: Double, ambientPressurePascals: Double) throws -> Double { + guard compressionMetres.isFinite, compressionMetres >= 0, ambientPressurePascals.isFinite, ambientPressurePascals >= 0 else { + throw LandingMechanicsError.invalidParameter + } + let volume=max(uncompressedVolumeCubicMetres*minimumVolumeFraction,uncompressedVolumeCubicMetres-areaSquareMetres*compressionMetres) + let force=max(0,gasInventory/pow(volume,heatCapacityRatio)-ambientPressurePascals)*areaSquareMetres + guard force.isFinite else { throw LandingMechanicsError.numericalRange } + return force + } +} + +/// Local terrain plane and its instantaneous surface velocity. The terrain +/// provider must refresh these in the same reference frame and epoch as bodies. +public struct MechanicalGround { + public let pointMetres: SIMD3 + public let normal: SIMD3 + public let velocityMetresPerSecond: SIMD3 + public init(pointMetres: SIMD3, normal: SIMD3, velocityMetresPerSecond: SIMD3 = .zero) throws { + guard attitudeFinite(pointMetres), attitudeFinite(velocityMetresPerSecond), attitudeFinite(normal), + (normal*normal).sum().isFinite, (normal*normal).sum() > 1e-24 else { throw LandingMechanicsError.invalidParameter } + self.pointMetres=pointMetres; self.normal=normal/sqrt((normal*normal).sum()); self.velocityMetresPerSecond=velocityMetresPerSecond + } +} + +/// Spherical contact lobe/foot at a body-local mount. Spring plus compression- +/// only damping; regularized Coulomb friction cannot add tangential energy. +/// A gas cushion adds pressure support; the spring remains the bottom-out guard. +public struct MechanicalContact { + public let body: String + public let mountMetres: SIMD3 + public let radiusMetres: Double + public let stiffnessNewtonsPerMetre: Double + public let dampingNewtonSecondsPerMetre: Double + public let frictionCoefficient: Double + public let frictionTransitionMetresPerSecond: Double + public let rollingResistanceMetres: Double + public var cushion: GasCushion? + public init(body: String, mountMetres: SIMD3, radiusMetres: Double, + stiffnessNewtonsPerMetre: Double, dampingNewtonSecondsPerMetre: Double, + frictionCoefficient: Double = 0.6, frictionTransitionMetresPerSecond: Double = 0.05, rollingResistanceMetres: Double = 0, cushion: GasCushion? = nil) throws { + guard !body.isEmpty, attitudeFinite(mountMetres), + [radiusMetres,stiffnessNewtonsPerMetre,dampingNewtonSecondsPerMetre,frictionCoefficient,rollingResistanceMetres].allSatisfy({$0.isFinite && $0 >= 0}), + frictionTransitionMetresPerSecond.isFinite, frictionTransitionMetresPerSecond > 0 else { throw LandingMechanicsError.invalidParameter } + self.body=body; self.mountMetres=mountMetres; self.radiusMetres=radiusMetres + self.stiffnessNewtonsPerMetre=stiffnessNewtonsPerMetre; self.dampingNewtonSecondsPerMetre=dampingNewtonSecondsPerMetre + self.frictionCoefficient=frictionCoefficient; self.frictionTransitionMetresPerSecond=frictionTransitionMetresPerSecond; self.cushion=cushion; self.rollingResistanceMetres=rollingResistanceMetres + } + public func load(on state: MechanicalBody, ground: MechanicalGround, ambientPressurePascals: Double = 0) throws -> MechanicalWrench { + guard state.id == body else { throw LandingMechanicsError.invalidTopology } + let penetration=radiusMetres-((state.point(mountMetres)-ground.pointMetres)*ground.normal).sum() + guard penetration > 0 else { return MechanicalWrench() } + let local=mountMetres-state.bodyToReference.invMatrix*(ground.normal*radiusMetres) + let velocity=state.pointVelocity(local)-ground.velocityMetresPerSecond + let normalSpeed=(velocity*ground.normal).sum() + let normalForce=stiffnessNewtonsPerMetre*penetration+dampingNewtonSecondsPerMetre*max(0,-normalSpeed) + + (try cushion?.forceNewtons(compressionMetres:penetration,ambientPressurePascals:ambientPressurePascals) ?? 0)*min(1,penetration/max(0.001,radiusMetres*0.5)) + let tangent=velocity-ground.normal*normalSpeed, speed=sqrt((tangent*tangent).sum()) + let force=ground.normal*normalForce-tangent*(frictionCoefficient*normalForce/max(frictionTransitionMetresPerSecond,speed)) + guard attitudeFinite(force) else { throw LandingMechanicsError.numericalRange } + var result=MechanicalWrench(); result.apply(force:force,at:local,to:state) + let nBody=state.bodyToReference.invMatrix*ground.normal + let roll=state.angularVelocityBody-nBody*(state.angularVelocityBody*nBody).sum() + result.torqueBodyNewtonMetres -= roll*(normalForce*rollingResistanceMetres/max(0.1,sqrt((roll*roll).sum()))) + return result + } +} + +/// Shared inertial frame and epoch, independent 6-DOF bodies, arbitrary cable graph. +/// Explicit midpoint integration with bounded user-selected substeps. This is a +/// compliant model: select timestep by stiffness/mass and verify convergence. +/// External loads must include gravity/controls and be pure at each stage. +public struct MechanicalSystem { + public let frame: Frame + public private(set) var epoch: Epoch + public private(set) var bodies: [MechanicalBody] + public private(set) var tethers: [MechanicalTether] + public init(frame: Frame, epoch: Epoch, bodies: [MechanicalBody], tethers: [MechanicalTether] = []) throws { + let ids=Set(bodies.map(\.id)) + guard frame.pseudoInertial, !bodies.isEmpty, ids.count == bodies.count, + Set(tethers.map(\.id)).count == tethers.count, + tethers.allSatisfy({ids.contains($0.firstBody) && ids.contains($0.secondBody)}) else { throw LandingMechanicsError.invalidTopology } + self.frame=frame; self.epoch=epoch; self.bodies=bodies; self.tethers=tethers + } + public mutating func cut(tether id: String) throws { + guard let i=tethers.firstIndex(where:{$0.id == id}) else { throw LandingMechanicsError.invalidTopology } + tethers[i].cut() + } + public func cableLoads(payout: [String:Double] = [:]) throws -> [TetherLoad] { + try tethers.map { t in try t.load(first:bodies.first{$0.id == t.firstBody}!,second:bodies.first{$0.id == t.secondBody}!,payoutMetresPerSecond:payout[t.id] ?? 0) } + } + /// Break events are committed at accepted step boundaries, never during load + /// evaluation. Returns IDs of newly broken cables. Entire step is transactional. + @discardableResult public mutating func step(seconds: Double, payout: [String:Double] = [:], + externalLoads: (Epoch,[MechanicalBody]) throws -> [MechanicalWrench]) throws -> [String] { + guard seconds.isFinite, seconds > 0, seconds <= 0.01, + payout.allSatisfy({ key,value in value.isFinite && tethers.contains{$0.id == key} }) else { throw LandingMechanicsError.invalidParameter } + var next=self + var broken:[String]=[] + func breakOverloaded() throws { + let loads=try next.cableLoads(payout:payout) + for i in loads.indices where next.tethers[i].connected && loads[i].exceedsBreakingLoad { + broken.append(next.tethers[i].id); next.tethers[i].cut() + } + } + try breakOverloaded() + let start=next.bodies + func rates(_ states:[MechanicalBody], at time:Epoch, cableFraction:Double) throws -> [MechanicalWrench] { + var loads=try externalLoads(time,states) + guard loads.count == states.count else { throw LandingMechanicsError.invalidTopology } + for t in next.tethers { + let i=states.firstIndex{$0.id == t.firstBody}!, j=states.firstIndex{$0.id == t.secondBody}! + var cable=t; try cable.setLength(metres:t.lengthMetres+(payout[t.id] ?? 0)*seconds*cableFraction) + let f=try cable.load(first:states[i],second:states[j],payoutMetresPerSecond:payout[t.id] ?? 0).forceOnFirst + loads[i].apply(force:f,at:t.firstMountMetres,to:states[i]); loads[j].apply(force:-f,at:t.secondMountMetres,to:states[j]) + } + return loads + } + func advanced(_ base:MechanicalBody, using sample:MechanicalBody, load:MechanicalWrench, dt:Double) throws -> MechanicalBody { + let momentum=try sample.inertia.angularMomentumBody(for:sample.angularVelocityBody) + let angularAcceleration=try sample.inertia.solve(load.torqueBodyNewtonMetres-attitudeCross(sample.angularVelocityBody,momentum)) + let qdot=0.5*(sample.bodyToReference*Quaternion(x:sample.angularVelocityBody.x,y:sample.angularVelocityBody.y,z:sample.angularVelocityBody.z,w:0)) + return try MechanicalBody(id:base.id,massKilograms:base.massKilograms,inertia:base.inertia, + positionMetres:base.positionMetres+sample.velocityMetresPerSecond*dt, + velocityMetresPerSecond:base.velocityMetresPerSecond+load.forceReferenceNewtons*(dt/base.massKilograms), + bodyToReference:base.bodyToReference+dt*qdot,angularVelocityBody:base.angularVelocityBody+angularAcceleration*dt) + } + let first=try rates(start,at:epoch,cableFraction:0) + let half=try start.indices.map{try advanced(start[$0],using:start[$0],load:first[$0],dt:seconds/2)} + let middle=try rates(half,at:epoch.advanced(by:Duration(seconds:seconds/2)),cableFraction:0.5) + next.bodies=try start.indices.map{try advanced(start[$0],using:half[$0],load:middle[$0],dt:seconds)} + for i in next.tethers.indices { + try next.tethers[i].setLength(metres:next.tethers[i].lengthMetres+(payout[next.tethers[i].id] ?? 0)*seconds) + } + next.epoch=try epoch.advanced(by:Duration(seconds:seconds)) + try breakOverloaded() + self=next; return broken + } +} +private func mechanicalUnit(_ q:Quaternion) throws -> Quaternion { + let norm=sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w) + guard norm.isFinite, norm > 1e-12 else { throw LandingMechanicsError.invalidParameter } + return Quaternion(x:q.x/norm,y:q.y/norm,z:q.z/norm,w:q.w/norm) +} diff --git a/Tests/CelMekTests/FiniteBurnControlStepTests.swift b/Tests/CelMekTests/FiniteBurnControlStepTests.swift new file mode 100644 index 0000000..b920e39 --- /dev/null +++ b/Tests/CelMekTests/FiniteBurnControlStepTests.swift @@ -0,0 +1,26 @@ +import Testing +import CelMek +import Math + +@Test func finiteBurnStateTargetingAcceptsFineControlStepsAndRejectsInvalidOnes() throws { + let frame=CelestialFrames.icrf,epoch=Epoch.j2000 + let initial=try SpacecraftState(cartesian:CartesianState(epoch:epoch,frame:frame,position:SIMD3(1e8,0,0),velocity:.zero), + mass:SpacecraftMassProperties(dryMassKilograms:1000,propellantMassKilograms:1000)) + let context=try ForceEvaluationContext(frame:frame,origin:.solarSystemBarycentre) + func burn(_ start: Double,_ duration: Double,_ direction: SIMD3) throws -> FiniteBurn { + try FiniteBurn(start:epoch.advanced(by:CelMek.Duration(seconds:start)),end:epoch.advanced(by:CelMek.Duration(seconds:start+duration)), + thrust:FiniteThrust(frame:frame,thrustNewtons:100,effectiveExhaustVelocityMetresPerSecond:10000,direction:direction)) + } + let reference=try ScheduledThrust(frame:frame,burns:[burn(10,100,SIMD3(1,0,0)),burn(700,100,SIMD3(0,1,0))]) + let arrival=try epoch.advanced(by:CelMek.Duration(seconds:1000)) + let target=try ScheduledThrustPropagator(initialState:initial,schedule:reference,context:context).state(at:arrival) + let seed=try ScheduledThrust(frame:frame,burns:[burn(10,105,SIMD3(1,0,0)),burn(700,95,SIMD3(0,1,0))]) + let result=try FiniteBurnStateTargeting.refine(initialState:initial,initialSchedule:seed,target:target,context:context,backgroundModels:[], + positionToleranceMetres:0.01,velocityToleranceMetresPerSecond:1e-5,relativeControlStep:1e-6) + #expect(result.positionErrorMetres<0.01 && result.velocityErrorMetresPerSecond<1e-5) + for value in [0.0,-0.1,Double.nan,Double.infinity,0.1] { + #expect(throws:FiniteTransferPlanningError.invalidInput) { + try FiniteBurnStateTargeting.refine(initialState:initial,initialSchedule:seed,target:target,context:context,backgroundModels:[],relativeControlStep:value) + } + } +} diff --git a/Tests/CelMekTests/HarpoonTests.swift b/Tests/CelMekTests/HarpoonTests.swift new file mode 100644 index 0000000..7daf09e --- /dev/null +++ b/Tests/CelMekTests/HarpoonTests.swift @@ -0,0 +1,42 @@ +import Foundation +import Testing +import CelMek +import Math + +private func cometNorm(_ v:SIMD3)->Double {sqrt((v*v).sum())} +private func cometCross(_ a:SIMD3,_ b:SIMD3)->SIMD3 {SIMD3(a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x)} + +@Test func harpoonInternalLaunchConservesMassAndMomentumIncludingRecoilTorque() throws { + let parent=try MechanicalBody(id:"probe",massKilograms:100,inertia:RigidBodyInertia(xx:90,yy:90,zz:80),positionMetres:SIMD3(1000,20,30), + velocityMetresPerSecond:SIMD3(1,2,3),angularVelocityBody:SIMD3(0.1,0.2,0.3)) + let launch=try parent.launching(id:"dart",massKilograms:0.05,inertia:RigidBodyInertia(xx:0.0001,yy:0.0001,zz:0.00005),centreBodyMetres:SIMD3(0.3,0,-0.2),impulseBodyNewtonSeconds:SIMD3(0,0,-0.75)) + #expect(abs(launch.retained.massKilograms+launch.projectile.massKilograms-parent.massKilograms)<1e-12) + let delta=launch.retained.velocityMetresPerSecond*launch.retained.massKilograms+launch.projectile.velocityMetresPerSecond*launch.projectile.massKilograms-parent.velocityMetresPerSecond*parent.massKilograms + #expect(cometNorm(delta)<1e-10) + var momentum=SIMD3.zero + for b in [launch.retained,launch.projectile] { + momentum += cometCross(b.positionMetres-parent.positionMetres,b.velocityMetresPerSecond*b.massKilograms)+b.bodyToReference.matrix*(try b.inertia.angularMomentumBody(for:b.angularVelocityBody)) + } + #expect(cometNorm(momentum-(try parent.inertia.angularMomentumBody(for:parent.angularVelocityBody)))<1e-9) + #expect(launch.retained.velocityMetresPerSecond.z>parent.pointVelocity(launch.retainedCentreBodyMetres).z) + #expect(launch.retained.angularVelocityBody.y != parent.angularVelocityBody.y) +} +@Test func harpoonAttachmentDepthFramePulloutCutAndCableBreak() throws { + let fixed=FrameTree(name:"prescribed surface").root + let substrate=try HarpoonSubstrate(penetrationResistanceNewtons:25,minimumDepthMetres:0.04,maximumDepthMetres:0.8,holdingForceNewtons:20) + var anchor=try HarpoonAnchor(id:"a",surfaceFrame:fixed) + try anchor.fire() + #expect(throws:LandingMechanicsError.self) {try anchor.impact(pointMetres:.zero,in:CelestialFrames.icrf,massKilograms:0.05,normalSpeedMetresPerSecond:15,incidenceCosine:1,substrate:substrate)} + #expect(anchor.phase == .flying) + #expect(try anchor.impact(pointMetres:SIMD3(1,2,3),in:fixed,massKilograms:0.05,normalSpeedMetresPerSecond:15,incidenceCosine:1,substrate:substrate)) + #expect(abs(anchor.penetrationMetres-0.225)<1e-12) + var pullout=anchor,broken=anchor + try pullout.commitLoad(tensionNewtons:21,breakingTensionNewtons:30);#expect(pullout.phase == .pulledOut) + try broken.commitLoad(tensionNewtons:31,breakingTensionNewtons:30);#expect(broken.cableState == .broken);#expect(broken.phase == .anchored) + anchor.cut();try anchor.commitLoad(tensionNewtons:100,breakingTensionNewtons:30) + #expect(anchor.phase == .anchored);#expect(anchor.cableState == .cut) + var rejected=try HarpoonAnchor(id:"r",surfaceFrame:fixed);try rejected.fire() + #expect(try !rejected.impact(pointMetres:.zero,in:fixed,massKilograms:0.05,normalSpeedMetresPerSecond:0.1,incidenceCosine:1,substrate:substrate)) + #expect(rejected.phase == .rejected) + #expect(throws:LandingMechanicsError.self) {try HarpoonSubstrate(penetrationResistanceNewtons:0,minimumDepthMetres:1,maximumDepthMetres:2,holdingForceNewtons:1)} +} diff --git a/Tests/CelMekTests/LandingConservationTests.swift b/Tests/CelMekTests/LandingConservationTests.swift new file mode 100644 index 0000000..0819e3b --- /dev/null +++ b/Tests/CelMekTests/LandingConservationTests.swift @@ -0,0 +1,29 @@ +import Foundation +import Testing +import CelMek +import Math + +private func lcCross(_ a:SIMD3,_ b:SIMD3) -> SIMD3 { SIMD3(a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x) } +@Test func landingOffCentreBridleConservesAngularMomentumAndConverges() throws { + func run(_ dt:Double,_ offset:SIMD3) throws -> (MechanicalSystem,SIMD3) { + let tensor=try RigidBodyInertia(xx:10,yy:10,zz:10) + let a=try MechanicalBody(id:"a",massKilograms:10,inertia:tensor,positionMetres:offset) + let b=try MechanicalBody(id:"b",massKilograms:20,inertia:tensor,positionMetres:offset+SIMD3(0,0,3)) + let cable=try MechanicalTether(id:"c",firstBody:"a",secondBody:"b",firstMountMetres:SIMD3(1,0,0),secondMountMetres:SIMD3(-1,0,0),lengthMetres:3,stiffnessNewtonsPerMetre:100,dampingNewtonSecondsPerMetre:5) + var s=try MechanicalSystem(frame:CelestialFrames.icrf,epoch:.j2000,bodies:[a,b],tethers:[cable]) + for _ in 0...zero + for body in s.bodies { + angular += lcCross(body.positionMetres-offset,body.velocityMetresPerSecond*body.massKilograms) + + body.bodyToReference.matrix*(try body.inertia.angularMomentumBody(for:body.angularVelocityBody)) + } + return (s,angular) + } + let coarse=try run(0.002,.zero),fine=try run(0.001,.zero),shift=try run(0.001,SIMD3(1e8,-1e8,1e8)) + #expect(sqrt((fine.1*fine.1).sum())<0.001) + #expect(abs(fine.0.bodies[0].angularVelocityBody.y)>0.01) + let delta=coarse.0.bodies[0].positionMetres-fine.0.bodies[0].positionMetres + #expect(sqrt((delta*delta).sum())<0.001) + let shifted=shift.0.bodies[0].positionMetres-SIMD3(1e8,-1e8,1e8)-fine.0.bodies[0].positionMetres + #expect(sqrt((shifted*shifted).sum())<0.001) +} diff --git a/Tests/CelMekTests/LandingMechanicsTests.swift b/Tests/CelMekTests/LandingMechanicsTests.swift new file mode 100644 index 0000000..21fdd2c --- /dev/null +++ b/Tests/CelMekTests/LandingMechanicsTests.swift @@ -0,0 +1,76 @@ +import Foundation +import Testing +import CelMek +import Math + +private func mechanicalBody(_ id:String,_ p:SIMD3,_ v:SIMD3 = .zero) throws -> MechanicalBody { + try MechanicalBody(id:id,massKilograms:10,inertia:RigidBodyInertia(xx:4,yy:4,zz:4),positionMetres:p,velocityMetresPerSecond:v) +} +@Test func landingTetherSlackPayoutAndBreakAreExplicit() throws { + let a=try mechanicalBody("a",.zero), b=try mechanicalBody("b",SIMD3(3,0,0),SIMD3(1,0,0)) + var cable=try MechanicalTether(id:"c",firstBody:"a",secondBody:"b",lengthMetres:2,stiffnessNewtonsPerMetre:100,dampingNewtonSecondsPerMetre:20,breakingTensionNewtons:110) + #expect(try cable.load(first:a,second:b).tensionNewtons == 120) + #expect(try cable.load(first:a,second:b,payoutMetresPerSecond:1).tensionNewtons == 100) + var system=try MechanicalSystem(frame:CelestialFrames.icrf,epoch:.j2000,bodies:[a,b],tethers:[cable]) + #expect(try system.step(seconds:0.001){_,states in states.map{_ in MechanicalWrench()} } == ["c"]) + #expect(system.tethers[0].connected == false) + #expect(system.bodies[0].velocityMetresPerSecond == .zero) + try cable.setLength(metres:4) + #expect(try cable.load(first:a,second:b).tensionNewtons == 0) + cable.cut();try cable.setLength(metres:1) + #expect(try cable.load(first:a,second:b).tensionNewtons == 0) +} +@Test func landingCableGraphConservesLinearMomentumAndCutHasNoImpulse() throws { + let a=try mechanicalBody("a",.zero), b=try mechanicalBody("b",SIMD3(0,0,3)) + let cables=try [-1.0,0,1].map{ x in try MechanicalTether(id:"c\(x)",firstBody:"a",secondBody:"b",firstMountMetres:SIMD3(x,0,0),secondMountMetres:SIMD3(x,0,0),lengthMetres:2,stiffnessNewtonsPerMetre:100,dampingNewtonSecondsPerMetre:10) } + var system=try MechanicalSystem(frame:CelestialFrames.icrf,epoch:.j2000,bodies:[a,b],tethers:cables) + for _ in 0..<1000 { try system.step(seconds:0.001){_,states in states.map{_ in MechanicalWrench()} } } + let momentum=(system.bodies[0].velocityMetresPerSecond+system.bodies[1].velocityMetresPerSecond)*10 + #expect((momentum*momentum).sum()<1e-20) + let before=system.bodies + try system.cut(tether:cables[0].id) + #expect(system.bodies[0].positionMetres == before[0].positionMetres) + #expect(system.bodies[1].velocityMetresPerSecond == before[1].velocityMetresPerSecond) + let epoch=system.epoch + #expect(throws:LandingMechanicsError.self) { try system.step(seconds:0.001,payout:["missing":1]){_,_ in []} } + #expect(system.epoch == epoch) +} +@Test func landingContactSupportsBouncesAndDissipatesEnergy() throws { + let ground=try MechanicalGround(pointMetres:.zero,normal:SIMD3(0,0,1)) + let contact=try MechanicalContact(body:"lander",mountMetres:.zero,radiusMetres:1,stiffnessNewtonsPerMetre:10000,dampingNewtonSecondsPerMetre:150,frictionCoefficient:0.7) + var system=try MechanicalSystem(frame:CelestialFrames.icrf,epoch:.j2000,bodies:[mechanicalBody("lander",SIMD3(0,0,4),SIMD3(2,0,0))]) + var rebounded=false,previousVelocity=0.0,lowest=4.0 + for _ in 0..<20000 { + try system.step(seconds:0.001){_,states in + var load=try contact.load(on:states[0],ground:ground) + load.forceReferenceNewtons += SIMD3(0,0,-37.1) + return [load] + } + let b=system.bodies[0];lowest=min(lowest,b.positionMetres.z) + if previousVelocity < 0 && b.velocityMetresPerSecond.z > 0 { rebounded=true } + previousVelocity=b.velocityMetresPerSecond.z + } + let b=system.bodies[0] + #expect(rebounded);#expect(lowest > 0.8) + #expect(abs(b.positionMetres.z-(1-37.1/10000))<0.001) + #expect(abs(b.velocityMetresPerSecond.z)<0.01) + #expect(abs(b.velocityMetresPerSecond.x)<2) + // A single sphere rolls without slip; friction alone must not halt rolling. + let local=b.bodyToReference.invMatrix*SIMD3(0,0,-1) + #expect(abs(b.pointVelocity(local).x)<0.01) + let airborne=try mechanicalBody("lander",SIMD3(0,0,2),SIMD3(0,0,-10)) + #expect(try contact.load(on:airborne,ground:ground).forceReferenceNewtons == .zero) +} +@Test func landingGasCompressionVentingAndMountTorque() throws { + var bag=try GasCushion(volumeCubicMetres:1,areaSquareMetres:1,absolutePressurePascals:1000) + let initial=try bag.forceNewtons(compressionMetres:0,ambientPressurePascals:600) + let compressed=try bag.forceNewtons(compressionMetres:0.5,ambientPressurePascals:600) + #expect(compressed > initial) + try bag.vent(retaining:0) + #expect(try bag.forceNewtons(compressionMetres:0.5,ambientPressurePascals:600) == 0) + #expect(throws:LandingMechanicsError.self) { try bag.vent(retaining:1.1) } + let foot=try MechanicalContact(body:"b",mountMetres:SIMD3(1,0,0),radiusMetres:1,stiffnessNewtonsPerMetre:100,dampingNewtonSecondsPerMetre:0) + let load=try foot.load(on:mechanicalBody("b",SIMD3(0,0,0.5)),ground:MechanicalGround(pointMetres:.zero,normal:SIMD3(0,0,1))) + #expect(load.forceReferenceNewtons.z == 50) + #expect(load.torqueBodyNewtonMetres.y == -50) +}