private static Vertex CreateDefault(Obj originalObj, StraightLine segm, int colorsCount, int den)
{
Vertex defaultPoint;
if (den == 1)
{
defaultPoint = new Vertex(originalObj.Vertices[segm.Number1].Coordinates.X,
originalObj.Vertices[segm.Number1].Coordinates.Y,
originalObj.Vertices[segm.Number1].Coordinates.Z,
colorsCount + 1,
originalObj.Vertices.Count);
originalObj.Vertices.Add(defaultPoint);
originalObj.Normals.Add(new Normal(originalObj.Vertices[segm.Number1].Coordinates.X,
originalObj.Vertices[segm.Number1].Coordinates.Y,
originalObj.Vertices[segm.Number1].Coordinates.Z,
defaultPoint.Number));
}
else
{
defaultPoint = new Vertex(originalObj.Vertices[segm.Number2].Coordinates.X,
originalObj.Vertices[segm.Number2].Coordinates.Y,
originalObj.Vertices[segm.Number2].Coordinates.Z,
colorsCount + 1,
originalObj.Vertices.Count);
originalObj.Vertices.Add(defaultPoint);
originalObj.Normals.Add(new Normal(originalObj.Vertices[segm.Number2].Coordinates.X,
originalObj.Vertices[segm.Number2].Coordinates.Y,
originalObj.Vertices[segm.Number2].Coordinates.Z,
defaultPoint.Number));
}
return(defaultPoint);
}
//Find the line of intersection between two planes.
//The inputs are two game objects which represent the planes.
//The outputs are a point on the line and a vector which indicates it's direction.
internal static bool PlanesIntersection(Plane3D plane1, Plane3D plane2, out StraightLine intersection)
{
var linePoint = new WM.Point3D();
var lineVec = new WM.Vector3D();
//Get the normals of the planes.
var plane1Normal = plane1.NormalVector;
var plane2Normal = plane2.NormalVector;
//We can get the direction of the line of intersection of the two planes by calculating the
//cross product of the normals of the two planes. Note that this is just a direction and the line
//is not fixed in space yet.
lineVec = WM.Vector3D.CrossProduct(plane1Normal, plane2Normal);
//Next is to calculate a point on the line to fix it's position. This is done by finding a vector from
//the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
//errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
//the cross product of the normal of plane2 and the lineDirection.
var ldir = WM.Vector3D.CrossProduct(plane2Normal, lineVec);
var numerator = WM.Vector3D.DotProduct(plane1Normal, ldir);
//Prevent divide by zero.
if (Math.Abs(numerator) > 0.000001)
{
var plane1ToPlane2 = plane1.PointOnPlane - plane2.PointOnPlane;
var t = WM.Vector3D.DotProduct(plane1Normal, plane1ToPlane2) / numerator;
linePoint = plane2.PointOnPlane + t * ldir;
intersection = new StraightLine(ref linePoint, ref lineVec);
return(true);
}
intersection = default(StraightLine);
return(false);
}
public static Point DistanceFromStart(this StraightLine line, double distance)
{
double x = line.StartPoint.X + (distance * line.NormalizedVector).X;
double y = line.StartPoint.Y + (distance * line.NormalizedVector).Y;
return(new Point((int)x, (int)y));
}
public static StraightLine From2Point(Vector2 a, Vector2 b)
{
StraightLine l = new StraightLine ();
l.point = a;
l.tangent = (b - a).normalized;
return l;
}
public static StraightLine FromPointAndDirection(Vector2 pt, Vector2 direction)
{
StraightLine l = new StraightLine ();
l.point = pt;
l.tangent = direction.normalized;
return l;
}
public static void NeighboursAndTJP(Informer start, Informer finish,
Node[,] nodesArray, out DebugInformationAlgorithm debugInfo)
{
var startNode = new Node(start, NodeState.Processed);
startNode.NormMatrix = nodesArray[startNode.X(), startNode.Y()].NormMatrix;
startNode.Distance = MetricsAStar(start, finish);
var startTreeNode = new Tree_Node(null, startNode);
var finishNode = new Node(finish, NodeState.Processed);
var sraightLinesFromStart = new StraightLinesFromNode(startNode);
var sraightLinesFromFinish = new StraightLinesFromNode(finishNode);
var neighbours = Neighbours(startTreeNode, nodesArray, finishNode);
var minMetrics = startNode.Distance;
var tempList = new List <Node>();
if (sraightLinesFromStart.Lines != null)
{
foreach (var lineFromFinish in sraightLinesFromFinish.Lines)
{
foreach (var line in sraightLinesFromStart.Lines)
{
var coordinates = StraightLine.Crossing(line, lineFromFinish);
if (coordinates != null && Reachable(nodesArray[coordinates.X, coordinates.Y], finishNode, nodesArray) &&
Reachable(startNode, nodesArray[coordinates.X, coordinates.Y], nodesArray))
{
var tempNode = new Node(nodesArray[coordinates.X, coordinates.Y]);
tempNode.Distance = Metrics(new Tree_Node(startTreeNode, tempNode), finishNode);
tempNode.TargetJP = true;
tempNode.DestinationToFinish = DestinationInverse(lineFromFinish.Destination);
tempNode.Visited = NodeState.Discovered;
if (tempNode.Distance < minMetrics)
{
minMetrics = tempNode.Distance;
tempList.Clear();
tempList.Add(tempNode);
}
else if (Math.Abs(tempNode.Distance - minMetrics) < 0.00000000001)
{
tempList.Add(tempNode);
}
}
}
}
}
var straightLines = StraightLinesFromNode.JoinLines(sraightLinesFromStart, sraightLinesFromFinish,
nodesArray);
debugInfo = new DebugInformationAlgorithm
{
From = startNode.InformerNode,
To = finishNode.InformerNode,
Observed = ToNodes(neighbours),
LinesFromFinish = straightLines,
CrossPoints = tempList,
Destroy = false
};
}
public void setup()
{
List <double> args = new List <double>();
args.Add(0);
args.Add(0);
args.Add(1);
args.Add(0);
test = new StraightLine(args);
List <double> args1 = new List <double>();
args1.Add(0);
args1.Add(1);
args1.Add(1);
args1.Add(2);
straightLine = new StraightLine(args1);
List <double> args2 = new List <double>();
args2.Add(0);
args2.Add(1);
args2.Add(1);
args2.Add(2);
rayLine1 = new RayLine(args2);
List <double> args3 = new List <double>();
args3.Add(1);
args3.Add(2);
args3.Add(0);
args3.Add(1);
rayLine2 = new RayLine(args3);
List <double> args4 = new List <double>();
args4.Add(1);
args4.Add(2);
args4.Add(0);
args4.Add(1);
lineSegment = new LineSegment(args4);
List <double> args5 = new List <double>();
args5.Add(0);
args5.Add(1);
args5.Add(1);
circle = new Circle(args5);
List <double> args6 = new List <double>();
args6.Add(1);
args6.Add(1);
args6.Add(1);
args6.Add(2);
straightLine1 = new StraightLine(args6);
}
public bool isOverlap(StraightLine line)
{
//return IsOverlap();
//for (int i = 0; i < poly.points.Length; i++) {
// if (this.IsOverlap(poly.points[i]))
// return true;
//}
return false;
}
public override AFigure Create(Point start, Point end, Color colorLine, Color fillColor, int width)
{
StraightLine line = new StraightLine();
figure = line;
base.Create(start, end, colorLine, fillColor, width);
line._centerPoint = start;
return(line);
}
public void TestDistanceAEqualToZero()
{
//Arrange
Point point = new Point(1, 1);
StraightLine aDLine = new StraightLine(0, -1, 10);
//Act
Distance(point, aDLine, out Point result);
//Assert
Assert.AreEqual(1, result.X);
}
/// <summary>
/// Переход от параметризации к уравнению прямой.
/// </summary>
/// <param name="straightLine">Параметризация прямой: расстояние до неё и угол отклонения перпендикуляра к ней</param>
private static EquationLine MakeEquationLine(StraightLine straightLine)
{
var x1 = (int)(straightLine.Distance * Math.Cos(straightLine.Angle));
var y1 = (int)(straightLine.Distance * Math.Sin(straightLine.Angle));
var normVector = new Point(x1, y1);
var x2 = (int)(x1 + Math.Cos(straightLine.Angle + Math.PI / 2) * 20);
var y2 = (int)(y1 + Math.Sin(straightLine.Angle + Math.PI / 2) * 20);
var point2 = new Point(x2, y2);
return(new EquationLine(normVector, point2, TypeEquation.NormVector));
}
public bool IsIntersecting(LineSegment secondLine)
{
double a, b, c;
if (secondLine.StartPoint.X != secondLine.EndPoint.X)
{
a = (secondLine.StartPoint.Y - secondLine.EndPoint.Y) / (secondLine.StartPoint.X - secondLine.EndPoint.X);
c = secondLine.StartPoint.Y - a * secondLine.StartPoint.X;
b = -1;
}
else
{
a = 1;
c = -secondLine.StartPoint.X;
b = 0;
}
StraightLine secondStraightLine = new StraightLine(a, b, c);
if (StartPoint.X != EndPoint.X)
{
a = (StartPoint.Y - EndPoint.Y) / (StartPoint.X - EndPoint.X);
c = StartPoint.Y - a * StartPoint.X;
b = -1;
}
else
{
a = 1;
c = -StartPoint.X;
b = 0;
}
StraightLine currentStraightLine = new StraightLine(a, b, c);
if (Mathematics.TryGetIntersection(currentStraightLine, secondStraightLine, out Point intersectionPoint))
{
if (StartPoint.X - EndPoint.X != 0)
{
if (intersectionPoint.X < Math.Max(StartPoint.X, EndPoint.X) && intersectionPoint.X > Math.Min(StartPoint.X, EndPoint.X) &&
intersectionPoint.Y < Math.Max(secondLine.StartPoint.Y, secondLine.EndPoint.Y) && intersectionPoint.Y > Math.Min(secondLine.StartPoint.Y, secondLine.EndPoint.Y))
{
return(true);
}
}
else
{
if (intersectionPoint.Y < Math.Max(StartPoint.Y, EndPoint.Y) && intersectionPoint.Y > Math.Min(StartPoint.Y, EndPoint.Y) &&
intersectionPoint.Y < Math.Max(secondLine.StartPoint.X, secondLine.EndPoint.X) && intersectionPoint.X > Math.Min(secondLine.StartPoint.X, secondLine.EndPoint.X))
{
return(true);
}
}
}
return(false);
}
public void LineGetPointsTest(int i, int x1, int y1, int x2, int y2)
{
StraightLine figure = new StraightLine();
lineMoks lineMoks = new lineMoks();
figure._startPoint = new Point(x1, y1);
figure._endPoint = new Point(x2, y2);
List <Point> Exp = lineMoks.Get(i);
List <Point> Act = figure.GetPoints();
CollectionAssert.AreEqual(Exp, Act);
}
public Vector2 Intersection(StraightLine other)
{
if (slope == other.slope)
{
return(new Vector2());
}
float X = B * other.C - other.B * C;
float Y = A * other.C - other.A * C;
float Z = B * other.A - A * other.B;
return(new Vector2(X / Z, Y / Z));
}
public override bool CheckResult(string answer)
{
var answers = answer.ToInts();
if (answers.Length != 3)
{
return(false);
}
var answerLine = new StraightLine(answers[0], answers[1], answers[2]);
return(Line.IsNormalTo(answerLine) && answerLine.IsPointOnLine(_point));
}
public static List <Point> RemoveStraightEdges(IEnumerable <Point> points, float maxDeviation = 5.0f)
{
List <Point> result = points.ToList();
if (result.Count <= 2)
{
return(result);
}
StraightLine line = new StraightLine(result[0], result[1]);
List <Point> pointsToRemove = new List <Point>();
int startIndex = 0;
int endIndex = 1;
for (int i = 2; i < result.Count; i++)
{
Point actualPoint = result[i];
Point expectedPoint = new Point(actualPoint.X, (int)line.Result(actualPoint.X));
float delta = actualPoint.Distance(expectedPoint);
if (delta < maxDeviation)
{
//We have a straight line
endIndex = i;
}
else
{
//No straight line
if (endIndex - startIndex > 2)
{
for (int j = startIndex; j <= endIndex; j++)
{
pointsToRemove.Add(result[j]);
}
}
startIndex = i - 1;
endIndex = i;
line = new StraightLine(result[startIndex], result[endIndex]);
}
}
foreach (var point in pointsToRemove)
{
result.Remove(point);
}
return(result);
}
private void AppBarButton_Click_Add(object sender, RoutedEventArgs e)
{
// todo: maybe let user input added object?
List <double> args = new List <double>();
Random rnd = new Random();
args.Add(rnd.Next(0, 1000));
args.Add(rnd.Next(0, 1000));
args.Add(rnd.Next(0, 1000));
args.Add(rnd.Next(0, 1000));
SimpleObject obj = new StraightLine(args);
Add(obj);
simpleObjects.Add(obj);
}
private static Vertex GetCenter(MinAndMaxVertex minAndMaxVertex)
{
var minXLine = new StraightLine(minAndMaxVertex.MinX, new Vertex(minAndMaxVertex.MinX.Coordinates.X, minAndMaxVertex.MinX.Coordinates.Y - 10, minAndMaxVertex.MinX.Coordinates.Z));
var maxXLine = new StraightLine(minAndMaxVertex.MaxX, new Vertex(minAndMaxVertex.MaxX.Coordinates.X, minAndMaxVertex.MaxX.Coordinates.Y - 10, minAndMaxVertex.MaxX.Coordinates.Z));
var minYLine = new StraightLine(minAndMaxVertex.MinY, new Vertex(minAndMaxVertex.MinY.Coordinates.X - 10, minAndMaxVertex.MinY.Coordinates.Y, minAndMaxVertex.MinX.Coordinates.Z));
var maxYLine = new StraightLine(minAndMaxVertex.MaxY, new Vertex(minAndMaxVertex.MaxX.Coordinates.X - 10, minAndMaxVertex.MaxY.Coordinates.Y, minAndMaxVertex.MaxX.Coordinates.Z));
var minXAndminYVertex = Vertex.IntersectionPointOfTwoLines2D(minXLine, minYLine);
var minXAndMaxYVertex = Vertex.IntersectionPointOfTwoLines2D(minXLine, maxYLine);
var maxXAndminYVertex = Vertex.IntersectionPointOfTwoLines2D(maxXLine, minYLine);
var maxXAndMaxYVertex = Vertex.IntersectionPointOfTwoLines2D(maxXLine, maxYLine);
var center = Vertex.IntersectionPointOfTwoLines2D(minXAndMaxYVertex, maxXAndminYVertex, minXAndminYVertex, maxXAndMaxYVertex);
return(center);
}
/// <summary>
/// determine whether the point is above or below a line
/// </summary>
/// <param name="line"></param>
/// <param name="ptPoint"></param>
/// <returns></returns>
public static AboveBelowLine PointAboveOrBelowtheLine(StraightLine line, IPoint ptPoint)
{
double Y0 = line.k * ptPoint.X + line.b;
if (ptPoint.Y > Y0)
{
return(AboveBelowLine.ABOVE);
}
else if (ptPoint.Y < Y0)
{
return(AboveBelowLine.BELOW);
}
else
{
return(AboveBelowLine.ONTHELINE);
}
}
static void Main(string[] args)
{
Line straightLine = new StraightLine();
Line circle = new Circle();
Circle circle2 = new Circle();
// Calls "Line.Intersect(Line)", and correctly
// prints "Circle intersecting a straight line.".
Console.WriteLine(circle.Intersect(straightLine));
// Also calls "Line.Intersect(Line)",
// since the argument's compile-time type is "Line".
Console.WriteLine(circle2.Intersect(straightLine));
// Calls "Line.Intersect(Circle)",
// since the argument's compile-time type is "Circle".
// At runtime, the call will be resolved to
// "StraightLine.Intersect(Circle)" via single dispatch.
Console.WriteLine(straightLine.Intersect(circle2));
Console.ReadLine();
}
/* Función estática que devuelve el punto de intersección de
* dos rectas que se le pasan. */
public static Vector CalculateIntersection(StraightLine line1, StraightLine line2)
{
/* Calculo el punto x de intersección de ambas. */
double xIntersection;
if (line1.m != line2.m)
{
xIntersection = (line2.b - line1.b) / (line1.m - line2.m);
}
else
{
/* Dos rectas paralelas no se cortan, devolvemos nan. */
return(new Vector(double.NaN, double.NaN));
}
/* Calculo la coordenada y de la intersección. */
double yIntersection = line1.m * xIntersection + line1.b;
return(new Vector(xIntersection, yIntersection));
}
public static Point FindMouseNoseTip(IEnumerable <Point> points, IEnumerable <Point> contourPoints)
{
List <Point> headPoints = points.ToList();
List <Point> allContourPoints = contourPoints.ToList();
StraightLine vectorPointsLine = new StraightLine(headPoints[1], headPoints[3]);
PointF closestPointToNose;
double distance = vectorPointsLine.FindDistanceToSegment(headPoints[2], out closestPointToNose);
Point closestIntPoint = new Point((int)closestPointToNose.X, (int)closestPointToNose.Y);
StraightLine direction = new StraightLine(closestIntPoint, headPoints[2]);
float targetExtensionDistance = 3;
Point targetLinePoint1 = new Point((int)(vectorPointsLine.StartPoint.X + (direction.NormalizedVector * distance * targetExtensionDistance).X), (int)(vectorPointsLine.StartPoint.Y + (direction.NormalizedVector * distance * targetExtensionDistance).Y));
Point targetLinePoint2 = new Point((int)(vectorPointsLine.EndPoint.X + (direction.NormalizedVector * distance * targetExtensionDistance).X), (int)(vectorPointsLine.EndPoint.Y + (direction.NormalizedVector * distance * targetExtensionDistance).Y));
StraightLine targetLine = new StraightLine(targetLinePoint1, targetLinePoint2);
int numberOfTargets = 5;
float offset = targetLine.GetMagnitude() / numberOfTargets;
float finalDist = 10000;
Point nosePoint = new Point();
for (float currentTargetLength = 0; currentTargetLength <= targetLine.GetMagnitude(); currentTargetLength += offset)
{
Point currentTargetPoint = targetLine.DistanceFromStart(currentTargetLength);
foreach (Point point in allContourPoints)
{
float dist = currentTargetPoint.Distance(point);
if (dist < finalDist)
{
finalDist = dist;
nosePoint = point;
}
}
}
return(nosePoint);
}
public static double Distance(Point point, StraightLine line, out Point closestPoint)
{
if (line.B != 0 && line.A != 0)
{
StraightLine perpendicular = new StraightLine(-(1 / line.A), -1, point.Y + (point.X / line.A));
Util.Mathematics.TryGetIntersection(line, perpendicular, out closestPoint);
return(Math.Abs(line.A * point.X - point.Y + line.C) / Math.Sqrt(Math.Pow(line.A, 2) + 1));
}
else if (line.B != 0 && line.A == 0)
{
StraightLine perpendicular = new StraightLine(1, 0, -point.X);
Util.Mathematics.TryGetIntersection(line, perpendicular, out closestPoint);
return(Math.Abs(point.Y - line.C));
}
else
{
StraightLine perpendicular = new StraightLine(0, -1, point.Y);
Util.Mathematics.TryGetIntersection(line, perpendicular, out closestPoint);
return(Math.Abs(point.X - line.C));
}
}
// intersction entre deux droite du plan
public static IntersectionResult FindIntersection(StraightLine l0, StraightLine l1)
{
Vector2 delta = l1.point - l0.point;
Vector3 crossVector = Vector3.Cross (l0.tangent, l1.tangent);
IntersectionResult ir = new IntersectionResult ();
if (!Utils.Approximately (crossVector.z, 0))
{
// unique intersection point
ir.intersectionType = IntersectionType.UniqueIntersection;
ir.intersectionCount = 1;
Vector3 delta1CrossVector = Vector3.Cross (delta, l1.tangent);
float l0Factor = delta1CrossVector.z / crossVector.z;
Vector2 intersectedPoint = l0.point + l0Factor * l0.tangent;
ir.intersectedPoints = new Vector2 [1];
ir.intersectedPoints[0] = intersectedPoint;
}
else
{
Vector3 delta0CrossVector = Vector3.Cross (l0.tangent, delta);
if (Utils.Approximately (delta0CrossVector.z, 0))
{
// infinitely many points
ir.intersectionType = IntersectionType.InfinityIntersections;
ir.intersectionCount = -1;
}
else
{
// lines are parallel, no intersection
ir.intersectionType = IntersectionType.NoIntersection;
ir.intersectionCount = 0;
}
}
return ir;
}
public static bool Reachable(Node from, Node to, Node[,] nodesArray)
{
if (from.InformerNode.IsObstacle)
{
return(false);
}
var pointsBetween = StraightLine.FindMiddlePoints(from, to);
var destination = StraightLine.FindDestination(from, to);
pointsBetween.Remove(new Point(from.X(), from.Y()));
pointsBetween.Remove(new Point(to.X(), to.Y()));
foreach (var point in pointsBetween)
{
if (destination == Destinations.UpRight &&
nodesArray[point.X, point.Y].NormMatrix[0, 2] == 0)
{
return(false);
}
if (destination == Destinations.UpLeft &&
nodesArray[point.X, point.Y].NormMatrix[0, 0] == 0)
{
return(false);
}
if (destination == Destinations.DownRight &&
nodesArray[point.X, point.Y].NormMatrix[2, 2] == 0)
{
return(false);
}
if (destination == Destinations.DownLeft &&
nodesArray[point.X, point.Y].NormMatrix[2, 0] == 0)
{
return(false);
}
if (nodesArray[point.X, point.Y].InformerNode.IsObstacle)
{
return(false);
}
}
return(true);
}
public static StraightLine CreateBisector(KeyPoint firstKeyPoint, KeyPoint secondKeyPoint)
{
double a, b, c;
Point midPoint = new Point((firstKeyPoint.X + secondKeyPoint.X) / 2, (firstKeyPoint.Y + secondKeyPoint.Y) / 2);
if (firstKeyPoint.X != secondKeyPoint.X)
{
a = (firstKeyPoint.Y - secondKeyPoint.Y) / (firstKeyPoint.X - secondKeyPoint.X);
c = firstKeyPoint.Y - a * firstKeyPoint.X;
b = -1;
StraightLine line = new StraightLine(a, b, c);
return(new StraightLine(-(1 / line.A), -1, midPoint.Y + (midPoint.X / line.A)));
}
else
{
a = 1;
c = -firstKeyPoint.X;
b = 0;
StraightLine line = new StraightLine(a, b, c);
return(new StraightLine(0, -1, midPoint.Y));
}
}
public void GoalBounding()
{
var controller = GetComponentInChildren <Controller>();
if (!controller.IsPrecomputed)
{
controller.PrecomputeMap();
}
if (StartInformer != null && FinishInformer != null)
{
var startNode = controller.NodesArray[(int)StartInformer.transform.position.x / 3,
(int)StartInformer.transform.position.z / 3];
var goalNode = controller.NodesArray[(int)FinishInformer.transform.position.x / 3,
(int)FinishInformer.transform.position.z / 3];
var destination = StraightLine.FindDestination(startNode, goalNode);
var minRow = startNode.GoalBounds[(int)destination - 1, 0];
var maxRow = startNode.GoalBounds[(int)destination - 1, 1];
var minCol = startNode.GoalBounds[(int)destination - 1, 2];
var maxCol = startNode.GoalBounds[(int)destination - 1, 3];
Debug.Log("Bounding Box: Destination = " + destination + " MinRow = " + minRow + " MaxRow = " + maxRow +
" MinCol = " + minCol
+ " MaxCol = " + maxCol);
for (var i = minRow; i < maxRow; ++i)
{
for (var j = minCol; j < maxCol; ++j)
{
var renderer = controller.NodesArray[i, j].InformerNode.GetComponent <Renderer>();
renderer.material.SetColor("_Color", Color.green);
}
}
}
else
{
Debug.LogError("Enter proper arguments");
}
}
/// <summary>
/// Поиск самой длинной линии на изображении.
/// </summary>
/// <param name="houghSpace">Пространство Хафа(фазовое пространство параметров: длина перпендикуляра до прямой и угол отклонения перпендикуляра)</param>
private static StraightLine GetMaxStraightLine(HoughSpace houghSpace)
{
var max = new StraightLine(0, 0, 0);
var midDistance = houghSpace.DiagonalImage;
var midStep = houghSpace.Size.Width / 2;
var pRes = Parallel.For(0, houghSpace.Accumulator.GetLength(0), angle =>
{
for (int distance = 0; distance < houghSpace.Accumulator.GetLength(1); distance++)
{
var vote = houghSpace.Accumulator[angle, distance];
if (vote > max.Vote && distance - midDistance > 0)
{
max = new StraightLine(distance - midDistance, (angle - midStep) * HoughSpace.stepForAngle + HoughSpace.shiftForAngle, vote);
}
}
});
if (!pRes.IsCompleted)
{
throw new Exception("Parallel error");
}
return(max);
}
/// <summary>
/// Gets the straight lines
/// </summary>
private StraightLine[] GetLines(DataPoint[] dataPoints)
{
var lines = new List <StraightLine>();
for (var i = 1; i < dataPoints.Length; i++)
{
var curve = new StraightLine(
dataPoints[i - 1],
dataPoints[i],
dataPoints[dataPoints.Length - 1]);
lines.Add(curve);
}
//if (style.HasFill)
//{
// var bottomLeft = new DataPoint(TopLeftX, BottomRightY);
// var bottomRight = new DataPoint(BottomRightX, BottomRightY);
// lines.Add(new StraightLine(
// lines.First().StartPoint,
// bottomLeft,
// bottomLeft));
// lines.Add(new StraightLine(
// bottomLeft,
// bottomRight,
// bottomRight));
// lines.Add(new StraightLine(
// bottomRight,
// lines.Last().EndPointPoint,
// lines.Last().EndPointPoint));
//}
return(lines.ToArray());
}
// calcul des donnees : angle, bissectrice, normales en fonction de sa position et de la position des points voisins
public virtual void Update(bool recursive=true)
{
if (_prevEdge != null && _nextEdge != null)
{
_prevEdge.Update ();
_nextEdge.Update ();
Vector2 prevNormal = _prevEdge.GetNormal ();
Vector2 nextNormal = _nextEdge.GetNormal ();
Vector2 nextEdgeDirection = _nextEdge.ToVector2 ();
_normal = (prevNormal + nextNormal).normalized;
_angle = 180 - Vector2.Angle (prevNormal, nextNormal);
Vector2 prevDirection = _prevEdge.ToVector2 ().normalized * -1;
Vector2 nextDirection = _nextEdge.ToVector2 ().normalized;
Vector2 abp = (Vector2)this + (prevDirection + nextDirection);
_angularBisector = StraightLine.From2Point (this, abp);
float prevNormalNextEdgeDot = Vector2.Dot (prevNormal, nextEdgeDirection);
if (Utils.Approximately (prevNormalNextEdgeDot, 0))
{
_angleType = AngleType.None;
}
else if (prevNormalNextEdgeDot > 0)
{
_angleType = AngleType.Inside;
}
else if (prevNormalNextEdgeDot < 0)
{
_angleType = AngleType.Outside;
}
else
{
_angleType = AngleType.None;
}
if (recursive)
{
_prevEdge.GetPrevPoint2 ().Update (false);
_nextEdge.GetNextPoint2 ().Update (false);
}
}
else if (_prevEdge != null && _nextEdge == null)
{
_prevEdge.Update ();
_angularBisector = StraightLine.FromPointAndDirection (this, Vector2.zero);
_normal = _prevEdge.GetNormal ();
_angle = float.NaN;
_angleType = AngleType.None;
if (recursive)
{
_prevEdge.GetPrevPoint2 ().Update (false);
}
}
else if (_prevEdge == null && _nextEdge != null)
{
_nextEdge.Update ();
_angularBisector = StraightLine.FromPointAndDirection (this, Vector2.zero);
_normal = _nextEdge.GetNormal ();
_angle = float.NaN;
_angleType = AngleType.None;
if (recursive)
{
_nextEdge.GetNextPoint2 ().Update (false);
}
}
else
{
_angularBisector = StraightLine.FromPointAndDirection (this, Vector2.zero);
_normal = Vector2.zero;
_angle = float.NaN;
_angleType = AngleType.None;
}
_calculatedNormal = _normal;
_normalScaleFactor = 1;
if (!float.IsNaN (_angle))
{
float radAngle = _angle * Mathf.Deg2Rad;
float sinus = Mathf.Sin (radAngle / 2);
if (sinus != 0)
{
_calculatedNormal = (1 / sinus) * _normal;
_normalScaleFactor = 1 + (_calculatedNormal.magnitude - 1);
//Debug.Log (_angle + " " + sinus + " " + _normalScaleFactor);
}
else
{
_calculatedNormal = _normal;
_normalScaleFactor = 1;
}
}
}
protected IList <IStatement> Schedule(IList <IStatement> isc)
{
List <StatementBlock> blocks = new List <StatementBlock>();
StatementBlock currentBlock = new StraightLine();
Dictionary <NodeIndex, StatementBlock> blockOfNode = new Dictionary <NodeIndex, StatementBlock>();
int firstIterPostBlockCount = 0;
IConditionStatement firstIterPostStatement = null;
// must include back edges for computing InitializerSets
DependencyGraph2 g = new DependencyGraph2(context, isc, DependencyGraph2.BackEdgeHandling.Include,
delegate(IWhileStatement iws)
{
blocks.Add(currentBlock);
currentBlock = new Loop(iws);
},
delegate(IWhileStatement iws)
{
blocks.Add(currentBlock);
currentBlock = new StraightLine();
},
delegate(IConditionStatement ics)
{
firstIterPostBlockCount++;
firstIterPostStatement = ics;
},
delegate(IConditionStatement ics)
{
firstIterPostBlockCount--;
},
delegate(IStatement ist, int index)
{
if (firstIterPostBlockCount > 0)
{
((Loop)currentBlock).firstIterPostBlock.Add(index);
}
currentBlock.indices.Add(index);
blockOfNode[index] = currentBlock;
});
var dependencyGraph = g.dependencyGraph;
blocks.Add(currentBlock);
Set <NodeIndex> usedNodes = Set <NodeIndex> .FromEnumerable(g.outputNodes);
Set <NodeIndex> usedBySelf = new Set <NodeIndex>();
// loop blocks in reverse order
for (int i = blocks.Count - 1; i >= 0; i--)
{
StatementBlock block = blocks[i];
if (block is Loop loop)
{
if (!pruneDeadCode)
{
usedNodes = CollectUses(dependencyGraph, block.indices);
}
else
{
usedBySelf.Clear();
block.indices = PruneDeadNodesCyclic(g, block.indices, usedNodes, usedBySelf, out List <int> tailStmts); // modifies usedNodes
loop.tail = tailStmts;
}
RemoveSuffix(block.indices, loop.firstIterPostBlock);
}
else
{
// StraightLine
if (pruneDeadCode)
{
block.indices = PruneDeadNodes(g, block.indices, usedNodes, usedBySelf); // modifies usedNodes
}
}
AddLoopInitializers(block, usedNodes, blockOfNode, g);
}
IList <IStatement> sc = Builder.StmtCollection();
foreach (StatementBlock block in blocks)
{
if (block is Loop loop)
{
context.OpenStatement(loop.loopStatement);
IWhileStatement ws = Builder.WhileStmt(loop.loopStatement);
context.SetPrimaryOutput(ws);
IList <IStatement> sc2 = ws.Body.Statements;
foreach (NodeIndex i in loop.indices)
{
IStatement st = ConvertStatement(g.nodes[i]);
sc2.Add(st);
}
context.CloseStatement(loop.loopStatement);
context.InputAttributes.CopyObjectAttributesTo(loop.loopStatement, context.OutputAttributes, ws);
sc.Add(ws);
List <IStatement> initStmts = new List <IStatement>();
initStmts.AddRange(loop.initializers);
if (loop.firstIterPostBlock.Count > 0)
{
var firstIterPostStatements = loop.firstIterPostBlock.Select(i => g.nodes[i]);
var thenBlock = Builder.BlockStmt();
ConvertStatements(thenBlock.Statements, firstIterPostStatements);
var firstIterPostStmt = Builder.CondStmt(firstIterPostStatement.Condition, thenBlock);
context.OutputAttributes.Set(firstIterPostStmt, new FirstIterationPostProcessingBlock());
sc2.Add(firstIterPostStmt);
loopMergingInfo.AddNode(firstIterPostStmt);
}
context.OutputAttributes.Remove <InitializerSet>(ws);
context.OutputAttributes.Set(ws, new InitializerSet(initStmts));
if (loop.tail != null)
{
foreach (NodeIndex i in loop.tail)
{
IStatement st = g.nodes[i];
sc.Add(st);
}
}
}
else
{
foreach (NodeIndex i in block.indices)
{
IStatement st = ConvertStatement(g.nodes[i]);
sc.Add(st);
}
}
}
return(sc);
}
/*public override void Update (bool recursive=true)
{
base.Update (recursive);
if (_prevEdge != null && _nextEdge != null)
{
_prevTangent = Line.FromPointAndDirection (this, _prevEdge.ToVector2 () * -1);
_nextTangent = Line.FromPointAndDirection (this, _nextEdge.ToVector2 ());
// find the farthest center position
float prevLength = _prevEdge.GetLength ();
Point2 prevPoint = _prevEdge.GetPrevPoint2 ();
Vector2 prevPointNextTangentPoint = prevPoint;
if (prevPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aPrevPoint = _prevEdge.GetPrevPoint2 () as ArchedPoint2;
prevPointNextTangentPoint = aPrevPoint.GetNextTangentPoint ();
prevLength = Vector2.Distance (prevPointNextTangentPoint, this);
}
float nextLength = _nextEdge.GetLength ();
Point2 nextPoint = _nextEdge.GetNextPoint2 ();
Vector2 nextPointPrevTangentPoint = nextPoint;
if (nextPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aNextPoint = _nextEdge.GetPrevPoint2 () as ArchedPoint2;
nextPointPrevTangentPoint = aNextPoint.GetPrevTangentPoint ();
nextLength = Vector2.Distance (nextPointPrevTangentPoint, this);
}
Line tangentPerp;
if (prevLength < nextLength)
{
tangentPerp = GeometryTools.GetPerpendicularLine (_prevTangent, prevPointNextTangentPoint);
}
else
{
tangentPerp = GeometryTools.GetPerpendicularLine (_nextTangent, nextPointPrevTangentPoint);
}
IntersectionResult maxCenterIntersection = GeometryTools.FindIntersection (_angularBisector, tangentPerp);
if (maxCenterIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
_junction = JunctionType.Curved;
// find maxCenter, maxRadius and the anchorPoint in relation to both neighbooring point
Vector2 thisVector2 = this;
Vector2 maxCenter = maxCenterIntersection.intersectedPoints[0];
float maxThis2Center = Vector2.Distance (thisVector2, maxCenter);
float maxRadius = Vector2.Distance (maxCenter, tangentPerp.point);
float lengthArcFactor = (maxThis2Center - maxRadius) * _size;
_anchorPoint = thisVector2 + lengthArcFactor * _angularBisector.tangent.normalized;
if (Utils.Approximately (_size, 1))
{
_center = maxCenter;
_radius = maxRadius;
if (prevLength < nextLength)
{
_prevTangentPoint = prevPointNextTangentPoint;
Line tangentPerpLine = GeometryTools.GetPerpendicularLine (_nextTangent, _center);
IntersectionResult tangentPointIntersection =
GeometryTools.FindIntersection (_nextTangent, tangentPerpLine);
if (tangentPointIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
_nextTangentPoint = tangentPointIntersection.intersectedPoints[0];
}
else
{
Debug.Log ("NO NEXT TANGENT POINT");
}
}
else
{
_nextTangentPoint = nextPointPrevTangentPoint;
Line tangentPerpLine = GeometryTools.GetPerpendicularLine (_prevTangent, _center);
IntersectionResult tangentPointIntersection =
GeometryTools.FindIntersection (_prevTangent, tangentPerpLine);
if (tangentPointIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
_prevTangentPoint = tangentPointIntersection.intersectedPoints[0];
}
else
{
Debug.Log ("NO PREV TANGENT POINT");
}
}
}
else
{
// find both intersection point between both tangent
// and the angular bisector's perpendicular line passing by the anchor point
Line bisectorPerpLine = GeometryTools.GetPerpendicularLine (_angularBisector, _anchorPoint);
IntersectionResult prevTangentIntersection =
GeometryTools.FindIntersection (bisectorPerpLine, _prevTangent);
IntersectionResult nextTangentIntersection =
GeometryTools.FindIntersection (bisectorPerpLine, _nextTangent);
if (prevTangentIntersection.intersectionType == IntersectionType.UniqueIntersection &&
nextTangentIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
Vector2 prevTangentOffsetingPoint = prevTangentIntersection.intersectedPoints[0];
Vector2 nextTangentOffsetingPoint = nextTangentIntersection.intersectedPoints[0];
// find tangent points
float bisector2tangent = Vector2.Distance (prevTangentOffsetingPoint, _anchorPoint);
_prevTangentPoint = prevTangentOffsetingPoint + _prevTangent.tangent * bisector2tangent;
_nextTangentPoint = nextTangentOffsetingPoint + _nextTangent.tangent * bisector2tangent;
// find center with tangent perpendicular and angular bisector
Line tangentPerpLine = GeometryTools.GetPerpendicularLine (_prevTangent, _prevTangentPoint);
IntersectionResult centerIntersection =
GeometryTools.FindIntersection (_angularBisector, tangentPerpLine);
if (centerIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
_center = centerIntersection.intersectedPoints[0];
_radius = Vector2.Distance (_center, _anchorPoint);
}
else
{
Debug.Log ("NO CENTER FOUND");
}
}
else
{
Debug.Log ("NO TANGENT POINT FOUND");
}
}
}
else
{
Debug.Log ("NO MAX CENTER FOUND");
_anchorPoint = new Vector2 (float.PositiveInfinity, float.PositiveInfinity);
_center = new Vector2 (float.PositiveInfinity, float.PositiveInfinity);
_radius = float.PositiveInfinity;
_prevTangentPoint = this;
_nextTangentPoint = this;
_junction = JunctionType.Broken;
}
}
}*/
// calcule le centre de l'arc de cercle, le rayon maximum, la position des points tangents
// la longueur de l'arc, l'angle entre les points tantgents et le centre. Tout cela en fonction
// de la position du point et des points voisins.
// voir http://debart.pagesperso-orange.fr/seconde/contruc_cercle.html#ch5
public override void Update(bool recursive=true)
{
base.Update (recursive);
if (_prevEdge != null && _nextEdge != null)
{
_prevTangent = StraightLine.FromPointAndDirection (this, _prevEdge.ToUnitVector2 () * -1);
_nextTangent = StraightLine.FromPointAndDirection (this, _nextEdge.ToUnitVector2 ());
_prevTangentPointMerged = false;
_nextTangentPointMerged = false;
// find the farthest center position
float prevLength = _prevEdge.GetLength ();
Point2 prevPoint = _prevEdge.GetPrevPoint2 ();
Vector2 prevPointNextTangentPoint = prevPoint;
if (prevPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aPrevPoint = prevPoint as ArchedPoint2;
prevPointNextTangentPoint = aPrevPoint.GetNextTangentPoint ();
prevLength = Vector2.Distance (prevPointNextTangentPoint, this);
}
float nextLength = _nextEdge.GetLength ();
Point2 nextPoint = _nextEdge.GetNextPoint2 ();
Vector2 nextPointPrevTangentPoint = nextPoint;
if (nextPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aNextPoint = nextPoint as ArchedPoint2;
nextPointPrevTangentPoint = aNextPoint.GetPrevTangentPoint ();
nextLength = Vector2.Distance (nextPointPrevTangentPoint, this);
}
prevPointNextTangentPoint = prevPointNextTangentPoint + _prevTangent.tangent * -40;
nextPointPrevTangentPoint = nextPointPrevTangentPoint + _nextTangent.tangent * -40;
StraightLine tangentPerp;
if (prevLength < nextLength)
{
tangentPerp = GeometryTools.GetPerpendicularLine (_prevTangent, prevPointNextTangentPoint);
}
else
{
tangentPerp = GeometryTools.GetPerpendicularLine (_nextTangent, nextPointPrevTangentPoint);
}
IntersectionResult maxCenterIntersection = GeometryTools.FindIntersection (_angularBisector, tangentPerp);
if (maxCenterIntersection.intersectionType == IntersectionType.UniqueIntersection)
{
_junction = JunctionType.Curved;
// find maxCenter, maxRadius and the anchorPoint in relation to both neighbooring point
Vector2 thisVector2 = this;
Vector2 maxCenter = maxCenterIntersection.intersectedPoints[0];
_maxRadius = Vector2.Distance (maxCenter, tangentPerp.point);
float radHalfAngle = Mathf.Deg2Rad * (_angle / 2);
if (_radius > _maxRadius)
{
_measuredRadius = _maxRadius;
_center = maxCenter;
/*if (prevLength < nextLength)
{
if (prevPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aPrevPoint = prevPoint as ArchedPoint2;
if(!aPrevPoint._nextTangentPointMerged)
{
_prevTangentPointMerged = true;
}
}
else
{
_prevTangentPointMerged = true;
}
}
else
{
if (nextPoint.GetJunction () == JunctionType.Curved)
{
ArchedPoint2 aNextPoint = nextPoint as ArchedPoint2;
if(!aNextPoint._prevTangentPointMerged)
{
_nextTangentPointMerged = true;
}
}
else
{
_nextTangentPointMerged = true;
}
}*/
}
else
{
_measuredRadius = _radius;
float this2CenterTranslationFactor = _measuredRadius / Mathf.Sin (radHalfAngle);
_center = thisVector2 + _angularBisector.tangent * this2CenterTranslationFactor;
}
_anchorPoint = _center + _angularBisector.tangent * -_measuredRadius;
_arcAngle = 180 - ((_angle / 2) + 90);
_arcLength = _arcAngle * Mathf.Deg2Rad * _measuredRadius;
float tangentPointTranslationFactor = _measuredRadius / Mathf.Tan (radHalfAngle);
_prevTangentPoint = thisVector2 + _prevTangent.tangent * tangentPointTranslationFactor;
_nextTangentPoint = thisVector2 + _nextTangent.tangent * tangentPointTranslationFactor;
}
else
{
Debug.Log ("NO MAX CENTER FOUND");
_anchorPoint = new Vector2 (float.PositiveInfinity, float.PositiveInfinity);
_center = new Vector2 (float.PositiveInfinity, float.PositiveInfinity);
_measuredRadius = float.PositiveInfinity;
_arcAngle = 0;
_arcLength = 0;
_prevTangentPoint = this;
_nextTangentPoint = this;
_junction = JunctionType.Broken;
}
}
else
{
Debug.Log ("EDGE NULL");
}
}
public List <PointF> FindKeyPoints(Point[] points, float offset = 0, bool removeDuplicates = true)
{
//Stopwatch sw = new Stopwatch();
//sw.Start();
//Point[] allPoints = points.ToArray();
List <PointF> result = new List <PointF>();
List <PointF> visitedPoints = new List <PointF>();
PointF currentPoint = points[0];
if (removeDuplicates)
{
visitedPoints.Add(currentPoint);
}
double distanceCounter = 0;
if (offset == 0)
{
result.Add(currentPoint);
}
else
{
distanceCounter += offset;
}
int pointCounter = 1;
while (true)
{
Point targetPoint = points[pointCounter];
double dist = currentPoint.Distance(targetPoint);
if (distanceCounter + dist >= Settings.GapDistance)
{
StraightLine line = new StraightLine(currentPoint, targetPoint);
double addOn = Settings.GapDistance - distanceCounter;
PointF keyPoint = line.DistanceFromStart(addOn);
result.Add(keyPoint);
currentPoint = keyPoint;
distanceCounter = 0;
}
else
{
distanceCounter += dist;
currentPoint = points[pointCounter];
pointCounter++;
if (pointCounter == points.Length)
{
break;
}
if (removeDuplicates)
{
if (visitedPoints.Contains(points[pointCounter]))
{
break;
}
else
{
visitedPoints.Add(points[pointCounter]);
}
}
}
}
//sw.Stop();
//Console.WriteLine("FindKeyPoints: " + sw.ElapsedMilliseconds);
return(result);
}
public List <List <PointF> > FindKeyPoints(Point[] points, int numberOfSlides, bool removeDuplicates = true)
{
//Stopwatch sw = new Stopwatch();
//sw.Start();
//Point[] allPoints = points.ToArray();
List <List <PointF> > result = new List <List <PointF> >();
if (points.Length <= 1)
{
return(result);
}
int listCounter = 0;
for (int i = 0; i <= numberOfSlides; i++)
{
result.Add(new List <PointF>());
}
List <PointF> visitedPoints = new List <PointF>();
PointF currentPoint = points[0];
if (removeDuplicates)
{
visitedPoints.Add(currentPoint);
}
double distanceCounter = 0;
double offset = Settings.GapDistance / numberOfSlides;
result[listCounter].Add(currentPoint);
listCounter++;
if (listCounter > numberOfSlides)
{
listCounter = 0;
}
int pointCounter = 0;
int targetLimit = Settings.NumberOfPoints + 1;
int limitCounter = 0;
int?targetList = null;
while (true)
{
Point targetPoint = points[pointCounter];
double dist = currentPoint.Distance(targetPoint);
if (distanceCounter + dist >= offset)
{
StraightLine line = new StraightLine(currentPoint, targetPoint);
double addOn = offset - distanceCounter;
PointF keyPoint = line.DistanceFromStartFloat(addOn);
result[listCounter].Add(keyPoint);
if (listCounter == targetList)
{
limitCounter++;
if (limitCounter == targetLimit)
{
break;
}
}
listCounter++;
if (listCounter > numberOfSlides)
{
listCounter = 0;
}
currentPoint = keyPoint;
distanceCounter = 0;
}
else
{
distanceCounter += dist;
currentPoint = points[pointCounter];
pointCounter++;
if (pointCounter == points.Length)
{
targetList = listCounter;
pointCounter -= points.Length;
}
if (removeDuplicates)
{
if (visitedPoints.Contains(points[pointCounter]))
{
break;
}
else
{
visitedPoints.Add(points[pointCounter]);
}
}
}
}
//sw.Stop();
//Console.WriteLine("FindKeyPoints: " + sw.ElapsedMilliseconds);
return(result);
}
public static AutomatedRodentTracker.Services.RBSK.RBSK GetStandardMouseRules()
{
RBSKRules mouseRules = new RBSKRules();
//Always make sure the number of parameters is correct first
mouseRules.AddRule(new RBSKRule((x, s, b) =>
{
if (x.Length != s.NumberOfPoints)
{
return(false);
}
return(true);
}));
//Rules regarding the points layout
mouseRules.AddRule(new RBSKRule((x, s, b) =>
{
PointF nosePoint = x[2];
PointF[] vectorPoints = { x[1], x[3] };
PointF[] rearPoints = { x[0], x[4] };
double gapDistance = s.GapDistance;
//Calculate target areas
double gapDistanceSquared = gapDistance * gapDistance;
double targetHeadArea = 0.433f * gapDistanceSquared;
//float targetArea = gapDistanceSquared + targetHeadArea;
//Check total area
//float area = MathExtension.PolygonArea(x);
//if (area < targetArea / 1.5)
//{
// return false;
//}
//check head area
float headArea = MathExtension.PolygonArea(new[] { vectorPoints[0], nosePoint, vectorPoints[1] });
if (headArea < targetHeadArea / 2)
{
return(false);
}
//Check which side the points lie on
PointSideVector result = MathExtension.FindSide(vectorPoints[0], vectorPoints[1], nosePoint);
if (result == PointSideVector.On)
{
return(false);
}
PointSideVector rearResult1 = MathExtension.FindSide(vectorPoints[0], vectorPoints[1], rearPoints[0]);
PointSideVector rearResult2 = MathExtension.FindSide(vectorPoints[0], vectorPoints[1], rearPoints[1]);
//If the points are on the line then continue
if (rearResult1 == PointSideVector.On || rearResult2 == PointSideVector.On)
{
return(false);
}
//Make sure the rear points are on the opposite side to the nose point
if (!(rearResult1 == rearResult2 && rearResult1 != result))
{
return(false);
}
//Check rear points are correctly aligned with the center line
PointF vectorMidPoint = new PointF((vectorPoints[0].X + vectorPoints[1].X) / 2, (vectorPoints[0].Y + vectorPoints[1].Y) / 2);
rearResult1 = MathExtension.FindSide(nosePoint, vectorMidPoint, rearPoints[0]);
rearResult2 = MathExtension.FindSide(nosePoint, vectorMidPoint, rearPoints[1]);
if (rearResult1 == rearResult2)
{
return(false);
}
StraightLine centerLine = new StraightLine(nosePoint, vectorMidPoint);
PointF targetRearMidPoint = centerLine.DistanceFromEndFloat(-s.GapDistance);
centerLine = new StraightLine(nosePoint, targetRearMidPoint);
double rearDist1 = centerLine.FindDistanceToSegment(rearPoints[0]);
if (rearDist1 < s.GapDistance / 3)
{
return(false);
}
rearDist1 = centerLine.FindDistanceToSegment(rearPoints[1]);
if (rearDist1 < s.GapDistance / 3)
{
return(false);
}
//The points are correctly aligned, make sure it's covering black pixels
int blackCounter = 0;
int whiteCounter = 0;
StraightLine line = new StraightLine(vectorPoints[0], vectorPoints[1]);
double distance = line.GetMagnitude();
float increment = (float)distance / 10.0f;
for (int j = 1; j < 9; j++)
{
Point pointToCheck = line.DistanceFromStart(j * increment);
if (b[pointToCheck].Intensity <= 120)
{
blackCounter++;
}
else
{
whiteCounter++;
}
}
if (whiteCounter > blackCounter)
{
return(false);
}
//Don't want the distance to be too great between the vector points
//double distance = vectorPoints[0].Distance(vectorPoints[1]);
//double distance = Math.Sqrt((vectorPoints[1].X - vectorPoints[0].X) + (vectorPoints[1].Y - vectorPoints[0].Y));
if (distance > 1.4f * gapDistance)
{
return(false);
}
//Rear distance must fall between a certain range
double rearDistance = rearPoints[0].Distance(rearPoints[1]);
if (rearDistance > 2.5f * gapDistance)
{
return(false);
}
if (rearDistance < 1.2 * gapDistance)
{
return(false);
}
return(true);
}));
RBSKRules advancedRules = new RBSKRules();
advancedRules.AddRule(new RBSKRule((x, s, b) =>
{
Point intNosePoint = x[2].ToPoint();
Point intVectorPoint1 = x[1].ToPoint();
Point intVectorPoint2 = x[3].ToPoint();
Image <Gray, Byte> mask = new Image <Gray, byte>(b.Cols, b.Rows, new Gray(0));
mask.FillConvexPoly(new[] { intNosePoint, intVectorPoint1, intVectorPoint2 }, new Gray(255), LineType.FourConnected);
double avgIntensity = b.GetAverage(mask).Intensity;
if (avgIntensity > 10)
{
return(false);
}
return(true);
}));
//Create probability funcation
RBSKProbability rbskProbability = new RBSKProbability((p, s) =>
{
PointF[] points = p;
if (points.Length != 5)
{
return(0);
}
//float gapDistanceSquared = s.GapDistance*s.GapDistance;
StraightLine line = new StraightLine(points[1], points[3]);
//Need to normalize the line
double targetNoseDistance = 0.866d * s.GapDistance;
double actualNoseDistance = line.FindDistanceToSegment(points[2]);
double noseProbability;
if (targetNoseDistance > actualNoseDistance)
{
noseProbability = actualNoseDistance / targetNoseDistance;
}
else
{
noseProbability = targetNoseDistance / actualNoseDistance;
}
//float targetHeadArea = 0.433f * gapDistanceSquared;
//float targetHeadArea = (float)MathExtension.PolygonArea(new[]
//{
// new Point(0, 0),
// new Point((int)s.GapDistance, 0),
// new Point((int)(s.GapDistance/2), (int)(s.GapDistance*0.866f)),
//});
//float actualHeadArea = MathExtension.PolygonArea(new[]
//{
// points[1], points[2], points[3]
//});
//float areaProbability;
//if (targetHeadArea > actualHeadArea)
//{
// areaProbability = actualHeadArea / targetHeadArea;
//}
//else
//{
// areaProbability = targetHeadArea / actualHeadArea;
//}
return(noseProbability);// * areaProbability;
});
return(new AutomatedRodentTracker.Services.RBSK.RBSK(mouseRules, advancedRules, new RBSKSettings(5), rbskProbability));
}
// obtient une droite perpendiculaire a une droite et passant par un point
public static StraightLine GetPerpendicularLine(StraightLine l, Vector2 p)
{
Vector2 normal = new Vector2 (-l.tangent.y, l.tangent.x);
normal.Normalize ();
StraightLine perpendicularLine = new StraightLine ();
perpendicularLine.tangent = normal;
perpendicularLine.point = p;
return perpendicularLine;
}
private void btn_volt_cali_Click(object sender, EventArgs e)
{
num_pair np_tmp = new num_pair((float)curr_numUpDn.Value, (float)MainV2.comPort.MAV.cs.battery_voltage, (float)MainV2.comPort.MAV.cs.battery_voltage2);
this.list_num.Add(np_tmp);
PointSet mainSet = new PointSet();
int valid_data = 0;
foreach (var num in this.list_num)
{
if (!(num.num_meas1.Equals(0) && num.num_meas2.Equals(0) && num.num_ref.Equals(0)))
{
coordinates[valid_data].x = (double)(num.num_meas1);
coordinates[valid_data].y = (double)(num.num_ref);
valid_data++;
}
}
for (int i = 0; (i < valid_data) && (i < 20); i++)
{
mainSet.Add(coordinates[i]);
}
StraightLine result = mainSet.FindApproximateSolution();
if (Double.IsNaN(result.a) || Double.IsNaN(result.b))
{
MessageBox.Show("抱歉，校准值中有异常值，请检查后重新校准!", "校准值异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
int try_times = 0;
for (try_times = 0; try_times < 5; try_times++)
{
// save horizon spd
if (MainV2.comPort.setParam("BATT_VM", result.a))
{
if (MainV2.comPort.setParam("BATT_V_OFS", result.b))
{
MessageBox.Show("恭喜，校准电压1成功!", "校准成功", MessageBoxButtons.OK, MessageBoxIcon.Information);
break;
}
}
}
PointSet mainSet2 = new PointSet();
this.coordinates.Initialize();
valid_data = 0;
foreach (var num in this.list_num)
{
if (!(num.num_meas1.Equals(0) && num.num_meas2.Equals(0) && num.num_ref.Equals(0)))
{
coordinates[valid_data].x = (double)(num.num_meas2);
coordinates[valid_data].y = (double)(num.num_ref);
valid_data++;
}
}
for (int i = 0; (i < valid_data) && (i < 20); i++)
{
mainSet2.Add(coordinates[i]);
}
StraightLine result2 = mainSet2.FindApproximateSolution();
if (Double.IsNaN(result2.a) || Double.IsNaN(result2.b))
{
MessageBox.Show("抱歉，校准值中有异常值，请检查后重新校准!", "校准值异常", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
try_times = 0;
for (try_times = 0; try_times < 5; try_times++)
{
// save horizon spd
if (MainV2.comPort.setParam("BATT2_VM", result2.a))
{
if (MainV2.comPort.setParam("BATT2_V_OFS", result2.b))
{
MessageBox.Show("恭喜，校准电压2成功!", "校准成功", MessageBoxButtons.OK, MessageBoxIcon.Information);
break;
}
}
}
if (5 == try_times)
{
MessageBox.Show("抱歉，校准值保存到飞控失败，请尝试重新连接飞控后再次尝试!", "校准失败", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
