Fix Eigen3 unaligned array in iTaSC. The unaligned array cause crash when SSE2 is enabled. The source of unaligned array were: matrix passing by value in std::make_pair() and offset of matrix member in structure.

3 files changed, 15 insertions, 9 deletions

diff --git a/intern/itasc/kdl/segment.hpp b/intern/itasc/kdl/segment.hpp
index 7c82ab418fa..87d972ead70 100644
--- a/intern/itasc/kdl/segment.hpp
+++ b/intern/itasc/kdl/segment.hpp
@@ -46,8 +46,8 @@ namespace KDL {
     class Segment {
         friend class Chain;
     private:
-        Joint joint;
         Inertia M;
+        Joint joint;
         Frame f_tip;
 
     public:
diff --git a/intern/itasc/kdl/tree.cpp b/intern/itasc/kdl/tree.cpp
index 097a729f577..8776e43f5b4 100644
--- a/intern/itasc/kdl/tree.cpp
+++ b/intern/itasc/kdl/tree.cpp
@@ -29,15 +29,18 @@ using namespace std;
 
 Tree::Tree() :
     nrOfJoints(0), nrOfSegments(0) {
-    segments.insert(make_pair("root", TreeElement::Root()));
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
 }
 
 Tree::Tree(const Tree& in) {
     segments.clear();
     nrOfSegments = 0;
     nrOfJoints = 0;
-
-    segments.insert(make_pair("root", TreeElement::Root()));
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
     this->addTree(in, "", "root");
 
 }
@@ -46,8 +49,9 @@ Tree& Tree::operator=(const Tree& in) {
     segments.clear();
     nrOfSegments = 0;
     nrOfJoints = 0;
-
-    segments.insert(make_pair("root", TreeElement::Root()));
+	TreeElement root;
+	std::pair<std::string, TreeElement> val("root", root);
+    segments.insert(val);
     this->addTree(in, "", "root");
     return *this;
 }
@@ -60,8 +64,10 @@ bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
         return false;
     pair<SegmentMap::iterator, bool> retval;
     //insert new element
-    retval = segments.insert(make_pair(segment_name, TreeElement(segment,
-            parent, nrOfJoints)));
+	TreeElement elem(segment, parent, nrOfJoints);
+	std::pair<std::string, TreeElement> val(segment_name, elem);
+
+    retval = segments.insert(val);
     //check if insertion succeeded
     if (!retval.second)
         return false;
diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp
index 4dfb55c7823..08c1aadc6de 100644
--- a/intern/itasc/kdl/tree.hpp
+++ b/intern/itasc/kdl/tree.hpp
@@ -43,7 +43,7 @@ namespace KDL
 #endif
     class TreeElement
     {
-    private:
+    public:
         TreeElement():q_nr(0)
         {};
     public: