gbadev.org forum archive

This is a read-only mirror of the content originally found on forum.gbadev.org (now offline), salvaged from Wayback machine copies. A new forum can be found here.

DS development > How avoid nasty gimbal lock for correct rotations?

#157852 - muKO - Fri May 30, 2008 6:43 pm

Hi all,
First things first, my name is muKO and I'm a new DS coder.
I'm trying to implement a very basic 3D engine for my first DS game.
It's a basic 3D puzzle game, based on cubes rotations (on all axis)...

Now to my BIG question: "How avoid nasty gimbal lock for correct rotations?"
Even thought I've ported a Quaternion class to my engine I can't avoid that nasty Gimbal Lock error. And I can't understand where I'm wrong... or better how correctly use Quaternions...

I've cleaned my code a bit and here it is, ok it's a lot of code but I think the error is in the SceneGraph drawRecursive method... Where is the error?

Code:

//------------------------------------------------------------------------------

/* includes */
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <sstream>
#include <string>

#include <nds.h>

//------------------------------------------------------------------------------

/* defines */
const float PI = 3.14159265358979323846f; // pi greco
const float   EPSILON   = 0.005f; // error tolerance for check
#define String std::string
#define   DEGTORAD(x) ( ((x) * PI) / 180.0 ) // conversion from degree to radius
#define   RADTODEG(x) ( ((x) * 180.0) / PI ) // conversion from radius to degree
#define FLOAT_EQ(x, v) ( ((v) - EPSILON) < (x) && (x) < ((v) + EPSILON) ) // float equality test with tollerance
#define ZERO_CLAMP(x) ( (EPSILON > fabs (x)) ? 0.0f : (x) ) // set float to 0 if within tolerance
#define   SQR(x) ( (x) * (x) ) // square operation

//------------------------------------------------------------------------------
class Vector3D
{
public:
   Vector3D ( float xp = 0.0, float yp = 0.0, float zp = 0.0 ) : x (xp), y (yp), z (zp) { }

   Vector3D & operator = ( const Vector3D & vp )
   {
      x = vp.x, y = vp.y, z = vp.z;
      return *this;
   }

   Vector3D operator + ( const Vector3D & vp )
   {
      return Vector3D (x+vp.x, y+vp.y, z+vp.z);
   }

   Vector3D operator - ( const Vector3D & vp )
   {
      return Vector3D (x-vp.x, y-vp.y, z-vp.z);
   }

   float getLength ( void )
   {
      return (sqrt (SQR (x) + SQR (y) + SQR (z)));
   }

   float normalize ( void )
   {
      float length = getLength ();

      if (FLOAT_EQ (0.0f, length)) // if length is zero
      {
         x =   0.0f;
         y =   0.0f;
         z =   0.0f;
      }
      else // normalize
      {
         x = x / length;
         y = y / length;
         z = z / length;

         zeroClamp ();
      }

      return length;
   }

   void zeroClamp ( void )
   {
      x = ZERO_CLAMP (x);
      y = ZERO_CLAMP (y);
      z = ZERO_CLAMP (z);
   }

   bool isNormalized ( void )
   {
      return (FLOAT_EQ (1.0f, getLength ()) == true);
   }

   String toString ( void )
   {
      std::ostringstream oss;
      oss << "x = " << x << " u, y = " << y << " u, z = " << z << " u";
      return oss.str ();
   }

public:
   float x; // in units
   float y; // in units
   float z; // in units
};
   
//------------------------------------------------------------------------------
class Quaternion
{
public:
   Quaternion ( float wp = 1.0, float xp = 0.0, float yp = 0.0, float zp = 0.0 ) : w (wp), x (xp), y (yp), z (zp) { }

   Quaternion & operator = ( const Quaternion & qp )
   {
      w = qp.w, x = qp.x, y = qp.y, z = qp.z;
      return *this;
   }

   Quaternion operator * ( const Quaternion & q )
   {
      Quaternion res;

      res.x = w * q.x + x * q.w + y * q.z - z * q.y;
      res.y = w * q.y + y * q.w + z * q.x - x * q.z;
      res.z = w * q.z + z * q.w + x * q.y - y * q.x;
      res.w = w * q.w - x * q.x - y * q.y - z * q.z;

      // make sure the resulting quaternion is a unit quat.
      res.normalize ();

      return res;
   }

   float getLength ( void )
   {
      return (sqrt (SQR (x) + SQR (y) + SQR (z) + SQR (w)));
   }

   void normalize ( void )
   {
      float dist, square;

      square = SQR (x) + SQR (y) + SQR (z) + SQR (w);
      
      if (square > 0.0)
         dist = (float)(1.0 / sqrt (square));
      else dist = 1;

      x *= dist;
      y *= dist;
      z *= dist;
      w *= dist;
   }

   m4x4 toMatrix ( void )
   {
      m4x4 matrix;
      float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

      x2 = x + x; y2 = y + y; z2 = z + z;
      xx = x * x2;   xy = x * y2;   xz = x * z2;
      yy = y * y2;   yz = y * z2;   zz = z * z2;
      wx = w * x2;   wy = w * y2;   wz = w * z2;

      matrix.m[0] = floattof32(1.0 - (yy + zz));
      matrix.m[1] = floattof32(xy - wz);
      matrix.m[2] = floattof32(xz + wy);
      matrix.m[3] = floattof32(0.0);

      matrix.m[4] = floattof32(xy + wz);
      matrix.m[5] = floattof32(1.0 - (xx + zz));
      matrix.m[6] = floattof32(yz - wx);
      matrix.m[7] = floattof32(0.0);

      matrix.m[8] = floattof32(xz - wy);
      matrix.m[9] = floattof32(yz + wx);
      matrix.m[10] = floattof32(1.0 - (xx + yy));
      matrix.m[11] = floattof32(0.0);

      matrix.m[12] = floattof32(0);
      matrix.m[13] = floattof32(0);
      matrix.m[14] = floattof32(0);
      matrix.m[15] = floattof32(1);

      return matrix;
   }

   void eulerToQuat ( float xp, float yp, float zp )
   {
      float ex, ey, ez; // temp half euler angles
      float cr, cp, cy, sr, sp, sy, cpcy, spsy; // temp vars in roll, pitch and yaw

      ex = DEGTORAD (xp) / 2.0; // convert to rads and half them
      ey = DEGTORAD (yp) / 2.0;
      ez = DEGTORAD (zp) / 2.0;

      cr = cos(ex);
      cp = cos(ey);
      cy = cos(ez);

      sr = sin(ex);
      sp = sin(ey);
      sy = sin(ez);
      
      cpcy = cp * cy;
      spsy = sp * sy;

      this->w = cr * cpcy + sr * spsy;
      this->x = sr * cpcy - cr * spsy;
      this->y = cr * sp * cy + sr * cp * sy;
      this->z = cr * cp * sy - sr * sp * cy;
   }

   String toString ( void )
   {
      std::ostringstream oss;
      oss << "w = " << w << " u,\n x = " << x << " u,\n y = " << y << " u,\n z = " << z << " u\n";
      return oss.str ();
   }

public:
   float w;
   float x;
   float y;
   float z;
};
   
//------------------------------------------------------------------------------
class SceneNode
{
public:
   SceneNode ( String idp, Vector3D posp = Vector3D (0, 0, 0), Vector3D rotp = Vector3D (0, 0, 0) )
      : id (idp), pos (posp), rot (rotp), father (NULL), brother (NULL), child (NULL) {
   }
   virtual ~SceneNode ( void ) { }

   virtual bool update ( int timeCount ) { return true; }
   virtual void draw ( void ) { }

   SceneNode * getFather ( void ) { return father; }
   SceneNode * getBrother ( void ) { return brother; }
   SceneNode * getChild ( void ) { return child; }
   bool hasFather ( void ) { return father != NULL; }
   bool hasBrother ( void ) { return brother != NULL; }
   bool hasChild ( void ) { return child != NULL; }

   bool addChild ( SceneNode * child )
   {
      if (child == NULL)
         return false;
      if (child->hasFather () == true)
         return false;
      if (child->hasBrother () == true)
         return false;

      if (this->hasChild () == false)
      {
         this->setChild (child);
         child->setFather (this);
      }
      else
      {
         SceneNode* tmp = this->getChild ();
         while (tmp->hasBrother () == true)
            tmp = tmp->getBrother ();
         tmp->setBrother (child);
         child->setFather (this);
      }
      return true;
   }

   bool deleteChildren ()
   {
      bool result = deleteChildrenRecursive (this->getChild ());
      this->setChild (NULL);
      return result;
   }
private:
   bool deleteChildrenRecursive (SceneNode* node)
   {
      if (node==NULL)
         return true;

      if (node->hasChild () == true)
         if (deleteChildrenRecursive (node->getChild ()) == false)
            return true;

      if(node->hasBrother () == true)
         if (deleteChildrenRecursive (node->getBrother ()) == false)
            return true;

      delete node;
      return true;
   }
   
public:
   const String id;
   Vector3D pos; // contains OBJECT translations
   Vector3D rot; // contains OBJECT rotations in eurelian angles

private:
   void setFather (SceneNode* fatherp) { father = fatherp; }
   void setBrother (SceneNode* brotherp) { brother = brotherp;}
   void setChild (SceneNode* childp) { child = childp; }

   SceneNode* father;
   SceneNode* brother;
   SceneNode* child;
};
   
//------------------------------------------------------------------------------
class SceneGraph
{
public:
   SceneGraph ( void )
      : root (NULL)
   {
      root = new SceneNode ("root");
   }

   ~SceneGraph ( void )
   {
      if (root != NULL)
      {
         clear ();
         delete root;
         root = NULL;
      }
   }

   bool clear ( void )
   {
      return root->deleteChildren ();
   }

   bool update ( int timeCount )
   {
      return updateRecursive (root, timeCount);
   }
private:
   bool updateRecursive ( SceneNode* node, int timeCount )
   {
      if (node == NULL)
         return false;

      if (node->update (timeCount) == false)
         return false;

      if (node->hasChild () == true)
         if (updateRecursive (node->getChild (), timeCount) == false)
            return false;

      if (node->hasBrother () == true)
         if (updateRecursive (node->getBrother (), timeCount) == false)
            return false;

      return true;
   }

public:
   void draw ( void )
   {
      drawRecursive (root);
      return;
   }
private:
   void drawRecursive (SceneNode* node)
   {
      if (node == NULL)
         return;

      glPushMatrix ();

      // translations
      glTranslatef (node->pos.x, node->pos.y, node->pos.z);

      // rotations
      m4x4 mRotation;
      Quaternion qRotation;

      qRotation.eulerToQuat (node->rot.x, node->rot.y, node->rot.z);
      mRotation = qRotation.toMatrix ();
      glMultMatrix4x4 (&mRotation);

      // drawing
      node->draw();

      if(node->hasChild()==true)
         drawRecursive(node->getChild());

      glPopMatrix(1);

      if(node->hasBrother()==true)
         drawRecursive(node->getBrother());
   }

public:
   SceneNode* root;
};
   
//------------------------------------------------------------------------------
class Cube : public SceneNode
{
public:
   Cube(String idp, Vector3D posp = Vector3D(0, 0, 0), Vector3D rotp = Vector3D(0, 0, 0), float sidep = DEFAULT_SIDE)
      : SceneNode(idp, posp, rotp), side(sidep) { }
   virtual ~Cube(void) {}

   void draw(void)
   {
      float midSide = side / 2.0;

      // set the first outline color to white border color
      glSetOutlineColor (0, RGB15 (50, 50, 50));
      // set a poly ID for outlining
      glPolyFmt (POLY_ALPHA (31) | POLY_CULL_BACK | POLY_ID (1));
      // enable edge outlining
      glEnable (GL_OUTLINE);

      glBegin (GL_QUADS);
      {
         // back face (blue)
         glColor3f (0.0, 0.0, 1.0);
         glVertex3f (-midSide, +midSide, -midSide);
         glVertex3f (+midSide, +midSide, -midSide);
         glVertex3f (+midSide, -midSide, -midSide);
         glVertex3f (-midSide, -midSide, -midSide);

         // front face (green)
         glColor3f (0.0, 1.0, 0.0);
         glVertex3f (+midSide, -midSide, +midSide);
         glVertex3f (+midSide, +midSide, +midSide);
         glVertex3f (-midSide, +midSide, +midSide);
         glVertex3f (-midSide, -midSide, +midSide);

         // left face (red)
         glColor3f (1.0, 0.0, 0.0);
         glVertex3f (-midSide, -midSide, +midSide);
         glVertex3f (-midSide, +midSide, +midSide);
         glVertex3f (-midSide, +midSide, -midSide);
         glVertex3f (-midSide, -midSide, -midSide);

         // right face (yellow)
         glColor3f (1.0, 1.0, 0.0);
         glVertex3f (+midSide, +midSide, -midSide);
         glVertex3f (+midSide, +midSide, +midSide);
         glVertex3f (+midSide, -midSide, +midSide);
         glVertex3f (+midSide, -midSide, -midSide);

         // bottom face (violet)
         glColor3f (1.0, 0.0, 1.0);
         glVertex3f (+midSide, -midSide, -midSide);
         glVertex3f (+midSide, -midSide, +midSide);
         glVertex3f (-midSide, -midSide, +midSide);
         glVertex3f (-midSide, -midSide, -midSide);

         // top face (light blue)
         glColor3f (0.0, 1.0, 1.0);
         glVertex3f (-midSide, +midSide, +midSide);
         glVertex3f (+midSide, +midSide, +midSide);
         glVertex3f (+midSide, +midSide, -midSide);
         glVertex3f (-midSide, +midSide, -midSide);
      }
      glEnd ();
   }

public:
   static const int DEFAULT_SIDE = 1;
   float side;
};
   
//---------------------------------------------------------------------------------
void init(void)
{
   // enable the 3D core
   powerON(POWER_3D_CORE | POWER_MATRIX);

   // put 3D (Main Screen) on bottom
   lcdMainOnBottom();

   // setup the sub screen for basic printing
   consoleDemoInit();

   // setup the Main screen for 3D
   videoSetMode(MODE_6_3D /*| DISPLAY_BG3_ACTIVE*/);

   // set the first bank as background memory and the third as sub background memory
   // B and D are not used
   vramSetMainBanks(VRAM_A_MAIN_BG_0x06000000, VRAM_B_LCD, VRAM_C_SUB_BG , VRAM_D_LCD);

   // by default font will be rendered with color 255
   BG_PALETTE_SUB[255] = RGB15(31,31,31);

   // IRQ basic setup
   irqInit();
   irqEnable(IRQ_VBLANK);

   // initialize gl
   glInit();

   // enable antialiasing
   glEnable(GL_ANTIALIAS);

   // setup the (trasparent) rear plane
   glClearColor(0, 0, 0, 0); // BG is totally trasparent to allow 2D / 3D merging (should be opaque for AA to work)
   glClearPolyID(63); // BG must have a unique polygon ID for AA to work
   glClearDepth(0x7FFF);

   // set our view port to be the same size as the screen
   glViewport(0, 0, 255, 191);
}

//---------------------------------------------------------------------------------
int main(void)
{
   init ();

   // objects init
   SceneGraph sceneGraph;
   Cube * cube1;
   Cube * cube2;
   Cube * cube3;

   cube1 = new Cube("cube1", Vector3D(-2, 0, 0));
   sceneGraph.root->addChild(cube1);

   cube2 = new Cube("cube2", Vector3D( 2, 0, 0));
   sceneGraph.root->addChild(cube2);

   cube3 = new Cube("cube3", Vector3D( 0, -2, 0));
   cube1->addChild(cube3);

   //change ortho vs perspective
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   gluPerspective(70, 256.0 / 192.0, 0.1, 10);

   // main loop
   while (1) {

      // handle input
      scanKeys();
      int held = keysHeld();
      if( held & KEY_LEFT) cube1->rot.y++;
      if( held & KEY_RIGHT) cube1->rot.y--;
      if( held & KEY_UP) cube1->rot.x++;
      if( held & KEY_DOWN) cube1->rot.x--;

      // Set the current matrix to be the model matrix
      glMatrixMode(GL_MODELVIEW);
      glLoadIdentity();

      // handle camera
      glTranslatef(0, 0, -6);

      sceneGraph.draw();

      // console messages
      consoleClear ();
      printf("use directional pad to rotate...\n");
      printf("rotate down 90 degrees and then rotate left and...\n");
      printf("NASTY GIMBAL LOCK!\n");

      while (GFX_STATUS & (1<<27)); // wait until the geometry engine is not busy

      // flush to the screen
      glFlush(0);
   }

   return 0;
}


P.S. I know it's not very optimized for a DS but for now I'm focusing on make it work! Any suggestion is welcome... 2 weeks are passed since I'm reading stuff about OpenGL rotations and gimbal lock... and gimbal lock is winning me... :(

#157857 - sajiimori - Fri May 30, 2008 11:23 pm

Sticking with matrices will simplify things. Quaternions can be seen as an optimization of matrices, and they're only faster in some cases (especially if you have to concatenate a lot of rotations in series).

Angles should only be used in very high-level code, e.g. a Character class that has a 'yaw' angle. Angles should be "compiled down" to matrices before you get anywhere near rendering.

At any rate, converting Euler angles to matrices (or quaternions) doesn't make gimbal lock magically go away. =) If the lock happened before switching to matrices, you can't really fix it after the fact.

For cubes that rotate based on physics, don't use angles at all: use matrices top to bottom! You might even be able to use the same matrix for rendering and physics.

#157860 - silent_code - Fri May 30, 2008 11:56 pm

this might help: http://www.sjbrown.co.uk/?article=quaternions

good luck and happy coding! :^)
_________________
July 5th 08: "Volumetric Shadow Demo" 1.6.0 (final) source released
June 5th 08: "Zombie NDS" WIP released!
It's all on my page, just click WWW below.

#157868 - zeruda - Sat May 31, 2008 6:32 am

Code:

      qRotation.eulerToQuat (node->rot.x, node->rot.y, node->rot.z);
      mRotation = qRotation.toMatrix ();
      glMultMatrix4x4 (&mRotation);


Yeah, here's your problem. You are doing your rotations as a Euler rotation. You are converting the euler rotation to quaternion, and from quaternion to matrix. Neither of the last two had anything to do with rotation. Only the first one did, and that of course is subject to gimbal lock.

You could do something like as follows(with a few alterations).

Code:
CMatrix4x4 InitialMatrix;

void AppendRotationToInitalMatrix(float CurrentRotation, CVector Axis)
{
    float CurrentRotation;
    CMatrix4x4 AppendMatrix;
    CQuaternion CurrentQuaternion;

    CurrentQuaternion.CreateFromAxisAngle(-CurrentRotation, Axis);
    CurrentQuaternion.CreateMatrix(AppendMatrix.matrix);
    InitialMatrix = AppendMatrix * InitialMatrix;
}

CVector SetAxisToRotate(int i)
{
    CVector CurrentAxis;
    if (i == 1) {
        CurrentAxis = CVector(InitialMatrix.matrix[0], InitialMatrix.matrix[1], InitialMatrix.matrix[2]);
    }
    if (i == 2) {
        CurrentAxis = CVector(InitialMatrix.matrix[4], InitialMatrix.matrix[5], InitialMatrix.matrix[6]);
    }
    if (i == 3) {
        CurrentAxis = CVector(InitialMatrix.matrix[8], InitialMatrix.matrix[9], InitialMatrix.matrix[10]);
    }
    return CurrentAxis;
}

void main ()
{
    while (1) {
        int whichaxis = DoIWantXYorZAxis();
        CVector Axis = SetAxisToRotate(whichaxis)
        if( held & KEY_LEFT) cube1->rot.y++;
        AppendRotationToInitalMatrix(cube1->rot.y, Axis)
        glMultMatrixf(InitialMatrix.matrix);
        ...
        ...
        ...
        Profit();
    }
}