diff --git a/.github/workflows/swift.yml b/.github/workflows/swift.yml index fc01ce5..2067edf 100644 --- a/.github/workflows/swift.yml +++ b/.github/workflows/swift.yml @@ -20,7 +20,7 @@ jobs: - os: macos-latest build-system: swiftbuild # Use the previous backend while investigating opaque Linux/Windows failures. - - os: ubuntu-24.04 + - os: ubuntu-26.04 build-system: native - os: windows-latest build-system: native diff --git a/Documentation/MassGeometry.md b/Documentation/MassGeometry.md new file mode 100644 index 0000000..275ccf4 --- /dev/null +++ b/Documentation/MassGeometry.md @@ -0,0 +1,25 @@ +# Rigid mass components + +`MassComponent` is a value tree, independent of scene nodes and ephemeris frames. +Each component defines its own mass, centre and intrinsic inertia, a rigid pose +relative to its parent, and child components. Structural nodes may have no mass. +Identifiers are unique throughout an assembly. Intrinsic inertia excludes children. +Cylinder helpers use the local X axis as the cylinder axis. + +`massGeometry()` rotates each intrinsic tensor and applies the parallel-axis +theorem about the assembled centre. The result is expressed in the root parent's +axes and origin, including the root's own placement. + +`detaching(id:)` returns retained and detached value trees atomically. The detached +root carries the composed parent transforms, so both results remain in the same +structural coordinate system. Recompute both geometries, then call +`offsetKinematics(from:angularVelocity:)` for each result. Rotate these offsets +into the simulation reference frame and add the original assembly position and +velocity. Both bodies initially inherit the original orientation and angular +velocity; their subsequent dynamics use their respective inertia tensors. + +Separation springs and motors require explicit impulses with matching recoil. +This API does not integrate flexible structures, slosh, articulated joints, +propellant momentum transport, or time-dependent internal motion. Fuel depletion +can rebuild the value tree at a step boundary; instantaneous reference-point +kinematics alone is not a complete variable-mass dynamics model. diff --git a/README.md b/README.md index 413da72..8d6cf21 100644 --- a/README.md +++ b/README.md @@ -107,3 +107,19 @@ optional incompressible rotor/body CFD and a compressible Sod reference problem. Liquid properties and vehicle coefficients in these examples are illustrative. See the FluidDynamics and Hydrodynamics DocC guides for coverage, numerical cost, pressure-reference rules and the remaining real-gas/free-surface fidelity work. + + +Single finite-burn position targeting is available through +`FiniteBurnPositionTargeting.refine`. It keeps ignition, thrust and exhaust speed +fixed, correcting duration and inertial direction through numerical endpoint +sensitivities and bounded backtracking. The result reports position convergence, +velocity residual, propellant, evaluation count and rejection count. Velocity is +**not** a targeted constraint; callers must separately verify it along with +clearance, attitude and reserve. Every trial uses `ScheduledThrustPropagator` and +caller-supplied force models; no gravity or physical origin is assumed implicitly. + +`FiniteBurnStateTargeting.refine` corrects two fixed-ignition finite burns against +both terminal position and velocity using six duration-vector controls, central +differences, pivoted elimination and bounded backtracking/evaluation count. +It returns the best evaluated schedule, including on nonconvergence: callers must +check both residuals and independently validate clearance, resources and operations. diff --git a/Sources/Atmospheres/EarthEntryAtmosphere.swift b/Sources/Atmospheres/EarthEntryAtmosphere.swift new file mode 100644 index 0000000..2ea93cb --- /dev/null +++ b/Sources/Atmospheres/EarthEntryAtmosphere.swift @@ -0,0 +1,40 @@ +// SPDX-License-Identifier: Apache-2.0 +// Copyright 2026 Mattias Holm +import Foundation + +/// ISA lower layers joined continuously to a sparse US Standard Atmosphere 1976 +/// upper table. Log-linear pressure/density, linear kinetic temperature. Fixed +/// nominal air, not weather/solar activity, chemistry or continuum certification. +public struct EarthEntryAtmosphere: AtmosphericProfile { + public let altitudeRangeMetres = -500.0...300000.0 + public let altitudeDatum = "Geometric metres above the simulation mean sea level" + public let source = "https://www.ngdc.noaa.gov/stp/space-weather/online-publications/miscellaneous/us-standard-atmosphere-1976/us-standard-atmosphere_st76-1562_noaa.pdf" + public let assumptions = "ISA to its explicit upper limit; USSA1976 Table I pp.68–70 at 90–300 km. Sparse log interpolation; match ISA at join. Upper sound speed uses gamma=1.4 and p/rho; Sutherland viscosity is a nominal transport approximation, not resolved composition." + public init() {} + public func properties(at altitudeMetres: Double) throws -> AtmosphericProperties { + guard altitudeMetres.isFinite, altitudeRangeMetres.contains(altitudeMetres) else { throw AtmosphericModelError.altitudeOutsideCoverage } + let isa = ISAAtmosphere(), join = isa.altitudeRangeMetres.upperBound + if altitudeMetres <= join { return try isa.properties(at:altitudeMetres) } + let base = try isa.properties(at:join) + // Geometric km, kinetic K, pressure Pa (publication mb ×100), rho kg/m³. + let rows: [(Double,Double,Double,Double)] = [ + (join,base.temperatureKelvin,base.pressurePascals,base.densityKilogramsPerCubicMetre), + (90000,186.87,0.18359,3.416e-6), (95000,188.42,0.075966,1.393e-6), + (100000,195.08,0.032011,5.604e-7), (105000,208.84,0.014477,2.325e-7), + (110000,240,0.0071042,9.708e-8), (115000,300,0.0040075,4.061e-8), + (120000,360,0.0025382,2.222e-8), (125000,417.23,0.0017340,1.291e-8), + (130000,469.27,0.0012505,8.152e-9), + (140000,559.63,0.00072028,3.831e-9), (150000,634.39,0.00045422,2.076e-9), + (160000,696.29,0.00030395,1.233e-9), (170000,747.57,0.00021210,7.815e-10), + (180000,790.07,0.00015271,5.194e-10), (190000,825.31,0.00011266,3.581e-10), + (200000,854.56,0.000084736,2.541e-10), (220000,899.01,0.000050149,1.367e-10), + (230000,915.78,0.000039276,1.029e-10), (240000,929.73,0.000031059,7.858e-11), + (250000,941.33,0.000024767,6.073e-11), (300000,976.00,0.0000087704,1.916e-11)] + let i = min(rows.count-2,rows.lastIndex { $0.0 <= altitudeMetres }!) + let a = rows[i], b = rows[i+1], u = (altitudeMetres-a.0)/(b.0-a.0) + let t = a.1+(b.1-a.1)*u, p = exp(log(a.2)+(log(b.2)-log(a.2))*u) + let rho = exp(log(a.3)+(log(b.3)-log(a.3))*u) + return AtmosphericProperties(pressurePascals:p,temperatureKelvin:t,densityKilogramsPerCubicMetre:rho, + speedOfSoundMetresPerSecond:sqrt(1.4*p/rho),dynamicViscosityPascalSeconds:1.458e-6*pow(t,1.5)/(t+110.4)) + } +} diff --git a/Sources/CelMek/FiniteBurnPositionTargeting.swift b/Sources/CelMek/FiniteBurnPositionTargeting.swift new file mode 100644 index 0000000..8bcb5e2 --- /dev/null +++ b/Sources/CelMek/FiniteBurnPositionTargeting.swift @@ -0,0 +1,100 @@ +// SPDX-License-Identifier: Apache-2.0 +import Foundation +import Math + +public enum FiniteBurnPositionTargetingStatus: Equatable { + case positionConverged, evaluationLimitReached, stepLimitReached, singularSensitivity +} +public struct FiniteBurnPositionTargetingResult { + public let status: FiniteBurnPositionTargetingStatus + public let evaluations, rejectedTrials: Int + public let initial, best: FiniteTransferEvaluation +} + +/// Differential correction for one fixed-ignition, constant inertial burn. +/// Adjusts duration and direction using a numerically differentiated position +/// sensitivity and bounded backtracking. Velocity is diagnostic, NOT targeted. +/// Callers must independently verify clearance, attitude, resources and velocity. +public enum FiniteBurnPositionTargeting { + public static func refine(initialState: SpacecraftState, initialBurn: FiniteBurn, + target: CartesianState, context: ForceEvaluationContext, backgroundModels: [any ForceModel] = [], + positionToleranceMetres: Double = 1, minimumRemainingPropellantKilograms: Double = 0, + maximumEvaluations: Int = 120, burnWindow: ClosedRange? = nil, + numericalOptions: NumericalPropagationOptions = .preset(for:.high)) throws -> FiniteBurnPositionTargetingResult { + guard target.epoch > initialState.epoch, initialBurn.start >= initialState.epoch, + initialBurn.end <= target.epoch, initialBurn.thrust.thrustNewtons > 0, + positionToleranceMetres.isFinite, positionToleranceMetres > 0, + minimumRemainingPropellantKilograms.isFinite, minimumRemainingPropellantKilograms >= 0, + (1...10_000).contains(maximumEvaluations) else { throw FiniteTransferPlanningError.invalidInput } + guard target.frame === initialState.frame, initialBurn.thrust.frame === initialState.frame, + context.frame === initialState.frame else { throw SpacecraftDynamicsError.frameMismatch } + let thrust = initialBurn.thrust + var evaluations = 0, rejected = 0 + func norm(_ v: SIMD3) -> Double { hypot(hypot(v.x,v.y),v.z) } + func cross(_ 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) + } + func evaluate(_ control: SIMD3) throws -> FiniteTransferEvaluation? { + let seconds = norm(control) + guard seconds.isFinite, seconds > 1e-8 else { rejected += 1; return nil } + let end = try initialBurn.start.advanced(by:Duration(seconds:seconds)) + let used = seconds*thrust.propellantConsumptionKilogramsPerSecond + guard end <= target.epoch, initialState.mass.propellantMassKilograms-used > 0, + initialState.mass.propellantMassKilograms-used >= minimumRemainingPropellantKilograms, + burnWindow.map({ $0.contains(initialBurn.start) && $0.contains(end) }) ?? true else { rejected += 1; return nil } + let burn = try FiniteBurn(start:initialBurn.start,end:end,thrust:FiniteThrust(frame:thrust.frame, + thrustNewtons:thrust.thrustNewtons,effectiveExhaustVelocityMetresPerSecond:thrust.effectiveExhaustVelocityMetresPerSecond,direction:control)) + let schedule = try ScheduledThrust(frame:initialState.frame,burns:[burn]) + evaluations += 1 + do { + let state = try ScheduledThrustPropagator(initialState:initialState,schedule:schedule, + backgroundModels:backgroundModels,context:context,options:numericalOptions).spacecraftState(at:target.epoch) + return FiniteTransferEvaluation(schedule:schedule,arrivalState:state, + positionErrorMetres:norm(state.cartesian.position-target.position), + velocityErrorMetresPerSecond:norm(state.cartesian.velocity-target.velocity), + propellantUsedKilograms:initialState.mass.propellantMassKilograms-state.mass.propellantMassKilograms) + } catch is NumericalPropagationError { rejected += 1; return nil } + } + var control = thrust.direction * (try initialBurn.end.duration(since:initialBurn.start).seconds) + guard let initial = try evaluate(control) else { throw FiniteTransferPlanningError.invalidInput } + var best = initial, status = FiniteBurnPositionTargetingStatus.evaluationLimitReached + while best.positionErrorMetres > positionToleranceMetres { + guard evaluations+7 <= maximumEvaluations else { break } + let step = max(1e-4,norm(control)*1e-4) + var columns: [SIMD3] = [] + for axis in 0..<3 { + var plus = control, minus = control + plus[axis] += step; minus[axis] -= step + let a = try evaluate(plus), b = try evaluate(minus) + if let a, let b { columns.append((a.arrivalState.cartesian.position-b.arrivalState.cartesian.position)/(2*step)) } + else if let a { columns.append((a.arrivalState.cartesian.position-best.arrivalState.cartesian.position)/step) } + else if let b { columns.append((best.arrivalState.cartesian.position-b.arrivalState.cartesian.position)/step) } + else { break } + } + guard columns.count == 3 else { status = .stepLimitReached; break } + let lengths = columns.map(norm) + guard lengths.allSatisfy({$0.isFinite && $0 > 1e-15}) else { status = .singularSensitivity; break } + let a = columns[0]/lengths[0], b = columns[1]/lengths[1], c = columns[2]/lengths[2] + let determinant = (a*cross(b,c)).sum() + guard determinant.isFinite, abs(determinant) > 1e-10 else { status = .singularSensitivity; break } + let residual = target.position-best.arrivalState.cartesian.position + var correction = SIMD3((residual*cross(b,c)).sum()/lengths[0], + (a*cross(residual,c)).sum()/lengths[1],(a*cross(b,residual)).sum()/lengths[2])/determinant + let length = norm(correction) + guard length.isFinite else { status = .singularSensitivity; break } + correction *= min(1,max(0.01,norm(control)*0.2)/max(length,1e-30)) + var accepted = false, fraction = 1.0 + for _ in 0..<10 where evaluations < maximumEvaluations { + let candidate = control+correction*fraction + if let trial = try evaluate(candidate), trial.positionErrorMetres < best.positionErrorMetres { + control = candidate; best = trial; accepted = true; break + } + fraction *= 0.5 + } + guard accepted else { status = evaluations >= maximumEvaluations ? .evaluationLimitReached : .stepLimitReached; break } + } + if best.positionErrorMetres <= positionToleranceMetres { status = .positionConverged } + return FiniteBurnPositionTargetingResult(status:status,evaluations:evaluations,rejectedTrials:rejected,initial:initial,best:best) + } +} + diff --git a/Sources/CelMek/FiniteBurnStateTargeting.swift b/Sources/CelMek/FiniteBurnStateTargeting.swift new file mode 100644 index 0000000..f6deaac --- /dev/null +++ b/Sources/CelMek/FiniteBurnStateTargeting.swift @@ -0,0 +1,121 @@ +// SPDX-License-Identifier: Apache-2.0 +import Foundation +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. +public struct FiniteBurnStateTargetingReport { + public let best: FiniteTransferEvaluation + public let evaluations: Int +} + +public enum FiniteBurnStateTargeting { + public static func refine(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 -> 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 + } + 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), + initialSchedule.frame === initialState.frame, + positionToleranceMetres.isFinite, velocityToleranceMetresPerSecond.isFinite, + minimumRemainingPropellantKilograms.isFinite, + positionToleranceMetres > 0, velocityToleranceMetresPerSecond > 0, + minimumRemainingPropellantKilograms >= 0, + target.frame === initialState.frame, context.frame === initialState.frame, + target.epoch > initialState.epoch else { throw FiniteTransferPlanningError.invalidInput } + let seeds = initialSchedule.burns + var controls = try seeds.flatMap { b -> [Double] in + let v = b.thrust.direction * (try b.end.duration(since:b.start).seconds) + return [v.x,v.y,v.z] + } + var count = 0 + func norm(_ v: SIMD3) -> Double { hypot(hypot(v.x,v.y),v.z) } + func residual(_ e: FiniteTransferEvaluation) -> [Double] { + let p = (e.arrivalState.cartesian.position-target.position)/positionToleranceMetres + let v = (e.arrivalState.cartesian.velocity-target.velocity)/velocityToleranceMetresPerSecond + return [p.x,p.y,p.z,v.x,v.y,v.z] + } + func score(_ e: FiniteTransferEvaluation) -> Double { residual(e).reduce(0) { $0+$1*$1 } } + func evaluate(_ x: [Double]) throws -> FiniteTransferEvaluation? { + guard count < maximumEvaluations else { return nil } + var burns = [FiniteBurn](), fuel = 0.0 + for i in 0..<2 { + let seed = seeds[i], vector = SIMD3(x[i*3],x[i*3+1],x[i*3+2]), duration = norm(vector) + guard duration.isFinite, duration > 0.001 else { return nil } + let end = try seed.start.advanced(by:Duration(seconds:duration)) + guard seed.start >= initialState.epoch, end <= (i == 0 ? seeds[1].start : target.epoch) else { return nil } + let ve = try exhaustVelocityForDirection?(vector/duration) ?? seed.thrust.effectiveExhaustVelocityMetresPerSecond + let thrust = try FiniteThrust(frame:initialState.frame,thrustNewtons:seed.thrust.thrustNewtons, + effectiveExhaustVelocityMetresPerSecond:ve,direction:vector) + fuel += duration*thrust.propellantConsumptionKilogramsPerSecond + burns.append(try FiniteBurn(start:seed.start,end:end,thrust:thrust)) + } + guard fuel < initialState.mass.propellantMassKilograms, + initialState.mass.propellantMassKilograms-fuel >= minimumRemainingPropellantKilograms else { return nil } + count += 1 + let schedule = try ScheduledThrust(frame:initialState.frame,burns:burns) + let state = try ScheduledThrustPropagator(initialState:initialState,schedule:schedule,backgroundModels:backgroundModels, + context:context,options:numericalOptions).spacecraftState(at:target.epoch) + return FiniteTransferEvaluation(schedule:schedule,arrivalState:state,positionErrorMetres:norm(state.cartesian.position-target.position), + velocityErrorMetresPerSecond:norm(state.cartesian.velocity-target.velocity),propellantUsedKilograms:fuel) + } + guard var best = try evaluate(controls) else { throw FiniteTransferPlanningError.invalidInput } + while best.positionErrorMetres > positionToleranceMetres || best.velocityErrorMetresPerSecond > velocityToleranceMetresPerSecond { + guard count+13 < maximumEvaluations else { break } + 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) + 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 } + if let ea, let eb { + let ra = residual(ea), rb = residual(eb) + for i in 0..<6 { matrix[i][j] = (ra[i]-rb[i])/(2*h) } + } else if let ea { + let ra = residual(ea) + for i in 0..<6 { matrix[i][j] = (ra[i]-r[i])/h } + } else if let eb { + let rb = residual(eb) + for i in 0..<6 { matrix[i][j] = (r[i]-rb[i])/h } + } + } + for i in 0..<6 { matrix[i][6] = -r[i] } + for j in 0..<6 { + let pivot = (j..<6).max { abs(matrix[$0][j]) < abs(matrix[$1][j]) }! + guard abs(matrix[pivot][j]) > 1e-12 else { throw FiniteTransferPlanningError.invalidInput } + matrix.swapAt(j,pivot) + let scale = matrix[j][j] + for k in j...6 { matrix[j][k] /= scale } + for i in 0..<6 where i != j { + let factor = matrix[i][j] + for k in j...6 { matrix[i][k] -= factor*matrix[j][k] } + } + } + let correction = matrix.map { $0[6] } + var fraction = 1.0, accepted = false + for _ in 0..<12 { + let x = zip(controls,correction).map { $0+$1*fraction } + if let trial = try evaluate(x), score(trial) < score(best) { + controls = x; best = trial; accepted = true; break + } + fraction *= 0.5 + } + if !accepted { break } + } + return FiniteBurnStateTargetingReport(best:best,evaluations:count) + } +} diff --git a/Sources/CelMek/MassComponent.swift b/Sources/CelMek/MassComponent.swift new file mode 100644 index 0000000..e03f2cc --- /dev/null +++ b/Sources/CelMek/MassComponent.swift @@ -0,0 +1,123 @@ +// SPDX-License-Identifier: Apache-2.0 +// Copyright 2026 Mattias Holm +import Foundation +import Math + +public enum MassGeometryError: Error { case invalidComponent, duplicateIdentifier, missingComponent, cannotDetachRoot, emptyAssembly } + +/// Rigid, instantaneous mass hierarchy. Translation and rotation map each local +/// structural frame into its parent's frame. Mass and intrinsic inertia belong +/// to the node itself; children are additional mass, never included twice. +/// Empty structural nodes have zero mass and nil inertia. No slosh is implied. +public struct MassComponent { + public let id: String + public let massKilograms: Double + public let inertiaAtCentre: RigidBodyInertia? + public let centreLocalMetres: SIMD3 + public let translationParentMetres: SIMD3 + public let localToParent: Quaternion + public private(set) var children: [MassComponent] + + public init(id: String, massKilograms: Double = 0, inertiaAtCentre: RigidBodyInertia? = nil, + centreLocalMetres: SIMD3 = .zero, translationParentMetres: SIMD3 = .zero, + localToParent: Quaternion = Quaternion(x:0,y:0,z:0,w:1), children: [MassComponent] = []) throws { + let q=[localToParent.x,localToParent.y,localToParent.z,localToParent.w] + guard !id.isEmpty, massKilograms.isFinite, massKilograms >= 0, + (massKilograms == 0) == (inertiaAtCentre == nil), + (0..<3).allSatisfy({ centreLocalMetres[$0].isFinite && translationParentMetres[$0].isFinite }), + q.allSatisfy(\.isFinite), abs(q.reduce(0) { $0+$1*$1 }-1)<1e-10 else { throw MassGeometryError.invalidComponent } + self.id=id; self.massKilograms=massKilograms; self.inertiaAtCentre=inertiaAtCentre + self.centreLocalMetres=centreLocalMetres; self.translationParentMetres=translationParentMetres + self.localToParent=localToParent; self.children=children + var ids=Set() + func validate(_ node: MassComponent) throws { + guard ids.insert(node.id).inserted else { throw MassGeometryError.duplicateIdentifier } + for child in node.children { try validate(child) } + } + try validate(self) + } + + public static func cylinderInertia(mass: Double, length: Double, radius: Double) throws -> RigidBodyInertia { + guard [mass,length,radius].allSatisfy({ $0.isFinite && $0>0 }) else { throw MassGeometryError.invalidComponent } + return try RigidBodyInertia(xx:mass*radius*radius/2, + yy:mass*(3*radius*radius+length*length)/12,zz:mass*(3*radius*radius+length*length)/12) + } + + /// Nonmutating partition. Both results retain the original parent's axes and + /// origin, so their centres can be mapped directly into the same rigid state. + /// Failed lookup leaves the original tree untouched. Separation impulses are + /// a dynamics concern and must be applied as equal/opposite impulses there. + public func detaching(id target: String) throws -> (retained: MassComponent, detached: MassComponent) { + guard target != id else { throw MassGeometryError.cannotDetachRoot } + func remove(_ node: MassComponent) throws -> (MassComponent, MassComponent?) { + var kept=node + for i in node.children.indices { + let child=node.children[i] + if child.id==target { + kept.children.remove(at:i) + return (kept,child) + } + let (replacement,found)=try remove(child) + if let found { + kept.children[i]=replacement + // Carry the detached subtree through this intervening frame. + let transformed=try MassComponent(id:found.id,massKilograms:found.massKilograms, + inertiaAtCentre:found.inertiaAtCentre,centreLocalMetres:found.centreLocalMetres, + translationParentMetres:child.translationParentMetres+child.localToParent.matrix*found.translationParentMetres, + localToParent:child.localToParent*found.localToParent,children:found.children) + return (kept,transformed) + } + } + return (kept,nil) + } + let (retained,found)=try remove(self) + guard let found else { throw MassGeometryError.missingComponent } + // Root's placement also applies to the detached result. + let detached=try MassComponent(id:found.id,massKilograms:found.massKilograms, + inertiaAtCentre:found.inertiaAtCentre,centreLocalMetres:found.centreLocalMetres, + translationParentMetres:translationParentMetres+localToParent.matrix*found.translationParentMetres, + localToParent:localToParent*found.localToParent,children:found.children) + return (retained,detached) + } + + public func massGeometry() throws -> CompositeMassGeometry { + var elements:[(Double,SIMD3,Quaternion,RigidBodyInertia)]=[] + func visit(_ node: MassComponent,_ origin: SIMD3,_ rotation: Quaternion) { + let p=origin+rotation.matrix*node.translationParentMetres, q=rotation*node.localToParent + if let inertia=node.inertiaAtCentre { elements.append((node.massKilograms,p+q.matrix*node.centreLocalMetres,q,inertia)) } + for child in node.children { visit(child,p,q) } + } + visit(self,.zero,Quaternion(x:0,y:0,z:0,w:1)) + let mass=elements.reduce(0) { $0+$1.0 } + guard mass>0, mass.isFinite else { throw MassGeometryError.emptyAssembly } + let centre=elements.reduce(SIMD3.zero) { $0+$1.0*$1.1 }/mass + let axes=[SIMD3(1,0,0),SIMD3(0,1,0),SIMD3(0,0,1)] + var tensor=Array(repeating:0.0,count:9) + for (m,p,q,inertia) in elements { + let r=p-centre, r2=(r*r).sum() + for i in 0..<3 { for j in 0..<3 { + let a=q.conj.matrix*axes[i], b=q.conj.matrix*axes[j] + let rotated=(a*(inertia.matrix*b)).sum() + let diagonal: Double = i==j ? r2 : 0 + let shifted=m*(diagonal-r[i]*r[j]) + tensor[i*3+j] += rotated+shifted + } } + } + return CompositeMassGeometry(massKilograms:mass,centreMetres:centre, + inertia:try RigidBodyInertia(xx:tensor[0],yy:tensor[4],zz:tensor[8],xy:tensor[1],xz:tensor[2],yz:tensor[5])) + } +} + +public struct CompositeMassGeometry { + public let massKilograms: Double + public let centreMetres: SIMD3 + public let inertia: RigidBodyInertia + + /// Instantaneous rigid point kinematics in the assembly's axes. Includes the + /// rotational velocity at this COM, without inventing a separation impulse. + public func offsetKinematics(from assembly: CompositeMassGeometry, angularVelocity: SIMD3) -> (position: SIMD3, velocity: SIMD3) { + let r=centreMetres-assembly.centreMetres + return (r,SIMD3(angularVelocity.y*r.z-angularVelocity.z*r.y, + angularVelocity.z*r.x-angularVelocity.x*r.z,angularVelocity.x*r.y-angularVelocity.y*r.x)) + } +} diff --git a/Sources/CelMek/PlaneCrossingEvent.swift b/Sources/CelMek/PlaneCrossingEvent.swift new file mode 100644 index 0000000..7ffc256 --- /dev/null +++ b/Sources/CelMek/PlaneCrossingEvent.swift @@ -0,0 +1,25 @@ +// SPDX-License-Identifier: Apache-2.0 +// Copyright 2026 Mattias Holm +import Foundation +import Math + +/// Signed distance to an origin-centred fixed plane in an explicit inertial frame. +/// Use EventDetectedPropagation for bounded window searches over moving sites. +public struct PlaneCrossingEvent: PropagationEventDetector { + public let identifier: String + public let frame: Frame + public let normal: SIMD3 + public let direction: EventCrossingDirection + public let action: PropagationEventAction + public init(identifier: String, frame: Frame, normal: SIMD3, direction: EventCrossingDirection = .any, + action: PropagationEventAction = .record) throws { + let length = hypot(hypot(normal.x,normal.y),normal.z) + guard !identifier.isEmpty, length.isFinite, length > 0 else { throw PropagationEventError.invalidDetector } + self.identifier = identifier; self.frame = frame; self.normal = normal/length + self.direction = direction; self.action = action + } + public func value(at state: SpacecraftState) throws -> Double { + guard state.frame === frame else { throw SpacecraftDynamicsError.frameMismatch } + return (state.cartesian.position*normal).sum() + } +} diff --git a/Tests/CelMekTests/FiniteBurnPositionTargetingTests.swift b/Tests/CelMekTests/FiniteBurnPositionTargetingTests.swift new file mode 100644 index 0000000..20a83fc --- /dev/null +++ b/Tests/CelMekTests/FiniteBurnPositionTargetingTests.swift @@ -0,0 +1,70 @@ +// SPDX-License-Identifier: Apache-2.0 +import Testing +import Math +@testable import CelMek + +@Suite struct FiniteBurnPositionTargetingTests { + let frame = Frame(name:"Position corrector test",pseudoInertial:true) + func epoch(_ seconds: Double) throws -> Epoch { try Epoch.j2000.advanced(by:CelMek.Duration(seconds:seconds)) } + func state() throws -> SpacecraftState { + try SpacecraftState(cartesian:CartesianState(epoch:.j2000,frame:frame,position:SIMD3(10,0,0),velocity:SIMD3(0,1,0)), + mass:SpacecraftMassProperties(dryMassKilograms:50,propellantMassKilograms:50)) + } + func burn(_ seconds: Double, _ direction: SIMD3) throws -> FiniteBurn { + try FiniteBurn(start:epoch(1),end:epoch(1+seconds),thrust:FiniteThrust(frame:frame,thrustNewtons:10, + effectiveExhaustVelocityMetresPerSecond:100,direction:direction)) + } + @Test func correctsThreeDimensionalPositionWithFixedIgnitionAndFiniteFuel() throws { + let initial = try state(), context = try ForceEvaluationContext(frame:frame,origin:.earth) + let exact = try ScheduledThrust(frame:frame,burns:[burn(2,SIMD3(1,0.3,0.2))]) + let target = try ScheduledThrustPropagator(initialState:initial,schedule:exact,context:context).state(at:epoch(10)) + let result = try FiniteBurnPositionTargeting.refine(initialState:initial,initialBurn:burn(1.6,SIMD3(1,0.1,0.3)), + target:target,context:context,positionToleranceMetres:1e-7,maximumEvaluations:100,burnWindow:epoch(1)...epoch(6)) + #expect(result.status == .positionConverged) + #expect(result.best.positionErrorMetres < 1e-7) + #expect(result.best.positionErrorMetres < result.initial.positionErrorMetres) + #expect(result.best.schedule.burns[0].start == (try epoch(1))) + #expect(result.evaluations <= 100) + #expect(abs(result.best.propellantUsedKilograms-0.2) < 1e-5) + } + @Test func respectsEvaluationReserveAndTimingLimits() throws { + let initial = try state(), context = try ForceEvaluationContext(frame:frame,origin:.earth) + let target = try CartesianState(epoch:epoch(10),frame:frame,position:SIMD3(12,10,0),velocity:.zero) + let limited = try FiniteBurnPositionTargeting.refine(initialState:initial,initialBurn:burn(1,SIMD3(1,0,0)),target:target, + context:context,positionToleranceMetres:1e-12,maximumEvaluations:1) + #expect(limited.status == .evaluationLimitReached && limited.evaluations == 1) + #expect(throws:FiniteTransferPlanningError.invalidInput) { + try FiniteBurnPositionTargeting.refine(initialState:initial,initialBurn:burn(1,SIMD3(1,0,0)),target:target, + context:context,minimumRemainingPropellantKilograms:50) + } + #expect(throws:FiniteTransferPlanningError.invalidInput) { + try FiniteBurnPositionTargeting.refine(initialState:initial,initialBurn:burn(1,SIMD3(1,0,0)),target:target, + context:context,burnWindow:epoch(3)...epoch(5)) + } + } +} + +@Suite struct FiniteBurnStateTargetingTests { + @Test func correctsTwoBurnsAgainstPositionAndVelocityAndRejectsInvalidInputs() throws { + let frame = Frame(name:"State target",pseudoInertial:true) + func epoch(_ s: Double) throws -> Epoch { try Epoch.j2000.advanced(by:CelMek.Duration(seconds:s)) } + let initial = try SpacecraftState(cartesian:CartesianState(epoch:.j2000,frame:frame,position:.zero,velocity:SIMD3(0,1,0)),mass:SpacecraftMassProperties(dryMassKilograms:50,propellantMassKilograms:50)) + let context = try ForceEvaluationContext(frame:frame,origin:.earth) + func schedule(_ duration: Double, _ bias: Double) throws -> ScheduledThrust { + try ScheduledThrust(frame:frame,burns:[1.0,8.0].map { start in + try FiniteBurn(start:epoch(start),end:epoch(start+duration),thrust:FiniteThrust(frame:frame,thrustNewtons:10,effectiveExhaustVelocityMetresPerSecond:100,direction:SIMD3(1,bias,start/10))) + }) + } + let exact = try schedule(2,0.3), seed = try schedule(1.8,0.2) + let target = try ScheduledThrustPropagator(initialState:initial,schedule:exact,context:context).state(at:epoch(15)) + let result = try FiniteBurnStateTargeting.refine(initialState:initial,initialSchedule:seed,target:target,context:context,backgroundModels:[],positionToleranceMetres:1e-6,velocityToleranceMetresPerSecond:1e-7) + #expect(result.positionErrorMetres < 1e-6 && result.velocityErrorMetresPerSecond < 1e-7) + #expect(abs(result.propellantUsedKilograms-0.4) < 1e-5) + #expect(throws:FiniteTransferPlanningError.invalidInput) { + try FiniteBurnStateTargeting.refine(initialState:initial,initialSchedule:seed,target:target,context:context,backgroundModels:[],minimumRemainingPropellantKilograms:50) + } + #expect(throws:FiniteTransferPlanningError.invalidInput) { + try FiniteBurnStateTargeting.refine(initialState:initial,initialSchedule:seed,target:target,context:context,backgroundModels:[],positionToleranceMetres:.nan) + } + } +} diff --git a/Tests/CelMekTests/MassHierarchyTests.swift b/Tests/CelMekTests/MassHierarchyTests.swift new file mode 100644 index 0000000..fb63708 --- /dev/null +++ b/Tests/CelMekTests/MassHierarchyTests.swift @@ -0,0 +1,36 @@ +import Foundation +import Testing +import CelMek +import Math + +@Test func nestedMassHierarchyDetachmentPreservesMassAndMomentum() throws { + let inertia=try RigidBodyInertia(xx:2,yy:2,zz:2) + let rotation=Quaternion(x:0,y:0,z:sin(.pi/4),w:cos(.pi/4)) + let a=try MassComponent(id:"a",massKilograms:10,inertiaAtCentre:inertia) + let b=try MassComponent(id:"b",massKilograms:20,inertiaAtCentre:inertia,translationParentMetres:SIMD3(2,0,0)) + let group=try MassComponent(id:"group",translationParentMetres:SIMD3(0,3,0),localToParent:rotation,children:[b]) + let root=try MassComponent(id:"root",children:[a,group]) + let whole=try root.massGeometry(), split=try root.detaching(id:"b") + let kept=try split.retained.massGeometry(), detached=try split.detached.massGeometry() + #expect(abs(whole.massKilograms-kept.massKilograms-detached.massKilograms)<1e-12) + #expect(abs(detached.centreMetres.y-5)<1e-12) + #expect(abs(whole.centreMetres.y-100/30)<1e-12) + let w=SIMD3(0,0,2) + let k=kept.offsetKinematics(from:whole,angularVelocity:w) + let d=detached.offsetKinematics(from:whole,angularVelocity:w) + let momentum=k.velocity*kept.massKilograms+d.velocity*detached.massKilograms + #expect((momentum*momentum).sum()<1e-18) + #expect(abs((whole.inertia.matrix*SIMD3(1,0,0)).x-(4+10*pow(whole.centreMetres.y,2)+20*pow(5-whole.centreMetres.y,2)))<1e-9) + #expect(throws:MassGeometryError.self) { try root.detaching(id:"missing") } + #expect(throws:MassGeometryError.self) { try MassComponent(id:"duplicate",children:[a,a]) } +} + +@Test func rotatedComponentInertiaAndEmptyStructure() throws { + let q=Quaternion(x:0,y:0,z:sin(.pi/4),w:cos(.pi/4)) + let node=try MassComponent(id:"rotated",massKilograms:1, + inertiaAtCentre:RigidBodyInertia(xx:2,yy:3,zz:4),localToParent:q) + let result=try node.massGeometry() + #expect(abs((result.inertia.matrix*SIMD3(1,0,0)).x-3)<1e-12) + #expect(abs((result.inertia.matrix*SIMD3(0,1,0)).y-2)<1e-12) + #expect(throws:MassGeometryError.self) { try MassComponent(id:"empty").massGeometry() } +}