korchoon
3/31/2020 - 6:51 AM

average vector3

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);
        }
    }
}