ChVector<double> Quat_to_Angle(AngleSet angset, const ChQuaternion<double>& mquat) {
ChVector<double> res;
ChMatrix33<> Acoord;
Acoord.Set_A_quaternion(mquat);
switch (angset) {
case AngleSet::EULERO:
res = Acoord.Get_A_Eulero();
break;
case AngleSet::CARDANO:
res = Acoord.Get_A_Cardano();
break;
case AngleSet::HPB:
res = Acoord.Get_A_Hpb();
break;
case AngleSet::RXYZ:
res = Acoord.Get_A_Rxyz();
break;
case AngleSet::RODRIGUEZ:
res = Acoord.Get_A_Rodriguez();
break;
default:
break;
}
return res;
}
ChVector<double> Angle_to_Angle(AngleSet setfrom, AngleSet setto, const ChVector<double>& mangles) {
ChVector<double> res;
ChMatrix33<> Acoord;
switch (setfrom) {
case AngleSet::EULERO:
Acoord.Set_A_Eulero(mangles);
break;
case AngleSet::CARDANO:
Acoord.Set_A_Cardano(mangles);
break;
case AngleSet::HPB:
Acoord.Set_A_Hpb(mangles);
break;
case AngleSet::RXYZ:
Acoord.Set_A_Rxyz(mangles);
break;
case AngleSet::RODRIGUEZ:
Acoord.Set_A_Rodriguez(mangles);
break;
default:
break;
}
switch (setto) {
case AngleSet::EULERO:
res = Acoord.Get_A_Eulero();
break;
case AngleSet::CARDANO:
res = Acoord.Get_A_Cardano();
break;
case AngleSet::HPB:
res = Acoord.Get_A_Hpb();
break;
case AngleSet::RXYZ:
res = Acoord.Get_A_Rxyz();
break;
case AngleSet::RODRIGUEZ:
res = Acoord.Get_A_Rodriguez();
break;
default:
break;
}
return res;
}