Форумы GIS-Lab.info

Код: Выделить всё

using System;

using nRsn.Core.Utils;

using nRsn.Map.CoordinateSystem.Elements;



namespace nRsn.Map.CoordinateSystem.Transformations

{

    public class PositionVectorTransformation : IGeocentricTransformation

    {

     

        public virtual CartesianPoint Transform (CartesianPoint sourcePoint, TransformationParameter parameter)

        {

            double m = (1 + parameter.Ds * (Math.Pow (10, -6)));



            var sourceMatrix = InitSourceCoordMatrix (sourcePoint);

            var translationVectorMatrix = InitTranslationVectorMatrix (parameter);

            var rotationMatrix = InitRotationMatrix (parameter);

            var frameRotationMatrix = InitFrameRotationMatrix (parameter);



            var resultMatrix = m * rotationMatrix * sourceMatrix + frameRotationMatrix + translationVectorMatrix;



            return new CartesianPoint (resultMatrix [0, 0], resultMatrix [1, 0], resultMatrix [2, 0]);

        }



     

        protected virtual Matrix InitSourceCoordMatrix (CartesianPoint sourcePoint)

        {

            var matrix = new Matrix (3);

            matrix [0, 0] = sourcePoint.X;

            matrix [1, 0] = sourcePoint.Y;

            matrix [2, 0] = sourcePoint.Z;

            return matrix;

        }



        protected virtual Matrix InitTranslationVectorMatrix (TransformationParameter parameter)

        {

            var matrix = new Matrix (3);

            matrix [0, 0] = parameter.Dx;

            matrix [1, 0] = parameter.Dy;

            matrix [2, 0] = parameter.Dz;



            return matrix;

        }



        protected virtual Matrix InitRotationMatrix (TransformationParameter parameter)

        {

            double rZRad = parameter.Rz / 3600 * (Math.PI / 180.0);

            var matrix = new Matrix (3, 3);

            matrix [0, 0] = 1;

            matrix [0, 1] = -rZRad;

            matrix [0, 2] = parameter.Ry;

            matrix [1, 0] = rZRad;

            matrix [1, 1] = 1;

            matrix [1, 2] = -parameter.Rx;

            matrix [2, 0] = -parameter.Ry;

            matrix [2, 1] = parameter.Rx;

            matrix [2, 2] = 1;

            return matrix;

        }



        protected virtual Matrix InitFrameRotationMatrix (TransformationParameter parameter)

        {

            var matrix = new Matrix (3);

            matrix [0, 0] = parameter.Px;

            matrix [1, 0] = parameter.Py;

            matrix [2, 0] = parameter.Pz;

            return matrix;

        }

    }

}