Merge origin/master into eureka/aetheria-release-mods-in-tree
AetheriaStores.Open keeps both: the registry parameter and the missing-catalog refusal. Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
commit
2be1f8efa2
26 changed files with 1747 additions and 24 deletions
|
|
@ -1,4 +1,5 @@
|
|||
using System;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using GameCult.Caching;
|
||||
using GameCult.Caching.MessagePack;
|
||||
|
|
@ -18,6 +19,9 @@ public static class AetheriaStores
|
|||
public static CultCache Open(string catalogPath, string runPath = null, string playerPath = null, bool catalogWritable = false,
|
||||
CultDocumentRegistry registry = null)
|
||||
{
|
||||
// A read-only open of an absent catalog would hydrate an empty game; a shipped player missing its catalog stops here.
|
||||
if (!catalogWritable && !File.Exists(catalogPath))
|
||||
throw new FileNotFoundException($"Catalog {catalogPath} does not exist; a read-only open never invents one.", catalogPath);
|
||||
var cache = new CultCache(registry);
|
||||
try
|
||||
{
|
||||
|
|
|
|||
|
|
@ -130,8 +130,8 @@ public static class FireControl
|
|||
// arc, so the gun can fire along it. Direction: where a round from this gun travels and where its barrel points --
|
||||
// the subject's intercept when it bears, else the arc-clamped aim. A chunk subject bears through its pose and
|
||||
// flies the aim, as an untargeted round (Subject is null for it). Derived on every read and never stored: turrets
|
||||
// traverse instantly in the simulation. Traverse speed belongs to the link (ruling arc-and-traverse-on-the-link),
|
||||
// which will make this stateful behind the same signature. It never prices; HitProbability is the one price.
|
||||
// traverse instantly in the simulation. Traverse is the weapon's Tracking (ruling mount-arc-gun-traverse), priced in
|
||||
// TrackingRate; this holds no slew state, and a rig's slew is presentation. It never prices; HitProbability is the one price.
|
||||
public static GunSolution Solution(Weapon weapon, Entity shooter, TargetRef subject)
|
||||
{
|
||||
var bears = false;
|
||||
|
|
|
|||
|
|
@ -17,6 +17,22 @@ public sealed class ShipAuthoring
|
|||
[Key(2)] public string ModelAsset;
|
||||
[Key(3)] public List<ShipAnchor> Anchors = new List<ShipAnchor>();
|
||||
[Key(4)] public List<ShipPolyline> SchematicLines = new List<ShipPolyline>();
|
||||
// The moving parts of the rig, presentation only (ruling rig-is-presentation): no joint value feeds the simulation.
|
||||
[Key(5)] public List<ShipJoint> Joints = new List<ShipJoint>();
|
||||
}
|
||||
|
||||
// A rig joint: a bone of the model's armature. Axes are three floats per degree of freedom, a unit vector in Ship Root
|
||||
// space at rest (glTF axes, +Y up); Min and Max are that degree of freedom's limits in degrees.
|
||||
[MessagePackObject]
|
||||
public sealed class ShipJoint
|
||||
{
|
||||
// The bone's aetheria.id, which is also its GLB node id.
|
||||
[Key(0)] public string Id;
|
||||
// The nearest joint above this one in the model, or null for a root joint.
|
||||
[Key(1)] public string Parent;
|
||||
[Key(2)] public float[] Axes;
|
||||
[Key(3)] public float[] Min;
|
||||
[Key(4)] public float[] Max;
|
||||
}
|
||||
|
||||
[MessagePackObject]
|
||||
|
|
@ -25,13 +41,15 @@ public sealed class ShipAnchor
|
|||
// Stable semantic identity. HullData.Hardpoints[].Transform names this ID for mounts.
|
||||
[Key(0)] public string Id;
|
||||
// One of: map-icon, hull-collider, shield, tractor, thruster-emitter, weapon-mount,
|
||||
// weapon-muzzle, radiator-mesh, or articulation (a pivot, never a mount).
|
||||
// weapon-muzzle, or radiator-mesh.
|
||||
[Key(1)] public string Role;
|
||||
// Stable node ID in the compiled model, never a Blender display name.
|
||||
[Key(2)] public string ModelNodeId;
|
||||
// Only child roles use ParentId; e.g. a muzzle names its weapon hardpoint.
|
||||
[Key(3)] public string ParentId;
|
||||
[Key(4)] public int Order;
|
||||
// On a weapon-mount anchor: the id of the nearest joint above its node in the model; null or empty is a fixed mount.
|
||||
[Key(5)] public string Joint;
|
||||
}
|
||||
|
||||
// Evaluated or baked Grease Pencil stroke coordinates in Blender's right-handed Z-up
|
||||
|
|
@ -53,8 +71,7 @@ public static class ShipAuthoringStore
|
|||
// ShipAnchor.Role's vocabulary.
|
||||
private static readonly HashSet<string> Roles = new HashSet<string>(StringComparer.Ordinal)
|
||||
{
|
||||
"map-icon", "hull-collider", "shield", "tractor", "thruster-emitter", "weapon-mount", "weapon-muzzle", "radiator-mesh",
|
||||
"articulation"
|
||||
"map-icon", "hull-collider", "shield", "tractor", "thruster-emitter", "weapon-mount", "weapon-muzzle", "radiator-mesh"
|
||||
};
|
||||
|
||||
// The anchor role a visible hardpoint's mount plays, or null for an internal hardpoint, which has no anchor.
|
||||
|
|
@ -272,6 +289,7 @@ public static class ShipAuthoringStore
|
|||
foreach (var hardpoint in mounts.Values.Where(hardpoint => IsWeapon(hardpoint.Type)))
|
||||
if (!anchors.Any(anchor => anchor.Role == "weapon-muzzle" && anchor.ParentId == hardpoint.Transform))
|
||||
throw new InvalidOperationException($"{ship.Id}: weapon hardpoint {hardpoint.Transform} needs at least one muzzle anchor.");
|
||||
ValidateRig(ship, anchors, mounts, nodeIds);
|
||||
|
||||
foreach (var line in ship.SchematicLines ?? new List<ShipPolyline>())
|
||||
{
|
||||
|
|
@ -291,4 +309,111 @@ public static class ShipAuthoringStore
|
|||
throw new InvalidOperationException($"{ship.Id}: schematic line color must be finite RGBA.");
|
||||
}
|
||||
}
|
||||
|
||||
// A degree of freedom counts as yaw when its axis lies within 5 degrees of Ship Root up (glTF +Y).
|
||||
private static readonly float YawAxisCos = MathF.Cos(5f * MathF.PI / 180f);
|
||||
|
||||
// The joints, which anchors ride them, and whether each driven joint's chain can sweep the arc its guns state. The
|
||||
// joints are presentation, so this judges only that the rig could point the gun where the hardpoint says it may fire.
|
||||
private static void ValidateRig(ShipAuthoring ship, List<ShipAnchor> anchors, Dictionary<string, HardpointData> mounts, HashSet<string> nodeIds)
|
||||
{
|
||||
var joints = ship.Joints ?? new List<ShipJoint>();
|
||||
var byId = new Dictionary<string, ShipJoint>(StringComparer.Ordinal);
|
||||
foreach (var joint in joints)
|
||||
{
|
||||
if (joint == null || string.IsNullOrWhiteSpace(joint.Id) || byId.ContainsKey(joint.Id))
|
||||
throw new InvalidOperationException($"{ship.Id}: joint IDs must be present and unique.");
|
||||
if (nodeIds.Contains(joint.Id))
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} is also an anchor's model node.");
|
||||
byId.Add(joint.Id, joint);
|
||||
}
|
||||
foreach (var joint in joints)
|
||||
{
|
||||
var dofs = joint.Min?.Length ?? 0;
|
||||
if (dofs < 1 || joint.Max?.Length != dofs || joint.Axes?.Length != 3 * dofs)
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} needs one or more degrees of freedom: Axes holds three values each, Min and Max one each.");
|
||||
for (var dof = 0; dof < dofs; dof++)
|
||||
{
|
||||
var (x, y, z) = (joint.Axes[3 * dof], joint.Axes[3 * dof + 1], joint.Axes[3 * dof + 2]);
|
||||
var (min, max) = (joint.Min[dof], joint.Max[dof]);
|
||||
if (!float.IsFinite(x) || !float.IsFinite(y) || !float.IsFinite(z) || !float.IsFinite(min) || !float.IsFinite(max))
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} axes and limits must be finite.");
|
||||
if (MathF.Abs(MathF.Sqrt(x * x + y * y + z * z) - 1f) > 1e-3f)
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} axis {dof} must be a unit vector.");
|
||||
if (min < -180f || max > 180f || min > max)
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} limits {dof} must satisfy -180 <= Min <= Max <= 180.");
|
||||
}
|
||||
if (!string.IsNullOrEmpty(joint.Parent) && !byId.ContainsKey(joint.Parent))
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} has an unknown parent joint.");
|
||||
}
|
||||
foreach (var joint in joints)
|
||||
{
|
||||
var step = joint;
|
||||
for (var depth = 0; !string.IsNullOrEmpty(step.Parent); depth++)
|
||||
{
|
||||
if (depth >= joints.Count)
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} is on a parent cycle.");
|
||||
step = byId[step.Parent];
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var anchor in anchors.Where(anchor => !string.IsNullOrEmpty(anchor.Joint)))
|
||||
{
|
||||
if (anchor.Role == "thruster-emitter" || anchor.Role == "radiator-mesh")
|
||||
throw new InvalidOperationException($"{ship.Id}: {anchor.Role} anchor {anchor.Id} may not ride a joint (follow-up gimballed-thrusters).");
|
||||
if (anchor.Role != "weapon-mount")
|
||||
throw new InvalidOperationException($"{ship.Id}: {anchor.Role} anchor {anchor.Id} may not name a joint; only a weapon mount rides one.");
|
||||
if (!byId.ContainsKey(anchor.Joint))
|
||||
throw new InvalidOperationException($"{ship.Id}: weapon mount {anchor.Id} names an unknown joint.");
|
||||
}
|
||||
|
||||
var ridden = anchors.Where(anchor => anchor.Role == "weapon-mount" && !string.IsNullOrEmpty(anchor.Joint))
|
||||
.Select(anchor => (Hardpoint: mounts[anchor.Id], Joint: anchor.Joint)).ToArray();
|
||||
foreach (var (hardpoint, _) in ridden)
|
||||
if (!(hardpoint.FiringArc > 0f))
|
||||
throw new InvalidOperationException($"{ship.Id}: weapon hardpoint {hardpoint.Transform} rides a joint, so it must state its firing arc above 0.");
|
||||
foreach (var group in ridden.GroupBy(mount => mount.Joint, StringComparer.Ordinal))
|
||||
{
|
||||
var lead = group.First().Hardpoint;
|
||||
foreach (var (hardpoint, _) in group)
|
||||
if (hardpoint.Rotation != lead.Rotation || hardpoint.FiringArc != lead.FiringArc)
|
||||
throw new InvalidOperationException($"{ship.Id}: weapon hardpoints {lead.Transform} and {hardpoint.Transform} ride joint {group.Key} and must share Rotation and FiringArc.");
|
||||
var reach = YawReach(ship, group.Key);
|
||||
if (reach < lead.FiringArc)
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {group.Key} chain reaches {reach} degrees of yaw, short of the {lead.FiringArc} degree firing arc of {lead.Transform}.");
|
||||
}
|
||||
}
|
||||
|
||||
// The yaw a driven joint's chain can sweep: the summed range of every degree of freedom whose axis is yaw, capped at a turn.
|
||||
private static float YawReach(ShipAuthoring ship, string jointId)
|
||||
{
|
||||
var joints = ship.Joints.ToDictionary(joint => joint.Id, StringComparer.Ordinal);
|
||||
var reach = 0f;
|
||||
foreach (var joint in RigChains.Chain(ship, jointId).Select(id => joints[id]))
|
||||
for (var dof = 0; dof < joint.Min.Length; dof++)
|
||||
if (MathF.Abs(joint.Axes[3 * dof + 1]) >= YawAxisCos)
|
||||
reach += joint.Max[dof] - joint.Min[dof];
|
||||
return MathF.Min(reach, 360f);
|
||||
}
|
||||
}
|
||||
|
||||
// A rig's driven joints and the chains that move them. A driven joint is one a weapon-mount anchor names. Its chain runs
|
||||
// from the joint up through its ancestors and stops before the nearest ancestor that is itself driven (that joint has
|
||||
// its own gun, and its pose is that gun's business), listed root-first. The judge and the plan both read this one rule.
|
||||
public static class RigChains
|
||||
{
|
||||
public static HashSet<string> Driven(ShipAuthoring ship) =>
|
||||
new HashSet<string>(ship.Anchors.Where(anchor => anchor.Role == "weapon-mount" && !string.IsNullOrEmpty(anchor.Joint)).Select(anchor => anchor.Joint),
|
||||
StringComparer.Ordinal);
|
||||
|
||||
public static string[] Chain(ShipAuthoring ship, string jointId)
|
||||
{
|
||||
var joints = ship.Joints.ToDictionary(joint => joint.Id, StringComparer.Ordinal);
|
||||
var driven = Driven(ship);
|
||||
var chain = new List<string> { jointId };
|
||||
for (var parent = joints[jointId].Parent; !string.IsNullOrEmpty(parent) && !driven.Contains(parent); parent = joints[parent].Parent)
|
||||
chain.Add(parent);
|
||||
chain.Reverse();
|
||||
return chain.ToArray();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -183,6 +183,7 @@ public static class ShipModCatalog
|
|||
foreach (var anchor in ship.Anchors)
|
||||
if ((anchor.Role == "thruster-emitter" || anchor.Role == "radiator-mesh") && !modelNodes[anchor.ModelNodeId].HasMesh)
|
||||
throw new InvalidOperationException($"{ship.Id}: {anchor.Role} anchor {anchor.Id} needs a mesh, but model node {anchor.ModelNodeId} has none.");
|
||||
BindRig(ship, modelNodes);
|
||||
CultRecordRefs.Validate(hull);
|
||||
return new Package
|
||||
{
|
||||
|
|
@ -191,9 +192,43 @@ public static class ShipModCatalog
|
|||
};
|
||||
}
|
||||
|
||||
// GLB is the xenos asset boundary. Only its node extras, and whether each node names a mesh, are read here; geometry
|
||||
// belongs to the runtime importer.
|
||||
public static Dictionary<string, (uint Index, bool HasMesh)> ReadNodeIds(string path)
|
||||
// The rig the record states must be the rig the model has: every joint is a node, each joint's Parent and each mount's
|
||||
// Joint is the nearest joint above its node, ancestors without an id being walked through by ReadNodeIds, and a thruster or radiator
|
||||
// mesh sits under no joint (follow-up gimballed-thrusters).
|
||||
private static void BindRig(ShipAuthoring ship, Dictionary<string, (uint Index, bool HasMesh, int Parent)> nodes)
|
||||
{
|
||||
var joints = new HashSet<string>((ship.Joints ?? new List<ShipJoint>()).Select(joint => joint.Id), StringComparer.Ordinal);
|
||||
var idAt = nodes.ToDictionary(node => (int)node.Value.Index, node => node.Key);
|
||||
// Parent is already the nearest ancestor that carries an id, so the walk up is the chain of those.
|
||||
string JointAbove(string nodeId)
|
||||
{
|
||||
// glTF forbids cycles; the bound keeps a malformed model from hanging the binder.
|
||||
var parent = nodes[nodeId].Parent;
|
||||
for (var steps = 0; parent >= 0 && steps <= nodes.Count; steps++, parent = nodes[idAt[parent]].Parent)
|
||||
if (joints.Contains(idAt[parent])) return idAt[parent];
|
||||
return null;
|
||||
}
|
||||
|
||||
foreach (var joint in ship.Joints ?? new List<ShipJoint>())
|
||||
{
|
||||
if (!nodes.ContainsKey(joint.Id))
|
||||
throw new InvalidOperationException($"{ship.Id}: model has no node with aetheria.id={joint.Id} for a joint.");
|
||||
if (!string.Equals(JointAbove(joint.Id) ?? "", joint.Parent ?? "", StringComparison.Ordinal))
|
||||
throw new InvalidOperationException($"{ship.Id}: joint {joint.Id} must name the nearest joint above its model node as its Parent.");
|
||||
}
|
||||
foreach (var anchor in ship.Anchors.Where(anchor => anchor.Role == "weapon-mount" || anchor.Role == "thruster-emitter" || anchor.Role == "radiator-mesh"))
|
||||
{
|
||||
var above = JointAbove(anchor.ModelNodeId);
|
||||
if (string.Equals(above ?? "", anchor.Joint ?? "", StringComparison.Ordinal)) continue;
|
||||
throw new InvalidOperationException(anchor.Role == "weapon-mount"
|
||||
? $"{ship.Id}: weapon mount {anchor.Id} must name the nearest joint above its model node as its Joint."
|
||||
: $"{ship.Id}: {anchor.Role} anchor {anchor.Id} sits under a joint in the model (follow-up gimballed-thrusters).");
|
||||
}
|
||||
}
|
||||
|
||||
// GLB is the xenos asset boundary. Only its node extras, whether each node names a mesh, and its nearest identified ancestor's node index
|
||||
// (-1 for none) are read here; geometry belongs to the runtime importer.
|
||||
public static Dictionary<string, (uint Index, bool HasMesh, int Parent)> ReadNodeIds(string path)
|
||||
{
|
||||
using var stream = File.OpenRead(path);
|
||||
using var reader = new BinaryReader(stream);
|
||||
|
|
@ -204,17 +239,29 @@ public static class ShipModCatalog
|
|||
if (reader.ReadUInt32() != 0x4E4F534A || chunkLength > stream.Length - stream.Position)
|
||||
throw new InvalidOperationException($"{path}: missing GLB JSON chunk.");
|
||||
var json = JObject.Parse(Encoding.UTF8.GetString(reader.ReadBytes((int)chunkLength)));
|
||||
var ids = new Dictionary<string, (uint Index, bool HasMesh)>(StringComparer.Ordinal);
|
||||
var nodes = json["nodes"]?.OfType<JObject>() ?? Enumerable.Empty<JObject>();
|
||||
var ids = new Dictionary<string, (uint Index, bool HasMesh, int Parent)>(StringComparer.Ordinal);
|
||||
var nodes = (json["nodes"]?.OfType<JObject>() ?? Enumerable.Empty<JObject>()).ToArray();
|
||||
var parents = Enumerable.Repeat(-1, nodes.Length).ToArray();
|
||||
for (var parent = 0; parent < nodes.Length; parent++)
|
||||
foreach (var child in nodes[parent]["children"]?.Select(token => (int)token) ?? Enumerable.Empty<int>())
|
||||
if (child >= 0 && child < nodes.Length) parents[child] = parent;
|
||||
uint index = 0;
|
||||
foreach (var node in nodes)
|
||||
{
|
||||
var id = (string)node["extras"]?["aetheria.id"];
|
||||
if (string.IsNullOrEmpty(id)) { index++; continue; }
|
||||
if (!ids.TryAdd(id, (index, node["mesh"] != null && node["mesh"].Type != JTokenType.Null)))
|
||||
if (!ids.TryAdd(id, (index, node["mesh"] != null && node["mesh"].Type != JTokenType.Null, IdentifiedAncestor(parents, nodes, (int)index))))
|
||||
throw new InvalidOperationException($"{path}: duplicate GLB node aetheria.id={id}.");
|
||||
index++;
|
||||
}
|
||||
return ids;
|
||||
}
|
||||
|
||||
private static int IdentifiedAncestor(int[] parents, JObject[] nodes, int index)
|
||||
{
|
||||
var parent = parents[index];
|
||||
for (var steps = 0; parent >= 0 && steps <= nodes.Length; steps++, parent = parents[parent])
|
||||
if (!string.IsNullOrEmpty((string)nodes[parent]["extras"]?["aetheria.id"])) return parent;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,6 +15,10 @@ public sealed class ShipModPlan
|
|||
public string[] Radiators;
|
||||
// Each weapon mount (role weapon-mount) and its muzzle anchors, by Order then id.
|
||||
public (string Mount, string[] Muzzles)[] Weapons;
|
||||
// One entry per driven joint (one a weapon mount names), by its first weapon in hull order: the joint, its chain
|
||||
// root-first (RigChains), and the lead mount (the joint's first weapon in hull order) with that mount's first muzzle.
|
||||
// Presentation only: the assembler rigs these and nothing here reaches the simulation.
|
||||
public (string Joint, string[] Chain, string LeadMount, string LeadMuzzle)[] Rigs;
|
||||
|
||||
// Maps a pair Validate accepts; every rule about which anchor may play which part is Validate's, so this refuses nothing.
|
||||
public static ShipModPlan Build(HullData hull, ShipAuthoring visual)
|
||||
|
|
@ -26,6 +30,11 @@ public sealed class ShipModPlan
|
|||
var muzzles = visual.Anchors.Where(anchor => anchor.Role == "weapon-muzzle")
|
||||
.OrderBy(anchor => anchor.Order).ThenBy(anchor => anchor.Id, StringComparer.Ordinal).ToArray();
|
||||
|
||||
var jointOf = visual.Anchors.Where(anchor => anchor.Role == "weapon-mount" && !string.IsNullOrEmpty(anchor.Joint))
|
||||
.ToDictionary(anchor => anchor.Id, anchor => anchor.Joint, StringComparer.Ordinal);
|
||||
var leads = hardpoints.Where(hardpoint => jointOf.ContainsKey(hardpoint.Transform)).GroupBy(hardpoint => jointOf[hardpoint.Transform], StringComparer.Ordinal)
|
||||
.Select(group => (Joint: group.Key, Lead: group.First().Transform));
|
||||
|
||||
return new ShipModPlan
|
||||
{
|
||||
MapIcon = Only("map-icon"),
|
||||
|
|
@ -36,7 +45,9 @@ public sealed class ShipModPlan
|
|||
Radiators = Mounts(HardpointType.Radiator),
|
||||
Weapons = hardpoints.Where(hardpoint => ShipAuthoringStore.IsWeapon(hardpoint.Type)).Select(hardpoint => (
|
||||
hardpoint.Transform,
|
||||
muzzles.Where(muzzle => muzzle.ParentId == hardpoint.Transform).Select(muzzle => muzzle.Id).ToArray())).ToArray()
|
||||
muzzles.Where(muzzle => muzzle.ParentId == hardpoint.Transform).Select(muzzle => muzzle.Id).ToArray())).ToArray(),
|
||||
Rigs = leads.Select(rig => (rig.Joint, RigChains.Chain(visual, rig.Joint), rig.Lead,
|
||||
muzzles.First(muzzle => muzzle.ParentId == rig.Lead).Id)).ToArray()
|
||||
};
|
||||
}
|
||||
}
|
||||
|
|
|
|||
105
Assets/Scripts/ServerShared/ThrustAllocator.cs
Normal file
105
Assets/Scripts/ServerShared/ThrustAllocator.cs
Normal file
|
|
@ -0,0 +1,105 @@
|
|||
/* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at https://mozilla.org/MPL/2.0/. */
|
||||
|
||||
using System;
|
||||
using CultMath;
|
||||
|
||||
/// <summary>
|
||||
/// Turns intent (which way to go, which way to turn) into every thruster's throttle, whatever the thrusters' actual
|
||||
/// strength and position. One per ship. A column is one thruster's effect per unit throttle: x starboard acceleration,
|
||||
/// y forward acceleration, z clockwise yaw rate in rad/s. Throttles are bounded to [0,1].
|
||||
///
|
||||
/// One <see cref="BoundedLeastSquares"/> solve per call over the n throttles and four error variables (the unwanted or
|
||||
/// missing translation on each half-axis, paid for linearly). Yaw is a heavily weighted row, so a hold is exact and a
|
||||
/// turn is served first; the linear translation cost means the actuator with the least translation per unit of turn
|
||||
/// serves first and the next joins only as it saturates. Nothing classifies a column as attitude or drive: the onset
|
||||
/// at which a second actuator joins a turn falls out of the columns.
|
||||
/// </summary>
|
||||
public sealed class ThrustAllocator
|
||||
{
|
||||
const float TranslationWeight = 100f;
|
||||
const float YawWeight = 1000f;
|
||||
const float ErrorCostWeight = .3f;
|
||||
const float ErrorCostOffset = 10f;
|
||||
const float Ridge = .1f;
|
||||
const float ErrorMax = 2f;
|
||||
const double KktRelativeTolerance = 1e-12;
|
||||
const int ErrorVariables = 4;
|
||||
const int FixedRows = 3 + ErrorVariables;
|
||||
|
||||
int _columns = -1;
|
||||
float[] _a, _b, _lo, _hi, _x, _workspace;
|
||||
|
||||
public void Allocate(ReadOnlySpan<float3> columns, float2 move, float turn, Span<float> throttle)
|
||||
{
|
||||
var n = columns.Length;
|
||||
if (n != _columns) Resize(n);
|
||||
for (var i = 0; i < n; i++) throttle[i] = 0;
|
||||
|
||||
// Nothing asked, nothing fired: that is a fact about the intent, decided before the solve. The solver's warm
|
||||
// start is left as the last solved point, which is where the next non-zero call resumes.
|
||||
if (Intent(move, turn, 0) == 0 && Intent(move, turn, 1) == 0 && Intent(move, turn, 2) == 0) return;
|
||||
|
||||
var vars = n + ErrorVariables;
|
||||
var rows = FixedRows + n;
|
||||
|
||||
// Rows 0 and 1 are the translation axes (starboard, forward), row 2 the yaw.
|
||||
for (var axis = 0; axis < 3; axis++)
|
||||
{
|
||||
float plus = 0, minus = 0;
|
||||
for (var i = 0; i < n; i++)
|
||||
{
|
||||
var c = Component(columns[i], axis);
|
||||
if (c > 0) plus += c;
|
||||
else if (c < 0) minus -= c;
|
||||
}
|
||||
var extreme = Math.Max(plus, minus);
|
||||
var intent = Intent(move, turn, axis);
|
||||
var weight = axis == 2 ? YawWeight : TranslationWeight;
|
||||
// Intent is a fraction of the half-axis extreme it points into.
|
||||
var demand = intent >= 0 ? intent * plus : intent * minus;
|
||||
var scale = extreme > 0 ? weight / extreme : 0f;
|
||||
var row = axis * vars;
|
||||
for (var i = 0; i < n; i++) _a[row + i] = Component(columns[i], axis) * scale;
|
||||
_b[axis] = demand * scale;
|
||||
if (axis < 2)
|
||||
{
|
||||
var errorWeight = extreme > 0 ? weight : 0f;
|
||||
_a[row + n + 2 * axis] = -errorWeight;
|
||||
_a[row + n + 2 * axis + 1] = errorWeight;
|
||||
}
|
||||
}
|
||||
|
||||
// A linear cost on each error variable, as w*e = -w*kappa, and a ridge on each throttle.
|
||||
for (var k = 0; k < ErrorVariables; k++)
|
||||
{
|
||||
_a[(3 + k) * vars + n + k] = ErrorCostWeight;
|
||||
_b[3 + k] = -ErrorCostWeight * ErrorCostOffset;
|
||||
}
|
||||
for (var i = 0; i < n; i++) _a[(FixedRows + i) * vars + i] = Ridge;
|
||||
|
||||
var status = BoundedLeastSquares.Solve(rows, vars, _a, _b, _lo, _hi, _x, _workspace, out _, KktRelativeTolerance);
|
||||
if (status == BoundedLeastSquaresStatus.InvalidInput) return;
|
||||
for (var i = 0; i < n; i++) throttle[i] = _x[i];
|
||||
}
|
||||
|
||||
static float Intent(float2 move, float turn, int axis) =>
|
||||
Math.Min(Math.Max(axis == 0 ? move.x : axis == 1 ? move.y : turn, -1f), 1f);
|
||||
|
||||
static float Component(float3 c, int axis) => axis == 0 ? c.x : axis == 1 ? c.y : c.z;
|
||||
|
||||
void Resize(int n)
|
||||
{
|
||||
_columns = n;
|
||||
var vars = n + ErrorVariables;
|
||||
var rows = FixedRows + n;
|
||||
_a = new float[rows * vars];
|
||||
_b = new float[rows];
|
||||
_lo = new float[vars];
|
||||
_hi = new float[vars];
|
||||
_x = new float[vars];
|
||||
_workspace = new float[BoundedLeastSquares.WorkspaceLength(vars)];
|
||||
for (var i = 0; i < vars; i++) _hi[i] = i < n ? 1f : ErrorMax;
|
||||
}
|
||||
}
|
||||
11
Assets/Scripts/ServerShared/ThrustAllocator.cs.meta
Normal file
11
Assets/Scripts/ServerShared/ThrustAllocator.cs.meta
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
fileFormatVersion: 2
|
||||
guid: 5b0e3f7a9c2d4e61a8f4d3c7b1e90a26
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Loading…
Reference in a new issue