예제 #1
0
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;
}
예제 #2
0
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;
}