added Segment2.FastSquaredDistance, DGraph2 utility fns

Author: rms80

core/ProfileUtil.cs

@@ -48,7 +48,7 @@ public virtual void Reset()
         public override string ToString()
         {
             TimeSpan t = Watch.Elapsed;
-            return string.Format("{0:fffffff}", Watch.Elapsed);
+            return string.Format("{0:ss}.{0:fffffff}", Watch.Elapsed);
         }
     }
 
@@ -125,7 +125,7 @@ public string Elapsed(string label)
         }
         public string Accumulated(string label)
         {
-            return string.Format("{0:fffffff}", Timers[label].Accumulated);
+            return string.Format("{0:ss}.{0:fffffff}", Timers[label].Accumulated);
         }
 
         public string AllTicks(string prefix = "Times:")

curve/DGraph2.cs

@@ -158,9 +158,30 @@ public bool GetEdgeV(int eID, ref Vector2d a, ref Vector2d b)
 			return false;
 		}
 
+        public Segment2d GetEdgeSegment(int eID)
+        {
+            if (edges_refcount.isValid(eID)) {
+                int iv0 = 2 * edges[3 * eID];
+                int iv1 = 2 * edges[3 * eID + 1];
+                return new Segment2d(new Vector2d(vertices[iv0], vertices[iv0 + 1]),
+                                     new Vector2d(vertices[iv1], vertices[iv1 + 1]));
+            }
+            throw new Exception("DGraph2.GetEdgeSegment: invalid segment with id " + eID);
+        }
+
+        public Vector2d GetEdgeCenter(int eID)
+        {
+            if (edges_refcount.isValid(eID)) {
+                int iv0 = 2 * edges[3 * eID];
+                int iv1 = 2 * edges[3 * eID + 1];
+                return new Vector2d((vertices[iv0] + vertices[iv1]) * 0.5,
+                                    (vertices[iv0 + 1] + vertices[iv1 + 1]) * 0.5);
+            }
+            throw new Exception("DGraph2.GetEdgeCenter: invalid segment with id " + eID);
+        }
 
 
-		public int AppendVertex(Vector2d v)
+        public int AppendVertex(Vector2d v)
 		{
 			int vid = vertices_refcount.allocate();
 			int i = 2 * vid;

math/Segment2.cs

@@ -60,7 +60,7 @@ public double DistanceSquared(Vector2d p)
 			else if ( t <= -Extent )
 				return P0.DistanceSquared(p);
 			Vector2d proj = Center + t * Direction;
-			return (proj - p).LengthSquared;
+            return proj.DistanceSquared(p);
 		}
         public double DistanceSquared(Vector2d p, out double t)
         {
@@ -73,7 +73,7 @@ public double DistanceSquared(Vector2d p, out double t)
                 return P0.DistanceSquared(p);
             }
             Vector2d proj = Center + t * Direction;
-            return (proj - p).LengthSquared;
+            return proj.DistanceSquared(p);
         }
 
         public Vector2d NearestPoint(Vector2d p)
@@ -151,6 +151,33 @@ public void Reverse() {
         public IParametricCurve2d Clone() {
             return new Segment2d(this.Center, this.Direction, this.Extent);
         }
+
+
+
+        /// <summary>
+        /// distance from pt to segment (a,b), with no square roots
+        /// </summary>
+        public static double FastDistanceSquared(ref Vector2d a, ref Vector2d b, ref Vector2d pt)
+        {
+            double vx = b.x - a.x, vy = b.y - a.y;
+            double len2 = vx*vx + vy*vy;
+            double dx = pt.x - a.x, dy = pt.y - a.y;
+            if (len2 < 1e-13) {
+                return dx * dx + dy * dy;
+            }
+            double t = (dx*vx + dy*vy);
+            if (t <= 0) {
+                return dx * dx + dy * dy;
+            } else if (t >= len2) {
+                dx = pt.x - b.x; dy = pt.y - b.y;
+                return dx * dx + dy * dy;
+            }
+
+            dx = pt.x - (a.x + ((t * vx)/len2));
+            dy = pt.y - (a.y + ((t * vy)/len2));
+            return dx * dx + dy * dy;
+        }
+
     }
 
 
@@ -233,6 +260,34 @@ void update_from_endpoints(Vector2f p0, Vector2f p1)
             Direction = p1 - p0;
             Extent = 0.5f * Direction.Normalize();
         }
+
+
+
+
+        /// <summary>
+        /// distance from pt to segment (a,b), with no square roots
+        /// </summary>
+        public static float FastDistanceSquared(ref Vector2f a, ref Vector2f b, ref Vector2f pt)
+        {
+            float vx = b.x - a.x, vy = b.y - a.y;
+            float len2 = vx * vx + vy * vy;
+            float dx = pt.x - a.x, dy = pt.y - a.y;
+            if (len2 < 1e-7) {
+                return dx * dx + dy * dy;
+            }
+            float t = (dx * vx + dy * vy);
+            if (t <= 0) {
+                return dx * dx + dy * dy;
+            } else if (t >= len2) {
+                dx = pt.x - b.x; dy = pt.y - b.y;
+                return dx * dx + dy * dy;
+            }
+
+            dx = pt.x - (a.x + ((t * vx) / len2));
+            dy = pt.y - (a.y + ((t * vy) / len2));
+            return dx * dx + dy * dy;
+        }
+
     }