private float ComputeCalibratedAngle(PointF o, PointF a, PointF b, bool signed, bool ccw, CalibrationHelper calibration)
{
if (calibration == null)
{
throw new InvalidProgramException();
}
PointF o2 = calibration.GetPoint(o);
PointF a2 = calibration.GetPoint(a);
PointF b2 = calibration.GetPoint(b);
float value = 0;
if (ccw)
{
value = GeometryHelper.GetAngle(o2, a2, b2);
}
else
{
value = GeometryHelper.GetAngle(o2, b2, a2);
}
if (!signed && value < 0)
{
value = (float)(TAU + value);
}
return(value);
}
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 static void ProcessPointImpact(GenericPosture posture, CalibrationHelper calibrationHelper, GenericPostureAbstractImpact impact, int handle, PointF old)
{
switch (impact.Type)
{
case ImpactType.LineAlign:
AlignPointSegment(posture, impact as GenericPostureImpactLineAlign);
break;
case ImpactType.VerticalAlign:
AlignPointVertical(posture, calibrationHelper, handle, impact as GenericPostureImpactVerticalAlign);
break;
case ImpactType.HorizontalAlign:
AlignPointHorizontal(posture, handle, impact as GenericPostureImpactHorizontalAlign);
break;
case ImpactType.Pivot:
// Get rotation that was applied. Apply same rotation on all points.
GenericPostureImpactPivot impactPivot = impact as GenericPostureImpactPivot;
if (impact != null)
{
PointF a = posture.PointList[impactPivot.Pivot];
PointF b = old;
PointF c = posture.PointList[posture.Handles[handle].Reference];
float radians = GeometryHelper.GetAngle(a, b, c);
PivotPoints(posture, radians, impact as GenericPostureImpactPivot);
}
break;
case ImpactType.KeepAngle:
KeepPointAngle(posture, impact as GenericPostureImpactKeepAngle);
break;
case ImpactType.SegmentCenter:
SegmentCenter(posture, calibrationHelper, impact as GenericPostureImpactSegmentCenter);
break;
case ImpactType.PerdpendicularAlign:
AlignPointPerpendicular(posture, calibrationHelper, impact as GenericPosturePerpendicularAlign);
break;
case ImpactType.ParallelAlign:
AlignPointParallel(posture, calibrationHelper, impact as GenericPostureParallelAlign);
break;
}
}
private static void PrepareImpacts(GenericPosture posture, int handle)
{
foreach (GenericPostureAbstractImpact impact in posture.Handles[handle].Impacts)
{
// If there is a KeepAngle impact, we'll later need to know the current angle.
if (impact.Type == ImpactType.KeepAngle)
{
GenericPostureImpactKeepAngle impactKeepAngle = impact as GenericPostureImpactKeepAngle;
PointF origin = posture.PointList[impactKeepAngle.Origin];
PointF leg1 = posture.PointList[impactKeepAngle.Leg1];
PointF leg2 = posture.PointList[impactKeepAngle.Leg2];
if (origin == leg1 || origin == leg2)
{
continue;
}
impactKeepAngle.OldAngle = GeometryHelper.GetAngle(origin, leg1, leg2);
impactKeepAngle.OldDistance = GeometryHelper.GetDistance(origin, leg2);
}
}
}
private void ComputeAngles(TimeSeriesCollection tsc, CalibrationHelper calibrationHelper, Dictionary <string, FilteredTrajectory> trajs, AngleOptions angleOptions)
{
for (int i = 0; i < tsc.Length; i++)
{
PointF o = PointF.Empty;
PointF a = PointF.Empty;
PointF b = PointF.Empty;
if (trajs["o"].CanFilter)
{
o = trajs["o"].Coordinates(i);
a = trajs["a"].Coordinates(i);
b = trajs["b"].Coordinates(i);
}
else
{
o = trajs["o"].RawCoordinates(i);
a = trajs["a"].RawCoordinates(i);
b = trajs["b"].RawCoordinates(i);
}
// Compute the actual angle value. The logic here should match the one in AngleHelper.Update().
// They work on different type of inputs so it's difficult to factorize the functions.
if (angleOptions.Supplementary)
{
// Create a new point by point reflection of a around o.
PointF c = new PointF(2 * o.X - a.X, 2 * o.Y - a.Y);
a = b;
b = c;
}
float angle = 0;
if (angleOptions.CCW)
{
angle = GeometryHelper.GetAngle(o, a, b);
}
else
{
angle = GeometryHelper.GetAngle(o, b, a);
}
if (!angleOptions.Signed && angle < 0)
{
angle = (float)(TAU + angle);
}
positions[i] = angle;
radii[i] = GeometryHelper.GetDistance(o, b);
tsc[Kinematics.AngularPosition][i] = calibrationHelper.ConvertAngle(angle);
if (i == 0)
{
tsc[Kinematics.AngularDisplacement][i] = 0;
tsc[Kinematics.TotalAngularDisplacement][i] = 0;
}
else
{
float totalDisplacementAngle = angle - positions[0];
float displacementAngle = angle - positions[i - 1];
tsc[Kinematics.AngularDisplacement][i] = calibrationHelper.ConvertAngle(displacementAngle);
tsc[Kinematics.TotalAngularDisplacement][i] = calibrationHelper.ConvertAngle(totalDisplacementAngle);
}
}
}