C++ (Cpp) Pose::Yaw Examples

Example #1

File: path.cpp Project: shening/Robotics_Club

////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Returns a point [distance] meters from the end of the path
///         (follows last points heading).
///
///   \param[in] pose The position to be relative to.
///   \param[in] distance Distance from last point to the next point.
///   \param[in] angleOffset Optional parameter to offset current angle in radians.
///
///   \return Pose where the projected line would end.
///
////////////////////////////////////////////////////////////////////////////////////
Pose Path::CalculateNextPoint(const Pose pose, const double distance, const double angleOffset)
{
    Point3D point;
    CxUtils::Quaternion quat;
    Pose projection = pose;
    projection.mRotation.mZ += angleOffset;
    if(projection.mpPoseRotation != NULL)
    {
        projection.mpPoseRotation->mZ += angleOffset;
        quat = *projection.mpPoseRotation;
    }
    else
    {
        quat.SetRotationZ(-projection.Yaw());
    }
    point.mY = distance;
    point = quat.Rotate(point);
    projection.mPosition += point;
    return projection;
}