No replies to this topic

[ohadmits]

I'm tring to make a Rubik's cube but I have gimbal lock and I don't know how to fix it.
Code:

        private void rotateRC()

        {

            for (int i = 0; i < turns.GetLength(1); i++)

            {

                if (turns[0, i] != 0)

                {

                    TVertex newPos;

                    for (int j = 0; j < 3; j++)

                        for (int k = 0; k < 3; k++)

                        {

                            int index = (i + k * 3) * 3 + j;

                            newPos = Cubes[index].GetPos();

                            //Cubes[index].SetOrientationX(Cubes[index].GetOrientationX() + turns[0, i]);

                            //Cubes[index].SetPosAndAngle(newPos.X, newPos.Y, newPos.Z, (float)Math.Sin(Math3D.Deg2Rad(Cubes[index].GetOrientationX()) / 2), (float)Math.Cos(Math3D.Deg2Rad(Cubes[index].GetOrientationX()) / 2), 0);

                            Cubes[index].SetOrientationY(Cubes[index].GetOrientationY() + turns[0, i]);

                            Cubes[index].SetPosAndAngle(newPos.X, newPos.Y, newPos.Z, Math3D.Deg2Rad(Cubes[index].GetOrientationX()), Math3D.Deg2Rad(Cubes[index].GetOrientationY()), 0);

                        }

                    if (++count == 90 / ANGLE_TO_TURN)

                    {

                        turns[0, i] = 0;

                        count = 0;

                    }

                }

                else if (turns[1, i] != 0)

                {

                    TVertex newPos;

                    for (int j = 0; j < 3; j++)

                        for (int k = 0; k < 3; k++)

                        {

                            int index = (j + k * 3) * 3 + i;

                            newPos = Cubes[index].GetPos();

                            Cubes[index].SetOrientationX(Cubes[index].GetOrientationX() + turns[1, i]);

                            Cubes[index].SetPosAndAngle(newPos.X, newPos.Y, newPos.Z, Math3D.Deg2Rad(Cubes[index].GetOrientationX()), Math3D.Deg2Rad(Cubes[index].GetOrientationY()), Math3D.Deg2Rad(Cubes[index].GetOrientationY()));

                        }

                    if (++count == 90 / ANGLE_TO_TURN)

                    {

                        turns[1, i] = 0;

                        count = 0;

                    }

                }

            }

        }