Accord.Math.Geometry.GeometryTools.GetAngleBetweenVectors C# (CSharp) Method

        public static float GetAngleBetweenVectors( Point startPoint, Point vector1end, Point vector2end )
        {
            float x1 = vector1end.X - startPoint.X;
            float y1 = vector1end.Y - startPoint.Y;

            float x2 = vector2end.X - startPoint.X;
            float y2 = vector2end.Y - startPoint.Y;

            return (float) ( Math.Acos( ( x1 * x2 + y1 * y2 ) / ( Math.Sqrt( x1 * x1 + y1 * y1 ) * Math.Sqrt( x2 * x2 + y2 * y2 ) ) ) * 180.0 / Math.PI );
        }

        // Calculate average error between real angles of the specified quadrilateral and angles of the
        // quadrilateral which is the projection of currently estimated pose
        private float GetError(Point[] imagePoints, Matrix3x3 rotation, Vector3 translation)
        {
            Vector3 v1 = rotation * modelPoints[0] + translation;

            v1.X = v1.X * focalLength / v1.Z;
            v1.Y = v1.Y * focalLength / v1.Z;

            Vector3 v2 = rotation * modelPoints[1] + translation;

            v2.X = v2.X * focalLength / v2.Z;
            v2.Y = v2.Y * focalLength / v2.Z;

            Vector3 v3 = rotation * modelPoints[2] + translation;

            v3.X = v3.X * focalLength / v3.Z;
            v3.Y = v3.Y * focalLength / v3.Z;

            Vector3 v4 = rotation * modelPoints[3] + translation;

            v4.X = v4.X * focalLength / v4.Z;
            v4.Y = v4.Y * focalLength / v4.Z;

            Point[] modeledPoints = new Point[4]
            {
                new Point(v1.X, v1.Y),
                new Point(v2.X, v2.Y),
                new Point(v3.X, v3.Y),
                new Point(v4.X, v4.Y),
            };

            float ia1 = GeometryTools.GetAngleBetweenVectors(imagePoints[0], imagePoints[1], imagePoints[3]);
            float ia2 = GeometryTools.GetAngleBetweenVectors(imagePoints[1], imagePoints[2], imagePoints[0]);
            float ia3 = GeometryTools.GetAngleBetweenVectors(imagePoints[2], imagePoints[3], imagePoints[1]);
            float ia4 = GeometryTools.GetAngleBetweenVectors(imagePoints[3], imagePoints[0], imagePoints[2]);

            float ma1 = GeometryTools.GetAngleBetweenVectors(modeledPoints[0], modeledPoints[1], modeledPoints[3]);
            float ma2 = GeometryTools.GetAngleBetweenVectors(modeledPoints[1], modeledPoints[2], modeledPoints[0]);
            float ma3 = GeometryTools.GetAngleBetweenVectors(modeledPoints[2], modeledPoints[3], modeledPoints[1]);
            float ma4 = GeometryTools.GetAngleBetweenVectors(modeledPoints[3], modeledPoints[0], modeledPoints[2]);

            return((
                       System.Math.Abs(ia1 - ma1) +
                       System.Math.Abs(ia2 - ma2) +
                       System.Math.Abs(ia3 - ma3) +
                       System.Math.Abs(ia4 - ma4)
                       ) / 4);
        }