diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index 1d7b1191814046a2a6e5aa0d43a66d96b4203f1c..0626c8a661b847974cc1ff1ea63afd6623397e82 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -9,7 +9,6 @@
#include <cfloat>
#include <cstdlib>
-using namespace Eigen;
using namespace VirtualRobot;
namespace GraspStudio
@@ -95,7 +94,7 @@ namespace GraspStudio
return result;
}
- std::vector<Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Matrix4f& objectGP, const EndEffector::ContactInfoVector& contacts)
+ std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Eigen::Matrix4f& objectGP, const EndEffector::ContactInfoVector& contacts)
{
Eigen::Vector3f centerPos = getMean(contacts);
if (centerPos.hasNaN())
@@ -195,7 +194,7 @@ namespace GraspStudio
}
GraspEvaluationPoseUncertainty::PoseEvalResult GraspEvaluationPoseUncertainty::evaluatePose(
- EndEffectorPtr eef, ObstaclePtr object, const Matrix4f& objectPose,
+ EndEffectorPtr eef, ObstaclePtr object, const Eigen::Matrix4f& objectPose,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape,
float closingStepSize, float stepSizeSpeedFactor)
{
@@ -386,7 +385,7 @@ namespace GraspStudio
return res;
}
- Vector3f GraspEvaluationPoseUncertainty::getMean(const EndEffector::ContactInfoVector& contacts) const
+ Eigen::Vector3f GraspEvaluationPoseUncertainty::getMean(const EndEffector::ContactInfoVector& contacts) const
{
Eigen::Vector3f mean = mean.Zero();
if (contacts.empty())
diff --git a/GraspPlanning/MeshConverter.cpp b/GraspPlanning/MeshConverter.cpp
index 865047b661ad8db20dde7a4b56ae6a429adcc5b2..94d09c4a7cf6f2c27459f77ad3b336004784ba56 100644
--- a/GraspPlanning/MeshConverter.cpp
+++ b/GraspPlanning/MeshConverter.cpp
@@ -25,7 +25,6 @@
#include <GraspPlanning/ConvexHullGenerator.h>
using namespace std;
-using namespace Eigen;
using namespace VirtualRobot;
namespace GraspStudio
diff --git a/VirtualRobot/Dynamics/Dynamics.cpp b/VirtualRobot/Dynamics/Dynamics.cpp
index d965b7d1ca4e7da3f651a5900ba3ef7e05c0b9b9..1f69003649b63e6e9025e01b6a23fb104a928dbf 100644
--- a/VirtualRobot/Dynamics/Dynamics.cpp
+++ b/VirtualRobot/Dynamics/Dynamics.cpp
@@ -26,378 +26,372 @@
using std::cout;
using std::cin;
-using namespace VirtualRobot;
-using namespace RigidBodyDynamics;
-using namespace RigidBodyDynamics::Math;
+namespace VirtualRobot
+{
+ VirtualRobot::Dynamics::Dynamics(RobotNodeSetPtr rns, RobotNodeSetPtr rnsBodies, bool verbose) : rns(rns), rnsBodies(rnsBodies), verbose(verbose)
+ {
+ if (!rns)
+ {
+ THROW_VR_EXCEPTION("RobotNodeSetPtr for the joints is zero pointer");
+ }
+ VERBOSE_OUT << "joint values:\n" << rns->getJointValuesEigen() << endl;
+ gravity = RigidBodyDynamics::Math::Vector3d(0, 0, -9.81);
+ model = boost::shared_ptr<RigidBodyDynamics::Model>(new RigidBodyDynamics::Model());
+ model->gravity = gravity;
+ if (!rns->isKinematicChain())
+ {
+ THROW_VR_EXCEPTION("RobotNodeSet is not a kinematic chain!")
+ }
+ RobotNodePtr root = rns->getKinematicRoot();
+ //VERBOSE_OUT << "Root name: "<<root->getName()<<endl;
-VirtualRobot::Dynamics::Dynamics(RobotNodeSetPtr rns, RobotNodeSetPtr rnsBodies, bool verbose) : rns(rns), rnsBodies(rnsBodies), verbose(verbose)
-{
- if (!rns)
- {
- THROW_VR_EXCEPTION("RobotNodeSetPtr for the joints is zero pointer");
+ //Dynamics::toRBDLRecursive(model, root, root->getGlobalPose(), Eigen::Matrix4f::Identity());
+ Dynamics::toRBDL(model, rns->getNode(0), rns);
+ zeroVec = Eigen::VectorXd::Zero(model->dof_count);
}
- VERBOSE_OUT << "joint values:\n" << rns->getJointValuesEigen() << endl;
- gravity = Vector3d(0, 0, -9.81);
- model = boost::shared_ptr<RigidBodyDynamics::Model>(new Model());
- model->gravity = gravity;
-
- if (!rns->isKinematicChain())
+ Eigen::VectorXd Dynamics::getInverseDynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot, const Eigen::VectorXd& qddot)
{
- THROW_VR_EXCEPTION("RobotNodeSet is not a kinematic chain!")
+ Eigen::VectorXd tau = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ getInverseDynamics(q, qdot, qddot, tau);
+ return tau;
}
- RobotNodePtr root = rns->getKinematicRoot();
-
- //VERBOSE_OUT << "Root name: "<<root->getName()<<endl;
-
- //Dynamics::toRBDLRecursive(model, root, root->getGlobalPose(), Eigen::Matrix4f::Identity());
- Dynamics::toRBDL(model, rns->getNode(0), rns);
- zeroVec = Eigen::VectorXd::Zero(model->dof_count);
-}
-
-Eigen::VectorXd Dynamics::getInverseDynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot, const Eigen::VectorXd& qddot)
-{
- Eigen::VectorXd tau = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
- getInverseDynamics(q, qdot, qddot, tau);
- return tau;
-}
-
-void Dynamics::getInverseDynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, const Eigen::VectorXd &qddot, Eigen::VectorXd &tau)
-{
- tau.setZero();
- VR_ASSERT(tau.rows() == q.rows());
- InverseDynamics(*model.get(), q, qdot, qddot, tau);
-}
-
-Eigen::VectorXd Dynamics::getForwardDynamics(const Eigen::VectorXd &q, const Eigen::VectorXd &qdot, Eigen::VectorXd tau)
-{
- Eigen::VectorXd qddot = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
- ForwardDynamics(*model.get(), q, qdot, tau, qddot);
- return qddot;
-}
-
-
-Eigen::VectorXd Dynamics::getGravityMatrix(const Eigen::VectorXd &q)
-{
- Eigen::VectorXd tauGravity = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
- getGravityMatrix(q, tauGravity);
- return tauGravity;
-}
-
-void Dynamics::getGravityMatrix(const Eigen::VectorXd &q, Eigen::VectorXd &tau)
-{
- VR_ASSERT(q.rows() == Dynamics::model->dof_count);
- InverseDynamics(*model.get(), q, zeroVec, zeroVec, tau);
-}
-
-Eigen::VectorXd Dynamics::getCoriolisMatrix(const Eigen::VectorXd &q, const Eigen::VectorXd &qdot)
-{
-// Eigen::VectorXd qddot = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
- Eigen::VectorXd tauGravity = getGravityMatrix(q);
- Eigen::VectorXd tau = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
-
- Eigen::MatrixXd tauCoriolis = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
- InverseDynamics(*model.get(), q, qdot, zeroVec, tau);
- tauCoriolis = tau - tauGravity;
- return tauCoriolis;
-}
-
-
-
-Eigen::MatrixXd Dynamics::getInertiaMatrix(const Eigen::VectorXd &q, bool updateKinematics)
-{
- Eigen::MatrixXd inertia = Eigen::MatrixXd::Zero(model->dof_count, model->dof_count);
- getInertiaMatrix(q, inertia, updateKinematics);
- return inertia;
-}
-
-void Dynamics::getInertiaMatrix(const Eigen::VectorXd &q, Eigen::MatrixXd &inertiaMatrix, bool updateKinematics)
-{
- CompositeRigidBodyAlgorithm(*model.get(), q, inertiaMatrix, updateKinematics);
-}
-
-void Dynamics::setGravity(const Eigen::Vector3d &gravity)
-{
- model->gravity = gravity;
-}
-
-int Dynamics::getnDoF()
-{
- return model->dof_count;
-}
-
-void Dynamics::print()
-{
- std::string result = RigidBodyDynamics::Utils::GetModelHierarchy(*model.get());
- cout << "RBDL hierarchy of RNS:" << rns->getName() << endl;
- cout << result;
- result = RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(*model.get());
- cout << "RBDL origins of RNS:" << rns->getName() << endl;
- cout << result;
- result = RigidBodyDynamics::Utils::GetModelDOFOverview(*model.get());
- cout << "RBDL DoF of RNS:" << rns->getName() << endl;
- cout << result;
-}
-
-std::tuple<Eigen::Matrix3d, Eigen::Vector3d, double> Dynamics::computeCombinedPhysics(const std::set<RobotNodePtr> &nodes,
- const RobotNodePtr & referenceNode)
-{
- Eigen::Matrix3f combinedInertia;
- combinedInertia.setZero();
- Eigen::Vector3f combinedCoM ;
- combinedCoM.setZero();
- double massSum = referenceNode->getMass();
- for(auto& node : nodes)
+ void Dynamics::getInverseDynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot, const Eigen::VectorXd& qddot, Eigen::VectorXd& tau)
{
- massSum = node->getMass();
+ tau.setZero();
+ VR_ASSERT(tau.rows() == q.rows());
+ InverseDynamics(*model.get(), q, qdot, qddot, tau);
}
- Eigen::Matrix3f rotation = referenceNode->getGlobalPose().block<3,3>(0,0);
- Eigen::Vector3f comGlobalMeters = referenceNode->getCoMGlobal()/1000;
- Eigen::Matrix3f inertiaInGlobal = referenceNode->getInertiaMatrix(comGlobalMeters, rotation);
- combinedInertia += inertiaInGlobal;
- combinedCoM += referenceNode->getCoMGlobal() * referenceNode->getMass()/massSum;
-
- for(auto& node : nodes)
+ Eigen::VectorXd Dynamics::getForwardDynamics(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot, Eigen::VectorXd tau)
{
- Eigen::Matrix3f rotation = node->getGlobalPose().block<3,3>(0,0);
- Eigen::Vector3f comGlobalMeters = node->getCoMGlobal()/1000;
- Eigen::Matrix3f inertiaInGlobal = node->getInertiaMatrix(comGlobalMeters, rotation);
- combinedInertia += inertiaInGlobal;
- combinedCoM += node->getCoMGlobal() * node->getMass()/massSum;
+ Eigen::VectorXd qddot = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ ForwardDynamics(*model.get(), q, qdot, tau, qddot);
+ return qddot;
}
- combinedInertia = SceneObject::shiftInertia(combinedInertia, -combinedCoM/1000, massSum);
- rotation = referenceNode->getGlobalPose().block<3,3>(0,0).inverse();
- combinedInertia = rotation * combinedInertia * rotation.transpose();
- return std::make_tuple(combinedInertia.cast<double>(), combinedCoM.cast<double>(), massSum);
-}
-Body Dynamics::computeCombinedBody(const std::set<RobotNodePtr> &nodes, const RobotNodePtr &referenceNode) const
-{
-// VR_ASSERT(!nodes.empty());
- Eigen::Vector3d CoM = referenceNode->getCoMLocal().cast<double>()/1000;
- Matrix3d inertia = referenceNode->getInertiaMatrix().cast<double>();
- auto mainBody = rnsBodies && rnsBodies->hasRobotNode(referenceNode)?Body(referenceNode->getMass(), CoM, inertia):
- Body();
+ Eigen::VectorXd Dynamics::getGravityMatrix(const Eigen::VectorXd& q)
+ {
+ Eigen::VectorXd tauGravity = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ getGravityMatrix(q, tauGravity);
+ return tauGravity;
+ }
- for(auto node : nodes)
+ void Dynamics::getGravityMatrix(const Eigen::VectorXd& q, Eigen::VectorXd& tau)
{
- Eigen::Vector3d CoM = node->getCoMLocal().cast<double>()/1000;
- Matrix3d inertia = node->getInertiaMatrix().cast<double>();
+ VR_ASSERT(q.rows() == Dynamics::model->dof_count);
+ InverseDynamics(*model.get(), q, zeroVec, zeroVec, tau);
+ }
- auto otherBody = Body(node->getMass(), CoM, inertia);
- Eigen::Matrix4f transform = referenceNode->getTransformationTo(node);
- SpatialTransform rbdlTransform(transform.block<3,3>(0,0).cast<double>(), transform.block<3,1>(0,3).cast<double>()/1000);
- mainBody.Join(rbdlTransform, otherBody);
+ Eigen::VectorXd Dynamics::getCoriolisMatrix(const Eigen::VectorXd& q, const Eigen::VectorXd& qdot)
+ {
+ // Eigen::VectorXd qddot = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ Eigen::VectorXd tauGravity = getGravityMatrix(q);
+ Eigen::VectorXd tau = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+
+ Eigen::MatrixXd tauCoriolis = Eigen::VectorXd::Zero(Dynamics::model->dof_count);
+ InverseDynamics(*model.get(), q, qdot, zeroVec, tau);
+ tauCoriolis = tau - tauGravity;
+ return tauCoriolis;
}
- return mainBody;
-}
-bool Dynamics::getVerbose() const
-{
- return verbose;
-}
-void Dynamics::setVerbose(bool value)
-{
- verbose = value;
-}
-boost::shared_ptr<RigidBodyDynamics::Model> Dynamics::getModel() const
-{
- return model;
-}
+ Eigen::MatrixXd Dynamics::getInertiaMatrix(const Eigen::VectorXd& q, bool updateKinematics)
+ {
+ Eigen::MatrixXd inertia = Eigen::MatrixXd::Zero(model->dof_count, model->dof_count);
+ getInertiaMatrix(q, inertia, updateKinematics);
+ return inertia;
+ }
-// this method just selects the first node with an attached mass that is no Joint
-RobotNodePtr Dynamics::checkForConnectedMass(RobotNodePtr node)
-{
- if (!node)
+ void Dynamics::getInertiaMatrix(const Eigen::VectorXd& q, Eigen::MatrixXd& inertiaMatrix, bool updateKinematics)
{
- return RobotNodePtr();
+ CompositeRigidBodyAlgorithm(*model.get(), q, inertiaMatrix, updateKinematics);
}
- BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
+ void Dynamics::setGravity(const Eigen::Vector3d& gravity)
{
- RobotNodePtr childPtr = boost::dynamic_pointer_cast<RobotNode>(child);
+ model->gravity = gravity;
+ }
- if (childPtr != 0 && // existing
- childPtr->getMass() > 0 && // has mass
- (childPtr->isTranslationalJoint() // is translational joint
- || (!childPtr->isTranslationalJoint() && !childPtr->isRotationalJoint()))) // or fixed joint
- {
- return childPtr;
- }
+ int Dynamics::getnDoF()
+ {
+ return model->dof_count;
}
- BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
+
+ void Dynamics::print()
{
- RobotNodePtr rnPtr = boost::dynamic_pointer_cast<RobotNode>(child);
- RobotNodePtr childPtr;
+ std::string result = RigidBodyDynamics::Utils::GetModelHierarchy(*model.get());
+ cout << "RBDL hierarchy of RNS:" << rns->getName() << endl;
+ cout << result;
+ result = RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(*model.get());
+ cout << "RBDL origins of RNS:" << rns->getName() << endl;
+ cout << result;
+ result = RigidBodyDynamics::Utils::GetModelDOFOverview(*model.get());
+ cout << "RBDL DoF of RNS:" << rns->getName() << endl;
+ cout << result;
+ }
- if (rnPtr && !rnPtr->isRotationalJoint()) // break recursion if child is rot joint
+ std::tuple<Eigen::Matrix3d, Eigen::Vector3d, double> Dynamics::computeCombinedPhysics(const std::set<RobotNodePtr>& nodes,
+ const RobotNodePtr& referenceNode)
+ {
+ Eigen::Matrix3f combinedInertia;
+ combinedInertia.setZero();
+ Eigen::Vector3f combinedCoM ;
+ combinedCoM.setZero();
+ double massSum = referenceNode->getMass();
+ for (auto& node : nodes)
{
- childPtr = checkForConnectedMass(rnPtr);
+ massSum = node->getMass();
}
- if (childPtr)
+ Eigen::Matrix3f rotation = referenceNode->getGlobalPose().block<3, 3>(0, 0);
+ Eigen::Vector3f comGlobalMeters = referenceNode->getCoMGlobal() / 1000;
+ Eigen::Matrix3f inertiaInGlobal = referenceNode->getInertiaMatrix(comGlobalMeters, rotation);
+ combinedInertia += inertiaInGlobal;
+ combinedCoM += referenceNode->getCoMGlobal() * referenceNode->getMass() / massSum;
+
+ for (auto& node : nodes)
{
- return childPtr;
+ Eigen::Matrix3f rotation = node->getGlobalPose().block<3, 3>(0, 0);
+ Eigen::Vector3f comGlobalMeters = node->getCoMGlobal() / 1000;
+ Eigen::Matrix3f inertiaInGlobal = node->getInertiaMatrix(comGlobalMeters, rotation);
+ combinedInertia += inertiaInGlobal;
+ combinedCoM += node->getCoMGlobal() * node->getMass() / massSum;
}
+ combinedInertia = SceneObject::shiftInertia(combinedInertia, -combinedCoM / 1000, massSum);
+ rotation = referenceNode->getGlobalPose().block<3, 3>(0, 0).inverse();
+ combinedInertia = rotation * combinedInertia * rotation.transpose();
+ return std::make_tuple(combinedInertia.cast<double>(), combinedCoM.cast<double>(), massSum);
}
- return RobotNodePtr();
-}
-std::set<RobotNodePtr> Dynamics::getChildrenWithMass(const RobotNodePtr &node, const RobotNodeSetPtr &nodeSet) const
-{
- std::set<RobotNodePtr> result;
- for(auto& obj : node->getChildren())
+ RigidBodyDynamics::Body Dynamics::computeCombinedBody(const std::set<RobotNodePtr>& nodes, const RobotNodePtr& referenceNode) const
{
- auto node = boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(obj);
+ // VR_ASSERT(!nodes.empty());
+ Eigen::Vector3d CoM = referenceNode->getCoMLocal().cast<double>() / 1000;
+ RigidBodyDynamics::Math::Matrix3d inertia = referenceNode->getInertiaMatrix().cast<double>();
- if(node && nodeSet->hasRobotNode(node))
- {
- continue;
- }
- if(node && obj->getMass() > 0.01 && (!rnsBodies || rnsBodies->hasRobotNode(node)))
- {
- result.insert(node);
- }
- if(node)
+ auto mainBody = rnsBodies && rnsBodies->hasRobotNode(referenceNode) ? RigidBodyDynamics::Body(referenceNode->getMass(), CoM, inertia) :
+ RigidBodyDynamics::Body();
+
+ for (auto node : nodes)
{
- auto tmp = getChildrenWithMass(node, nodeSet);
- result.insert(tmp.begin(), tmp.end());
+ Eigen::Vector3d CoM = node->getCoMLocal().cast<double>() / 1000;
+ RigidBodyDynamics::Math::Matrix3d inertia = node->getInertiaMatrix().cast<double>();
+
+ auto otherBody = RigidBodyDynamics::Body(node->getMass(), CoM, inertia);
+ Eigen::Matrix4f transform = referenceNode->getTransformationTo(node);
+ RigidBodyDynamics::Math::SpatialTransform rbdlTransform(transform.block<3, 3>(0, 0).cast<double>(), transform.block<3, 1>(0, 3).cast<double>() / 1000);
+ mainBody.Join(rbdlTransform, otherBody);
}
+ return mainBody;
}
- return result;
-}
-
-// rbdl: (trafo->joint->body) -> (trafo->joint->body) -> (trafo->joint->body) ...
-void Dynamics::toRBDLRecursive(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, Eigen::Matrix4f accumulatedTransformPreJoint, Eigen::Matrix4f accumulatedTransformPostJoint, RobotNodePtr jointNode, int parentID)
-{
- VR_ASSERT(model);
- VR_ASSERT(node);
- int nodeID = parentID; // might be overwritten when adding new body
- float mass = node->getMass();
+ bool Dynamics::getVerbose() const
+ {
+ return verbose;
+ }
+ void Dynamics::setVerbose(bool value)
+ {
+ verbose = value;
+ }
- if (!jointNode)
+ boost::shared_ptr<RigidBodyDynamics::Model> Dynamics::getModel() const
{
- accumulatedTransformPreJoint *= node->getLocalTransformation();
+ return model;
+ }
- if (Dynamics::rns->hasRobotNode(node) && (node->isRotationalJoint() || node->isTranslationalJoint()))
+ // this method just selects the first node with an attached mass that is no RigidBodyDynamics::Joint
+ RobotNodePtr Dynamics::checkForConnectedMass(RobotNodePtr node)
+ {
+ if (!node)
{
- jointNode = node;
+ return RobotNodePtr();
}
- }
- else
- {
- accumulatedTransformPostJoint *= node->getLocalTransformation();
- if (Dynamics::rns->hasRobotNode(node) && (node->isRotationalJoint() || node->isTranslationalJoint()))
+ BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
{
- VR_ERROR << "Skipping joint " << node->getName() << ": multiple joints in row without masses inbetween..." << endl;
+ RobotNodePtr childPtr = boost::dynamic_pointer_cast<RobotNode>(child);
+
+ if (childPtr != 0 && // existing
+ childPtr->getMass() > 0 && // has mass
+ (childPtr->isTranslationalJoint() // is translational joint
+ || (!childPtr->isTranslationalJoint() && !childPtr->isRotationalJoint()))) // or fixed joint
+ {
+ return childPtr;
+ }
}
+ BOOST_FOREACH(SceneObjectPtr child, node->getChildren())
+ {
+ RobotNodePtr rnPtr = boost::dynamic_pointer_cast<RobotNode>(child);
+ RobotNodePtr childPtr;
+
+ if (rnPtr && !rnPtr->isRotationalJoint()) // break recursion if child is rot joint
+ {
+ childPtr = checkForConnectedMass(rnPtr);
+ }
+
+ if (childPtr)
+ {
+ return childPtr;
+ }
+ }
+ return RobotNodePtr();
}
-
- if (mass > 0 && Dynamics::rns->hasRobotNode(node))
+ std::set<RobotNodePtr> Dynamics::getChildrenWithMass(const RobotNodePtr& node, const RobotNodeSetPtr& nodeSet) const
{
- // create a body
- //Vector3d com = node->getCoMLocal().cast<double>() / 1000; // divide by 1000 because Simox defines lengths in mm while the RBDL defines lengths in m
- //Matrix3d inertia = node->getInertiaMatrix().cast<double>();
-
- // apply postJoint transform
- Eigen::Vector4f comTr;
- comTr.head(3) = node->getCoMLocal();
- comTr(3) = 1.0f;
- Vector3d com = (accumulatedTransformPostJoint * comTr).head(3).cast<double>() / 1000.0;
-
- // convert inertia from node to jointFrame (I_new = R * I_old * R^T)
- Eigen::Matrix3f trafo = accumulatedTransformPostJoint.block(0, 0, 3, 3);
- Eigen::Matrix3f inertia2 = trafo * node->getInertiaMatrix() * trafo.transpose();
- Matrix3d inertia = inertia2.cast<double>();
-
-
- Body body = Body(mass, com, inertia);
-
- Matrix3d spatial_rotation = accumulatedTransformPreJoint.block(0, 0, 3, 3).cast<double>();
- Vector3d spatial_translation = accumulatedTransformPreJoint.col(3).head(3).cast<double>() / 1000.0;
- SpatialTransform spatial_transform = SpatialTransform(spatial_rotation, spatial_translation);
- VERBOSE_OUT << "****** spatial_translation: " << spatial_translation.transpose() << endl;
-
- // joint
- Joint joint = Joint(JointTypeFixed);
-
- if (jointNode && jointNode->isRotationalJoint())
+ std::set<RobotNodePtr> result;
+ for (auto& obj : node->getChildren())
{
- JointType joint_type = JointTypeRevolute;
- boost::shared_ptr<RobotNodeRevolute> rev = boost::dynamic_pointer_cast<RobotNodeRevolute>(jointNode);
- VR_ASSERT(rev);
- Vector3d joint_axis = rev->getJointRotationAxisInJointCoordSystem().cast<double>();
-
- joint = Joint(joint_type, joint_axis);
-
- VR_INFO << "Adding Rotational Joint" << endl;
+ auto node = boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(obj);
+
+ if (node && nodeSet->hasRobotNode(node))
+ {
+ continue;
+ }
+ if (node && obj->getMass() > 0.01 && (!rnsBodies || rnsBodies->hasRobotNode(node)))
+ {
+ result.insert(node);
+ }
+ if (node)
+ {
+ auto tmp = getChildrenWithMass(node, nodeSet);
+ result.insert(tmp.begin(), tmp.end());
+ }
}
- else if (jointNode && jointNode->isTranslationalJoint())
- {
- JointType joint_type = JointTypePrismatic;
- boost::shared_ptr<RobotNodePrismatic> prism = boost::dynamic_pointer_cast<RobotNodePrismatic>(jointNode);
- Vector3d joint_axis = prism->getJointTranslationDirectionJointCoordSystem().cast<double>();
+ return result;
+ }
- joint = Joint(joint_type, joint_axis);
+ // rbdl: (trafo->joint->body) -> (trafo->joint->body) -> (trafo->joint->body) ...
+ void Dynamics::toRBDLRecursive(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, Eigen::Matrix4f accumulatedTransformPreJoint, Eigen::Matrix4f accumulatedTransformPostJoint, RobotNodePtr jointNode, int parentID)
+ {
+ VR_ASSERT(model);
+ VR_ASSERT(node);
+ int nodeID = parentID; // might be overwritten when adding new body
+ float mass = node->getMass();
- VR_INFO << "Adding Translational Joint" << endl;
- }
- std::string nodeName;
- if (jointNode)
- {
- nodeName = jointNode->getName();
- }
- else
+ if (!jointNode)
{
- nodeName = node->getName();
- }
-
- nodeID = model->AddBody(parentID, spatial_transform, joint, body, nodeName);
- this->identifierMap[nodeName] = nodeID;
+ accumulatedTransformPreJoint *= node->getLocalTransformation();
- VERBOSE_OUT << "New body:" << node->getName() << ", " << nodeID << " :" << endl; // Debugging Info
- VERBOSE_OUT << "** SPATIAL TRAFO: " << endl << spatial_transform.toMatrix() << endl;
- VERBOSE_OUT << "** MASS: " << body.mMass << endl;
- VERBOSE_OUT << "** COM: " << body.mCenterOfMass.transpose() << endl;
- VERBOSE_OUT << "** INERTIA: " << endl << body.mInertia << endl;
- VERBOSE_OUT << "** mIsVirtual: " << body.mIsVirtual << endl;
-
- if (jointNode)
- {
- VERBOSE_OUT << "** Joint: " << jointNode->getName() << endl;
+ if (Dynamics::rns->hasRobotNode(node) && (node->isRotationalJoint() || node->isTranslationalJoint()))
+ {
+ jointNode = node;
+ }
}
else
{
- VERBOSE_OUT << "** Joint: none" << endl;
- }
+ accumulatedTransformPostJoint *= node->getLocalTransformation();
- if (joint.mJointAxes)
- {
- VERBOSE_OUT << "** JOINT AXES " << endl << *(joint.mJointAxes) << endl;
+ if (Dynamics::rns->hasRobotNode(node) && (node->isRotationalJoint() || node->isTranslationalJoint()))
+ {
+ VR_ERROR << "Skipping joint " << node->getName() << ": multiple joints in row without masses inbetween..." << endl;
+ }
}
- // reset pre and post trafos and jointNode
- accumulatedTransformPreJoint = Eigen::Matrix4f::Identity();
- accumulatedTransformPostJoint = Eigen::Matrix4f::Identity();
- jointNode.reset();
- } /*else
+ if (mass > 0 && Dynamics::rns->hasRobotNode(node))
+ {
+ // create a body
+ //RigidBodyDynamics::Math::Vector3d com = node->getCoMLocal().cast<double>() / 1000; // divide by 1000 because Simox defines lengths in mm while the RBDL defines lengths in m
+ //RigidBodyDynamics::Math::Matrix3d inertia = node->getInertiaMatrix().cast<double>();
+
+ // apply postJoint transform
+ Eigen::Vector4f comTr;
+ comTr.head(3) = node->getCoMLocal();
+ comTr(3) = 1.0f;
+ RigidBodyDynamics::Math::Vector3d com = (accumulatedTransformPostJoint * comTr).head(3).cast<double>() / 1000.0;
+
+ // convert inertia from node to jointFrame (I_new = R * I_old * R^T)
+ Eigen::Matrix3f trafo = accumulatedTransformPostJoint.block(0, 0, 3, 3);
+ Eigen::Matrix3f inertia2 = trafo * node->getInertiaMatrix() * trafo.transpose();
+ RigidBodyDynamics::Math::Matrix3d inertia = inertia2.cast<double>();
+
+
+ RigidBodyDynamics::Body body = RigidBodyDynamics::Body(mass, com, inertia);
+
+ RigidBodyDynamics::Math::Matrix3d spatial_rotation = accumulatedTransformPreJoint.block(0, 0, 3, 3).cast<double>();
+ RigidBodyDynamics::Math::Vector3d spatial_translation = accumulatedTransformPreJoint.col(3).head(3).cast<double>() / 1000.0;
+ RigidBodyDynamics::Math::SpatialTransform spatial_transform = RigidBodyDynamics::Math::SpatialTransform(spatial_rotation, spatial_translation);
+ VERBOSE_OUT << "****** spatial_translation: " << spatial_translation.transpose() << endl;
+
+ // joint
+ RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
+
+ if (jointNode && jointNode->isRotationalJoint())
+ {
+ RigidBodyDynamics::JointType joint_type = RigidBodyDynamics::JointTypeRevolute;
+ boost::shared_ptr<RobotNodeRevolute> rev = boost::dynamic_pointer_cast<RobotNodeRevolute>(jointNode);
+ VR_ASSERT(rev);
+ RigidBodyDynamics::Math::Vector3d joint_axis = rev->getJointRotationAxisInJointCoordSystem().cast<double>();
+
+ joint = RigidBodyDynamics::Joint(joint_type, joint_axis);
+
+ VR_INFO << "Adding Rotational RigidBodyDynamics::Joint" << endl;
+ }
+ else if (jointNode && jointNode->isTranslationalJoint())
+ {
+ RigidBodyDynamics::JointType joint_type = RigidBodyDynamics::JointTypePrismatic;
+ boost::shared_ptr<RobotNodePrismatic> prism = boost::dynamic_pointer_cast<RobotNodePrismatic>(jointNode);
+ RigidBodyDynamics::Math::Vector3d joint_axis = prism->getJointTranslationDirectionJointCoordSystem().cast<double>();
+
+ joint = RigidBodyDynamics::Joint(joint_type, joint_axis);
+
+ VR_INFO << "Adding Translational RigidBodyDynamics::Joint" << endl;
+ }
+
+ std::string nodeName;
+
+ if (jointNode)
+ {
+ nodeName = jointNode->getName();
+ }
+ else
+ {
+ nodeName = node->getName();
+ }
+
+ nodeID = model->AddBody(parentID, spatial_transform, joint, body, nodeName);
+ this->identifierMap[nodeName] = nodeID;
+
+ VERBOSE_OUT << "New body:" << node->getName() << ", " << nodeID << " :" << endl; // Debugging Info
+ VERBOSE_OUT << "** SPATIAL TRAFO: " << endl << spatial_transform.toMatrix() << endl;
+ VERBOSE_OUT << "** MASS: " << body.mMass << endl;
+ VERBOSE_OUT << "** COM: " << body.mCenterOfMass.transpose() << endl;
+ VERBOSE_OUT << "** INERTIA: " << endl << body.mInertia << endl;
+ VERBOSE_OUT << "** mIsVirtual: " << body.mIsVirtual << endl;
+
+ if (jointNode)
+ {
+ VERBOSE_OUT << "** RigidBodyDynamics::Joint: " << jointNode->getName() << endl;
+ }
+ else
+ {
+ VERBOSE_OUT << "** RigidBodyDynamics::Joint: none" << endl;
+ }
+
+ if (joint.mJointAxes)
+ {
+ VERBOSE_OUT << "** JOINT AXES " << endl << *(joint.mJointAxes) << endl;
+ }
+
+ // reset pre and post trafos and jointNode
+ accumulatedTransformPreJoint = Eigen::Matrix4f::Identity();
+ accumulatedTransformPostJoint = Eigen::Matrix4f::Identity();
+ jointNode.reset();
+
+ } /*else
{
@@ -405,152 +399,154 @@ void Dynamics::toRBDLRecursive(boost::shared_ptr<RigidBodyDynamics::Model> model
}*/
- std::vector<SceneObjectPtr> children = node->getChildren();
-
- BOOST_FOREACH(SceneObjectPtr child, children)
- {
- boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
+ std::vector<SceneObjectPtr> children = node->getChildren();
- if (childRobotNode) // if cast returns 0 pointer, child is a sensor and can be omitted. also, child must be contained in the robotnodeset
+ BOOST_FOREACH(SceneObjectPtr child, children)
{
- //if (Dynamics::rns->hasRobotNode(childRobotNode))
- //{
- toRBDLRecursive(model, childRobotNode, accumulatedTransformPreJoint, accumulatedTransformPostJoint, jointNode, nodeID);
- //} else
- //{
- // VR_INFO << "skipping RN " << childRobotNode->getName() << " since it is not part of RNS" << endl;
- //}
+ boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
+
+ if (childRobotNode) // if cast returns 0 pointer, child is a sensor and can be omitted. also, child must be contained in the robotnodeset
+ {
+ //if (Dynamics::rns->hasRobotNode(childRobotNode))
+ //{
+ toRBDLRecursive(model, childRobotNode, accumulatedTransformPreJoint, accumulatedTransformPostJoint, jointNode, nodeID);
+ //} else
+ //{
+ // VR_INFO << "skipping RN " << childRobotNode->getName() << " since it is not part of RNS" << endl;
+ //}
+ }
}
- }
-}
+ }
-void Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, RobotNodeSetPtr nodeSet, RobotNodePtr parentNode, int parentID)
-{
- VERBOSE_OUT << "#######ADDING NODE " << node->getName() << endl;
- RobotNodePtr physicsFromChild;
- int nodeID = parentID;
- // need to define body, joint and spatial transform
- // body first
- auto relevantChildNodes = getChildrenWithMass(node, nodeSet);
- for(auto node : relevantChildNodes)
+ void Dynamics::toRBDL(boost::shared_ptr<RigidBodyDynamics::Model> model, RobotNodePtr node, RobotNodeSetPtr nodeSet, RobotNodePtr parentNode, int parentID)
{
- VERBOSE_OUT << "Additional child: " << node->getName() << " - " << node->getMass() << " kg" << endl;
- }
- auto combinedBody = computeCombinedBody(relevantChildNodes, node);
+ VERBOSE_OUT << "#######ADDING NODE " << node->getName() << endl;
+ RobotNodePtr physicsFromChild;
+ int nodeID = parentID;
+ // need to define body, joint and spatial transform
+ // body first
+ auto relevantChildNodes = getChildrenWithMass(node, nodeSet);
+ for (auto node : relevantChildNodes)
+ {
+ VERBOSE_OUT << "Additional child: " << node->getName() << " - " << node->getMass() << " kg" << endl;
+ }
+ auto combinedBody = computeCombinedBody(relevantChildNodes, node);
- Body body = combinedBody; //Body(mass, com, inertia);
+ RigidBodyDynamics::Body body = combinedBody; //Body(mass, com, inertia);
- // spatial transform next
- Eigen::Matrix4d trafo = Eigen::Matrix4d::Identity();
+ // spatial transform next
+ Eigen::Matrix4d trafo = Eigen::Matrix4d::Identity();
- if (parentNode)
- {
- trafo = node->getTransformationFrom(parentNode).cast<double>();
- }
- else if (!parentNode)
- {
- trafo = node->getGlobalPose().cast<double>();//node->getTransformationFrom(rns->getKinematicRoot()).cast<double>();
- }
+ if (parentNode)
+ {
+ trafo = node->getTransformationFrom(parentNode).cast<double>();
+ }
+ else if (!parentNode)
+ {
+ trafo = node->getGlobalPose().cast<double>();//node->getTransformationFrom(rns->getKinematicRoot()).cast<double>();
+ }
- Matrix3d spatial_rotation = trafo.block(0, 0, 3, 3);
- Vector3d spatial_translation = trafo.col(3).head(3) / 1000;
+ RigidBodyDynamics::Math::Matrix3d spatial_rotation = trafo.block(0, 0, 3, 3);
+ RigidBodyDynamics::Math::Vector3d spatial_translation = trafo.col(3).head(3) / 1000;
- VERBOSE_OUT << "****** spatial_translation: " << spatial_translation.transpose() << endl;
+ VERBOSE_OUT << "****** spatial_translation: " << spatial_translation.transpose() << endl;
- SpatialTransform spatial_transform = SpatialTransform(spatial_rotation.transpose(), spatial_translation);
+ RigidBodyDynamics::Math::SpatialTransform spatial_transform = RigidBodyDynamics::Math::SpatialTransform(spatial_rotation.transpose(), spatial_translation);
- // last, joint
- Joint joint = Joint(JointTypeFixed);
+ // last, joint
+ RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
- if (node->isRotationalJoint())
- {
- JointType joint_type = JointTypeRevolute;
- boost::shared_ptr<RobotNodeRevolute> rev = boost::dynamic_pointer_cast<RobotNodeRevolute>(node);
- Vector3d joint_axis = rev->getJointRotationAxisInJointCoordSystem().cast<double>();
+ if (node->isRotationalJoint())
+ {
+ RigidBodyDynamics::JointType joint_type = RigidBodyDynamics::JointTypeRevolute;
+ boost::shared_ptr<RobotNodeRevolute> rev = boost::dynamic_pointer_cast<RobotNodeRevolute>(node);
+ RigidBodyDynamics::Math::Vector3d joint_axis = rev->getJointRotationAxisInJointCoordSystem().cast<double>();
- joint = Joint(joint_type, joint_axis);
+ joint = RigidBodyDynamics::Joint(joint_type, joint_axis);
- VERBOSE_OUT << "Rotational Joint added:" << endl;
- }
- else if (node->isTranslationalJoint())
- {
- JointType joint_type = JointTypePrismatic;
- boost::shared_ptr<RobotNodePrismatic> prism = boost::dynamic_pointer_cast<RobotNodePrismatic>(node);
- Vector3d joint_axis = prism->getJointTranslationDirectionJointCoordSystem().cast<double>();
+ VERBOSE_OUT << "Rotational RigidBodyDynamics::Joint added:" << endl;
+ }
+ else if (node->isTranslationalJoint())
+ {
+ RigidBodyDynamics::JointType joint_type = RigidBodyDynamics::JointTypePrismatic;
+ boost::shared_ptr<RobotNodePrismatic> prism = boost::dynamic_pointer_cast<RobotNodePrismatic>(node);
+ RigidBodyDynamics::Math::Vector3d joint_axis = prism->getJointTranslationDirectionJointCoordSystem().cast<double>();
- joint = Joint(joint_type, joint_axis);
+ joint = RigidBodyDynamics::Joint(joint_type, joint_axis);
- VERBOSE_OUT << "translational Joint added" << endl;
- }
-
- if (joint.mJointType != JointTypeFixed)
- {
- nodeID = model->AddBody(parentID, spatial_transform, joint, body, node->getName());
- this->identifierMap[node->getName()] = nodeID;
- Eigen::VectorXd QDDot = Eigen::VectorXd::Zero(identifierMap.size());
-
- VERBOSE_OUT << "New body:" << node->getName() << ", " << nodeID << " :" << endl; // Debugging Info
- VERBOSE_OUT << "** SPATIAL TRAFO: " << endl << spatial_transform.r << endl << spatial_transform.E << endl;
- VERBOSE_OUT << "** MASS: " << body.mMass << endl;
- VERBOSE_OUT << "** COM: " << body.mCenterOfMass.transpose() << endl;
- VERBOSE_OUT << "** INERTIA: " << endl << body.mInertia << endl;
- VERBOSE_OUT << "** mIsVirtual: " << body.mIsVirtual << endl;
- Eigen::Vector3d bodyPosition = RigidBodyDynamics::CalcBodyToBaseCoordinates(*model, Eigen::VectorXd::Zero(identifierMap.size()), nodeID, Eigen::Vector3d::Zero(), true);
- Eigen::Vector3f positionInVirtualRobot = node->getGlobalPosition();//rns->getKinematicRoot()->transformTo(node, zeroVec);
- auto diff = (positionInVirtualRobot - bodyPosition.cast<float>()*1000).norm();
- if(diff > 0.01)
- {
- VR_ERROR << "forward kinematics between virtual robot and rbdl differ: " << diff << " mm";
- throw VirtualRobotException("forward kinematics between virtual robot and rbdl differ!");
+ VERBOSE_OUT << "translational RigidBodyDynamics::Joint added" << endl;
}
- // VERBOSE_OUT << "** position:\n" << bodyPosition << "\nposition in virtual robot:\n" << positionInVirtualRobot << endl << endl;
- if (joint.mJointAxes)
+ if (joint.mJointType != RigidBodyDynamics::JointTypeFixed)
{
- VERBOSE_OUT << "** JOINT AXES " << endl << *(joint.mJointAxes) << endl;
+ nodeID = model->AddBody(parentID, spatial_transform, joint, body, node->getName());
+ this->identifierMap[node->getName()] = nodeID;
+ Eigen::VectorXd QDDot = Eigen::VectorXd::Zero(identifierMap.size());
+
+ VERBOSE_OUT << "New body:" << node->getName() << ", " << nodeID << " :" << endl; // Debugging Info
+ VERBOSE_OUT << "** SPATIAL TRAFO: " << endl << spatial_transform.r << endl << spatial_transform.E << endl;
+ VERBOSE_OUT << "** MASS: " << body.mMass << endl;
+ VERBOSE_OUT << "** COM: " << body.mCenterOfMass.transpose() << endl;
+ VERBOSE_OUT << "** INERTIA: " << endl << body.mInertia << endl;
+ VERBOSE_OUT << "** mIsVirtual: " << body.mIsVirtual << endl;
+ Eigen::Vector3d bodyPosition = RigidBodyDynamics::CalcBodyToBaseCoordinates(*model, Eigen::VectorXd::Zero(identifierMap.size()), nodeID, Eigen::Vector3d::Zero(), true);
+ Eigen::Vector3f positionInVirtualRobot = node->getGlobalPosition();//rns->getKinematicRoot()->transformTo(node, zeroVec);
+ auto diff = (positionInVirtualRobot - bodyPosition.cast<float>() * 1000).norm();
+ if (diff > 0.01)
+ {
+ VR_ERROR << "forward kinematics between virtual robot and rbdl differ: " << diff << " mm";
+ throw VirtualRobotException("forward kinematics between virtual robot and rbdl differ!");
+ }
+ // VERBOSE_OUT << "** position:\n" << bodyPosition << "\nposition in virtual robot:\n" << positionInVirtualRobot << endl << endl;
+
+ if (joint.mJointAxes)
+ {
+ VERBOSE_OUT << "** JOINT AXES " << endl << *(joint.mJointAxes) << endl;
+ }
}
- }
- for (size_t i = 0; i < nodeSet->getSize(); ++i) {
- if(nodeSet->getNode(i) == node && i+1 < nodeSet->getSize())
+ for (size_t i = 0; i < nodeSet->getSize(); ++i)
{
- toRBDL(model, nodeSet->getNode(i+1), rns, node, nodeID);
+ if (nodeSet->getNode(i) == node && i + 1 < nodeSet->getSize())
+ {
+ toRBDL(model, nodeSet->getNode(i + 1), rns, node, nodeID);
+ }
}
- }
-// std::vector<SceneObjectPtr> children;
+ // std::vector<SceneObjectPtr> children;
-// // pick correct children to proceed the recursion
-// if (physicsFromChild != 0)
-// {
-// children = physicsFromChild->getChildren();
-// }
-// else
-// {
-// children = node->getChildren();
-// }
+ // // pick correct children to proceed the recursion
+ // if (physicsFromChild != 0)
+ // {
+ // children = physicsFromChild->getChildren();
+ // }
+ // else
+ // {
+ // children = node->getChildren();
+ // }
-// BOOST_FOREACH(SceneObjectPtr child, children)
-// {
-// boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
+ // BOOST_FOREACH(SceneObjectPtr child, children)
+ // {
+ // boost::shared_ptr<RobotNode> childRobotNode = boost::dynamic_pointer_cast<RobotNode>(child);
-// if (childRobotNode && Dynamics::rns->hasRobotNode(childRobotNode)) // if cast returns 0 pointer, child is a sensor and can be omitted. also, child must be contained in the robotnodeset
-// {
-// if (joint.mJointType == JointTypeFixed) // if current node is fixed, make its parent the parent of the next recursion step and thereby skip it
-// {
-// node = parentNode;
-// }
+ // if (childRobotNode && Dynamics::rns->hasRobotNode(childRobotNode)) // if cast returns 0 pointer, child is a sensor and can be omitted. also, child must be contained in the robotnodeset
+ // {
+ // if (joint.mJointType == RigidBodyDynamics::JointTypeFixed) // if current node is fixed, make its parent the parent of the next recursion step and thereby skip it
+ // {
+ // node = parentNode;
+ // }
-// toRBDL(model, childRobotNode, rns, node, nodeID);
+ // toRBDL(model, childRobotNode, rns, node, nodeID);
-// }
-// }
+ // }
+ // }
- return;
+ return;
+ }
}
diff --git a/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp b/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
index cd579e253363b04a2f12413a99cde7c67eae12c4..c1bf2d961fbe675a2374d30c2691129fb087f2e6 100644
--- a/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
+++ b/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
@@ -42,11 +42,11 @@ BOOST_AUTO_TEST_CASE(testRBDLTransformationOrientation)
model.AppendBody(
SpatialTransform(m.inverse(), // I have to use the rotation matrix for the frame transformation from A to B (Type 2) <------------
Eigen::Vector3d(0,0,0)),
- jointA, Body(), "bodyA");
+ jointA, RigidBodyDynamics::Body(), "bodyA");
Joint jointB = Joint(JointTypeRevolute, Eigen::Vector3d::UnitZ());
model.AppendBody(SpatialTransform(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0)),
- jointB, Body(), "bodyB");
+ jointB, RigidBodyDynamics::Body(), "bodyB");
Eigen::Vector3d positionInBodyB = CalcBaseToBodyCoordinates(model, Eigen::Vector2d::Zero(), 1, Eigen::Vector3d(1,0,0));
std::cout << "rotation: position in bodyB\n" << positionInBodyB << endl;
@@ -67,11 +67,11 @@ BOOST_AUTO_TEST_CASE(testRBDLTransformationTranslation)
model.AppendBody(
SpatialTransform(Eigen::Matrix3d::Identity(),
Eigen::Vector3d(1,0,0)), // I have to use the translation for the transformation between body A and B (type 1) <--------------------
- jointA, Body(), "bodyA");
+ jointA, RigidBodyDynamics::Body(), "bodyA");
Joint jointB = Joint(JointTypeRevolute, Eigen::Vector3d::UnitZ());
model.AppendBody(SpatialTransform(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0)),
- jointB, Body(), "bodyB");
+ jointB, RigidBodyDynamics::Body(), "bodyB");
Eigen::Vector3d positionInBodyB = CalcBaseToBodyCoordinates(model, Eigen::Vector2d::Zero(), 1, Eigen::Vector3d(0,0,0));
std::cout << "translation: position in bodyB\n" << positionInBodyB << endl;
diff --git a/VirtualRobot/IK/AdvancedIKSolver.cpp b/VirtualRobot/IK/AdvancedIKSolver.cpp
index a91953ec03263f10fe633c2034e3286247de97e0..019ce818b1e1f4fb0f0ac7596734ed8c8219025d 100644
--- a/VirtualRobot/IK/AdvancedIKSolver.cpp
+++ b/VirtualRobot/IK/AdvancedIKSolver.cpp
@@ -19,8 +19,6 @@
#include <algorithm>
-using namespace Eigen;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index 4cb382d6eaaa81d1217adf2e5f0299aef6df47ae..31aff3cf205cc3034c17a8d2be7a070c8b1c8577 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -13,7 +13,6 @@
//#define CHECK_PERFORMANCE
-using namespace Eigen;
namespace VirtualRobot
{
@@ -100,7 +99,7 @@ namespace VirtualRobot
}
}
- MatrixXf DifferentialIK::getJacobianMatrix()
+ Eigen::MatrixXf DifferentialIK::getJacobianMatrix()
{
updateJacobianMatrix(currentJacobian);
return currentJacobian;
@@ -129,7 +128,7 @@ namespace VirtualRobot
auto& mode = this->modes[tcp];
updateJacobianMatrix(partJacobians[tcp], tcp, mode);
//updatePartJacobian(tcp, mode, jacobian.block(index, 0, partJacobians[tcp].rows(), nDoF));
- //MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
+ //Eigen::MatrixXf partJacobian = this->getJacobianMatrix(tcp, mode);
jacobian.block(index, 0, partJacobians[tcp].rows(), nDoF) = partJacobians[tcp];
if (mode & IKSolver::X)
@@ -159,13 +158,13 @@ namespace VirtualRobot
}
}
- VectorXf DifferentialIK::getError(float stepSize)
+ Eigen::VectorXf DifferentialIK::getError(float stepSize)
{
updateError(currentError, stepSize);
return currentError;
}
- void DifferentialIK::updateError(VectorXf& error, float stepSize)
+ void DifferentialIK::updateError(Eigen::VectorXf& error, float stepSize)
{
VR_ASSERT(initialized);
@@ -179,7 +178,7 @@ namespace VirtualRobot
#endif
VR_ASSERT(static_cast<std::size_t>(error.rows()) == nRows);
- //VectorXf error(nRows);
+ //Eigen::VectorXf error(nRows);
// compute error
size_t index = 0;
@@ -234,17 +233,17 @@ namespace VirtualRobot
}
}
- MatrixXf DifferentialIK::getJacobianMatrix(SceneObjectPtr tcp)
+ Eigen::MatrixXf DifferentialIK::getJacobianMatrix(SceneObjectPtr tcp)
{
return getJacobianMatrix(tcp, IKSolver::All);
}
- MatrixXf DifferentialIK::getJacobianMatrix(IKSolver::CartesianSelection mode)
+ Eigen::MatrixXf DifferentialIK::getJacobianMatrix(IKSolver::CartesianSelection mode)
{
return getJacobianMatrix(SceneObjectPtr(), mode);
}
- MatrixXf DifferentialIK::getJacobianMatrix(SceneObjectPtr tcp, IKSolver::CartesianSelection mode)
+ Eigen::MatrixXf DifferentialIK::getJacobianMatrix(SceneObjectPtr tcp, IKSolver::CartesianSelection mode)
{
if (!initialized)
{
@@ -523,7 +522,7 @@ namespace VirtualRobot
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
- MatrixXf Jacobian = this->getJacobianMatrix(tcp, mode);
+ Eigen::MatrixXf Jacobian = this->getJacobianMatrix(tcp, mode);
#ifdef CHECK_PERFORMANCE
clock_t startT2 = clock();
#endif
@@ -585,8 +584,8 @@ namespace VirtualRobot
currentJacobian.resize(nRows, nDoF);
currentInvJacobian.resize(nDoF, nRows);
- tmpUpdateJacobianPosition = MatrixXf::Zero(3, nDoF);
- tmpUpdateJacobianOrientation = MatrixXf::Zero(3, nDoF);
+ tmpUpdateJacobianPosition = Eigen::MatrixXf::Zero(3, nDoF);
+ tmpUpdateJacobianOrientation = Eigen::MatrixXf::Zero(3, nDoF);
tmpComputeStepTheta.resize(nDoF);
@@ -660,7 +659,7 @@ namespace VirtualRobot
void DifferentialIK::setGoal(const Eigen::Vector3f& goal, SceneObjectPtr tcp, IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, bool performInitialization)
{
- Matrix4f trafo;
+ Eigen::Matrix4f trafo;
trafo.setIdentity();
trafo.block(0, 3, 3, 1) = goal;
this->setGoal(trafo, tcp, mode, tolerancePosition, toleranceRotation, performInitialization);
@@ -710,7 +709,7 @@ namespace VirtualRobot
delta.setZero();
- Vector3f position = goal.block(0, 3, 3, 1) - current.block(0, 3, 3, 1);
+ Eigen::Vector3f position = goal.block(0, 3, 3, 1) - current.block(0, 3, 3, 1);
if (mode & IKSolver::X)
{
@@ -731,19 +730,19 @@ namespace VirtualRobot
{
tmpDeltaOrientation = goal * current.inverse();
tmpDeltaAA = tmpDeltaOrientation.block<3, 3>(0, 0);
- //AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
+ //Eigen::AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
// TODO: make sure that angle is >0!?
delta.tail(3) = tmpDeltaAA.axis() * tmpDeltaAA.angle();
}
}
- VectorXf DifferentialIK::computeStep(float stepSize)
+ Eigen::VectorXf DifferentialIK::computeStep(float stepSize)
{
VR_ASSERT(initialized);
updateError(currentError, stepSize);
updateJacobianMatrix(currentJacobian);
- //VectorXf dTheta(nDoF);
+ //Eigen::VectorXf dTheta(nDoF);
updatePseudoInverseJacobianMatrix(currentInvJacobian, currentJacobian);
@@ -777,7 +776,7 @@ namespace VirtualRobot
tcp = getDefaultTCP();
}
- Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
+ Eigen::Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
float result = 0.0f;
if (modes[tcp] & IKSolver::X)
@@ -810,8 +809,8 @@ namespace VirtualRobot
tcp = getDefaultTCP();
}
- Matrix4f orientation = this->targets[tcp] * tcp->getGlobalPose().inverse();
- AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
+ Eigen::Matrix4f orientation = this->targets[tcp] * tcp->getGlobalPose().inverse();
+ Eigen::AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
return aa.angle();
}
@@ -880,7 +879,7 @@ namespace VirtualRobot
while (step < maxNStep)
{
- VectorXf dTheta = this->computeStep(stepSize);
+ Eigen::VectorXf dTheta = this->computeStep(stepSize);
for (unsigned int i = 0; i < nodes.size(); i++)
{
diff --git a/VirtualRobot/IK/DifferentialIK.h b/VirtualRobot/IK/DifferentialIK.h
index 249328b27c21a1c781bfa0fbdf52e494e14dc8f8..183396e0d58480e71ad214237c046d9aa93cfb22 100644
--- a/VirtualRobot/IK/DifferentialIK.h
+++ b/VirtualRobot/IK/DifferentialIK.h
@@ -80,7 +80,7 @@ namespace VirtualRobot
DifferentialIK dIK(leftArm);
// should yield the target vector.
- Vector3f target_position;
+ Eigen::Vector3f target_position;
// ...
// Set the target for the end effector.
@@ -151,10 +151,10 @@ namespace VirtualRobot
Given a target pose matrix and the actual tcp pose, the pseudo inverse Jacobian matrix can be used to compute the first Taylor expansion of the IK as follows:
\code
// given pose matrices
- Matrix4f target_pose, actual_pose;
+ Eigen::Matrix4f target_pose, actual_pose;
// the error vector
- VectorXd e(6);
+ Eigen::VectorXd e(6);
// The translational error is just the vector between the actual and the target position
e.segment(0,3) = target_pose(0,3,3,1) - actual_pose(0,3,3,1);
@@ -162,11 +162,11 @@ namespace VirtualRobot
// For the rotational error, the transformation between the poses has to be calculated and
// reformulated into the rotation axis and angle. The error is then the rotation axis scaled
// by the angle in radians.
- Matrix4f orientation = targets_pose * actual_pose.inverse();
- AngleAxis<float> aa(orientation.block<3,3>(0,0));
+ Eigen::Matrix4f orientation = targets_pose * actual_pose.inverse();
+ Eigen::AngleAxis<float> aa(orientation.block<3,3>(0,0));
e.segment(3,3) = aa.axis()*aa.angle();
// or
- AngleAxis orientation( target_pose * actual_pose.inverse() )
+ Eigen::AngleAxis orientation( target_pose * actual_pose.inverse() )
e.block(3,3) = orientation.axis() * orientation.angle();
// Calculate the IK
diff --git a/VirtualRobot/IK/GenericIKSolver.cpp b/VirtualRobot/IK/GenericIKSolver.cpp
index b0f0268859463a0dace4b84d07f3002777413081..01baa6e6f686f0741ae84fdabd78b3d96da2ded9 100644
--- a/VirtualRobot/IK/GenericIKSolver.cpp
+++ b/VirtualRobot/IK/GenericIKSolver.cpp
@@ -15,8 +15,6 @@
#include <algorithm>
-using namespace Eigen;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index 5ae8f2dd633623e98d53dfa5472d6767a7b51ad3..93f894bd38f42ec6c52dd3ffc1f5d1a894a97307 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -5,8 +5,6 @@
#include <algorithm>
-using namespace Eigen;
-
//#define CHECK_PERFORMANCE
namespace VirtualRobot
@@ -24,12 +22,12 @@ namespace VirtualRobot
JacobiProvider::~JacobiProvider()
= default;
- MatrixXd JacobiProvider::getJacobianMatrixD()
+ Eigen::MatrixXd JacobiProvider::getJacobianMatrixD()
{
return getJacobianMatrix().cast<double>();
}
- MatrixXd JacobiProvider::getJacobianMatrixD(SceneObjectPtr tcp)
+ Eigen::MatrixXd JacobiProvider::getJacobianMatrixD(SceneObjectPtr tcp)
{
return getJacobianMatrix(tcp).cast<double>();
}
@@ -39,7 +37,7 @@ namespace VirtualRobot
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
- MatrixXf Jacobian = this->getJacobianMatrix(tcp);
+ Eigen::MatrixXf Jacobian = this->getJacobianMatrix(tcp);
#ifdef CHECK_PERFORMANCE
clock_t startT2 = clock();
#endif
@@ -58,7 +56,7 @@ namespace VirtualRobot
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
- MatrixXd Jacobian = this->getJacobianMatrixD(tcp);
+ Eigen::MatrixXd Jacobian = this->getJacobianMatrixD(tcp);
#ifdef CHECK_PERFORMANCE
clock_t startT2 = clock();
#endif
@@ -106,28 +104,28 @@ namespace VirtualRobot
Eigen::MatrixXf JacobiProvider::getPseudoInverseJacobianMatrix(const Eigen::VectorXf regularization)
{
- MatrixXf Jacobian = this->getJacobianMatrix();
+ Eigen::MatrixXf Jacobian = this->getJacobianMatrix();
return computePseudoInverseJacobianMatrix(Jacobian, regularization);
//return getPseudoInverseJacobianMatrix(rns->getTCP());
}
Eigen::MatrixXd JacobiProvider::getPseudoInverseJacobianMatrixD(const Eigen::VectorXd regularization)
{
- MatrixXd Jacobian = this->getJacobianMatrixD();
+ Eigen::MatrixXd Jacobian = this->getJacobianMatrixD();
return computePseudoInverseJacobianMatrixD(Jacobian, regularization);
}
- Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, const VectorXf regularization) const
+ Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, const Eigen::VectorXf regularization) const
{
return computePseudoInverseJacobianMatrix(m, 0.0f, regularization);
}
- MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const MatrixXd& m, const Eigen::VectorXd regularization) const
+ Eigen::MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, const Eigen::VectorXd regularization) const
{
return computePseudoInverseJacobianMatrixD(m, 0.0, regularization);
}
- Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter, const VectorXf regularization) const
+ Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter, const Eigen::VectorXf regularization) const
{
Eigen::MatrixXf result(m.cols(), m.rows());
updatePseudoInverseJacobianMatrix(result, m, invParameter, regularization);
@@ -141,13 +139,13 @@ namespace VirtualRobot
return result;
}
- void JacobiProvider::updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter, VectorXf regularization) const
+ void JacobiProvider::updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter, Eigen::VectorXf regularization) const
{
Eigen::MatrixXf m2 = m;
VR_ASSERT(regularization.rows() == 0 || regularization.rows() == m2.rows());
if(regularization.rows() != m2.rows())
{
- regularization = VectorXf::Ones(m2.rows());
+ regularization = Eigen::VectorXf::Ones(m2.rows());
}
//std::cout << "regularization: " << regularization.transpose() << std::endl;
m2 = regularization.asDiagonal() * m2;
@@ -242,7 +240,7 @@ namespace VirtualRobot
VR_ASSERT(regularization.rows() == 0 || regularization.rows() == m2.rows());
if(regularization.rows() != m2.rows())
{
- regularization = VectorXd::Ones(m2.rows());
+ regularization = Eigen::VectorXd::Ones(m2.rows());
}
m2 = regularization.asDiagonal() * m2;
updatePseudoInverseJacobianMatrixDInternal(invJac, m2, invParameter);
diff --git a/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp b/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
index ee225221c58d6bc8bbceac7636cb496ae9ea0b3f..9e114dc4537659923240d966eab5fa471252216e 100644
--- a/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
+++ b/VirtualRobot/Import/COLLADA-light/ColladaSimox.cpp
@@ -12,7 +12,6 @@
#include <Eigen/Geometry>
using namespace std;
-using namespace boost;
//using namespace VirtualRobot;
#define DBG_NODE(NAME) (this->name.compare(NAME)==0)
@@ -140,12 +139,12 @@ namespace Collada
{
VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
- float jointLimitLow = lexical_cast<float>(this->kinematics_info.select_single_node("limits/min/float").node().child_value());
- float jointLimitHigh = lexical_cast<float>(this->kinematics_info.select_single_node("limits/max/float").node().child_value());
- float acceleration = lexical_cast<float>(this->motion_info.select_single_node("acceleration/float").node().child_value());
- float torque = lexical_cast<float>(this->motion_info.select_single_node("jerk/float").node().child_value());
- float velocity = lexical_cast<float>(this->motion_info.select_single_node("speed/float").node().child_value());
- float deceleration = lexical_cast<float>(this->motion_info.select_single_node("deceleration/float").node().child_value());
+ float jointLimitLow = boost::lexical_cast<float>(this->kinematics_info.select_single_node("limits/min/float").node().child_value());
+ float jointLimitHigh = boost::lexical_cast<float>(this->kinematics_info.select_single_node("limits/max/float").node().child_value());
+ float acceleration = boost::lexical_cast<float>(this->motion_info.select_single_node("acceleration/float").node().child_value());
+ float torque = boost::lexical_cast<float>(this->motion_info.select_single_node("jerk/float").node().child_value());
+ float velocity = boost::lexical_cast<float>(this->motion_info.select_single_node("speed/float").node().child_value());
+ float deceleration = boost::lexical_cast<float>(this->motion_info.select_single_node("deceleration/float").node().child_value());
Eigen::Vector3f axis = getVector3f(this->joint_axis.child("axis"));
float jointOffset = 0.0;
Eigen::Matrix4f preJointTransformation = Eigen::Matrix4f::Identity();
@@ -184,7 +183,7 @@ namespace Collada
{
//assert(rigidBodies.size()==1);
pugi::xml_node technique = rigidBodies[0].child("technique_common");
- float mass = lexical_cast<float>(technique.child("mass").child_value());
+ float mass = boost::lexical_cast<float>(technique.child("mass").child_value());
Eigen::Matrix4f massFrameTransformation = Eigen::Matrix4f::Identity();
BOOST_FOREACH(pugi::xpath_node trafo, technique.select_nodes(".//mass_frame/*"))
{
diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
index 93684b693104bdf8e5809bbabb63b0ab754d86b1..0f80bbf279ba4a5e83469cd1c7e0f9b846fd0171 100644
--- a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
+++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
@@ -18,7 +18,6 @@
using namespace std;
-using namespace urdf;
namespace VirtualRobot
{
@@ -202,7 +201,7 @@ namespace VirtualRobot
return result;
}
- VirtualRobot::VisualizationNodePtr SimoxURDFFactory::convertVisu(std::shared_ptr<Geometry> g, urdf::Pose& pose, const std::string& basePath)
+ VirtualRobot::VisualizationNodePtr SimoxURDFFactory::convertVisu(std::shared_ptr<urdf::Geometry> g, urdf::Pose& pose, const std::string& basePath)
{
const float scale = 1000.0f; // mm
@@ -218,14 +217,14 @@ namespace VirtualRobot
{
case urdf::Geometry::BOX:
{
- std::shared_ptr<Box> b = boost::dynamic_pointer_cast<Box>(g);
+ std::shared_ptr<urdf::Box> b = boost::dynamic_pointer_cast<urdf::Box>(g);
res = factory->createBox(b->dim.x * scale, b->dim.y * scale, b->dim.z * scale);
}
break;
case urdf::Geometry::SPHERE:
{
- std::shared_ptr<Sphere> s = boost::dynamic_pointer_cast<Sphere>(g);
+ std::shared_ptr<urdf::Sphere> s = boost::dynamic_pointer_cast<urdf::Sphere>(g);
res = factory->createSphere(s->radius * scale);
}
break;
@@ -233,7 +232,7 @@ namespace VirtualRobot
case urdf::Geometry::CYLINDER:
{
- std::shared_ptr<Cylinder> c = boost::dynamic_pointer_cast<Cylinder>(g);
+ std::shared_ptr<urdf::Cylinder> c = boost::dynamic_pointer_cast<urdf::Cylinder>(g);
res = factory->createCylinder(c->radius * scale, c->length * scale);
}
@@ -241,7 +240,7 @@ namespace VirtualRobot
case urdf::Geometry::MESH:
{
- std::shared_ptr<Mesh> m = boost::dynamic_pointer_cast<Mesh>(g);
+ std::shared_ptr<urdf::Mesh> m = boost::dynamic_pointer_cast<urdf::Mesh>(g);
std::string filename = getFilename(m->filename, basePath);
res = factory->getVisualizationFromFile(filename, false, m->scale.x, m->scale.y, m->scale.z);
}
@@ -311,7 +310,7 @@ namespace VirtualRobot
return res;
}
- RobotNodePtr SimoxURDFFactory::createBodyNode(RobotPtr robo, std::shared_ptr<Link> urdfBody, const std::string& basePath, bool useColModelsIfNoVisuModel)
+ RobotNodePtr SimoxURDFFactory::createBodyNode(RobotPtr robo, std::shared_ptr<urdf::Link> urdfBody, const std::string& basePath, bool useColModelsIfNoVisuModel)
{
RobotNodePtr result;
@@ -396,7 +395,7 @@ namespace VirtualRobot
return result;
}
- RobotNodePtr SimoxURDFFactory::createJointNode(RobotPtr robo, std::shared_ptr<Joint> urdfJoint)
+ RobotNodePtr SimoxURDFFactory::createJointNode(RobotPtr robo, std::shared_ptr<urdf::Joint> urdfJoint)
{
RobotNodePtr result;
diff --git a/VirtualRobot/LinkedCoordinate.cpp b/VirtualRobot/LinkedCoordinate.cpp
index 7e66fb4d56808544c75a891c079536bc37860f78..7ebbe6fc929381e57a9227ce6cf335575e195233 100644
--- a/VirtualRobot/LinkedCoordinate.cpp
+++ b/VirtualRobot/LinkedCoordinate.cpp
@@ -3,7 +3,6 @@
using namespace VirtualRobot;
-using namespace boost;
using namespace std;
@@ -28,12 +27,12 @@ void LinkedCoordinate::set(const RobotNodePtr& frame, const Eigen::Matrix4f& pos
{
if (!frame)
{
- THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!this->robot->hasRobotNode(frame))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
this->pose = pose;
@@ -45,7 +44,7 @@ void LinkedCoordinate::set(const std::string& frame, const Eigen::Matrix4f& pose
{
if (!this->robot->hasRobotNode(frame))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % frame % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
this->set(this->robot->getRobotNode(frame), pose);
@@ -72,12 +71,12 @@ void LinkedCoordinate::changeFrame(const RobotNodePtr& destination)
{
if (!destination)
{
- THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!this->robot->hasRobotNode(destination))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
if (!this->frame)
@@ -96,7 +95,7 @@ void LinkedCoordinate::changeFrame(const std::string& destination)
{
if (!this->robot->hasRobotNode(destination))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
this->changeFrame(this->robot->getRobotNode(destination));
@@ -106,12 +105,12 @@ Eigen::Matrix4f LinkedCoordinate::getInFrame(const RobotNodePtr& destination) co
{
if (!destination)
{
- THROW_VR_EXCEPTION(format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!this->robot->hasRobotNode(destination))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
return LinkedCoordinate::getCoordinateTransformation(this->frame, destination, this->robot) * this->pose;
@@ -122,7 +121,7 @@ Eigen::Matrix4f LinkedCoordinate::getInFrame(const std::string& destination) con
{
if (!this->robot->hasRobotNode(destination))
{
- THROW_VR_EXCEPTION(format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination % this->robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination % this->robot->getName() % BOOST_CURRENT_FUNCTION);
}
return LinkedCoordinate::getCoordinateTransformation(this->frame, this->robot->getRobotNode(destination), this->robot) * this->pose;
@@ -136,27 +135,27 @@ Eigen::Matrix4f LinkedCoordinate::getCoordinateTransformation(const RobotNodePtr
if (!destination)
{
- THROW_VR_EXCEPTION(format("Destination RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Destination RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!origin)
{
- THROW_VR_EXCEPTION(format("Origin RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Origin RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!robot)
{
- THROW_VR_EXCEPTION(format("RobotPtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("RobotPtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
}
if (!robot->hasRobotNode(origin))
{
- THROW_VR_EXCEPTION(format("Origin robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % origin->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Origin robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % origin->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
}
if (!robot->hasRobotNode(destination))
{
- THROW_VR_EXCEPTION(format("Destination robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
+ THROW_VR_EXCEPTION(boost::format("Destination robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
}
// std::cout << "Destination: " << destination->getName() <<std::endl << "Origin: " << origin->getName() << std::endl;
diff --git a/VirtualRobot/LinkedCoordinate.h b/VirtualRobot/LinkedCoordinate.h
index fcfabc06d5cf3da714216f9d584b17d8e0e6d653..1fa839c6f8adc0e4bfdfac9ae962aedcbf0c10b3 100644
--- a/VirtualRobot/LinkedCoordinate.h
+++ b/VirtualRobot/LinkedCoordinate.h
@@ -139,13 +139,13 @@ namespace VirtualRobot
* unit marix.
* @returns A homogeneous matrix of the pose
* @sa getPose()
- * @details If you are only interested in the translational part use iCoord::getPosition() or Matrix4f::block<3,1>(0,3).
+ * @details If you are only interested in the translational part use iCoord::getPosition() or Eigen::Matrix4f::block<3,1>(0,3).
* @throw VirtualRobotException An exception is thrown if frame is NULL.
*/
Eigen::Matrix4f getInFrame(const RobotNodePtr& frame) const;
/** Returns the actual pose stored in this object.
- * @details If you are only interested in the translational part use iCoord::getPosition() or Matrix4f::block<3,1>(0,3).
+ * @details If you are only interested in the translational part use iCoord::getPosition() or Eigen::Matrix4f::block<3,1>(0,3).
*/
inline Eigen::Matrix4f getPose() const
{
diff --git a/VirtualRobot/Nodes/CameraSensor.cpp b/VirtualRobot/Nodes/CameraSensor.cpp
index 5f8a6f1038d20422c303165bb8dc5581b183f386..297a80123b4bbaad96a35cffbe17616b7904af7e 100644
--- a/VirtualRobot/Nodes/CameraSensor.cpp
+++ b/VirtualRobot/Nodes/CameraSensor.cpp
@@ -2,8 +2,6 @@
#include "CameraSensor.h"
#include "CameraSensorFactory.h"
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/Nodes/ContactSensor.cpp b/VirtualRobot/Nodes/ContactSensor.cpp
index 5f78e2ab576bb22c72da12b95e73622d73a9f31e..48bffdc9695be35f49680b9544bafa8d983f9397 100644
--- a/VirtualRobot/Nodes/ContactSensor.cpp
+++ b/VirtualRobot/Nodes/ContactSensor.cpp
@@ -3,8 +3,6 @@
#include "ContactSensorFactory.h"
#include "../XML/BaseIO.h"
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.cpp b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
index 67d1be48e9e2dbca10986aad8f81674caf790b7d..a6c6f7195961714ca482367cea3bd7ea5d5cecb7 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensor.cpp
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
@@ -3,8 +3,6 @@
#include "ForceTorqueSensorFactory.h"
#include "../XML/BaseIO.h"
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/Nodes/PositionSensor.cpp b/VirtualRobot/Nodes/PositionSensor.cpp
index 136581a49e129a32e0e635c8eac359d189469866..fb16be5e084dd002de30879a682c13382b7b29a9 100644
--- a/VirtualRobot/Nodes/PositionSensor.cpp
+++ b/VirtualRobot/Nodes/PositionSensor.cpp
@@ -3,8 +3,6 @@
#include "PositionSensorFactory.h"
#include "../XML/BaseIO.h"
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index be8cbb8143e8ae31caba6037f536c8fc1ed71e3a..e5585c8767f01d2447f31f3d2787e89ae8915d71 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -16,8 +16,6 @@
#include <Eigen/Core>
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/Nodes/Sensor.cpp b/VirtualRobot/Nodes/Sensor.cpp
index 5dd29e474b7cabb222666602e811faa54d600a58..d3efe6497dd57f915bf60183cc82166dcf9a0acb 100644
--- a/VirtualRobot/Nodes/Sensor.cpp
+++ b/VirtualRobot/Nodes/Sensor.cpp
@@ -8,8 +8,6 @@
#include <Eigen/Core>
-using namespace boost;
-
namespace VirtualRobot
{
diff --git a/VirtualRobot/TimeOptimalTrajectory/Path.cpp b/VirtualRobot/TimeOptimalTrajectory/Path.cpp
index 2769c757927b9fd88794368e69417da5b0a92f06..00e686cb4986043eb2873884c2d97e6236088539 100644
--- a/VirtualRobot/TimeOptimalTrajectory/Path.cpp
+++ b/VirtualRobot/TimeOptimalTrajectory/Path.cpp
@@ -45,7 +45,6 @@
#include <Eigen/Geometry>
using namespace std;
-using namespace Eigen;
namespace VirtualRobot
{
@@ -170,22 +169,22 @@ namespace VirtualRobot
- Path::Path(const list<VectorXd> &path, double maxDeviation) :
+ Path::Path(const list<Eigen::VectorXd> &path, double maxDeviation) :
length(0.0)
{
if(path.size() < 2)
return;
- list<VectorXd>::const_iterator config1 = path.begin();
- list<VectorXd>::const_iterator config2 = config1;
+ list<Eigen::VectorXd>::const_iterator config1 = path.begin();
+ list<Eigen::VectorXd>::const_iterator config2 = config1;
config2++;
- list<VectorXd>::const_iterator config3;
- VectorXd startConfig = *config1;
+ list<Eigen::VectorXd>::const_iterator config3;
+ Eigen::VectorXd startConfig = *config1;
while(config2 != path.end()) {
config3 = config2;
config3++;
if(maxDeviation > 0.0 && config3 != path.end()) {
CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation);
- VectorXd endConfig = blendSegment->getConfig(0.0);
+ Eigen::VectorXd endConfig = blendSegment->getConfig(0.0);
if((endConfig - startConfig).norm() > 0.000001) {
pathSegments.push_back(new LinearPathSegment(startConfig, endConfig));
}
@@ -247,17 +246,17 @@ namespace VirtualRobot
return *it;
}
- VectorXd Path::getConfig(double s) const {
+ Eigen::VectorXd Path::getConfig(double s) const {
const PathSegment* pathSegment = getPathSegment(s);
return pathSegment->getConfig(s);
}
- VectorXd Path::getTangent(double s) const {
+ Eigen::VectorXd Path::getTangent(double s) const {
const PathSegment* pathSegment = getPathSegment(s);
return pathSegment->getTangent(s);
}
- VectorXd Path::getCurvature(double s) const {
+ Eigen::VectorXd Path::getCurvature(double s) const {
const PathSegment* pathSegment = getPathSegment(s);
return pathSegment->getCurvature(s);
}
diff --git a/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp b/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
index cb73911a03cb33615736aa3b131b8fd0edab4768..b61e88823b6c1a5730a51bdd71bc95757435d1fa 100644
--- a/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
+++ b/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
@@ -41,14 +41,13 @@
#include <iostream>
#include <fstream>
-using namespace Eigen;
using namespace std;
namespace VirtualRobot
{
const double TimeOptimalTrajectory::eps = 0.000001;
- TimeOptimalTrajectory::TimeOptimalTrajectory(const Path &path, const VectorXd &maxVelocity, const VectorXd &maxAcceleration, double timeStep) :
+ TimeOptimalTrajectory::TimeOptimalTrajectory(const Path &path, const Eigen::VectorXd &maxVelocity, const Eigen::VectorXd &maxAcceleration, double timeStep) :
path(path),
maxVelocity(maxVelocity),
maxAcceleration(maxAcceleration),
@@ -363,8 +362,8 @@ namespace VirtualRobot
}
double TimeOptimalTrajectory::getMinMaxPathAcceleration(double pathPos, double pathVel, bool max) {
- VectorXd configDeriv = path.getTangent(pathPos);
- VectorXd configDeriv2 = path.getCurvature(pathPos);
+ Eigen::VectorXd configDeriv = path.getTangent(pathPos);
+ Eigen::VectorXd configDeriv2 = path.getCurvature(pathPos);
double factor = max ? 1.0 : -1.0;
double maxPathAcceleration = numeric_limits<double>::max();
for(unsigned int i = 0; i < n; i++) {
@@ -382,8 +381,8 @@ namespace VirtualRobot
double TimeOptimalTrajectory::getAccelerationMaxPathVelocity(double pathPos) const {
double maxPathVelocity = numeric_limits<double>::infinity();
- const VectorXd configDeriv = path.getTangent(pathPos);
- const VectorXd configDeriv2 = path.getCurvature(pathPos);
+ const Eigen::VectorXd configDeriv = path.getTangent(pathPos);
+ const Eigen::VectorXd configDeriv2 = path.getCurvature(pathPos);
for(unsigned int i = 0; i < n; i++) {
if(configDeriv[i] != 0.0) {
for(unsigned int j = i + 1; j < n; j++) {
@@ -406,7 +405,7 @@ namespace VirtualRobot
double TimeOptimalTrajectory::getVelocityMaxPathVelocity(double pathPos) const {
- const VectorXd tangent = path.getTangent(pathPos);
+ const Eigen::VectorXd tangent = path.getTangent(pathPos);
double maxPathVelocity = numeric_limits<double>::max();
for(unsigned int i = 0; i < n; i++) {
maxPathVelocity = min(maxPathVelocity, maxVelocity[i] / abs(tangent[i]));
@@ -419,7 +418,7 @@ namespace VirtualRobot
}
double TimeOptimalTrajectory::getVelocityMaxPathVelocityDeriv(double pathPos) {
- const VectorXd tangent = path.getTangent(pathPos);
+ const Eigen::VectorXd tangent = path.getTangent(pathPos);
double maxPathVelocity = numeric_limits<double>::max();
unsigned int activeConstraint;
for(unsigned int i = 0; i < n; i++) {
@@ -459,7 +458,7 @@ namespace VirtualRobot
}
}
- VectorXd TimeOptimalTrajectory::getPosition(double time) const {
+ Eigen::VectorXd TimeOptimalTrajectory::getPosition(double time) const {
list<TimeOptimalTrajectoryStep>::const_iterator it = getTrajectorySegment(time);
list<TimeOptimalTrajectoryStep>::const_iterator previous = it;
previous--;
@@ -473,7 +472,7 @@ namespace VirtualRobot
return path.getConfig(pathPos);
}
- VectorXd TimeOptimalTrajectory::getVelocity(double time) const {
+ Eigen::VectorXd TimeOptimalTrajectory::getVelocity(double time) const {
list<TimeOptimalTrajectoryStep>::const_iterator it = getTrajectorySegment(time);
list<TimeOptimalTrajectoryStep>::const_iterator previous = it;
previous--;
diff --git a/VirtualRobot/Util/json/eigen_conversion.cpp b/VirtualRobot/Util/json/eigen_conversion.cpp
index 53b1d7e60afc2f9f131357184faae9534c609655..12c4885e6a38f7cc9920ee5804b241837b390233 100644
--- a/VirtualRobot/Util/json/eigen_conversion.cpp
+++ b/VirtualRobot/Util/json/eigen_conversion.cpp
@@ -4,7 +4,7 @@
namespace Eigen
{
template <>
- void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector)
+ void from_json<Eigen::Vector3f>(const nlohmann::json& j, Eigen::MatrixBase<Eigen::Vector3f>& vector)
{
if (j.is_object())
{
@@ -20,7 +20,7 @@ namespace Eigen
template <>
- void from_json<Matrix4f>(const nlohmann::json& j, MatrixBase<Matrix4f>& matrix)
+ void from_json<Eigen::Matrix4f>(const nlohmann::json& j, Eigen::MatrixBase<Eigen::Matrix4f>& matrix)
{
if (j.is_object())
{
diff --git a/VirtualRobot/Util/json/eigen_conversion.h b/VirtualRobot/Util/json/eigen_conversion.h
index 7fb64973f72cecdee9b1196525db02a000676f96..b5ef381e76914f4d9385c2a7a5ffc4e6185a4275 100644
--- a/VirtualRobot/Util/json/eigen_conversion.h
+++ b/VirtualRobot/Util/json/eigen_conversion.h
@@ -31,25 +31,25 @@ namespace Eigen
{
- // MatrixBase (non-specialized).
+ // Eigen::MatrixBase (non-specialized).
/// Writes the matrix as list of rows.
template <typename Derived>
- void to_json(nlohmann::json& j, const MatrixBase<Derived>& matrix);
+ void to_json(nlohmann::json& j, const Eigen::MatrixBase<Derived>& matrix);
/// Reads the matrix from list of rows.
template <typename Derived>
- void from_json(const nlohmann::json& j, MatrixBase<Derived>& matrix);
+ void from_json(const nlohmann::json& j, Eigen::MatrixBase<Derived>& matrix);
- // Specialization for Vector3f (implemented in .cpp)
+ // Specialization for Eigen::Vector3f (implemented in .cpp)
/// If `j` is an object, reads vector from `x, y, z` keys. Otherwise, reads it as matrix.
template <>
- void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector);
+ void from_json<Eigen::Vector3f>(const nlohmann::json& j, Eigen::MatrixBase<Eigen::Vector3f>& vector);
- // Specialization for Matrix4f as transformation matrix (implemented in .cpp).
+ // Specialization for Eigen::Matrix4f as transformation matrix (implemented in .cpp).
/**
* @brief Reads a 4x4 matrix from list of rows or `pos` and `ori` keys.
@@ -58,18 +58,18 @@ namespace Eigen
* Otherweise, reads it from list of rows.
*/
template <>
- void from_json<Matrix4f>(const nlohmann::json& j, MatrixBase<Matrix4f>& matrix);
+ void from_json<Eigen::Matrix4f>(const nlohmann::json& j, Eigen::MatrixBase<Eigen::Matrix4f>& matrix);
- // Quaternion
+ // Eigen::Quaternion
/// Writes the quaternion with `qw, qx, qy, qz` keys.
template <typename Derived>
- void to_json(nlohmann::json& j, const QuaternionBase<Derived>& quat);
+ void to_json(nlohmann::json& j, const Eigen::QuaternionBase<Derived>& quat);
/// Reads the quaternion from `qw, qx, qy, qz` keys.
template <typename Derived>
- void from_json(const nlohmann::json& j, QuaternionBase<Derived>& quat);
+ void from_json(const nlohmann::json& j, Eigen::QuaternionBase<Derived>& quat);
@@ -84,7 +84,7 @@ namespace jsonbase
/// Writes the matrix as list of rows.
template <typename Derived>
- void to_json(nlohmann::json& j, const MatrixBase<Derived>& matrix)
+ void to_json(nlohmann::json& j, const Eigen::MatrixBase<Derived>& matrix)
{
for (int row = 0; row < matrix.rows(); ++row)
{
@@ -99,10 +99,10 @@ namespace jsonbase
/// Reads the matrix from list of rows.
template <typename Derived>
- void from_json(const nlohmann::json& j, MatrixBase<Derived>& matrix)
+ void from_json(const nlohmann::json& j, Eigen::MatrixBase<Derived>& matrix)
{
- using Scalar = typename MatrixBase<Derived>::Scalar;
- using Index = typename MatrixBase<Derived>::Index;
+ using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
+ using Index = typename Eigen::MatrixBase<Derived>::Index;
for (std::size_t row = 0; row < j.size(); ++row)
{
@@ -118,20 +118,20 @@ namespace jsonbase
template <typename Derived>
- void to_json(nlohmann::json& j, const MatrixBase<Derived>& matrix)
+ void to_json(nlohmann::json& j, const Eigen::MatrixBase<Derived>& matrix)
{
jsonbase::to_json(j, matrix);
}
template <typename Derived>
- void from_json(const nlohmann::json& j, MatrixBase<Derived>& matrix)
+ void from_json(const nlohmann::json& j, Eigen::MatrixBase<Derived>& matrix)
{
jsonbase::from_json(j, matrix);
}
template <typename Derived>
- void to_json(nlohmann::json& j, const QuaternionBase<Derived>& quat)
+ void to_json(nlohmann::json& j, const Eigen::QuaternionBase<Derived>& quat)
{
j["qw"] = quat.w();
j["qx"] = quat.x();
@@ -140,9 +140,9 @@ namespace jsonbase
}
template <typename Derived>
- void from_json(const nlohmann::json& j, QuaternionBase<Derived>& quat)
+ void from_json(const nlohmann::json& j, Eigen::QuaternionBase<Derived>& quat)
{
- using Scalar = typename QuaternionBase<Derived>::Scalar;
+ using Scalar = typename Eigen::QuaternionBase<Derived>::Scalar;
quat.w() = j.at("qw").get<Scalar>();
quat.x() = j.at("qx").get<Scalar>();
quat.y() = j.at("qy").get<Scalar>();
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
index 40c8a1d559bbc7f4e52ac59e47d53491fefb747e..9efa31303859e403ae9347f8c25668030152d25e 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
@@ -26,16 +26,7 @@
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-
-namespace
-{
- namespace fs = std::filesystem;
- inline fs::path remove_trailing_separator(fs::path p)
- {
- p /= "dummy";
- return p.parent_path();
- }
-}
+#include <SimoxUtility/filesystem/remove_trailing_separator.h>
namespace VirtualRobot
@@ -446,8 +437,8 @@ namespace VirtualRobot
std::string outFile = filename;
bool vrml = true; // may be changed later according to file extension
- auto completePath = remove_trailing_separator(modelPath);
- auto fn = remove_trailing_separator(outFile);
+ auto completePath = simox::fs::remove_trailing_separator(modelPath);
+ auto fn = simox::fs::remove_trailing_separator(outFile);
if (!std::filesystem::is_directory(completePath))
{
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index d3964c04448899a2056fb5592aefc13e71b2b1ef..88da726b47dc4a1a00b571206cc0882093073466 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -5,6 +5,7 @@
* @copyright 2010 Manfred Kroehnert
*/
+#include <SimoxUtility/filesystem/remove_trailing_separator.h>
#include "VisualizationNode.h"
#include "TriMeshModel.h"
@@ -13,16 +14,6 @@
#include "VirtualRobot/VirtualRobotException.h"
#include "VirtualRobot/XML/BaseIO.h"
-namespace
-{
- namespace fs = std::filesystem;
- inline fs::path remove_trailing_separator(fs::path p)
- {
- p /= "dummy";
- return p.parent_path();
- }
-}
-
namespace VirtualRobot
{
@@ -267,7 +258,7 @@ namespace VirtualRobot
bool VisualizationNode::saveModel(const std::string& modelPath, const std::string& filename)
{
- const auto completePath = remove_trailing_separator(modelPath);
+ const auto completePath = simox::fs::remove_trailing_separator(modelPath);
if (!std::filesystem::is_directory(completePath))
{
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 6db6d8634f3252a4ab17a591e44cfaf2a9c86c3a..522b7fbb5a1575bda2a6fbe79f51659b89d9253d 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -579,7 +579,7 @@ namespace VirtualRobot
*/
void BaseIO::getLowerCase(std::string& aString)
{
- std::transform(aString.begin(), aString.end(), aString.begin(), tolower);
+ std::transform(aString.begin(), aString.end(), aString.begin(), [](unsigned char c){ return std::tolower(c); });
}
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index d932c0b16aff0cfc845e8687b140a5e6a63a1e01..1ceac861bfc65dd610d791b8ee76181e5b7a092c 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -17,21 +17,12 @@
#include "rapidxml.hpp"
#include "mujoco/RobotMjcf.h"
+#include <SimoxUtility/filesystem/remove_trailing_separator.h>
#include <vector>
#include <fstream>
#include <iostream>
-namespace
-{
- namespace fs = std::filesystem;
- inline fs::path remove_trailing_separator(fs::path p)
- {
- p /= "dummy";
- return p.parent_path();
- }
-}
-
namespace VirtualRobot
{
@@ -1477,9 +1468,9 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(!robot, "NULL data");
- std::filesystem::path p = remove_trailing_separator(basePath);
- std::filesystem::path fn = remove_trailing_separator(filename);
- std::filesystem::path pModelDir = remove_trailing_separator(modelDir);
+ std::filesystem::path p = simox::fs::remove_trailing_separator(basePath);
+ std::filesystem::path fn = simox::fs::remove_trailing_separator(filename);
+ std::filesystem::path pModelDir = simox::fs::remove_trailing_separator(modelDir);
std::filesystem::path fnComplete = p / fn;
std::filesystem::path modelDirComplete = p / pModelDir;
diff --git a/VirtualRobot/math/AbstractFunctionR1Ori.cpp b/VirtualRobot/math/AbstractFunctionR1Ori.cpp
index 31d9d8f53b61b6fa854f3c00cb2c8dc11ca94919..5b0865a4a209ab1fbe921e009a1e00f1184e1b6c 100644
--- a/VirtualRobot/math/AbstractFunctionR1Ori.cpp
+++ b/VirtualRobot/math/AbstractFunctionR1Ori.cpp
@@ -23,8 +23,6 @@
#include "AbstractFunctionR1Ori.h"
-using namespace armarx;
-
AbstractFunctionR1Ori::AbstractFunctionR1Ori()
{
}
@@ -32,4 +30,4 @@ AbstractFunctionR1Ori::AbstractFunctionR1Ori()
Eigen::Quaternionf math::AbstractFunctionR1Ori::Get(float t)
{
-}
\ No newline at end of file
+}
diff --git a/VirtualRobot/math/GaussianImplicitSurface3DCombined.h b/VirtualRobot/math/GaussianImplicitSurface3DCombined.h
index 1d3fbd60b330b92acf5c160f9424f806433b3731..e5eb3ce9383ebb1cd40c351444bcc70bb331c6fd 100644
--- a/VirtualRobot/math/GaussianImplicitSurface3DCombined.h
+++ b/VirtualRobot/math/GaussianImplicitSurface3DCombined.h
@@ -56,9 +56,7 @@ private:
float Predict(const Eigen::Vector3f& pos);
void CalculateCovariance(const std::vector<Eigen::Vector3f>& points, const std::vector<Eigen::Vector3f>& normalPoints, float R, const std::vector<float>& noise, float normalNoise);
- //static VectorXf Cholesky(MatrixXf matrix);
- //static VectorXf FitModel(MatrixXf L, List<DataR3R1> targets);
- //VectorXf SpdMatrixSolve(MatrixXf a, bool IsUpper, VectorXf b);
+
void MatrixInvert(const Eigen::VectorXd& b);
Eigen::VectorXd getCux(const Eigen::Vector3f& pos);
};
diff --git a/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp b/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp
index fff4296fd0e17fafaee97525dbcbe04886c7b19a..fcd4693a293cd43c66e720ff8b4a83e6616507ea 100644
--- a/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp
+++ b/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp
@@ -79,7 +79,7 @@ Eigen::VectorXd GaussianImplicitSurface3DNormals::getCux(const Eigen::Vector3f&
Cux(i * 4 + 2) = kernel->Kernel_dj(pos, samples.at(i).Position(), R, 1);
Cux(i * 4 + 3) = kernel->Kernel_dj(pos, samples.at(i).Position(), R, 2);
}
- return Cux;//VectorXf;
+ return Cux;//Eigen::VectorXf;
}
float GaussianImplicitSurface3DNormals::Predict(const Eigen::Vector3f& pos)
{
diff --git a/VirtualRobot/math/GaussianImplicitSurface3DNormals.h b/VirtualRobot/math/GaussianImplicitSurface3DNormals.h
index 93bf473ca9a0a1c06b5bc31a17618e139b8d2fd7..7467962da0e318438d30c23616325424502b299d 100644
--- a/VirtualRobot/math/GaussianImplicitSurface3DNormals.h
+++ b/VirtualRobot/math/GaussianImplicitSurface3DNormals.h
@@ -53,9 +53,7 @@ private:
float Predict(const Eigen::Vector3f& pos);
void CalculateCovariance(const std::vector<Eigen::Vector3f>& points, float R, float noise, float normalNoise);
- //static VectorXf Cholesky(MatrixXf matrix);
- //static VectorXf FitModel(MatrixXf L, List<DataR3R1> targets);
- //VectorXf SpdMatrixSolve(MatrixXf a, bool IsUpper, VectorXf b);
+
void MatrixInvert(const Eigen::VectorXd& b);
static Eigen::Vector3f Average(const ContactList& samples);
Eigen::VectorXd getCux(const Eigen::Vector3f& pos);
diff --git a/VirtualRobot/math/GaussianObjectModelNormals.cpp b/VirtualRobot/math/GaussianObjectModelNormals.cpp
index 2ec6e19e59c5535b2a730c3b638fd6be8317eaf3..5ba6b44d9779ac1c3ebd23ba60970ab448e80f7d 100644
--- a/VirtualRobot/math/GaussianObjectModelNormals.cpp
+++ b/VirtualRobot/math/GaussianObjectModelNormals.cpp
@@ -25,35 +25,37 @@
#include "MathForwardDefinitions.h"
#include "Kernels.h"
-using namespace math;
-
-GaussianObjectModelNormals::GaussianObjectModelNormals(float noise, float normalNoise, float normalScale)
- : noise(noise), normalNoise(normalNoise), normalScale(normalScale)
-{
- model = gpModel = GaussianImplicitSurface3DNormalsPtr(new GaussianImplicitSurface3DNormals(std::unique_ptr<WilliamsPlusKernel>(new WilliamsPlusKernel)));
-}
-void GaussianObjectModelNormals::AddContact(Contact contact)
-{
- //std::cout<<"addContact"<<std::endl;
- ImplicitObjectModel::AddContact(contact);
-}
-void GaussianObjectModelNormals::Update()
+namespace math
{
- std::cout<<"contacts"<<contacts->size()<<std::endl;
- if(contacts->size()==1){
- Contact c = contacts->at(0);
- Eigen::Vector3f dir1, dir2;
- contacts->push_back(Contact(c.Position()+Helpers::GetOrthonormalVectors(c.Normal(),dir1,dir2),c.Normal()));
+ GaussianObjectModelNormals::GaussianObjectModelNormals(float noise, float normalNoise, float normalScale)
+ : noise(noise), normalNoise(normalNoise), normalScale(normalScale)
+ {
+ model = gpModel = GaussianImplicitSurface3DNormalsPtr(new GaussianImplicitSurface3DNormals(std::unique_ptr<WilliamsPlusKernel>(new WilliamsPlusKernel)));
+ }
+ void GaussianObjectModelNormals::AddContact(Contact contact)
+ {
+ //std::cout<<"addContact"<<std::endl;
+ ImplicitObjectModel::AddContact(contact);
+ }
+ void GaussianObjectModelNormals::Update()
+ {
+ std::cout << "contacts" << contacts->size() << std::endl;
+ if (contacts->size() == 1)
+ {
+ Contact c = contacts->at(0);
+ Eigen::Vector3f dir1, dir2;
+ contacts->push_back(Contact(c.Position() + Helpers::GetOrthonormalVectors(c.Normal(), dir1, dir2), c.Normal()));
+ }
+ gpModel->Calculate(*contacts, noise, normalNoise, normalScale);
}
- gpModel->Calculate(*contacts,noise,normalNoise,normalScale);
-}
-std::vector<float> GaussianObjectModelNormals::GetContactWeights()
-{
- std::vector<float> result;
- for (size_t i = 0; i < contacts->size(); ++i) {
- result.push_back(1.f);
+ std::vector<float> GaussianObjectModelNormals::GetContactWeights()
+ {
+ std::vector<float> result;
+ for (size_t i = 0; i < contacts->size(); ++i)
+ {
+ result.push_back(1.f);
+ }
+ return result;
}
- return result;
}
-
diff --git a/VirtualRobot/math/Grid3D.cpp b/VirtualRobot/math/Grid3D.cpp
index 8b9247b52dc7e957ff9607391fd6540e6d839a9f..e9037de1319bed156bf673ac627606a1a1f02dec 100644
--- a/VirtualRobot/math/Grid3D.cpp
+++ b/VirtualRobot/math/Grid3D.cpp
@@ -23,80 +23,87 @@
#include "cmath"
#include "Helpers.h"
-using namespace math;
-
-
-Grid3D::Grid3D(Eigen::Vector3f p1, Eigen::Vector3f p2, int stepsX, int stepsY, int stepsZ)
- : p1(p1), p2(p2), stepsX(stepsX), stepsY(stepsY), stepsZ(stepsZ)
-{ }
-
-Grid3DPtr Grid3D::CreateFromBox(Eigen::Vector3f p1, Eigen::Vector3f p2, float stepLength)
+namespace math
{
- Eigen::Vector3f steps = (p2 - p1) / stepLength;
- return Grid3DPtr(new Grid3D(p1, p2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
-}
+ Grid3D::Grid3D(Eigen::Vector3f p1, Eigen::Vector3f p2, int stepsX, int stepsY, int stepsZ)
+ : p1(p1), p2(p2), stepsX(stepsX), stepsY(stepsY), stepsZ(stepsZ)
+ { }
-Grid3DPtr Grid3D::CreateFromCenterAndSize(const Eigen::Vector3f ¢er, const Eigen::Vector3f &size, float stepLength)
-{
- return CreateFromBox(center - size / 2, center + size / 2, stepLength);
-}
+ Grid3DPtr Grid3D::CreateFromBox(Eigen::Vector3f p1, Eigen::Vector3f p2, float stepLength)
+ {
+ Eigen::Vector3f steps = (p2 - p1) / stepLength;
+ return Grid3DPtr(new Grid3D(p1, p2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
+ }
-Grid3DPtr Grid3D::CreateFromCenterAndSteps(const Eigen::Vector3f ¢er, const Eigen::Vector3f &steps, float stepLength)
-{
- return Grid3DPtr(new Grid3D(center - steps * stepLength / 2, center + steps * stepLength / 2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
-}
+ Grid3DPtr Grid3D::CreateFromCenterAndSize(const Eigen::Vector3f& center, const Eigen::Vector3f& size, float stepLength)
+ {
+ return CreateFromBox(center - size / 2, center + size / 2, stepLength);
+ }
+ Grid3DPtr Grid3D::CreateFromCenterAndSteps(const Eigen::Vector3f& center, const Eigen::Vector3f& steps, float stepLength)
+ {
+ return Grid3DPtr(new Grid3D(center - steps * stepLength / 2, center + steps * stepLength / 2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
+ }
-Eigen::Vector3f Grid3D::Get(int x, int y, int z) const{
- return Eigen::Vector3f(Helpers::Lerp(p1.x(), p2.x(), 0, stepsX, x),
- Helpers::Lerp(p1.y(), p2.y(), 0, stepsY, y),
- Helpers::Lerp(p1.z(), p2.z(), 0, stepsZ, z));
-}
-Eigen::Vector3f Grid3D::Get(const Eigen::Vector3i &index) const
-{
- return Get(index.x(), index.y(), index.z());
-}
+ Eigen::Vector3f Grid3D::Get(int x, int y, int z) const
+ {
+ return Eigen::Vector3f(Helpers::Lerp(p1.x(), p2.x(), 0, stepsX, x),
+ Helpers::Lerp(p1.y(), p2.y(), 0, stepsY, y),
+ Helpers::Lerp(p1.z(), p2.z(), 0, stepsZ, z));
+ }
-std::vector<Eigen::Vector3f> Grid3D::AllGridPoints() const
-{
- std::vector<Eigen::Vector3f> points;
- for (int x = 0; x <= stepsX; x++)
+ Eigen::Vector3f Grid3D::Get(const Eigen::Vector3i& index) const
{
- for (int y = 0; y <= stepsY; y++)
+ return Get(index.x(), index.y(), index.z());
+ }
+
+ std::vector<Eigen::Vector3f> Grid3D::AllGridPoints() const
+ {
+ std::vector<Eigen::Vector3f> points;
+ for (int x = 0; x <= stepsX; x++)
{
- for (int z = 0; z <= stepsZ; z++)
+ for (int y = 0; y <= stepsY; y++)
{
- points.push_back(Get(x, y, z));
+ for (int z = 0; z <= stepsZ; z++)
+ {
+ points.push_back(Get(x, y, z));
+ }
}
}
+ return points;
}
- return points;
-}
-Eigen::Vector3i Grid3D::GetFirstIndex() const
-{
- return Eigen::Vector3i::Zero();
-}
+ Eigen::Vector3i Grid3D::GetFirstIndex() const
+ {
+ return Eigen::Vector3i::Zero();
+ }
-bool Grid3D::IncrementIndex(Eigen::Vector3i &index) const
-{
- Eigen::Vector3i steps = Steps();
- for(int i = 0; i < 3; i++)
+ bool Grid3D::IncrementIndex(Eigen::Vector3i& index) const
{
- index(i)++;
- if(index(i) <= steps(i)) return true;
- index(i) = 0;
+ Eigen::Vector3i steps = Steps();
+ for (int i = 0; i < 3; i++)
+ {
+ index(i)++;
+ if (index(i) <= steps(i))
+ {
+ return true;
+ }
+ index(i) = 0;
+ }
+ return false;
}
- return false;
-}
-bool Grid3D::IndexValid(const Eigen::Vector3i &index) const
-{
- Eigen::Vector3i steps = Steps();
- for(int i = 0; i < 3; i++)
+ bool Grid3D::IndexValid(const Eigen::Vector3i& index) const
{
- if(index(i) < 0 || index(i) > steps(i)) return false;
+ Eigen::Vector3i steps = Steps();
+ for (int i = 0; i < 3; i++)
+ {
+ if (index(i) < 0 || index(i) > steps(i))
+ {
+ return false;
+ }
+ }
+ return true;
}
- return true;
}
diff --git a/VirtualRobot/math/GridCacheFloat3.cpp b/VirtualRobot/math/GridCacheFloat3.cpp
index e7dbc3bd992c8b7d71caa738e029154ac7a8a713..c674a170cc4ada3c069f8f5909b6f1440a4843e5 100644
--- a/VirtualRobot/math/GridCacheFloat3.cpp
+++ b/VirtualRobot/math/GridCacheFloat3.cpp
@@ -22,42 +22,39 @@
#include "GridCacheFloat3.h"
#include "Array3D.h"
-using namespace math;
-
-
-
-GridCacheFloat3::GridCacheFloat3(int size, std::function<float (Index3)>& getData)
+namespace math
{
- this->size = size;
- this->getData = getData;
- data = Array3DFloatPtr(new Array3D<float>(size));
- valid = Array3DBoolPtr(new Array3D<bool>(size));
-
- for (int x = 0; x < size; x++)
+ GridCacheFloat3::GridCacheFloat3(int size, std::function<float (Index3)>& getData)
{
- for (int y = 0; y < size; y++)
+ this->size = size;
+ this->getData = getData;
+ data = Array3DFloatPtr(new Array3D<float>(size));
+ valid = Array3DBoolPtr(new Array3D<bool>(size));
+
+ for (int x = 0; x < size; x++)
{
- for (int z = 0; z < size; z++)
+ for (int y = 0; y < size; y++)
{
- valid->Set(x,y,z, false);
+ for (int z = 0; z < size; z++)
+ {
+ valid->Set(x, y, z, false);
+ }
}
}
}
-}
-
-
-float GridCacheFloat3::Get(int x, int y, int z)
-{
- if (valid->Get(x,y,z))
- {
- return data->Get(x,y,z);
- }
- else
+ float GridCacheFloat3::Get(int x, int y, int z)
{
- float val = getData(Index3(x, y, z));
- data->Set(x,y,z,val);
- valid->Set(x,y,z, true);
- return val;
+ if (valid->Get(x, y, z))
+ {
+ return data->Get(x, y, z);
+ }
+ else
+ {
+ float val = getData(Index3(x, y, z));
+ data->Set(x, y, z, val);
+ valid->Set(x, y, z, true);
+ return val;
+ }
}
}
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 41c3f09163ee4e35684fc6bc7eea374ec72ee3d6..2995ec60b8cd733c313e467f19779c8c6f04aab6 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -27,476 +27,507 @@
#include <stdexcept>
-using namespace math;
-
-
+namespace math
+{
#define M_PI_F (static_cast<float>(M_PI))
-Eigen::Vector3f math::Helpers::GetOrthonormalVectors(
- Eigen::Vector3f vec, Eigen::Vector3f &dir1, Eigen::Vector3f &dir2)
-{
- vec = vec.normalized();
- dir1 = vec.cross(Eigen::Vector3f::UnitZ());
- if (dir1.norm() < 0.1f)
+ Eigen::Vector3f math::Helpers::GetOrthonormalVectors(
+ Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2)
{
- dir1 = vec.cross(Eigen::Vector3f::UnitY());
+ vec = vec.normalized();
+ dir1 = vec.cross(Eigen::Vector3f::UnitZ());
+ if (dir1.norm() < 0.1f)
+ {
+ dir1 = vec.cross(Eigen::Vector3f::UnitY());
+ }
+ dir1 = -dir1.normalized();
+ dir2 = vec.cross(dir1).normalized();
+ return vec;
}
- dir1 = -dir1.normalized();
- dir2 = vec.cross(dir1).normalized();
- return vec;
-}
-float Helpers::ShiftAngle0_2PI(float a)
-{
- while (a < 0) a += 2*M_PI;
- while (a > 2 * M_PI_F)
- a -= 2*M_PI;
- return a;
-}
+ float Helpers::ShiftAngle0_2PI(float a)
+ {
+ while (a < 0)
+ {
+ a += 2 * M_PI;
+ }
+ while (a > 2 * M_PI_F)
+ {
+ a -= 2 * M_PI;
+ }
+ return a;
+ }
-float Helpers::AngleModPI(float value)
-{
- return ShiftAngle0_2PI(value + M_PI_F) - M_PI_F;
-}
+ float Helpers::AngleModPI(float value)
+ {
+ return ShiftAngle0_2PI(value + M_PI_F) - M_PI_F;
+ }
-void Helpers::GetIndex(float t, float minT, float maxT, int count, int &i, float &f)
-{
- if (minT == maxT)
+ void Helpers::GetIndex(float t, float minT, float maxT, int count, int& i, float& f)
{
- f = i = 0;
- return;
+ if (minT == maxT)
+ {
+ f = i = 0;
+ return;
+ }
+ f = ((t - minT) / (maxT - minT)) * (count - 1);
+ i = std::max(0, std::min(count - 2, static_cast<int>(f)));
+ f -= i;
}
- f = ((t - minT) / (maxT - minT)) * (count - 1);
- i = std::max(0, std::min(count - 2, static_cast<int>(f)));
- f -= i;
-}
-float Helpers::Clamp(float min, float max, float value)
-{
- if (value < min) return min;
- if (value > max) return max;
- return value;
-}
+ float Helpers::Clamp(float min, float max, float value)
+ {
+ if (value < min)
+ {
+ return min;
+ }
+ if (value > max)
+ {
+ return max;
+ }
+ return value;
+ }
-int Helpers::Clampi(int min, int max, int value)
-{
- if (value < min) return min;
- if (value > max) return max;
- return value;
-}
+ int Helpers::Clampi(int min, int max, int value)
+ {
+ if (value < min)
+ {
+ return min;
+ }
+ if (value > max)
+ {
+ return max;
+ }
+ return value;
+ }
-float Helpers::Lerp(float a, float b, float f)
-{
- return a * (1 - f) + b * f;
-}
+ float Helpers::Lerp(float a, float b, float f)
+ {
+ return a * (1 - f) + b * f;
+ }
-Eigen::Vector3f Helpers::Lerp(const Eigen::Vector3f &a, const Eigen::Vector3f &b, float f)
-{
- return Eigen::Vector3f(Lerp(a(0), b(0), f),
- Lerp(a(1), b(1), f),
- Lerp(a(2), b(2), f));
-}
+ Eigen::Vector3f Helpers::Lerp(const Eigen::Vector3f& a, const Eigen::Vector3f& b, float f)
+ {
+ return Eigen::Vector3f(Lerp(a(0), b(0), f),
+ Lerp(a(1), b(1), f),
+ Lerp(a(2), b(2), f));
+ }
-Eigen::Quaternionf Helpers::Lerp(const Eigen::Quaternionf &a, const Eigen::Quaternionf &b, float f)
-{
- return LinearInterpolatedOrientation(a, b, 0, 1, false).Get(f);
-}
+ Eigen::Quaternionf Helpers::Lerp(const Eigen::Quaternionf& a, const Eigen::Quaternionf& b, float f)
+ {
+ return LinearInterpolatedOrientation(a, b, 0, 1, false).Get(f);
+ }
-Eigen::Quaternionf Helpers::LerpClamp(const Eigen::Quaternionf& a, const Eigen::Quaternionf& b, float f)
-{
- return LinearInterpolatedOrientation(a, b, 0, 1, true).Get(f);
-}
+ Eigen::Quaternionf Helpers::LerpClamp(const Eigen::Quaternionf& a, const Eigen::Quaternionf& b, float f)
+ {
+ return LinearInterpolatedOrientation(a, b, 0, 1, true).Get(f);
+ }
-float Helpers::ILerp(float a, float b, float f)
-{
- return (f - a) / (b - a);
-}
+ float Helpers::ILerp(float a, float b, float f)
+ {
+ return (f - a) / (b - a);
+ }
-float Helpers::Lerp(float a, float b, int min, int max, int val)
-{
- if (min == max) return a; //TODO
- return Lerp(a, b, (val - min) / static_cast<float>(max - min));
-}
+ float Helpers::Lerp(float a, float b, int min, int max, int val)
+ {
+ if (min == max)
+ {
+ return a; //TODO
+ }
+ return Lerp(a, b, (val - min) / static_cast<float>(max - min));
+ }
-float Helpers::Angle(Eigen::Vector2f v)
-{
- return static_cast<float>(std::atan2(v.y(), v.x()));
-}
+ float Helpers::Angle(Eigen::Vector2f v)
+ {
+ return static_cast<float>(std::atan2(v.y(), v.x()));
+ }
-float Helpers::Angle(const Eigen::Vector3f &a, const Eigen::Vector3f &b, const Eigen::Vector3f &up)
-{
- Eigen::Vector3f side = a.cross(up);
- float sign = Sign(b.dot(side));
- return (float)std::atan2(a.cross(b).norm() * sign, a.dot(b));
-}
+ float Helpers::Angle(const Eigen::Vector3f& a, const Eigen::Vector3f& b, const Eigen::Vector3f& up)
+ {
+ Eigen::Vector3f side = a.cross(up);
+ float sign = Sign(b.dot(side));
+ return (float)std::atan2(a.cross(b).norm() * sign, a.dot(b));
+ }
-int Helpers::Sign(float x)
-{
- return x > 0 ? 1 : (x == 0 ? 0 : -1);
-}
+ int Helpers::Sign(float x)
+ {
+ return x > 0 ? 1 : (x == 0 ? 0 : -1);
+ }
-void Helpers::AssertNormalized(Eigen::Vector3f vec, float epsilon)
-{
- float len = vec.squaredNorm();
- if (len < 1 - epsilon || len > 1 + epsilon) throw std::runtime_error("Vector is not normalized");
-}
+ void Helpers::AssertNormalized(Eigen::Vector3f vec, float epsilon)
+ {
+ float len = vec.squaredNorm();
+ if (len < 1 - epsilon || len > 1 + epsilon)
+ {
+ throw std::runtime_error("Vector is not normalized");
+ }
+ }
-std::vector<float> Helpers::FloatRange(float start, float end, int steps)
-{
- std::vector<float> result;
- for (int i = 0; i < steps+1; ++i) {
- result.push_back(Lerp(start, end, i/static_cast<float>(steps)));
+ std::vector<float> Helpers::FloatRange(float start, float end, int steps)
+ {
+ std::vector<float> result;
+ for (int i = 0; i < steps + 1; ++i)
+ {
+ result.push_back(Lerp(start, end, i / static_cast<float>(steps)));
+ }
+ return result;
}
- return result;
-}
-std::vector<Eigen::Vector3f> Helpers::VectorRangeSymmetric(float start, float end, int steps)
-{
- std::vector<float> vals = FloatRange(start, end, steps);
- return VectorRange(vals, vals, vals);
-}
+ std::vector<Eigen::Vector3f> Helpers::VectorRangeSymmetric(float start, float end, int steps)
+ {
+ std::vector<float> vals = FloatRange(start, end, steps);
+ return VectorRange(vals, vals, vals);
+ }
-std::vector<Eigen::Vector3f> Helpers::VectorRange(std::vector<float> xvals, std::vector<float> yvals, std::vector<float> zvals)
-{
- std::vector<Eigen::Vector3f> result;
- for (float x : xvals)
+ std::vector<Eigen::Vector3f> Helpers::VectorRange(std::vector<float> xvals, std::vector<float> yvals, std::vector<float> zvals)
{
- for (float y : yvals)
+ std::vector<Eigen::Vector3f> result;
+ for (float x : xvals)
{
- for (float z : zvals)
+ for (float y : yvals)
{
- result.push_back(Eigen::Vector3f(x, y, z));
+ for (float z : zvals)
+ {
+ result.push_back(Eigen::Vector3f(x, y, z));
+ }
}
}
+ return result;
}
- return result;
-}
-float Helpers::SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b)
-{
- return static_cast<float>(std::atan2(a.cross(b).norm(), a.dot(b)));
-}
+ float Helpers::SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b)
+ {
+ return static_cast<float>(std::atan2(a.cross(b).norm(), a.dot(b)));
+ }
-Eigen::Vector3f Helpers::CwiseMin(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
-{
- return a.cwiseMin(b);
-}
+ Eigen::Vector3f Helpers::CwiseMin(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a.cwiseMin(b);
+ }
-Eigen::Vector3f Helpers::CwiseMax(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
-{
- return a.cwiseMax(b);
-}
+ Eigen::Vector3f Helpers::CwiseMax(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a.cwiseMax(b);
+ }
-Eigen::Vector3f Helpers::CwiseDivide(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
-{
- return a.cwiseQuotient(b);
-}
+ Eigen::Vector3f Helpers::CwiseDivide(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a.cwiseQuotient(b);
+ }
-Eigen::Vector3f Helpers::CwiseClamp(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& val)
-{
- return Eigen::Vector3f(
- Clamp(min(0), max(0), val(0)),
- Clamp(min(1), max(1), val(1)),
- Clamp(min(2), max(2), val(2))
- );
-}
+ Eigen::Vector3f Helpers::CwiseClamp(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& val)
+ {
+ return Eigen::Vector3f(
+ Clamp(min(0), max(0), val(0)),
+ Clamp(min(1), max(1), val(1)),
+ Clamp(min(2), max(2), val(2))
+ );
+ }
-Eigen::Vector3f Helpers::CwiseClamp(float min, float max, const Eigen::Vector3f& val)
-{
- return Eigen::Vector3f(
- Clamp(min, max, val(0)),
- Clamp(min, max, val(1)),
- Clamp(min, max, val(2))
- );
-}
+ Eigen::Vector3f Helpers::CwiseClamp(float min, float max, const Eigen::Vector3f& val)
+ {
+ return Eigen::Vector3f(
+ Clamp(min, max, val(0)),
+ Clamp(min, max, val(1)),
+ Clamp(min, max, val(2))
+ );
+ }
-Eigen::Vector3f Helpers::Average(const std::vector<Eigen::Vector3f>& vectors)
-{
- Eigen::Vector3f avg = Eigen::Vector3f::Zero();
- if (vectors.size() == 0) return avg;
- for (const Eigen::Vector3f& v : vectors)
+ Eigen::Vector3f Helpers::Average(const std::vector<Eigen::Vector3f>& vectors)
{
- avg += v;
+ Eigen::Vector3f avg = Eigen::Vector3f::Zero();
+ if (vectors.size() == 0)
+ {
+ return avg;
+ }
+ for (const Eigen::Vector3f& v : vectors)
+ {
+ avg += v;
+ }
+ avg /= vectors.size();
+ return avg;
}
- avg /= vectors.size();
- return avg;
-}
-void Helpers::Swap(float& a,float& b)
-{
- std::swap(a, b);
-}
+ void Helpers::Swap(float& a, float& b)
+ {
+ std::swap(a, b);
+ }
-Eigen::Matrix4f Helpers::CreateTranslationPose(const Eigen::Vector3f &pos)
-{
- return Pose(pos, Eigen::Matrix3f::Identity());
-}
+ Eigen::Matrix4f Helpers::CreateTranslationPose(const Eigen::Vector3f& pos)
+ {
+ return Pose(pos, Eigen::Matrix3f::Identity());
+ }
-Eigen::Matrix4f Helpers::CreateRotationPose(const Eigen::Matrix3f &ori)
-{
- return Pose(Eigen::Vector3f::Zero(), ori);
-}
+ Eigen::Matrix4f Helpers::CreateRotationPose(const Eigen::Matrix3f& ori)
+ {
+ return Pose(Eigen::Vector3f::Zero(), ori);
+ }
-Eigen::Matrix4f Helpers::CreateTranslationRotationTranslationPose(const Eigen::Vector3f &translation1, const Eigen::Matrix3f &rotation, const Eigen::Vector3f &translation2)
-{
- return CreateTranslationPose(translation2) * CreateRotationPose(rotation) * CreateTranslationPose(translation1);
-}
+ Eigen::Matrix4f Helpers::CreateTranslationRotationTranslationPose(const Eigen::Vector3f& translation1, const Eigen::Matrix3f& rotation, const Eigen::Vector3f& translation2)
+ {
+ return CreateTranslationPose(translation2) * CreateRotationPose(rotation) * CreateTranslationPose(translation1);
+ }
-Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori)
-{
- return Pose(pos, ori);
-}
+ Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori)
+ {
+ return Pose(pos, ori);
+ }
-Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Matrix3f& ori)
-{
- return Pose(pos, ori);
-}
+ Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Matrix3f& ori)
+ {
+ return Pose(pos, ori);
+ }
-Eigen::Matrix3f Helpers::CreateOrientation(const Eigen::Vector3f &e1, const Eigen::Vector3f &e2, const Eigen::Vector3f &e3)
-{
- Eigen::Matrix3f ori;
- ori.block<3,1>(0,0) = e1;
- ori.block<3,1>(0,1) = e2;
- ori.block<3,1>(0,2) = e3;
- return ori;
-}
+ Eigen::Matrix3f Helpers::CreateOrientation(const Eigen::Vector3f& e1, const Eigen::Vector3f& e2, const Eigen::Vector3f& e3)
+ {
+ Eigen::Matrix3f ori;
+ ori.block<3, 1>(0, 0) = e1;
+ ori.block<3, 1>(0, 1) = e2;
+ ori.block<3, 1>(0, 2) = e3;
+ return ori;
+ }
-Eigen::Vector3f Helpers::GetPosition(const Eigen::Matrix4f& pose)
-{
- return Position(pose);
-}
+ Eigen::Vector3f Helpers::GetPosition(const Eigen::Matrix4f& pose)
+ {
+ return Position(pose);
+ }
-Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose)
-{
- return Orientation(pose);
-}
+ Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose)
+ {
+ return Orientation(pose);
+ }
-Eigen::Matrix4f math::Helpers::TranslatePose(const Eigen::Matrix4f& pose, const Eigen::Vector3f& offset)
-{
- Eigen::Matrix4f p = pose;
- Position(p) += offset;
- return p;
-}
+ Eigen::Matrix4f math::Helpers::TranslatePose(const Eigen::Matrix4f& pose, const Eigen::Vector3f& offset)
+ {
+ Eigen::Matrix4f p = pose;
+ Position(p) += offset;
+ return p;
+ }
-Eigen::Matrix4f Helpers::TranslateAndRotatePose(const Eigen::Matrix4f &pose, const Eigen::Vector3f &offset, const Eigen::Matrix3f &rotation)
-{
- Eigen::Matrix4f p = pose;
- Position(p) += offset;
- p = CreatePose(Eigen::Vector3f::Zero(), rotation) * p;
- return p;
-}
+ Eigen::Matrix4f Helpers::TranslateAndRotatePose(const Eigen::Matrix4f& pose, const Eigen::Vector3f& offset, const Eigen::Matrix3f& rotation)
+ {
+ Eigen::Matrix4f p = pose;
+ Position(p) += offset;
+ p = CreatePose(Eigen::Vector3f::Zero(), rotation) * p;
+ return p;
+ }
-void Helpers::InvertPose(Eigen::Matrix4f& pose)
-{
- Orientation(pose).transposeInPlace();
- Position(pose) = - Orientation(pose) * Position(pose);
-}
+ void Helpers::InvertPose(Eigen::Matrix4f& pose)
+ {
+ Orientation(pose).transposeInPlace();
+ Position(pose) = - Orientation(pose) * Position(pose);
+ }
-void Helpers::ScaleTranslation(Eigen::Matrix4f& pose, float scale)
-{
- Position(pose) *= scale;
-}
+ void Helpers::ScaleTranslation(Eigen::Matrix4f& pose, float scale)
+ {
+ Position(pose) *= scale;
+ }
-Eigen::Matrix4f Helpers::ScaledTranslation(const Eigen::Matrix4f& pose, float scale)
-{
- Eigen::Matrix4f scaled = pose;
- ScaleTranslation(scaled, scale);
- return scaled;
-}
+ Eigen::Matrix4f Helpers::ScaledTranslation(const Eigen::Matrix4f& pose, float scale)
+ {
+ Eigen::Matrix4f scaled = pose;
+ ScaleTranslation(scaled, scale);
+ return scaled;
+ }
-Eigen::Matrix3f Helpers::GetRotationMatrix(const Eigen::Vector3f& source, const Eigen::Vector3f& target)
-{
- // no normalization needed
- return Eigen::Quaternionf::FromTwoVectors(source, target).toRotationMatrix();
-}
+ Eigen::Matrix3f Helpers::GetRotationMatrix(const Eigen::Vector3f& source, const Eigen::Vector3f& target)
+ {
+ // no normalization needed
+ return Eigen::Quaternionf::FromTwoVectors(source, target).toRotationMatrix();
+ }
-Eigen::Vector3f Helpers::CreateVectorFromCylinderCoords(float r, float angle, float z)
-{
- return CartesianFromCylinder(r, angle, z);
-}
+ Eigen::Vector3f Helpers::CreateVectorFromCylinderCoords(float r, float angle, float z)
+ {
+ return CartesianFromCylinder(r, angle, z);
+ }
-Eigen::Vector3f Helpers::CartesianFromCylinder(float radius, float angle, float height)
-{
- return { radius * std::cos(angle), radius * std::sin(angle), height };
-}
+ Eigen::Vector3f Helpers::CartesianFromCylinder(float radius, float angle, float height)
+ {
+ return { radius* std::cos(angle), radius* std::sin(angle), height };
+ }
-Eigen::Vector3f Helpers::CartesianFromSphere(float radius, float elevation, float azimuth)
-{
- const float sinElevation = std::sin(elevation);
- return { radius * sinElevation * std::cos(azimuth),
- radius * sinElevation * std::sin(azimuth),
- radius * std::cos(elevation) };
-}
+ Eigen::Vector3f Helpers::CartesianFromSphere(float radius, float elevation, float azimuth)
+ {
+ const float sinElevation = std::sin(elevation);
+ return { radius* sinElevation* std::cos(azimuth),
+ radius* sinElevation* std::sin(azimuth),
+ radius* std::cos(elevation) };
+ }
-Eigen::Matrix3f Helpers::RotateOrientationToFitVector(
+ Eigen::Matrix3f Helpers::RotateOrientationToFitVector(
const Eigen::Matrix3f& ori, const Eigen::Vector3f& localSource,
const Eigen::Vector3f& globalTarget)
-{
- Eigen::Vector3f vec = ori * localSource;
- return GetRotationMatrix(vec, globalTarget) * ori;
-}
-
-Eigen::Matrix4f Helpers::GetTransformFromTo(const Eigen::Matrix4f& sourceFramePose, const Eigen::Matrix4f& targetFramePose)
-{
- return InvertedPose(targetFramePose) * sourceFramePose;
-}
-
-
-Eigen::Vector3f Helpers::TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f& pos)
-{
- return (transform * pos.homogeneous()).head<3>();
-}
+ {
+ Eigen::Vector3f vec = ori * localSource;
+ return GetRotationMatrix(vec, globalTarget) * ori;
+ }
-Eigen::Vector3f Helpers::TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f& dir)
-{
- return Orientation(transform) * dir;
-}
+ Eigen::Matrix4f Helpers::GetTransformFromTo(const Eigen::Matrix4f& sourceFramePose, const Eigen::Matrix4f& targetFramePose)
+ {
+ return InvertedPose(targetFramePose) * sourceFramePose;
+ }
-Eigen::Matrix3f Helpers::TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f& ori)
-{
- return Orientation(transform) * ori;
-}
-Eigen::Matrix3f Helpers::Orthogonalize(const Eigen::Matrix3f& matrix)
-{
- return OrthogonalizeSVD(matrix);
-}
+ Eigen::Vector3f Helpers::TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f& pos)
+ {
+ return (transform * pos.homogeneous()).head<3>();
+ }
-Eigen::Matrix3f Helpers::OrthogonalizeSVD(const Eigen::Matrix3f& matrix)
-{
- auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Eigen::Vector3f Helpers::TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f& dir)
+ {
+ return Orientation(transform) * dir;
+ }
- Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV().transpose();
- if (orth.determinant() >= 0)
- return orth;
- else
- return -orth;
-}
+ Eigen::Matrix3f Helpers::TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f& ori)
+ {
+ return Orientation(transform) * ori;
+ }
-Eigen::Matrix3f Helpers::OrthogonalizeQR(const Eigen::Matrix3f& matrix)
-{
- auto householder = matrix.householderQr();
- Eigen::Matrix3f orth = householder.householderQ();
+ Eigen::Matrix3f Helpers::Orthogonalize(const Eigen::Matrix3f& matrix)
+ {
+ return OrthogonalizeSVD(matrix);
+ }
- // Upper right triangular matrix of matrixQR() is R matrix.
- // If a diagonal entry of R is negative, the corresponding column
- // in Q must be inverted.
- for (int i = 0; i < matrix.cols(); ++i)
+ Eigen::Matrix3f Helpers::OrthogonalizeSVD(const Eigen::Matrix3f& matrix)
{
- if (householder.matrixQR().diagonal()(i) < 0)
+ auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+ Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV().transpose();
+ if (orth.determinant() >= 0)
+ {
+ return orth;
+ }
+ else
{
- orth.col(i) *= -1;
+ return -orth;
}
}
- return orth;
-}
-Eigen::Matrix4f Helpers::Orthogonalize(const Eigen::Matrix4f& pose)
-{
- Eigen::Matrix4f orth = pose;
- Orientation(orth) = Orthogonalize(Orientation(orth).eval());
- orth.row(3) << 0, 0, 0, 1;
- return orth;
-}
+ Eigen::Matrix3f Helpers::OrthogonalizeQR(const Eigen::Matrix3f& matrix)
+ {
+ auto householder = matrix.householderQr();
+ Eigen::Matrix3f orth = householder.householderQ();
-float Helpers::Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor)
-{
- Eigen::AngleAxisf aa(Orientation(b) * Orientation(a).inverse());
- float dist = (Position(a) - Position(b)).norm();
- return dist + AngleModPI(aa.angle()) * rad2mmFactor;
-}
+ // Upper right triangular matrix of matrixQR() is R matrix.
+ // If a diagonal entry of R is negative, the corresponding column
+ // in Q must be inverted.
+ for (int i = 0; i < matrix.cols(); ++i)
+ {
+ if (householder.matrixQR().diagonal()(i) < 0)
+ {
+ orth.col(i) *= -1;
+ }
+ }
+ return orth;
+ }
+ Eigen::Matrix4f Helpers::Orthogonalize(const Eigen::Matrix4f& pose)
+ {
+ Eigen::Matrix4f orth = pose;
+ Orientation(orth) = Orthogonalize(Orientation(orth).eval());
+ orth.row(3) << 0, 0, 0, 1;
+ return orth;
+ }
-Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen)
-{
- if(maxLen.rows() != 1 && maxLen.rows() != vec.rows())
+ float Helpers::Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor)
{
- throw std::invalid_argument("maxLen.rows != 1 and != maxLen.rows");
+ Eigen::AngleAxisf aa(Orientation(b) * Orientation(a).inverse());
+ float dist = (Position(a) - Position(b)).norm();
+ return dist + AngleModPI(aa.angle()) * rad2mmFactor;
}
- float scale = 1;
- for(int i = 0; i < vec.rows(); i++)
+
+
+ Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen)
{
- int j = maxLen.rows() == 1 ? 0 : i;
- if(std::abs(vec(i)) > maxLen(j) && maxLen(j) >= 0)
+ if (maxLen.rows() != 1 && maxLen.rows() != vec.rows())
+ {
+ throw std::invalid_argument("maxLen.rows != 1 and != maxLen.rows");
+ }
+ float scale = 1;
+ for (int i = 0; i < vec.rows(); i++)
{
- scale = std::min(scale, maxLen(j) / std::abs(vec(i)));
+ int j = maxLen.rows() == 1 ? 0 : i;
+ if (std::abs(vec(i)) > maxLen(j) && maxLen(j) >= 0)
+ {
+ scale = std::min(scale, maxLen(j) / std::abs(vec(i)));
+ }
}
+ return vec / scale;
}
- return vec / scale;
-}
-Eigen::AngleAxisf Helpers::GetAngleAxisFromTo(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
-{
- return Eigen::AngleAxisf(target * start.inverse());
-}
+ Eigen::AngleAxisf Helpers::GetAngleAxisFromTo(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
+ {
+ return Eigen::AngleAxisf(target * start.inverse());
+ }
-Eigen::Vector3f Helpers::GetRotationVector(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
-{
- Eigen::AngleAxisf aa = GetAngleAxisFromTo(start, target);
- return aa.axis() * aa.angle();
-}
+ Eigen::Vector3f Helpers::GetRotationVector(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
+ {
+ Eigen::AngleAxisf aa = GetAngleAxisFromTo(start, target);
+ return aa.axis() * aa.angle();
+ }
-Eigen::Matrix3f Helpers::RotationVectorToOrientation(const Eigen::Vector3f& rotation)
-{
- if(rotation.squaredNorm() == 0)
+ Eigen::Matrix3f Helpers::RotationVectorToOrientation(const Eigen::Vector3f& rotation)
{
- return Eigen::Matrix3f::Identity();
+ if (rotation.squaredNorm() == 0)
+ {
+ return Eigen::Matrix3f::Identity();
+ }
+ Eigen::AngleAxisf aa(rotation.norm(), rotation.normalized());
+ return aa.toRotationMatrix();
}
- Eigen::AngleAxisf aa(rotation.norm(), rotation.normalized());
- return aa.toRotationMatrix();
-}
-float Helpers::ScalarProjection(const Eigen::Vector3f &a, const Eigen::Vector3f &b)
-{
- return a.dot(b) / b.norm();
-}
+ float Helpers::ScalarProjection(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a.dot(b) / b.norm();
+ }
-Eigen::Vector3f Helpers::VectorProjection(const Eigen::Vector3f &a, const Eigen::Vector3f &b)
-{
- return a.dot(b) / b.dot(b) * b;
-}
+ Eigen::Vector3f Helpers::VectorProjection(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a.dot(b) / b.dot(b) * b;
+ }
-Eigen::Vector3f Helpers::VectorRejection(const Eigen::Vector3f &a, const Eigen::Vector3f &b)
-{
- return a - VectorProjection(a, b);
-}
+ Eigen::Vector3f Helpers::VectorRejection(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
+ {
+ return a - VectorProjection(a, b);
+ }
-float Helpers::rad2deg(float rad)
-{
- return rad * (180.0f / M_PI_F);
-}
+ float Helpers::rad2deg(float rad)
+ {
+ return rad * (180.0f / M_PI_F);
+ }
-float Helpers::deg2rad(float deg)
-{
- return deg * (M_PI_F / 180.0f);
-}
+ float Helpers::deg2rad(float deg)
+ {
+ return deg * (M_PI_F / 180.0f);
+ }
-std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const std::vector<Eigen::Vector3f> &positions, const Eigen::Matrix3f &orientation)
-{
- std::vector<Eigen::Matrix3f> orientations;
- orientations.push_back(orientation);
- return CreatePoses(positions, orientations);
-}
+ std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const std::vector<Eigen::Vector3f>& positions, const Eigen::Matrix3f& orientation)
+ {
+ std::vector<Eigen::Matrix3f> orientations;
+ orientations.push_back(orientation);
+ return CreatePoses(positions, orientations);
+ }
-std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const Eigen::Vector3f &position, const std::vector<Eigen::Matrix3f> &orientations)
-{
- std::vector<Eigen::Vector3f> positions;
- positions.push_back(position);
- return CreatePoses(positions, orientations);
-}
+ std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const Eigen::Vector3f& position, const std::vector<Eigen::Matrix3f>& orientations)
+ {
+ std::vector<Eigen::Vector3f> positions;
+ positions.push_back(position);
+ return CreatePoses(positions, orientations);
+ }
-std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const std::vector<Eigen::Vector3f> &positions, const std::vector<Eigen::Matrix3f> &orientations)
-{
- std::vector<Eigen::Matrix4f> poses;
- for(const Eigen::Vector3f& pos : positions)
+ std::vector<Eigen::Matrix4f> Helpers::CreatePoses(const std::vector<Eigen::Vector3f>& positions, const std::vector<Eigen::Matrix3f>& orientations)
{
- for(const Eigen::Matrix3f& ori : orientations)
+ std::vector<Eigen::Matrix4f> poses;
+ for (const Eigen::Vector3f& pos : positions)
{
- poses.emplace_back(CreatePose(pos, ori));
+ for (const Eigen::Matrix3f& ori : orientations)
+ {
+ poses.emplace_back(CreatePose(pos, ori));
+ }
}
+ return poses;
}
- return poses;
}
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index b9d83d681262c4ceccffd775a8871918f48a0c09..9cd7e58abc6138aabc336b932604d874feb6fbb5 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -385,7 +385,7 @@ namespace math
namespace Eigen
{
template <typename Derived>
- std::ostream& operator<< (std::ostream& os, const QuaternionBase<Derived>& quat)
+ std::ostream& operator<< (std::ostream& os, const Eigen::QuaternionBase<Derived>& quat)
{
os << "[ " << quat.w() << " | "
<< quat.x() << " " << quat.y() << " " << quat.z() << " ]";
diff --git a/VirtualRobot/math/ImplicitObjectModel.cpp b/VirtualRobot/math/ImplicitObjectModel.cpp
index 632658fc38c70a75bb99b33428c47fd212e5575e..f88c39f701e027bacad9f8d664d9486807a72a88 100644
--- a/VirtualRobot/math/ImplicitObjectModel.cpp
+++ b/VirtualRobot/math/ImplicitObjectModel.cpp
@@ -21,25 +21,22 @@
#include "ImplicitObjectModel.h"
-using namespace math;
-
-
+ namespace math
+{
ImplicitObjectModel::ImplicitObjectModel()
{
contacts = ContactListPtr(new ContactList());
}
-
void ImplicitObjectModel::AddContact(Contact contact)
{
contacts->push_back(contact);
Update();
}
-
void ImplicitObjectModel::Clear()
{
contacts->clear();
//Update();
}
-
+ }
diff --git a/VirtualRobot/math/ImplicitPlane.cpp b/VirtualRobot/math/ImplicitPlane.cpp
index 34890dfd934f27a0ad9a4c2f8c3c7a58a4d9deaa..ea5ad103886dfa839a9b97043b7c7120772ded79 100644
--- a/VirtualRobot/math/ImplicitPlane.cpp
+++ b/VirtualRobot/math/ImplicitPlane.cpp
@@ -22,57 +22,56 @@
#include "ImplicitPlane.h"
#include "Contact.h"
#include "Plane.h"
-using namespace math;
-
-
-ImplicitPlane::ImplicitPlane(float a, float b, float c, float d)
- : a(a), b(b), c(c), d(d)
-{
-}
-
-ImplicitPlane ImplicitPlane::Normalize()
-{
- float len = GetNormal().norm();
- return ImplicitPlane(a / len, b / len, c / len, d / len);
-}
-
-ImplicitPlane ImplicitPlane::Flipped()
-{
- return ImplicitPlane(-a, -b, -c, -d);
-}
-
-Eigen::Vector3f ImplicitPlane::GetNormal()
-{
- return Eigen::Vector3f(a, b, c);
-}
-
-Eigen::Vector3f ImplicitPlane::GetClosestPoint(Eigen::Vector3f v)
-{
- Eigen::Vector3f normal = GetNormal();
- float d = this->d - v.dot(normal);
- float denominator = a * a + b * b + c * c;
- return normal * d / denominator + v;
-
-}
-
-ImplicitPlane ImplicitPlane::FromPositionNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
-{
- return ImplicitPlane(normal.x(), normal.y(), normal.z(), normal.dot(pos));
-
-}
-
-ImplicitPlane ImplicitPlane::FromContact(Contact c)
-{
- return FromPositionNormal(c.Position(), c.Normal());
-}
-
-float ImplicitPlane::GetSignedDistance(const Eigen::Vector3f &p)
-{
- return GetNormal().dot(p) + d;
-}
-
-Line ImplicitPlane::Intersect(Plane plane)
+namespace math
{
+ ImplicitPlane::ImplicitPlane(float a, float b, float c, float d)
+ : a(a), b(b), c(c), d(d)
+ {
+ }
+
+ ImplicitPlane ImplicitPlane::Normalize()
+ {
+ float len = GetNormal().norm();
+ return ImplicitPlane(a / len, b / len, c / len, d / len);
+ }
+
+ ImplicitPlane ImplicitPlane::Flipped()
+ {
+ return ImplicitPlane(-a, -b, -c, -d);
+ }
+
+ Eigen::Vector3f ImplicitPlane::GetNormal()
+ {
+ return Eigen::Vector3f(a, b, c);
+ }
+
+ Eigen::Vector3f ImplicitPlane::GetClosestPoint(Eigen::Vector3f v)
+ {
+ Eigen::Vector3f normal = GetNormal();
+ float d = this->d - v.dot(normal);
+ float denominator = a * a + b * b + c * c;
+ return normal * d / denominator + v;
+
+ }
+
+ ImplicitPlane ImplicitPlane::FromPositionNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
+ {
+ return ImplicitPlane(normal.x(), normal.y(), normal.z(), normal.dot(pos));
+
+ }
+
+ ImplicitPlane ImplicitPlane::FromContact(Contact c)
+ {
+ return FromPositionNormal(c.Position(), c.Normal());
+ }
+
+ float ImplicitPlane::GetSignedDistance(const Eigen::Vector3f& p)
+ {
+ return GetNormal().dot(p) + d;
+ }
+
+ Line ImplicitPlane::Intersect(Plane plane)
+ {
Eigen::Vector3f n = GetNormal();
Eigen::Vector3f p = plane.Pos();
@@ -88,11 +87,10 @@ Line ImplicitPlane::Intersect(Plane plane)
Eigen::Vector3f pos = p + (d - n.dot(p)) / n.dot(u) * u;
Eigen::Vector3f dir = v - n.dot(v) / n.dot(u) * u;
return Line(pos, dir);
+ }
-}
-
-
-float ImplicitPlane::Get(Eigen::Vector3f pos)
-{
- return GetNormal().dot(pos - GetClosestPoint(pos));
+ float ImplicitPlane::Get(Eigen::Vector3f pos)
+ {
+ return GetNormal().dot(pos - GetClosestPoint(pos));
+ }
}
diff --git a/VirtualRobot/math/Index3.cpp b/VirtualRobot/math/Index3.cpp
index ee1d2e5f30f659c11cabe894f7f9eae2a145d42c..070d9ba9574ee048d9347a7070b671fa668543e0 100644
--- a/VirtualRobot/math/Index3.cpp
+++ b/VirtualRobot/math/Index3.cpp
@@ -21,18 +21,17 @@
#include "Index3.h"
-using namespace math;
-
-
-Index3::Index3(int x, int y, int z)
- : x(x), y(y), z(z)
+namespace math
{
-}
+ Index3::Index3(int x, int y, int z)
+ : x(x), y(y), z(z)
+ {
+ }
-std::string Index3::ToString()
-{
- std::stringstream ss;
- ss << x << " " << y << " " << " " << z;
- return ss.str();
+ std::string Index3::ToString()
+ {
+ std::stringstream ss;
+ ss << x << " " << y << " " << " " << z;
+ return ss.str();
+ }
}
-
diff --git a/VirtualRobot/math/Kernels.cpp b/VirtualRobot/math/Kernels.cpp
index 3ccae94bf02ddcc698153e28fd282ea34b74277c..c1a7ec40a63bf441bef08675197fa950519ca2d4 100644
--- a/VirtualRobot/math/Kernels.cpp
+++ b/VirtualRobot/math/Kernels.cpp
@@ -24,136 +24,164 @@
#include <cmath>
#include <iostream>
-using namespace math;
-
-float KernelWithDerivatives::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i, int j) const
-{
- if (i == 0 && j == 0) return Kernel(p1, p2, R);
- if (i == 0) return Kernel_dj(p1, p2, R, j - 1);
- if (j == 0) return Kernel_di(p1, p2, R, i - 1);
- return Kernel_didj(p1, p2, R, i - 1, j - 1);
-}
+namespace math
+{
+ float KernelWithDerivatives::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i, int j) const
+ {
+ if (i == 0 && j == 0)
+ {
+ return Kernel(p1, p2, R);
+ }
+ if (i == 0)
+ {
+ return Kernel_dj(p1, p2, R, j - 1);
+ }
+ if (j == 0)
+ {
+ return Kernel_di(p1, p2, R, i - 1);
+ }
+ return Kernel_didj(p1, p2, R, i - 1, j - 1);
+ }
-float KernelWithDerivatives::Kernel_di(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i) const
-{
- float r = (p1-p2).norm();
- float x = p1.x() - p2.x();
- float y = p1.y() - p2.y();
- float z = p1.z() - p2.z();
- swap(x, y, z, i);
- return Kernel_dx(x, y, z, r, R);
-}
-float KernelWithDerivatives::Kernel_dj(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int j) const
-{
- float r = (p1-p2).norm();
- float x = p1.x() - p2.x();
- float y = p1.y() - p2.y();
- float z = p1.z() - p2.z();
- swap(x, y, z, j);
- return -Kernel_dx(x, y, z, r, R);
-}
+ float KernelWithDerivatives::Kernel_di(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i) const
+ {
+ float r = (p1 - p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ swap(x, y, z, i);
+ return Kernel_dx(x, y, z, r, R);
+ }
+ float KernelWithDerivatives::Kernel_dj(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int j) const
+ {
+ float r = (p1 - p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ swap(x, y, z, j);
+ return -Kernel_dx(x, y, z, r, R);
+ }
-float KernelWithDerivatives::Kernel_didj(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i, int j) const
-{
- float r = (p1-p2).norm();
- float x = p1.x() - p2.x();
- float y = p1.y() - p2.y();
- float z = p1.z() - p2.z();
- if (i == j)
- {
- swap(x, y, z, i);
- return -Kernel_ddx(x, y, z, r, R);
- }
- std::vector<float> a(3);
- a.at(0)=x;
- a.at(1)=y;
- a.at(2)=z;
-
- x = a.at(i);
- y = a.at(j);
- z = a.at(3 - i - j);
- return -Kernel_dxdy(x, y, z, r, R);
-}
+ float KernelWithDerivatives::Kernel_didj(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R, int i, int j) const
+ {
+ float r = (p1 - p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ if (i == j)
+ {
+ swap(x, y, z, i);
+ return -Kernel_ddx(x, y, z, r, R);
+ }
+ std::vector<float> a(3);
+ a.at(0) = x;
+ a.at(1) = y;
+ a.at(2) = z;
+
+ x = a.at(i);
+ y = a.at(j);
+ z = a.at(3 - i - j);
+ return -Kernel_dxdy(x, y, z, r, R);
+ }
-void KernelWithDerivatives::swap(float &x, float &y, float &z, int index) const
-{
- switch (index)
- {
- case 0: break;
- case 1: math::Helpers::Swap(x, y); break;
- case 2: math::Helpers::Swap(x, z); break;
- default: throw std::invalid_argument("index must be >= 0 and <= 2" );
+ void KernelWithDerivatives::swap(float& x, float& y, float& z, int index) const
+ {
+ switch (index)
+ {
+ case 0:
+ break;
+ case 1:
+ math::Helpers::Swap(x, y);
+ break;
+ case 2:
+ math::Helpers::Swap(x, z);
+ break;
+ default:
+ throw std::invalid_argument("index must be >= 0 and <= 2");
+ }
}
-}
-GaussianKernel::GaussianKernel(float lengthScale)
- : lengthScale(lengthScale) {}
+ GaussianKernel::GaussianKernel(float lengthScale)
+ : lengthScale(lengthScale) {}
-float GaussianKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float /*R*/) const
-{
- const float tmp = (p1 - p2).squaredNorm() / (2*lengthScale*lengthScale);
- //std::cout << (2*lengthScale*lengthScale) << " " << (p1 - p2).squaredNorm() << " " << tmp << std::endl;
- return std::exp(-tmp);
-}
+ float GaussianKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float /*R*/) const
+ {
+ const float tmp = (p1 - p2).squaredNorm() / (2 * lengthScale * lengthScale);
+ //std::cout << (2*lengthScale*lengthScale) << " " << (p1 - p2).squaredNorm() << " " << tmp << std::endl;
+ return std::exp(-tmp);
+ }
-float GaussianKernel::Kernel_dx(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
-{
- throw std::runtime_error("function not implemented");
-}
+ float GaussianKernel::Kernel_dx(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
+ {
+ throw std::runtime_error("function not implemented");
+ }
-float GaussianKernel::Kernel_ddx(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
-{
- throw std::runtime_error("function not implemented");
-}
+ float GaussianKernel::Kernel_ddx(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
+ {
+ throw std::runtime_error("function not implemented");
+ }
-float GaussianKernel::Kernel_dxdy(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
-{
- throw std::runtime_error("function not implemented");
-}
+ float GaussianKernel::Kernel_dxdy(float /*x*/, float /*y*/, float /*z*/, float /*r*/, float /*R*/) const
+ {
+ throw std::runtime_error("function not implemented");
+ }
-float WilliamsPlusKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R) const
-{
- float r = (p1-p2).norm();
- return 2 * r*r*r + 3 * R * r*r + R*R*R;
-}
+ float WilliamsPlusKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R) const
+ {
+ float r = (p1 - p2).norm();
+ return 2 * r * r * r + 3 * R * r * r + R * R * R;
+ }
-float WilliamsPlusKernel::Kernel_dx(float x, float /*y*/, float /*z*/, float r, float R) const
-{
- return 6 * x * (r + R);
-}
+ float WilliamsPlusKernel::Kernel_dx(float x, float /*y*/, float /*z*/, float r, float R) const
+ {
+ return 6 * x * (r + R);
+ }
-float WilliamsPlusKernel::Kernel_ddx(float x, float /*y*/, float /*z*/, float r, float R) const
-{
- if (r == 0) return 6 * R;
- return 6 * (R + r) + 6 * x * x / r;
-}
+ float WilliamsPlusKernel::Kernel_ddx(float x, float /*y*/, float /*z*/, float r, float R) const
+ {
+ if (r == 0)
+ {
+ return 6 * R;
+ }
+ return 6 * (R + r) + 6 * x * x / r;
+ }
-float WilliamsPlusKernel::Kernel_dxdy(float x, float y, float /*z*/, float r, float /*R*/) const
-{
- if (r == 0) return 0;
- return 6 * x * y / r;
-}
+ float WilliamsPlusKernel::Kernel_dxdy(float x, float y, float /*z*/, float r, float /*R*/) const
+ {
+ if (r == 0)
+ {
+ return 0;
+ }
+ return 6 * x * y / r;
+ }
-float WilliamsMinusKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R) const
-{
- float r = (p1-p2).norm();
- return 2 * r*r*r - 3 * R * r*r + R*R*R;
-}
+ float WilliamsMinusKernel::Kernel(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float R) const
+ {
+ float r = (p1 - p2).norm();
+ return 2 * r * r * r - 3 * R * r * r + R * R * R;
+ }
-float WilliamsMinusKernel::Kernel_dx(float x, float /*y*/, float /*z*/, float r, float R) const
-{
- return 6 * x * (r - R);
-}
+ float WilliamsMinusKernel::Kernel_dx(float x, float /*y*/, float /*z*/, float r, float R) const
+ {
+ return 6 * x * (r - R);
+ }
-float WilliamsMinusKernel::Kernel_ddx(float x, float /*y*/, float /*z*/, float r, float R) const
-{
- if (r == 0) return 6 * R;
- return -6 * (r - R) + 6 * x * x / r;
-}
+ float WilliamsMinusKernel::Kernel_ddx(float x, float /*y*/, float /*z*/, float r, float R) const
+ {
+ if (r == 0)
+ {
+ return 6 * R;
+ }
+ return -6 * (r - R) + 6 * x * x / r;
+ }
-float WilliamsMinusKernel::Kernel_dxdy(float x, float y, float /*z*/, float r, float /*R*/) const
-{
- if (r == 0) return 0;
- return -6 * x * y / r;
+ float WilliamsMinusKernel::Kernel_dxdy(float x, float y, float /*z*/, float r, float /*R*/) const
+ {
+ if (r == 0)
+ {
+ return 0;
+ }
+ return -6 * x * y / r;
+ }
}
diff --git a/VirtualRobot/math/Line.cpp b/VirtualRobot/math/Line.cpp
index ddc2519a28113ddd2f646a5e618293c0ff6ace3d..d64121317ffc1a9a85dec2b26a47e18c48439b15 100644
--- a/VirtualRobot/math/Line.cpp
+++ b/VirtualRobot/math/Line.cpp
@@ -25,138 +25,146 @@
#include "float.h"
#include "Helpers.h"
-using namespace math;
-
-
-
-Line::Line(Eigen::Vector3f pos, Eigen::Vector3f dir)
- : pos(pos), dir(dir)
+namespace math
{
-}
+ Line::Line(Eigen::Vector3f pos, Eigen::Vector3f dir)
+ : pos(pos), dir(dir)
+ {
+ }
-Line Line::Normalized() const
-{
- return Line(pos, dir.normalized());
-}
+ Line Line::Normalized() const
+ {
+ return Line(pos, dir.normalized());
+ }
-Eigen::Vector3f Line::Get(float t)
-{
- return pos + t * dir;
-}
+ Eigen::Vector3f Line::Get(float t)
+ {
+ return pos + t * dir;
+ }
-Eigen::Vector3f Line::GetDerivative(float)
-{
- return dir;
-}
+ Eigen::Vector3f Line::GetDerivative(float)
+ {
+ return dir;
+ }
-Eigen::Vector3f Line::GetClosestPoint(Eigen::Vector3f p) const
-{
- return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
-}
+ Eigen::Vector3f Line::GetClosestPoint(Eigen::Vector3f p) const
+ {
+ return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
+ }
-Eigen::Vector3f Line::GetDistanceToPoint(Eigen::Vector3f p) const
-{
- const Eigen::Vector3f n = dir.normalized();
- const Eigen::Vector3f pInLine = (p - pos);
- const float len = (n.transpose() * pInLine);
- return pInLine - n * len;
-}
+ Eigen::Vector3f Line::GetDistanceToPoint(Eigen::Vector3f p) const
+ {
+ const Eigen::Vector3f n = dir.normalized();
+ const Eigen::Vector3f pInLine = (p - pos);
+ const float len = (n.transpose() * pInLine);
+ return pInLine - n * len;
+ }
-float Line::GetT(Eigen::Vector3f p) const
-{
- return (p - pos).dot(dir) / dir.squaredNorm();
+ float Line::GetT(Eigen::Vector3f p) const
+ {
+ return (p - pos).dot(dir) / dir.squaredNorm();
-}
+ }
-std::string Line::ToString() const
-{
- std::stringstream ss;
- ss << "(" << pos << ") (" << dir << ")";
- return ss.str();
-}
+ std::string Line::ToString() const
+ {
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir << ")";
+ return ss.str();
+ }
-//https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
-bool Line::IntersectsTriangle(Triangle tri, float &out) const
-{
- const float EPS = 0.000; //TODO
- Eigen::Vector3f e1, e2; //Edge1, Edge2
- Eigen::Vector3f P, Q, T;
- float det, inv_det, u, v;
- float t;
- Eigen::Vector3f V1 = tri.P1();
- Eigen::Vector3f V2 = tri.P2();
- Eigen::Vector3f V3 = tri.P3();
-
- //Find vectors for two edges sharing V1
- e1 = V2 -V1;
- e2 = V3 -V1;
-
- //Begin calculating determinant - also used to calculate u parameter
- P = dir.cross(e2);
- //if determinant is near zero, ray lies in plane of triangle
- det =e1.dot( P);
- //NOT CULLING
- if(det > -EPS && det < EPS) return 0;
- inv_det = 1.f / det;
-
- //calculate distance from V1 to ray origin
- T = pos - V1;
-
- //Calculate u parameter and test bound
- u = T.dot(P) * inv_det;
- //The intersection lies outside of the triangle
- if(u < 0.f || u > 1.f) return 0;
-
- //Prepare to test v parameter
- Q = T.cross(e1);
-
- //Calculate V parameter and test bound
- v = dir.dot(Q) * inv_det;
- //The intersection lies outside of the triangle
- if(v < 0.f || u + v > 1.f) return 0;
-
- t = e2.dot(Q) * inv_det;
-
- if(t > EPS) { //ray intersection
- out = t;
- return 1;
- }
+ //https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+ bool Line::IntersectsTriangle(Triangle tri, float& out) const
+ {
+ const float EPS = 0.000; //TODO
+ Eigen::Vector3f e1, e2; //Edge1, Edge2
+ Eigen::Vector3f P, Q, T;
+ float det, inv_det, u, v;
+ float t;
+ Eigen::Vector3f V1 = tri.P1();
+ Eigen::Vector3f V2 = tri.P2();
+ Eigen::Vector3f V3 = tri.P3();
+
+ //Find vectors for two edges sharing V1
+ e1 = V2 - V1;
+ e2 = V3 - V1;
+
+ //Begin calculating determinant - also used to calculate u parameter
+ P = dir.cross(e2);
+ //if determinant is near zero, ray lies in plane of triangle
+ det = e1.dot(P);
+ //NOT CULLING
+ if (det > -EPS && det < EPS)
+ {
+ return 0;
+ }
+ inv_det = 1.f / det;
- // No hit, no win
- return 0;
-}
+ //calculate distance from V1 to ray origin
+ T = pos - V1;
-bool Line::IntersectsPrimitive(PrimitivePtr p, float &out) const
-{
- float min = FLT_MAX;
- float t;
- for(Triangle tri : *p){
- if (IntersectsTriangle(tri, t)){
- if(t<min) min = t;
+ //Calculate u parameter and test bound
+ u = T.dot(P) * inv_det;
+ //The intersection lies outside of the triangle
+ if (u < 0.f || u > 1.f)
+ {
+ return 0;
}
- }
- out = min;
- return out < FLT_MAX;
-}
-Line Line::FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2)
-{
- return Line(p1, p2 - p1);
-}
+ //Prepare to test v parameter
+ Q = T.cross(e1);
-Line Line::FromPoses(const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2)
-{
- return FromPoints(p1.block<3,1>(0,3), p2.block<3,1>(0,3));
-}
+ //Calculate V parameter and test bound
+ v = dir.dot(Q) * inv_det;
+ //The intersection lies outside of the triangle
+ if (v < 0.f || u + v > 1.f)
+ {
+ return 0;
+ }
-Line Line::Transform(const Eigen::Matrix4f &pose) const
-{
- return Line(Helpers::TransformPosition(pose, pos), Helpers::TransformDirection(pose, dir));
-}
+ t = e2.dot(Q) * inv_det;
+ if (t > EPS) //ray intersection
+ {
+ out = t;
+ return 1;
+ }
+ // No hit, no win
+ return 0;
+ }
+ bool Line::IntersectsPrimitive(PrimitivePtr p, float& out) const
+ {
+ float min = FLT_MAX;
+ float t;
+ for (Triangle tri : *p)
+ {
+ if (IntersectsTriangle(tri, t))
+ {
+ if (t < min)
+ {
+ min = t;
+ }
+ }
+ }
+ out = min;
+ return out < FLT_MAX;
+ }
+ Line Line::FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2)
+ {
+ return Line(p1, p2 - p1);
+ }
+ Line Line::FromPoses(const Eigen::Matrix4f& p1, const Eigen::Matrix4f& p2)
+ {
+ return FromPoints(p1.block<3, 1>(0, 3), p2.block<3, 1>(0, 3));
+ }
+ Line Line::Transform(const Eigen::Matrix4f& pose) const
+ {
+ return Line(Helpers::TransformPosition(pose, pos), Helpers::TransformDirection(pose, dir));
+ }
+}
diff --git a/VirtualRobot/math/LineR2.cpp b/VirtualRobot/math/LineR2.cpp
index 752295ad1a7798125e921456f674aac6f91a77fe..5f7db534c1b1b58853cbae74d400a1bc0417bb21 100644
--- a/VirtualRobot/math/LineR2.cpp
+++ b/VirtualRobot/math/LineR2.cpp
@@ -22,63 +22,62 @@
#include "LineR2.h"
-using namespace math;
-
-
-
-LineR2::LineR2(Eigen::Vector3f pos, Eigen::Vector3f dir)
- : pos(pos), dir(dir)
+namespace math
{
-}
+ LineR2::LineR2(Eigen::Vector3f pos, Eigen::Vector3f dir)
+ : pos(pos), dir(dir)
+ {
+ }
-LineR2 LineR2::Normalized()
-{
- return LineR2(pos,dir.normalized());
-}
+ LineR2 LineR2::Normalized()
+ {
+ return LineR2(pos, dir.normalized());
+ }
-Eigen::Vector3f LineR2::Get(float t)
-{
- return pos + t * dir;
-}
+ Eigen::Vector3f LineR2::Get(float t)
+ {
+ return pos + t * dir;
+ }
-Eigen::Vector3f LineR2::GetDerivative(float)
-{
- return dir;
-}
+ Eigen::Vector3f LineR2::GetDerivative(float)
+ {
+ return dir;
+ }
-Eigen::Vector3f LineR2::GetClosestPoint(Eigen::Vector3f p)
-{
- return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
-}
+ Eigen::Vector3f LineR2::GetClosestPoint(Eigen::Vector3f p)
+ {
+ return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
+ }
-float LineR2::GetT(Eigen::Vector3f p)
-{
- return (p - pos).dot(dir) / dir.squaredNorm();
+ float LineR2::GetT(Eigen::Vector3f p)
+ {
+ return (p - pos).dot(dir) / dir.squaredNorm();
-}
+ }
-std::string LineR2::ToString()
-{
- std::stringstream ss;
- ss << "(" << pos << ") (" << dir << ")";
- return ss.str();
+ std::string LineR2::ToString()
+ {
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir << ")";
+ return ss.str();
-}
+ }
-//bool LineR2::Intersect(const Eigen::Vector3f &pos1, const Eigen::Vector3f &dir1, const Eigen::Vector3f &pos2, const Eigen::Vector3f &dir2, float &t, float &u)
-//{
-//
-//}
-//
-//Vec3Opt LineR2::Intersect(const LineR2& l2)
-//{
-// float t, u;
-// if (Intersect(this, l2, &t, &u))
-// {
-// return Vec3Opt(Get(t));
-// }
-// else
-// {
-// return Vec3Opt();
-// }
-//}
+ //bool LineR2::Intersect(const Eigen::Vector3f &pos1, const Eigen::Vector3f &dir1, const Eigen::Vector3f &pos2, const Eigen::Vector3f &dir2, float &t, float &u)
+ //{
+ //
+ //}
+ //
+ //Vec3Opt LineR2::Intersect(const LineR2& l2)
+ //{
+ // float t, u;
+ // if (Intersect(this, l2, &t, &u))
+ // {
+ // return Vec3Opt(Get(t));
+ // }
+ // else
+ // {
+ // return Vec3Opt();
+ // }
+ //}
+}
diff --git a/VirtualRobot/math/LineStrip.cpp b/VirtualRobot/math/LineStrip.cpp
index cb35fe1c2a15954687bebb43b850ee1753812046..49d436444a3c32d5a38d9a71a5171c1fa0091cda 100644
--- a/VirtualRobot/math/LineStrip.cpp
+++ b/VirtualRobot/math/LineStrip.cpp
@@ -22,34 +22,33 @@
#include "LineStrip.h"
#include "Helpers.h"
-using namespace math;
-
-
-
-LineStrip::LineStrip(const std::vector<Eigen::Vector3f> &points, float minT, float maxT)
- : points(points), minT(minT), maxT(maxT)
-{
-}
-
-bool LineStrip::InLimits(float t)
-{
- return t >= minT && t <= maxT;
-}
-
-Eigen::Vector3f LineStrip::Get(float t)
-{
- int i; float f;
- GetIndex(t, i, f);
- return points.at(i) * (1 - f) + points.at(i+1) * f;
-}
-
-Eigen::Vector3f LineStrip::GetDirection(int i)
+namespace math
{
- return points.at(i+1) - points.at(i);
+ LineStrip::LineStrip(const std::vector<Eigen::Vector3f>& points, float minT, float maxT)
+ : points(points), minT(minT), maxT(maxT)
+ {
+ }
+
+ bool LineStrip::InLimits(float t)
+ {
+ return t >= minT && t <= maxT;
+ }
+
+ Eigen::Vector3f LineStrip::Get(float t)
+ {
+ int i;
+ float f;
+ GetIndex(t, i, f);
+ return points.at(i) * (1 - f) + points.at(i + 1) * f;
+ }
+
+ Eigen::Vector3f LineStrip::GetDirection(int i)
+ {
+ return points.at(i + 1) - points.at(i);
+ }
+
+ void LineStrip::GetIndex(float t, int& i, float& f)
+ {
+ Helpers::GetIndex(t, minT, maxT, points.size(), i, f);
+ }
}
-
-void LineStrip::GetIndex(float t, int &i, float &f)
-{
- Helpers::GetIndex(t, minT, maxT, points.size(), i, f);
-}
-
diff --git a/VirtualRobot/math/LinearContinuedBezier.cpp b/VirtualRobot/math/LinearContinuedBezier.cpp
index 67da10bf7fe4416fe3bc4fea9142838731ebcc16..34a4d0c73ea2dd5386341297b75c0478d25bc424 100644
--- a/VirtualRobot/math/LinearContinuedBezier.cpp
+++ b/VirtualRobot/math/LinearContinuedBezier.cpp
@@ -21,23 +21,36 @@
#include "LinearContinuedBezier.h"
-using namespace math;
-
-math::LinearContinuedBezier::LinearContinuedBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
- : p0(p0), p1(p1), p2(p2), p3(p3)
+namespace math
{
-}
+ math::LinearContinuedBezier::LinearContinuedBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
+ : p0(p0), p1(p1), p2(p2), p3(p3)
+ {
+ }
-Eigen::Vector3f math::LinearContinuedBezier::Get(float t)
-{
- if (0 <= t && t <= 1) return Bezier::CubicBezierPoint(p0, p1, p2, p3, t);
- if (t < 0) return p0 + (p0 - p1) * 3 * -t;
- return p3 + (p3 - p2) * 3 * (t - 1);
-}
+ Eigen::Vector3f math::LinearContinuedBezier::Get(float t)
+ {
+ if (0 <= t && t <= 1)
+ {
+ return Bezier::CubicBezierPoint(p0, p1, p2, p3, t);
+ }
+ if (t < 0)
+ {
+ return p0 + (p0 - p1) * 3 * -t;
+ }
+ return p3 + (p3 - p2) * 3 * (t - 1);
+ }
-Eigen::Vector3f math::LinearContinuedBezier::GetDerivative(float t)
-{
- if (0 <= t && t <= 1) return Bezier::CubicBezierDerivative(p0, p1, p2, p3, t);
- if (t < 0) return (p0 - p1) * -3;
- return (p3 - p2) * 3;
+ Eigen::Vector3f math::LinearContinuedBezier::GetDerivative(float t)
+ {
+ if (0 <= t && t <= 1)
+ {
+ return Bezier::CubicBezierDerivative(p0, p1, p2, p3, t);
+ }
+ if (t < 0)
+ {
+ return (p0 - p1) * -3;
+ }
+ return (p3 - p2) * 3;
+ }
}
diff --git a/VirtualRobot/math/LinearInterpolatedOrientation.cpp b/VirtualRobot/math/LinearInterpolatedOrientation.cpp
index ffea50ce96c339d71f32d4cff083e83ae30cc367..2d2d95706a513153a10873021a8bef987651a1f5 100644
--- a/VirtualRobot/math/LinearInterpolatedOrientation.cpp
+++ b/VirtualRobot/math/LinearInterpolatedOrientation.cpp
@@ -24,49 +24,49 @@
//#include <iostream>
-using namespace math;
-
-LinearInterpolatedOrientation::LinearInterpolatedOrientation(const Eigen::Quaternionf &startOri, const Eigen::Quaternionf &endOri, float startT, float endT, bool clamp)
- :startOri(startOri), endOri(endOri), startT(startT), endT(endT), clamp(clamp)
+namespace math
{
- Eigen::AngleAxisf aa(endOri * startOri.inverse());
- angle = Helpers::AngleModPI(aa.angle());
- axis = aa.axis();
- Eigen::Vector3f oriDelta = fabs(angle) < 0.01 ? Eigen::Vector3f::Zero() : Eigen::Vector3f(aa.axis() * angle);
-
- derivative = oriDelta / (endT - startT);
-}
-
-LinearInterpolatedOrientation::LinearInterpolatedOrientation(const Eigen::Matrix3f &startOri, const Eigen::Matrix3f &endOri, float startT, float endT, bool clamp)
- : LinearInterpolatedOrientation(Eigen::Quaternionf(startOri), Eigen::Quaternionf(endOri), startT, endT, clamp)
-{
-}
+ LinearInterpolatedOrientation::LinearInterpolatedOrientation(const Eigen::Quaternionf& startOri, const Eigen::Quaternionf& endOri, float startT, float endT, bool clamp)
+ : startOri(startOri), endOri(endOri), startT(startT), endT(endT), clamp(clamp)
+ {
+ Eigen::AngleAxisf aa(endOri * startOri.inverse());
+ angle = Helpers::AngleModPI(aa.angle());
+ axis = aa.axis();
+ Eigen::Vector3f oriDelta = fabs(angle) < 0.01 ? Eigen::Vector3f::Zero() : Eigen::Vector3f(aa.axis() * angle);
+ derivative = oriDelta / (endT - startT);
+ }
-Eigen::Quaternionf math::LinearInterpolatedOrientation::Get(float t)
-{
- if(derivative.squaredNorm() < 0.001)
+ LinearInterpolatedOrientation::LinearInterpolatedOrientation(const Eigen::Matrix3f& startOri, const Eigen::Matrix3f& endOri, float startT, float endT, bool clamp)
+ : LinearInterpolatedOrientation(Eigen::Quaternionf(startOri), Eigen::Quaternionf(endOri), startT, endT, clamp)
{
- return startOri;
}
- float f = Helpers::ILerp(startT, endT, t);
- //std::cout << "before clamp: " << f << std::endl;
- if(clamp)
+
+
+ Eigen::Quaternionf math::LinearInterpolatedOrientation::Get(float t)
{
- f = Helpers::Clamp(0, 1, f);
+ if (derivative.squaredNorm() < 0.001)
+ {
+ return startOri;
+ }
+ float f = Helpers::ILerp(startT, endT, t);
+ //std::cout << "before clamp: " << f << std::endl;
+ if (clamp)
+ {
+ f = Helpers::Clamp(0, 1, f);
+ }
+ //std::cout << "after clamp: " << f << std::endl;
+ //Helpers::Lerp(startOri, endOri, f);
+ Eigen::AngleAxisf aa(f * angle, axis);
+ return Eigen::Quaternionf(aa * startOri);
}
- //std::cout << "after clamp: " << f << std::endl;
- //Helpers::Lerp(startOri, endOri, f);
- Eigen::AngleAxisf aa(f * angle, axis);
- return Eigen::Quaternionf(aa * startOri);
-}
-Eigen::Vector3f math::LinearInterpolatedOrientation::GetDerivative(float t)
-{
- if(clamp && (t < startT || t > endT))
+ Eigen::Vector3f math::LinearInterpolatedOrientation::GetDerivative(float t)
{
- return Eigen::Vector3f::Zero();
+ if (clamp && (t < startT || t > endT))
+ {
+ return Eigen::Vector3f::Zero();
+ }
+ return derivative;
}
- return derivative;
-
}
diff --git a/VirtualRobot/math/LinearInterpolatedPose.cpp b/VirtualRobot/math/LinearInterpolatedPose.cpp
index b49ce0322ffb2067b7cda63559018d2d8533f7d9..d5cf9c8286593abd11af48f265433d12256baf5b 100644
--- a/VirtualRobot/math/LinearInterpolatedPose.cpp
+++ b/VirtualRobot/math/LinearInterpolatedPose.cpp
@@ -22,23 +22,24 @@
#include "LinearInterpolatedPose.h"
#include "Helpers.h"
-using namespace math;
-
-LinearInterpolatedPose::LinearInterpolatedPose(const Eigen::Matrix4f &startPose, const Eigen::Matrix4f &endPose, float startT, float endT, bool clamp)
- : ori(Helpers::GetOrientation(startPose), Helpers::GetOrientation(endPose), startT, endT, clamp),
- startPos(Helpers::GetPosition(startPose)),
- endPos(Helpers::GetPosition(endPose)),
- startT(startT), endT(endT), clamp(clamp)
+namespace math
{
+ LinearInterpolatedPose::LinearInterpolatedPose(const Eigen::Matrix4f& startPose, const Eigen::Matrix4f& endPose, float startT, float endT, bool clamp)
+ : ori(Helpers::GetOrientation(startPose), Helpers::GetOrientation(endPose), startT, endT, clamp),
+ startPos(Helpers::GetPosition(startPose)),
+ endPos(Helpers::GetPosition(endPose)),
+ startT(startT), endT(endT), clamp(clamp)
+ {
-}
+ }
-Eigen::Matrix4f LinearInterpolatedPose::Get(float t)
-{
- float f = Helpers::ILerp(startT, endT, t);
- if(clamp)
+ Eigen::Matrix4f LinearInterpolatedPose::Get(float t)
{
- f = Helpers::Clamp(0, 1, f);
+ float f = Helpers::ILerp(startT, endT, t);
+ if (clamp)
+ {
+ f = Helpers::Clamp(0, 1, f);
+ }
+ return Helpers::CreatePose(Helpers::Lerp(startPos, endPos, f), ori.Get(t));
}
- return Helpers::CreatePose(Helpers::Lerp(startPos, endPos, f), ori.Get(t));
}
diff --git a/VirtualRobot/math/MarchingCubes.cpp b/VirtualRobot/math/MarchingCubes.cpp
index eb3efb2fed3b5f39ab1a5fc5f4c283a22f9099b6..4ac033d1f2edb9bd0c534d9c173f3c042ef66269 100644
--- a/VirtualRobot/math/MarchingCubes.cpp
+++ b/VirtualRobot/math/MarchingCubes.cpp
@@ -29,304 +29,367 @@
#include "stdio.h"
#include "GridCacheFloat3.h"
-using namespace math;
-
-
-static const float eps = 0.0001f;
-
-
-void MarchingCubes::Init(int size, Eigen::Vector3f /*center*/, Eigen::Vector3f /*start*/, int /*steps*/, float /*stepLength*/, GridCacheFloat3Ptr cache)
+namespace math
{
- _size = size;
- _grids = Array3DGridCellPtr(new Array3D<GridCell>(size));
- Gdata = cache;
- for (int i = 0; i < size; i++)
- for (int j = 0; j < size; j++)
- for (int k = 0; k < size; k++)
- {
- GridCell cell = GridCell();
+ static const float eps = 0.0001f;
- cell.P[0] = Eigen::Vector3f(i, j, k);
- cell.P[1] = Eigen::Vector3f(i + 1, j, k);
- cell.P[2] = Eigen::Vector3f(i + 1, j + 1, k);
- cell.P[3] = Eigen::Vector3f(i, j + 1, k);
- cell.P[4] = Eigen::Vector3f(i, j, k + 1);
- cell.P[5] = Eigen::Vector3f(i + 1, j, k + 1);
- cell.P[6] = Eigen::Vector3f(i + 1, j + 1, k + 1);
- cell.P[7] = Eigen::Vector3f(i, j + 1, k + 1);
- _grids->Set(i,j,k,cell);
- }
-}
+ void MarchingCubes::Init(int size, Eigen::Vector3f /*center*/, Eigen::Vector3f /*start*/, int /*steps*/, float /*stepLength*/, GridCacheFloat3Ptr cache)
+ {
+ _size = size;
+ _grids = Array3DGridCellPtr(new Array3D<GridCell>(size));
+ Gdata = cache;
+ for (int i = 0; i < size; i++)
+ for (int j = 0; j < size; j++)
+ for (int k = 0; k < size; k++)
+ {
+ GridCell cell = GridCell();
+
+ cell.P[0] = Eigen::Vector3f(i, j, k);
+ cell.P[1] = Eigen::Vector3f(i + 1, j, k);
+ cell.P[2] = Eigen::Vector3f(i + 1, j + 1, k);
+ cell.P[3] = Eigen::Vector3f(i, j + 1, k);
+ cell.P[4] = Eigen::Vector3f(i, j, k + 1);
+ cell.P[5] = Eigen::Vector3f(i + 1, j, k + 1);
+ cell.P[6] = Eigen::Vector3f(i + 1, j + 1, k + 1);
+ cell.P[7] = Eigen::Vector3f(i, j + 1, k + 1);
+
+ _grids->Set(i, j, k, cell);
+ }
+ }
-PrimitivePtr MarchingCubes::Process(float isolevel)
-{
- PrimitivePtr res = PrimitivePtr(new Primitive());
- Process(isolevel, res); //TODO ref
- return res;
-}
+ PrimitivePtr MarchingCubes::Process(float isolevel)
+ {
+ PrimitivePtr res = PrimitivePtr(new Primitive());
+ Process(isolevel, res); //TODO ref
+ return res;
+ }
-void MarchingCubes::Process(float isolevel, PrimitivePtr primitive) //TODO ref?
-{
- primitive->clear();
- int triNum = 0;
- for (int i = 0; i < _size; i++)
- for (int j = 0; j < _size; j++)
- for (int k = 0; k < _size; k++)
- {
- triNum = Polygonise(i, j, k, isolevel);
- if (triNum > 0)
+ void MarchingCubes::Process(float isolevel, PrimitivePtr primitive) //TODO ref?
+ {
+ primitive->clear();
+ int triNum = 0;
+ for (int i = 0; i < _size; i++)
+ for (int j = 0; j < _size; j++)
+ for (int k = 0; k < _size; k++)
{
- Build(primitive, triNum); //TODO ref
+ triNum = Polygonise(i, j, k, isolevel);
+ if (triNum > 0)
+ {
+ Build(primitive, triNum); //TODO ref
+ }
}
- }
- //if (primitive.CurrentVertex == 0) primitive.isDrawable = false;
- //else primitive.InitializePrimitive(graphicsDevice);
-}
+ //if (primitive.CurrentVertex == 0) primitive.isDrawable = false;
+ //else primitive.InitializePrimitive(graphicsDevice);
+ }
-PrimitivePtr MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, Index3 start)
-{
- PrimitivePtr res = PrimitivePtr(new Primitive());
- ProcessSingleSurfaceOptimized(isolevel, res, start); //TODO ref
- return res;
-}
+ PrimitivePtr MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, Index3 start)
+ {
+ PrimitivePtr res = PrimitivePtr(new Primitive());
+ ProcessSingleSurfaceOptimized(isolevel, res, start); //TODO ref
+ return res;
+ }
-void MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, PrimitivePtr primitive, Index3 start) //TODO ref
-{
- FringePtr fringe = FringePtr(new std::queue<Index3>);
- fringe->push(start);
- primitive->clear();
- Array3DBoolPtr marked = Array3DBoolPtr(new Array3D<bool>(_size + 1));
- marked->Set(start,true);
- bool foundSurface = false;
- while (fringe->size() > 0)
+ void MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, PrimitivePtr primitive, Index3 start) //TODO ref
{
- Index3 index = fringe->front();
- fringe->pop();
- int i = index.X();
- int j = index.Y();
- int k = index.Z();
- int triNum = Polygonise(i, j, k, isolevel);
- if (triNum > 0)
- {
- Build(primitive, triNum);
- foundSurface = true;
- }
- if (triNum > 0 || !foundSurface)
- {
- AddAndMark(i + 1, j, k, fringe, marked);
- AddAndMark(i - 1, j, k, fringe, marked);
- AddAndMark(i, j + 1, k, fringe, marked);
- AddAndMark(i, j - 1, k, fringe, marked);
- AddAndMark(i, j, k + 1, fringe, marked);
- AddAndMark(i, j, k - 1, fringe, marked);
+ FringePtr fringe = FringePtr(new std::queue<Index3>);
+ fringe->push(start);
+ primitive->clear();
+ Array3DBoolPtr marked = Array3DBoolPtr(new Array3D<bool>(_size + 1));
+ marked->Set(start, true);
+ bool foundSurface = false;
+ while (fringe->size() > 0)
+ {
+ Index3 index = fringe->front();
+ fringe->pop();
+ int i = index.X();
+ int j = index.Y();
+ int k = index.Z();
+ int triNum = Polygonise(i, j, k, isolevel);
+ if (triNum > 0)
+ {
+ Build(primitive, triNum);
+ foundSurface = true;
+ }
+ if (triNum > 0 || !foundSurface)
+ {
+ AddAndMark(i + 1, j, k, fringe, marked);
+ AddAndMark(i - 1, j, k, fringe, marked);
+ AddAndMark(i, j + 1, k, fringe, marked);
+ AddAndMark(i, j - 1, k, fringe, marked);
+ AddAndMark(i, j, k + 1, fringe, marked);
+ AddAndMark(i, j, k - 1, fringe, marked);
+ }
}
}
-}
-PrimitivePtr MarchingCubes::Calculate(Eigen::Vector3f center, Eigen::Vector3f start, int steps, float stepLength, SimpleAbstractFunctionR3R1Ptr modelPtr, float isolevel)
-{
- int size = steps * 2;
- start = (start - center) / stepLength;
- Index3 startIndex = Index3((int)start.x() + steps, (int)start.y() + steps, (int)start.z() + steps);
- MarchingCubes mc = MarchingCubes();
- std::function<float(Index3)> getData = [steps, stepLength, center, modelPtr](Index3 index)
+ PrimitivePtr MarchingCubes::Calculate(Eigen::Vector3f center, Eigen::Vector3f start, int steps, float stepLength, SimpleAbstractFunctionR3R1Ptr modelPtr, float isolevel)
{
- Eigen::Vector3f pos = Eigen::Vector3f((index.X() - steps) * stepLength + center.x(),
- (index.Y() - steps) * stepLength + center.y(),
- (index.Z() - steps) * stepLength + center.z());
- return modelPtr->Get(pos);
+ int size = steps * 2;
+ start = (start - center) / stepLength;
+ Index3 startIndex = Index3((int)start.x() + steps, (int)start.y() + steps, (int)start.z() + steps);
+ MarchingCubes mc = MarchingCubes();
+ std::function<float(Index3)> getData = [steps, stepLength, center, modelPtr](Index3 index)
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f((index.X() - steps) * stepLength + center.x(),
+ (index.Y() - steps) * stepLength + center.y(),
+ (index.Z() - steps) * stepLength + center.z());
+ return modelPtr->Get(pos);
- };
- mc.Init(size, center, start, steps, stepLength, GridCacheFloat3Ptr(new GridCacheFloat3(size+1, getData)));
- PrimitivePtr triangles = mc.ProcessSingleSurfaceOptimized(isolevel, startIndex);
+ };
+ mc.Init(size, center, start, steps, stepLength, GridCacheFloat3Ptr(new GridCacheFloat3(size + 1, getData)));
+ PrimitivePtr triangles = mc.ProcessSingleSurfaceOptimized(isolevel, startIndex);
- PrimitivePtr result = PrimitivePtr(new Primitive());
- for (Triangle t : *triangles)
- {
- result->push_back(t.Transform(- Eigen::Vector3f(1, 1, 1) * stepLength * steps + center, stepLength));
+ PrimitivePtr result = PrimitivePtr(new Primitive());
+ for (Triangle t : *triangles)
+ {
+ result->push_back(t.Transform(- Eigen::Vector3f(1, 1, 1) * stepLength * steps + center, stepLength));
+ }
+ return result;
}
- return result;
-}
-float MarchingCubes::GetVal(int x, int y, int z, int i)
-{
- switch (i)
+ float MarchingCubes::GetVal(int x, int y, int z, int i)
{
- case 0:
- return Gdata->Get(x, y, z);
- case 1:
- return Gdata->Get(x + 1, y, z);
- case 2:
- return Gdata->Get(x + 1, y + 1, z);
- case 3:
- return Gdata->Get(x, y + 1, z);
- case 4:
- return Gdata->Get(x, y, z + 1);
- case 5:
- return Gdata->Get(x + 1, y, z + 1);
- case 6:
- return Gdata->Get(x + 1, y + 1, z + 1);
- case 7:
- return Gdata->Get(x, y + 1, z + 1);
+ switch (i)
+ {
+ case 0:
+ return Gdata->Get(x, y, z);
+ case 1:
+ return Gdata->Get(x + 1, y, z);
+ case 2:
+ return Gdata->Get(x + 1, y + 1, z);
+ case 3:
+ return Gdata->Get(x, y + 1, z);
+ case 4:
+ return Gdata->Get(x, y, z + 1);
+ case 5:
+ return Gdata->Get(x + 1, y, z + 1);
+ case 6:
+ return Gdata->Get(x + 1, y + 1, z + 1);
+ case 7:
+ return Gdata->Get(x, y + 1, z + 1);
+ }
+ return 0;
}
- return 0;
-}
-Eigen::Vector3f MarchingCubes::GetPos(int x, int y, int z, int i)
-{
- return _grids->Get(x, y, z).P[i];
-}
+ Eigen::Vector3f MarchingCubes::GetPos(int x, int y, int z, int i)
+ {
+ return _grids->Get(x, y, z).P[i];
+ }
-void MarchingCubes::AddAndMark(int x, int y, int z, FringePtr fringe, Array3DBoolPtr marked)
-{
- if (!IsValidIndex(x, y, z)) return;
- if (marked->Get(x,y,z)) return;
- fringe->push( Index3(x, y, z));
- marked->Set(x,y,z,true);
-}
+ void MarchingCubes::AddAndMark(int x, int y, int z, FringePtr fringe, Array3DBoolPtr marked)
+ {
+ if (!IsValidIndex(x, y, z))
+ {
+ return;
+ }
+ if (marked->Get(x, y, z))
+ {
+ return;
+ }
+ fringe->push(Index3(x, y, z));
+ marked->Set(x, y, z, true);
+ }
-bool MarchingCubes::IsValidIndex(int x, int y, int z)
-{
- return x >= 0 && x < _size && y >= 0 && y < _size && z >= 0 && z < _size;
-}
+ bool MarchingCubes::IsValidIndex(int x, int y, int z)
+ {
+ return x >= 0 && x < _size && y >= 0 && y < _size && z >= 0 && z < _size;
+ }
-int MarchingCubes::Polygonise(int x, int y, int z, float isolevel)
-{
- Eigen::Vector3f vertlist[12];
- int i, ntriang = 0;
- unsigned int cubeindex = 0; //TODO unsigned char == C# char?
+ int MarchingCubes::Polygonise(int x, int y, int z, float isolevel)
+ {
+ Eigen::Vector3f vertlist[12];
+ int i, ntriang = 0;
+ unsigned int cubeindex = 0; //TODO unsigned char == C# char?
- if (GetVal(x, y, z, 0) > isolevel) cubeindex |= 1;
- if (GetVal(x, y, z, 1) > isolevel) cubeindex |= 2;
- if (GetVal(x, y, z, 2) > isolevel) cubeindex |= 4;
- if (GetVal(x, y, z, 3) > isolevel) cubeindex |= 8;
- if (GetVal(x, y, z, 4) > isolevel) cubeindex |= 16;
- if (GetVal(x, y, z, 5) > isolevel) cubeindex |= 32;
- if (GetVal(x, y, z, 6) > isolevel) cubeindex |= 64;
- if (GetVal(x, y, z, 7) > isolevel) cubeindex |= 128;
+ if (GetVal(x, y, z, 0) > isolevel)
+ {
+ cubeindex |= 1;
+ }
+ if (GetVal(x, y, z, 1) > isolevel)
+ {
+ cubeindex |= 2;
+ }
+ if (GetVal(x, y, z, 2) > isolevel)
+ {
+ cubeindex |= 4;
+ }
+ if (GetVal(x, y, z, 3) > isolevel)
+ {
+ cubeindex |= 8;
+ }
+ if (GetVal(x, y, z, 4) > isolevel)
+ {
+ cubeindex |= 16;
+ }
+ if (GetVal(x, y, z, 5) > isolevel)
+ {
+ cubeindex |= 32;
+ }
+ if (GetVal(x, y, z, 6) > isolevel)
+ {
+ cubeindex |= 64;
+ }
+ if (GetVal(x, y, z, 7) > isolevel)
+ {
+ cubeindex |= 128;
+ }
- /* Cube is entirely in/out of the surface */
- if (_edgeTable[cubeindex] == 0)
- return 0;
+ /* Cube is entirely in/out of the surface */
+ if (_edgeTable[cubeindex] == 0)
+ {
+ return 0;
+ }
- /* Find the vertices where the surface intersects the cube */
- if ((_edgeTable[cubeindex] & 1) > 0)
- vertlist[0] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 1), GetVal(x, y, z, 0), GetVal(x, y, z, 1));
- if ((_edgeTable[cubeindex] & 2) > 0)
- vertlist[1] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 2), GetVal(x, y, z, 1), GetVal(x, y, z, 2));
- if ((_edgeTable[cubeindex] & 4) > 0)
- vertlist[2] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 3), GetVal(x, y, z, 2), GetVal(x, y, z, 3));
- if ((_edgeTable[cubeindex] & 8) > 0)
- vertlist[3] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 0), GetVal(x, y, z, 3), GetVal(x, y, z, 0));
- if ((_edgeTable[cubeindex] & 16) > 0)
- vertlist[4] = VertexInterp(isolevel, GetPos(x, y, z, 4), GetPos(x, y, z, 5), GetVal(x, y, z, 4), GetVal(x, y, z, 5));
- if ((_edgeTable[cubeindex] & 32) > 0)
- vertlist[5] = VertexInterp(isolevel, GetPos(x, y, z, 5), GetPos(x, y, z, 6), GetVal(x, y, z, 5), GetVal(x, y, z, 6));
- if ((_edgeTable[cubeindex] & 64) > 0)
- vertlist[6] = VertexInterp(isolevel, GetPos(x, y, z, 6), GetPos(x, y, z, 7), GetVal(x, y, z, 6), GetVal(x, y, z, 7));
- if ((_edgeTable[cubeindex] & 128) > 0)
- vertlist[7] = VertexInterp(isolevel, GetPos(x, y, z, 7), GetPos(x, y, z, 4), GetVal(x, y, z, 7), GetVal(x, y, z, 4));
- if ((_edgeTable[cubeindex] & 256) > 0)
- vertlist[8] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 4), GetVal(x, y, z, 0), GetVal(x, y, z, 4));
- if ((_edgeTable[cubeindex] & 512) > 0)
- vertlist[9] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 5), GetVal(x, y, z, 1), GetVal(x, y, z, 5));
- if ((_edgeTable[cubeindex] & 1024) > 0)
- vertlist[10] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 6), GetVal(x, y, z, 2), GetVal(x, y, z, 6));
- if ((_edgeTable[cubeindex] & 2048) > 0)
- vertlist[11] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 7), GetVal(x, y, z, 3), GetVal(x, y, z, 7));
+ /* Find the vertices where the surface intersects the cube */
+ if ((_edgeTable[cubeindex] & 1) > 0)
+ {
+ vertlist[0] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 1), GetVal(x, y, z, 0), GetVal(x, y, z, 1));
+ }
+ if ((_edgeTable[cubeindex] & 2) > 0)
+ {
+ vertlist[1] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 2), GetVal(x, y, z, 1), GetVal(x, y, z, 2));
+ }
+ if ((_edgeTable[cubeindex] & 4) > 0)
+ {
+ vertlist[2] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 3), GetVal(x, y, z, 2), GetVal(x, y, z, 3));
+ }
+ if ((_edgeTable[cubeindex] & 8) > 0)
+ {
+ vertlist[3] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 0), GetVal(x, y, z, 3), GetVal(x, y, z, 0));
+ }
+ if ((_edgeTable[cubeindex] & 16) > 0)
+ {
+ vertlist[4] = VertexInterp(isolevel, GetPos(x, y, z, 4), GetPos(x, y, z, 5), GetVal(x, y, z, 4), GetVal(x, y, z, 5));
+ }
+ if ((_edgeTable[cubeindex] & 32) > 0)
+ {
+ vertlist[5] = VertexInterp(isolevel, GetPos(x, y, z, 5), GetPos(x, y, z, 6), GetVal(x, y, z, 5), GetVal(x, y, z, 6));
+ }
+ if ((_edgeTable[cubeindex] & 64) > 0)
+ {
+ vertlist[6] = VertexInterp(isolevel, GetPos(x, y, z, 6), GetPos(x, y, z, 7), GetVal(x, y, z, 6), GetVal(x, y, z, 7));
+ }
+ if ((_edgeTable[cubeindex] & 128) > 0)
+ {
+ vertlist[7] = VertexInterp(isolevel, GetPos(x, y, z, 7), GetPos(x, y, z, 4), GetVal(x, y, z, 7), GetVal(x, y, z, 4));
+ }
+ if ((_edgeTable[cubeindex] & 256) > 0)
+ {
+ vertlist[8] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 4), GetVal(x, y, z, 0), GetVal(x, y, z, 4));
+ }
+ if ((_edgeTable[cubeindex] & 512) > 0)
+ {
+ vertlist[9] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 5), GetVal(x, y, z, 1), GetVal(x, y, z, 5));
+ }
+ if ((_edgeTable[cubeindex] & 1024) > 0)
+ {
+ vertlist[10] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 6), GetVal(x, y, z, 2), GetVal(x, y, z, 6));
+ }
+ if ((_edgeTable[cubeindex] & 2048) > 0)
+ {
+ vertlist[11] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 7), GetVal(x, y, z, 3), GetVal(x, y, z, 7));
+ }
- /* Create the triangle */
- for (i = 0; _triTable[cubeindex][ i] != -1; i += 3)
- {
- _triangles[ntriang] = Triangle(vertlist[static_cast<std::size_t>(_triTable[cubeindex][i ])],
- vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 1])],
- vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 2])]);
- ntriang++;
+ /* Create the triangle */
+ for (i = 0; _triTable[cubeindex][ i] != -1; i += 3)
+ {
+ _triangles[ntriang] = Triangle(vertlist[static_cast<std::size_t>(_triTable[cubeindex][i ])],
+ vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 1])],
+ vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 2])]);
+ ntriang++;
+ }
+ return ntriang;
}
- return ntriang;
-}
-Eigen::Vector3f MarchingCubes::VertexInterp(float isolevel, Eigen::Vector3f p1, Eigen::Vector3f p2, float valp1, float valp2)
-{
- if (std::abs(isolevel - valp1) < 0.00001f)
- return p1;
- if (std::abs(isolevel - valp2) < 0.00001f)
- return p2;
- if (std::abs(valp1 - valp2) < 0.00001f)
- return p1;
- double mu = (isolevel - valp1) / (valp2 - valp1);
- float x = (float)(p1.x() + mu * (p2.x() - p1.x()));
- float y = (float)(p1.y() + mu * (p2.y() - p1.y()));
- float z = (float)(p1.z() + mu * (p2.z() - p1.z()));
+ Eigen::Vector3f MarchingCubes::VertexInterp(float isolevel, Eigen::Vector3f p1, Eigen::Vector3f p2, float valp1, float valp2)
+ {
+ if (std::abs(isolevel - valp1) < 0.00001f)
+ {
+ return p1;
+ }
+ if (std::abs(isolevel - valp2) < 0.00001f)
+ {
+ return p2;
+ }
+ if (std::abs(valp1 - valp2) < 0.00001f)
+ {
+ return p1;
+ }
+ double mu = (isolevel - valp1) / (valp2 - valp1);
+ float x = (float)(p1.x() + mu * (p2.x() - p1.x()));
+ float y = (float)(p1.y() + mu * (p2.y() - p1.y()));
+ float z = (float)(p1.z() + mu * (p2.z() - p1.z()));
- return Eigen::Vector3f(x, y, z);
-}
+ return Eigen::Vector3f(x, y, z);
+ }
-void MarchingCubes::Build(PrimitivePtr res, int tianglesNum)
-{
- for (int i = 0; i < tianglesNum; i++)
+ void MarchingCubes::Build(PrimitivePtr res, int tianglesNum)
{
- res->push_back(_triangles[i]);
+ for (int i = 0; i < tianglesNum; i++)
+ {
+ res->push_back(_triangles[i]);
+ }
}
-}
- const int MarchingCubes::_edgeTable[256] = {
- 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
+ const int MarchingCubes::_edgeTable[256] =
+ {
+ 0x0, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
- 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
+ 0x190, 0x99, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
- 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,
+ 0x230, 0x339, 0x33, 0x13a, 0x636, 0x73f, 0x435, 0x53c,
0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
- 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,
+ 0x3a0, 0x2a9, 0x1a3, 0xaa, 0x7a6, 0x6af, 0x5a5, 0x4ac,
0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
- 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,
+ 0x460, 0x569, 0x663, 0x76a, 0x66, 0x16f, 0x265, 0x36c,
0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
- 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,
+ 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff, 0x3f5, 0x2fc,
0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
- 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,
+ 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55, 0x15c,
0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
- 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,
+ 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc,
0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
- 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
+ 0xcc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
- 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
+ 0x15c, 0x55, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
- 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
+ 0x2fc, 0x3f5, 0xff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
- 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,
+ 0x36c, 0x265, 0x16f, 0x66, 0x76a, 0x663, 0x569, 0x460,
0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
- 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,
+ 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa, 0x1a3, 0x2a9, 0x3a0,
0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
- 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,
+ 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33, 0x339, 0x230,
0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
- 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,
+ 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99, 0x190,
0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0
- };
+ };
- #pragma GCC diagnostic push
- #pragma GCC diagnostic ignored "-Wnarrowing"
- const char MarchingCubes::_triTable[256][16] =
- {{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
- {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
- {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
- {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
- {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
- {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wnarrowing"
+ const char MarchingCubes::_triTable[256][16] =
+ {
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
@@ -567,7 +630,6 @@ void MarchingCubes::Build(PrimitivePtr res, int tianglesNum)
{0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
-};
- #pragma GCC diagnostic pop
-
-
+ };
+#pragma GCC diagnostic pop
+}
diff --git a/VirtualRobot/math/MathForwardDefinitions.h b/VirtualRobot/math/MathForwardDefinitions.h
index b260249427c4b8c3a8a5352fa089325fd5149396..ece4c1e6bc56157537eb4fca30ab4ebf212bf6ed 100644
--- a/VirtualRobot/math/MathForwardDefinitions.h
+++ b/VirtualRobot/math/MathForwardDefinitions.h
@@ -65,14 +65,6 @@ private:
namespace math
{
-
-
- //typedef Eigen::Vector2f Eigen::Vector2f;
- //typedef Eigen::Vector3f Eigen::Vector3f;
- //typedef Eigen::Matrix3f Matrix3f;
- //typedef Eigen::MatrixXf MatrixXf;
- //typedef Eigen::VectorXf VectorXf;
- //typedef Nullable<float> floatOpt;
typedef Nullable<Eigen::Vector3f> Vec3Opt;
typedef boost::shared_ptr<class AbstractFunctionR1R2> AbstractFunctionR1R2Ptr;
diff --git a/VirtualRobot/math/Plane.cpp b/VirtualRobot/math/Plane.cpp
index 141d164134928413ee9f2e9142cc10ef69ab79c6..bcd7500f122bee0e68e093afaaaf7ecda8eb4f38 100644
--- a/VirtualRobot/math/Plane.cpp
+++ b/VirtualRobot/math/Plane.cpp
@@ -22,94 +22,98 @@
#include "Plane.h"
#include "Helpers.h"
-using namespace math;
-
-Plane::Plane(Eigen::Vector3f pos, Eigen::Vector3f dir1, Eigen::Vector3f dir2)
- : pos(pos), dir1(dir1), dir2(dir2)
-{
-}
-
-Eigen::Vector3f Plane::GetPoint(float u, float v)
-{
- return pos + u * dir1 + v * dir2;
-}
-
-Eigen::Vector3f Plane::GetDdu(float, float)
-{
- return dir1;
-}
-
-Eigen::Vector3f Plane::GetDdv(float, float)
-{
- return dir2;
-}
-
-void Plane::GetUV(Eigen::Vector3f pos, float& u, float& v)
-{
- float w;
- GetUVW(pos,u,v,w);
-}
-
-Eigen::Vector3f Plane::GetNormal()
-{
- return dir1.cross(dir2);
-}
-
-Eigen::Vector3f Plane::GetNormal(const Eigen::Vector3f &preferredDirection)
-{
- Eigen::Vector3f normal = GetNormal();
- if(normal.dot(preferredDirection) < 0) normal = -normal;
- return normal;
-}
-
-Plane Plane::SwappedDirections()
-{
- return Plane(pos, dir2, dir1);
-}
-
-Plane Plane::Normalized()
-{
- return Plane(pos, dir1.normalized(), dir2.normalized());
-}
-
-ImplicitPlane Plane::ToImplicit()
-{
- return ImplicitPlane::FromPositionNormal(pos, GetNormal());
-}
-
-Eigen::Matrix3f Plane::GetRotationMatrix()
-{
- Helpers::AssertNormalized(dir1);
- Helpers::AssertNormalized(dir2);
- Eigen::Matrix3f result;
- result << dir1 , dir2 , GetNormal();
- return result;
-}
-
-std::string Plane::ToString()
-{
- std::stringstream ss;
- ss << "(" << pos << ") (" << dir1 << ")" << ") (" << dir2 << ")";
- return ss.str();
-}
-
-void Plane::GetUVW(Eigen::Vector3f pos, float& u, float& v, float& w)
-{
- Eigen::Vector3f rotated = GetRotationMatrix().transpose() * (pos - this->pos);
- u = rotated.x();
- v = rotated.y();
- w = rotated.z();
-
-}
-
-Plane Plane::FromNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
-{
- Eigen::Vector3f d1, d2;
- Helpers::GetOrthonormalVectors(normal, d1,d2);
- return Plane(pos, d1, d2);
-}
-
-math::Plane math::Plane::Transform(const Eigen::Matrix4f &transform)
+namespace math
{
- return Plane(Helpers::TransformPosition(transform, pos), Helpers::TransformDirection(transform, dir1), Helpers::TransformDirection(transform, dir2));
+ Plane::Plane(Eigen::Vector3f pos, Eigen::Vector3f dir1, Eigen::Vector3f dir2)
+ : pos(pos), dir1(dir1), dir2(dir2)
+ {
+ }
+
+ Eigen::Vector3f Plane::GetPoint(float u, float v)
+ {
+ return pos + u * dir1 + v * dir2;
+ }
+
+ Eigen::Vector3f Plane::GetDdu(float, float)
+ {
+ return dir1;
+ }
+
+ Eigen::Vector3f Plane::GetDdv(float, float)
+ {
+ return dir2;
+ }
+
+ void Plane::GetUV(Eigen::Vector3f pos, float& u, float& v)
+ {
+ float w;
+ GetUVW(pos, u, v, w);
+ }
+
+ Eigen::Vector3f Plane::GetNormal()
+ {
+ return dir1.cross(dir2);
+ }
+
+ Eigen::Vector3f Plane::GetNormal(const Eigen::Vector3f& preferredDirection)
+ {
+ Eigen::Vector3f normal = GetNormal();
+ if (normal.dot(preferredDirection) < 0)
+ {
+ normal = -normal;
+ }
+ return normal;
+ }
+
+ Plane Plane::SwappedDirections()
+ {
+ return Plane(pos, dir2, dir1);
+ }
+
+ Plane Plane::Normalized()
+ {
+ return Plane(pos, dir1.normalized(), dir2.normalized());
+ }
+
+ ImplicitPlane Plane::ToImplicit()
+ {
+ return ImplicitPlane::FromPositionNormal(pos, GetNormal());
+ }
+
+ Eigen::Matrix3f Plane::GetRotationMatrix()
+ {
+ Helpers::AssertNormalized(dir1);
+ Helpers::AssertNormalized(dir2);
+ Eigen::Matrix3f result;
+ result << dir1, dir2, GetNormal();
+ return result;
+ }
+
+ std::string Plane::ToString()
+ {
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir1 << ")" << ") (" << dir2 << ")";
+ return ss.str();
+ }
+
+ void Plane::GetUVW(Eigen::Vector3f pos, float& u, float& v, float& w)
+ {
+ Eigen::Vector3f rotated = GetRotationMatrix().transpose() * (pos - this->pos);
+ u = rotated.x();
+ v = rotated.y();
+ w = rotated.z();
+
+ }
+
+ Plane Plane::FromNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
+ {
+ Eigen::Vector3f d1, d2;
+ Helpers::GetOrthonormalVectors(normal, d1, d2);
+ return Plane(pos, d1, d2);
+ }
+
+ math::Plane math::Plane::Transform(const Eigen::Matrix4f& transform)
+ {
+ return Plane(Helpers::TransformPosition(transform, pos), Helpers::TransformDirection(transform, dir1), Helpers::TransformDirection(transform, dir2));
+ }
}
diff --git a/VirtualRobot/math/Primitive.cpp b/VirtualRobot/math/Primitive.cpp
index e0b6ae62d617e5daa099250e1fbf293971940e17..a1e7e284ad7467377bd8c286ea9c835768d2f507 100644
--- a/VirtualRobot/math/Primitive.cpp
+++ b/VirtualRobot/math/Primitive.cpp
@@ -22,17 +22,10 @@
#include "Line.h"
#include "Primitive.h"
-using namespace math;
-
-
-
-void Primitive::AddTriangle(Eigen::Vector3f v1, Eigen::Vector3f v2, Eigen::Vector3f v3)
+namespace math
{
- push_back(Triangle(v1, v2, v3));
+ void Primitive::AddTriangle(Eigen::Vector3f v1, Eigen::Vector3f v2, Eigen::Vector3f v3)
+ {
+ push_back(Triangle(v1, v2, v3));
+ }
}
-
-
-
-
-
-
diff --git a/VirtualRobot/math/SimpleAbstractFunctionR1Ori.cpp b/VirtualRobot/math/SimpleAbstractFunctionR1Ori.cpp
index 40312db73e9f24929b325daf9fb5011420b0d8dc..7b79e296b2dd207431eb00d217bd8b338927d0dd 100644
--- a/VirtualRobot/math/SimpleAbstractFunctionR1Ori.cpp
+++ b/VirtualRobot/math/SimpleAbstractFunctionR1Ori.cpp
@@ -23,8 +23,6 @@
#include "SimpleAbstractFunctionR1Ori.h"
-using namespace armarx;
-
SimpleAbstractFunctionR1Ori::SimpleAbstractFunctionR1Ori()
{
}
diff --git a/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp b/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp
index b31e89426a11a615de9cbf0ebd760f08c9282d72..7742cc27ca84c4dd9e0dfd1d291b039bc98df898 100644
--- a/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp
+++ b/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp
@@ -24,8 +24,6 @@
#include "SimpleAbstractFunctionR1R6.h"
#include "Helpers.h"
-using namespace armarx;
-
SimpleAbstractFunctionR1R6::SimpleAbstractFunctionR1R6()
{
}
diff --git a/VirtualRobot/math/TransformedFunctionR1R3.cpp b/VirtualRobot/math/TransformedFunctionR1R3.cpp
index 9ae7bd6fc15a4b73abbf33f35c6b68478f6f6466..42a92b4206c3ec233c95587c8d291906d3765c2e 100644
--- a/VirtualRobot/math/TransformedFunctionR1R3.cpp
+++ b/VirtualRobot/math/TransformedFunctionR1R3.cpp
@@ -23,23 +23,21 @@
#include "TransformedFunctionR1R3.h"
#include "Helpers.h"
-using namespace math;
-
-
-
-TransformedFunctionR1R3::TransformedFunctionR1R3(const Eigen::Matrix4f& transformation, AbstractFunctionR1R3Ptr func)
- : transformation(transformation), func(func)
+namespace math
{
+ TransformedFunctionR1R3::TransformedFunctionR1R3(const Eigen::Matrix4f& transformation, AbstractFunctionR1R3Ptr func)
+ : transformation(transformation), func(func)
+ {
-}
-
+ }
-Eigen::Vector3f TransformedFunctionR1R3::Get(float t)
-{
- return Helpers::TransformPosition(transformation, func->Get(t));
-}
+ Eigen::Vector3f TransformedFunctionR1R3::Get(float t)
+ {
+ return Helpers::TransformPosition(transformation, func->Get(t));
+ }
-Eigen::Vector3f TransformedFunctionR1R3::GetDerivative(float t)
-{
- return Helpers::TransformDirection(transformation, func->GetDerivative(t));
+ Eigen::Vector3f TransformedFunctionR1R3::GetDerivative(float t)
+ {
+ return Helpers::TransformDirection(transformation, func->GetDerivative(t));
+ }
}
diff --git a/VirtualRobot/math/TransformedFunctionR2R3.cpp b/VirtualRobot/math/TransformedFunctionR2R3.cpp
index 652d549f8a5023d9fe6b9b95d19441c82143f8f2..75fedc86fdf4bd149de8e1f70082f1eca7412c16 100644
--- a/VirtualRobot/math/TransformedFunctionR2R3.cpp
+++ b/VirtualRobot/math/TransformedFunctionR2R3.cpp
@@ -23,34 +23,31 @@
#include "TransformedFunctionR2R3.h"
#include "Helpers.h"
-using namespace math;
-
-
-
-TransformedFunctionR2R3::TransformedFunctionR2R3(const Eigen::Matrix4f& transformation, AbstractFunctionR2R3Ptr func)
- : transformation(transformation), inv(transformation.inverse()), func(func)
-{
-
-}
-
-
-
-Eigen::Vector3f math::TransformedFunctionR2R3::GetPoint(float u, float v)
-{
- return Helpers::TransformPosition(transformation, func->GetPoint(u, v));
-}
-
-Eigen::Vector3f math::TransformedFunctionR2R3::GetDdu(float u, float v)
-{
- return Helpers::TransformDirection(transformation, func->GetDdu(u, v));
-}
-
-Eigen::Vector3f math::TransformedFunctionR2R3::GetDdv(float u, float v)
-{
- return Helpers::TransformDirection(transformation, func->GetDdv(u, v));
-}
-
-void math::TransformedFunctionR2R3::GetUV(Eigen::Vector3f pos, float& u, float& v)
+namespace math
{
- func->GetUV(Helpers::TransformPosition(inv, pos), u, v);
+ TransformedFunctionR2R3::TransformedFunctionR2R3(const Eigen::Matrix4f& transformation, AbstractFunctionR2R3Ptr func)
+ : transformation(transformation), inv(transformation.inverse()), func(func)
+ {
+
+ }
+
+ Eigen::Vector3f math::TransformedFunctionR2R3::GetPoint(float u, float v)
+ {
+ return Helpers::TransformPosition(transformation, func->GetPoint(u, v));
+ }
+
+ Eigen::Vector3f math::TransformedFunctionR2R3::GetDdu(float u, float v)
+ {
+ return Helpers::TransformDirection(transformation, func->GetDdu(u, v));
+ }
+
+ Eigen::Vector3f math::TransformedFunctionR2R3::GetDdv(float u, float v)
+ {
+ return Helpers::TransformDirection(transformation, func->GetDdv(u, v));
+ }
+
+ void math::TransformedFunctionR2R3::GetUV(Eigen::Vector3f pos, float& u, float& v)
+ {
+ func->GetUV(Helpers::TransformPosition(inv, pos), u, v);
+ }
}
diff --git a/VirtualRobot/math/Triangle.cpp b/VirtualRobot/math/Triangle.cpp
index e0c257aa62296aa288b5885367d8fcdcc8909307..860afd1d2651f68bb515db66ada1ac8db928958a 100644
--- a/VirtualRobot/math/Triangle.cpp
+++ b/VirtualRobot/math/Triangle.cpp
@@ -21,72 +21,65 @@
#include "Triangle.h"
-using namespace math;
-
-
-Triangle::Triangle()
- : p1(Eigen::Vector3f(0,0,0)), p2(Eigen::Vector3f(0,0,0)), p3(Eigen::Vector3f(0,0,0))
+namespace math
{
-}
+ Triangle::Triangle()
+ : p1(Eigen::Vector3f(0, 0, 0)), p2(Eigen::Vector3f(0, 0, 0)), p3(Eigen::Vector3f(0, 0, 0))
+ {
+ }
-Triangle::Triangle(Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
- : p1(p1), p2(p2), p3(p3)
-{
-}
+ Triangle::Triangle(Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
+ : p1(p1), p2(p2), p3(p3)
+ {
+ }
-std::string Triangle::ToString()
-{
- std::stringstream ss;
- ss << "(" << p1 << ") (" << p2 << ") (" << p3 << ")";
- return ss.str();
-}
+ std::string Triangle::ToString()
+ {
+ std::stringstream ss;
+ ss << "(" << p1 << ") (" << p2 << ") (" << p3 << ")";
+ return ss.str();
+ }
-Eigen::Vector3f Triangle::Normal() const
-{
- return (p2 - p1).cross(p3 - p1);
-}
+ Eigen::Vector3f Triangle::Normal() const
+ {
+ return (p2 - p1).cross(p3 - p1);
+ }
-Triangle Triangle::Flipped()
-{
- return Triangle(p2, p1, p3);
-}
+ Triangle Triangle::Flipped()
+ {
+ return Triangle(p2, p1, p3);
+ }
-Eigen::Vector3f Triangle::Centroid()
-{
- return (p1 + p2 + p3) / 3;
-}
+ Eigen::Vector3f Triangle::Centroid()
+ {
+ return (p1 + p2 + p3) / 3;
+ }
-std::vector<Triangle> Triangle::Subdivide(int depth)
-{
- std::vector<Triangle> list;
- Subdivide(depth, p1, p2, p3, list);
- return list;
-}
+ std::vector<Triangle> Triangle::Subdivide(int depth)
+ {
+ std::vector<Triangle> list;
+ Subdivide(depth, p1, p2, p3, list);
+ return list;
+ }
-void Triangle::Subdivide(int depth, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, std::vector<Triangle> &list)
-{
- if (depth <= 0)
+ void Triangle::Subdivide(int depth, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, std::vector<Triangle>& list)
{
- list.push_back(Triangle(p1, p2, p3));
- return;
+ if (depth <= 0)
+ {
+ list.push_back(Triangle(p1, p2, p3));
+ return;
+ }
+ Eigen::Vector3f c = (p1 + p2 + p3) / 3;
+ Subdivide(depth - 1, p1, p2, c, list);
+ Subdivide(depth - 1, p2, p3, c, list);
+ Subdivide(depth - 1, p3, p1, c, list);
}
- Eigen::Vector3f c = (p1 + p2 + p3) / 3;
- Subdivide(depth - 1, p1, p2, c, list);
- Subdivide(depth - 1, p2, p3, c, list);
- Subdivide(depth - 1, p3, p1, c, list);
-}
-Triangle Triangle::Transform(Eigen::Vector3f center, float unitLength)
-{
- return Triangle(
- p1 * unitLength + center,
- p2 * unitLength + center,
- p3 * unitLength + center);
+ Triangle Triangle::Transform(Eigen::Vector3f center, float unitLength)
+ {
+ return Triangle(
+ p1 * unitLength + center,
+ p2 * unitLength + center,
+ p3 * unitLength + center);
+ }
}
-
-
-
-
-
-
-
diff --git a/VirtualRobot/math/WeightedAverage.cpp b/VirtualRobot/math/WeightedAverage.cpp
index d8ba6264c02e66f53d4a118cf785fe7bada8dac2..bfc48a40e61901bf63b824de47f5fa2c58c7cb95 100644
--- a/VirtualRobot/math/WeightedAverage.cpp
+++ b/VirtualRobot/math/WeightedAverage.cpp
@@ -21,37 +21,36 @@
#include "WeightedAverage.h"
-using namespace math;
-
-
-
-void math::WeightedFloatAverage::Add(float value, float weight)
-{
- sum += value * weight;
- weightSum += weight;
-}
-
-float WeightedFloatAverage::Average()
-{
- return sum / weightSum;
-}
-
-float WeightedFloatAverage::WeightSum()
-{
- return weightSum;
-}
-void math::WeightedVec3Average::Add(Eigen::Vector3f value, float weight)
-{
- sum += value * weight;
- weightSum += weight;
-}
-
-Eigen::Vector3f WeightedVec3Average::Average()
-{
- return sum / weightSum;
-}
-
-float WeightedVec3Average::WeightSum()
+namespace math
{
- return weightSum;
+ void math::WeightedFloatAverage::Add(float value, float weight)
+ {
+ sum += value * weight;
+ weightSum += weight;
+ }
+
+ float WeightedFloatAverage::Average()
+ {
+ return sum / weightSum;
+ }
+
+ float WeightedFloatAverage::WeightSum()
+ {
+ return weightSum;
+ }
+ void math::WeightedVec3Average::Add(Eigen::Vector3f value, float weight)
+ {
+ sum += value * weight;
+ weightSum += weight;
+ }
+
+ Eigen::Vector3f WeightedVec3Average::Average()
+ {
+ return sum / weightSum;
+ }
+
+ float WeightedVec3Average::WeightSum()
+ {
+ return weightSum;
+ }
}
diff --git a/VirtualRobot/tests/MathHelpersTest.cpp b/VirtualRobot/tests/MathHelpersTest.cpp
index c8d480041f8be398a22a25bbe25d843e9ae0a1a0..2682b8723eb57d69a857d5def658b0d381d57a51 100644
--- a/VirtualRobot/tests/MathHelpersTest.cpp
+++ b/VirtualRobot/tests/MathHelpersTest.cpp
@@ -12,11 +12,8 @@
#include <stdio.h>
#include <random>
-
-using namespace Eigen;
using Helpers = math::Helpers;
-
BOOST_AUTO_TEST_SUITE(MathHelpers)
@@ -118,15 +115,15 @@ struct BlockFixture
{
BlockFixture()
{
- quat = Quaternionf{
- AngleAxisf(static_cast<float>(M_PI), Vector3f::UnitZ())
- * AngleAxisf(static_cast<float>(M_PI_2), Vector3f::UnitY())
+ quat = Eigen::Quaternionf{
+ Eigen::AngleAxisf(static_cast<float>(M_PI), Eigen::Vector3f::UnitZ())
+ * Eigen::AngleAxisf(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY())
};
- quat2 = AngleAxisf(static_cast<float>(M_PI_4), Vector3f::UnitX()) * quat;
+ quat2 = Eigen::AngleAxisf(static_cast<float>(M_PI_4), Eigen::Vector3f::UnitX()) * quat;
- pos = Vector3f{ 1, 2, 3 };
- pos2 = Vector3f{ 4, 5, 6 };
+ pos = Eigen::Vector3f{ 1, 2, 3 };
+ pos2 = Eigen::Vector3f{ 4, 5, 6 };
ori = quat.toRotationMatrix();
ori2 = quat2.toRotationMatrix();
@@ -136,10 +133,10 @@ struct BlockFixture
pose.block<3, 3>(0, 0) = ori;
}
- Matrix4f pose;
- Vector3f pos, pos2;
- Matrix3f ori, ori2;
- Quaternionf quat, quat2;
+ Eigen::Matrix4f pose;
+ Eigen::Vector3f pos, pos2;
+ Eigen::Matrix3f ori, ori2;
+ Eigen::Quaternionf quat, quat2;
};
@@ -150,7 +147,7 @@ using namespace math;
BOOST_AUTO_TEST_CASE(test_Position_const)
{
- BOOST_CHECK_EQUAL(Helpers::Position(const_cast<const Matrix4f&>(pose)), pos);
+ BOOST_CHECK_EQUAL(Helpers::Position(const_cast<const Eigen::Matrix4f&>(pose)), pos);
}
BOOST_AUTO_TEST_CASE(test_Position_nonconst)
@@ -188,21 +185,21 @@ BOOST_AUTO_TEST_CASE(test_Pose_matrix_and_rotation_matrix)
BOOST_AUTO_TEST_CASE(test_Pose_position)
{
- Matrix4f posePos = posePos.Identity();
+ Eigen::Matrix4f posePos = posePos.Identity();
posePos.block<3, 1>(0, 3) = pos;
BOOST_CHECK_EQUAL(Helpers::Pose(pos), posePos);
}
BOOST_AUTO_TEST_CASE(test_Pose_orientation_matrix)
{
- Matrix4f poseOri = poseOri.Identity();
+ Eigen::Matrix4f poseOri = poseOri.Identity();
poseOri.block<3, 3>(0, 0) = ori;
BOOST_CHECK_EQUAL(Helpers::Pose(ori), poseOri);
}
BOOST_AUTO_TEST_CASE(test_Pose_quaternion)
{
- Matrix4f poseOri = poseOri.Identity();
+ Eigen::Matrix4f poseOri = poseOri.Identity();
poseOri.block<3, 3>(0, 0) = ori;
BOOST_CHECK_EQUAL(Helpers::Pose(quat), poseOri);
}
@@ -213,8 +210,8 @@ BOOST_AUTO_TEST_SUITE_END()
struct OrthogonolizeFixture
{
- void test(double angle, const Vector3d& axis, float noiseAmpl, float precAngularDist, float precOrth = -1);
- Eigen::Matrix3f test(Matrix3f matrix, float noiseAmpl, float precOrth = -1);
+ void test(double angle, const Eigen::Vector3d& axis, float noiseAmpl, float precAngularDist, float precOrth = -1);
+ Eigen::Matrix3f test(Eigen::Matrix3f matrix, float noiseAmpl, float precOrth = -1);
template <typename Distribution>
static Eigen::Matrix3f Random(Distribution& distrib)
@@ -226,7 +223,7 @@ struct OrthogonolizeFixture
void OrthogonolizeFixture::test(
- double angle, const Vector3d& axis, float noiseAmpl, float precAngularDist, float precOrth)
+ double angle, const Eigen::Vector3d& axis, float noiseAmpl, float precAngularDist, float precOrth)
{
// construct matrix with double to avoid rounding errors
Eigen::AngleAxisd rot(angle, axis);
@@ -236,12 +233,12 @@ void OrthogonolizeFixture::test(
Eigen::Matrix3f orth = test(matrix, noiseAmpl, precOrth);
- Quaternionf quatOrth(orth);
+ Eigen::Quaternionf quatOrth(orth);
BOOST_TEST_MESSAGE("Angular distance: " << quatOrth.angularDistance(quat.cast<float>()));
BOOST_CHECK_LE(quatOrth.angularDistance(quat.cast<float>()), precAngularDist);
}
-Matrix3f OrthogonolizeFixture::test(Matrix3f matrix, float noiseAmpl, float _precOrth)
+Eigen::Matrix3f OrthogonolizeFixture::test(Eigen::Matrix3f matrix, float noiseAmpl, float _precOrth)
{
const float precOrth = _precOrth > 0 ? _precOrth : 1e-6f;
diff --git a/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
index edf5bf9a4f085a3163ebdce3f28fd8e7615129b9..38f8ce4f0a047f1b046a71e54579583ef42de5c7 100644
--- a/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
@@ -15,29 +15,25 @@
namespace Eigen
{
- bool operator==(const Quaternionf& lhs, const Quaternionf& rhs)
+ bool operator==(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs)
{
return lhs.isApprox(rhs, 0);
}
- std::ostream& operator<<(std::ostream& os, const Quaternionf& rhs)
+ std::ostream& operator<<(std::ostream& os, const Eigen::Quaternionf& rhs)
{
os << "[ " << rhs.w() << " | " << rhs.x() << " " << rhs.y() << " " << rhs.z() << " ]";
return os;
}
}
-
-using namespace Eigen;
-
-
BOOST_AUTO_TEST_SUITE(VirtualRobotJsonEigenConversionTest)
BOOST_AUTO_TEST_CASE(test_Matrix4f_non_transform)
{
- Matrix4f in = in.Identity(), out = out.Zero();
+ Eigen::Matrix4f in = in.Identity(), out = out.Zero();
for (int i = 0; i < in.size(); ++i)
{
@@ -51,15 +47,15 @@ BOOST_AUTO_TEST_CASE(test_Matrix4f_non_transform)
out = j;
BOOST_CHECK_EQUAL(in, out);
- out = j.get<Matrix4f>();
+ out = j.get<Eigen::Matrix4f>();
BOOST_CHECK_EQUAL(in, out);
}
BOOST_AUTO_TEST_CASE(test_Matrix4f_transform)
{
- Matrix4f in = math::Helpers::Pose(Vector3f { 3, 2, 3 },
- AngleAxisf( 1.2f, Eigen::Vector3f(1,2,3).normalized()));
- Matrix4f out = out.Zero();
+ Eigen::Matrix4f in = math::Helpers::Pose(Eigen::Vector3f { 3, 2, 3 },
+ Eigen::AngleAxisf( 1.2f, Eigen::Vector3f(1,2,3).normalized()));
+ Eigen::Matrix4f out = out.Zero();
nlohmann::json j;
j = in;
@@ -68,14 +64,14 @@ BOOST_AUTO_TEST_CASE(test_Matrix4f_transform)
out = j;
BOOST_CHECK_EQUAL(in, out);
- out = j.get<Matrix4f>();
+ out = j.get<Eigen::Matrix4f>();
BOOST_CHECK_EQUAL(in, out);
}
BOOST_AUTO_TEST_CASE(test_Vector3f)
{
- Vector3f in = in.Identity(), out = out.Zero();
+ Eigen::Vector3f in = in.Identity(), out = out.Zero();
for (int i = 0; i < in.size(); ++i)
{
@@ -89,7 +85,7 @@ BOOST_AUTO_TEST_CASE(test_Vector3f)
out = j;
BOOST_CHECK_EQUAL(in, out);
- out = j.get<Vector3f>();
+ out = j.get<Eigen::Vector3f>();
BOOST_CHECK_EQUAL(in, out);
}
@@ -114,8 +110,8 @@ BOOST_AUTO_TEST_CASE(test_Vector3f_from_xyz)
BOOST_AUTO_TEST_CASE(test_Quaternionf)
{
- Quaternionf in { AngleAxisf(static_cast<float>(M_PI), Vector3f(1, 1, 1).normalized()) };
- Quaternionf out = out.Identity();
+ Eigen::Quaternionf in { Eigen::AngleAxisf(static_cast<float>(M_PI), Eigen::Vector3f(1, 1, 1).normalized()) };
+ Eigen::Quaternionf out = out.Identity();
nlohmann::json j;
j = in;
@@ -123,7 +119,7 @@ BOOST_AUTO_TEST_CASE(test_Quaternionf)
// out = j; cannot be correctly resolved
- out = j.get<Quaternionf>();
+ out = j.get<Eigen::Quaternionf>();
BOOST_CHECK_EQUAL(in, out);
}
@@ -131,11 +127,11 @@ BOOST_AUTO_TEST_CASE(test_Quaternionf)
BOOST_AUTO_TEST_CASE(test_Matrix4f_transform_from_pos_ori)
{
Eigen::Vector3f pos(0, -1, 2.5);
- Eigen::Quaternionf ori(AngleAxisf(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY()));
- Matrix4f in = math::Helpers::Pose(pos, ori);
- Matrix4f out = out.Zero();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY()));
+ Eigen::Matrix4f in = math::Helpers::Pose(pos, ori);
+ Eigen::Matrix4f out = out.Zero();
- // ori = Quaternion
+ // ori = Eigen::Quaternion
nlohmann::json j;
j["pos"] = pos;
j["ori"] = ori;
@@ -144,7 +140,7 @@ BOOST_AUTO_TEST_CASE(test_Matrix4f_transform_from_pos_ori)
out = j;
BOOST_CHECK_EQUAL(in, out);
- out = j.get<Matrix4f>();
+ out = j.get<Eigen::Matrix4f>();
BOOST_CHECK_EQUAL(in, out);
// ori = Matrix
@@ -157,7 +153,7 @@ BOOST_AUTO_TEST_CASE(test_Matrix4f_transform_from_pos_ori)
out = j;
BOOST_CHECK_EQUAL(in, out);
- out = j.get<Matrix4f>();
+ out = j.get<Eigen::Matrix4f>();
BOOST_CHECK_EQUAL(in, out);
}
diff --git a/VirtualRobot/tests/VirtualRobotMjcfTest.cpp b/VirtualRobot/tests/VirtualRobotMjcfTest.cpp
index 532155d507a2a20d1119e5da7e2b0e6c1d9a09d6..a23e8abdd6f46c2a71f18c7a16df8aed1003bf4d 100644
--- a/VirtualRobot/tests/VirtualRobotMjcfTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotMjcfTest.cpp
@@ -15,19 +15,19 @@
namespace Eigen
{
- std::ostream& operator<<(std::ostream& os, const Vector3f& rhs)
+ std::ostream& operator<<(std::ostream& os, const Eigen::Vector3f& rhs)
{
static const IOFormat iof(4, 0, " ", " ", "", "", "[", "]");
os << rhs.format(iof);
return os;
}
- bool operator==(const Quaternionf& lhs, const Quaternionf& rhs)
+ bool operator==(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs)
{
return lhs.isApprox(rhs);
}
- std::ostream& operator<<(std::ostream& os, const Quaternionf& rhs)
+ std::ostream& operator<<(std::ostream& os, const Eigen::Quaternionf& rhs)
{
os << "[ " << rhs.w() << " | " << rhs.x() << " " << rhs.y() << " " << rhs.z() << " ]";
return os;
@@ -56,9 +56,6 @@ BOOST_AUTO_TEST_CASE(test_boost_lexical_cast)
BOOST_AUTO_TEST_SUITE_END()
-using namespace Eigen;
-
-
BOOST_AUTO_TEST_CASE(test_parseCoeffs)
{
const std::string string = "1 -3 2.4";
@@ -74,7 +71,7 @@ BOOST_AUTO_TEST_CASE(test_parseCoeffs)
BOOST_AUTO_TEST_CASE(test_attrib_conversion_vector3f)
{
- Vector3f in(1, -3, 2.4f), out;
+ Eigen::Vector3f in(1, -3, 2.4f), out;
const std::string string = mjcf::toAttr(in);
mjcf::fromAttr(string, out);
@@ -85,7 +82,7 @@ BOOST_AUTO_TEST_CASE(test_attrib_conversion_vector3f)
BOOST_AUTO_TEST_CASE(test_attrib_conversion_quaternionf)
{
- Quaternionf in(AngleAxisf(1.4f, Vector3f(.5, -1, .3f).normalized())), out;
+ Eigen::Quaternionf in(Eigen::AngleAxisf(1.4f, Eigen::Vector3f(.5, -1, .3f).normalized())), out;
const std::string string = mjcf::toAttr(in);
mjcf::fromAttr(string, out);