diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.cpp b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
index 7d4edde7e557156e71e05c9d54a3d021b0ba3764..4bf7e59c677a2c46400e7c02c3c5a84b55f8f618 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
@@ -23,249 +23,179 @@
*/
#include "SingleChainManipulability.h"
+#include <Visualization/VisualizationFactory.h>
+#include <Visualization/VisualizationNode.h>
#include <Eigen/Dense>
-
-#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
-
#include "../IK/DifferentialIK.h"
-#include "VirtualRobot/Robot.h"
#include "VirtualRobot/RobotNodeSet.h"
-#include <Visualization/VisualizationFactory.h>
-#include <Visualization/VisualizationNode.h>
+#include "VirtualRobot/Robot.h"
+#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
namespace VirtualRobot
{
- Eigen::MatrixXd
- AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd& jacobian,
- AbstractManipulability::Type type)
- {
- if (weightMatrix.rows() == 0)
- {
- int nbJoints = jacobian.cols();
- weightMatrix.setIdentity(nbJoints, nbJoints);
- }
- Eigen::MatrixXd velocityManipulability =
- jacobian * weightMatrix * weightMatrix.transpose() * jacobian.transpose();
-
- switch (type)
- {
- case Velocity:
- return velocityManipulability;
- case Force:
- return velocityManipulability.inverse();
- default:
- throw std::runtime_error("Unkown manipulability type");
- }
+Eigen::MatrixXd AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd &jacobian, AbstractManipulability::Type type) {
+ if (weightMatrix.rows() == 0){
+ int nbJoints = jacobian.cols();
+ weightMatrix.setIdentity(nbJoints, nbJoints);
}
+ Eigen::MatrixXd velocityManipulability = jacobian * weightMatrix * weightMatrix.transpose() * jacobian.transpose();
- SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(
- const RobotNodeSetPtr& rns,
- Mode mode,
- Type type,
- Eigen::MatrixXd weightMatrixInit,
- bool convertMMtoM) :
- SingleRobotNodeSetManipulability(rns,
- rns->getTCP(),
- mode,
- type,
- RobotNodePtr(),
- weightMatrixInit,
- convertMMtoM)
- {
+ switch (type) {
+ case Velocity:
+ return velocityManipulability;
+ case Force:
+ return velocityManipulability.inverse();
+ default:
+ throw std::runtime_error("Unkown manipulability type");
}
+}
- SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(
- const RobotNodeSetPtr& rns,
- const RobotNodePtr& node,
- Mode mode,
- Type type,
- const RobotNodePtr& coordSystem,
- Eigen::MatrixXd weightMatrixInit,
- bool convertMMtoM) :
- AbstractSingleChainManipulability(mode, type, weightMatrixInit),
- rns(rns),
- node(node),
- coordSystem(coordSystem)
- {
- ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDampedDynamic));
- ik->convertModelScalingtoM(convertMMtoM);
- }
- RobotNodeSetPtr
- SingleRobotNodeSetManipulability::getRobotNodeSet()
- {
- return rns;
- }
- RobotPtr
- SingleRobotNodeSetManipulability::getRobot()
- {
- return rns->getRobot();
- }
- void
- SingleRobotNodeSetManipulability::setConvertMMtoM(bool value)
- {
- ik->convertModelScalingtoM(value);
- }
- Eigen::Vector3f
- SingleRobotNodeSetManipulability::getLocalPosition()
- {
- return rns->getTCP()->getPositionInRootFrame();
- }
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, Mode mode, Type type, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
+ : SingleRobotNodeSetManipulability(rns, rns->getTCP(), mode, type, RobotNodePtr(), weightMatrixInit, convertMMtoM)
+{
+}
- Eigen::Vector3f
- SingleRobotNodeSetManipulability::getGlobalPosition()
- {
- return rns->getTCP()->getGlobalPosition();
- }
+SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
+ : AbstractSingleChainManipulability(mode, type, weightMatrixInit), rns(rns), node(node), coordSystem(coordSystem)
+{
+ ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDampedDynamic));
+ ik->convertModelScalingtoM(convertMMtoM);
+}
- Eigen::Matrix4f
- SingleRobotNodeSetManipulability::getCoordinateSystem()
- {
- return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
- }
+RobotNodeSetPtr SingleRobotNodeSetManipulability::getRobotNodeSet() {
+ return rns;
+}
- std::vector<std::string>
- SingleRobotNodeSetManipulability::getJointNames()
- {
- return rns->getNodeNames();
- }
+RobotPtr SingleRobotNodeSetManipulability::getRobot() {
+ return rns->getRobot();
+}
- Eigen::VectorXd
- SingleRobotNodeSetManipulability::getJointAngles()
- {
- return rns->getJointValuesEigen().cast<double>();
- }
+void SingleRobotNodeSetManipulability::setConvertMMtoM(bool value) {
+ ik->convertModelScalingtoM(value);
+}
+
+Eigen::Vector3f SingleRobotNodeSetManipulability::getLocalPosition() {
+ return rns->getTCP()->getPositionInRootFrame();
+}
- Eigen::VectorXd
- SingleRobotNodeSetManipulability::getJointLimitsHigh()
+Eigen::Vector3f SingleRobotNodeSetManipulability::getGlobalPosition() {
+ return rns->getTCP()->getGlobalPosition();
+}
+
+Eigen::Matrix4f SingleRobotNodeSetManipulability::getCoordinateSystem() {
+ return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
+}
+
+std::vector<std::string> SingleRobotNodeSetManipulability::getJointNames() {
+ return rns->getNodeNames();
+}
+
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointAngles() {
+ return rns->getJointValuesEigen().cast<double>();
+}
+
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsHigh() {
+ Eigen::VectorXd jointLimitHi(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
{
- Eigen::VectorXd jointLimitHi(rns->getSize());
- for (size_t i = 0; i < rns->getSize(); i++)
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
{
- RobotNodePtr rn = rns->getNode(i);
- if (rn->isLimitless())
- {
- jointLimitHi(i) = 0;
- }
- else
- jointLimitHi(i) = rn->getJointLimitHi();
+ jointLimitHi(i) = 0;
}
- return jointLimitHi;
- }
-
- Eigen::VectorXd
- SingleRobotNodeSetManipulability::getJointLimitsLow()
- {
- Eigen::VectorXd jointLimitLo(rns->getSize());
- for (size_t i = 0; i < rns->getSize(); i++)
+ else
+ jointLimitHi(i) = rn->getJointLimitHi();
+ }
+ return jointLimitHi;
+}
+
+Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsLow() {
+ Eigen::VectorXd jointLimitLo(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
{
- RobotNodePtr rn = rns->getNode(i);
- if (rn->isLimitless())
- {
- jointLimitLo(i) = 0;
- }
- else
- jointLimitLo(i) = rn->getJointLimitLo();
+ jointLimitLo(i) = 0;
}
- return jointLimitLo;
+ else
+ jointLimitLo(i) = rn->getJointLimitLo();
}
+ return jointLimitLo;
+}
- Eigen::MatrixXd
- SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode)
- {
- return ik->getJacobianMatrix(node, mode).cast<double>();
- }
+Eigen::MatrixXd SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
+ return ik->getJacobianMatrix(node, mode).cast<double>();
+}
- SingleChainManipulability::SingleChainManipulability(
- const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian,
- AbstractManipulability::Mode mode,
- AbstractManipulability::Type type,
- const Eigen::Matrix<double, Eigen::Dynamic, 1>& jointAngles,
- const Eigen::VectorXd& jointLimitsHigh,
- const Eigen::VectorXd& jointLimitsLow,
- const std::vector<std::string>& jointNames,
- const Eigen::Vector3f& globalPosition,
- const Eigen::Vector3f& localPosition) :
- AbstractSingleChainManipulability(mode, type),
- jacobian(jacobian),
- jointNames(jointNames),
- globalPosition(globalPosition),
- localPosition(localPosition),
- jointAngles(jointAngles),
- jointLimitsHigh(jointLimitsHigh),
- jointLimitsLow(jointLimitsLow)
- {
- }
- Eigen::Vector3f
- SingleChainManipulability::getLocalPosition()
- {
- return localPosition;
- }
- Eigen::Vector3f
- SingleChainManipulability::getGlobalPosition()
- {
- return globalPosition;
- }
- Eigen::Matrix4f
- SingleChainManipulability::getCoordinateSystem()
- {
- return Eigen::Matrix4f::Identity();
- }
+SingleChainManipulability::SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian,
+ AbstractManipulability::Mode mode, AbstractManipulability::Type type,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &jointAngles, const Eigen::VectorXd &jointLimitsHigh, const Eigen::VectorXd &jointLimitsLow,
+ const std::vector<std::string> &jointNames,
+ const Eigen::Vector3f &globalPosition, const Eigen::Vector3f &localPosition) :
+ AbstractSingleChainManipulability(mode, type),
+ jacobian(jacobian),
+ jointNames(jointNames),
+ globalPosition(globalPosition),
+ localPosition(localPosition),
+ jointAngles(jointAngles),
+ jointLimitsHigh(jointLimitsHigh),
+ jointLimitsLow(jointLimitsLow)
+{
+}
- std::vector<std::string>
- SingleChainManipulability::getJointNames()
- {
- return jointNames;
- }
+Eigen::Vector3f SingleChainManipulability::getLocalPosition() {
+ return localPosition;
+}
- Eigen::VectorXd
- SingleChainManipulability::getJointAngles()
- {
- return jointAngles;
- }
+Eigen::Vector3f SingleChainManipulability::getGlobalPosition() {
+ return globalPosition;
+}
- Eigen::VectorXd
- SingleChainManipulability::getJointLimitsHigh()
- {
- return jointLimitsHigh;
- }
+Eigen::Matrix4f SingleChainManipulability::getCoordinateSystem() {
+ return Eigen::Matrix4f::Identity();
+}
- Eigen::VectorXd
- SingleChainManipulability::getJointLimitsLow()
- {
- return jointLimitsLow;
- }
+std::vector<std::string> SingleChainManipulability::getJointNames() {
+ return jointNames;
+}
- void
- SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
- {
- this->jacobian = jacobian;
- }
+Eigen::VectorXd SingleChainManipulability::getJointAngles() {
+ return jointAngles;
+}
- void
- SingleChainManipulability::setLocalPosition(const Eigen::Vector3f& localPosition)
- {
- this->localPosition = localPosition;
- }
+Eigen::VectorXd SingleChainManipulability::getJointLimitsHigh() {
+ return jointLimitsHigh;
+}
+
+Eigen::VectorXd SingleChainManipulability::getJointLimitsLow() {
+ return jointLimitsLow;
+}
- void
- SingleChainManipulability::setGlobalPosition(const Eigen::Vector3f& globalPosition)
- {
- this->globalPosition = globalPosition;
- }
+void SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian) {
+ this->jacobian = jacobian;
+}
+
+void SingleChainManipulability::setLocalPosition(const Eigen::Vector3f &localPosition) {
+ this->localPosition = localPosition;
+}
+
+void SingleChainManipulability::setGlobalPosition(const Eigen::Vector3f &globalPosition) {
+ this->globalPosition = globalPosition;
+}
+
+Eigen::MatrixXd SingleChainManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
+ return GetJacobianSubMatrix(jacobian, mode);
+}
- Eigen::MatrixXd
- SingleChainManipulability::computeJacobian(IKSolver::CartesianSelection mode)
- {
- return GetJacobianSubMatrix(jacobian, mode);
- }
-} // namespace VirtualRobot
+}
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index b0ff271bfe8b0d2ee517da6fb8b86691c3b2cffa..acc643f34da79d7bb49d2b465560bf4a7cbf6660 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -1,11 +1,11 @@
#include "Obstacle.h"
-
-#include "CollisionDetection/CollisionChecker.h"
#include "CollisionDetection/CollisionModel.h"
+#include "CollisionDetection/CollisionChecker.h"
#include "Nodes/RobotNode.h"
#include "Visualization/VisualizationFactory.h"
#include "Visualization/VisualizationNode.h"
+#include <vector>
namespace VirtualRobot
{
@@ -15,12 +15,8 @@ namespace VirtualRobot
// obstacle models start with 20000
int Obstacle::idCounter = 20000;
- Obstacle::Obstacle(const std::string& name,
- VisualizationNodePtr visualization,
- CollisionModelPtr collisionModel,
- const SceneObject::Physics& p,
- CollisionCheckerPtr colChecker) :
- GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
+ Obstacle::Obstacle(const std::string& name, VisualizationNodePtr visualization, CollisionModelPtr collisionModel, const SceneObject::Physics& p, CollisionCheckerPtr colChecker)
+ : GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
{
if (name == "")
{
@@ -45,35 +41,25 @@ namespace VirtualRobot
}
}
- Obstacle::Obstacle(const std::string& name,
- const TriMeshModelPtr& trimesh,
- const std::string& filename) :
- Obstacle(TagTrimeshCtor{}, name, std::make_shared<VisualizationNode>(trimesh))
+ Obstacle::Obstacle(const std::string& name, const TriMeshModelPtr& trimesh, const std::string& filename)
+ : Obstacle(TagTrimeshCtor{}, name, std::make_shared<VisualizationNode>(trimesh))
{
getVisualization()->setFilename(filename, false);
getCollisionModel()->getVisualization()->setFilename(filename, false);
}
- Obstacle::Obstacle(TagTrimeshCtor, const std::string& name, const VisualizationNodePtr& vis) :
- Obstacle(name, vis, std::make_shared<CollisionModel>(vis))
- {
- }
+ Obstacle::Obstacle(TagTrimeshCtor, const std::string& name, const VisualizationNodePtr& vis)
+ : Obstacle(name, vis, std::make_shared<CollisionModel>(vis))
+ {}
Obstacle::~Obstacle() = default;
- int
- Obstacle::getID()
+ int Obstacle::getID()
{
return id;
}
- VirtualRobot::ObstaclePtr
- Obstacle::createBox(float width,
- float height,
- float depth,
- VisualizationFactory::Color color,
- std::string visualizationType,
- CollisionCheckerPtr colChecker)
+ VirtualRobot::ObstaclePtr Obstacle::createBox(float width, float height, float depth, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
@@ -104,8 +90,7 @@ namespace VirtualRobot
if (!visu)
{
- VR_ERROR << "Could not create box visualization with visu type " << visualizationType
- << std::endl;
+ VR_ERROR << "Could not create box visualization with visu type " << visualizationType << std::endl;
return result;
}
@@ -126,11 +111,8 @@ namespace VirtualRobot
return result;
}
- VirtualRobot::ObstaclePtr
- Obstacle::createSphere(float radius,
- VisualizationFactory::Color color,
- std::string visualizationType,
- CollisionCheckerPtr colChecker)
+
+ VirtualRobot::ObstaclePtr Obstacle::createSphere(float radius, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
@@ -159,8 +141,7 @@ namespace VirtualRobot
*/
if (!visu)
{
- VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType
- << std::endl;
+ VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType << std::endl;
return result;
}
@@ -182,12 +163,8 @@ namespace VirtualRobot
return result;
}
- VirtualRobot::ObstaclePtr
- Obstacle::createCylinder(float radius,
- float height,
- VisualizationFactory::Color color,
- std::string visualizationType,
- CollisionCheckerPtr colChecker)
+
+ VirtualRobot::ObstaclePtr Obstacle::createCylinder(float radius, float height, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
@@ -216,8 +193,7 @@ namespace VirtualRobot
*/
if (!visu)
{
- VR_ERROR << "Could not create cylinder visualization with visu type "
- << visualizationType << std::endl;
+ VR_ERROR << "Could not create cylinder visualization with visu type " << visualizationType << std::endl;
return result;
}
@@ -239,10 +215,8 @@ namespace VirtualRobot
return result;
}
- VirtualRobot::ObstaclePtr
- Obstacle::createFromMesh(TriMeshModelPtr mesh,
- std::string visualizationType,
- CollisionCheckerPtr colChecker)
+
+ VirtualRobot::ObstaclePtr Obstacle::createFromMesh(TriMeshModelPtr mesh, std::string visualizationType, CollisionCheckerPtr colChecker)
{
THROW_VR_EXCEPTION_IF(!mesh, "Null data");
@@ -270,8 +244,7 @@ namespace VirtualRobot
if (!visu)
{
- VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType
- << std::endl;
+ VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType << std::endl;
return result;
}
@@ -291,8 +264,7 @@ namespace VirtualRobot
return result;
}
- void
- Obstacle::print(bool printDecoration /*= true*/)
+ void Obstacle::print(bool printDecoration /*= true*/)
{
if (printDecoration)
{
@@ -310,8 +282,7 @@ namespace VirtualRobot
}
}
- ObstaclePtr
- Obstacle::clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
+ ObstaclePtr Obstacle::clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
{
VisualizationNodePtr clonedVisualizationNode;
@@ -327,8 +298,7 @@ namespace VirtualRobot
clonedCollisionModel = collisionModel->clone(colChecker, scaling);
}
- ObstaclePtr result(
- new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
+ ObstaclePtr result(new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
result->primitiveApproximation = primitiveApproximation;
@@ -339,11 +309,7 @@ namespace VirtualRobot
return result;
}
- std::string
- Obstacle::toXML(const std::string& basePath,
- int tabs,
- const std::string& modelPathRelative,
- bool storeSensors)
+ std::string Obstacle::toXML(const std::string& basePath, int tabs, const std::string& modelPathRelative, bool storeSensors)
{
std::stringstream ss;
std::string t = "\t";
@@ -365,16 +331,15 @@ namespace VirtualRobot
return ss.str();
}
- std::string
- Obstacle::getFilename() const
+
+ std::string Obstacle::getFilename() const
{
return filename;
}
- void
- Obstacle::setFilename(const std::string& fname)
+ void Obstacle::setFilename(const std::string& fname)
{
this->filename = fname;
}
-} // namespace VirtualRobot
+} // namespace
diff --git a/VirtualRobot/Visualization/TriMeshModel.cpp b/VirtualRobot/Visualization/TriMeshModel.cpp
index 4ae51bb6ba2a82b21b0764ba9fe4a22abe9723fe..d8d15ddc0f49f3eae9f96911064e21d055df20e4 100644
--- a/VirtualRobot/Visualization/TriMeshModel.cpp
+++ b/VirtualRobot/Visualization/TriMeshModel.cpp
@@ -5,22 +5,23 @@
*/
#include "TriMeshModel.h"
+#include "../VirtualRobot.h"
+#include "../DataStructures/nanoflann.hpp"
+#include "../DataStructures/KdTreePointCloud.h"
+
+#include "../Import/MeshImport/AssimpReader.h"
+#include "VisualizationFactory.h"
+#include "VisualizationNode.h"
+
+#include<Eigen/Geometry>
#include <algorithm>
-#include <filesystem>
+#include <iostream>
#include <fstream>
#include <iomanip>
-#include <iostream>
#include <set>
+#include <filesystem>
-#include <Eigen/Geometry>
-
-#include "../DataStructures/KdTreePointCloud.h"
-#include "../DataStructures/nanoflann.hpp"
-#include "../Import/MeshImport/AssimpReader.h"
-#include "../VirtualRobot.h"
-#include "VisualizationFactory.h"
-#include "VisualizationNode.h"
namespace VirtualRobot
{
@@ -29,8 +30,7 @@ namespace VirtualRobot
TriMeshModel::TriMeshModel() = default;
- TriMeshModelPtr
- TriMeshModel::FromFile(const std::string& str)
+ TriMeshModelPtr TriMeshModel::FromFile(const std::string &str)
{
const auto tolower = [](std::string s)
{
@@ -49,7 +49,7 @@ namespace VirtualRobot
{
VisualizationFactoryPtr visualizationFactory;
visualizationFactory = VisualizationFactory::fromName("inventor", nullptr);
- std::array<char*, 1> argv = {nullptr};
+ std::array<char*,1> argv = {nullptr};
int argc = 0;
visualizationFactory->init(argc, argv.data(), "");
const auto vnode = visualizationFactory->getVisualizationFromFile(str);
@@ -57,9 +57,8 @@ namespace VirtualRobot
}
return nullptr;
}
-
- TriMeshModel
- TriMeshModel::MakeBox(float a, float b, float c)
+
+ TriMeshModel TriMeshModel::MakeBox(float a, float b, float c)
{
TriMeshModel m;
m.addVertex(0, 0, 0);
@@ -85,23 +84,17 @@ namespace VirtualRobot
addF(4, 5, 1, 0);
return m;
}
-
- TriMeshModel
- TriMeshModel::MakePoint(float x, float y, float z)
+ TriMeshModel TriMeshModel::MakePoint(float x, float y, float z)
{
std::vector<triangle> tr;
tr.emplace_back(x, y, z);
return {tr};
}
-
- TriMeshModelPtr
- TriMeshModel::MakePointPtr(float x, float y, float z)
+ TriMeshModelPtr TriMeshModel::MakePointPtr(float x, float y, float z)
{
return std::make_shared<VirtualRobot::TriMeshModel>(MakePoint(x, y, z));
}
-
- TriMeshModelPtr
- TriMeshModel::MakePointPtr(const Eigen::Vector3f& p)
+ TriMeshModelPtr TriMeshModel::MakePointPtr(const Eigen::Vector3f& p)
{
return MakePointPtr(p.x(), p.y(), p.z());
}
@@ -127,6 +120,8 @@ namespace VirtualRobot
}
}
+
+
/**
* This method adds the vertices \p vertex1,
* \p vertex2 and \p vertex3 to TriMeshModel::vertices and creates a new
@@ -136,23 +131,15 @@ namespace VirtualRobot
* \param vertex2 second vertex to use in the calculation
* \param vertex3 third vertex to use in the calculation
*/
- void
- TriMeshModel::addTriangleWithFace(const Eigen::Vector3f& vertex1,
- const Eigen::Vector3f& vertex2,
- const Eigen::Vector3f& vertex3)
+ void TriMeshModel::addTriangleWithFace(const Eigen::Vector3f& vertex1, const Eigen::Vector3f& vertex2, const Eigen::Vector3f& vertex3)
{
Eigen::Vector3f normal = TriMeshModel::CreateNormal(vertex1, vertex2, vertex3);
addTriangleWithFace(vertex1, vertex2, vertex3, normal);
}
- void
- TriMeshModel::addTriangleWithFace(const Eigen::Vector3f& vertex1,
- const Eigen::Vector3f& vertex2,
- const Eigen::Vector3f& vertex3,
- Eigen::Vector3f normal,
- const VisualizationFactory::Color& color1,
- const VisualizationFactory::Color& color2,
- const VisualizationFactory::Color& color3)
+ void TriMeshModel::addTriangleWithFace(
+ const Eigen::Vector3f& vertex1, const Eigen::Vector3f& vertex2, const Eigen::Vector3f& vertex3, Eigen::Vector3f normal,
+ const VisualizationFactory::Color& color1, const VisualizationFactory::Color& color2, const VisualizationFactory::Color& color3)
{
// VR_INFO << vertex1 << "\n\n" << vertex2 << "\n\n" << vertex3 << "\n\n" << std::endl;
this->addVertex(vertex1);
@@ -191,40 +178,26 @@ namespace VirtualRobot
this->addFace(face);
}
- void
- TriMeshModel::addTriangleWithFace(const Eigen::Vector3f& vertex1,
- const Eigen::Vector3f& vertex2,
- const Eigen::Vector3f& vertex3,
- const Eigen::Vector4f& vertexColor1,
- const Eigen::Vector4f& vertexColor2,
- const Eigen::Vector4f& vertexColor3)
+ void TriMeshModel::addTriangleWithFace(const Eigen::Vector3f& vertex1, const Eigen::Vector3f& vertex2, const Eigen::Vector3f& vertex3, const Eigen::Vector4f& vertexColor1, const Eigen::Vector4f& vertexColor2, const Eigen::Vector4f& vertexColor3)
{
Eigen::Vector3f normal = TriMeshModel::CreateNormal(vertex1, vertex2, vertex3);
- VisualizationFactory::Color color1(
- vertexColor1(0), vertexColor1(1), vertexColor1(2), vertexColor1(4));
- VisualizationFactory::Color color2(
- vertexColor2(0), vertexColor2(1), vertexColor2(2), vertexColor2(4));
- VisualizationFactory::Color color3(
- vertexColor3(0), vertexColor3(1), vertexColor3(2), vertexColor3(4));
+ VisualizationFactory::Color color1(vertexColor1(0), vertexColor1(1), vertexColor1(2), vertexColor1(4));
+ VisualizationFactory::Color color2(vertexColor2(0), vertexColor2(1), vertexColor2(2), vertexColor2(4));
+ VisualizationFactory::Color color3(vertexColor3(0), vertexColor3(1), vertexColor3(2), vertexColor3(4));
addTriangleWithFace(vertex1, vertex2, vertex3, normal, color1, color2, color3);
}
- void
- TriMeshModel::addMesh(const TriMeshModel& mesh)
+ void TriMeshModel::addMesh(const TriMeshModel& mesh)
{
for (const auto& face : mesh.faces)
{
- addTriangleWithFace(mesh.vertices.at(face.id1),
- mesh.vertices.at(face.id2),
- mesh.vertices.at(face.id3),
- face.normal,
- mesh.colors.at(face.idColor1),
- mesh.colors.at(face.idColor2),
- mesh.colors.at(face.idColor3));
+ addTriangleWithFace(mesh.vertices.at(face.id1), mesh.vertices.at(face.id2), mesh.vertices.at(face.id3),
+ face.normal, mesh.colors.at(face.idColor1), mesh.colors.at(face.idColor2), mesh.colors.at(face.idColor3));
}
// VR_INFO << " size after : " << vertices.size() << std::endl;
}
+
/**
* This method creates the normal belonging to the vertices \p vertex1,
* \p vertex2 and \p vertex3.
@@ -235,10 +208,7 @@ namespace VirtualRobot
*
* \return normal vector
*/
- Eigen::Vector3f
- TriMeshModel::CreateNormal(const Eigen::Vector3f& vertex1,
- const Eigen::Vector3f& vertex2,
- const Eigen::Vector3f& vertex3)
+ Eigen::Vector3f TriMeshModel::CreateNormal(const Eigen::Vector3f& vertex1, const Eigen::Vector3f& vertex2, const Eigen::Vector3f& vertex3)
{
static bool warningPrinted = false;
// calculate normal
@@ -252,8 +222,7 @@ namespace VirtualRobot
{
if (!warningPrinted)
{
- VR_INFO << ": Warning: tiny normal found in TriMeshModel. This error is printed "
- "only once!\n";
+ VR_INFO << ": Warning: tiny normal found in TriMeshModel. This error is printed only once!\n";
warningPrinted = true;
}
}
@@ -265,17 +234,16 @@ namespace VirtualRobot
return normal;
}
+
/**
* This method adds a face to the internal data structure TriMeshModel::faces.
*/
- void
- TriMeshModel::addFace(const MathTools::TriangleFace& face)
+ void TriMeshModel::addFace(const MathTools::TriangleFace& face)
{
faces.push_back(face);
}
- void
- TriMeshModel::addFace(unsigned int id0, unsigned int id1, unsigned int id2)
+ void TriMeshModel::addFace(unsigned int id0, unsigned int id1, unsigned int id2)
{
MathTools::TriangleFace f;
f.id1 = id0;
@@ -284,11 +252,11 @@ namespace VirtualRobot
addFace(f);
}
+
/**
* This method adds a vertex to the internal data structure TriMeshModel::vertices.
*/
- int
- TriMeshModel::addVertex(const Eigen::Vector3f& vertex)
+ int TriMeshModel::addVertex(const Eigen::Vector3f& vertex)
{
if (std::isnan(vertex[0]) || std::isnan(vertex[1]) || std::isnan(vertex[2]))
{
@@ -303,28 +271,27 @@ namespace VirtualRobot
/**
* This method adds a normal to the internal data structure TriMeshModel::normals.
*/
- unsigned int
- TriMeshModel::addNormal(const Eigen::Vector3f& normal)
+ unsigned int TriMeshModel::addNormal(const Eigen::Vector3f& normal)
{
normals.push_back(normal);
return static_cast<unsigned int>(normals.size() - 1);
+
}
/**
* This method adds a color to the internal data structure TriMeshModel::colors
*/
- unsigned int
- TriMeshModel::addColor(const VisualizationFactory::Color& color)
+ unsigned int TriMeshModel::addColor(const VisualizationFactory::Color& color)
{
colors.push_back(color);
return static_cast<unsigned int>(colors.size() - 1);
+
}
/**
* This method converts and adds a color to the internal data structure TriMeshModel::colors
*/
- unsigned int
- TriMeshModel::addColor(const Eigen::Vector4f& color)
+ unsigned int TriMeshModel::addColor(const Eigen::Vector4f& color)
{
return addColor(VisualizationFactory::Color(color(0), color(1), color(2), color(3)));
}
@@ -332,19 +299,18 @@ namespace VirtualRobot
/**
* This method converts and adds a color to the internal data structure TriMeshModel::materials
*/
- unsigned int
- TriMeshModel::addMaterial(const VisualizationFactory::PhongMaterial& material)
+ unsigned int TriMeshModel::addMaterial(const VisualizationFactory::PhongMaterial& material)
{
materials.push_back(material);
return static_cast<unsigned int>(materials.size() - 1);
}
+
/**
* This method clears the internal data structures TriMeshModel::faces and
* TriMeshModel::vertices.
*/
- void
- TriMeshModel::clear()
+ void TriMeshModel::clear()
{
vertices.clear();
colors.clear();
@@ -353,23 +319,19 @@ namespace VirtualRobot
boundingBox.clear();
}
- unsigned int
- TriMeshModel::addMissingNormals()
+ unsigned int TriMeshModel::addMissingNormals()
{
int counter = 0;
for (auto& face : faces)
{
- if (face.idNormal1 != UINT_MAX && face.idNormal2 != UINT_MAX &&
- face.idNormal3 != UINT_MAX)
+ if (face.idNormal1 != UINT_MAX && face.idNormal2 != UINT_MAX && face.idNormal3 != UINT_MAX)
{
continue;
}
- if (face.normal.norm() < 1e-10f || std::isnan(face.normal[0]) ||
- std::isnan(face.normal[1]) || std::isnan(face.normal[2]))
+ if (face.normal.norm() < 1e-10f || std::isnan(face.normal[0]) || std::isnan(face.normal[1]) || std::isnan(face.normal[2]))
{
- face.normal = CreateNormal(
- vertices.at(face.id1), vertices.at(face.id2), vertices.at(face.id3));
+ face.normal = CreateNormal(vertices.at(face.id1), vertices.at(face.id2), vertices.at(face.id3));
}
auto normalId = UINT_MAX;
if (face.idNormal1 == UINT_MAX)
@@ -405,8 +367,7 @@ namespace VirtualRobot
return static_cast<unsigned int>(counter);
}
- unsigned int
- TriMeshModel::addMissingColors(const VisualizationFactory::Color& color)
+ unsigned int TriMeshModel::addMissingColors(const VisualizationFactory::Color& color)
{
mergeVertices();
int counter = 0;
@@ -455,32 +416,32 @@ namespace VirtualRobot
face.idColor3 = getAndAddColorId(face.id3);
counter++;
}
+
}
return static_cast<unsigned int>(counter);
}
- void
- TriMeshModel::smoothNormalSurface()
+ void TriMeshModel::smoothNormalSurface()
{
mergeVertices();
addMissingNormals();
fattenShrink(0.0f, true);
}
+
/**
* This method calls TriangleFace::flipOrientation() on each entry in
* TriMeshModel::faces.
*/
- void
- TriMeshModel::flipVertexOrientations()
+ void TriMeshModel::flipVertexOrientations()
{
- std::for_each(faces.begin(),
- faces.end(),
- std::mem_fun_ref(&MathTools::TriangleFace::flipOrientation));
+ std::for_each(faces.begin(), faces.end(), std::mem_fun_ref(&MathTools::TriangleFace::flipOrientation));
}
- void
- TriMeshModel::mergeVertices(float mergeThreshold, bool removeVertices)
+
+
+
+ void TriMeshModel::mergeVertices(float mergeThreshold, bool removeVertices)
{
unsigned int size = static_cast<unsigned int>(vertices.size());
unsigned int faceCount = static_cast<unsigned int>(faces.size());
@@ -497,25 +458,25 @@ namespace VirtualRobot
cloud.pts.reserve(size);
for (unsigned int i = 0; i < size; ++i)
{
- cloud.pts.emplace_back(
- PointCloud<float>::Point{vertices.at(i)[0], vertices.at(i)[1], vertices.at(i)[2]});
+ cloud.pts.emplace_back(PointCloud<float>::Point{vertices.at(i)[0],
+ vertices.at(i)[1],
+ vertices.at(i)[2]});
}
using num_t = float;
// construct a kd-tree index:
- typedef nanoflann::KDTreeSingleIndexAdaptor<
- nanoflann::L2_Simple_Adaptor<num_t, PointCloud<num_t>>,
- PointCloud<num_t>,
- 3 /* dim */
- >
- my_kd_tree_t;
-
- my_kd_tree_t index(
- 3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
+ typedef nanoflann::KDTreeSingleIndexAdaptor <
+ nanoflann::L2_Simple_Adaptor<num_t, PointCloud<num_t> >,
+ PointCloud<num_t>,
+ 3 /* dim */
+ > my_kd_tree_t;
+
+ my_kd_tree_t index(3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
index.buildIndex();
+
const num_t search_radius = static_cast<num_t>(mergeThreshold);
- std::vector<std::pair<size_t, num_t>> ret_matches;
+ std::vector<std::pair<size_t, num_t> > ret_matches;
nanoflann::SearchParams params;
num_t query_pt[3];
@@ -526,8 +487,7 @@ namespace VirtualRobot
query_pt[0] = p1[0];
query_pt[1] = p1[1];
query_pt[2] = p1[2];
- const size_t nMatches =
- index.radiusSearch(&query_pt[0], search_radius, ret_matches, params);
+ const size_t nMatches = index.radiusSearch(&query_pt[0], search_radius, ret_matches, params);
for (size_t k = 0; k < nMatches; ++k)
{
unsigned int foundIndex = static_cast<unsigned int>(ret_matches.at(k).first);
@@ -607,8 +567,7 @@ namespace VirtualRobot
}
}
- size_t
- TriMeshModel::removeUnusedVertices()
+ size_t TriMeshModel::removeUnusedVertices()
{
auto size = vertices.size();
auto faceCount = faces.size();
@@ -650,33 +609,29 @@ namespace VirtualRobot
return removed;
}
- std::vector<float>
- TriMeshModel::getFaceAreas() const
+ std::vector<float> TriMeshModel::getFaceAreas() const
{
std::vector<float> areas;
for (const auto& face : faces)
{
float area = MathTools::getTriangleArea(
- vertices.at(face.id1), vertices.at(face.id2), vertices.at(face.id3));
+ vertices.at(face.id1), vertices.at(face.id2), vertices.at(face.id3));
areas.push_back(area);
}
return areas;
}
- float
- TriMeshModel::getVolume() const
+ float TriMeshModel::getVolume() const
{
float volume = 0.0f;
for (const auto& face : faces)
{
- volume += std::abs(
- vertices.at(face.id1).dot(vertices.at(face.id2).cross(vertices.at(face.id3))));
+ volume += std::abs(vertices.at(face.id1).dot(vertices.at(face.id2).cross(vertices.at(face.id3))));
}
return volume;
}
- void
- TriMeshModel::rotate(const Eigen::Matrix3f& mx)
+ void TriMeshModel::rotate(const Eigen::Matrix3f& mx)
{
for (auto& vec3f : normals)
{
@@ -690,8 +645,7 @@ namespace VirtualRobot
}
}
- void
- TriMeshModel::fattenShrink(float offset, bool updateNormals)
+ void TriMeshModel::fattenShrink(float offset, bool updateNormals)
{
size_t i;
size_t size = this->faces.size();
@@ -702,14 +656,10 @@ namespace VirtualRobot
auto& p1 = vertices.at(face.id1);
auto& p2 = vertices.at(face.id2);
auto& p3 = vertices.at(face.id3);
- auto normal1 =
- face.idNormal1 < normals.size() ? normals.at(face.idNormal1) : face.normal;
- auto normal2 =
- face.idNormal2 < normals.size() ? normals.at(face.idNormal2) : face.normal;
- auto normal3 =
- face.idNormal3 < normals.size() ? normals.at(face.idNormal3) : face.normal;
- if (std::isnan(face.normal[0]) || std::isnan(face.normal[1]) ||
- std::isnan(face.normal[2]))
+ auto normal1 = face.idNormal1 < normals.size() ? normals.at(face.idNormal1) : face.normal;
+ auto normal2 = face.idNormal2 < normals.size() ? normals.at(face.idNormal2) : face.normal;
+ auto normal3 = face.idNormal3 < normals.size() ? normals.at(face.idNormal3) : face.normal;
+ if (std::isnan(face.normal[0]) || std::isnan(face.normal[1]) || std::isnan(face.normal[2]))
{
std::cout << "NAN in face " << i << " : " << face.normal << std::endl;
}
@@ -765,7 +715,7 @@ namespace VirtualRobot
{
if (std::isnan(normal[0]) || std::isnan(normal[1]) || std::isnan(normal[2]))
{
- std::cout << "NAN in " << i << " : " << normal << std::endl;
+ std::cout << "NAN in " << i << " : " << normal << std::endl;
}
p += normal * offset;
}
@@ -783,10 +733,11 @@ namespace VirtualRobot
face.idNormal3 = addNormal(averageNormals.at(face.id3));
}
}
+
+
}
- void
- TriMeshModel::setColor(VisualizationFactory::Color color)
+ void TriMeshModel::setColor(VisualizationFactory::Color color)
{
colors.clear();
for (size_t i = 0; i < vertices.size(); i++)
@@ -795,12 +746,12 @@ namespace VirtualRobot
}
}
+
/**
* This method calculates the center of mass by accumulating all vertices and
* dividing the sum by the number of vertices.
*/
- Eigen::Vector3f
- TriMeshModel::getCOM()
+ Eigen::Vector3f TriMeshModel::getCOM()
{
Eigen::Vector3f centerOfMass = Eigen::Vector3f::Zero();
@@ -819,8 +770,7 @@ namespace VirtualRobot
return centerOfMass;
}
- bool
- TriMeshModel::getSize(Eigen::Vector3f& storeMinSize, Eigen::Vector3f& storeMaxSize)
+ bool TriMeshModel::getSize(Eigen::Vector3f& storeMinSize, Eigen::Vector3f& storeMaxSize)
{
if (vertices.size() == 0)
{
@@ -859,10 +809,7 @@ namespace VirtualRobot
*
* \return true if both faces share the same edge and false if not
*/
- bool
- TriMeshModel::checkFacesHaveSameEdge(const MathTools::TriangleFace& face1,
- const MathTools::TriangleFace& face2,
- std::vector<std::pair<int, int>>& commonVertexIds) const
+ bool TriMeshModel::checkFacesHaveSameEdge(const MathTools::TriangleFace& face1, const MathTools::TriangleFace& face2, std::vector<std::pair<int, int> >& commonVertexIds) const
{
commonVertexIds.clear();
Eigen::Vector2i vertexIds;
@@ -929,16 +876,16 @@ namespace VirtualRobot
return (2 == commonVertexIds.size());
}
+
/**
* This method checks if all normals of the model point inwards or outwards and
* flippes the faces which have a wrong orientation.
* \param inverted inverts the check if set to true
* \return the number of flipped faces
*/
- unsigned int
- TriMeshModel::checkAndCorrectNormals(bool inverted)
+ unsigned int TriMeshModel::checkAndCorrectNormals(bool inverted)
{
- MathTools::TriangleFace *f1, *f2;
+ MathTools::TriangleFace* f1, *f2;
int a1, a2, b1, b2;
int flippedFacesCount = 0;
@@ -956,7 +903,7 @@ namespace VirtualRobot
}
f2 = &(faces[j]);
- std::vector<std::pair<int, int>> commonVertexIds;
+ std::vector<std::pair<int, int> > commonVertexIds;
if (checkFacesHaveSameEdge(*f1, *f2, commonVertexIds))
{
@@ -964,10 +911,8 @@ namespace VirtualRobot
a2 = commonVertexIds[1].first; // second common vertex id face1
b1 = commonVertexIds[0].second; // first common vertex id face
b2 = commonVertexIds[1].second; // second common vertex id face2
- bool bAok =
- ((a1 == 1 && a2 == 2) || (a1 == 2 && a2 == 3) || (a1 == 3 && a2 == 1));
- bool bBok =
- ((b1 == 1 && b2 == 3) || (b1 == 3 && b2 == 2) || (b1 == 2 && b2 == 1));
+ bool bAok = ((a1 == 1 && a2 == 2) || (a1 == 2 && a2 == 3) || (a1 == 3 && a2 == 1));
+ bool bBok = ((b1 == 1 && b2 == 3) || (b1 == 3 && b2 == 2) || (b1 == 2 && b2 == 1));
if (inverted)
{
@@ -980,7 +925,7 @@ namespace VirtualRobot
flippedFacesCount++;
f2->flipOrientation();
}
- else if (!bAok && bBok)
+ else if (!bAok && bBok)
{
flippedFacesCount++;
f2->flipOrientation();
@@ -992,42 +937,36 @@ namespace VirtualRobot
return static_cast<unsigned int>(flippedFacesCount);
}
- Eigen::Vector3f
- TriMeshModel::getNormalOfFace(std::size_t faceId) const
+ Eigen::Vector3f TriMeshModel::getNormalOfFace(std::size_t faceId) const
{
const auto& face = faces.at(faceId);
- if (face.idNormal1 == UINT_MAX || face.idNormal2 == UINT_MAX || face.idNormal3 == UINT_MAX)
+ if (face.idNormal1 == UINT_MAX || face.idNormal2 == UINT_MAX || face.idNormal3 == UINT_MAX)
{
- return (normals.at(face.idNormal1) + normals.at(face.idNormal2) +
- normals.at(face.idNormal3))
- .normalized();
+ return (normals.at(face.idNormal1) + normals.at(face.idNormal2) + normals.at(face.idNormal3)).normalized();
}
return face.normal;
}
- void
- TriMeshModel::print()
+
+ void TriMeshModel::print()
{
- std::cout << "TriMeshModel\nNr of Faces:" << faces.size()
- << "\nNr of vertices:" << vertices.size() << std::endl;
+ std::cout << "TriMeshModel\nNr of Faces:" << faces.size() << "\nNr of vertices:" << vertices.size() << std::endl;
boundingBox.print();
}
- void
- TriMeshModel::printNormals()
+
+ void TriMeshModel::printNormals()
{
std::cout << "TriMeshModel Normals:" << std::endl;
std::streamsize pr = cout.precision(2);
for (auto& face : faces)
{
- std::cout << "<" << face.normal(0) << "," << face.normal(1) << "," << face.normal(2)
- << ">,";
+ std::cout << "<" << face.normal(0) << "," << face.normal(1) << "," << face.normal(2) << ">,";
}
cout.precision(pr);
}
- void
- TriMeshModel::printVertices()
+ void TriMeshModel::printVertices()
{
std::cout << "TriMeshModel Vertices:" << std::endl;
std::streamsize pr = cout.precision(2);
@@ -1038,8 +977,7 @@ namespace VirtualRobot
cout.precision(pr);
}
- void
- TriMeshModel::printFaces()
+ void TriMeshModel::printFaces()
{
std::cout << "TriMeshModel Faces (vertex indices):" << std::endl;
std::streamsize pr = cout.precision(2);
@@ -1050,8 +988,8 @@ namespace VirtualRobot
cout.precision(pr);
}
- void
- TriMeshModel::scale(const Eigen::Vector3f& scaleFactor)
+
+ void TriMeshModel::scale(const Eigen::Vector3f& scaleFactor)
{
if (scaleFactor(0) == 1.0f && scaleFactor(1) == 1.0f && scaleFactor(2) == 1.0f)
{
@@ -1069,22 +1007,19 @@ namespace VirtualRobot
boundingBox.scale(scaleFactor);
}
- void
- TriMeshModel::scale(float scaleFactor)
+ void TriMeshModel::scale(float scaleFactor)
{
scale(Eigen::Vector3f{scaleFactor, scaleFactor, scaleFactor});
}
- TriMeshModelPtr
- TriMeshModel::clone() const
+ TriMeshModelPtr TriMeshModel::clone() const
{
Eigen::Vector3f scaleFactor;
scaleFactor << 1.0f, 1.0f, 1.0f;
return clone(scaleFactor);
}
- TriMeshModelPtr
- TriMeshModel::clone(const Eigen::Vector3f& scaleFactor) const
+ TriMeshModelPtr TriMeshModel::clone(const Eigen::Vector3f& scaleFactor) const
{
TriMeshModelPtr r(new TriMeshModel());
r->vertices = vertices;
@@ -1096,9 +1031,7 @@ namespace VirtualRobot
r->scale(scaleFactor);
return r;
}
-
- TriMeshModelPtr
- TriMeshModel::clone(float x, float y, float z) const
+ TriMeshModelPtr TriMeshModel::clone(float x, float y, float z) const
{
return clone(Eigen::Vector3f{x, y, z});
}