SimoxObjectShape.cpp

Go to the documentation of this file.

#include "SimoxObjectShape.h"
 
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/math/Helpers.h>
 
namespace armarx::semantic
{
 
    SimoxObjectShape::SimoxObjectShape() = default;
 
    SimoxObjectShape::SimoxObjectShape(const VirtualRobot::ManipulationObjectPtr& object,
                                       const std::string& objectClassName) :
        object(object), objectClassName(objectClassName)
    {
    }
 
    Eigen::Vector3f
    SimoxObjectShape::getPosition() const
    {
        return object->getGlobalPosition();
    }
 
    void
    SimoxObjectShape::setPosition(const Eigen::Vector3f& position)
    {
        Eigen::Matrix4f pose = object->getGlobalPose();
        math::Helpers::Position(pose) = position;
        object->setGlobalPose(pose);
    }
 
    Eigen::Quaternionf
    SimoxObjectShape::getOrientation() const
    {
        return Eigen::Quaternionf(object->getGlobalOrientation());
    }
 
    void
    SimoxObjectShape::setOrientation(const Eigen::Quaternionf& orientation)
    {
        Eigen::Matrix4f pose = object->getGlobalPose();
        math::Helpers::Orientation(pose) = Eigen::Matrix3f(orientation);
        object->setGlobalPose(pose);
    }
 
    semrel::TriMesh
    SimoxObjectShape::getTriMeshLocal() const
    {
        VirtualRobot::TriMeshModelPtr triMesh = object->getCollisionModel()->getTriMeshModel();
 
        std::vector<VirtualRobot::MathTools::TriangleFace> const& faces = triMesh->faces;
        std::vector<Eigen::Vector3f> const& vertices = triMesh->vertices;
        //std::vector<Eigen::Vector3f> const& normals = triMesh->normals;
 
        semrel::TriMesh result;
 
        for (VirtualRobot::MathTools::TriangleFace const& face : faces)
        {
            Eigen::Vector3f v0 = vertices.at(face.id1);
            Eigen::Vector3f v1 = vertices.at(face.id2);
            Eigen::Vector3f v2 = vertices.at(face.id3);
 
            Eigen::Vector3f normal = face.normal;
 
            semrel::TriMesh::Triangle triangle;
            triangle.v0 = result.numVertices();
            result.addVertex(v0);
            triangle.v1 = result.numVertices();
            result.addVertex(v1);
            triangle.v2 = result.numVertices();
            result.addVertex(v2);
 
            int n = result.numNormals();
            triangle.n0 = n;
            triangle.n1 = n;
            triangle.n2 = n;
            result.addNormal(normal);
 
            result.addTriangle(triangle);
        }
 
        return result;
    }
 
    semrel::AxisAlignedBoundingBox
    SimoxObjectShape::getAABBLocal() const
    {
        const VirtualRobot::BoundingBox& bb =
            object->getCollisionModel()->getTriMeshModel()->boundingBox;
        return {bb.getMin(), bb.getMax()};
    }
 
    semrel::AxisAlignedBoundingBox
    SimoxObjectShape::getAABB() const
    {
        const std::vector<Eigen::Vector3f> vertices =
            object->getCollisionModel()->getModelVeticesGlobal();
 
        simox::AxisAlignedBoundingBox aabb;
        for (Eigen::Vector3f v : vertices)
        {
            aabb.expand_to(v);
        }
 
        return {aabb.limits()};
    }
 
    std::shared_ptr<btCollisionShape>
    SimoxObjectShape::getBulletCollisionShape(float margin) const
    {
        (void)margin;
        return nullptr;
    }
 
    void
    SimoxObjectShape::addMargin(float margin)
    {
        // TODO: Implement
        (void)margin;
    }
 
    std::string
    SimoxObjectShape::tagPrefix() const
    {
        return object->getName();
    }
 
 
} // namespace armarx::semantic