private void PadVelocities(TrajectoryKinematics kinematics)
{
PadUncomputables(kinematics.Speed, 1);
PadUncomputables(kinematics.HorizontalVelocity, 1);
PadUncomputables(kinematics.VerticalVelocity, 1);
return;
}
private void PadAccelerations(TrajectoryKinematics kinematics)
{
PadUncomputables(kinematics.Acceleration, 2);
PadUncomputables(kinematics.HorizontalAcceleration, 2);
PadUncomputables(kinematics.VerticalAcceleration, 2);
return;
}
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 ComputeAngularAccelerations(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 4)
{
PadAngularAccelerations(kinematics);
return;
}
for (int i = 2; i < kinematics.Length - 2; i++)
{
double d1 = GetDisplacementAngle(kinematics, i - 1, i - 2);
double d2 = GetDisplacementAngle(kinematics, i, i - 1);
double d3 = GetDisplacementAngle(kinematics, i + 1, i);
double d4 = GetDisplacementAngle(kinematics, i + 2, i + 1);
float t02 = calibrationHelper.GetTime(2);
float t24 = calibrationHelper.GetTime(2);
float t13 = calibrationHelper.GetTime(2);
float v1 = (float)((d1 + d2) / t02);
float v2 = (float)((d3 + d4) / t24);
float a = (float)((v2 - v1) / t13);
kinematics.AngularAcceleration[i] = calibrationHelper.ConvertAngularAcceleration(a);
}
PadAngularAccelerations(kinematics);
}
/// <summary>
/// Computes displacement angle in radians.
/// </summary>
private double GetDisplacementAngle(TrajectoryKinematics kinematics, int a, int b)
{
double displacement = Math.Abs(kinematics.AbsoluteAngle[a] - kinematics.AbsoluteAngle[b]);
// To solve the ambiguity when moving through the x axis we keep the smallest value.
// This assumes the motion is incrementing by small pieces rather than more than a half circle at a time.
displacement = Math.Min(displacement, 2 * Math.PI - displacement);
return(displacement);
}
private void ComputeRawCoordinates(TrajectoryKinematics kinematics, List <TimedPoint> input, CalibrationHelper calibrationHelper)
{
for (int i = 0; i < input.Count; i++)
{
PointF point = calibrationHelper.GetPointAtTime(input[i].Point, input[i].T);
kinematics.RawXs[i] = point.X;
kinematics.RawYs[i] = point.Y;
kinematics.Times[i] = input[i].T;
}
}
public FormTrackAnalysis(Metadata metadata, DrawingTrack track)
{
this.metadata = metadata;
this.kinematics = track.TrajectoryKinematics;
this.color = track.MainColor;
InitializeComponent();
plotHelper = new PlotHelper(plotView);
Localize();
CreatePlots();
}
private void ComputeFilteredCoordinates(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
double framerate = calibrationHelper.CaptureFramesPerSecond;
ButterworthFilter filter = new ButterworthFilter();
int bestCutoffIndex;
kinematics.FilterResultXs = filter.FilterSamples(kinematics.RawXs, framerate, 100, out bestCutoffIndex);
kinematics.XCutoffIndex = bestCutoffIndex;
kinematics.FilterResultYs = filter.FilterSamples(kinematics.RawYs, framerate, 100, out bestCutoffIndex);
kinematics.YCutoffIndex = bestCutoffIndex;
}
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 ComputeDisplacementAngle(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
double total = 0;
kinematics.DisplacementAngle[0] = 0;
kinematics.TotalDisplacementAngle[0] = total;
for (int i = 1; i < kinematics.Length; i++)
{
double displacement = GetDisplacementAngle(kinematics, i, i - 1);
total += displacement;
kinematics.DisplacementAngle[i] = calibrationHelper.ConvertAngle((float)displacement);
kinematics.TotalDisplacementAngle[i] = calibrationHelper.ConvertAngle((float)total);
}
}
public TrajectoryKinematics AnalyzeTrajectory(List <TimedPoint> samples, CalibrationHelper calibrationHelper)
{
TrajectoryKinematics kinematics = new TrajectoryKinematics();
if (samples.Count == 0)
{
return(kinematics);
}
kinematics.Initialize(samples.Count);
ComputeRawCoordinates(kinematics, samples, calibrationHelper);
ComputeRawTotalDistance(kinematics, calibrationHelper);
ComputeRawVelocities(kinematics, calibrationHelper);
ComputeRawAccelerations(kinematics, calibrationHelper);
if (kinematics.CanFilter)
{
ComputeFilteredCoordinates(kinematics, calibrationHelper);
ComputeTotalDistance(kinematics, calibrationHelper);
ComputeVelocities(kinematics, calibrationHelper);
ComputeAccelerations(kinematics, calibrationHelper);
}
else
{
kinematics.ForceRawSeries();
}
try
{
// Angular kinematics based on the best fit circle.
ComputeRotationCircle(kinematics, calibrationHelper);
ComputeDisplacementAngle(kinematics, calibrationHelper);
ComputeAngularVelocities(kinematics, calibrationHelper);
ComputeAngularAccelerations(kinematics, calibrationHelper);
}
catch (Exception e)
{
log.ErrorFormat("Error while computing angular kinematics from best fit circle on trajectory.");
log.ErrorFormat(e.ToString());
}
return(kinematics);
}
private void ComputeRawVelocities(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 2)
{
PadRawVelocities(kinematics);
return;
}
for (int i = 1; i < kinematics.Length - 1; i++)
{
PointF a = kinematics.RawCoordinates(i - 1);
PointF b = kinematics.RawCoordinates(i + 1);
float t = calibrationHelper.GetTime(2);
kinematics.RawSpeed[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Magnitude));
kinematics.RawHorizontalVelocity[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Horizontal));
kinematics.RawVerticalVelocity[i] = (double)calibrationHelper.ConvertSpeed(GetSpeed(a, b, t, Component.Vertical));
}
PadRawVelocities(kinematics);
}
private void ComputeAngularVelocities(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 2)
{
PadAngularVelocities(kinematics);
return;
}
for (int i = 1; i < kinematics.Length - 1; i++)
{
double d1 = GetDisplacementAngle(kinematics, i, i - 1);
double d2 = GetDisplacementAngle(kinematics, i + 1, i);
float time = calibrationHelper.GetTime(2);
float inRadPerSecond = (float)((d1 + d2) / time);
kinematics.AngularVelocity[i] = calibrationHelper.ConvertAngularVelocity(inRadPerSecond);
kinematics.TangentialVelocity[i] = calibrationHelper.ConvertSpeed((float)(inRadPerSecond * kinematics.RotationRadius));
kinematics.CentripetalAcceleration[i] = calibrationHelper.ConvertAcceleration((float)(inRadPerSecond * inRadPerSecond * kinematics.RotationRadius));
}
PadAngularVelocities(kinematics);
}
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);
}
private void ComputeRawAccelerations(TrajectoryKinematics kinematics, CalibrationHelper calibrationHelper)
{
if (kinematics.Length <= 4)
{
PadRawAccelerations(kinematics);
return;
}
for (int i = 2; i < kinematics.Length - 2; i++)
{
PointF p0 = kinematics.RawCoordinates(i - 2);
PointF p2 = kinematics.RawCoordinates(i);
PointF p4 = kinematics.RawCoordinates(i + 2);
float t02 = calibrationHelper.GetTime(2);
float t24 = calibrationHelper.GetTime(2);
float t13 = calibrationHelper.GetTime(2);
kinematics.RawAcceleration[i] = calibrationHelper.ConvertAcceleration(GetAcceleration(p0, p2, p4, t02, t24, t13, Component.Magnitude));
kinematics.RawHorizontalAcceleration[i] = calibrationHelper.ConvertAcceleration(GetAcceleration(p0, p2, p4, t02, t24, t13, Component.Horizontal));
kinematics.RawVerticalAcceleration[i] = calibrationHelper.ConvertAcceleration(GetAcceleration(p0, p2, p4, t02, t24, t13, Component.Vertical));
}
PadRawAccelerations(kinematics);
}
private void PadAngularAccelerations(TrajectoryKinematics kinematics)
{
PadUncomputables(kinematics.AngularAcceleration, 2);
}
private void PadAngularVelocities(TrajectoryKinematics kinematics)
{
PadUncomputables(kinematics.AngularVelocity, 1);
PadUncomputables(kinematics.CentripetalAcceleration, 1);
PadUncomputables(kinematics.TangentialVelocity, 1);
}
