void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
{
double NewR[9];
quatToDCM(Q, NewR);
q[0]=Q[0];
q[1]=Q[1];
q[2]=Q[2];
q[3]=Q[3];
//for (int i=0; i<16; i++)
// Pose[i]=NewPose[i];
Pose[0]=NewR[0];
Pose[1]=NewR[1];
Pose[2]=NewR[2];
Pose[3]=NewT[0];
Pose[4]=NewR[3];
Pose[5]=NewR[4];
Pose[6]=NewR[5];
Pose[7]=NewT[1];
Pose[8]=NewR[6];
Pose[9]=NewR[7];
Pose[10]=NewR[8];
Pose[11]=NewT[2];
Pose[12]=0;
Pose[13]=0;
Pose[14]=0;
Pose[15]=1;
// compute the angle
const double trace = NewR[0] + NewR[4] + NewR[8];
if (fabs(trace - 3) <= EPS)
{
angle = 0;
}
else
if (fabs(trace + 1) <= EPS)
{
angle = M_PI;
}
else
{
angle = ( acos((trace - 1)/2) );
}
}
void getRandomRotation(double R[9])
{
double q[4];
getRandQuat(q);
quatToDCM(q, R);
}
