01-Siyuan-MesProject/DataMirrorTools_Simulator/Foundation/Kinematics/Kinematics.OPERATION.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Windows.Forms;

namespace KinematicsBase
{
   
    public class OPERATION_FramesList
    {
        public C_OPERATION FramesListCreate(string startPoseId, string endPoseId, int Interval)
        {
            Pose startPose;
            Pose endPose;
            startPose = Kinematics.GetPoseByPoseId(startPoseId);
            endPose = Kinematics.GetPoseByPoseId(endPoseId);

            if(startPose != null && endPose != null)
            {
                return FramesListCreate(startPose, endPose, Interval);
            }
            else
            {
                MessageBox.Show("姿态不存在", "提示");
                throw new Exception("Pose不存在");
            }
            
        }
        public C_OPERATION FramesListCreate(Pose startPose, Pose endPose, int Interval)
        {
            // int framStep = 10;
            int framStep = PP.PCP.FrameValue;
            List<Pose> PoseList;
            PoseList = OPERATION_GetDifferencePoseList(startPose, endPose, Interval, framStep);
            return FramesCreate(PoseList);

        }

        public C_OPERATION FramesCreate(List<Pose> PoseList)
        {
            List<C_ObjStates> objStates;
            double timeIndex;
            int framStep = 10;
            C_Frames frames;
            C_OPERATION m_OPERATION_Frames = new C_OPERATION();
            for (int i = 0; i < PoseList.Count; i++)
            {
                Pose pose = PoseList[i];

                timeIndex = (double)(framStep * i) / 1000;
                objStates = GetObjStates(pose);
                frames = new C_Frames();
                frames.time = timeIndex.ToString("0.00");
                frames.objStates = objStates;

                m_OPERATION_Frames.frames.Add(frames);
            }
            return m_OPERATION_Frames;
        }
        #region Method

        /// <summary>
        /// 
        /// </summary>
        /// <param name="startPose"></param>
        /// <param name="endPose"></param>
        /// <param name="Interval">单位毫秒</param>
        public List<Pose> OPERATION_GetDifferencePoseList(Pose startPose, Pose endPose, int Interval, int framStep)
        {
            List<Pose> PoseList = new List<Pose>();
            Pose pose;
            Dictionary<string, double> jointStep = new Dictionary<string, double>();
            int frameCount;

            frameCount = Interval / framStep;
            //计算Joint的步长
            foreach (var item in endPose.PoseInfo)
            {
                double value = (endPose.PoseInfo[item.Key] - startPose.PoseInfo[item.Key]) / frameCount;
                jointStep.Add(item.Key, value);
            }
            for (int i = 0; i < frameCount; i++)
            {
                if (i == 0)
                {
                    pose = new Pose();
                    pose.ModelId = endPose.ModelId;
                    pose.PoseInfo = new Dictionary<string, double>();
                    foreach (var item in endPose.PoseInfo)
                    {
                        pose.PoseInfo.Add(item.Key, startPose.PoseInfo[item.Key]);
                    }
                    PoseList.Add(pose);
                }
                else if (i == frameCount - 1)
                {
                    pose = new Pose();
                    pose.ModelId = endPose.ModelId;
                    pose.PoseInfo = new Dictionary<string, double>();
                    foreach (var item in endPose.PoseInfo)
                    {
                        pose.PoseInfo.Add(item.Key, endPose.PoseInfo[item.Key]);
                    }
                    PoseList.Add(pose);
                }
                else
                {
                    pose = new Pose();
                    pose.ModelId = endPose.ModelId;
                    pose.PoseInfo = new Dictionary<string, double>();
                    foreach (var item in endPose.PoseInfo)
                    {
                        pose.PoseInfo.Add(item.Key, startPose.PoseInfo[item.Key] + i * jointStep[item.Key]);
                    }
                    PoseList.Add(pose);
                }
            }

            return PoseList;
        }

        #endregion

        public List<C_ObjStates> GetObjStates(Pose pose)
        {
            List<Joint> JointList = Kinematics.GetJointListByModelId(pose.ModelId);
            List<C_ObjStates> objStates = new List<C_ObjStates>();
            foreach (var item in pose.PoseInfo)
            {
                var joint = JointList.FirstOrDefault(t => t.Id == item.Key);

                var objStateSubList = GetObjStates(pose.ModelId, joint.Id, item.Value);

                foreach (var objState in objStateSubList)
                {
                    objStates.Add(objState);
                }
            }
            return objStates;
        }

        public List<C_ObjStates> GetObjStates(string modelId, string jId, double jValue)
        {
            List<Joint> JointList = Kinematics.GetJointListByModelId(modelId);
            List<C_ObjStates> objStates = new List<C_ObjStates>();
            //foreach (var item in pose.PoseInfo)
            var joint = JointList.FirstOrDefault(t => t.Id == jId);
            
            //1.反向，0.正常
            if (joint.Axis.Flip == 1)
            {
                jValue = -jValue;
            }

            //值按比例和初始值变换
            jValue = jValue * joint.ValueFactor + joint.Value;

            //var value = ChangeToModelValue(JointList, jId, jValue);
            PositionQuaternion positionQuaternion = new Joint().TransformCoordinates(joint, jValue);
            C_ObjStates objState = new C_ObjStates();
            objState.i = joint.CId;
            objState.tx = positionQuaternion.Position.X.ToString("0.000000");
            objState.ty = positionQuaternion.Position.Y.ToString("0.000000");
            objState.tz = positionQuaternion.Position.Z.ToString("0.000000");
            objState.qx = positionQuaternion.Quaternion.X.ToString("0.000000");
            objState.qy = positionQuaternion.Quaternion.Y.ToString("0.000000");
            objState.qz = positionQuaternion.Quaternion.Z.ToString("0.000000");
            objState.qw = positionQuaternion.Quaternion.W.ToString("0.000000");
            objStates.Add(objState);
            return objStates;
        }
    }

 

}