public class SpeedOMeter {
public AverageVector3 AngularVelocity;
public AverageVector3 Velocity;
Quaternion _lastRotation;
Vector3 _lastPos;
Transform _t;
public SpeedOMeter(Transform t) {
_t = t;
Velocity = new AverageVector3();
AngularVelocity = new AverageVector3();
_lastRotation = _t.rotation;
_lastPos = _t.position;
}
public void Process(float dt, float avgCount) {
var bufPos = _lastPos;
_lastPos = _t.position;
Velocity.Next((_lastPos - bufPos) / dt, avgCount);
var bufRotation = _lastRotation;
_lastRotation = _t.rotation;
var deltaRotation = _lastRotation * Quaternion.Inverse(bufRotation);
var rad = WrapAngle(deltaRotation.eulerAngles) * Mathf.Deg2Rad;
AngularVelocity.Next(rad / dt, avgCount);
}
static Vector3 WrapAngle(Vector3 euler) {
for (var i = 0; i < 3; i++)
euler[i] = Mathf.DeltaAngle(0f, euler[i]);
return euler;
}
public class AverageVector3 {
public Vector3 Value { get; private set; }
public void Next(in Vector3 valueToAdd, in float count) {
Value = (Value * count + valueToAdd) / (count + 1);
}
}
}