TransformTypes.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_
#define GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_
 
#include <string>
#include <optional>
 
#include <gz/math/Vector3.hh>
#include <gz/math/Pose3.hh>
 
namespace gz {
namespace sim {
enum TransformType {
  ADD_VELOCITY_LOCAL,
  ADD_VELOCITY_GLOBAL,
  LOCAL,
  GLOBAL
};
 
std::optional<TransformType> getTransformType(const std::string &_str)
{
  if(_str == "ADD_VELOCITY_LOCAL")
    return TransformType::ADD_VELOCITY_LOCAL;
  if(_str == "ADD_VELOCITY_GLOBAL")
    return TransformType::ADD_VELOCITY_GLOBAL;
  if(_str == "LOCAL")
    return TransformType::LOCAL;
  if(_str == "GLOBAL")
    return TransformType::GLOBAL;
  return std::nullopt;
}
 
math::Vector3d transformFrame(
  const TransformType _type, const math::Pose3d& _pose,
  const math::Vector3d& _velocity, const math::Vector3d& _reading)
{
  math::Vector3d result;
  math::Vector3d offset{0, 0, 0};
 
  if (_type == ADD_VELOCITY_LOCAL || _type == ADD_VELOCITY_GLOBAL)
  {
    offset = -_velocity;
  }
 
  switch (_type) {
      case ADD_VELOCITY_LOCAL:
      case LOCAL:
          result = _pose.Rot().Inverse() * (_reading + offset);
          break;
      default:
          result = (_reading + offset);
  }
 
  return result;
}
}
}
#endif