Quaternion.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// Header
#include "Quaternion.h"
 
#include <SimoxUtility/meta/type_name.h>
 
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
 
namespace armarx::aron::codegenerator::cpp::generator
{
    const std::map<type::quaternion::ElementType, std::tuple<std::string, int, std::string>>
        ElementType2Cpp = {{type::quaternion::ElementType::FLOAT32,
                            {TypeName<float>::Get(),
                             sizeof(float),
                             "::armarx::aron::type::quaternion::ElementType::FLOAT32"}},
                           {type::quaternion::ElementType::FLOAT64,
                            {TypeName<double>::Get(),
                             sizeof(double),
                             "::armarx::aron::type::quaternion::ElementType::FLOAT64"}}};
 
    // constructors
    Quaternion::Quaternion(const type::Quaternion& n) :
        detail::NDArrayGenerator<type::Quaternion, Quaternion>(
            "Eigen::Quaternion<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ">",
            "Eigen::Quaternion<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ">",
            simox::meta::get_type_name<data::dto::NDArray>(),
            simox::meta::get_type_name<type::dto::Quaternion>(),
            n)
    {
    }
 
    std::vector<std::string>
    Quaternion::getRequiredIncludes() const
    {
        return {"<RobotAPI/libraries/aron/common/rw/eigen.h>"};
    }
 
    std::pair<std::vector<std::pair<std::string, std::string>>, bool>
    Quaternion::getCtorInitializers(const std::string& name) const
    {
 
 
        if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT ||
            type.getDefaultValue() == aron::type::quaternion::default_value::IDENTITY)
        {
            return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
        }
        else if (type.getMaybe() == type::Maybe::OPTIONAL)
        {
            return {{{name, "std::nullopt"}}, true};
        }
 
        else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
        {
            std::cerr << "WARNING: using ZEROS as a default value for Quaternions is highly "
                         "frowned upon and will be removed in the future";
            return {{{name, getInstantiatedCppTypename() + "(0, 0, 0, 0)"}}, true};
        }
        else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
        {
            std::cerr << "WARNING: using ONES as a default value for Quaternions is highly frowned "
                         "upon and will be removed in the future";
            return {{{name, getInstantiatedCppTypename() + "(1, 1, 1, 1)"}}, true};
        }
        else
        {
            // try to parse num. We ensure from typereader that defaultValue is valid number
            return {{{name, getInstantiatedCppTypename() + "(" + type.getDefaultValue() + ")"}},
                    true};
        }
    }
 
    CppBlockPtr
    Quaternion::getResetHardBlock(const std::string& cppAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
 
        if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "::Identity();");
        }
        else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "(0, 0, 0, 0);");
        }
        else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
        {
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
                                   "(1, 1, 1, 1);");
        }
        else if (not type.getDefaultValue().empty())
        {
            // try to parse num. We ensure from typereader that defaultValue is valid number
            block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(" +
                                   type.getDefaultValue() + ");");
        }
        return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine(cppAccessor + nextEl() + "setIdentity();");
        return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Quaternion::getWriteTypeBlock(const std::string& typeAccessor,
                                  const std::string& cppAccessor,
                                  const Path& p,
                                  std::string& variantAccessor) const
    {
        CppBlockPtr b = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
 
        b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeQuaternion(" +
                   std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " + "\"" +
                   type.getDefaultValue() + "\", " +
                   conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
                   ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
                   "})); // of " + cppAccessor);
        return b;
    }
 
    CppBlockPtr
    Quaternion::getWriteBlock(const std::string& cppAccessor,
                              const Path& p,
                              std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
 
        block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " +
                               resolved_accessor + ", " + variantAccessor + ", " +
                               "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
                               simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
 
        return resolveMaybeWriteBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    Quaternion::getReadBlock(const std::string& cppAccessor,
                             const std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
 
        if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
        {
            block_if_data->addLine(reset);
        }
 
        block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " +
                               variantAccessor + ", " + resolved_accessor + "); // of " +
                               cppAccessor);
 
        return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
    }
 
    CppBlockPtr
    Quaternion::getEqualsBlock(const std::string& accessor,
                               const std::string& otherInstanceAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" +
                               (type.getMaybe() != type::Maybe::NONE ? "*" + otherInstanceAccessor
                                                                     : otherInstanceAccessor) +
                               ")))");
        block_if_data->addLineAsBlock("return false;");
        return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
    }
} // namespace armarx::aron::codegenerator::cpp::generator