public Vector3D CrossProduct(Vector3D vector)
        {
            return new Vector3D(
                    (this.Y * vector.Z - this.Z * vector.Y),
                    (this.Z * vector.X - this.X * vector.Z),
                    (this.X * vector.Y - this.Y * vector.X)

                );
        }
 public Vector3D Add(Vector3D vector)
 {
     if (vector == null)
     {
         return this;
     }
     return new Vector3D(
         this.X + vector.X,
         this.Y + vector.Y,
         this.Z + vector.Z
     );
 }
        /// <summary>
        /// This method is used to calculate the RealWorldCalculation-coordinates for a given KinectPoint
        /// When to use: use only to calculate the corner points' RealWorldCalculation-coordinates. For other KinectPoints use the other "CalculateRealWorldVector" 
        /// method.
        /// How to use: The parameters rwA, rwB and rwC represent the corner points of the detected field. rwB is the RealWorld-point which is a direct
        /// of the other RealWorld-points rwA and rwC. Direct means, that those point can be reached from rwB by following the adjacent outlines of the field.
        /// Important: The passed KinectPoint must not be NULL!
        /// </summary>
        /// <param name="p">the KinectPoint which should be transformed to a RealWorldPoint </param>
        /// <param name="rA">the RealWorldPoint A which represents a corner point and 
        ///     is used to calculate the coordinates for the new RealWorldPoints </param>
        /// <param name="rB">the RealWorldPoint B which represents a corner point and 
        ///     is used to calculate the coordinates for the new RealWorldPoints </param>
        /// <param name="rC">the RealWorldPoint C which represents a corner point and 
        ///     is used to calculate the coordinates for the new RealWorldPoints </param>
        /// <returns>Returns the RealWorldPoint corresponding to the passed KinectPoint</returns>
        private RealWorldPoint CalculateRealWorldPoint(KinectPoint p, RealWorldPoint rA, RealWorldPoint rB, RealWorldPoint rC)
        {
            var rwPoint = new RealWorldPoint();
            var vec_rA = CreateRealWorldVector(rA);
            var vec_rB = CreateRealWorldVector(rB);
            var vec_rC = CreateRealWorldVector(rC);

            var vec_N = (vec_rA.Subtract(vec_rB)).CrossProduct(vec_rC.Subtract(vec_rB));
            var q = vec_N.ScalarProduct(vec_rB);

            var x = (int)((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2)) /Kinect.WIDTH_CONST);
            var y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) /Kinect.HEIGHT_CONST);

            var vec_rP = new Vector3D(x,y,1);

            rwPoint.Z = (int)(q / (vec_N.ScalarProduct(vec_rP)));

            rwPoint.X = (int) ((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2))*rwPoint.Z/Kinect.WIDTH_CONST);
            rwPoint.Y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) * rwPoint.Z /Kinect.HEIGHT_CONST);

            return rwPoint;
        }
 public Vector3D Subtract(Vector3D vector)
 {
     return new Vector3D(
         this.X - vector.X,
         this.Y - vector.Y,
         this.Z - vector.Z
     );
 }
 public double ScalarProduct(Vector3D vector)
 {
     return (double) (this.X*vector.X + this.Y*vector.Y + this.Z*vector.Z);
 }
        private Vector3D[] GetOriginPoint(Vector3D rwvA, Vector3D rwvB, Vector3D rwvC, Vector3D rwvD)
        {
            Vector3D[] coordinateSystemPoints = new Vector3D[3];

            var dictVectorsA = new Dictionary<Vector3D, int>();
            dictVectorsA.Add(rwvB, (int)(rwvB.Subtract(rwvA)).GetLength());
            dictVectorsA.Add(rwvD, (int)(rwvD.Subtract(rwvA)).GetLength());
            int sumA = dictVectorsA.Values.Sum();

            var dictVectorsB = new Dictionary<Vector3D, int>();
            dictVectorsB.Add(rwvA, (int)(rwvA.Subtract(rwvB)).GetLength());
            dictVectorsB.Add(rwvC, (int)(rwvC.Subtract(rwvB)).GetLength());
            int sumB = dictVectorsB.Values.Sum();

            var dictVectorsC = new Dictionary<Vector3D, int>();
            dictVectorsC.Add(rwvB, (int)(rwvB.Subtract(rwvC)).GetLength());
            dictVectorsC.Add(rwvD, (int)(rwvD.Subtract(rwvC)).GetLength());
            int sumC = dictVectorsC.Values.Sum();

            var dictVectorsD = new Dictionary<Vector3D, int>();
            dictVectorsD.Add(rwvC, (int)(rwvC.Subtract(rwvD)).GetLength());
            dictVectorsD.Add(rwvA, (int)(rwvA.Subtract(rwvD)).GetLength());
            int sumD = dictVectorsD.Values.Sum();

            if (sumA < sumB && sumA < sumC && sumA < sumD)
            {
                coordinateSystemPoints[0] = rwvA;
                coordinateSystemPoints[1] = dictVectorsA.FirstOrDefault((x) => x.Value == dictVectorsA.Values.Max()).Key;
                coordinateSystemPoints[2] = dictVectorsA.FirstOrDefault((x) => x.Value == dictVectorsA.Values.Min()).Key;
            }
            else if (sumB < sumA && sumB < sumC && sumB < sumD)
            {
                coordinateSystemPoints[0] = rwvB;
                coordinateSystemPoints[1] = dictVectorsB.FirstOrDefault((x) => x.Value == dictVectorsB.Values.Max()).Key;
                coordinateSystemPoints[2] = dictVectorsB.FirstOrDefault((x) => x.Value == dictVectorsB.Values.Min()).Key;
            }
            else if (sumC < sumA && sumC < sumB && sumC < sumD)
            {
                coordinateSystemPoints[0] = rwvC;
                coordinateSystemPoints[1] = dictVectorsC.FirstOrDefault((x) => x.Value == dictVectorsC.Values.Max()).Key;
                coordinateSystemPoints[2] = dictVectorsC.FirstOrDefault((x) => x.Value == dictVectorsC.Values.Min()).Key;
            }
            else
            {
                coordinateSystemPoints[0] = rwvD;
                coordinateSystemPoints[1] = dictVectorsD.FirstOrDefault((x) => x.Value == dictVectorsD.Values.Max()).Key;
                coordinateSystemPoints[2] = dictVectorsD.FirstOrDefault((x) => x.Value == dictVectorsD.Values.Min()).Key;
            }

            var originPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[0].ToRealWorldPoint()));
            var xAxisPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[1].ToRealWorldPoint()));
            var yAxisPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[2].ToRealWorldPoint()));

            originPoint.PointType = PointType.Origin;
            xAxisPoint.PointType = PointType.xAxis;
            yAxisPoint.PointType = PointType.yAxis;

            return coordinateSystemPoints;
        }