﻿using UnityEngine;
using UnityEditor;

namespace FOHEART_Mocap
{
    /*
        判断一个四元数角度所处的象限。
        判断方法，将一个长度为1的线段，左端固定在原点，
        然后设置线段的角度为此四元数，通过判断右端的落点坐标，得到象限
         */
    public class QuadJudge 
    {
        /*象限的枚举定义，总共8个象限*/
        public enum Quadrant_TypeDef
        {
            QUAD0_NONE,
            QUAD1,/*x>0 y>0 z>0*/
            QUAD2,/*x<0 y>0 z>0*/
            QUAD3,/*x<0 y<0 z>0*/
            QUAD4,/*x<0 y>0 z>0*/
            QUAD5,/*x>0 y>0 z<0*/
            QUAD6,/*x<0 y>0 z<0*/
            QUAD7,/*x<0 y<0 z<0*/
            QUAD8,/*x>0 y<0 z<0*/
            QUAD_CNT
        };

        /*范围在坐标轴正半轴*/
        private bool inPositive(float x)
        {
            if ((x >= 0) && (x <= 1))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /*范围在坐标轴负半轴*/
        private bool inNegative(float x)
        {
            if ((x >= -1) && (x <= 0))
            {
                return true;
            }
            else
            {
                return false;
            }
        }
      

        public QuadJudge()
        {

        }

        /*
         传入一个四元数，从而判断它的象限
         输入：
         q_w:四元数的w
          q_x:四元数的x
           q_y:四元数的y
            q_z:四元数的z

            输出：角度所在的象限

             */
        public Quadrant_TypeDef getQuadFromQuat(float q_w, float q_x, float q_y, float q_z)
        {
            Quadrant_TypeDef ret = Quadrant_TypeDef.QUAD_CNT;

            Vector3 endPoint= PointRotWithQuat(q_w, q_x, q_y, q_z);
            float x = endPoint[0];
            float y = endPoint[1];
            float z = endPoint[2];

            if (inPositive(x) && inPositive(y) && inPositive(z))
            {
                ret = Quadrant_TypeDef.QUAD1;

            }
            else if (inNegative(x) && inPositive(y) && inPositive(z))
            {
                ret = Quadrant_TypeDef.QUAD2;

            }
            else if (inNegative(x) && inNegative(y) && inPositive(z))
            {
                ret = Quadrant_TypeDef.QUAD3;


            }
            else if (inPositive(x) && inNegative(y) && inPositive(z))
            {
                ret = Quadrant_TypeDef.QUAD4;

            }
            else if (inPositive(x) && inPositive(y) && inNegative(z))
            {
                ret = Quadrant_TypeDef.QUAD5;

            }
            else if (inNegative(x) && inPositive(y) && inNegative(z))
            {
                ret = Quadrant_TypeDef.QUAD6;

            }
            else if (inNegative(x) && inNegative(y) && inNegative(z))
            {
                ret = Quadrant_TypeDef.QUAD7;

            }
            else if (inPositive(x) && inNegative(y) && inNegative(z))
            {
                ret = Quadrant_TypeDef.QUAD8;


            }
            else
            {
                //QuadrantMgr.undef++;
            }

            return ret;

        }

        private Vector3 PointRotWithQuat(float q_w, float q_x, float q_y, float q_z)
        {
            float[] pos = new float[3];
            pos[0] = 0;
            pos[1] = 0;
            pos[2] = 0;

            float[] intialPos = new float[3];
            intialPos[0] = 1.0f;
            intialPos[1] = 0;
            intialPos[2] = 0;

            float[] posResult = new float[3] { 0, 0, 0 };

            float[] q = new float[4] { q_w, q_x, q_y, q_z };

            /*
            根据父节点的位置和四元数求解子节点的位置
            */

            //四元数转旋转矩阵
            float[,] r=new float[3,3];
            r[0,0] = 1 - 2 * q[2] * q[2] - 2 * q[3] * q[3];
            r[1,0] = 2 * q[1] * q[2] + 2 * q[0] * q[3];
            r[2,0] = 2 * q[1] * q[3] - 2 * q[0] * q[2];
            r[0,1] = 2 * q[1] * q[2] - 2 * q[0] * q[3];
            r[1, 1] = 1 - 2 * q[1] * q[1] - 2 * q[3] * q[3];
            r[2, 1] = 2 * q[2] * q[3] + 2 * q[0] * q[1];
            r[0, 2] = 2 * q[1] * q[3] + 2 * q[0] * q[2];
            r[1, 2] = 2 * q[2] * q[3] - 2 * q[0] * q[1];
            r[2, 2] = 1 - 2 * q[1] * q[1] - 2 * q[2] * q[2];

            //子节点的初始位置
            for (int i = 0; i < 3; i++)
            {
                posResult[i] = 0;
                for (int j = 0; j < 3; j++)
                {
                    posResult[i] += r[i,j] * intialPos[j];
                }
                posResult[i] += pos[i];
            }

            return new Vector3(posResult[0], posResult[1], posResult[2]);
        }

    }

}
