From 19a41e5a1095504a524333879593b8945fe8ea06 Mon Sep 17 00:00:00 2001 From: Brecht Van Lommel Date: Thu, 28 Nov 2013 21:26:55 +0100 Subject: Fix iTaSC build error when building with libc++. This was using TreeElement before it was fully defined, which gives undefined behavior that happened to work with other libraries but not libc++. Based on patch by Marcus von Appen, modifications for brevity and to ensure we don't dereference invalid memory. Ref T37477. --- intern/itasc/Armature.cpp | 2 +- intern/itasc/kdl/tree.cpp | 2 +- intern/itasc/kdl/tree.hpp | 24 ++++++++++++++---------- intern/itasc/kdl/treefksolverpos_recursive.cpp | 17 ++++++++--------- intern/itasc/kdl/treefksolverpos_recursive.hpp | 2 +- intern/itasc/kdl/treejnttojacsolver.cpp | 6 +++--- 6 files changed, 28 insertions(+), 25 deletions(-) (limited to 'intern/itasc') diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp index e6f5fda4066..0c736566b25 100644 --- a/intern/itasc/Armature.cpp +++ b/intern/itasc/Armature.cpp @@ -402,7 +402,7 @@ bool Armature::finalize() m_armlength = 0.0; for (i=0; ifirst != "root") { Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr)); length += tip.p.Norm(); diff --git a/intern/itasc/kdl/tree.cpp b/intern/itasc/kdl/tree.cpp index 8776e43f5b4..a31ac79bdf5 100644 --- a/intern/itasc/kdl/tree.cpp +++ b/intern/itasc/kdl/tree.cpp @@ -64,7 +64,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& segment_name, return false; pair retval; //insert new element - TreeElement elem(segment, parent, nrOfJoints); + TreeElement elem(segment, *parent, nrOfJoints); std::pair val(segment_name, elem); retval = segments.insert(val); diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp index 82794f96b94..8f971200969 100644 --- a/intern/itasc/kdl/tree.hpp +++ b/intern/itasc/kdl/tree.hpp @@ -41,32 +41,28 @@ namespace KDL { //Forward declaration class TreeElement; -#if defined(__APPLE__) -# if MAC_OS_X_VERSION_MIN_REQUIRED <= 1050 +#if defined(__APPLE__) && MAC_OS_X_VERSION_MIN_REQUIRED <= 1050 typedef std::map SegmentMap; -# else - // Eigen allocator is needed for alignment of Eigen data types - typedef std::map, Eigen::aligned_allocator > > SegmentMap; -# endif /* MAC_OS_X_VERSION_MIN_REQUIRED */ #else // Eigen allocator is needed for alignment of Eigen data types typedef std::map, Eigen::aligned_allocator > > SegmentMap; #endif + class TreeElement { public: - TreeElement():q_nr(0) + TreeElement():q_nr(0),parent(0) {}; public: Segment segment; unsigned int q_nr; - SegmentMap::const_iterator parent; + SegmentMap::value_type const *parent; std::vector children; - TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in) + TreeElement(const Segment& segment_in,const SegmentMap::value_type& parent_in,unsigned int q_nr_in) { q_nr=q_nr_in; segment=segment_in; - parent=parent_in; + parent=&parent_in; }; static TreeElement Root() { @@ -167,7 +163,15 @@ namespace KDL return segments.find(segment_name); }; + SegmentMap::value_type const* getSegmentPtr(const std::string& segment_name)const + { + SegmentMap::const_iterator it = segments.find(segment_name); + + if (it == segments.end()) + return 0; + return &*it; + }; const SegmentMap& getSegments()const { diff --git a/intern/itasc/kdl/treefksolverpos_recursive.cpp b/intern/itasc/kdl/treefksolverpos_recursive.cpp index 9103a2165bb..fd78c46f837 100644 --- a/intern/itasc/kdl/treefksolverpos_recursive.cpp +++ b/intern/itasc/kdl/treefksolverpos_recursive.cpp @@ -35,22 +35,22 @@ namespace KDL { int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName) { - SegmentMap::const_iterator it = tree.getSegment(segmentName); - SegmentMap::const_iterator baseit = tree.getSegment(baseName); + SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName); + SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName); if(q_in.rows() != tree.getNrOfJoints()) return -1; - else if(it == tree.getSegments().end()) //if the segment name is not found + else if(!it) //if the segment name is not found return -2; - else if(baseit == tree.getSegments().end()) //if the base segment name is not found + else if(!baseit) //if the base segment name is not found return -3; else{ - p_out = recursiveFk(q_in, it, baseit); - return 0; + p_out = recursiveFk(q_in, it, baseit); + return 0; } } - Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit) + Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit) { //gets the frame for the current element (segment) const TreeElement& currentElement = it->second; @@ -60,8 +60,7 @@ namespace KDL { } else{ Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr)); - SegmentMap::const_iterator parentIt = currentElement.parent; - return recursiveFk(q_in, parentIt, baseit) * currentFrame; + return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame; } } diff --git a/intern/itasc/kdl/treefksolverpos_recursive.hpp b/intern/itasc/kdl/treefksolverpos_recursive.hpp index c22fe4af75b..2081f23a9ff 100644 --- a/intern/itasc/kdl/treefksolverpos_recursive.hpp +++ b/intern/itasc/kdl/treefksolverpos_recursive.hpp @@ -45,7 +45,7 @@ namespace KDL { private: const Tree tree; - Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit); + Frame recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit); }; } diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp index 624bbef7990..e8b4d385ab2 100644 --- a/intern/itasc/kdl/treejnttojacsolver.cpp +++ b/intern/itasc/kdl/treejnttojacsolver.cpp @@ -28,16 +28,16 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, return -1; //Lets search the tree-element - SegmentMap::const_iterator it = tree.getSegments().find(segmentname); + SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname); //If segmentname is not inside the tree, back out: - if (it == tree.getSegments().end()) + if (!it) return -2; //Let's make the jacobian zero: SetToZero(jac); - SegmentMap::const_iterator root = tree.getSegments().find("root"); + SegmentMap::value_type const* root = tree.getSegmentPtr("root"); Frame T_total = Frame::Identity(); Frame T_local, T_joint; -- cgit v1.2.3