public static void FABRIKTest()
        {
            TDPoint target = new TDPoint(70f, 70f, 70f);
            float   r      = (float)(Math.PI / 180);

            ArmPartQ a = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);
            ArmPartQ b = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);
            ArmPartQ c = new ArmPartQ(50, 0, 0, 0, 0, 0 * r, 180 * r);

            ArmPartQ[] asdf = { a, b, c };
            float[]    Zs   = { 0f, 0f, 0f };

            ArmQ arm1 = new ArmQ(Zs, 0 * r, asdf);

            arm1.converge(target);
            arm1.printAngles();
        }
        public void solve(int armNum)
        {
            Quaternion arm;
            Quaternion XAxis;
            Quaternion YAxis;
            Quaternion ZAxis;

            if (armNum == 0)
            {
                arm   = new Quaternion(0, 1, 0, 0);
                XAxis = new Quaternion(0, 1, 0, 0);
                YAxis = new Quaternion(0, 0, 1, 0);
                ZAxis = new Quaternion(0, 0, 0, 1);
            }
            else
            {
                arm   = quatList[armNum].arm.Normalized();
                XAxis = XAxes[armNum];
                YAxis = YAxes[armNum];
                ZAxis = ZAxes[armNum];
            }

            //Quaternion R;
            //Quaternion P;
            //Quaternion Y;

            //Rotating
            for (int b = armNum; b < angleX.Length; b++)
            {
                float theta, phi, alpha;

                //checks if angle is fixed or not. A negative angle means the angle of the joint
                //is fixed at that angle (-30 means fixed at 30)
                {
                    if (angleX[b] < 0)
                    {
                        theta = -angleX[b];
                    }
                    else
                    {
                        theta = angleX[b];
                    }
                    if (angleY[b] < 0)
                    {
                        phi = -angleY[b];
                    }
                    else
                    {
                        phi = angleY[b];
                    }
                    if (angleZ[b] < 0)
                    {
                        alpha = -angleZ[b];
                    }
                    else
                    {
                        alpha = angleZ[b];
                    }
                }

                double     ro = Math.Sin(rad(theta / 2));
                Quaternion R  = new Quaternion((float)Math.Cos(rad(theta / 2)), (float)(XAxis.i * ro), (float)(XAxis.j * ro), (float)(XAxis.k * ro));

                ro = Math.Sin(rad(phi / 2));
                Quaternion Y = new Quaternion((float)Math.Cos(rad(phi / 2)), (float)(YAxis.i * ro), (float)(YAxis.j * ro), (float)(YAxis.k * ro));

                ro = Math.Sin(rad(alpha / 2));
                Quaternion P = new Quaternion((float)Math.Cos(rad(alpha / 2)), (float)(ZAxis.i * ro), (float)(ZAxis.j * ro), (float)(ZAxis.k * ro));

                arm         = (R * (Y * (P * arm * !P) * !Y) * !R);
                quatList[b] = new ArmPartQ(arm.r, arm.i, arm.j, arm.k, Xmin[b], Xmax[b], Ymin[b], Ymax[b], Zmin[b], Zmax[b]);

                XAxes[b] = XAxis;
                YAxes[b] = YAxis;
                ZAxes[b] = ZAxis;

                XAxis = R * (Y * (P * XAxis * !P) * !Y) * !R; //x axis always equals temp so change later maybe
                YAxis = R * (Y * (P * YAxis * !P) * !Y) * !R;
                ZAxis = R * (Y * (P * ZAxis * !P) * !Y) * !R;
            }

            //Elongating
            for (int i = armNum; i < armLength.Length; i++)
            {
                quatList[i].arm *= armLength[i];
            }
        }