Created
March 31, 2020 06:51
-
-
Save korchoon/f11324833cb4216b01b5c79eda9ebb50 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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