improved fingerDetection and Vector2D+Line2D

bbiwarg/Detectors/Fingers/Finger.cs

@@ -13,147 +13,49 @@ namespace bbiwarg.Detectors.Fingers
 {
     class Finger
     {
-        private Vector2D tipPoint;
-        private Vector2D handPoint;
-        private Vector2D direction;
-        private float length;
+        private Line2D line;
+        public Line2D Line { get { return line; } }
 
-        public Finger(Vector2D p1, Vector2D p2) {
-            assignTipAndHandPoint(p1, p2);
-        }
-
-        public float getLength() {
-            return length;
+        public Finger(Line2D line) {
+            this.line = line;
         }
 
         public Vector2D getTipPoint() {
-            return tipPoint;
+            if (line.P1.Y < line.P2.Y)
+                return line.P1;
+            else
+                return line.P2;
         }
 
         public Vector2D getHandPoint() {
-            return handPoint;
+            if (line.P1.Y < line.P2.Y)
+                return line.P2;
+            else
+                return line.P1;
         }
 
-        public Vector2D getDirection() {
-            return direction;
-        }
+        public float getSimilarity(Finger compareFinger) {
+            Line2D compareLine = compareFinger.Line;
 
-        public float getSimilarity(Finger compareFinger)
-        {
             //thresholds
             float thresholdMaxAngle = (float)(30 * Math.PI / 180); // 30°
             float thresholdMaxParallelDistance = 40;
             float thresholdMaxVerticalDistance = 40;
 
             //check angle
-            float angle = direction.getMinAngleBetween(compareFinger.direction);
+            float angle = Line.getAngleBetween(compareLine);
             float angleSimilarity = Math.Max(1 - angle / thresholdMaxAngle, 0);
 
             //check parallel distance
-            float minParallelDistance = getMinParallelDistance(compareFinger);
-            float parallelDistanceSimilarity = Math.Max(1 - minParallelDistance / thresholdMaxParallelDistance, 0);
+            float parallelDistance = Line.getParallelDistanceTo(compareLine);
+            float parallelDistanceSimilarity = Math.Max(1 - parallelDistance / thresholdMaxParallelDistance, 0);
 
             //check vertical distance
-            float minVerticalDistance = getMinVerticalDistance(compareFinger);
-            float verticalDistanceSimilarity = Math.Max(1 - minVerticalDistance / thresholdMaxVerticalDistance, 0);
+            float verticalDistance = Line.getVerticalDistanceTo(compareLine);
+            float verticalDistanceSimilarity = Math.Max(1 - verticalDistance / thresholdMaxVerticalDistance, 0);
 
             return (angleSimilarity + parallelDistanceSimilarity + verticalDistanceSimilarity) / 3;
         }
 
-        public bool isMergeable(Finger mergeFinger)
-        {
-            //thresholds
-            float thresholdMaxAngle = (float) (15 * Math.PI / 180); // 15°
-            float thresholdMaxParallelDistance = 10;
-            float thresholdMaxVerticalDistance = 20;
-
-            //check angle
-            float angle = direction.getMinAngleBetween(mergeFinger.direction);
-            if (angle > thresholdMaxAngle) return false;
-
-            //check parallel distance
-            float minParallelDistance = getMinParallelDistance(mergeFinger);
-            if (minParallelDistance > thresholdMaxParallelDistance) return false;
-
-            //check vertical distance
-            float minVerticalDistance = getMinVerticalDistance(mergeFinger);
-            if (minVerticalDistance > thresholdMaxVerticalDistance) return false;
-
-            return true;
-        }
-
-        public void mergeFingers(Finger mergeFinger) {
-            Vector2D a1 = tipPoint;
-            Vector2D a2 = handPoint;
-            Vector2D b1 = mergeFinger.getTipPoint();
-            Vector2D b2 = mergeFinger.getHandPoint();
-
-            //find two points that are furthest apart
-            float distanceA1A2 = length;
-            float distanceA1B1 = a1.getDistanceTo(b1);
-            float distanceA1B2 = a1.getDistanceTo(b2);
-            float distanceA2B1 = a2.getDistanceTo(b1);
-            float distanceA2B2 = a2.getDistanceTo(b2);
-            float distanceB1B2 = mergeFinger.getLength();
-            float maxDistance = Math.Max(Math.Max(distanceA1A2, distanceA1B1), Math.Max(Math.Max(distanceA1B2, distanceA2B1), Math.Max(distanceA2B2, distanceB1B2)));
-
-            if(maxDistance == distanceA1A2) assignTipAndHandPoint(a1,a2);
-            else if(maxDistance == distanceA1B1) assignTipAndHandPoint(a1, b1);
-            else if(maxDistance == distanceA1B2) assignTipAndHandPoint(a1, b2);
-            else if(maxDistance == distanceA2B1) assignTipAndHandPoint(a2, b1);
-            else if(maxDistance == distanceA2B2) assignTipAndHandPoint(a2, b2);
-            else if(maxDistance == distanceB1B2) assignTipAndHandPoint(b1, b2);
-        }
-
-        private void assignTipAndHandPoint(Vector2D p1, Vector2D p2)
-        {
-            if (p1.y < p2.y)
-            {
-                this.tipPoint = p1;
-                this.handPoint = p2;
-            }
-            else
-            {
-                this.tipPoint = p2;
-                this.handPoint = p1;
-            }
-
-            direction = handPoint - tipPoint;
-            length = direction.getLength();
-        }
-
-        private float getMinParallelDistance(Finger finger)
-        {
-            Vector2D a1 = tipPoint;
-            Vector2D a2 = handPoint;
-            Vector2D b1 = finger.getTipPoint();
-            Vector2D b2 = finger.getHandPoint();
-
-            //project on finger line
-            Vector2D p1 = Vector2D.projectToLine(b1, direction, a1);
-            Vector2D p2 = Vector2D.projectToLine(b2, direction, a1);
-
-            float distanceP1B1 = p1.getDistanceTo(b1);
-            float distanceP2B2 = p2.getDistanceTo(b2);
-            return Math.Min(distanceP1B1, distanceP2B2);
-        }
-
-        private float getMinVerticalDistance(Finger finger)
-        {
-            Vector2D a1 = tipPoint;
-            Vector2D a2 = handPoint;
-            Vector2D b1 = finger.getTipPoint();
-            Vector2D b2 = finger.getHandPoint();
-
-            //project on finger line
-            Vector2D p1 = Vector2D.projectToLine(b1, direction, a1);
-            Vector2D p2 = Vector2D.projectToLine(b2, direction, a1);
-
-            float distanceP1A1 = p1.getDistanceTo(a1);
-            float distanceP1A2 = p1.getDistanceTo(a2);
-            float distanceP2A1 = p2.getDistanceTo(a1);
-            float distanceP2A2 = p2.getDistanceTo(a2);
-            return Math.Min(Math.Min(distanceP1A1, distanceP1A2), Math.Min(distanceP2A1, distanceP2A2));
-        }
     }
 }

bbiwarg/Detectors/Fingers/FingerDetector.cs

@@ -17,8 +17,10 @@ namespace bbiwarg.Detectors.Fingers
         private DepthImage depthImage;
         private EdgeImage edgeImage;
         private FingerImage fingerImage;
+        private Image<Gray, byte> fingerPointsImage;
+        private List<Line2D> fingerLines;
         private List<Finger> fingers;
-        Image<Gray, byte> fingerPoints;
+
 
         public FingerDetector(DepthImage depthImage, EdgeImage edgeImage, FingerImage fingerImage) {
             this.depthImage = depthImage;
@@ -34,10 +36,18 @@ namespace bbiwarg.Detectors.Fingers
             Console.WriteLine("findFingerPoints:" + sw.ElapsedMilliseconds);
             sw.Restart();
 
+            findFingerLines();
+
+            sw.Stop();
+            Console.WriteLine("findFingerLines:" + sw.ElapsedMilliseconds);
+            sw.Restart();
+
+
             findFingers();
 
-            Console.WriteLine("findFingers:" + sw.ElapsedMilliseconds);
             sw.Stop();
+            Console.WriteLine("findFingers:" + sw.ElapsedMilliseconds);
+
         }
 
         public List<Finger> getFingers() {
@@ -48,17 +58,17 @@ namespace bbiwarg.Detectors.Fingers
             int width = depthImage.getWidth();
             int height = depthImage.getHeight();
 
-            fingerPoints = new Image<Gray, byte>(width, height);
+            fingerPointsImage = new Image<Gray, byte>(width, height);
             
             for (int y = 0; y < height; y++) {
                 for (int x = 0; x < width; x++) {
                     if (edgeImage.isEdgeAt(x, y))
                     {
                         Vector2D startPoint = new Vector2D(x, y);
-                        searchFingerPoint(startPoint, new Vector2D(1, 0), fingerPoints);
-                        searchFingerPoint(startPoint, new Vector2D(0, 1), fingerPoints);
-                        //searchFingerPoint(startPoint, new Vector2D(1, -1), fingerPoints);
-                        //searchFingerPoint(startPoint, new Vector2D(1, 1), fingerPoints);
+                        searchFingerPoint(startPoint, new Vector2D(1, 0), fingerPointsImage);
+                        searchFingerPoint(startPoint, new Vector2D(0, 1), fingerPointsImage);
+                        //searchFingerPoint(startPoint, new Vector2D(1, -1).normalize(), fingerPointsImage);
+                        //searchFingerPoint(startPoint, new Vector2D(1, 1).normalize(), fingerPointsImage);
                     }
                 }
             }
@@ -69,31 +79,31 @@ namespace bbiwarg.Detectors.Fingers
             int height = fingerPointsImage.Height;
             float minFingerSize = 10;
             float maxFingerSize = 30;
-            float stepSize = direction.getLength();
+            direction = direction.normalize();
 
 
             bool edgeFound = false;
             Vector2D edge = start + minFingerSize * direction;
             float index = minFingerSize;
-            float maxXIndex = (direction.x == 0) ? float.MaxValue : ((direction.x > 0) ? ((width - start.x) / direction.x) : (-start.x / direction.x));
-            float maxYIndex = (direction.y == 0) ? float.MaxValue : ((direction.y > 0) ? ((height - start.y) / direction.y) : (-start.y / direction.y));
+            float maxXIndex = (direction.X == 0) ? float.MaxValue : ((direction.X > 0) ? ((width - start.X) / direction.X) : (-start.X / direction.X));
+            float maxYIndex = (direction.Y == 0) ? float.MaxValue : ((direction.Y > 0) ? ((height - start.Y) / direction.Y) : (-start.Y / direction.Y));
 
             float maxIndex = Math.Min(maxFingerSize, Math.Min(maxXIndex, maxYIndex));
 
             if (index < maxIndex)
             {
-                Int16 depthStart = depthImage.getDepthAt((int)start.x, (int)start.y);
-                Int16 depthEdge = depthImage.getDepthAt((int)edge.x, (int)edge.y);
+                Int16 depthStart = depthImage.getDepthAt((int)start.X, (int)start.Y);
+                Int16 depthEdge = depthImage.getDepthAt((int)edge.X, (int)edge.Y);
 
                 if (depthStart > depthEdge)
                 {
                     while (!edgeFound && index < maxIndex)
                     {
-                        if (edgeImage.isEdgeAt((int)edge.x, (int)edge.y))
+                        if (edgeImage.isEdgeAt((int)edge.X, (int)edge.Y))
                             edgeFound = true;
                         else
                         {
-                            index += stepSize;
+                            index++;
                             edge += direction;
                         }
                     }
@@ -103,65 +113,73 @@ namespace bbiwarg.Detectors.Fingers
             if (edgeFound && fingerDepthTest(start, edge))
             {
                 Vector2D mid = 0.5f * (start + edge);
-                fingerPointsImage.Data[(int)mid.y, (int)mid.x, 0] = byte.MaxValue;
-                fingerImage.setFingerAt((int)mid.x, (int)mid.y, FingerImageState.possibleFinger);
+                fingerPointsImage.Data[(int)mid.Y, (int)mid.X, 0] = byte.MaxValue;
+                fingerImage.setFingerAt((int)mid.X, (int)mid.Y, FingerImageState.possibleFinger);
             }
         }
 
         private bool fingerDepthTest(Vector2D p1, Vector2D p2) {
             Vector2D mid = 0.5f * (p1 + p2);
-            Int16 depthP1 = depthImage.getDepthAt((int)p1.x, (int)p1.y);
-            Int16 depthMid = depthImage.getDepthAt((int)mid.x, (int)mid.y);
-            Int16 depthP2 = depthImage.getDepthAt((int)p2.x, (int)p2.y);
+            Int16 depthP1 = depthImage.getDepthAt((int)p1.X, (int)p1.Y);
+            Int16 depthMid = depthImage.getDepthAt((int)mid.X, (int)mid.Y);
+            Int16 depthP2 = depthImage.getDepthAt((int)p2.X, (int)p2.Y);
             return (depthP1 > depthMid && depthMid < depthP2);
         }
 
-        private void findFingers()
-        {
-            int width = depthImage.getWidth();
-            int height = depthImage.getHeight();
-
-            fingerPoints = fingerPoints.Dilate(1);
-
-            double rhoResolution = 1;
-            double thetaResolution = Math.PI / 90.0;
-            int threshold = 10;
-            double minLineWidth = 20;
-            double gapBetweenLines = 2;
-
-            LineSegment2D[] lines = fingerPoints.HoughLinesBinary(rhoResolution, thetaResolution, threshold, minLineWidth, gapBetweenLines)[0];
+        private void findFingerLines() {
+            float maxCombinableAngle = (float) (Math.PI * 20 / 180);
+            float maxCombinableParallelDistance = 10;
+            float maxCombinableVerticalDistance = 10;
 
-            fingers = new List<Finger>();
-
-            foreach (LineSegment2D line in lines) {
-                Vector2D p1 = new Vector2D(line.P1.X, line.P1.Y);
-                Vector2D p2 = new Vector2D(line.P2.X, line.P2.Y);
-                Finger finger = new Finger(p1, p2);
+            fingerPointsImage = fingerPointsImage.Dilate(1);
+            LineSegment2D[] lineSegments = fingerPointsImage.HoughLinesBinary(1, Math.PI / 90, 10, 20, 2)[0];
 
-                List<Finger> mergeableFingers = new List<Finger>();
-
-                foreach (Finger f in fingers) {
-                    if (finger.isMergeable(f))
-                        mergeableFingers.Add(f);
+            fingerLines = new List<Line2D>();
+            foreach (LineSegment2D lineSegment in lineSegments)
+            {
+                Line2D line = new Line2D(new Vector2D(lineSegment.P1), new Vector2D(lineSegment.P2));
+
+                List<Line2D> combineableLines = new List<Line2D>();
+                foreach(Line2D fingerLine in fingerLines) {
+                    float angle = line.getAngleBetween(fingerLine);
+                    if (angle <= maxCombinableAngle) {
+                        float parallelDistance = line.getParallelDistanceTo(fingerLine);
+                        if (parallelDistance <= maxCombinableParallelDistance) {
+                            float verticalDistance = line.getVerticalDistanceTo(fingerLine);
+                            if (verticalDistance <= maxCombinableVerticalDistance) {
+                                combineableLines.Add(fingerLine);
+                            }
+                        }
+                    }
+                }
+                
+                if (combineableLines.Count == 0) {
+                    fingerLines.Add(line);
                 }
-
-                if (mergeableFingers.Count == 0)
-                    fingers.Add(finger);
-                else if (mergeableFingers.Count == 1)
-                    mergeableFingers[0].mergeFingers(finger);
                 else {
-                    foreach (Finger mf in mergeableFingers) {
-                        finger.mergeFingers(mf);
-                        fingers.Remove(mf);
+                    foreach (Line2D mergableLine in combineableLines) {
+                        fingerLines.Remove(mergableLine);
                     }
-                    fingers.Add(finger);
+                    combineableLines.Add(line);
+                    fingerLines.Add(new Line2D(combineableLines));
                 }
             }
+        }
+
+        private void findFingers()
+        {
+            float minLength = 20;
+            float minNumCombinedLines = 2;
 
-            //draw fingers in fingerImage
-            foreach(Finger finger in fingers) {
-                fingerImage.drawLine(finger.getTipPoint(), finger.getHandPoint(), FingerImageState.fingerDetected);
+            fingers = new List<Finger>();
+            foreach (Line2D line in fingerLines) {
+                if (line.Length >= minLength && line.NumCombinedLines >= minNumCombinedLines)
+                {
+                    fingers.Add(new Finger(line));
+                    fingerImage.drawLine(line, FingerImageState.fingerDetected);
+                }
             }
+
         }
     }
 }

bbiwarg/Detectors/Fingers/FingerTracker.cs

@@ -63,7 +63,7 @@ namespace bbiwarg.Detectors.Fingers
                 }
                 if (tracked)
                 {
-                    fingerImage.drawLine(finger.getTipPoint(), finger.getHandPoint(), FingerImageState.fingerTracked);
+                    fingerImage.drawLine(finger.Line, FingerImageState.fingerTracked);
                     trackedFingers.Add(finger);
                 }
             }

bbiwarg/Detectors/Touch/TouchDetector.cs

@@ -29,18 +29,18 @@ namespace bbiwarg.Detectors.Touch
             foreach (Finger finger in fingers) {
                  Vector2D tipPoint = finger.getTipPoint();
 
-                float floodValue = getFloodValue((int)tipPoint.x, (int)tipPoint.y);
+                float floodValue = getFloodValue((int)tipPoint.X, (int)tipPoint.Y);
                 if (floodValue > floodValueThreshold)
                 {
                     //correct touchEvent position
-                    Vector2D direction = finger.getDirection();
+                    Vector2D direction = finger.Line.Direction;
                     float directionFactor = -10;
-                    float x = Math.Min(Math.Max(tipPoint.x + directionFactor * direction.x, 0), depthImage.getWidth()-1);
-                    float y = Math.Min(Math.Max(tipPoint.y + directionFactor * direction.y, 0), depthImage.getHeight()-1);
+                    float x = Math.Min(Math.Max(tipPoint.X + directionFactor * direction.X, 0), depthImage.getWidth()-1);
+                    float y = Math.Min(Math.Max(tipPoint.Y + directionFactor * direction.Y, 0), depthImage.getHeight()-1);
                     Vector2D tep = new Vector2D(x,y);
 
-                    touchImage.setTouchAt((int)tep.x, (int)tep.y, TouchImageState.touchDetected);
-                    TouchEvent touchEvent = new TouchEvent((int)tep.x, (int)tep.y, floodValue, finger);
+                    touchImage.setTouchAt((int)tep.X, (int)tep.Y, TouchImageState.touchDetected);
+                    TouchEvent touchEvent = new TouchEvent((int)tep.X, (int)tep.Y, floodValue, finger);
                     touchEvents.Add(touchEvent);
                 }
             }

bbiwarg/Images/EdgeImage.cs

@@ -17,7 +17,7 @@ namespace bbiwarg.Images
             int maxDepth = depthImage.getMaxDepth();
             Image<Gray, Int16> depthImageInt16 = depthImage.getImage();
             Image<Gray, Byte> depthImageByte = depthImageInt16.Convert<Byte>(delegate(Int16 depth) { return (byte)(((depth - minDepth) * Byte.MaxValue) / (maxDepth - minDepth)); });
-            image = depthImageByte.Canny(150, 75, 3);
+            image = depthImageByte.Canny(100, 75, 3);
         }
 
         public bool isEdgeAt(int x, int y) {

bbiwarg/Images/FingerImage.cs

@@ -34,13 +34,16 @@ namespace bbiwarg.Images
             return (FingerImageState)image.Data[y, x, 0];
         }
 
-        public void drawLine(Vector2D start, Vector2D end, FingerImageState state)
+        public void drawLine(Line2D line, FingerImageState state)
         {
+            Vector2D start = line.P1;
+            Vector2D end = line.P2;
+
             int width = image.Width;
             int height = image.Height;
 
             // bresenham from wikipedia
-            int xstart = (int)start.x, xend = (int)end.x, ystart = (int)start.y, yend = (int)end.y;
+            int xstart = (int)start.X, xend = (int)end.X, ystart = (int)start.Y, yend = (int)end.Y;
             int x, y, t, dx, dy, incx, incy, pdx, pdy, ddx, ddy, es, el, err;
 
             /* Entfernung in beiden Dimensionen berechnen */

bbiwarg/Images/PalmImage.cs

@@ -27,8 +27,8 @@ namespace bbiwarg.Images
             
             foreach (Finger f in fingerTracker.getFingers())
             {
-                Vector<float> tip = new Vector<float>(new float[] { f.getTipPoint().x, f.getTipPoint().y });
-                Vector<float> hand = new Vector<float>(new float[] { f.getHandPoint().x, f.getHandPoint().y });
+                Vector<float> tip = new Vector<float>(new float[] { f.getTipPoint().X, f.getTipPoint().Y });
+                Vector<float> hand = new Vector<float>(new float[] { f.getHandPoint().X, f.getHandPoint().Y });
 
 
                 for (int x = 0; x < width; ++x)

bbiwarg/Utility/Line2D.cs

@@ -0,0 +1,144 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace bbiwarg.Utility
+{
+    public enum LineSide {
+        onLine = 0,
+        above = 1,
+        below = 2
+    }
+
+    class Line2D
+    {
+        private Vector2D p1;
+        private Vector2D p2;
+        private Vector2D direction;
+        private float length;
+        private int numCombinedLines;
+        public Vector2D P1 { get { return p1; } private set { p1 = value; } }
+        public Vector2D P2 { get { return p2; } private set { p2 = value; } }
+        public Vector2D Direction { get { return direction; } private set { direction = value; } }
+        public float Length { get { return length; } private set { length = value; } }
+        public int NumCombinedLines { get { return numCombinedLines; } private set { numCombinedLines = value; } }
+
+        public Line2D(Vector2D p1, Vector2D p2) {
+            setPoints(p1, p2);
+            NumCombinedLines = 1;
+        }
+
+        public Line2D(List<Line2D> lines) {
+            //list all points
+            List<Vector2D> points = new List<Vector2D>();
+            List<Vector2D> directions = new List<Vector2D>();
+            NumCombinedLines = 0;
+            foreach (Line2D line in lines) {
+                points.Add(line.P1);
+                points.Add(line.P2);
+                directions.Add(line.Direction);
+                NumCombinedLines += line.NumCombinedLines;
+            }
+
+            //find two points with max-distance
+            Vector2D a = points[0];
+            Vector2D b = points[0];
+            float length = 0;
+            foreach (Vector2D p in points) {
+                float distanceToA = p.getDistanceTo(a);
+                float distanceToB = p.getDistanceTo(b);
+
+                if (distanceToA > length && distanceToA >= distanceToB) {
+                    b = p;
+                    length = distanceToA;
+                }
+                else if (distanceToB > length && distanceToB > distanceToA) {
+                    a = p;
+                    length = distanceToA;
+                }
+            }
+
+            //project a and b on mean-line
+            Vector2D meanPoint = Vector2D.mean(points);
+            Vector2D meanDirection = Vector2D.mean(directions);
+
+            Line2D meanLine = new Line2D(meanPoint, meanPoint + meanDirection);
+            Vector2D meanA = meanLine.projectToLine(a);
+            Vector2D meanB = meanLine.projectToLine(b);
+
+            setPoints(meanA, meanB);
+        }
+
+        private void setPoints(Vector2D p1, Vector2D p2) {
+            //endpoints
+            P1 = p1;
+            P2 = p2;
+
+            //direction
+            direction = (P1-P2).normalize();
+
+            //length
+            Length = P1.getDistanceTo(P2);
+        }
+
+        public float getAngleBetween(Line2D line) {
+            float angle = direction.getAngleBetween(line.Direction);
+            return Math.Min(angle, 180-angle);
+        }
+
+        public float getParallelDistanceTo(Line2D line) {
+            if (onSide(line.P1) != onSide(line.P2)) return 0;
+
+            Vector2D a1 = projectToLine(line.P1);
+            Vector2D a2 = projectToLine(line.P2);
+
+            float distanceA1 = a1.getDistanceTo(line.P1);
+            float distanceA2 = a2.getDistanceTo(line.P2);
+            return Math.Min(distanceA1, distanceA2);
+        }
+
+        public float getVerticalDistanceTo(Line2D line) {
+            Vector2D a1 = projectToLine(line.P1);
+            Vector2D a2 = projectToLine(line.P2);
+
+            if (P1.isInBox(a1, a2) || P2.isInBox(a1, a2)) return 0;
+
+            float distanceP1A1 = P1.getDistanceTo(a1);
+            float distanceP1A2 = P1.getDistanceTo(a2);
+            float distanceP2A1 = p2.getDistanceTo(a1);
+            float distanceP2A2 = p2.getDistanceTo(a2);
+            return Math.Min(Math.Min(distanceP1A1, distanceP1A2), Math.Min(distanceP2A1, distanceP2A2));
+        }
+
+        public LineSide onSide(Vector2D point) {
+            float yPerX = direction.Y / direction.X;
+            float xDiff = point.X - P1.X;
+            float newY = P1.Y + yPerX * xDiff;
+            if (newY < point.Y) return LineSide.above;
+            else if (newY > point.Y) return LineSide.below;
+            else return LineSide.onLine;
+        }
+
+        public Vector2D projectToLine(Vector2D point) {
+            float px = point.X, py = point.Y, dx = Direction.X, dy = Direction.Y, ox = P1.X, oy = P1.Y;
+            float diffx = px - ox;
+            float diffy = py - oy;
+
+            float diff_d = (diffx * dx + diffy * dy);
+            float d_d = (dx * dx + dy * dy);
+            float q = diff_d / d_d;
+
+            float newX = ox + q * dx;
+            float newY = oy + q * dy;
+
+            return new Vector2D(newX, newY);
+        }
+
+        public override string ToString()
+        {
+            return (int)P1.X + "|" + (int)P1.Y + " --- " + (int)P2.X + "|" + (int)P2.Y;
+        }
+    }
+}

bbiwarg/Utility/Vector2D.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Drawing;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
@@ -8,68 +9,83 @@ namespace bbiwarg.Utility
 {
     class Vector2D
     {
-        public float x;
-        public float y;
+        private float x;
+        private float y;
+        private float length;
+        public float X { get { return x; } private set { x = value; } }
+        public float Y { get { return y; } private set { y = value; } }
+        public float Length { get { return length; } private set { length = value; } }
 
         public Vector2D(float x, float y) {
-            this.x = x;
-            this.y = y;
+            setXY(x, y);
         }
 
-        public float getLength() {
-            return (float) Math.Sqrt(x * x + y * y);
+        public Vector2D(Point point) {
+            setXY(point.X, point.Y);
         }
 
-        public float getDistanceTo(Vector2D vector) {
-            float xDiff = x - vector.x;
-            float yDiff = y - vector.y;
-            return (float)Math.Sqrt(xDiff * xDiff + yDiff * yDiff);
+        private void setXY(float x, float y) {
+            X = x;
+            Y = y;
+            Length = (float)Math.Sqrt(X * X + Y * Y);
         }
 
-        public float getMinAngleBetween(Vector2D vector) {
-            float angle = getAngleBetween(vector);
-            if (angle > Math.PI/2)
-                return (float) (Math.PI - angle);
-            else
-                return angle;
+        public float getDistanceTo(Vector2D point) {
+            return (this - point).Length;
         }
 
         public float getAngleBetween(Vector2D vector) {
-            return (float)Math.Acos(dotProduct(vector) / (getLength() * vector.getLength()));
+            return (float)Math.Acos(dotProduct(vector) / (Length * vector.Length));
         }
 
         public float dotProduct(Vector2D vector) {
-            return x * vector.x + y * vector.y;
+            return X * vector.X + Y * vector.Y;
         }
 
-        public static Vector2D projectToLine(Vector2D point, Vector2D direction, Vector2D pointOnLine) {
-            float px = point.x, py = point.y, dx = direction.x, dy = direction.y, ox = pointOnLine.x, oy = pointOnLine.y;
-            float diffx = px - ox;
-            float diffy = py - oy;
+        public bool isInBox(Vector2D corner1, Vector2D corner2) {
+            float minX = Math.Min(corner1.X, corner2.X);
+            float maxX = Math.Max(corner1.X, corner2.X);
+            float minY = Math.Min(corner1.Y, corner2.Y);
+            float maxY = Math.Max(corner1.Y, corner2.Y);
+            return (minX <= X && X <= maxX && minY <= Y && Y <= maxY);
+        }
 
-            float diff_d = (diffx * dx + diffy * dy);
-            float d_d = (dx * dx + dy * dy);
-            float q = diff_d / d_d;
+        public Vector2D normalize() {
+            return new Vector2D(X / Length, Y / Length);
+        }
 
-            float newX = ox + q * dx;
-            float newY = oy + q * dy;
+        public static Vector2D operator *(float scalar, Vector2D vector)
+        {
+            return new Vector2D(scalar * vector.X, scalar * vector.Y);
+        }
 
-            return new Vector2D(newX, newY);
+        public static Vector2D operator *(Vector2D vector, float scalar)
+        {
+            return new Vector2D(scalar * vector.X, scalar * vector.Y);
         }
 
-        public static Vector2D operator *(float scalar, Vector2D vector)
+        public static Vector2D operator /(Vector2D vector, float scalar)
         {
-            return new Vector2D(scalar * vector.x, scalar * vector.y);
+            return new Vector2D(vector.X / scalar, vector.Y / scalar);
         }
 
         public static Vector2D operator +(Vector2D vector1, Vector2D vector2)
         {
-            return new Vector2D(vector1.x + vector2.x, vector1.y + vector2.y);
+            return new Vector2D(vector1.X + vector2.X, vector1.Y + vector2.Y);
         }
 
         public static Vector2D operator -(Vector2D vector1, Vector2D vector2)
         {
-            return new Vector2D(vector1.x - vector2.x, vector1.y - vector2.y);
+            return new Vector2D(vector1.X - vector2.X, vector1.Y - vector2.Y);
         }
+
+        public static Vector2D mean(List<Vector2D> vectors) {
+            Vector2D meanVector = new Vector2D(0,0);
+            foreach (Vector2D vector in vectors) {
+                meanVector += vector;
+            }
+            return meanVector / vectors.Count;
+        }
+
     }
 }

bbiwarg/bbiwarg.csproj

@@ -83,6 +83,7 @@
     <Compile Include="InputProvider\IisuInputProvider.cs" />
     <Compile Include="MainBBWIWARG.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Utility\Line2D.cs" />
     <Compile Include="Utility\Vector.cs" />
     <Compile Include="Utility\Vector2D.cs" />
     <Compile Include="VideoHandle.cs" />