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); }