From 8db6e682e9c0d55a34c64a490cbf29ff301c4973 Mon Sep 17 00:00:00 2001
From: Benoit Bolsee <benoit.bolsee@online.be>
Date: Mon, 4 Jun 2012 22:29:17 +0000
Subject: [PATCH] Fix [#31544]: iTaSC assertion when creating armature with no
 joint. This degenerated case can be obtained by having a single bone in the
 IK chain and locking all 3 axis. This case was causing an assert in the KDL
 library. The bug is fixed by simply not creating the IK scene in this case.

---
 intern/itasc/Armature.cpp    | 7 +++++--
 intern/itasc/Armature.hpp    | 2 +-
 intern/itasc/FixedObject.cpp | 5 +++--
 intern/itasc/FixedObject.hpp | 2 +-
 intern/itasc/MovingFrame.cpp | 3 ++-
 intern/itasc/MovingFrame.hpp | 2 +-
 intern/itasc/Object.hpp      | 2 +-
 intern/itasc/Scene.cpp       | 3 ++-
 8 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp
index 916b0bc7bf3..dd5c1921a98 100644
--- a/intern/itasc/Armature.cpp
+++ b/intern/itasc/Armature.cpp
@@ -369,11 +369,13 @@ int Armature::addEndEffector(const std::string& name)
 	return m_neffector++;
 }
 
-void Armature::finalize()
+bool Armature::finalize()
 {
 	unsigned int i, j, c;
 	if (m_finalized)
-		return;
+		return true;
+	if (m_njoint == 0)
+		return false;
 	initialize(m_njoint, m_noutput, m_neffector);
 	for (i=c=0; i<m_nconstraint; i++) {
 		JointConstraint_struct* pConstraint = m_constraints[i];
@@ -410,6 +412,7 @@ void Armature::finalize()
 	if (m_armlength < KDL::epsilon)
 		m_armlength = KDL::epsilon;
 	m_finalized = true;
+	return true;
 }
 
 void Armature::pushCache(const Timestamp& timestamp)
diff --git a/intern/itasc/Armature.hpp b/intern/itasc/Armature.hpp
index 0f4abeab4f0..63d1899f4e9 100644
--- a/intern/itasc/Armature.hpp
+++ b/intern/itasc/Armature.hpp
@@ -31,7 +31,7 @@ public:
 	bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
 	bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
 
-	virtual void finalize();
+	virtual bool finalize();
 
 	virtual int addEndEffector(const std::string& name);
 	virtual const Frame& getPose(const unsigned int end_effector);
diff --git a/intern/itasc/FixedObject.cpp b/intern/itasc/FixedObject.cpp
index 9dc4d69878a..2a1e66f9c6f 100644
--- a/intern/itasc/FixedObject.cpp
+++ b/intern/itasc/FixedObject.cpp
@@ -53,12 +53,13 @@ int FixedObject::addEndEffector(const std::string& name)
 	return -1;
 }
 
-void FixedObject::finalize()
+bool FixedObject::finalize()
 {
 	if (m_finalized)
-		return;
+		return true;
 	initialize(0, m_nframe);
 	m_finalized = true;
+	return true;
 }
 
 const Frame& FixedObject::getPose(const unsigned int frameIndex)
diff --git a/intern/itasc/FixedObject.hpp b/intern/itasc/FixedObject.hpp
index b4279d9adcb..ad26e7cb2d6 100644
--- a/intern/itasc/FixedObject.hpp
+++ b/intern/itasc/FixedObject.hpp
@@ -23,7 +23,7 @@ public:
 
 	virtual void updateCoordinates(const Timestamp& timestamp) {};
 	virtual int addEndEffector(const std::string& name);
-	virtual void finalize();
+	virtual bool finalize();
 	virtual const Frame& getPose(const unsigned int frameIndex);
 	virtual void updateKinematics(const Timestamp& timestamp) {};
 	virtual void pushCache(const Timestamp& timestamp) {};
diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp
index 3b0ee7842ca..90ebe091eb5 100644
--- a/intern/itasc/MovingFrame.cpp
+++ b/intern/itasc/MovingFrame.cpp
@@ -27,9 +27,10 @@ MovingFrame::~MovingFrame()
 {
 }
 
-void MovingFrame::finalize()
+bool MovingFrame::finalize()
 {
 	updateJacobian();
+	return true;
 }
 
 void MovingFrame::initCache(Cache *_cache)
diff --git a/intern/itasc/MovingFrame.hpp b/intern/itasc/MovingFrame.hpp
index 21cb344980a..d2a956d7312 100644
--- a/intern/itasc/MovingFrame.hpp
+++ b/intern/itasc/MovingFrame.hpp
@@ -28,7 +28,7 @@ public:
 	virtual void updateKinematics(const Timestamp& timestamp);
     virtual void pushCache(const Timestamp& timestamp);
 	virtual void initCache(Cache *_cache);
-	virtual void finalize();
+	virtual bool finalize();
 protected:
 	virtual void updateJacobian();
 
diff --git a/intern/itasc/Object.hpp b/intern/itasc/Object.hpp
index 5aa4346fc4b..bf80d83e5aa 100644
--- a/intern/itasc/Object.hpp
+++ b/intern/itasc/Object.hpp
@@ -33,7 +33,7 @@ public:
     virtual ~Object(){};
 
 	virtual int addEndEffector(const std::string& name){return 0;};
-	virtual void finalize(){};
+	virtual bool finalize(){return true;};
 	virtual const KDL::Frame& getPose(const unsigned int end_effector=0){return m_internalPose;};
     virtual const ObjectType getType(){return m_type;};
     virtual const unsigned int getNrOfCoordinates(){return 0;};
diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp
index 16f8551bfc7..877cd883208 100644
--- a/intern/itasc/Scene.cpp
+++ b/intern/itasc/Scene.cpp
@@ -91,7 +91,8 @@ bool Scene::setParam(SceneParam paramId, double value)
 bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame)
 {
 	// finalize the object before adding
-	object->finalize();
+	if (!object->finalize())
+		return false;
     //Check if Object is controlled or uncontrolled.
     if(object->getType()==Object::Controlled){
 		int baseFrameIndex = base->addEndEffector(baseFrame);