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.

6 files changed, 28 insertions, 25 deletions

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; i<m_neffector; i++) {
 		length = 0.0;
-		KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
+		KDL::SegmentMap::value_type const *sit = m_tree.getSegmentPtr(m_effectors[i].name);
 		while (sit->first != "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<SegmentMap::iterator, bool> retval;
     //insert new element
-	TreeElement elem(segment, parent, nrOfJoints);
+	TreeElement elem(segment, *parent, nrOfJoints);
 	std::pair<std::string, TreeElement> 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<std::string,TreeElement> SegmentMap;
-#  else
-    // Eigen allocator is needed for alignment of Eigen data types
-    typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > SegmentMap;
-#  endif /* MAC_OS_X_VERSION_MIN_REQUIRED */
 #else
     // Eigen allocator is needed for alignment of Eigen data types
     typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<std::string, TreeElement> > > 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<SegmentMap::const_iterator > 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;