diff --git a/restoration/source410/BT/MECH.CPP b/restoration/source410/BT/MECH.CPP index 41109675..4ee0e504 100644 --- a/restoration/source410/BT/MECH.CPP +++ b/restoration/source410/BT/MECH.CPP @@ -49,6 +49,7 @@ #include // ProjectileWeapon #include // MissileLauncher #include // AmmoBin +#include // MechControlsMapper -- the drive reads its demands // //############################################################################# @@ -141,8 +142,26 @@ Mech::Mech( legAnimation.Init(this); bodyAnimation.Init(this); + // + // Locomotion parameters. BRING-UP DEFAULTS: the authentic values come from + // the Mech model resource (WalkingTurnRate / RunningTurnRate / MaxAcceleration) + // and LoadLocomotionClips (the stride/top speeds measured from the walk/run + // animation clips). Wiring those in is a later refinement (needs the model- + // resource pointer + clip loader); until then these sane defaults make the + // mech drivable with the authentic control-interpretation + drive math. + // + walkingTurnRate = 50.0f * RAD_PER_DEG; // rad/s (walk / turn-in-place) + runningTurnRate = 25.0f * RAD_PER_DEG; // rad/s (at run speed) + reverseStrideLength = 30.0f; // top/run speed (u/s) + walkStrideLength = 12.0f; // walk speed (u/s) + reverseSpeedMax = 2.0f; // low-speed turn-rate gate (u/s) + forwardThrottleScale= 1.0f; + maxBodyAcceleration = 30.0f; // u/s^2 + bodyTargetSpeed = 0.0f; + currentBodySpeed = 0.0f; + { - for (int i = 0; i < 220; ++i) + for (int i = 0; i < 211; ++i) { reservedState[i] = 0; } @@ -377,8 +396,102 @@ void { Check(this); - Vector3D velocity = worldLinearVelocity; + // + //----------------------------------------------------------------------- + // Read the control-mapper locomotion demands. The mapper lives at roster + // slot 0 and its InterpretControls Performance ticks in the Entity::Perform + // AndWatch roster walk BEFORE this (the mech's own Performance runs last), + // so speedDemand/turnDemand are this frame's. + //----------------------------------------------------------------------- + // + Scalar speedDemand = 0.0f; + Scalar turnDemand = 0.0f; + if (subsystemArray != NULL && subsystemArray[0] != NULL) + { + MechControlsMapper *mapper = (MechControlsMapper *)subsystemArray[0]; + speedDemand = mapper->GetSpeedDemand(); + turnDemand = mapper->GetTurnDemand(); + } + bodyTargetSpeed = speedDemand; + // + //----------------------------------------------------------------------- + // Accelerate the actual body speed toward the demand (bounded per frame by + // the mech's max acceleration). + //----------------------------------------------------------------------- + // + { + Scalar dv = bodyTargetSpeed - currentBodySpeed; + Scalar maxStep = maxBodyAcceleration * time_slice; + if (dv > maxStep) dv = maxStep; + if (dv < -maxStep) dv = -maxStep; + currentBodySpeed += dv; + } + + // + //----------------------------------------------------------------------- + // Authentic per-mech turn rate: lerp(walkingTurnRate, runningTurnRate) by + // ground speed, with a runningTurnRate/t^2 over-run falloff past top speed; + // clamp >= 0. (mech4.cpp master-perf @0x4aa3d3.) + //----------------------------------------------------------------------- + // + Scalar authTurnRate = walkingTurnRate; + { + Scalar spd = (currentBodySpeed < 0.0f) ? -currentBodySpeed : currentBodySpeed; + if (spd >= reverseSpeedMax) + { + Scalar den = reverseStrideLength - walkStrideLength; + Scalar t = (den != 0.0f) ? (spd - walkStrideLength) / den : 0.0f; + if (t <= 1.0f) + { + authTurnRate = walkingTurnRate + (runningTurnRate - walkingTurnRate) * t; + } + else + { + authTurnRate = runningTurnRate / (t * t); + } + } + if (authTurnRate < 0.0f) + { + authTurnRate = 0.0f; + } + } + + // + //----------------------------------------------------------------------- + // Integrate heading (yaw) into the body orientation quaternion via the + // engine's rotation-integrate op (Quaternion::Add(source, omega*dt)), then + // rebuild the world transform so the facing axis below is current. + //----------------------------------------------------------------------- + // + { + Vector3D angStep; + angStep.x = 0.0f; + angStep.y = turnDemand * authTurnRate * time_slice; + angStep.z = 0.0f; + Quaternion prevPose = localOrigin.angularPosition; + localOrigin.angularPosition.Add(prevPose, angStep); + } + localToWorld = localOrigin; + + // + //----------------------------------------------------------------------- + // Forward step: the mech faces local -Z (gun ports / eyepoint at -Z). Take + // the world Z basis and negate for the facing direction; move at the current + // body speed. (The animation-exact per-frame advance from the gait clip is + // the deferred fidelity layer; this is the procedural equivalent.) + //----------------------------------------------------------------------- + // + UnitVector zAxis; + localToWorld.GetFromAxis(Z_Axis, &zAxis); + worldLinearVelocity.x = -zAxis.x * currentBodySpeed; + worldLinearVelocity.y = -zAxis.y * currentBodySpeed; + worldLinearVelocity.z = -zAxis.z * currentBodySpeed; + + // + // DEV override: BT_DRIVE forces a raw world velocity (bypasses the demands, + // for the pure integrate/transform test). + // { const char *drive = getenv("BT_DRIVE"); if (drive != NULL) @@ -386,20 +499,21 @@ void float dx = 0.0f, dy = 0.0f, dz = 0.0f; if (sscanf(drive, "%f,%f,%f", &dx, &dy, &dz) == 3) { - velocity.x = dx; - velocity.y = dy; - velocity.z = dz; + worldLinearVelocity.x = dx; + worldLinearVelocity.y = dy; + worldLinearVelocity.z = dz; } } } // - // Integrate position (pos = pos + velocity*dt) and commit the Origin to the - // world transform. + //----------------------------------------------------------------------- + // Integrate position and commit the Origin to the world transform. + //----------------------------------------------------------------------- // localOrigin.linearPosition.AddScaled( localOrigin.linearPosition, - velocity, + worldLinearVelocity, time_slice ); localToWorld = localOrigin; @@ -411,10 +525,14 @@ void if (reportAccum >= 1.0f) { reportAccum = 0.0f; - DEBUG_STREAM << "[sim] mech pos=(" + EulerAngles ypr; + ypr = localOrigin.angularPosition; + DEBUG_STREAM << "[sim] pos=(" << localOrigin.linearPosition.x << "," << localOrigin.linearPosition.y << "," << localOrigin.linearPosition.z << ")" + << " yaw=" << (Scalar)ypr.yaw + << " spd=" << currentBodySpeed << endl << flush; } } diff --git a/restoration/source410/BT/MECH.HPP b/restoration/source410/BT/MECH.HPP index 8fe57579..431ea179 100644 --- a/restoration/source410/BT/MECH.HPP +++ b/restoration/source410/BT/MECH.HPP @@ -285,6 +285,19 @@ Subsystem* GetSensorSubsystem() { Check(this); return sensorSubsystem; } + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Locomotion parameters (read by MechControlsMapper::InterpretControls to + // shape the demands, and by the drive in Simulate). reverseStrideLength is + // the top/run cycle speed the throttle scales (the naming is the 1995 + // field's; LoadLocomotionClips measures it from the run clips), walkStride + // Length the walk speed. Turn rates are radians/sec. + // + public: + Scalar GetReverseStrideLength() const { Check(this); return reverseStrideLength; } + Scalar GetWalkStrideLength() const { Check(this); return walkStrideLength; } + Scalar GetForwardThrottleScale() const { Check(this); return forwardThrottleScale; } + void SetForwardThrottleScale(Scalar s){ Check(this); forwardThrottleScale = s; } + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Local Data // @@ -329,11 +342,28 @@ CString resourceNameC; // - // Per-frame locomotion / targeting / animation state, advanced by the - // mech2/mech3/mech4 simulation. Reserved until that path (phase 5) is + // Locomotion state (Phase 5.3). Turn rates in rad/s, speeds/strides in + // world-units/s. reverseStrideLength = top (run) speed; walkStrideLength + // = walk speed; reverseSpeedMax = the low-speed turn-rate gate; forward + // ThrottleScale multiplies the forward demand; bodyTargetSpeed = the + // demanded speed; currentBodySpeed = the accel-tracked actual speed. + // + Scalar walkingTurnRate; + Scalar runningTurnRate; + Scalar reverseStrideLength; + Scalar walkStrideLength; + Scalar reverseSpeedMax; + Scalar forwardThrottleScale; + Scalar maxBodyAcceleration; + Scalar bodyTargetSpeed; + Scalar currentBodySpeed; + + // + // Remaining per-frame targeting / animation state, advanced by the + // mech2/mech3/mech4 simulation. Reserved until that path is // reconstructed with named fields. // - int reservedState[220]; + int reservedState[211]; }; #endif diff --git a/restoration/source410/BT/MECH.NOTES.md b/restoration/source410/BT/MECH.NOTES.md index ba73f68e..f87e4e7d 100644 --- a/restoration/source410/BT/MECH.NOTES.md +++ b/restoration/source410/BT/MECH.NOTES.md @@ -184,3 +184,44 @@ is a retained DEV hook (world-velocity injection) for headless motion tests. 3. terrain-height drop (`BoundingBoxTreeNode::FindBoundingBoxUnder`) resting the feet on the ground; 4. cockpit telemetry `FilteredScalar`s (head/aim/leg/torso angular rates). + +## PHASE 5.3 INCREMENT 2: DRIVABLE via authentic control interpretation (2026-07-21) + +The mech is now **drivable**: `Mech::Simulate` consumes the control-mapper +demands and drives locomotion with the authentic model. Chain: + +`MechControlsMapper::InterpretControls` (roster slot 0, ticks before the mech's +Performance each frame — see MECHMPPR.NOTES.md) reads the pushed raw inputs and +publishes `speedDemand` (world u/s = topSpeed·throttle·fwdScale) and `turnDemand` +([-1..1]). `Mech::Simulate` then: +- reads speedDemand/turnDemand from `subsystemArray[0]`; +- accelerates `currentBodySpeed` toward the demand (bounded by `maxBodyAcceleration`); +- computes the authentic per-mech turn rate `authTurnRate` = + lerp(walkingTurnRate, runningTurnRate) by ground speed with a `runTR/t²` + over-run falloff (mech4.cpp master-perf @0x4aa3d3); +- integrates heading: `localOrigin.angularPosition.Add(prevPose, + (0, turnDemand·authTurnRate·dt, 0))` — the engine rotation-integrate op; +- takes the world −Z basis (`localToWorld.GetFromAxis(Z_Axis,…)`, negated) as the + facing and sets `worldLinearVelocity = facing · currentBodySpeed`; +- integrates position (the increment-1 core). + +New named Mech members (out of `reservedState`, now [211]): `walkingTurnRate`, +`runningTurnRate`, `reverseStrideLength` (top speed), `walkStrideLength`, +`reverseSpeedMax`, `forwardThrottleScale`, `maxBodyAcceleration`, +`bodyTargetSpeed`, `currentBodySpeed`. **BRING-UP DEFAULTS** — the authentic +values come from the Mech model resource (WalkingTurnRate/RunningTurnRate/ +MaxAcceleration) + LoadLocomotionClips (stride/top speeds from the walk/run +clips); wiring the model-resource pointer + clip loader is the next refinement. + +**VERIFIED headlessly** (`BT_MECH_LOG` `[sim]`/`[mppr]`; forced-input dev hooks +`BT_FORCE_THROTTLE`/`BT_FORCE_TURN` in InterpretControls): +- `throttle=1.0` → speedDemand=30, mech accelerates to 30 u/s and walks straight + along its heading (per-second position delta magnitude = 30.1); +- `throttle=0.3, turn=1.0` → spd=9, mech walks a CIRCLE of radius ≈ 10 = + speed/turnRate (9 / 0.87 rad·s⁻¹) — heading integrates continuously; +- neutral (RIO, no hardware) → speedDemand=0, mech holds pose; +- zero Fail/Exception throughout. + +Still deferred: gait-clip-exact advance + leg animation (needs the animation +subsystem + renderer), model-resource/LoadLocomotionClips sourcing, terrain +drop, torso/free-look aiming (Torso/HUD analog axes), telemetry filters. diff --git a/restoration/source410/BT/MECHMPPR.CPP b/restoration/source410/BT/MECHMPPR.CPP index 74068c4f..4adb12da 100644 --- a/restoration/source410/BT/MECHMPPR.CPP +++ b/restoration/source410/BT/MECHMPPR.CPP @@ -15,6 +15,10 @@ # include #endif +#if !defined(MECH_HPP) +# include +#endif + Derivation MechControlsMapper::ClassDerivations( Subsystem::ClassDerivations, @@ -117,6 +121,11 @@ MechControlsMapper::MechControlsMapper( reserved[i] = 0; } } + // + // Install the per-frame control-interpretation Performance. + // + SetPerformance(&MechControlsMapper::InterpretControls); + if (getenv("BT_MECH_LOG")) { DEBUG_STREAM << "[mapper] ctor id=" << subsystem_ID @@ -131,6 +140,124 @@ MechControlsMapper::~MechControlsMapper() { } +// +//############################################################################# +// InterpretControls -- the mapper's per-frame Performance. Reads the raw input +// attributes (the engine controls push refreshes throttle/stick/pedals/buttons +// from the RIO/keyboard before this runs) and publishes the locomotion demands +// (speedDemand in world-u/s, turnDemand [-1..1]) the mech drive consumes. +// +// Reconstructs the authentic demand math (mechmppr.cpp @004afd10): +// * throttle -> forward speed: speedDemand = topSpeed * throttle * fwdScale +// (reverse thrust inverts and drops the forward scale); +// * soft stick response: square the yaw (sign preserved), cube the pedals; +// * Basic: stick yaw = turn; Standard/Veteran: pedals = turn; +// * speed is clamped down while turning hard (max_turn ramp). +// +// DEV hook (headless verification, no RIO/keyboard): BT_FORCE_THROTTLE / +// BT_FORCE_TURN override the pushed raw inputs so the demand math + mech drive +// are exercisable without hardware. The torso-aim / free-look interpretation +// (torso analog axes, look/eyepoint commit) is the aiming wave, deferred. +//############################################################################# +// +void + MechControlsMapper::InterpretControls(Scalar time_slice) +{ + Check(this); + + Mech *mech = GetMech(); + Check(mech); + + // + // DEV forced-input override. + // + { + const char *force_throttle = getenv("BT_FORCE_THROTTLE"); + if (force_throttle != NULL) + { + Scalar t = (Scalar)atof(force_throttle); + throttlePosition = (t >= 0.0f) ? t : -t; + reverseThrust = (t < 0.0f) ? 1 : 0; + } + const char *force_turn = getenv("BT_FORCE_TURN"); + if (force_turn != NULL) + { + stickPosition.x = (Scalar)atof(force_turn); + } + } + + Scalar topSpeed = mech->GetReverseStrideLength(); + Scalar walkSpeed = mech->GetWalkStrideLength(); + + // + // Throttle -> forward speed demand. + // + if (reverseThrust < 1) + { + speedDemand = topSpeed * throttlePosition * mech->GetForwardThrottleScale(); + } + else + { + speedDemand = -topSpeed * throttlePosition; + } + + // + // Soft response: square the stick yaw (sign preserved), cube the pedals. + // + Scalar stick_x = stickPosition.x * stickPosition.x; + if (stickPosition.x < 0.0f) + { + stick_x = -stick_x; + } + Scalar pedal_3 = pedalsPosition * pedalsPosition * pedalsPosition; + + turnDemand = 0.0f; + + if (controlMode == BasicMode) + { + turnDemand = stick_x; + } + else + { + turnDemand = pedal_3; + } + + // + // Clamp forward speed down while turning hard (except VeteranMode, which has + // no speed-dependent turn clamp). max_turn ramps from topSpeed (no turn) to + // walkSpeed (full turn). + // + if (controlMode != VeteranMode) + { + Scalar turn_mag = (turnDemand < 0.0f) ? -turnDemand : turnDemand; + if (turn_mag > 0.001f) + { + Scalar max_turn = (topSpeed - walkSpeed) * (1.0f - turn_mag) + walkSpeed; + if (speedDemand > max_turn) speedDemand = max_turn; + if (speedDemand < -max_turn) speedDemand = -max_turn; + } + } + + if (getenv("BT_MECH_LOG")) + { + static Scalar reportAccum = 0.0f; + reportAccum += time_slice; + if (reportAccum >= 1.0f) + { + reportAccum = 0.0f; + DEBUG_STREAM << "[mppr] thr=" << throttlePosition + << " rev=" << reverseThrust + << " stickX=" << stickPosition.x + << " mode=" << controlMode + << " -> speedDemand=" << speedDemand + << " turnDemand=" << turnDemand + << endl << flush; + } + } + + Check_Fpu(); +} + Logical MechControlsMapper::TestInstance() const { diff --git a/restoration/source410/BT/MECHMPPR.HPP b/restoration/source410/BT/MECHMPPR.HPP index bd3bcac1..83b2d78a 100644 --- a/restoration/source410/BT/MECHMPPR.HPP +++ b/restoration/source410/BT/MECHMPPR.HPP @@ -108,6 +108,31 @@ Logical TestInstance() const; + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Per-frame control interpretation (the mapper's Performance): reads the + // engine-pushed raw input attributes (throttle / stick / pedals / buttons) + // and publishes the locomotion + aim demands the mech drive consumes. + // + public: + typedef void + (MechControlsMapper::*Performance)(Scalar time_slice); + void + SetPerformance(Performance performance) + { + Check(this); + activePerformance = (Simulation::Performance)performance; + } + + void + InterpretControls(Scalar time_slice); + + Mech* + GetMech() { Check(this); return (Mech *)GetEntity(); } + Scalar + GetSpeedDemand() const { Check(this); return speedDemand; } + Scalar + GetTurnDemand() const { Check(this); return turnDemand; } + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Local Data -- the published control inputs (the streamed mappings write // these; InterpretControls, phase 5.3, reads them to drive the mech). diff --git a/restoration/source410/BT/MECHMPPR.NOTES.md b/restoration/source410/BT/MECHMPPR.NOTES.md new file mode 100644 index 00000000..f8343151 --- /dev/null +++ b/restoration/source410/BT/MECHMPPR.NOTES.md @@ -0,0 +1,45 @@ +# MECHMPPR.CPP / .HPP — reconstruction notes + +`MechControlsMapper` (: Subsystem) is the mech's control-input subsystem, +installed at roster slot 0 via `Mech::SetMappingSubsystem`. It publishes the +control-input attributes (StickPosition..PilotArray, IDs 3–0x16) that the +streamed control mappings bind to, and — as of Phase 5.3 — interprets those raw +inputs into the locomotion/aim demands the mech drive consumes. + +## Reconstructed + +- The attribute table + AttributeIndex + shared-data statics (earlier wave). +- The ctor: primes the published inputs to neutral and (Phase 5.3) installs + `InterpretControls` as the per-frame Performance. +- `InterpretControls(Scalar)` — the per-frame control interpretation + (mechmppr.cpp @004afd10, authentic demand math): + - throttle → forward speed: `speedDemand = topSpeed · throttle · fwdScale` + (reverse thrust inverts and drops the forward scale). `topSpeed`/`walkSpeed` + are read from the owner via `Mech::GetReverseStrideLength()/GetWalkStrideLength()`. + - soft response: square the stick yaw (sign preserved), cube the pedals. + - Basic mode: stick yaw = `turnDemand`; Standard/Veteran: pedals = `turnDemand`. + - speed clamp while turning hard (`max_turn` ramps topSpeed → walkSpeed by + |turnDemand|); VeteranMode has no clamp. +- Demand accessors `GetSpeedDemand()/GetTurnDemand()` (read by `Mech::Simulate`). +- `GetMech()` = `(Mech *)GetEntity()`. + +The mapper ticks in the engine's `Entity::PerformAndWatch` roster walk (slot 0), +which runs BEFORE the mech's own Performance (`Simulation::PerformAndWatch` at +the tail), so the demands are same-frame — no input latency. + +## DEV hook (headless verification) + +`BT_FORCE_THROTTLE` / `BT_FORCE_TURN` override the pushed raw inputs (the RIO/ +keyboard aren't present in the headless smoke test), so the demand math + mech +drive are exercisable without hardware. `BT_MECH_LOG` prints a 1 Hz `[mppr]` +demand trace. Verified: `throttle=1.0` → speedDemand=30; `throttle=0.3,turn=1.0` +→ the mech walks a circle; neutral → 0. See MECH.NOTES.md (Phase 5.3 inc 2). + +## Deferred (the aiming wave) + +The torso/free-look half of the authentic InterpretControls is not yet +reconstructed: the torso analog axes (`Torso::SetAnalogElevationAxis/ +SetAnalogTwistAxis`), HUD free-aim slew (`HUD::SetFreeAimSlew`), the recenter +command, and the look/eyepoint commit (`BTCommitLookState`). These are the +aiming/camera surface, separate from locomotion; they come with the +Torso/HUD/eyepoint wave.