103 lines
3.6 KiB
C#
103 lines
3.6 KiB
C#
using System;
|
||
|
||
namespace FluidSim.Components
|
||
{
|
||
public class Crankshaft
|
||
{
|
||
public float AngularVelocity;
|
||
public float CrankAngle;
|
||
public float PreviousAngle;
|
||
|
||
public float Inertia = 0.2f;
|
||
public float FrictionConstant;
|
||
public float FrictionViscous;
|
||
public float LastNetTorque { get; private set; }
|
||
public float AveragePower { get; private set; }
|
||
public float AverageTorque { get; private set; }
|
||
|
||
private float externalTorque;
|
||
private float _loadTorque;
|
||
|
||
private readonly float[] _powerBuffer;
|
||
private int _powerBufIdx, _powerBufCount;
|
||
private float _powerBufSum;
|
||
private readonly float[] _torqueBuffer;
|
||
private int _torqueBufIdx, _torqueBufCount;
|
||
private float _torqueBufSum;
|
||
|
||
/// <summary>Engine cycle length in radians. 4π = four‑stroke, 2π = two‑stroke.</summary>
|
||
public float CycleLength { get; set; } = 4f * MathF.PI;
|
||
|
||
public Crankshaft(float initialRPM = 400f)
|
||
{
|
||
AngularVelocity = initialRPM * 2f * MathF.PI / 60f;
|
||
CrankAngle = 0f;
|
||
PreviousAngle = 0f;
|
||
|
||
_powerBuffer = new float[16384];
|
||
_torqueBuffer = new float[16384];
|
||
}
|
||
|
||
public void AddTorque(float torque) => externalTorque += torque;
|
||
|
||
public void SetLoadTorque(float torque) => _loadTorque = Math.Max(torque, 0f);
|
||
|
||
private float _effectiveInertia; // if >0, overrides Inertia
|
||
|
||
public void SetEffectiveInertia(float inertia)
|
||
{
|
||
_effectiveInertia = inertia;
|
||
}
|
||
|
||
public void Step(float dt)
|
||
{
|
||
if (float.IsNaN(AngularVelocity) || float.IsInfinity(AngularVelocity))
|
||
AngularVelocity = 0f;
|
||
if (float.IsNaN(externalTorque) || float.IsInfinity(externalTorque))
|
||
externalTorque = 0f;
|
||
|
||
PreviousAngle = CrankAngle;
|
||
|
||
float friction = FrictionConstant * MathF.Sign(AngularVelocity)
|
||
+ FrictionViscous * AngularVelocity;
|
||
|
||
float netTorque = externalTorque - friction;
|
||
LastNetTorque = netTorque;
|
||
|
||
float totalNetTorque = netTorque - _loadTorque;
|
||
float currentInertia = _effectiveInertia > 0f ? _effectiveInertia : Inertia;
|
||
float alpha = totalNetTorque / currentInertia;
|
||
AngularVelocity += alpha * dt;
|
||
if (AngularVelocity < 0f) AngularVelocity = 0f;
|
||
|
||
CrankAngle += AngularVelocity * dt;
|
||
if (CrankAngle >= CycleLength)
|
||
CrankAngle -= CycleLength;
|
||
else if (CrankAngle < 0f)
|
||
CrankAngle += CycleLength;
|
||
|
||
// Power averaging
|
||
float instantPower = netTorque * AngularVelocity;
|
||
if (_powerBufCount == _powerBuffer.Length)
|
||
_powerBufSum -= _powerBuffer[_powerBufIdx];
|
||
else
|
||
_powerBufCount++;
|
||
_powerBuffer[_powerBufIdx] = instantPower;
|
||
_powerBufSum += instantPower;
|
||
_powerBufIdx = (_powerBufIdx + 1) % _powerBuffer.Length;
|
||
AveragePower = _powerBufSum / _powerBufCount;
|
||
|
||
// Torque averaging
|
||
if (_torqueBufCount == _torqueBuffer.Length)
|
||
_torqueBufSum -= _torqueBuffer[_torqueBufIdx];
|
||
else
|
||
_torqueBufCount++;
|
||
_torqueBuffer[_torqueBufIdx] = netTorque;
|
||
_torqueBufSum += netTorque;
|
||
_torqueBufIdx = (_torqueBufIdx + 1) % _torqueBuffer.Length;
|
||
AverageTorque = _torqueBufSum / _torqueBufCount;
|
||
|
||
externalTorque = 0f;
|
||
}
|
||
}
|
||
} |