Skip to content

Instantly share code, notes, and snippets.

@korchoon
Created March 31, 2020 06:51
Show Gist options
  • Save korchoon/f11324833cb4216b01b5c79eda9ebb50 to your computer and use it in GitHub Desktop.
Save korchoon/f11324833cb4216b01b5c79eda9ebb50 to your computer and use it in GitHub Desktop.
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);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment