Elements.cpp

Go to the documentation of this file.

#include "Elements.h"
 
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/normal/normal_to_mat4.h>
#include <SimoxUtility/math/pose/transform.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
 
namespace armarx::viz
{
 
    struct Convert
    {
        static Eigen::Quaternionf
        directionToQuaternion(Eigen::Vector3f dir)
        {
            dir = dir.normalized();
            Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
            Eigen::Vector3f cross = naturalDir.cross(dir);
            float dot = naturalDir.dot(dir);
            float angle = std::acos(dot);
            if (cross.squaredNorm() < 1.0e-12)
            {
                // Directions are almost colinear ==> Do no rotation
                cross = Eigen::Vector3f::UnitX();
                if (dot < 0)
                {
                    angle = M_PI;
                }
                else
                {
                    angle = 0.0f;
                }
            }
            Eigen::Vector3f axis = cross.normalized();
            Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
 
            return ori;
        }
    };
 
    const std::string Object::DefaultObjectsPackage =
        armarx::ObjectFinder::DefaultObjectsPackageName;
    const std::string Object::DefaultRelativeObjectsDirectory =
        armarx::ObjectFinder::DefaultObjectsDirectory;
 
    Object&
    Object::fileByObjectFinder(const std::string& objectID,
                               const std::string& objectsPackage,
                               const std::string& relativeObjectsDirectory)
    {
        return this->fileByObjectFinder(
            armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
    }
 
    Object&
    Object::fileByObjectFinder(const armarx::ObjectID& objectID,
                               const std::string& objectsPackage,
                               const std::string& relativeObjectsDirectory)
    {
        ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
        armarx::PackageFileLocation file = info.simoxXML();
        return this->file(file.package, file.relativePath);
    }
 
    Object&
    Object::alpha(float alpha)
    {
        if (alpha < 1)
        {
            overrideColor(simox::Color::white().with_alpha(alpha));
        }
        return *this;
    }
 
    Box&
    Box::set(const simox::OrientedBoxBase<float>& b)
    {
        size(b.dimensions());
        return pose(b.transformation_centered());
    }
 
    Box&
    Box::set(const simox::OrientedBoxBase<double>& b)
    {
        size(b.dimensions<float>());
        return pose(b.transformation_centered<float>());
    }
 
    Cylinder&
    Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
    {
        position((to + from) / 2);
        orientation(Convert::directionToQuaternion((to - from).normalized()));
        height((to - from).norm());
 
        return *this;
    }
 
    Cylinder&
    Cylinder::direction(Eigen::Vector3f direction)
    {
        orientation(Convert::directionToQuaternion(direction));
 
        return *this;
    }
 
    Arrow&
    Arrow::direction(Eigen::Vector3f dir)
    {
        return orientation(Convert::directionToQuaternion(dir));
    }
 
    ArrowCircle&
    ArrowCircle::normal(Eigen::Vector3f dir)
    {
        Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
        Eigen::Vector3f cross = naturalDir.cross(dir);
        float angle = std::acos(naturalDir.dot(dir));
        if (cross.squaredNorm() < 1.0e-12f)
        {
            // Directions are almost colinear ==> Angle is either 0 or 180 deg
            cross = Eigen::Vector3f::UnitX();
            // Keep angle
        }
        Eigen::Vector3f axis = cross.normalized();
        Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
 
        return orientation(ori);
    }
 
    Polygon&
    Polygon::points(const std::vector<Eigen::Vector3f>& ps)
    {
        auto& points = data_->points;
        points.clear();
        points.reserve(ps.size());
        for (auto& p : ps)
        {
            points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
        }
 
        return *this;
    }
 
    Polygon&
    Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
    {
        const Eigen::Quaternionf ori =
            Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), plane.normal());
        return this->plane(plane.projection(at), ori, size);
    }
 
    Polygon&
    Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
    {
        const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
        const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
 
        addPoint(center + x + y);
        addPoint(center - x + y);
        addPoint(center - x - y);
        addPoint(center + x - y);
 
        return *this;
    }
 
    Polygon&
    Polygon::circle(Eigen::Vector3f center,
                    Eigen::Vector3f normal,
                    float radius,
                    std::size_t tessellation)
    {
        const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
        ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
 
        const float angle = 2 * M_PI / tessellation;
        const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
 
        Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
        addPoint(simox::math::transform_position(pose, lastLocalPoint));
        while (--tessellation)
        {
            const Eigen::Vector3f localPoint = rot * lastLocalPoint;
            addPoint(simox::math::transform_position(pose, localPoint));
            lastLocalPoint = localPoint;
        }
        return *this;
    }
 
 
} // namespace armarx::viz