private void DrawDebug(double opacity, Graphics canvas, IImageToViewportTransformer transformer, List <Point> points)
{
// Note: some of the labels are drawn during the normal method with an extra test there.
PointF offset = new PointF(10, 10);
// Points id.
for (int i = 0; i < genericPosture.PointList.Count; i++)
{
string label = string.Format("P{0}", i);
PointF p = points[i];
DrawDebugText(p, offset, label, canvas, opacity, transformer);
}
// Segments id and name.
for (int i = 0; i < genericPosture.Segments.Count; i++)
{
GenericPostureSegment segment = genericPosture.Segments[i];
if (segment.Start < 0 || segment.End < 0)
{
continue;
}
PointF start = points[segment.Start];
PointF end = points[segment.End];
PointF middle = GeometryHelper.GetMiddlePoint(start, end);
string label = "";
if (!string.IsNullOrEmpty(segment.Name))
{
label = string.Format("S{0} [P{1}, P{2}]: {3}", i, segment.Start, segment.End, segment.Name);
}
else
{
label = string.Format("S{0} [P{1}, P{2}]", i, segment.Start, segment.End);
}
DrawDebugText(middle, offset, label, canvas, opacity, transformer);
}
}
private static void SegmentCenter(GenericPosture posture, CalibrationHelper calibrationHelper, GenericPostureImpactSegmentCenter impact)
{
// The point is moved so that it stays at the center of the specified segment.
// This should take perspective into account.
if (impact == null)
{
return;
}
PointF p1 = posture.PointList[impact.Point1];
PointF p2 = posture.PointList[impact.Point2];
PointF p1Plane = calibrationHelper.GetPoint(p1);
PointF p2Plane = calibrationHelper.GetPoint(p2);
PointF resultPlane = GeometryHelper.GetMiddlePoint(p1Plane, p2Plane);
PointF result = calibrationHelper.GetImagePoint(resultPlane);
posture.PointList[impact.PointToMove] = result;
}
public override void MoveHandle(PointF point, int handleNumber, Keys modifiers)
{
int constraintAngleSubdivisions = 8; // (Constraint by 45° steps).
switch (handleNumber)
{
case 1:
if ((modifiers & Keys.Shift) == Keys.Shift)
{
points["a"] = GeometryHelper.GetPointAtClosestRotationStepCardinal(points["b"], point, constraintAngleSubdivisions);
}
else
{
points["a"] = point;
}
SignalTrackablePointMoved("a");
break;
case 2:
if ((modifiers & Keys.Shift) == Keys.Shift)
{
points["b"] = GeometryHelper.GetPointAtClosestRotationStepCardinal(points["a"], point, constraintAngleSubdivisions);
}
else
{
points["b"] = point;
}
SignalTrackablePointMoved("b");
break;
case 3:
// Move the center of the mini label to the mouse coord.
labelMeasure.SetLabel(point);
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 static void AlignPointParallel(GenericPosture posture, CalibrationHelper calibrationHelper, GenericPostureParallelAlign impact)
{
// The point is moved so that it stays on a segment parallel to another segment.
if (impact == null)
{
return;
}
PointF a = posture.PointList[impact.A];
PointF b = posture.PointList[impact.B];
PointF c = posture.PointList[impact.C];
PointF pointToMove = posture.PointList[impact.PointToMove];
PointF aPlane = calibrationHelper.GetPoint(a);
PointF bPlane = calibrationHelper.GetPoint(b);
PointF cPlane = calibrationHelper.GetPoint(c);
PointF pointPlane = calibrationHelper.GetPoint(pointToMove);
PointF resultPlane = GeometryHelper.GetPointOnParallel(aPlane, bPlane, cPlane, pointPlane);
PointF result = calibrationHelper.GetImagePoint(resultPlane);
posture.PointList[impact.PointToMove] = result;
}
private static void MovePointHandleAlongPerpendicular(GenericPosture posture, CalibrationHelper calibrationHelper, int handle, PointF point, GenericPostureConstraintPerpendicularSlide constraint)
{
if (constraint == null)
{
return;
}
PointF pivot = posture.PointList[constraint.Origin];
PointF leg1 = posture.PointList[constraint.Leg1];
if (pivot == leg1)
{
return;
}
PointF pivotPlane = calibrationHelper.GetPoint(pivot);
PointF leg1Plane = calibrationHelper.GetPoint(leg1);
PointF pointPlane = calibrationHelper.GetPoint(point);
PointF resultPlane = GeometryHelper.GetPointAtAngle(pivotPlane, leg1Plane, pointPlane, 90);
PointF result = calibrationHelper.GetImagePoint(resultPlane);
posture.PointList[posture.Handles[handle].Reference] = result;
}
private static void MovePointHandleAtDistance(GenericPosture posture, int handle, PointF point, GenericPostureConstraintDistanceToPoint constraint, Keys modifiers)
{
if (constraint == null)
{
MovePointHandleFreely(posture, handle, point);
return;
}
PointF parent = posture.PointList[constraint.RefPoint];
PointF child = posture.PointList[posture.Handles[handle].Reference];
float distance = GeometryHelper.GetDistance(parent, child);
PointF temp = GeometryHelper.GetPointAtDistance(parent, point, distance);
int constraintAngleSubdivisions = 8; // (Constraint by 45° steps).
if ((modifiers & Keys.Shift) == Keys.Shift)
{
posture.PointList[posture.Handles[handle].Reference] = GeometryHelper.GetPointAtClosestRotationStepCardinal(parent, temp, constraintAngleSubdivisions);
}
else
{
posture.PointList[posture.Handles[handle].Reference] = temp;
}
}
private PointF GetMiddlePoint()
{
// Used only to attach the measure.
return(GeometryHelper.GetMiddlePoint(points["a"], points["b"]));
}
private static CoordinateSystemGrid FindForPlaneCalibration(CalibrationHelper calibrationHelper)
{
CoordinateSystemGrid grid = new CoordinateSystemGrid();
RectangleF imageBounds = new RectangleF(PointF.Empty, calibrationHelper.ImageSize);
RectangleF clipWindow = imageBounds;
CalibrationPlane calibrator = calibrationHelper.CalibrationByPlane_GetCalibrator();
RectangleF plane = new RectangleF(PointF.Empty, calibrator.Size);
int targetSteps = 15;
float stepVertical = 1.0f;
float stepHorizontal = 1.0f;
// The extended plane is used for vanishing point replacement and iteration stop condition.
QuadrilateralF extendedPlane;
bool orthogonal;
if (!calibrator.QuadImage.IsRectangle)
{
// If perspective plane, define as 2n times the nominal plane centered on origin.
orthogonal = false;
float n = 4;
PointF a = new PointF(-calibrator.Size.Width * n, calibrator.Size.Height * n);
PointF b = new PointF(calibrator.Size.Width * n, calibrator.Size.Height * n);
PointF c = new PointF(calibrator.Size.Width * n, -calibrator.Size.Height * n);
PointF d = new PointF(-calibrator.Size.Width * n, -calibrator.Size.Height * n);
extendedPlane = new QuadrilateralF(a, b, c, d);
QuadrilateralF quadImage = calibrator.QuadImage;
float projectedWidthLength = GeometryHelper.GetDistance(quadImage.A, quadImage.B);
float scaledTargetHorizontal = targetSteps / (calibrationHelper.ImageSize.Width / projectedWidthLength);
stepHorizontal = RangeHelper.FindUsableStepSize(plane.Width, scaledTargetHorizontal);
float projectedHeightLength = GeometryHelper.GetDistance(quadImage.A, quadImage.D);
float scaledTargetVertical = targetSteps / (calibrationHelper.ImageSize.Height / projectedHeightLength);
stepVertical = RangeHelper.FindUsableStepSize(plane.Height, scaledTargetVertical);
}
else
{
// If flat plane (and no distortion) we know there is no way to get any vanishing point inside the image,
// so we can safely use the whole image reprojection as an extended plane.
orthogonal = true;
QuadrilateralF quadImageBounds = new QuadrilateralF(imageBounds);
PointF a = calibrationHelper.GetPointFromRectified(quadImageBounds.A);
PointF b = calibrationHelper.GetPointFromRectified(quadImageBounds.B);
PointF c = calibrationHelper.GetPointFromRectified(quadImageBounds.C);
PointF d = calibrationHelper.GetPointFromRectified(quadImageBounds.D);
extendedPlane = new QuadrilateralF(a, b, c, d);
float width = extendedPlane.B.X - extendedPlane.A.X;
stepHorizontal = RangeHelper.FindUsableStepSize(width, targetSteps);
float height = extendedPlane.A.Y - extendedPlane.D.Y;
stepVertical = RangeHelper.FindUsableStepSize(height, targetSteps);
}
//-------------------------------------------------------------------------------------------------
// There is a complication with points behind the camera, as they projects above the vanishing line.
// The general strategy is the following:
// Find out if the vanishing point is inside the image. Reminder: parallel lines share the same vanishing point.
// If it is not inside the image, there is no risk, so we take two points on the line and draw an infinite line that we clip against the image bounds.
// If it is inside the image:
// Take two points on the line and project them in image space. They are colinear with the vanishing point in image space.
// Find on which side of the quadrilateral the vanishing point is.
// If the vanishing point is above the quad, draw a ray from the top of the extended quad, down to infinity.
// If the vanishing point is below the quad, draw a ray from the bottom of the extended quad, up to infinity.
//
// Stepping strategy:
// We start at origin and progress horizontally and vertically until we find a gridline that is completely clipped out.
//-------------------------------------------------------------------------------------------------
// Vertical lines.
PointF yVanish = new PointF(0, float.MinValue);
bool yVanishVisible = false;
Vector3 yv = calibrator.Project(new Vector3(0, 1, 0));
if (yv.Z != 0)
{
yVanish = new PointF(yv.X / yv.Z, yv.Y / yv.Z);
yVanishVisible = clipWindow.Contains(yVanish);
}
CreateVerticalGridLines(grid, 0, -stepHorizontal, calibrator, clipWindow, plane, extendedPlane, orthogonal, yVanishVisible, yVanish);
CreateVerticalGridLines(grid, stepHorizontal, stepHorizontal, calibrator, clipWindow, plane, extendedPlane, orthogonal, yVanishVisible, yVanish);
// Horizontal lines
PointF xVanish = new PointF(float.MinValue, 0);
bool xVanishVisible = false;
Vector3 xv = calibrator.Project(new Vector3(1, 0, 0));
if (xv.Z != 0)
{
xVanish = new PointF(xv.X / xv.Z, xv.Y / xv.Z);
xVanishVisible = clipWindow.Contains(xVanish);
}
CreateHorizontalGridLines(grid, 0, -stepVertical, calibrator, clipWindow, plane, extendedPlane, orthogonal, xVanishVisible, xVanish);
CreateHorizontalGridLines(grid, stepVertical, stepVertical, calibrator, clipWindow, plane, extendedPlane, orthogonal, xVanishVisible, xVanish);
return(grid);
}
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);
}
}
}
