private void ComputeVelocities(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 2)
{
PadVelocities(kinematics);
return;
}
for (int i = 1; i < kinematics.Length - 1; i++)
{
PointF a = kinematics.Coordinates(i - 1);
PointF b = kinematics.Coordinates(i + 1);
float t = calibrationHelper.GetTime(2);
kinematics.Speed[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Magnitude));
kinematics.HorizontalVelocity[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Horizontal));
kinematics.VerticalVelocity[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Vertical));
}
PadVelocities(kinematics);
double constantVelocitySpan = 40;
MovingAverage filter = new MovingAverage();
kinematics.Speed = filter.FilterSamples(kinematics.Speed, calibrationHelper.CaptureFramesPerSecond, constantVelocitySpan, 1);
kinematics.HorizontalVelocity = filter.FilterSamples(kinematics.HorizontalVelocity, calibrationHelper.CaptureFramesPerSecond, constantVelocitySpan, 1);
kinematics.VerticalVelocity = filter.FilterSamples(kinematics.VerticalVelocity, calibrationHelper.CaptureFramesPerSecond, constantVelocitySpan, 1);
}
private void ComputeRotationCircle(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length < 3)
{
return;
}
// Least-squares circle fitting.
// Ref: "Circle fitting by linear and nonlinear least squares", Coope, I.D.,
// Journal of Optimization Theory and Applications Volume 76, Issue 2, New York: Plenum Press, February 1993.
// Implementation based on JS implementation:
// http://jsxgraph.uni-bayreuth.de/wiki/index.php/Least-squares_circle_fitting
int rows = kinematics.Length;
Matrix m = new Matrix(rows, 3);
Matrix v = new Matrix(rows, 1);
for (int i = 0; i < rows; i++)
{
PointF point = kinematics.Coordinates(i);
m[i, 0] = point.X;
m[i, 1] = point.Y;
m[i, 2] = 1.0;
v[i, 0] = point.X * point.X + point.Y * point.Y;
}
Matrix mt = m.Clone();
mt.Transpose();
Matrix b = mt.Multiply(m);
Matrix c = mt.Multiply(v);
Matrix z = b.Solve(c);
PointF center = new PointF((float)(z[0, 0] * 0.5), (float)(z[1, 0] * 0.5));
double radius = Math.Sqrt(z[2, 0] + (center.X * center.X) + (center.Y * center.Y));
kinematics.RotationCenter = center;
kinematics.RotationRadius = radius;
for (int i = 0; i < rows; i++)
{
PointF xAxis = center.Translate(100.0f, 0.0f);
float angle = GeometryHelper.GetAngle(center, xAxis, kinematics.Coordinates(i));
if (angle < 0)
{
angle = (float)((Math.PI * 2) + angle);
}
kinematics.AbsoluteAngle[i] = angle;
}
}
private void ComputeTotalDistance(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
float distance = 0;
kinematics.TotalDistance[0] = distance;
for (int i = 1; i < kinematics.Length; i++)
{
PointF a = kinematics.Coordinates(i - 1);
PointF b = kinematics.Coordinates(i);
float d = GeometryHelper.GetDistance(a, b);
distance += d;
kinematics.TotalDistance[i] = distance;
}
}
private void ComputeAccelerations(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 4)
{
PadAccelerations(kinematics);
return;
}
// First pass: average speed over 2t centered on each data point.
for (int i = 2; i < kinematics.Length - 2; i++)
{
PointF p0 = kinematics.Coordinates(i - 2);
PointF p2 = kinematics.Coordinates(i);
PointF p4 = kinematics.Coordinates(i + 2);
float t02 = calibrationHelper.GetTime(2);
float t24 = calibrationHelper.GetTime(2);
float t13 = calibrationHelper.GetTime(2);
double acceleration = (kinematics.Speed[i + 1] - kinematics.Speed[i - 1]) / t13;
kinematics.Acceleration[i] = calibrationHelper.ConvertAccelerationFromVelocity((float)acceleration);
double horizontalAcceleration = (kinematics.HorizontalVelocity[i + 1] - kinematics.HorizontalVelocity[i - 1]) / t13;
kinematics.HorizontalAcceleration[i] = calibrationHelper.ConvertAccelerationFromVelocity((float)horizontalAcceleration);
double verticalAcceleration = (kinematics.VerticalVelocity[i + 1] - kinematics.VerticalVelocity[i - 1]) / t13;
kinematics.VerticalAcceleration[i] = calibrationHelper.ConvertAccelerationFromVelocity((float)verticalAcceleration);
}
PadAccelerations(kinematics);
double constantAccelerationSpan = 50;
MovingAverage filter = new MovingAverage();
kinematics.Acceleration = filter.FilterSamples(kinematics.Acceleration, calibrationHelper.CaptureFramesPerSecond, constantAccelerationSpan, 2);
kinematics.HorizontalAcceleration = filter.FilterSamples(kinematics.HorizontalAcceleration, calibrationHelper.CaptureFramesPerSecond, constantAccelerationSpan, 2);
kinematics.VerticalAcceleration = filter.FilterSamples(kinematics.VerticalAcceleration, calibrationHelper.CaptureFramesPerSecond, constantAccelerationSpan, 2);
}