int
bot_quat_pos_to_matrix(const double quat[4], const double pos[3], double mat[16])
{
double rot[9];
bot_quat_to_matrix(quat, rot);
mat[0] = rot[0];
mat[1] = rot[1];
mat[2] = rot[2];
mat[3] = pos[0];
mat[4] = rot[3];
mat[5] = rot[4];
mat[6] = rot[5];
mat[7] = pos[1];
mat[8] = rot[6];
mat[9] = rot[7];
mat[10] = rot[8];
mat[11] = pos[2];
mat[12] = 0;
mat[13] = 0;
mat[14] = 0;
mat[15] = 1;
return 0;
}
void
bot_trans_get_mat_3x4(const BotTrans *btrans, double mat[12])
{
double tmp[9];
bot_quat_to_matrix(btrans->rot_quat, tmp);
// row 1
memcpy(mat+0, tmp+0, 3*sizeof(double));
mat[3] = btrans->trans_vec[0];
// row 2
memcpy(mat+4, tmp+3, 3*sizeof(double));
mat[7] = btrans->trans_vec[1];
// row 3
memcpy(mat+8, tmp+6, 3*sizeof(double));
mat[11] = btrans->trans_vec[2];
}
void
bot_trans_get_rot_mat_3x3(const BotTrans * btrans, double rot_mat[9])
{
bot_quat_to_matrix(btrans->rot_quat, rot_mat);
// memcpy(rot_mat, btrans->rot_mat, 9*sizeof(double));
}
