gistfile1.hpp

// g++ main.cpp -lGL -lGLU -lglut
#include <cmath>
#include <iostream>
#include <GL/glut.h>
#include <boost/shared_ptr.hpp>

//------------------------------------------------------------//
// vector, quaternion
//------------------------------------------------------------//
struct Quaternion;
struct Vector
{
  float x;
  float y;
  float z;

  Vector operator*(float factor)const
  {
    Vector v=*this;
    v.x *= factor;
    v.y *= factor;
    v.z *= factor;
    return v;
  }

  bool operator==(const Vector &rhs)const
  {
    return x==rhs.x && y==rhs.y && z==rhs.z;
  }

  Vector cross(const Vector &rhs)const
  {
    Vector v;
    v.x=y*rhs.z - z*rhs.y;
    v.y=z*rhs.x - x*rhs.z;
    v.z=x*rhs.y - y*rhs.x;
    return v;
  }

  float sqareNorm()const { return x*x+y*y+z*z; }
  double norm()const { return std::sqrt(sqareNorm()); }
  Vector getNormalized() const {
    return (*this) * static_cast<float>(1.0/norm());
  }

  Quaternion toQuaternion();
};
inline std::ostream& operator<<(std::ostream &os, const Vector &rhs)
{
  return os << '(' << rhs.x << ',' << rhs.y << ',' << rhs.z << ')' ;
}

struct Quaternion
{
  // real
  float t;
  // imaginary
  float x;
  float y;
  float z;

  Quaternion operator*(const Quaternion &rhs)const
  {
    Quaternion q;
    q.t = t * rhs.t - x * rhs.x - y * rhs.y - z * rhs.z;
    q.x = t * rhs.x + rhs.t * x + y * rhs.z - rhs.y * z;
    q.y = t * rhs.y + rhs.t * y + z * rhs.x - rhs.z * x;
    q.z = t * rhs.z + rhs.t * z + x * rhs.y - rhs.x * y;
    return q;
  }

  Quaternion getConjugate()const
  {
    Quaternion q=*this;
    q.x = -q.x;
    q.y = -q.y;
    q.z = -q.z;
    return q;
  }
  
  Vector toVector()const
  {
    Vector v={x, y, z};
    return v;
  }
};

Quaternion Vector::toQuaternion()
{
  Quaternion q={0, x, y, z};
  return q;
}

Quaternion createRotationQuaternionHalf(
  const Vector&axis, float angle)
{
    Vector normalize_v = axis.getNormalized();
    double cos_half_theta=cos( (angle * M_PI) / (180 * 2) );
    double sin_half_theta=sin( (angle * M_PI) / (180 * 2) );
    Quaternion calcA;
    calcA.t = cos_half_theta;
    calcA.x = normalize_v.x * sin_half_theta;
    calcA.y = normalize_v.y * sin_half_theta;
    calcA.z = normalize_v.z * sin_half_theta;
    return calcA;
}

//------------------------------------------------------------//
// Bone
//------------------------------------------------------------//
struct Bone
{
  Vector originInParent;
  Vector tail;
  Quaternion rotation;
  float color[3];

  boost::shared_ptr<Bone> next;

  void connect(boost::shared_ptr<Bone> child)
  {
    next=child;
    next->originInParent=tail;
  }
};

boost::shared_ptr<Bone> root;
Quaternion identity={1, 0, 0, 0};
double angle1=0;
double angle2=0;

//------------------------------------------------------------//
// glut callbacks
//------------------------------------------------------------//
void draw_(boost::shared_ptr<Bone> bone, const Quaternion &parentQuaternion)
{
  if(!bone){
    return;;
  }
  // apply Quaternion
  Quaternion quaternion =parentQuaternion * bone->rotation;


  // move to bone head
  Vector head=(parentQuaternion.getConjugate() 
      * bone->originInParent.toQuaternion()
      * parentQuaternion).toVector();
  glPushMatrix();
  glTranslatef(head.x, head.y, head.z);

  // bone tail
  Vector v=
    (quaternion.getConjugate() * bone->tail.toQuaternion() * quaternion).
    toVector();

  // draw a bone
  glColor3fv(bone->color);
  glBegin(GL_LINES);
  glVertex3f(0, 0, 0);
  glVertex3fv(&v.x);
  glEnd();

  glPointSize(5);
  glBegin(GL_POINTS);
  glVertex3f(0, 0, 0);
  glVertex3fv(&v.x);
  glEnd();

  draw_(bone->next, quaternion);

  glPopMatrix();
}

void idle()
{
  Vector axis={0, 0, 1};

  angle1+=0.1;
  root->next->rotation=createRotationQuaternionHalf(axis, angle1);

  angle2-=0.5;
  root->next->next->rotation=createRotationQuaternionHalf(axis, angle2);

  glutPostRedisplay();
}

void draw()
{
  glClear(GL_COLOR_BUFFER_BIT);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  // view
  glTranslatef(0, 0, -100);

  draw_(root, identity); 

  glutSwapBuffers();
}

void resize(int w, int h)
{
  glViewport(0, 0, w, h);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(30.0, (double)w / (double)h, 1.0, 100.0);

  glMatrixMode(GL_MODELVIEW);
}

//------------------------------------------------------------//
// main
//------------------------------------------------------------//
void setup()
{
  // root
  {
    boost::shared_ptr<Bone> bone(new Bone);
    bone->originInParent.x=-10;
    bone->originInParent.y=-3;
    bone->originInParent.z=0;
    bone->tail.x=8;
    bone->tail.y=0;
    bone->tail.z=0;
    bone->rotation=identity;
    bone->color[0]=1;
    bone->color[1]=0;
    bone->color[2]=0;
    root=bone;
  }

  // 2nd
  {
    boost::shared_ptr<Bone> bone(new Bone);
    bone->tail.x=15;
    bone->tail.y=0;
    bone->tail.z=0;
    bone->rotation=identity;
    bone->color[0]=0;
    bone->color[1]=1;
    bone->color[2]=0;
    root->connect(bone);
  }

  // 3rd
  {
    boost::shared_ptr<Bone> bone(new Bone);
    bone->tail.x=4;
    bone->tail.y=0;
    bone->tail.z=0;
    bone->rotation=identity;
    bone->color[0]=0;
    bone->color[1]=0;
    bone->color[2]=1;
    root->next->connect(bone);
  }
}

int main(int argc, char **argv)
{
  // initialize glut
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
  glutInitWindowSize(150,150);
  glutCreateWindow("glut");

  setup();

  // callback
  glutDisplayFunc(draw);
  glutReshapeFunc(resize);
  glutIdleFunc(idle);

  // main loop
  glutMainLoop();
  return 0;
}