Merge branch 'master' of https://git.tk.informatik.tu-darmstadt.de/etri-smartspaces

bbiwarg/Constants.cs

@@ -10,7 +10,7 @@ namespace bbiwarg
     class Constants
     {
         // BBIWARG
-        public static readonly bool OutputTimerEnabled = true;
+        public static readonly bool OutputTimerEnabled = false;
         public static readonly int OutputNumImages = 5;
         public static readonly int OutputNumImagesPerRow = 3;
         public static readonly float OutputScaleFactor = 1f; // output window size is scaled by this factor (from necessary size for images)
@@ -33,7 +33,7 @@ namespace bbiwarg
         public static readonly int FingerStepSize = 2;
         public static readonly int FingerMinNumSlices = 30 / FingerStepSize;
         public static readonly int FingerRemoveNumSlicesForCorrection = 5;
-        public static readonly int FingerMaxGapCounter = 10;
+        public static readonly int FingerMaxGapCounter = 5;
         public static readonly int FingerMaxSliceDifferencePerStep = 5;
         public static readonly int FingerMaxSize = 30;
         public static readonly int FingerMinSize = 5;
@@ -42,6 +42,8 @@ namespace bbiwarg
         public static readonly int FingerOutSliceFactor = 10;
         public static readonly int FingerContourMargin = 2;
         public static readonly int FingerSliceOverlapFactor = 2;
+        public static readonly int FingerCrippleOutFactor = 8;
+        public static readonly int FingerCrippleOutMinDifference = 15;
         public static readonly float FingerMinSimilarityForTracking = 0.75f;
 
         // hand detection
@@ -80,6 +82,7 @@ namespace bbiwarg
         public static readonly Color FingerTrackedColor = ColorTracked;
         public static readonly Color FingerTipOutSliceColor = Color.Gray;
         public static readonly Color FingerHandOutSliceColor = Color.DarkSlateGray;
+        public static readonly Color FingerContourColor = Color.Red;
 
         public static readonly Color TouchEventDetectedColor = ColorDetected;
         public static readonly Color TouchEventTrackedColor = ColorTracked;

bbiwarg/Detectors/FingerDetection/Finger.cs

@@ -12,42 +12,22 @@ namespace bbiwarg.Detectors.FingerDetection
 {
     class Finger
     {
-        public Vector2D TipPoint { get { return SliceTrail.Start.Mid; } }
-        public Vector2D HandPoint { get { return SliceTrail.End.Mid; } }
+        public Vector2D TipPoint { get { return SliceTrail.StartSlice.Mid; } }
+        public Vector2D HandPoint { get { return SliceTrail.EndSlice.Mid; } }
+        public Vector2D Direction { get { return LineSegment.Direction; } }
         public LineSegment2D LineSegment { get { return SliceTrail.LineSegment; } }
         public FingerSliceTrail SliceTrail { get; private set; }
+        public Contour<Point> Contour { get { return SliceTrail.getContour(); } }
 
         public Finger(FingerSliceTrail sliceTrail)
         {
             //check direction
-            if (sliceTrail.Start.Length > sliceTrail.End.Length)
-                sliceTrail.Slices.Reverse();
+            if (sliceTrail.StartSlice.Length > sliceTrail.EndSlice.Length)
+                sliceTrail.reverse();
 
             SliceTrail = sliceTrail;
         }
 
-        public Contour<Point> getContour()
-        {
-            List<Point> pointsA = new List<Point>();
-            List<Point> pointsB = new List<Point>();
-            foreach (FingerSlice slice in SliceTrail.Slices)
-            {
-                Vector2D direction = (slice.End - slice.Start).normalize();
-                pointsA.Add(slice.Start - Constants.FingerContourMargin * direction);
-                pointsB.Add(slice.End + Constants.FingerContourMargin * direction);
-            }
-
-            pointsA.Reverse();
-            pointsA.AddRange(pointsB);
-
-            Contour<Point> contour = new Contour<Point>(new MemStorage());
-            foreach (Point p in pointsA)
-            {
-                contour.Push(p);
-            }
-            return contour;
-        }
-
         //TODO: redo (not use similarity but actual distances and angle instead)
         public float getSimilarity(Finger compareFinger)
         {

bbiwarg/Detectors/FingerDetection/FingerDetector.cs

@@ -28,13 +28,14 @@ namespace bbiwarg.Detectors.FingerDetection
             this.edgeImageAdapted = edgeImage.copy();
             this.outputImage = outputImage;
 
-            findFingers();
+            detectFingers();
+            drawFingers();
         }
 
-        private void findFingers()
+        private void detectFingers()
         {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
+            int maxX = depthImage.BottomRight.IntX;
+            int maxY = depthImage.BottomRight.IntY;
 
             Fingers = new List<Finger>();
 
@@ -48,7 +49,7 @@ namespace bbiwarg.Detectors.FingerDetection
                         Vector2D edgeDirection = getEdgeDirection(edgePoint);
                         if (edgeDirection != null)
                         {
-                            Vector2D dir = edgeDirection.getOrthogonal(true);
+                            Vector2D dir = edgeDirection.getOrthogonal();
                             if (depthImage.getDepthAt(edgePoint - dir) < depthImage.getDepthAt(edgePoint + dir))
                                 dir = dir.getInverse();
 
@@ -81,15 +82,12 @@ namespace bbiwarg.Detectors.FingerDetection
 
         private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
         {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
-
             FingerSliceTrail trail = new FingerSliceTrail(startSlice);
 
             Vector2D direction = startDirection;
             Vector2D position = startSlice.Mid + Constants.FingerStepSize * direction;
 
-            if (position.isWithin(0, 0, maxX, maxY))
+            if (position.isInBound(Vector2D.Zero, depthImage.BottomRight))
             {
                 FingerSlice nextSlice = findFingerSliceFromMid(position, direction);
                 if (nextSlice != null)
@@ -99,10 +97,10 @@ namespace bbiwarg.Detectors.FingerDetection
 
                     if (trail.NumSlices > Constants.FingerMinNumSlices / 2)
                     {
-                        trail.Slices.RemoveRange(0, Constants.FingerRemoveNumSlicesForCorrection);
-                        trail.Slices.Reverse();
-                        trail = expandTrail(trail, true);
-                        trail.Slices.Reverse();
+                        trail.removeFirstSlices(Constants.FingerRemoveNumSlicesForCorrection);
+                        trail.reverse();
+                        trail = expandTrail(trail);
+                        trail.reverse();
                         return trail;
                     }
                 }
@@ -112,26 +110,20 @@ namespace bbiwarg.Detectors.FingerDetection
 
         }
 
-        private FingerSliceTrail expandTrail(FingerSliceTrail trail, bool reversed = false)
+        private FingerSliceTrail expandTrail(FingerSliceTrail trail)
         {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
-
             Vector2D currentDirection = trail.getEndDirection();
-            Vector2D currentPosition = trail.End.Mid + Constants.FingerStepSize * currentDirection;
+            Vector2D currentPosition = trail.EndSlice.Mid + Constants.FingerStepSize * currentDirection;
 
             int gapCounter = 0;
             int numSlices = trail.NumSlices;
 
-            FingerSlice lastSlice = trail.End;
+            FingerSlice lastSlice = trail.EndSlice;
             FingerSlice nextSlice;
 
-            while (currentPosition.isWithin(0, 0, maxX, maxY) && gapCounter <= Math.Min(numSlices, Constants.FingerMaxGapCounter))
+            while (currentPosition.isInBound(Vector2D.Zero, depthImage.BottomRight) && gapCounter <= Math.Min(numSlices, Constants.FingerMaxGapCounter))
             {
-                if (reversed)
-                    nextSlice = findFingerSliceFromMid(currentPosition, currentDirection.getInverse());
-                else
-                    nextSlice = findFingerSliceFromMid(currentPosition, currentDirection);
+                nextSlice = findFingerSliceFromMid(currentPosition, currentDirection);
 
                 if (nextSlice != null && Math.Abs(nextSlice.Length - lastSlice.Length) <= Constants.FingerMaxSliceDifferencePerStep)
                 {
@@ -160,73 +152,29 @@ namespace bbiwarg.Detectors.FingerDetection
             Vector2D dirStart = direction.getOrthogonal(true);
             Vector2D dirEnd = direction.getOrthogonal(false);
 
-            Vector2D start = findNextEdge(position, dirStart);
+            Vector2D start = edgeImageAdapted.findNextEdge(position, dirStart, Constants.FingerMaxSize);
             if (start == null) return null;
 
-            Vector2D end = findNextEdge(position, dirEnd);
+            Vector2D end = edgeImageAdapted.findNextEdge(position, dirEnd, Constants.FingerMaxSize);
             if (end == null) return null;
 
             return getFingerSlice(start, end);
         }
         private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
         {
-            Vector2D end = findNextEdge(start + Constants.FingerSliceOverlapFactor * direction, direction);
+            Vector2D searchStart = start + Constants.FingerSliceOverlapFactor * direction;
+            Vector2D end = edgeImageAdapted.findNextEdge(searchStart, direction, Constants.FingerMaxSize);
             if (end == null) return null;
 
             return getFingerSlice(start, end);
         }
 
-        private Vector2D findNextEdge(Vector2D start, Vector2D direction, bool adaptedEdgeImage = true, bool stopAtMaxFingerSize = true, bool returnBoundIfNoEdge = false)
-        {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
-
-            int maxStepsX;
-            if (direction.X > 0)
-                maxStepsX = (int)((maxX - start.X) / direction.X);
-            else if (direction.X < 0)
-                maxStepsX = (int)(start.X / Math.Abs(direction.X));
-            else
-                maxStepsX = int.MaxValue;
-
-            int maxStepsY;
-            if (direction.Y > 0)
-                maxStepsY = (int)((maxY - start.Y) / direction.Y);
-            else if (direction.Y < 0)
-                maxStepsY = (int)(start.Y / Math.Abs(direction.Y));
-            else
-                maxStepsY = int.MaxValue;
-
-            int maxSteps = Math.Min(maxStepsX, maxStepsY);
-
-            if (stopAtMaxFingerSize)
-                maxSteps = Math.Min(maxSteps, (int)(Constants.FingerMaxSize / direction.Length));
-
-            Vector2D end = new Vector2D(start);
-            for (int i = 0; i < maxSteps; i++)
-            {
-                end += direction;
-
-                if ((adaptedEdgeImage && edgeImageAdapted.isRoughEdgeAt(end)) || (!adaptedEdgeImage && edgeImageOriginal.isRoughEdgeAt(end)))
-                {
-                    return end;
-                }
-            }
-
-            if (returnBoundIfNoEdge)
-                return end;
-            else
-                return null;
-        }
-
         private FingerSlice getFingerSlice(Vector2D start, Vector2D end)
         {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
-
             Vector2D direction = (end - start).normalize();
-            Vector2D beforeStart = (start - Constants.FingerSliceOverlapFactor * direction).moveInBound(0, 0, maxX, maxY);
-            Vector2D behindEnd = (end + Constants.FingerSliceOverlapFactor * direction).moveInBound(0, 0, maxY, maxY);
+            Vector2D directionInv = direction.getInverse();
+            Vector2D beforeStart = (start + Constants.FingerSliceOverlapFactor * directionInv).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction);
+            Vector2D behindEnd = (end + Constants.FingerSliceOverlapFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, directionInv);
 
             FingerSlice slice = new FingerSlice(beforeStart, behindEnd);
             if (slice.Length >= Constants.FingerMinSize && slice.Length <= Constants.FingerMaxSize && fingerSliceDepthTest(slice))
@@ -252,26 +200,37 @@ namespace bbiwarg.Detectors.FingerDetection
             Finger finger = new Finger(trail);
 
             //add finger
-            Fingers.Add(finger);
-
-            //draw finger
-            drawDetectedFinger(finger);
+            if (!isCrippleFinger(finger))
+                Fingers.Add(finger);
 
             //remove edges around detected finger to improve performance
-            edgeImageAdapted.removeEdgesInsidePolygon(finger.getContour().ToArray());
+            edgeImageAdapted.removeEdgesInsidePolygon(finger.Contour.ToArray());
         }
 
-        private FingerSlice findOutSlice(Vector2D start, Vector2D direction)
+        private bool isCrippleFinger(Finger finger)
         {
-            int maxX = depthImage.Width - 1;
-            int maxY = depthImage.Height - 1;
+            FingerSlice midSlice = finger.SliceTrail.MidSlice;
+            Vector2D direction = midSlice.Direction;
+            Vector2D directionInv = direction.getInverse();
+            Vector2D out1 = (midSlice.Start + Constants.FingerCrippleOutFactor * directionInv).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction);
+            Vector2D out2 = (midSlice.End + Constants.FingerCrippleOutFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, directionInv);
+
+            Int16 depthAtFinger = depthImage.getDepthAt(midSlice.Mid);
+            Int16 depthAtOut1 = depthImage.getDepthAt(out1);
+            Int16 depthAtOut2 = depthImage.getDepthAt(out2);
+            int minDepthDifference = Math.Min(Math.Abs(depthAtFinger - depthAtOut1), Math.Abs(depthAtFinger - depthAtOut2));
+
+            return (minDepthDifference < Constants.FingerCrippleOutMinDifference);
+        }
 
+        private FingerSlice findOutSlice(Vector2D start, Vector2D direction)
+        {
             Vector2D dirOrth1 = direction.getOrthogonal(true);
             Vector2D dirOrth2 = direction.getOrthogonal(false);
 
-            Vector2D outPoint = (start + Constants.FingerOutSliceFactor * direction).moveInBound(0, 0, maxX, maxY);
-            Vector2D p1 = findNextEdge(outPoint, dirOrth1, false, false, true);
-            Vector2D p2 = findNextEdge(outPoint, dirOrth2, false, false, true);
+            Vector2D outPoint = (start + Constants.FingerOutSliceFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction.getInverse());
+            Vector2D p1 = edgeImageOriginal.findNextEdge(outPoint, dirOrth1, 0, true, false);
+            Vector2D p2 = edgeImageOriginal.findNextEdge(outPoint, dirOrth2, 0, true, false);
 
             FingerSlice slice = new FingerSlice(p1, p2);
 
@@ -280,11 +239,8 @@ namespace bbiwarg.Detectors.FingerDetection
 
         private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
         {
-            int maxX = depthImage.Width - 2;
-            int maxY = depthImage.Height - 2;
-
-            FingerSlice start = trail.Start;
-            FingerSlice end = trail.End;
+            FingerSlice start = trail.StartSlice;
+            FingerSlice end = trail.EndSlice;
 
             Vector2D direction = (end.Mid - start.Mid).normalize();
 
@@ -294,15 +250,15 @@ namespace bbiwarg.Detectors.FingerDetection
             float startOutLength = float.MaxValue;
             float endOutLength = float.MaxValue;
 
-            if (startOutSlice.Start.isWithin(1, 1, maxX, maxY) && startOutSlice.End.isWithin(1, 1, maxX, maxY))
+            if (startOutSlice.Start.isInBound(Vector2D.Zero, depthImage.BottomRight) && startOutSlice.End.isInBound(Vector2D.Zero, depthImage.BottomRight))
                 startOutLength = startOutSlice.Length;
 
-            if (endOutSlice.Start.isWithin(1, 1, maxX, maxY) && endOutSlice.End.isWithin(1, 1, maxX, maxY))
+            if (endOutSlice.Start.isInBound(Vector2D.Zero, depthImage.BottomRight) && endOutSlice.End.isInBound(Vector2D.Zero, depthImage.BottomRight))
                 endOutLength = endOutSlice.Length;
 
             if (startOutLength <= endOutLength)
             {
-                trail.Slices.Reverse();
+                trail.reverse();
                 outputImage.drawLineSegment(startOutSlice.LineSegment, Constants.FingerHandOutSliceColor);
                 outputImage.drawLineSegment(endOutSlice.LineSegment, Constants.FingerTipOutSliceColor);
             }
@@ -315,15 +271,23 @@ namespace bbiwarg.Detectors.FingerDetection
             return trail;
         }
 
-        private void drawDetectedFinger(Finger finger)
+        private void drawFingers()
+        {
+            foreach (Finger finger in Fingers)
+            {
+                drawFinger(finger);
+            }
+        }
+
+        private void drawFinger(Finger finger)
         {
             FingerSliceTrail trail = finger.SliceTrail;
             for (int i = 0; i < trail.NumSlices; i++)
             {
-                outputImage.drawLineSegment(trail.Slices[i].LineSegment, Constants.FingerSliceColor);
+                outputImage.drawLineSegment(trail[i].LineSegment, Constants.FingerSliceColor);
             }
             outputImage.drawLineSegment(finger.LineSegment, Constants.FingerDetectedColor);
-            outputImage.drawContour(finger.getContour(), Color.Red, 1);
+            outputImage.drawContour(finger.Contour, Constants.FingerContourColor, 1);
         }
     }
 }

bbiwarg/Detectors/FingerDetection/FingerSlice.cs

@@ -12,8 +12,9 @@ namespace bbiwarg.Detectors.FingerDetection
         public Vector2D Start { get; private set; }
         public Vector2D Mid { get; private set; }
         public Vector2D End { get; private set; }
+        public Vector2D Direction { get { return LineSegment.Direction; } }
         public LineSegment2D LineSegment { get; private set; }
-        public float Length { get; private set; }
+        public float Length { get { return LineSegment.Length; } }
 
         public FingerSlice(Vector2D start, Vector2D end)
         {
@@ -21,7 +22,6 @@ namespace bbiwarg.Detectors.FingerDetection
             End = end;
             Mid = (start + end) / 2;
             LineSegment = new LineSegment2D(Start, End);
-            Length = Start.getDistanceTo(End); ;
         }
 
     }

bbiwarg/Detectors/FingerDetection/FingerSliceTrail.cs

@@ -1,36 +1,91 @@
 using System;
 using System.Collections.Generic;
+using System.Drawing;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
 using bbiwarg.Utility;
+using Emgu.CV;
 
 namespace bbiwarg.Detectors.FingerDetection
 {
     class FingerSliceTrail
     {
-        public List<FingerSlice> Slices { get; private set; }
-        public FingerSlice Start { get { return Slices[0]; } }
-        public FingerSlice End { get { return Slices[Slices.Count - 1]; } }
-        public int NumSlices { get { return Slices.Count; } }
-        public LineSegment2D LineSegment { get { return new LineSegment2D(End.Mid, Start.Mid); } }
+        private List<FingerSlice> slices;
+        private List<Point> pointsA;
+        private List<Point> pointsB;
+        public FingerSlice StartSlice { get { return slices[0]; } }
+        public FingerSlice MidSlice { get { return slices[NumSlices / 2]; } }
+        public FingerSlice EndSlice { get { return slices[slices.Count - 1]; } }
+        public FingerSlice this[int index] { get { return slices[index]; } }
+        public int NumSlices { get { return slices.Count; } }
+        public LineSegment2D LineSegment { get; private set; }
 
         public FingerSliceTrail(FingerSlice slice)
         {
-            Slices = new List<FingerSlice>();
-            Slices.Add(slice);
+            slices = new List<FingerSlice>();
+            pointsA = new List<Point>();
+            pointsB = new List<Point>();
+            addSlice(slice);
         }
 
         public void addSlice(FingerSlice slice)
         {
-            Slices.Add(slice);
+            slices.Add(slice);
+            pointsA.Add(slice.Start - Constants.FingerContourMargin * slice.Direction);
+            pointsB.Add(slice.End + Constants.FingerContourMargin * slice.Direction);
+            pointsA.Add(slice.Start);
+            LineSegment = new LineSegment2D(StartSlice.Mid, slice.Mid);
         }
 
-        public Vector2D getEndDirection() {
-            int numSlicesToInnerEnd = Constants.FingerNumSlicesForRelativeDirection;
-            int innerEndIndex = Math.Max(0, NumSlices - numSlicesToInnerEnd);
+        public Vector2D getStartDirection()
+        {
+            int innerStartIndex = Math.Min(NumSlices, Constants.FingerNumSlicesForRelativeDirection);
+            return (StartSlice.Mid - slices[innerStartIndex].Mid).normalize();
+        }
+
+        public Vector2D getEndDirection()
+        {
+            int innerEndIndex = Math.Max(0, NumSlices - Constants.FingerNumSlicesForRelativeDirection);
+            return (EndSlice.Mid - slices[innerEndIndex].Mid).normalize();
+        }
+
+        public void removeFirstSlices(int numSlices)
+        {
+            slices.RemoveRange(0, numSlices);
+            pointsA.RemoveRange(0, numSlices);
+            pointsB.RemoveRange(0, numSlices);
+            LineSegment = new LineSegment2D(StartSlice.Mid, EndSlice.Mid);
+        }
+
+        public void reverse() {
+            slices.Reverse();
+            pointsA.Reverse();
+            pointsB.Reverse();
+
+            List<Point> dummy = pointsA;
+            pointsA = pointsB;
+            pointsB = dummy;
+
+            LineSegment = new LineSegment2D(StartSlice.Mid, EndSlice.Mid);
+        }
+
+
+
+        public Contour<Point> getContour()
+        {
+            pointsA.Reverse();
+            List<Point> points = new List<Point>();
+            points.AddRange(pointsA);
+            points.AddRange(pointsB);
+            pointsA.Reverse();
 
-            return (End.Mid - Slices[innerEndIndex].Mid).normalize();
+            Contour<Point> contour = new Contour<Point>(new MemStorage());
+            foreach (Point p in points)
+            {
+                contour.Push(p);
+            }
+            return contour;
         }
     }
 }

bbiwarg/Detectors/HandDetection/HandDetector.cs

@@ -45,11 +45,11 @@ namespace bbiwarg.Detectors.HandDetection
             foreach (Finger finger in fingers)
             {
                 // TODO: connect contour with other edges
-                Contour<Point> contour = finger.getContour();
+                Contour<Point> contour = finger.Contour;
                 image.FillConvexPoly(contour.ToArray(), new Gray(0));
-                image.DrawPolyline(finger.getContour().ToArray(), true, new Gray(255), 1);
+                image.DrawPolyline(finger.Contour.ToArray(), true, new Gray(255), 1);
 
-                FingerSlice slice = finger.SliceTrail.End;
+                FingerSlice slice = finger.SliceTrail.EndSlice;
                 Vector2D direction = slice.LineSegment.Line.Direction;
                 Vector2D start = slice.Start+(Constants.FingerContourMargin+Constants.FingerSliceOverlapFactor)*direction;
                 Vector2D end = slice.End-(Constants.FingerContourMargin+Constants.FingerSliceOverlapFactor)*direction;
@@ -81,7 +81,7 @@ namespace bbiwarg.Detectors.HandDetection
                     CvInvoke.cvFloodFill(image, finger.HandPoint, new MCvScalar(255), new MCvScalar(1), new MCvScalar(1), out comp, Emgu.CV.CvEnum.CONNECTIVITY.FOUR_CONNECTED, Emgu.CV.CvEnum.FLOODFILL_FLAG.DEFAULT, mask);
                     if (comp.area < maxArea * Constants.HandMaxSize)
                     {
-                        Image<Gray, byte> cropedMask = mask.Copy(new Rectangle(1, 1, width, height)).Mul(255);
+                        Image<Gray, byte> cropedMask = mask.Copy(new Rectangle(1, 1, width, height)).Mul(255).Dilate(1);
                         Hand hand = new Hand(cropedMask, finger);
                         Hands.Add(hand);
                         HandMask = HandMask.Or(cropedMask);

bbiwarg/Detectors/TouchDetection/TouchDetector.cs

@@ -37,8 +37,8 @@ namespace bbiwarg.Detectors.TouchDetection
                 if (floodValue > Constants.TouchEventMinFloodValue)
                 {
                     //correct touchEvent position
-                    Vector2D direction = finger.LineSegment.Line.Direction;
-                    Vector2D tep = (tipPoint + Constants.TouchEventTipCorrectionFactor * direction).moveInBound(0, 0, depthImage.Width - 1, depthImage.Height - 1);
+                    Vector2D direction = finger.Direction;
+                    Vector2D tep = (tipPoint + Constants.TouchEventTipCorrectionFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction.getInverse());
 
                     outputImage.fillCircle(tep.IntX, tep.IntY, 5, Constants.TouchEventDetectedColor);
                     TouchEvent touchEvent = new TouchEvent(tep, floodValue, finger);

bbiwarg/Images/DepthImage.cs

@@ -19,6 +19,7 @@ namespace bbiwarg.Images
         public Image<Gray, byte> BackgroundMask { get; private set; }
         public int Width { get; private set; }
         public int Height { get; private set; }
+        public Vector2D BottomRight { get; private set; }
         public Int16 MinDepth { get; private set; }
         public Int16 MaxDepth { get; private set; }
 
@@ -26,6 +27,7 @@ namespace bbiwarg.Images
         {
             Width = image.Width;
             Height = image.Height;
+            BottomRight = new Vector2D(Width - 1, Height - 1);
 
             image = image.SmoothMedian(Constants.DepthImageMedianSize);
 

bbiwarg/Images/EdgeImage.cs

@@ -57,6 +57,50 @@ namespace bbiwarg.Images
             RoughImage.FillConvexPoly(polygon, new Gray(0));
         }
 
+        public Vector2D findNextEdge(Vector2D start, Vector2D direction, int maxSearchSize = 0, bool roughEdge=true, bool returnNullIfNoEdgeFound=true)
+        {
+            Vector2D max = new Vector2D(Image.Width-1, Image.Height-1);
+            Vector2D maxGrow = (max - start) / direction;
+            Vector2D maxDecline = start / direction.getAbsolute();
+
+            int maxStepsX;
+            if (direction.X > 0)
+                maxStepsX = maxGrow.IntX;
+            else if (direction.X < 0)
+                maxStepsX = maxDecline.IntX;
+            else
+                maxStepsX = int.MaxValue;
+
+            int maxStepsY;
+            if (direction.Y > 0)
+                maxStepsY = maxGrow.IntY;
+            else if (direction.Y < 0)
+                maxStepsY = maxDecline.IntY;
+            else
+                maxStepsY = int.MaxValue;
+
+            int maxSteps = Math.Min(maxStepsX, maxStepsY);
+
+            if (maxSearchSize != 0)
+                maxSteps = Math.Min(maxSteps, maxSearchSize);
+
+            Vector2D end = new Vector2D(start);
+            for (int i = 0; i < maxSteps; i++)
+            {
+                end += direction;
+
+                if((roughEdge && isRoughEdgeAt(end)) || (!roughEdge && isEdgeAt(end)))
+                {
+                    return end;
+                }
+            }
+
+            if (returnNullIfNoEdgeFound)
+                return null;
+            else
+                return end;
+        }
+
         public EdgeImage copy()
         {
             return new EdgeImage(Image.Copy());

bbiwarg/Utility/Line2D.cs

@@ -63,8 +63,8 @@ namespace bbiwarg.Utility
             Vector2D q = line.PointOnLine;
             Vector2D s = line.Direction;
 
-            float r_cross_s = r.cross(s);
-            float q_p_cross_s = (q - p).cross(s);
+            float r_cross_s = r.crossProduct(s);
+            float q_p_cross_s = (q - p).crossProduct(s);
 
             if (r_cross_s == 0.0)
                 return null;

bbiwarg/Utility/LineSegment2D.cs

@@ -11,22 +11,18 @@ namespace bbiwarg.Utility
     {
         public Vector2D P1 { get; private set; }
         public Vector2D P2 { get; private set; }
+        public Vector2D Direction { get { return Line.Direction; } }
         public Line2D Line { get; private set; }
-        public float Length { get { return P1.getDistanceTo(P2); } }
+        public float Length { get; private set; }
 
         public LineSegment2D(Vector2D p1, Vector2D p2)
-        {
-            setPoints(p1, p2);
-        }
-
-        private void setPoints(Vector2D p1, Vector2D p2)
         {
             //endpoints
             P1 = p1;
             P2 = p2;
 
-            //line
-            Line = new Line2D(P1, P2-P1);
+            Line = new Line2D(P1, P2 - P1);
+            Length = P1.getDistanceTo(P2);
         }
 
         public float getParallelDistanceTo(LineSegment2D line)

bbiwarg/Utility/Vector2D.cs

@@ -9,6 +9,7 @@ namespace bbiwarg.Utility
 {
     class Vector2D
     {
+        public static Vector2D Zero { get { return new Vector2D(0, 0); } }
         public float X { get; private set; }
         public float Y { get; private set; }
         public int IntX { get { return (int)X; } }
@@ -54,7 +55,7 @@ namespace bbiwarg.Utility
             return X * vector.X + Y * vector.Y;
         }
 
-        public float cross(Vector2D v)
+        public float crossProduct(Vector2D v)
         {
             return X * v.Y - Y * v.X;
         }
@@ -68,36 +69,30 @@ namespace bbiwarg.Utility
             return (minX <= X && X <= maxX && minY <= Y && Y <= maxY);
         }
 
-        public Vector2D normalize()
-        {
-            float length = Length;
-            return new Vector2D(X / length, Y / length);
+        public bool isInBound(Vector2D topLeft, Vector2D bottomRight) {
+            return (X >= topLeft.X && X <= bottomRight.X && Y >= topLeft.Y && Y <= bottomRight.Y);
         }
 
-        public Vector2D normalizeComponentOne() {
-            if (Math.Abs(X) > Math.Abs(Y))
-                return new Vector2D(1, Y / X);
-            else
-                return new Vector2D(X / Y, 1);
-        } 
-
-        public bool isWithin(float minX, float minY, float maxX, float maxY)
+        /*public bool isOnBound(float minX, float minY, float maxX, float maxY)
         {
-            return (X >= minX && Y >= minY && X <= maxX && Y <= maxY);
-        }
+            return (IntX == minX || IntX == maxX || IntY == minY || IntY == maxY);
+        }*/
 
-        public Vector2D moveInBound(float minX, float minY, float maxX, float maxY)
-        {
-            float x = Math.Min(maxX, Math.Max(minX, X));
-            float y = Math.Min(maxY, Math.Max(minY, Y));
-            return new Vector2D(x, y);
+        public Vector2D moveInBound(Vector2D topLeft, Vector2D bottomRight, Vector2D direction) {
+            Vector2D pos = new Vector2D(X, Y);
+            while (!pos.isInBound(topLeft, bottomRight)) {
+                pos += direction;
+            }
+            return pos;
         }
 
-        public bool isOnBound(float minX, float minY, float maxX, float maxY) {
-            return (IntX == minX || IntX == maxX || IntY == minY || IntY == maxY);
+        public Vector2D normalize()
+        {
+            float length = Length;
+            return new Vector2D(X / length, Y / length);
         }
 
-        public Vector2D getOrthogonal(bool side)
+        public Vector2D getOrthogonal(bool side=true)
         {
             if (side)
                 return new Vector2D(Y, -X);
@@ -110,6 +105,10 @@ namespace bbiwarg.Utility
             return new Vector2D(-X, -Y);
         }
 
+        public Vector2D getAbsolute() {
+            return new Vector2D(Math.Abs(X), Math.Abs(Y));
+        }
+
         public override string ToString()
         {
             return "(" + X + "|" + Y + ")";
@@ -129,6 +128,10 @@ namespace bbiwarg.Utility
         {
             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);
+        }
 
         public static Vector2D operator +(Vector2D vector1, Vector2D vector2)
         {