SimoxObjectShape.cpp
Go to the documentation of this file.
1 #include "SimoxObjectShape.h"
2 
3 #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
4 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
5 #include <VirtualRobot/ManipulationObject.h>
6 #include <VirtualRobot/Visualization/TriMeshModel.h>
7 #include <VirtualRobot/math/Helpers.h>
8 
9 namespace armarx::semantic
10 {
11 
13 
14  SimoxObjectShape::SimoxObjectShape(const VirtualRobot::ManipulationObjectPtr& object,
15  const std::string& objectClassName) :
16  object(object), objectClassName(objectClassName)
17  {
18  }
19 
20  Eigen::Vector3f
22  {
23  return object->getGlobalPosition();
24  }
25 
26  void
27  SimoxObjectShape::setPosition(const Eigen::Vector3f& position)
28  {
29  Eigen::Matrix4f pose = object->getGlobalPose();
30  math::Helpers::Position(pose) = position;
31  object->setGlobalPose(pose);
32  }
33 
36  {
37  return Eigen::Quaternionf(object->getGlobalOrientation());
38  }
39 
40  void
42  {
43  Eigen::Matrix4f pose = object->getGlobalPose();
44  math::Helpers::Orientation(pose) = Eigen::Matrix3f(orientation);
45  object->setGlobalPose(pose);
46  }
47 
48  semrel::TriMesh
50  {
51  VirtualRobot::TriMeshModelPtr triMesh = object->getCollisionModel()->getTriMeshModel();
52 
53  std::vector<VirtualRobot::MathTools::TriangleFace> const& faces = triMesh->faces;
54  std::vector<Eigen::Vector3f> const& vertices = triMesh->vertices;
55  //std::vector<Eigen::Vector3f> const& normals = triMesh->normals;
56 
57  semrel::TriMesh result;
58 
59  for (VirtualRobot::MathTools::TriangleFace const& face : faces)
60  {
61  Eigen::Vector3f v0 = vertices.at(face.id1);
62  Eigen::Vector3f v1 = vertices.at(face.id2);
63  Eigen::Vector3f v2 = vertices.at(face.id3);
64 
65  Eigen::Vector3f normal = face.normal;
66 
67  semrel::TriMesh::Triangle triangle;
68  triangle.v0 = result.numVertices();
69  result.addVertex(v0);
70  triangle.v1 = result.numVertices();
71  result.addVertex(v1);
72  triangle.v2 = result.numVertices();
73  result.addVertex(v2);
74 
75  int n = result.numNormals();
76  triangle.n0 = n;
77  triangle.n1 = n;
78  triangle.n2 = n;
79  result.addNormal(normal);
80 
81  result.addTriangle(triangle);
82  }
83 
84  return result;
85  }
86 
89  {
90  const VirtualRobot::BoundingBox& bb =
91  object->getCollisionModel()->getTriMeshModel()->boundingBox;
92  return {bb.getMin(), bb.getMax()};
93  }
94 
97  {
98  const std::vector<Eigen::Vector3f> vertices =
99  object->getCollisionModel()->getModelVeticesGlobal();
100 
102  for (Eigen::Vector3f v : vertices)
103  {
104  aabb.expand_to(v);
105  }
106 
107  return {aabb.limits()};
108  }
109 
110  std::shared_ptr<btCollisionShape>
112  {
113  (void)margin;
114  return nullptr;
115  }
116 
117  void
119  {
120  // TODO: Implement
121  (void)margin;
122  }
123 
124  std::string
126  {
127  return object->getName();
128  }
129 
130 
131 } // namespace armarx::semantic
armarx::semantic::SimoxObjectShape::setPosition
void setPosition(Eigen::Vector3f const &position) override
Definition: SimoxObjectShape.cpp:27
armarx::semantic::SimoxObjectShape::addMargin
void addMargin(float margin) override
Definition: SimoxObjectShape.cpp:118
armarx::semantic::SimoxObjectShape::setOrientation
void setOrientation(Eigen::Quaternionf const &orientation) override
Definition: SimoxObjectShape.cpp:41
GfxTL::Orientation
ScalarT Orientation(const VectorXD< 2, ScalarT > &p1, const VectorXD< 2, ScalarT > &p2, const VectorXD< 2, ScalarT > &c)
Definition: Orientation.h:10
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::semantic::SimoxObjectShape::getOrientation
Eigen::Quaternionf getOrientation() const override
Definition: SimoxObjectShape.cpp:35
armarx::semantic::SimoxObjectShape::getAABB
semrel::AxisAlignedBoundingBox getAABB() const override
Definition: SimoxObjectShape.cpp:96
armarx::semantic::SimoxObjectShape::getAABBLocal
semrel::AxisAlignedBoundingBox getAABBLocal() const override
Definition: SimoxObjectShape.cpp:88
armarx::aron::simox::arondto::AxisAlignedBoundingBox
::simox::arondto::AxisAlignedBoundingBox AxisAlignedBoundingBox
Definition: simox.h:14
armarx::semantic::SimoxObjectShape::getTriMeshLocal
semrel::TriMesh getTriMeshLocal() const override
Definition: SimoxObjectShape.cpp:49
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
armarx::semantic::SimoxObjectShape::getPosition
Eigen::Vector3f getPosition() const override
Definition: SimoxObjectShape.cpp:21
armarx::semantic::SimoxObjectShape::tagPrefix
std::string tagPrefix() const override
Definition: SimoxObjectShape.cpp:125
armarx::semantic::SimoxObjectShape::getBulletCollisionShape
std::shared_ptr< btCollisionShape > getBulletCollisionShape(float margin) const override
Definition: SimoxObjectShape.cpp:111
armarx::semantic
Definition: ShapesSupportRelations.cpp:32
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::semantic::SimoxObjectShape::SimoxObjectShape
SimoxObjectShape()
armarx::Quaternion< float, 0 >
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
SimoxObjectShape.h
armarx::semantic::SimoxObjectShape::object
VirtualRobot::ManipulationObjectPtr object
Definition: SimoxObjectShape.h:37