text: #include "KinTree.h"
  
  #include <iostream>
  
  #include "util/FileUtil.h"
  #include "sim/RBDUtil.h"
  
  const int cKinTree::gPosDim = 3;
  const int cKinTree::gRotDim = 4;
  const int cKinTree::gRootDim = gPosDim + gRotDim;
  const int cKinTree::gInvalidJointID = -1;
  
  // Json keys
  const std::string gJointTypeNames[cKinTree::eJointTypeMax] =
  {
  	"revolute",
  	"planar",
  	"prismatic",
  	"fixed",
  	"spherical",
  	"none"
  };
  
  const std::string gJointsKey = "Joints";
  const std::string gJointDescKeys[cKinTree::eJointDescMax] = 
  {
  	"Type",
  	"Parent",
  	"AttachX",
  	"AttachY",
  	"AttachZ",
  	"AttachThetaX",
  	"AttachThetaY",
  	"AttachThetaZ",
  	"LimLow0",
  	"LimLow1",
  	"LimLow2",
  	"LimHigh0",
  	"LimHigh1",
  	"LimHigh2",
  	"TorqueLim",
  	"ForceLim",
  	"IsEndEffector",
  	"DiffWeight",
  	"Offset"
  };
  
  const std::string gBodyDefsKey = "BodyDefs";
  const std::string gBodyDescKeys[cKinTree::eBodyParamMax] =
  {
  	"Shape",
  	"Mass",
  	"ColGroup",
  	"EnableFallContact",
  	"AttachX",
  	"AttachY",
  	"AttachZ",
  	"AttachThetaX",
  	"AttachThetaY",
  	"AttachThetaZ",
  	"Param0",
  	"Param1",
  	"Param2",
  	"ColorR",
  	"ColorG",
  	"ColorB",
  	"ColorA"
  };
  
  const std::string gDrawShapeDefsKey = "DrawShapeDefs";
  const std::string gDrawShapeDescKeys[cKinTree::eDrawShapeParamMax] =
  {
  	"Shape",
  	"ParentJoint",
  	"AttachX",
  	"AttachY",
  	"AttachZ",
  	"AttachThetaX",
  	"AttachThetaY",
  	"AttachThetaZ",
  	"Param0",
  	"Param1",
  	"Param2",
  	"ColorR",
  	"ColorG",
  	"ColorB",
  	"ColorA",
  	"MeshID"
  };
  
  int cKinTree::GetRoot(const Eigen::MatrixXd& joint_desc)
  {
  	// this should always be true right?
  	return 0;
  }
  
  void cKinTree::FindChildren(const Eigen::MatrixXd& joint_desc, int joint_id, Eigen::VectorXi& out_children)
  {
  	const int max_size = 128;
  	int children_buffer[max_size];
  	int num_children = 0;
  	int num_joints = GetNumJoints(joint_desc);
  
  	for (int i = 0; i < num_joints; ++i)
  	{
  		int parent = GetParent(joint_desc, i);
  		if (parent == joint_id)
  		{
  			children_buffer[num_children] = i;
  			++num_children;
  
  			if (num_children >= max_size)
  			{
  				printf("Too many children, max = %i", max_size);
  				assert(false);
  				return;
  			}
  		}
  	}
  
  	out_children.resize(num_children);
  	for (int i = 0; i < num_children; ++i)
  	{
  		out_children[i] = children_buffer[i];
  	}
  }
  
  bool cKinTree::LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs)
  {
  	bool succ = true;
  	std::string str;
  	
  	std::ifstream f_stream(char_file.c_str());
  	Json::Value root;
  	Json::Reader reader;
  	succ = reader.parse(f_stream, root);
  	f_stream.close();
  
  	if (succ)
  	{
  		succ = false;
  		if (!root[gBodyDefsKey].isNull())
  		{
  			Json::Value body_defs = root.get(gBodyDefsKey, 0);
  			int num_bodies = body_defs.size();
  
  			succ = true;
  			out_body_defs.resize(num_bodies, eBodyParamMax);
  			for (int b = 0; b < num_bodies; ++b)
  			{
  				tBodyDef curr_def = BuildBodyDef();
  				Json::Value body_json = body_defs.get(b, 0);
  				bool succ_def = ParseBodyDef(body_json, curr_def);
  
  				if (succ)
  				{
  					out_body_defs.row(b) = curr_def;
  				}
  				else
  				{
  					succ = false;
  					break;
  				}
  			}
  		}
  	}
  	
  	if (!succ)
  	{
  		printf("Failed to load body definition from %s\n", char_file.c_str());
  		succ = false;
  	}
  
  	return succ;
  }
  
  bool cKinTree::ParseBodyDef(const Json::Value& root, cKinTree::tBodyDef& out_def)
  {
  	std::string shape_str = root.get(gBodyDescKeys[eBodyParamShape], "").asString();
  	cShape::eShape shape;
  	bool succ = cShape::ParseShape(shape_str, shape);
  	if (succ)
  	{
  		out_def(eBodyParamShape) = static_cast<double>(static_cast<int>(shape));
  	}
  
  	for (int i = 0; i < eBodyParamMax; ++i)
  	{
  		const std::string& curr_key = gBodyDescKeys[i];
  		if (!root[curr_key].isNull()
  			&& root[curr_key].isNumeric())
  		{
  			Json::Value json_val = root[curr_key];
  			double val = json_val.asDouble();
  			out_def(i) = val;
  		}
  	}
  
  	return succ;
  }
  
  bool cKinTree::LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs)
  {
  	bool succ = true;
  	std::string str;
  
  	std::ifstream f_stream(char_file.c_str());
  	Json::Value root;
  	Json::Reader reader;
  	succ = reader.parse(f_stream, root);
  	f_stream.close();
  
  	if (succ)
  	{
  		if (!root[gDrawShapeDefsKey].isNull())
  		{
  			Json::Value shape_defs = root.get(gDrawShapeDefsKey, 0);
  			int num_shapes = shape_defs.size();
  
  			succ = true;
  			out_draw_defs.resize(num_shapes, eDrawShapeParamMax);
  			for (int b = 0; b < num_shapes; ++b)
  			{
  				tDrawShapeDef curr_def = BuildDrawShapeDef();
  				Json::Value shape_json = shape_defs.get(b, 0);
  				bool succ_def = ParseDrawShapeDef(shape_json, curr_def);
  
  				if (succ)
  				{
  					out_draw_defs.row(b) = curr_def;
  				}
  				else
  				{
  					succ = false;
  					break;
  				}
  			}
  		}
  	}
  
  	if (!succ)
  	{
  		printf("Failed to load draw shape definition from %s\n", char_file.c_str());
  		assert(false);
  	}
  
  	return succ;
  }
  
  bool cKinTree::ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def)
  {
  	std::string shape_str = root.get(gDrawShapeDescKeys[eDrawShapeShape], "").asString();
  	cShape::eShape shape;
  	bool succ = cShape::ParseShape(shape_str, shape);
  	if (succ)
  	{
  		out_def(eDrawShapeShape) = static_cast<double>(static_cast<int>(shape));
  	}
  
  	for (int i = 0; i < eDrawShapeParamMax; ++i)
  	{
  		const std::string& curr_key = gDrawShapeDescKeys[i];
  		if (!root[curr_key].isNull()
  			&& root[curr_key].isNumeric())
  		{
  			Json::Value json_val = root[curr_key];
  			double val = json_val.asDouble();
  			out_def(i) = val;
  		}
  	}
  
  	return succ;
  }
  
  tVector cKinTree::CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id)
  {
  	assert(IsValidBody(body_defs, part_id));
  	tMatrix body_joint_trans = BodyJointTrans(body_defs, part_id);
  	tMatrix joint_to_world_trans = JointWorldTrans(joint_mat, state, part_id);
  	
  	tVector attach_pt = joint_to_world_trans * body_joint_trans.col(3);
  	attach_pt[3] = 0;
  	return attach_pt;
  }
  
  tVector cKinTree::CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id)
  {
  	tVector attach_pt = cKinTree::GetBodyAttachPt(body_defs, part_id);
  	return CalcWorldVel(joint_mat, state, vel, part_id, attach_pt);
  }
  
  cShape::eShape cKinTree::GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	cShape::eShape shape = static_cast<cShape::eShape>(static_cast<int>(body_defs(part_id, cKinTree::eBodyParamShape)));
  	return shape;
  }
  
  tVector cKinTree::GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	const cKinTree::tBodyDef& def = body_defs.row(part_id);
  	tVector attach_pt = tVector(def(eBodyParamAttachX), def(eBodyParamAttachY), def(eBodyParamAttachZ), 0);
  	return attach_pt;
  }
  
  tVector cKinTree::GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	tVector attach_theta = tVector(body_defs(part_id, eBodyParamAttachThetaX),
  									body_defs(part_id, eBodyParamAttachThetaY),
  									body_defs(part_id, eBodyParamAttachThetaZ), 0);
  	return attach_theta;
  }
  
  void cKinTree::GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta)
  {
  	tVector theta = GetBodyAttachTheta(body_defs, part_id);
  	cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta);
  }
  
  double cKinTree::GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	double mass = body_defs(part_id, eBodyParamMass);
  	return mass;
  }
  
  int cKinTree::GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	int col_group = static_cast<int>(body_defs(part_id, eBodyParamColGroup));
  	return col_group;
  }
  
  bool cKinTree::GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	double fall_val = body_defs(part_id, eBodyParamEnableFallContact);
  	return fall_val != 0;
  }
  
  void cKinTree::SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs)
  {
  	out_body_defs(part_id, eBodyParamEnableFallContact) = (enable) ? 1 : 0;
  }
  
  tVector cKinTree::GetBodySize(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	const tBodyDef& def = body_defs.row(part_id);
  	tVector size = tVector(def(eBodyParam0), def(eBodyParam1), def(eBodyParam2), 0);
  	return size;
  }
  
  tVector cKinTree::GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	const tBodyDef& def = body_defs.row(part_id);
  	tVector col = tVector(def(eBodyColorR), def(eBodyColorG), def(eBodyColorB), def(eBodyColorA));
  	return col;
  }
  
  double cKinTree::CalcTotalMass(const Eigen::MatrixXd& body_defs)
  {
  	double total_mass = 0;
  	for (int i = 0; i < body_defs.rows(); ++i)
  	{
  		if (IsValidBody(body_defs, i))
  		{
  			double mass = cKinTree::GetBodyMass(body_defs, i);
  			total_mass += mass;
  		}
  	}
  	return total_mass;
  }
  
  bool cKinTree::IsValidBody(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	cShape::eShape shape = GetBodyShape(body_defs, part_id);
  	if (shape != cShape::eShapeNull)
  	{
  		return true;
  	}
  	return false;
  }
  
  tVector cKinTree::GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	cShape::eShape shape = GetBodyShape(body_defs, part_id);
  	tVector com = tVector::Zero();
  	switch (shape)
  	{
  	case cShape::eShapeBox:
  	case cShape::eShapeCapsule:
  	case cShape::eShapeSphere:
  	case cShape::eShapeCylinder:
  		break;
  	default:
  		assert(false); // unsupported
  		break;
  	}
  
  	return com;
  }
  
  int cKinTree::GetDrawShapeParentJoint(const tDrawShapeDef& shape)
  {
  	return static_cast<int>(shape[eDrawShapeParentJoint]);
  }
  
  tVector cKinTree::GetDrawShapeAttachPt(const tDrawShapeDef& shape)
  {
  	return tVector(shape[eDrawShapeAttachX], shape[eDrawShapeAttachY], shape[cKinTree::eDrawShapeAttachZ], 0);
  }
  
  tVector cKinTree::GetDrawShapeAttachTheta(const tDrawShapeDef& shape)
  {
  	return tVector(shape[eDrawShapeAttachThetaX], shape[eDrawShapeAttachThetaY], shape[eDrawShapeAttachThetaZ], 0);
  }
  
  void cKinTree::GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta)
  {
  	tVector theta = GetDrawShapeAttachTheta(shape);
  	cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta);
  }
  
  tVector cKinTree::GetDrawShapeColor(const tDrawShapeDef& shape)
  {
  	return tVector(shape[eDrawShapeColorR], shape[eDrawShapeColorG], shape[eDrawShapeColorB], shape[cKinTree::eDrawShapeColorA]);
  }
  
  int cKinTree::GetDrawShapeMeshID(const tDrawShapeDef& shape)
  {
  	return static_cast<int>(shape[eDrawShapeMeshID]);
  }
  
  void cKinTree::CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta)
  {
  	tMatrix mat = BodyWorldTrans(joint_mat, body_defs, state, part_id);
  	cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta);
  }
  
  bool cKinTree::Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat)
  {
  	bool succ = false;
  
  	if (!root[gJointsKey].isNull())
  	{
  		Json::Value joints = root[gJointsKey];
  		int num_joints = joints.size();
  
  		out_joint_mat.resize(num_joints, eJointDescMax);
  		
  		for (int j = 0; j < num_joints; ++j)
  		{
  			tJointDesc curr_joint_desc = tJointDesc::Zero();
  
  			Json::Value joint_json = joints.get(j, 0);
  			succ = ParseJoint(joint_json, curr_joint_desc);
  			if (succ)
  			{
  				out_joint_mat.row(j) = curr_joint_desc;
  			}
  			else
  			{
  				printf("Failed to parse joint %i\n", j);
  				return false;
  			}
  		}
  
  		for (int j = 0; j < num_joints; ++j)
  		{
  			const auto& curr_desc = out_joint_mat.row(j);
  			int parent_id = static_cast<int>(curr_desc(eJointDescParent));
  			if (parent_id >= j)
  			{
  				printf("Parent id must be < child id, parent id: %i, child id: %i\n", parent_id, j);
  				out_joint_mat.resize(0, 0);
  				assert(false);
  
  				return false;
  			}
  
  			out_joint_mat.row(j) = curr_desc;
  		}
  
  		PostProcessJointMat(out_joint_mat);
  	}
  
  	return succ;
  }
  
  bool cKinTree::HasValidRoot(const Eigen::MatrixXd& joint_desc)
  {
  	int root = GetRoot(joint_desc);
  	return root != gInvalidJointID;
  }
  
  tVector cKinTree::GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state)
  {
  	int root_id = GetRoot(joint_mat);
  	tVector pos = tVector::Zero();
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	pos.segment(0, gPosDim) = state.segment(param_offset, gPosDim);
  	return pos;
  }
  
  void cKinTree::SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state)
  {
  	int root_id = GetRoot(joint_mat);
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	out_state.segment(param_offset, gPosDim) = pos.segment(0, gPosDim);
  }
  
  tQuaternion cKinTree::GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state)
  {
  	int root_id = GetRoot(joint_mat);
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	tQuaternion rot = cMathUtil::VecToQuat(state.segment(param_offset + gPosDim, gRotDim));
  	return rot;
  }
  
  void cKinTree::SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state)
  {
  	int root_id = GetRoot(joint_mat);
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	out_state.segment(param_offset + gPosDim, gRotDim) = cMathUtil::QuatToVec(rot).segment(0, gRotDim);
  }
  
  tVector cKinTree::GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel)
  {
  	int root_id = GetRoot(joint_mat);
  	tVector pos = tVector::Zero();
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	pos.segment(0, gPosDim) = vel.segment(param_offset, gPosDim);
  	return pos;
  }
  
  void cKinTree::SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel)
  {
  	int root_id = GetRoot(joint_mat);
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	out_vel.segment(param_offset, gPosDim) = vel.segment(0, gPosDim);
  }
  
  tVector cKinTree::GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel)
  {
  	int root_id = GetRoot(joint_mat);
  	tVector ang_vel = tVector::Zero();
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	ang_vel.segment(0, gRotDim) = vel.segment(param_offset + gPosDim, gRotDim);
  	return ang_vel;
  }
  
  void cKinTree::SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel)
  {
  	int root_id = GetRoot(joint_mat);
  	int param_offset = GetParamOffset(joint_mat, root_id);
  	out_vel.segment(param_offset + gPosDim, gRotDim) = ang_vel.segment(0, gRotDim);
  }
  
  tVector cKinTree::CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tVector pos = LocalToWorldPos(joint_mat, state, joint_id, tVector::Zero());
  	return pos;
  }
  
  tVector cKinTree::LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt)
  {
  	tMatrix local_to_world_trans = JointWorldTrans(joint_mat, state, parent_id);
  	tVector pos = attach_pt;
  	pos[3] = 1;
  	pos = local_to_world_trans * pos;
  	pos[3] = 0;
  
  	return pos;
  }
  
  tQuaternion cKinTree::CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix mat = JointWorldTrans(joint_mat, state, joint_id);
  	return cMathUtil::RotMatToQuaternion(mat);
  }
  
  void cKinTree::CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id,
  									tVector& out_axis, double& out_theta)
  {
  	tMatrix mat = JointWorldTrans(joint_mat, state, joint_id);
  	cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta);
  }
  
  tVector cKinTree::CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id)
  {
  	return CalcWorldVel(joint_mat, state, vel, joint_id, tVector::Zero());
  }
  
  tVector cKinTree::CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt)
  {
  	cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id);
  	tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt);
  	cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos);
  	sv = cSpAlg::ApplyTransM(world_to_pt, sv);
  	return cSpAlg::GetV(sv);
  }
  
  tVector cKinTree::CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id)
  {
  	return CalcWorldAngularVel(joint_mat, state, vel, joint_id, tVector::Zero());
  }
  
  tVector cKinTree::CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt)
  {
  	cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id);
  	tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt);
  	cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos);
  	sv = cSpAlg::ApplyTransM(world_to_pt, sv);
  	return cSpAlg::GetOmega(sv);
  }
  
  int cKinTree::GetNumJoints(const Eigen::MatrixXd& joint_mat)
  {
  	return static_cast<int>(joint_mat.rows());
  }
  
  int cKinTree::GetNumDof(const Eigen::MatrixXd& joint_mat)
  {
  	int num_joints = GetNumJoints(joint_mat);
  	int num_dof = cKinTree::GetParamOffset(joint_mat, num_joints - 1) + cKinTree::GetParamSize(joint_mat, num_joints - 1);
  	return num_dof;
  }
  
  void cKinTree::ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose)
  {
  	int root_id = GetRoot(joint_mat);
  	int num_joints = cKinTree::GetNumJoints(joint_mat);
  	out_pose += step;
  }
  
  
  Eigen::VectorXi cKinTree::FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end)
  {
  	Eigen::VectorXi chain;
  
  	const int max_length = 128;
  	int chain_buffer[max_length];
  	int buffer_idx = 0;
  
  	if (joint_beg == joint_end)
  	{
  		Eigen::VectorXi chain(1);
  		chain[0] = joint_beg;
  	}
  
  	int common_ancestor = gInvalidJointID;
  	int curr_id = joint_beg;
  	int end_len = 0;
  	while (curr_id != gInvalidJointID)
  	{
  		chain_buffer[buffer_idx] = curr_id;
  		++buffer_idx;
  
  		if (buffer_idx >= max_length)
  		{
  			printf("Exceeded maximum chain length %i\n", max_length);
  			assert(false);
  			return chain;
  		}
  
  		bool is_ancestor = IsAncestor(joint_mat, joint_end, curr_id, end_len);
  		if (is_ancestor)
  		{
  			common_ancestor = curr_id;
  			break;
  		}
  		else
  		{
  			curr_id = GetParent(joint_mat, curr_id);
  		}
  	}
  
  	bool found = common_ancestor != gInvalidJointID;
  	// tree should always connected?
  	assert(found);
  
  	if (found)
  	{
  		chain.resize(buffer_idx + end_len);
  		for (int i = 0; i < buffer_idx; ++i)
  		{
  			chain[i] = chain_buffer[i];
  		}
  
  		int idx = buffer_idx;
  		curr_id = joint_end;
  		while (curr_id != common_ancestor)
  		{
  			chain[idx] = curr_id;
  			curr_id = GetParent(joint_mat, curr_id);
  			++idx;
  		}
  
  		int num_flips = static_cast<int>(chain.size()) - buffer_idx;
  		chain.block(buffer_idx, 0, num_flips, 1).reverseInPlace();
  	}
  
  	return chain;
  }
  
  bool cKinTree::IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len)
  {
  	int curr_id = child_joint;
  	out_len = 0;
  	while (curr_id != gInvalidJointID)
  	{
  		if (curr_id == ancestor_joint)
  		{
  			return true;
  		}
  		else
  		{
  			curr_id = GetParent(joint_mat, curr_id);
  			++out_len;
  		}
  	}
  	return false;
  }
  
  double cKinTree::CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain)
  {
  	double len = 0;
  	int num_joints = static_cast<int>(chain.size());
  	for (int i = 1; i < num_joints; ++i)
  	{
  		int curr_id = chain(i);
  		int prev_id = chain(i - 1);
  
  		if (prev_id != gInvalidJointID)
  		{
  			int prev_parent = GetParent(joint_mat, prev_id);
  			bool is_parent = (prev_parent == curr_id);
  			if (is_parent)
  			{
  				double curr_len = CalcLinkLength(joint_mat, prev_id);
  				len += curr_len;
  			}
  		}
  
  		if (curr_id != gInvalidJointID)
  		{
  			int curr_parent = GetParent(joint_mat, curr_id);
  			bool is_child = (curr_parent == prev_id);
  			if (is_child)
  			{
  				double curr_len = CalcLinkLength(joint_mat, curr_id);
  				len += curr_len;
  			}
  		}
  	}
  
  	return len;
  }
  
  void cKinTree::CalcAABB(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max)
  {
  	out_min[0] = std::numeric_limits<double>::infinity();
  	out_min[1] = std::numeric_limits<double>::infinity();
  	out_min[2] = std::numeric_limits<double>::infinity();
  
  	out_max[0] = -std::numeric_limits<double>::infinity();
  	out_max[1] = -std::numeric_limits<double>::infinity();
  	out_max[2] = -std::numeric_limits<double>::infinity();
  
  	for (int i = 0; i < GetNumJoints(joint_desc); ++i)
  	{
  		tVector pos = CalcJointWorldPos(joint_desc, state, i);
  		out_min = out_min.cwiseMin(pos);
  		out_max = out_max.cwiseMax(pos);
  	}
  }
  
  int cKinTree::GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	int offset = static_cast<int>(joint_mat(joint_id, eJointDescParamOffset));
  	return offset;
  }
  
  int cKinTree::GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	eJointType joint_type = cKinTree::GetJointType(joint_mat, joint_id);
  	bool is_root = cKinTree::IsRoot(joint_mat, joint_id);
  	int size = (is_root) ? gRootDim : GetJointParamSize(joint_type);
  	return size;
  }
  
  int cKinTree::GetJointParamSize(eJointType joint_type)
  {
  	int size = 0;
  	switch (joint_type)
  	{
  	case eJointTypeRevolute:
  		size = 1;
  		break;
  	case eJointTypePrismatic:
  		size = 1;
  		break;
  	case eJointTypePlanar:
  		size = 3;
  		break;
  	case eJointTypeFixed:
  		size = 0;
  		break;
  	case eJointTypeSpherical:
  		size = 4;
  		break;
  	default:
  		assert(false); // unsupported joint type
  		break;
  	}
  
  	return size;
  }
  
  void cKinTree::GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params)
  {
  	int offset = GetParamOffset(joint_mat, j);
  	int dim = GetParamSize(joint_mat, j);
  	if (dim > 0)
  	{
  		out_params = state.block(offset, 0, dim, 1);
  	}
  	else
  	{
  		out_params = Eigen::VectorXd::Zero(1);
  	}
  }
  
  void cKinTree::SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state)
  {
  	int offset = GetParamOffset(joint_mat, j);
  	int dim = GetParamSize(joint_mat, j);
  	assert(dim == params.size());
  	out_state.block(offset, 0, dim, 1) = params;
  }
  
  cKinTree::eJointType cKinTree::GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	eJointType type = static_cast<eJointType>(static_cast<int>(joint_mat(joint_id, cKinTree::eJointDescType)));
  	return type;
  }
  
  int cKinTree::GetParent(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	int parent = static_cast<int>(joint_mat(joint_id, cKinTree::eJointDescParent));
  	assert(parent < joint_id); // joints should always be ordered as such
  								// since some algorithms will assume this ordering
  	return parent;
  }
  
  bool cKinTree::HasParent(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	int parent = GetParent(joint_mat, joint_id);
  	return parent != gInvalidJointID;
  }
  
  bool cKinTree::IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	return !HasParent(joint_mat, joint_id);
  }
  
  bool cKinTree::IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	return !IsRoot(joint_mat, joint_id);
  }
  
  double cKinTree::GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	double torque_lim = joint_mat(joint_id, cKinTree::eJointDescTorqueLim);
  	return torque_lim;
  }
  
  double cKinTree::GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	double force_lim = joint_mat(joint_id, cKinTree::eJointDescForceLim);
  	return force_lim;
  }
  
  bool cKinTree::IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	double end_eff_val = joint_mat(joint_id, cKinTree::eJointDescIsEndEffector);
  	return end_eff_val != 0;
  }
  
  tVector cKinTree::GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	return tVector(joint_mat(joint_id, cKinTree::eJointDescLimLow0), 
  					joint_mat(joint_id, cKinTree::eJointDescLimLow1), 
  					joint_mat(joint_id, cKinTree::eJointDescLimLow2), 1);
  }
  
  tVector cKinTree::GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	return tVector(joint_mat(joint_id, cKinTree::eJointDescLimHigh0), 
  					joint_mat(joint_id, cKinTree::eJointDescLimHigh1),
  					joint_mat(joint_id, cKinTree::eJointDescLimHigh2), 0);
  }
  
  double cKinTree::GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	return joint_mat(joint_id, cKinTree::eJointDescDiffWeight);
  }
  
  double cKinTree::CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	tVector attach_pt = GetAttachPt(joint_mat, joint_id);
  	bool is_root = IsRoot(joint_mat, joint_id);
  	double len = (is_root) ? 0 : attach_pt.norm();
  	return len;
  }
  
  tVector cKinTree::GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	tVector attach_pt = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachX),
  								joint_mat(joint_id, cKinTree::eJointDescAttachY),
  								joint_mat(joint_id, cKinTree::eJointDescAttachZ), 0);
  	return attach_pt;
  }
  
  tVector cKinTree::GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	tVector attach_theta = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachThetaX),
  									joint_mat(joint_id, cKinTree::eJointDescAttachThetaY),
  									joint_mat(joint_id, cKinTree::eJointDescAttachThetaZ), 0);
  	return attach_theta;
  }
  
  void cKinTree::CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths)
  {
  	int num_joints = static_cast<int>(joint_mat.rows());
  	out_lengths = Eigen::VectorXd::Zero(num_joints);
  
  	for (int j = num_joints - 1; j >= 0; --j)
  	{
  		int parent_id = GetParent(joint_mat, j);
  		if (parent_id != gInvalidJointID)
  		{
  			double curr_val = out_lengths(j);
  			double len = CalcLinkLength(joint_mat, j);
  			double& parent_val = out_lengths(parent_id);
  
  			if (parent_val < len + curr_val)
  			{
  				parent_val = len + curr_val;
  			}
  		}
  	}
  }
  
  void cKinTree::CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses)
  {
  	int num_joints = static_cast<int>(joint_mat.rows());
  	out_masses = Eigen::VectorXd::Zero(num_joints);
  
  	for (int j = num_joints - 1; j >= 0; --j)
  	{
  		double& curr_val = out_masses(j);
  		double mass = GetBodyMass(body_defs, j);
  		curr_val += mass;
  
  		int parent_id = GetParent(joint_mat, j);
  		if (parent_id != gInvalidJointID)
  		{
  			double& parent_val = out_masses(parent_id);
  			parent_val += curr_val;
  		}
  	}
  }
  
  bool cKinTree::ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc)
  {
  	out_joint_desc = BuildJointDesc();
  	eJointType joint_type = eJointTypeNone;
  	const Json::Value& type_json = root[gJointDescKeys[eJointDescType]];
  	if (type_json.isNull())
  	{
  		printf("No joint type specified\n");
  	}
  	else
  	{
  		std::string type_str = type_json.asString();
  		ParseJointType(type_str, joint_type);
  		out_joint_desc[eJointDescType] = static_cast<double>(static_cast<int>(joint_type));
  	}
  
  	for (int i = 0; i < eJointDescMax; ++i)
  	{
  		if (i != eJointDescType)
  		{
  			const std::string& key = gJointDescKeys[i];
  			if (!root[key].isNull())
  			{
  				out_joint_desc[i] = root[key].asDouble();
  			}
  		}
  	}
  	return true;
  }
  
  bool cKinTree::ParseJointType(const std::string& type_str, eJointType& out_joint_type)
  {
  	for (int i = 0; i < eJointTypeMax; ++i)
  	{
  		const std::string& name = gJointTypeNames[i];
  		if (type_str == name)
  		{
  			out_joint_type = static_cast<eJointType>(i);
  			return true;
  		}
  	}
  	printf("Unsupported joint type: %s\n", type_str.c_str());
  	assert(false); // unsupported joint type
  	return false;
  }
  
  void cKinTree::PostProcessJointMat(Eigen::MatrixXd& out_joint_mat)
  {
  	int num_joints = GetNumJoints(out_joint_mat);
  	int offset = 0;
  	for (int j = 0; j < num_joints; ++j)
  	{
  		int curr_size = GetParamSize(out_joint_mat, j);
  		out_joint_mat(j, eJointDescParamOffset) = offset;
  		offset += curr_size;
  	}
  	int root_id = GetRoot(out_joint_mat);
  
  	out_joint_mat(root_id, eJointDescAttachX) = 0;
  	out_joint_mat(root_id, eJointDescAttachY) = 0;
  	out_joint_mat(root_id, eJointDescAttachZ) = 0;
  }
  
  tMatrix cKinTree::BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id)
  {
  	// child to parent
  	tVector attach_pt = GetAttachPt(joint_mat, joint_id);
  	tVector attach_theta = GetAttachTheta(joint_mat, joint_id);
  	tMatrix mat = cMathUtil::RotateMat(attach_theta);
  	mat(0, 3) = attach_pt[0];
  	mat(1, 3) = attach_pt[1];
  	mat(2, 3) = attach_pt[2];
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix mat;
  	eJointType j_type = GetJointType(joint_mat, joint_id);
  	bool is_root = IsRoot(joint_mat, joint_id);
  
  	if (is_root)
  	{
  		mat = ChildParentTransRoot(joint_mat, state, joint_id);
  	}
  	else
  	{
  		switch (j_type)
  		{
  		case eJointTypeRevolute:
  			mat = ChildParentTransRevolute(joint_mat, state, joint_id);
  			break;
  		case eJointTypePrismatic:
  			mat = ChildParentTransPrismatic(joint_mat, state, joint_id);
  			break;
  		case eJointTypePlanar:
  			mat = ChildParentTransPlanar(joint_mat, state, joint_id);
  			break;
  		case eJointTypeFixed:
  			mat = ChildParentTransFixed(joint_mat, state, joint_id);
  			break;
  		case eJointTypeSpherical:
  			mat = ChildParentTransSpherical(joint_mat, state, joint_id);
  			break;
  		default:
  			break;
  		}
  	}
  	
  	return mat;
  }
  
  tMatrix cKinTree::ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix mat = ChildParentTrans(joint_mat, state, joint_id);
  	mat = cMathUtil::InvRigidMat(mat);
  	return mat;
  }
  
  tMatrix cKinTree::JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix m = tMatrix::Identity();
  	int curr_id = joint_id;
  	assert(joint_id != gInvalidIdx); // invalid joint
  	while (curr_id != gInvalidJointID)
  	{
  		tMatrix child_parent_mat = ChildParentTrans(joint_mat, state, curr_id);
  		m = child_parent_mat * m;
  		curr_id = GetParent(joint_mat, curr_id);
  	}
  
  	return m;
  }
  
  tMatrix cKinTree::WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix m = JointWorldTrans(joint_mat, state, joint_id);
  	m = cMathUtil::InvRigidMat(m);
  	return m;
  }
  
  tMatrix cKinTree::BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id)
  {
  	tMatrix body_trans = BodyJointTrans(body_defs, part_id);
  	tMatrix joint_trans = JointWorldTrans(joint_mat, state, part_id);
  	body_trans = joint_trans * body_trans;
  	return body_trans;
  }
  
  tMatrix cKinTree::BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id)
  {
  	tVector attach_pt = GetBodyAttachPt(body_defs, part_id);
  	tVector euler = GetBodyAttachTheta(body_defs, part_id);
  	tVector com = GetBodyLocalCoM(body_defs, part_id);
  
  	tMatrix rot = cMathUtil::RotateMat(euler);
  	tMatrix trans = cMathUtil::TranslateMat(attach_pt + com);
  	tMatrix m = trans * rot;
  	return m;
  }
  
  cKinTree::tJointDesc cKinTree::BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt)
  {
  	tJointDesc desc = BuildJointDesc();
  	desc(eJointDescType) = static_cast<double>(joint_type);
  	desc(eJointDescParent) = parent_id;
  	desc(eJointDescAttachX) = attach_pt[0];
  	desc(eJointDescAttachY) = attach_pt[1];
  	desc(eJointDescAttachZ) = attach_pt[2];
  	
  	return desc;
  }
  
  cKinTree::tJointDesc cKinTree::BuildJointDesc()
  {
  	tJointDesc desc;
  	desc(eJointDescType) = static_cast<double>(eJointTypeRevolute);
  	desc(eJointDescParent) = gInvalidIdx;
  	desc(eJointDescAttachX) = 0;
  	desc(eJointDescAttachY) = 0;
  	desc(eJointDescAttachZ) = 0;
  	desc(eJointDescAttachThetaX) = 0;
  	desc(eJointDescAttachThetaY) = 0;
  	desc(eJointDescAttachThetaZ) = 0;
  	desc(eJointDescLimLow0) = 1;
  	desc(eJointDescLimLow1) = 1;
  	desc(eJointDescLimLow2) = 1;
  	desc(eJointDescLimHigh0) = 0;
  	desc(eJointDescLimHigh1) = 0;
  	desc(eJointDescLimHigh2) = 0;
  	desc(eJointDescIsEndEffector) = 0;
  	desc(eJointDescTorqueLim) = std::numeric_limits<double>::infinity();
  	desc(eJointDescForceLim) = std::numeric_limits<double>::infinity();
  	desc(eJointDescDiffWeight) = 1;
  	desc(eJointDescParamOffset) = 0;
  
  	return desc;
  }
  
  cKinTree::tBodyDef cKinTree::BuildBodyDef()
  {
  	tBodyDef def;
  	def(eBodyParamShape) = static_cast<double>(cShape::eShapeNull);
  	def(eBodyParamMass) = 0;
  	def(eBodyParamColGroup) = gInvalidIdx;
  	def(eBodyParamEnableFallContact) = 0;
  	def(eBodyParamAttachX) = 0;
  	def(eBodyParamAttachY) = 0;
  	def(eBodyParamAttachZ) = 0;
  	def(eBodyParamAttachThetaX) = 0;
  	def(eBodyParamAttachThetaY) = 0;
  	def(eBodyParamAttachThetaZ) = 0;
  	def(eBodyParam0) = 0;
  	def(eBodyParam1) = 0;
  	def(eBodyParam2) = 0;
  	def(eBodyColorR) = 0;
  	def(eBodyColorG) = 0;
  	def(eBodyColorB) = 0;
  	def(eBodyColorA) = 1;
  	return def;
  }
  
  cKinTree::tDrawShapeDef cKinTree::BuildDrawShapeDef()
  {
  	tDrawShapeDef def;
  	def(eDrawShapeShape) = static_cast<double>(cShape::eShapeNull);
  	def(eDrawShapeParentJoint) = gInvalidIdx;
  	def(eDrawShapeAttachX) = 0;
  	def(eDrawShapeAttachY) = 0;
  	def(eDrawShapeAttachZ) = 0;
  	def(eDrawShapeAttachThetaX) = 0;
  	def(eDrawShapeAttachThetaY) = 0;
  	def(eDrawShapeAttachThetaZ) = 0;
  	def(eDrawShapeParam0) = 0;
  	def(eDrawShapeParam1) = 0;
  	def(eDrawShapeParam2) = 0;
  	def(eDrawShapeColorR) = 0;
  	def(eDrawShapeColorG) = 0;
  	def(eDrawShapeColorB) = 0;
  	def(eDrawShapeColorA) = 1;
  	def(eDrawShapeMeshID) = gInvalidIdx;
  	return def;
  }
  
  void cKinTree::BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
  {
  	int num_dof = GetNumDof(joint_mat);
  	out_pose = Eigen::VectorXd::Zero(num_dof);
  
  	int num_joints = GetNumJoints(joint_mat);
  
  	int root_id = GetRoot(joint_mat);
  	Eigen::VectorXd root_pose;
  	BuildDefaultPoseRoot(joint_mat, root_pose);
  	SetJointParams(joint_mat, root_id, root_pose, out_pose);
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		eJointType joint_type = GetJointType(joint_mat, j);
  		Eigen::VectorXd joint_pose;
  		switch (joint_type)
  		{
  		case eJointTypeRevolute:
  			BuildDefaultPoseRevolute(joint_pose);
  			break;
  		case eJointTypePrismatic:
  			BuildDefaultPosePrismatic(joint_pose);
  			break;
  		case eJointTypePlanar:
  			BuildDefaultPosePlanar(joint_pose);
  			break;
  		case eJointTypeFixed:
  			BuildDefaultPoseFixed(joint_pose);
  			break;
  		case eJointTypeSpherical:
  			BuildDefaultPoseSpherical(joint_pose);
  			break;
  		default:
  			assert(false); // unsupported joint type
  			break;
  		}
  
  		SetJointParams(joint_mat, j, joint_pose, out_pose);
  	}
  }
  
  void cKinTree::BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel)
  {
  	int num_dof = GetNumDof(joint_mat);
  	out_vel = Eigen::VectorXd::Zero(num_dof);
  
  	int num_joints = GetNumJoints(joint_mat);
  
  	int root_id = GetRoot(joint_mat);
  	Eigen::VectorXd root_pose;
  	BuildDefaultVelRoot(joint_mat, root_pose);
  	SetJointParams(joint_mat, root_id, root_pose, out_vel);
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		eJointType joint_type = GetJointType(joint_mat, j);
  		Eigen::VectorXd joint_pose;
  		switch (joint_type)
  		{
  		case eJointTypeRevolute:
  			BuildDefaultVelRevolute(joint_pose);
  			break;
  		case eJointTypePrismatic:
  			BuildDefaultVelPrismatic(joint_pose);
  			break;
  		case eJointTypePlanar:
  			BuildDefaultVelPlanar(joint_pose);
  			break;
  		case eJointTypeFixed:
  			BuildDefaultVelFixed(joint_pose);
  			break;
  		case eJointTypeSpherical:
  			BuildDefaultVelSpherical(joint_pose);
  			break;
  		default:
  			assert(false); // unsupported joint type
  			break;
  		}
  
  		SetJointParams(joint_mat, j, joint_pose, out_vel);
  	}
  }
  
  void cKinTree::CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff)
  {
  	int num_joints = GetNumJoints(joint_mat);
  	assert(out_diff.size() == pose0.size());
  	assert(pose1.size() == pose0.size());
  
  	out_diff.resize(pose1.size());
  	for (int j = 0; j < num_joints; ++j)
  	{
  		Eigen::VectorXd curr_diff;
  		CalcJointPoseDiff(joint_mat, j, pose0, pose1, curr_diff);
  
  		int param_offset = GetParamOffset(joint_mat, j);
  		int param_size = GetParamSize(joint_mat, j);
  		out_diff.segment(param_offset, param_size) = curr_diff;
  	}
  }
  
  tVector cKinTree::CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tVector root_pos0 = GetRootPos(joint_mat, pose0);
  	tVector root_pos1 = GetRootPos(joint_mat, pose1);
  	return root_pos1 - root_pos0;
  }
  
  tQuaternion cKinTree::CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
  	tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
  	return cMathUtil::QuatDiff(root_rot0, root_rot1);
  }
  
  double cKinTree::CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	bool is_root = IsRoot(joint_mat, joint_id);
  	double err = 0;
  	if (is_root)
  	{
  		double root_pos_err = CalcRootPosErr(joint_mat, pose0, pose1);
  		double root_rot_err = CalcRootRotErr(joint_mat, pose0, pose1);
  		err = root_pos_err + root_rot_err;
  	}
  	else
  	{
  		Eigen::VectorXd diff;
  		CalcJointPoseDiff(joint_mat, joint_id, pose0, pose1, diff);
  
  		eJointType joint_type = GetJointType(joint_mat, joint_id);
  		switch (joint_type)
  		{
  		case eJointTypeSpherical:
  		{
  			tQuaternion dq = cMathUtil::VecToQuat(diff);
  			double theta = cMathUtil::QuatTheta(dq);
  			err = theta * theta;
  			break;
  		}
  		default:
  			err = diff.squaredNorm();
  			break;
  		}
  	}
  	return err;
  }
  
  double cKinTree::CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tVector diff = CalcRootPosDiff(joint_mat, pose0, pose1);
  	return diff.squaredNorm();
  }
  
  double cKinTree::CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tQuaternion diff = CalcRootRotDiff(joint_mat, pose0, pose1);
  	double theta = cMathUtil::QuatTheta(diff);
  	return theta * theta;
  }
  
  
  void cKinTree::CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff)
  {
  	int num_joints = GetNumJoints(joint_mat);
  	assert(vel0.size() == vel1.size());
  
  	out_diff.resize(vel0.size());
  	for (int j = 0; j < num_joints; ++j)
  	{
  		Eigen::VectorXd curr_diff;
  		CalcJointVelDiff(joint_mat, j, vel0, vel1, curr_diff);
  
  		int param_offset = GetParamOffset(joint_mat, j);
  		int param_size = GetParamSize(joint_mat, j);
  		out_diff.segment(param_offset, param_size) = curr_diff;
  	}
  }
  
  tVector cKinTree::CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tVector root_vel0 = GetRootVel(joint_mat, pose0);
  	tVector root_vel1 = GetRootVel(joint_mat, pose1);
  	return root_vel1 - root_vel0;
  }
  
  tVector cKinTree::CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
  {
  	tVector root_ang_vel0 = GetRootAngVel(joint_mat, pose0);
  	tVector root_ang_vel1 = GetRootAngVel(joint_mat, pose1);
  	return root_ang_vel1 - root_ang_vel0;
  }
  
  double cKinTree::CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
  {
  	bool is_root = IsRoot(joint_mat, joint_id);
  	double err = 0;
  	if (is_root)
  	{
  		double root_vel_err = CalcRootVelErr(joint_mat, vel0, vel1);
  		double root_ang_vel_err = CalcRootAngVelErr(joint_mat, vel0, vel1);
  		err = root_vel_err + root_ang_vel_err;
  	}
  	else
  	{
  		Eigen::VectorXd diff;
  		CalcJointVelDiff(joint_mat, joint_id, vel0, vel1, diff);
  		err = diff.squaredNorm();
  	}
  	return err;
  }
  
  double cKinTree::CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
  {
  	tVector diff = CalcRootVelDiff(joint_mat, vel0, vel1);
  	return diff.squaredNorm();
  }
  
  double cKinTree::CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
  {
  	tVector diff = CalcRootAngVelDiff(joint_mat, vel0, vel1);
  	return diff.squaredNorm();
  }
  
  
  void cKinTree::CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff)
  {
  	int param_offset = GetParamOffset(joint_mat, joint_id);
  	int param_size = GetParamSize(joint_mat, joint_id);
  	bool is_root = cKinTree::IsRoot(joint_mat, joint_id);
  
  	if (is_root)
  	{
  		tVector root_pos_diff = CalcRootPosDiff(joint_mat, pose0, pose1);
  		tQuaternion root_rot_diff = CalcRootRotDiff(joint_mat, pose0, pose1);
  		out_diff.resize(gRootDim);
  		out_diff.segment(0, gPosDim) = root_pos_diff.segment(0, gPosDim);
  		out_diff.segment(gPosDim, gRotDim) = cMathUtil::QuatToVec(root_rot_diff);
  	}
  	else
  	{
  		eJointType joint_type = GetJointType(joint_mat, joint_id);
  		switch (joint_type)
  		{
  		case eJointTypeSpherical:
  		{
  			tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size));
  			tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size));
  			tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1);
  			out_diff = cMathUtil::QuatToVec(q_diff);
  			break;
  		}
  		default:
  			out_diff = pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size);
  			break;
  		}
  	}
  }
  
  void cKinTree::CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff)
  {
  	int param_offset = GetParamOffset(joint_mat, joint_id);
  	int param_size = GetParamSize(joint_mat, joint_id);
  	out_diff = vel1.segment(param_offset, param_size) - vel0.segment(param_offset, param_size);
  }
  
  void cKinTree::CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel)
  {
  	assert(pose0.size() == pose1.size());
  	out_vel.resize(pose0.size());
  
  	int num_joints = GetNumJoints(joint_mat);
  	tVector root_pos0 = GetRootPos(joint_mat, pose0);
  	tVector root_pos1 = GetRootPos(joint_mat, pose1);
  	tVector root_vel = (root_pos1 - root_pos0) / dt;
  
  	tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
  	tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
  	tVector root_rot_vel = cMathUtil::CalcQuaternionVel(root_rot0, root_rot1, dt);
  
  	cKinTree::SetRootVel(joint_mat, root_vel, out_vel);
  	cKinTree::SetRootAngVel(joint_mat, root_rot_vel, out_vel);
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		int param_offset = GetParamOffset(joint_mat, j);
  		int param_size = GetParamSize(joint_mat, j);
  		eJointType joint_type = GetJointType(joint_mat, j);
  
  		switch (joint_type)
  		{
  		case eJointTypeSpherical:
  		{
  			tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size));
  			tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size));
  			tVector rot_vel = cMathUtil::CalcQuaternionVelRel(q0, q1, dt);
  			out_vel.segment(param_offset, param_size) = rot_vel.segment(0, param_size);
  			break;
  		}
  		default:
  			out_vel.segment(param_offset, param_size) = (pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size)) / dt;
  			break;
  		}
  	}
  }
  
  void cKinTree::PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
  {
  	// mostly to normalize quaternions
  	int num_joints = GetNumJoints(joint_mat);
  	int root_id = GetRoot(joint_mat);
  	int root_offset = GetParamOffset(joint_mat, root_id);
  	out_pose.segment(root_offset + gPosDim, gRotDim).normalize();
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		eJointType joint_type = GetJointType(joint_mat, j);
  		if (joint_type == eJointTypeSpherical)
  		{
  			int offset = GetParamOffset(joint_mat, j);
  			out_pose.segment(offset, gRotDim).normalize();
  		}
  	}
  }
  
  void cKinTree::LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose)
  {
  	int num_joints = GetNumJoints(joint_mat);
  	int root_id = GetRoot(joint_mat);
  	int root_offset = GetParamOffset(joint_mat, root_id);
  
  	out_pose.resize(pose0.size());
  	assert(pose0.size() == pose1.size());
  
  	tVector root_pos0 = GetRootPos(joint_mat, pose0);
  	tVector root_pos1 = GetRootPos(joint_mat, pose1);
  	tVector root_pos_lerp = (1 - lerp) * root_pos0 + lerp * root_pos1;
  
  	tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
  	tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
  	assert(std::abs(root_rot0.norm() - 1) < 0.000001);
  	assert(std::abs(root_rot1.norm() - 1) < 0.000001);
  
  	tQuaternion root_rot_lerp = root_rot0.slerp(lerp, root_rot1);
  	root_rot_lerp.normalize();
  
  	cKinTree::SetRootPos(joint_mat, root_pos_lerp, out_pose);
  	cKinTree::SetRootRot(joint_mat, root_rot_lerp, out_pose);
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		eJointType joint_type = GetJointType(joint_mat, j);
  		int offset = GetParamOffset(joint_mat, j);
  		int size = GetParamSize(joint_mat, j);
  		if (joint_type == eJointTypeSpherical)
  		{
  			tQuaternion rot0 = cMathUtil::VecToQuat(pose0.segment(offset, size));
  			tQuaternion rot1 = cMathUtil::VecToQuat(pose1.segment(offset, size));
  			assert(std::abs(rot0.norm() - 1) < 0.000001);
  			assert(std::abs(rot1.norm() - 1) < 0.000001);
  			tQuaternion rot_lerp = rot0.slerp(lerp, rot1);
  			out_pose.segment(offset, size) = cMathUtil::QuatToVec(rot_lerp).segment(0, size);
  		}
  		else
  		{
  			out_pose.segment(offset, size) = (1 - lerp) * pose0.segment(offset, size) + lerp * pose1.segment(offset, size);
  		}
  	}
  }
  
  void cKinTree::VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff)
  {
  	out_pose_diff = vel;
  
  	int num_joints = GetNumJoints(joint_mat);
  	int root_id = GetRoot(joint_mat);
  	int root_offset = GetParamOffset(joint_mat, root_id);
  
  	tVector root_rot_vel = GetRootAngVel(joint_mat, vel);
  	root_rot_vel[3] = 0;
  	tQuaternion root_rot = GetRootRot(joint_mat, pose);
  	tMatrix root_diff_mat = cMathUtil::BuildQuaternionDiffMat(root_rot);
  	tVector root_diff = root_diff_mat * root_rot_vel;
  	tQuaternion root_quat = cMathUtil::VecToQuat(root_diff);
  
  	cKinTree::SetRootRot(joint_mat, root_quat, out_pose_diff);
  
  	for (int j = 1; j < num_joints; ++j)
  	{
  		eJointType joint_type = GetJointType(joint_mat, j);
  		int offset = GetParamOffset(joint_mat, j);
  		int size = GetParamSize(joint_mat, j);
  		if (joint_type == eJointTypeSpherical)
  		{
  			tQuaternion rot = cMathUtil::VecToQuat(pose.segment(offset, size));
  			tVector rot_vel = vel.segment(offset, size);
  			rot_vel[3] = 0;
  			tMatrix diff_mat = cMathUtil::BuildQuaternionDiffMat(rot);
  			tVector q_diff = diff_mat * rot_vel;
  
  			out_pose_diff.segment(offset, size) = q_diff;
  		}
  	}
  }
  
  double cKinTree::CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
  {
  	// heading is the direction of the root in the xz plane
  	tVector ref_dir = tVector(1, 0, 0, 0);
  	tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, pose);
  	tVector rot_dir = cMathUtil::QuatRotVec(root_rot, ref_dir);
  	double heading = std::atan2(-rot_dir[2], rot_dir[0]);
  	return heading;
  }
  
  tQuaternion cKinTree::CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
  {
  	double heading = CalcHeading(joint_mat, pose);
  	return cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), heading);
  }
  
  tMatrix cKinTree::BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
  {
  	double heading = CalcHeading(joint_mat, pose);
  	tVector axis = tVector(0, 1, 0, 0);
  	tMatrix mat = cMathUtil::RotateMat(axis, -heading);
  	return mat;
  }
  
  tMatrix cKinTree::BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
  {
  	// world to origin transform
  	// origin is the point right under the root of the character on the xz plane with x-axis
  	// aligned along the character's heading
  	tVector origin = GetRootPos(joint_mat, pose);
  	origin[1] = 0;
  	tMatrix rot_mat = BuildHeadingTrans(joint_mat, pose);
  	tMatrix trans_mat = cMathUtil::TranslateMat(-origin);
  	tMatrix mat = rot_mat * trans_mat;
  	return mat;
  }
  
  void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
  {
  	Eigen::VectorXd vel= Eigen::VectorXd::Zero(0);
  	NormalizePoseHeading(joint_mat, out_pose, vel);
  }
  
  void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel)
  {
  	tVector root_pos = GetRootPos(joint_mat, out_pose);
  	tQuaternion root_rot = GetRootRot(joint_mat, out_pose);
  
  	root_pos[0] = 0;
  	root_pos[2] = 0;
  
  	double heading = CalcHeading(joint_mat, out_pose);
  	tQuaternion heading_q = cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), -heading);
  	root_rot = heading_q * root_rot;
  
  	SetRootPos(joint_mat, root_pos, out_pose);
  	SetRootRot(joint_mat, root_rot, out_pose);
  
  	if (out_vel.size() > 0)
  	{
  		tVector root_vel = GetRootVel(joint_mat, out_vel);
  		tVector root_ang_vel = GetRootAngVel(joint_mat, out_vel);
  		root_vel = cMathUtil::QuatRotVec(heading_q, root_vel);
  		root_ang_vel = cMathUtil::QuatRotVec(heading_q, root_ang_vel);
  
  		SetRootVel(joint_mat, root_vel, out_vel);
  		SetRootAngVel(joint_mat, root_ang_vel, out_vel);
  	}
  }
  
  void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector<int> mirror_joints0, const std::vector<int> mirror_joints1, Eigen::VectorXd& out_pose)
  {
  	// mirrors along xy plane
  	assert(mirror_joints0.size() == mirror_joints1.size());
  	int num_joints = cKinTree::GetNumJoints(joint_mat);
  
  	tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, out_pose);
  	tVector root_pos = cKinTree::GetRootPos(joint_mat, out_pose);
  	root_pos[2] *= -1;
  	root_rot = cMathUtil::MirrorQuaternion(root_rot, cMathUtil::eAxisZ);
  
  	cKinTree::SetRootRot(joint_mat, root_rot, out_pose);
  	cKinTree::SetRootPos(joint_mat, root_pos, out_pose);
  
  	for (int j = 0; j < num_joints; ++j)
  	{
  		cKinTree::eJointType joint_type = cKinTree::GetJointType(joint_mat, j);
  		bool is_root = cKinTree::IsRoot(joint_mat, j);
  		if (!is_root)
  		{
  			int param_offset = cKinTree::GetParamOffset(joint_mat, j);
  			int param_size = cKinTree::GetParamSize(joint_mat, j);
  			auto joint_params = out_pose.segment(param_offset, param_size);
  
  			if (joint_type == cKinTree::eJointTypeRevolute
  				|| joint_type == cKinTree::eJointTypeFixed)
  			{
  			}
  			else if (joint_type == cKinTree::eJointTypeSpherical)
  			{
  				tQuaternion quat = cMathUtil::VecToQuat(joint_params);
  				quat = cMathUtil::MirrorQuaternion(quat, cMathUtil::eAxisZ);
  				joint_params = cMathUtil::QuatToVec(quat);
  			}
  			else
  			{
  				assert(false); // unsupported joint type
  			}
  		}
  	}
  
  	for (size_t i = 0; i < mirror_joints0.size(); ++i)
  	{
  		int id0 = mirror_joints0[i];
  		int id1 = mirror_joints1[i];
  
  		int offset0 = cKinTree::GetParamOffset(joint_mat, id0);
  		int size0 = cKinTree::GetParamSize(joint_mat, id0);
  		int offset1 = cKinTree::GetParamOffset(joint_mat, id1);
  		int size1 = cKinTree::GetParamSize(joint_mat, id1);
  
  		Eigen::VectorXd params0 = out_pose.segment(offset0, size0);
  		Eigen::VectorXd params1 = out_pose.segment(offset1, size1);
  		out_pose.segment(offset0, size0) = params1;
  		out_pose.segment(offset1, size1) = params0;
  	}
  }
  
  std::string cKinTree::BuildJointMatJson(const Eigen::MatrixXd& joint_mat)
  {
  	std::string json = "";
  	json += "{\n\"" + gJointsKey + "\":\n[\n";
  
  	int num_joints = GetNumJoints(joint_mat);
  	for (int j = 0; j < num_joints; ++j)
  	{
  		tJointDesc curr_desc = joint_mat.row(j);
  		std::string joint_json = BuildJointJson(j, curr_desc);
  
  		if (j != 0)
  		{
  			json += ",\n";
  		}
  
  		json += joint_json;
  	}
  
  	json += "\n]\n}";
  	return json;
  }
  
  std::string cKinTree::BuildJointJson(int id, const tJointDesc& joint_desc)
  {
  	std::string json = "";
  	json += "{\n";
  	json += "\"ID\": " + std::to_string(id);
  	for (int i = 0; i < eJointDescMax; ++i)
  	{
  		std::string param_name = gJointDescKeys[i];
  		if (i == eJointDescType)
  		{
  			json += ",\n";
  			int type_id = static_cast<int>(joint_desc[i]);
  			std::string type_str = gJointTypeNames[type_id];
  			json += "\"" + param_name + "\": \"" + type_str + "\"";
  		}
  		else if (i != eJointDescParamOffset)
  		{
  			double val = joint_desc[i];
  			if (!std::isfinite(val) && (i == eJointDescTorqueLim || i == eJointDescForceLim))
  			{
  				continue;
  			}
  
  			if (i != 0)
  			{
  				json += ",\n";
  			}
  
  			json += "\"" + param_name + "\": " + std::to_string(val);
  		}
  	}
  
  	json += "\n}";
  	return json;
  }
  
  tMatrix cKinTree::ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tVector offset = GetRootPos(joint_mat, state);
  	tQuaternion rot = GetRootRot(joint_mat, state);
  
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix R = cMathUtil::RotateMat(rot);
  	tMatrix T = cMathUtil::TranslateMat(offset);
  
  	tMatrix mat = A * T * R;
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
  	double theta = state(param_offset);
  
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta);
  	
  	tMatrix mat = A * R;
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
  	double theta = state(param_offset + 2);
  	tVector offset = tVector::Zero();
  	offset[0] = state(param_offset);
  	offset[1] = state(param_offset + 1);
  
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta);
  	tMatrix T = cMathUtil::TranslateMat(offset);
  
  	tMatrix mat = A * R * T;
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix mat = A;
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
  	tVector offset = tVector::Zero();
  	offset[0] = state(param_offset);
  
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix T = cMathUtil::TranslateMat(offset);
  
  	tMatrix mat = A * T;
  	return mat;
  }
  
  tMatrix cKinTree::ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
  {
  	int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
  	int param_size = cKinTree::GetParamSize(joint_mat, joint_id);
  	tQuaternion q = cMathUtil::VecToQuat(state.segment(param_offset, param_size));
  
  	tMatrix A = BuildAttachTrans(joint_mat, joint_id);
  	tMatrix R = cMathUtil::RotateMat(q);
  
  	tMatrix mat = A * R;
  	return mat;
  }
  
  
  
  void cKinTree::BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
  {
  	int dim = gRootDim;
  	out_pose = Eigen::VectorXd::Zero(dim);
  	out_pose(gPosDim) = 1;
  }
  
  void cKinTree::BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeRevolute);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypePrismatic);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultPosePlanar(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypePlanar);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultPoseFixed(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeFixed);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeSpherical);
  	out_pose = Eigen::VectorXd::Zero(dim);
  	out_pose(0) = 1;
  }
  
  
  void cKinTree::BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
  {
  	int dim = gRootDim;
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultVelRevolute(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeRevolute);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypePrismatic);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultVelPlanar(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypePlanar);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultVelFixed(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeFixed);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  
  void cKinTree::BuildDefaultVelSpherical(Eigen::VectorXd& out_pose)
  {
  	int dim = GetJointParamSize(eJointTypeSpherical);
  	out_pose = Eigen::VectorXd::Zero(dim);
  }
  