CrisisAtSwissStation.PhysicsObject.mapPointOnImage C# (CSharp) Method

        public Vector2 mapPointOnImage(Vector2 point)
        {
            Vector2 newPoint = new Vector2(point.X, point.Y);

            //Scale
            //newPoint = newPoint * ScaleVector;

            //The next thing we do is rotate. Since the origin for the rotation is the center, we need to
            //untranslate the image (we did this in the line above), and then retranslate it to the center.
            //float cos_theta = (float)System.Math.Cos(Angle);
            //float sin_theta = (float)System.Math.Sin(Angle);

            Vector2 center = new Vector2(boundingBox.Center.X*scale, boundingBox.Center.Y*scale);
            newPoint = newPoint - center;

            //unscale before rotating
            newPoint = newPoint / scale;

            double theta = System.Math.Atan2(newPoint.Y, newPoint.X) + Angle;
            Vector2 rotatedpos = new Vector2((float)(newPoint.Length() * System.Math.Cos(theta)), (float)(newPoint.Length() * System.Math.Sin(theta)));
            newPoint = rotatedpos;

            //rescale after rotating
            newPoint = newPoint * scale;

            /*
            newPoint = new Vector2(
                newPoint.X * cos_theta - newPoint.Y * sin_theta,
                newPoint.X * sin_theta + newPoint.Y * cos_theta
                );
             * */

            newPoint = newPoint + center;

            newPoint = newPoint + (Position * CASSWorld.SCALE);

            return newPoint;
        }