三维坐标转化

一，点转换:初系的点坐标转换为末系的点坐标

初始坐标系以StartOrigin为原点建立坐标系，末坐标系以FinalOrigin为原点建立坐标系，在初系中FinalOrigin点的坐标为Vector3 FinalOrigin_StartOrigin=new Vector3(x1,y1,z1);，将初系从StartOrigin点平移到FinalOrigin点，然后将初系绕Z轴旋转Yaw度，再绕Y轴旋转Pitch度，最后绕X轴旋转Roll度得到末系。

现在有一个点point在初系中的坐标为Vector3 point_StartOrigin =new Vector3(x2,y2,z2);请问在末系中point点的坐标为多少？

1.获取初系变为末系的转换矩阵：使用GetTransformationMatrix方法根据FinalOrigin_StartOrigin、Yaw、Pitch、Roll可以得到初系转为末系的转换矩阵，

2.用TransformPoint()方法利用转换矩阵转换点的坐标

        public static void Main()
        {
            Vector3 FinalOrigin_StartOrigin=new Vector3(x1,y1,z1);
            Vector3 point_StartOrigin =new Vector3(x2,y2,z2);
            StartToFinal=GetTransformationMatrix(FinalOrigin_StartOrigin,Yaw, Pitch, Roll);
            Vector3 point_FinalOrigin=TransformPoint(point_StartOrigin,StartToFinal);
            
        } 

        public static Matrix4x4 GetTransformationMatrix(Vector3 FinalOrigin, float yaw, float pitch, float roll)// FinalOrigin:初系当中，末系原点的坐标
        {
            // 定义旋转角度（弧度）
            yaw = (float)(yaw * Math.PI / 180); // 90度
            pitch = (float)(pitch * Math.PI / 180); // 0度
            roll = (float)(roll * Math.PI / 180); // 0度
            // 创建单位矩阵
            Matrix4x4 transformationMatrix = Matrix4x4.Identity;
            // 创建平移矩阵
            Vector3 translation = new Vector3(-FinalOrigin.X, -FinalOrigin.Y, -FinalOrigin.Z);
            Matrix4x4 translationMatrix = Matrix4x4.CreateTranslation(translation);
            // 添加平移部分
            transformationMatrix *= translationMatrix;
            // 将C1坐标系绕自身z轴顺时针旋转yaw度
            transformationMatrix *= Matrix4x4.CreateRotationZ((-yaw));
            // 将C1坐标系绕自身y轴顺时针旋转pitch度
            transformationMatrix *= Matrix4x4.CreateRotationY((-pitch));
            // 将C1坐标系绕自身x轴顺时针旋转roll度
            transformationMatrix *= Matrix4x4.CreateRotationX((-roll));
            return transformationMatrix;
        }

        public static Vector3 TransformPoint(Vector3 point, Matrix4x4 transformMatrix)//初坐标系中某点的坐标为point，初系经矩阵变换为末系，求末坐标系中该点的坐标
        {
            Vector4 homogeneousPoint = new Vector4(point, 1);
            Vector4 transformedPoint = Vector4.Transform(homogeneousPoint, transformMatrix);
            Vector3 result = new Vector3(transformedPoint.X, transformedPoint.Y, transformedPoint.Z);
            return result;
        }

注：

1.Vector3 point_FinalOrigin=TransformPoint(new Vector3(0,0,0),StartToFinal);可以求得末系中StartOrigin的坐标

2.使用以下代码求StartToFinal的逆矩阵可以求得末系转换成初系的转换矩阵FinalToStart

        public static void Main()
        {
            Matrix4x4 FinalToStart=GetInverseTransformationMatrix(StartToFinal);
        }
        public static Matrix4x4 GetInverseTransformationMatrix(Matrix4x4 transformationMatrix)//矩阵求逆
        {
            Matrix4x4 inverseTransformationMatrix;
            Matrix4x4.Invert(transformationMatrix, out inverseTransformationMatrix);

            return inverseTransformationMatrix;
        }

 

二，线转换:直线L与初系X轴夹角转换成与末系X轴夹角

初系中有一条直线L,L平行于初系的x-y平面，且与初系X轴的夹角为theta,请问L与末系X轴的夹角为多少？

1.用GetDirectionVector()获取直线L的方向向量Vector3 Final

2.将方向向量平移至初系的原点，此时方向向量的起点坐标为(0,0,0),终点坐标为Final

3.利用TransformPoint()方法和初系转末系的转换矩阵得到末系中方向向量起点、终点的坐标

4.两个坐标相减得到末系中直线L的方向向量

5.用GetAngleWithXAxis()求新的方向向量与末系X轴的夹角

        public static void Main()
        {
            float theta1 =TransformAngle(theta,StartToFinal);
        }
        public static float TransformAngle(float theta, Matrix4x4 transformMatrix)//
        {
            Vector3 Final = GetDirectionVector(theta);
            Vector3 Final_Chassis = TransformPoint(Final, transformMatrix);
            Vector3 Origin_Chassis = TransformPoint(new Vector3(0, 0, 0), transformMatrix);
            Vector3 Direction = Final_Chassis - Origin_Chassis;
            float theta1 = GetAngleWithXAxis(Direction);
            return theta1;
        }
        public static float GetAngleWithXAxis(Vector3 L)
        {
            // 计算方向向量在x轴上的投影长度
            float projectionOnXAxis = L.X;

            // 计算方向向量的模长
            float magnitudeOfL = (float)Math.Sqrt(L.X * L.X + L.Y * L.Y + L.Z * L.Z);

            // 计算夹角的余弦值
            float cosTheta = projectionOnXAxis / magnitudeOfL;

            // 计算夹角（以弧度为单位），然后转换为角度
            float thetaRadians = (float)Math.Acos(cosTheta);
            float thetaDegrees = thetaRadians * (180.0f / (float)Math.PI);
            if (L.Y < 0) thetaDegrees = -thetaDegrees;
            return thetaDegrees;
        }
        public static Vector3 GetDirectionVector(float theta)
        {
            // 将角度从度转换为弧度
            float thetaRadians = theta * (float)(Math.PI / 180.0);

            // 计算方向向量的各个分量
            float xComponent = (float)Math.Cos(thetaRadians);
            float yComponent = (float)Math.Sin(thetaRadians);
            float zComponent = 0.0f; // 因为直线平行于x-y平面，所以z分量为0
            Console.WriteLine($"Transformed Coordinate: ({xComponent}, {yComponent}, {zComponent})");
            return new Vector3(xComponent, yComponent, zComponent);
        }