public static double Distance(PointR p1, PointR p2) { var dx = p2.X - p1.X; var dy = p2.Y - p1.Y; return(Math.Sqrt(dx * dx + dy * dy)); }
// determines the angle, in radians, between two points. the angle is defined // by the circle centered on the start point with a radius to the end point, // where 0 radians is straight right from start (+x-axis) and PI/2 radians is // straight down (+y-axis). public static double AngleInRadians(PointR start, PointR end, bool positiveOnly) { double radians = 0.0; if (start.X != end.X) { radians = Math.Atan2(end.Y - start.Y, end.X - start.X); } else // pure vertical movement { if (end.Y < start.Y) { radians = -Math.PI / 2.0; // -90 degrees is straight up } else if (end.Y > start.Y) { radians = Math.PI / 2.0; // 90 degrees is straight down } } if (positiveOnly && radians < 0.0) { radians += Math.PI * 2.0; } return(radians); }
// Rotate a point 'p' around a point 'c' by the given radians. // Rotation (around the origin) amounts to a 2x2 matrix of the form: // // [ cos A -sin A ] [ p.x ] // [ sin A cos A ] [ p.y ] // // Note that the C# Math coordinate system has +x-axis stright right and // +y-axis straight down. Rotation is clockwise such that from +x-axis to // +y-axis is +90 degrees, from +x-axis to -x-axis is +180 degrees, and // from +x-axis to -y-axis is -90 degrees. public static PointR RotatePoint(PointR p, PointR c, double radians) { var q = PointR.Empty; q.X = (p.X - c.X) * Math.Cos(radians) - (p.Y - c.Y) * Math.Sin(radians) + c.X; q.Y = (p.X - c.X) * Math.Sin(radians) + (p.Y - c.Y) * Math.Cos(radians) + c.Y; return(q); }
// translates the points so that their centroid lies at 'toPt' //public static ArrayList TranslateCentroidTo(ArrayList points, PointR toPt) public static List <PointR> TranslateCentroidTo(List <PointR> points, PointR toPt) { //ArrayList newPoints = new ArrayList(points.Count); var newPoints = new List <PointR>(points.Count); var centroid = Centroid(points); for (var i = 0; i < points.Count; i++) { var p = points[i]; p.X += (toPt.X - centroid.X); p.Y += (toPt.Y - centroid.Y); newPoints.Add(p); } return(newPoints); }
// translates the points so that the upper-left corner of their bounding box lies at 'toPt' //public static ArrayList TranslateBBoxTo(ArrayList points, PointR toPt) public static List <PointR> TranslateBBoxTo(List <PointR> points, PointR toPt) { //ArrayList newPoints = new ArrayList(points.Count); var newPoints = new List <PointR>(points.Count); var r = FindBox(points); for (var i = 0; i < points.Count; i++) { var p = points[i]; p.X += (toPt.X - r.X); p.Y += (toPt.Y - r.Y); newPoints.Add(p); } return(newPoints); }
//public static ArrayList Resample(ArrayList points, int n) public static List <PointR> Resample(List <PointR> points, int n) { var I = PathLength(points) / (n - 1); // interval length var D = 0.0; //ArrayList srcPts = new ArrayList(points); var srcPts = new List <PointR>(points); //ArrayList dstPts = new ArrayList(n); var dstPts = new List <PointR>(n) { srcPts[0] }; for (var i = 1; i < srcPts.Count; i++) { var pt1 = srcPts[i - 1]; var pt2 = srcPts[i]; var d = Distance(pt1, pt2); if ((D + d) >= I) { var qx = pt1.X + ((I - D) / d) * (pt2.X - pt1.X); var qy = pt1.Y + ((I - D) / d) * (pt2.Y - pt1.Y); var q = new PointR(qx, qy); dstPts.Add(q); // append new point 'q' srcPts.Insert(i, q); // insert 'q' at position i in points s.t. 'q' will be the next i D = 0.0; } else { D += d; } } // somtimes we fall a rounding-error short of adding the last point, so add it if so if (dstPts.Count == n - 1) { dstPts.Add(srcPts[srcPts.Count - 1]); } return(dstPts); }
// copy constructor public PointR(PointR p) { X = p.X; Y = p.Y; T = p.T; }
// determines the angle, in degrees, between two points. the angle is defined // by the circle centered on the start point with a radius to the end point, // where 0 degrees is straight right from start (+x-axis) and 90 degrees is // straight down (+y-axis). public static double AngleInDegrees(PointR start, PointR end, bool positiveOnly) { double radians = AngleInRadians(start, end, positiveOnly); return(Rad2Deg(radians)); }