Quaternion Rotation

edit

I want to rotate point by quaternion. It it is to use library for rotation Quaternion or Matrix. But now I want to do it by myself. I wrote some code. but I'm not sure it is right. Before that code, I used Quaternion multiplication that has been implemented in tf::Quaternion(or Bullet).

now here is question.

I want to rotate [orientation quaternion] by multiplying [rotation quaternion]. Rotating Order is [ZYX].

//tf::Quaternion(x, y, z, w);
tf::Quaternion Qorien = tf::Quaternion(0.0, 0.0, 0.0, 1.0);
double angleX, angleY, angleZ;
angleX = 1.0;
angleY = 4.0;
angleZ = 2.0;

double cosX,sinX,cosY,sinY,cosZ,sinZ;
cosX = cos(angleX/2); sinX = sin(angleX/2);
cosY = cos(angleY/2); sinY = sin(angleY/2);
cosZ = cos(angleZ/2); sinZ = sin(angleZ/2);
tf::Quaternion QrotX,QrotY,QrotZ;

QrotX = tf::Quaternion( 1.0*sinX,
                        0.0*sinX,
                        0.0*sinX,
                        cosX);
QrotY = tf::Quaternion( 0.0*sinY,
                        1.0*sinY,
                        0.0*sinY,
                        cosY);
QrotZ = tf::Quaternion( 0.0*sinZ,
                        0.0*sinZ,
                        1.0*sinZ,
                        cosZ);
tf::Quaternion Qtotal = QrotX*QrotY*QrotZ;
Qorien = Qtotal * Qorien;

/*
now [Qorien] is the Orientation Quaternion that represents new orientation
after rotating by an order Z->Y->X.
/*