GUI002

CommonData.hpp

#pragma once

#include <Siv3D.hpp>
#include "dynamixel_sdk/DynamixelManager.hpp"


template<typename T>
Array<T> GetArray(const TOMLArrayView& arrayView)
{
	Array<T> values;

	for (const auto& object : arrayView)
	{
		values << object.get<T>();
	}

	return values;
}

HashTable<String, Font> GetFonts(const TOMLTableArrayView& arrayView)
{
	HashTable<String, Font> fontTable;
	{
		for (const auto& object : arrayView)
		{
			fontTable[object[U"label"].getString()] = Font(object[U"size"].get<int32>(),
				static_cast<Typeface>(object[U"typeface"].get<int32>()));
		}
	}

	return fontTable;
}

HashTable<String, Texture> GetIconTextures(const TOMLTableArrayView& arrayView)
{
	HashTable<String, Texture> textureTable;
	{
		for (const auto& object : arrayView)
		{
			textureTable[object[U"label"].getString()] = Texture(Icon(object[U"code"].get<uint32>(), object[U"size"].get<int32>()));
		}
	}

	return textureTable;
}

class Button
{
public:

	enum class State
	{
		LeftClicked,
		RightClicked
	};

	Button()
	{

	}

	Button(const String& textureName, const Vec2& pos)
		: m_textureName(textureName)
		, m_pos(pos)
	{

	}

	void draw(const HashTable<String, Texture>& textures
		, const ColorF& color, const ColorF& mouseOverColor, const ColorF& activeColor) const
	{
		ColorF col;

		if (m_isActive)
		{
			col = activeColor;
		}
		else
		{
			col = isMouseOver() ? mouseOverColor : color;
		}

		textures.at(m_textureName).drawAt(m_pos, col);
	}

	Optional<State> update()
	{
		Optional<State> result;

		auto region = getMouseRegion();

		if (region.leftClicked())
		{
			result = State::LeftClicked;
		}
		if (region.rightClicked())
		{
			result = State::RightClicked;
		}

		return result;
	}

	void setActive(bool isActive)
	{
		m_isActive = isActive;
	}

	bool isActive() const
	{
		return m_isActive;
	}

	bool isMouseOver() const
	{
		return getMouseRegion().mouseOver();
	}

	Circle getMouseRegion() const
	{
		return Circle(m_pos, m_radius);
	}

private:

	double m_radius = 30.0;
	bool m_isActive = false;
	String m_textureName;
	Vec2 m_pos;

};

struct MotorInfo
{
	String name;
	size_t id;
	String type;
	String source;
};

struct MotorDisplay
{
	String name;
	Vec2 pos;
};

struct MotorConnectInformation
{
	enum class State
	{
		Stop,
		Sending
	};

	std::shared_ptr<Dynamixel> dyna;
	Array<String> names;
	State state = State::Stop;
};

struct MotorData
{
	HashTable<String, MotorInfo> motorInfos;
	HashTable<String, MotorDisplay> motorDisplays;
	HashTable<String, MotorConnectInformation> comTable;
};

struct BodyPartInfo
{
	String name;
	String bindingType;
	String source;
};

struct BodyPartDisplay
{
	String name;
	Vec2 pos;
};

struct BodyData
{
	HashTable<String, BodyPartInfo> bodyPartInfos;
	HashTable<String, BodyPartDisplay> bodyPartDisplays;
};

struct ValueProcessInfo
{

	String name;
	String type;
	String to;
	HashTable<String, String> soureTable;
};

struct ValueProcessData
{
	HashTable<String, ValueProcessInfo> valueProcessInfos;
	Array<String> valueProcessSequence;
};

struct CommonData
{
	using Value = std::variant<bool, size_t, int32, double, Vec2, Vec3, Vec4, Size, String, Color>;

	struct ValuePrintVisitor
	{
		String operator()(const bool& value)
		{
			return Format(value);
		}
		String operator()(const size_t& value)
		{
			return Format(value);
		}
		String operator()(const int32& value)
		{
			return Format(value);
		}
		String operator()(const double& value)
		{
			return Format(value);
		}
		String operator()(const Vec2& value)
		{
			return Format(value);
		}
		String operator()(const Vec3& value)
		{
			return Format(value);
		}
		String operator()(const Vec4& value)
		{
			return Format(value);
		}
		String operator()(const Size& value)
		{
			return Format(value);
		}
		String operator()(const String& value)
		{
			return Format(value);
		}
		String operator()(const Color& value)
		{
			return Format(value);
		}
	};

	struct ValueTypePrintVisitor
	{
		String operator()(const bool&)
		{
			return U"TYPE: bool";
		}
		String operator()(const size_t&)
		{
			return U"TYPE: size_t";
		}
		String operator()(const int32&)
		{
			return U"TYPE: int32";
		}
		String operator()(const double&)
		{
			return U"TYPE: double";
		}
		String operator()(const Vec2&)
		{
			return U"TYPE: Vec2";
		}
		String operator()(const Vec3&)
		{
			return U"TYPE: Vec3";
		}
		String operator()(const Vec4&)
		{
			return U"TYPE: Vec4";
		}
		String operator()(const Size&)
		{
			return U"TYPE: Size";
		}
		String operator()(const String&)
		{
			return U"TYPE: String";
		}
		String operator()(const Color&)
		{
			return U"TYPE: Color";
		}
	};

	void printAllDataToConsole()
	{
		for (const auto& value : valueTable)
		{
			String text = U"KEY: " + value.first;
			text += U" " + std::visit(ValuePrintVisitor{}, value.second);

			Console << text;
		}
	}

	void printAllDataToWindnow()
	{
		for (const auto& value : valueTable)
		{
			String text = U"KEY: " + value.first;
			text += U" " + std::visit(ValuePrintVisitor{}, value.second);

			Print << text;
		}
	}


	HashTable<String, Value> valueTable;
	HashTable<String, Font> fontTable;
	HashTable<String, Texture> textureTable;
	HashTable<String, Button> buttonTable;
	HashTable<String, Array<String>> buttonGroupTable;
	HashTable <String, std::function<void(CommonData&)>> channelFunctionTable;

	MotorData motorData;
	BodyData bodyData;
	ValueProcessData valueProcessData;

	experimental::BasicCamera3D camera{};


	RenderTexture gaussianA1, gaussianB1;
	RenderTexture gaussianA4, gaussianB4;
	//RenderTexture gaussianA8, gaussianB8;

	double a8 = 1.0;
	double a4 = 1.0;
	double a1 = 1.0;
};


template<class T>
T GetValue(CommonData& data, const String& name)
{
	try
	{
		auto& value = data.valueTable[name];
		return std::get<T>(value);
	}
	catch (const ParseError & e)
	{
		// ��O�̏ڍׂ��擾���ĕ\��
		Print << e.what();
	}
}

String GetValueToString(CommonData& data, const String& name)
{
	try
	{
		return std::visit(CommonData::ValuePrintVisitor{}, data.valueTable[name]);
	}
	catch (const ParseError & e)
	{
		// ��O�̏ڍׂ��擾���ĕ\��
		Print << e.what();
	}
}

EigQuadProg.cpp

#include "EigQuadProg.hpp"

#include <vector>

using namespace Eigen;

template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
	using std::abs;
	using std::sqrt;
	Scalar a1, b1, t;
	a1 = abs(a);
	b1 = abs(b);
	if (a1 > b1)
	{
		t = (b1 / a1);
		return a1 * sqrt(1.0 + t * t);
	}
	else
		if (b1 > a1)
		{
			t = (a1 / b1);
			return b1 * sqrt(1.0 + t * t);
		}
	return a1 * sqrt(2.0);
}

// }

inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np)
{
	d = J.adjoint() * np;
}

inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq)
{
	z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
}

inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
{
	r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Upper>().solve(d.head(iq));
}

bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int p, int& iq, int l);

double solveEigenQuadProg(MatrixXd& G, VectorXd& g0,
	const MatrixXd& CE, const VectorXd& ce0,
	const MatrixXd& CI, const VectorXd& ci0,
	VectorXd& x)
{
	using std::abs;
	int i, j, k, l; /* indices */
	int ip, me, mi;
	int n = g0.size();
	int p = ce0.size();
	int m = ci0.size();
	MatrixXd R(G.rows(), G.cols()), J(G.rows(), G.cols());

	LLT<MatrixXd, Lower> chol(G.cols());

	VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
	VectorXd x_old(n), u_old(m + p);
	double f_value, psi, c1, c2, sum, ss, R_norm;
	const double inf = std::numeric_limits<double>::infinity();
	double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
	  * and the full step length t2 */
	VectorXi A(m + p), A_old(m + p), iai(m + p);
	int q;
	int iq, iter = 0;
	std::vector<bool> iaexcl(m + p);

	me = p; /* number of equality constraints */
	mi = m; /* number of inequality constraints */
	q = 0;  /* size of the active set A (containing the indices of the active constraints) */

	/*
	 * Preprocessing phase
	 */

	 /* compute the trace of the original matrix G */
	c1 = G.trace();

	/* decompose the matrix G in the form LL^T */
	chol.compute(G);

	/* initialize the matrix R */
	d.setZero();
	R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */

	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
	J.setIdentity();
	J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
	print_matrix("J", J, n);
#endif

	/* c1 * c2 is an estimate for cond(G) */

	/*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
	x = chol.solve(g0);
	x = -x;
	/* and compute the current solution value */
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
	std::cerr << "Unconstrained solution: " << f_value << std::endl;
	print_vector("x", x, n);
#endif

	/* Add equality constraints to the working set A */
	iq = 0;
	for (i = 0; i < me; i++)
	{
		np = CE.col(i);
		compute_d(d, J, np);
		update_z(z, J, d, iq);
		update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
		print_matrix("R", R, iq);
		print_vector("z", z, n);
		print_vector("r", r, iq);
		print_vector("d", d, n);
#endif

		/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
		  becomes feasible */
		t2 = 0.0;
		if (abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
			t2 = (-np.dot(x) - ce0(i)) / z.dot(np);

		x += t2 * z;

		/* set u = u+ */
		u(iq) = t2;
		u.head(iq) -= t2 * r.head(iq);

		/* compute the new solution value */
		f_value += 0.5 * (t2 * t2) * z.dot(np);
		A(i) = -i - 1;

		if (!add_constraint(R, J, d, iq, R_norm))
		{
			// FIXME: it should raise an error
			// Equality constraints are linearly dependent
			return f_value;
		}
	}

	/* set iai = K \ A */
	for (i = 0; i < mi; i++)
		iai(i) = i;

l1:	iter++;
#ifdef TRACE_SOLVER
	print_vector("x", x, n);
#endif
	/* step 1: choose a violated constraint */
	for (i = me; i < iq; i++)
	{
		ip = A(i);
		iai(ip) = -1;
	}

	/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
	ss = 0.0;
	psi = 0.0; /* this value will contain the sum of all infeasibilities */
	ip = 0; /* ip will be the index of the chosen violated constraint */
	for (i = 0; i < mi; i++)
	{
		iaexcl[i] = true;
		sum = CI.col(i).dot(x) + ci0(i);
		s(i) = sum;
		psi += std::min(0.0, sum);
	}
#ifdef TRACE_SOLVER
	print_vector("s", s, mi);
#endif


	if (abs(psi) <= mi * std::numeric_limits<double>::epsilon()* c1* c2 * 100.0)
	{
		/* numerically there are not infeasibilities anymore */
		q = iq;
		return f_value;
	}

	/* save old values for u, x and A */
	u_old.head(iq) = u.head(iq);
	A_old.head(iq) = A.head(iq);
	x_old = x;

l2: /* Step 2: check for feasibility and determine a new S-pair */
	for (i = 0; i < mi; i++)
	{
		if (s(i) < ss && iai(i) != -1 && iaexcl[i])
		{
			ss = s(i);
			ip = i;
		}
	}
	if (ss >= 0.0)
	{
		q = iq;
		return f_value;
	}

	/* set np = n(ip) */
	np = CI.col(ip);
	/* set u = (u 0)^T */
	u(iq) = 0.0;
	/* add ip to the active set A */
	A(iq) = ip;

#ifdef TRACE_SOLVER
	std::cerr << "Trying with constraint " << ip << std::endl;
	print_vector("np", np, n);
#endif

l2a:/* Step 2a: determine step direction */
  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
	compute_d(d, J, np);
	update_z(z, J, d, iq);
	/* compute N* np (if q > 0): the negative of the step direction in the dual space */
	update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
	std::cerr << "Step direction z" << std::endl;
	print_vector("z", z, n);
	print_vector("r", r, iq + 1);
	print_vector("u", u, iq + 1);
	print_vector("d", d, n);
	print_ivector("A", A, iq + 1);
#endif

	/* Step 2b: compute step length */
	l = 0;
	/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
	t1 = inf; /* +inf */
	/* find the index l s.t. it reaches the minimum of u+(x) / r */
	for (k = me; k < iq; k++)
	{
		double tmp;
		if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1))
		{
			t1 = tmp;
			l = A(k);
		}
	}
	/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
	if (abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
		t2 = -s(ip) / z.dot(np);
	else
		t2 = inf; /* +inf */

	  /* the step is chosen as the minimum of t1 and t2 */
	t = std::min(t1, t2);
#ifdef TRACE_SOLVER
	std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif

	/* Step 2c: determine new S-pair and take step: */

	/* case (i): no step in primal or dual space */
	if (t >= inf)
	{
		/* QPP is infeasible */
		// FIXME: unbounded to raise
		q = iq;
		return inf;
	}
	/* case (ii): step in dual space */
	if (t2 >= inf)
	{
		/* set u = u +  t * [-r 1) and drop constraint l from the active set A */
		u.head(iq) -= t * r.head(iq);
		u(iq) += t;
		iai(l) = l;
		delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
		std::cerr << " in dual space: "
			<< f_value << std::endl;
		print_vector("x", x, n);
		print_vector("z", z, n);
		print_ivector("A", A, iq + 1);
#endif
		goto l2a;
	}

	/* case (iii): step in primal and dual space */

	x += t * z;
	/* update the solution value */
	f_value += t * z.dot(np) * (0.5 * t + u(iq));

	u.head(iq) -= t * r.head(iq);
	u(iq) += t;
#ifdef TRACE_SOLVER
	std::cerr << " in both spaces: "
		<< f_value << std::endl;
	print_vector("x", x, n);
	print_vector("u", u, iq + 1);
	print_vector("r", r, iq + 1);
	print_ivector("A", A, iq + 1);
#endif

	if (t == t2)
	{
#ifdef TRACE_SOLVER
		std::cerr << "Full step has taken " << t << std::endl;
		print_vector("x", x, n);
#endif
		/* full step has taken */
		/* add constraint ip to the active set*/
		if (!add_constraint(R, J, d, iq, R_norm))
		{
			iaexcl[ip] = false;
			delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
			print_matrix("R", R, n);
			print_ivector("A", A, iq);
#endif
			for (i = 0; i < m; i++)
				iai(i) = i;
			for (i = 0; i < iq; i++)
			{
				A(i) = A_old(i);
				iai(A(i)) = -1;
				u(i) = u_old(i);
			}
			x = x_old;
			goto l2; /* go to step 2 */
		}
		else
			iai(ip) = -1;
#ifdef TRACE_SOLVER
		print_matrix("R", R, n);
		print_ivector("A", A, iq);
#endif
		goto l1;
	}

	/* a patial step has taken */
#ifdef TRACE_SOLVER
	std::cerr << "Partial step has taken " << t << std::endl;
	print_vector("x", x, n);
#endif
	/* drop constraint l */
	iai(l) = l;
	delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
	print_matrix("R", R, n);
	print_ivector("A", A, iq);
#endif

	s(ip) = CI.col(ip).dot(x) + ci0(ip);

#ifdef TRACE_SOLVER
	print_vector("s", s, mi);
#endif
	goto l2a;
}


inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm)
{
	using std::abs;
	int n = J.rows();
#ifdef TRACE_SOLVER
	std::cerr << "Add constraint " << iq << '/';
#endif
	int i, j, k;
	double cc, ss, h, t1, t2, xny;

	/* we have to find the Givens rotation which will reduce the element
		  d(j) to zero.
		  if it is already zero we don't have to do anything, except of
		  decreasing j */
	for (j = n - 1; j >= iq + 1; j--)
	{
		/* The Givens rotation is done with the matrix (cc cs, cs -cc).
				 If cc is one, then element (j) of d is zero compared with element
				 (j - 1). Hence we don't have to do anything.
				 If cc is zero, then we just have to switch column (j) and column (j - 1)
				 of J. Since we only switch columns in J, we have to be careful how we
				 update d depending on the sign of gs.
				 Otherwise we have to apply the Givens rotation to these columns.
				 The i - 1 element of d has to be updated to h. */
		cc = d(j - 1);
		ss = d(j);
		h = distance(cc, ss);
		if (h == 0.0)
			continue;
		d(j) = 0.0;
		ss = ss / h;
		cc = cc / h;
		if (cc < 0.0)
		{
			cc = -cc;
			ss = -ss;
			d(j - 1) = -h;
		}
		else
			d(j - 1) = h;
		xny = ss / (1.0 + cc);
		for (k = 0; k < n; k++)
		{
			t1 = J(k, j - 1);
			t2 = J(k, j);
			J(k, j - 1) = t1 * cc + t2 * ss;
			J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
		}
	}
	/* update the number of constraints added*/
	iq++;
	/* To update R we have to put the iq components of the d vector
	  into column iq - 1 of R
	  */
	R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
	std::cerr << iq << std::endl;
#endif

	if (abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon()* R_norm)
		// problem degenerate
		return false;
	R_norm = std::max<double>(R_norm, abs(d(iq - 1)));
	return true;
}


inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int p, int& iq, int l)
{

	int n = R.rows();
#ifdef TRACE_SOLVER
	std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
	int i, j, k, qq;
	double cc, ss, h, xny, t1, t2;

	/* Find the index qq for active constraint l to be removed */
	for (i = p; i < iq; i++)
		if (A(i) == l)
		{
			qq = i;
			break;
		}

	/* remove the constraint from the active set and the duals */
	for (i = qq; i < iq - 1; i++)
	{
		A(i) = A(i + 1);
		u(i) = u(i + 1);
		R.col(i) = R.col(i + 1);
	}

	A(iq - 1) = A(iq);
	u(iq - 1) = u(iq);
	A(iq) = 0;
	u(iq) = 0.0;
	for (j = 0; j < iq; j++)
		R(j, iq - 1) = 0.0;
	/* constraint has been fully removed */
	iq--;
#ifdef TRACE_SOLVER
	std::cerr << '/' << iq << std::endl;
#endif 

	if (iq == 0)
		return;

	for (j = qq; j < iq; j++)
	{
		cc = R(j, j);
		ss = R(j + 1, j);
		h = distance(cc, ss);
		if (h == 0.0)
			continue;
		cc = cc / h;
		ss = ss / h;
		R(j + 1, j) = 0.0;
		if (cc < 0.0)
		{
			R(j, j) = -h;
			cc = -cc;
			ss = -ss;
		}
		else
			R(j, j) = h;

		xny = ss / (1.0 + cc);
		for (k = j + 1; k < iq; k++)
		{
			t1 = R(j, k);
			t2 = R(j + 1, k);
			R(j, k) = t1 * cc + t2 * ss;
			R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
		}
		for (k = 0; k < n; k++)
		{
			t1 = J(k, j);
			t2 = J(k, j + 1);
			J(k, j) = t1 * cc + t2 * ss;
			J(k, j + 1) = xny * (J(k, j) + t1) - t2;
		}
	}
}

EigQuadProg.hpp

#ifndef _EIGEN_QUADSOLVE_HPP_
#define _EIGEN_QUADSOLVE_HPP_

//source http://www.labri.fr/perso/guenneba/code/QuadProg/eiquadprog.hpp
/*
 FILE eiquadprog.hh

 NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
	   uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
	   Luca Di Gaspero, working with ublas data structures.

 The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
 for the solution of a (convex) Quadratic Programming problem
by means of a dual method.

The problem is in the form:

min 0.5 * x G x + g0 x
s.t.
	CE^T x + ce0 = 0
	CI^T x + ci0 >= 0

 The matrix and vectors dimensions are as follows:
	 G: n * n
		g0: n

		CE: n * p
	 ce0: p

	  CI: n * m
   ci0: m

	 x: n

 The function will return the cost of the solution written in the x vector or
 std::numeric_limits::infinity() if the problem is infeasible. In the latter case
 the value of the x vector is not correct.

 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
			 strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.

 Notes:
  1. pay attention in setting up the vectors ce0 and ci0.
	   If the constraints of your problem are specified in the form
	   A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
  2. The matrix G is modified within the function since it is used to compute
	 the G = L^T L cholesky factorization for further computations inside the function.
	 If you need the original matrix G you should make a copy of it and pass the copy
	 to the function.


 The author will be grateful if the researchers using this software will
 acknowledge the contribution of this modified function and of Di Gaspero's
 original version in their research papers.


LICENSE

Copyright (2010) Gael Guennebaud
Copyright (2008) Angelo Furfaro
Copyright (2006) Luca Di Gaspero


This file is a porting of QuadProg++ routine, originally developed
by Luca Di Gaspero, exploiting uBlas data structures for vectors and
matrices instead of native C++ array.

uquadprog is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

uquadprog is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with uquadprog; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include <Eigen/Dense>

double solveEigenQuadProg(Eigen::MatrixXd& G, Eigen::VectorXd& g0,
	const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
	const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
	Eigen::VectorXd& x);

#endif

#pragma once

#include <Siv3D.hpp>
#include <variant>
#include "CommonData.hpp"
#include "Robot.hpp"

void DrawBackground(CommonData& data)
{
	Scene::Rect().draw(GetValue<Color>(data, U"Scene.MainBackground"));
	Rect(0, 0, GetValue<int32>(data, U"Scene.ChannelWidth"), Scene::Height()).draw(GetValue<Color>(data, U"Scene.ChannelBackground"));
	Rect(0, 0, GetValue<int32>(data, U"Scene.MenuWidth"), Scene::Height()).draw(GetValue<Color>(data, U"Scene.MenuBackground"));
}

void UpdateButtonGroup(CommonData& data, const String& groupName)
{
	String buttonActionName;
	Optional<Button::State> buttonActionResult;

	if (data.buttonGroupTable[groupName].size() == 0)
	{
		return;
	}

	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		buttonActionResult = data.buttonTable[buttonName].update();

		if (buttonActionResult.has_value())
		{
			buttonActionName = buttonName;
			break;
		}
	}

	if (buttonActionResult.has_value())
	{
		for (auto& buttonName : data.buttonGroupTable[groupName])
		{
			data.buttonTable[buttonName].setActive(false);
		}

		data.buttonTable[buttonActionName].setActive(true);
	}
}

void DrawButtonGroup(CommonData& data, const String& groupName)
{
	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		data.buttonTable[buttonName].draw(data.textureTable
			, GetValue<Color>(data, U"Menu.ButtonColor")
			, GetValue<Color>(data, U"Menu.ButtonMouseOverColor")
			, GetValue<Color>(data, U"Menu.ButtonActiveColor"));
	}
}

Optional<String> GetActiveButtonNameFromButtonGroup(CommonData& data, const String& groupName)
{
	Optional<String> result;

	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		if (data.buttonTable[buttonName].isActive())
		{
			result = buttonName;
			break;
		}
	}

	return result;
}



void UpdateMouseInformation(CommonData& data)
{
	data.valueTable[U"Mouse.Pos"] = Cursor::Pos();
	data.valueTable[U"Mouse.PosF"] = Cursor::PosF();
	data.valueTable[U"Mouse.Delta"] = Cursor::Delta();
	data.valueTable[U"Mouse.ScreenPos"] = Cursor::ScreenPos();
}

void UpdateXInputInformation(CommonData& data)
{
	for (size_t userIndex = 0; userIndex < 4; ++userIndex)
	{
		auto controller = XInput(userIndex);

		//if (controller.isConnected())
		{
			String prefix = Format(U"XInput.Controller", userIndex + 1);

			data.valueTable[Format(prefix, U".LeftTrigger")] = controller.leftTrigger;
			data.valueTable[Format(prefix, U".RightTrigger")] = controller.rightTrigger;

			data.valueTable[Format(prefix, U".ButtonLThumb.Pressed")] = controller.buttonLThumb.pressed();
			data.valueTable[Format(prefix, U".ButtonLThumb.Down")] = controller.buttonLThumb.down();
			data.valueTable[Format(prefix, U".LeftThumbX")] = controller.leftThumbX;
			data.valueTable[Format(prefix, U".LeftThumbY")] = controller.leftThumbY;


			data.valueTable[Format(prefix, U".ButtonRThumb.Pressed")] = controller.buttonRThumb.pressed();
			data.valueTable[Format(prefix, U".ButtonRThumb.Down")] = controller.buttonRThumb.down();
			data.valueTable[Format(prefix, U".RightThumbX")] = controller.rightThumbX;
			data.valueTable[Format(prefix, U".RightThumbY")] = controller.rightThumbY;

			data.valueTable[Format(prefix, U".ButtonRight.Pressed")] = controller.buttonRight.pressed();
			data.valueTable[Format(prefix, U".ButtonLeft.Pressed")] = controller.buttonLeft.pressed();
			data.valueTable[Format(prefix, U".ButtonUp.Pressed")] = controller.buttonUp.pressed();
			data.valueTable[Format(prefix, U".ButtonDown.Pressed")] = controller.buttonDown.pressed();

			data.valueTable[Format(prefix, U".ButtonA.Pressed")] = controller.buttonA.pressed();
			data.valueTable[Format(prefix, U".ButtonB.Pressed")] = controller.buttonB.pressed();
			data.valueTable[Format(prefix, U".ButtonX.Pressed")] = controller.buttonX.pressed();
			data.valueTable[Format(prefix, U".ButtonY.Pressed")] = controller.buttonY.pressed();

			data.valueTable[Format(prefix, U".ButtonBack.Pressed")] = controller.buttonBack.pressed();
			data.valueTable[Format(prefix, U".ButtonStart.Pressed")] = controller.buttonStart.pressed();
		}
	}
}

void UpdateInformations(CommonData& data)
{
	UpdateMouseInformation(data);
	UpdateXInputInformation(data);
	UpdateRobotData(data);
}



void DrawXInputController(CommonData& data, size_t userIndex)
{
	auto controller = XInput(userIndex);

	ColorF background = GetValue<Color>(data, U"Scene.MainBackground");

	constexpr Ellipse buttonLB(160, 140, 50, 24);
	constexpr Ellipse buttonRB(520, 140, 50, 24);

	constexpr RectF leftTrigger(150, 16, 40, 100);
	constexpr RectF rightTrigger(500, 16, 40, 100);

	constexpr Circle buttonLThumb(170, 250, 35);
	constexpr Circle buttonRThumb(420, 350, 35);
	constexpr Circle buttonDPad(260, 350, 40);

	constexpr Circle buttonA(510, 300, 20);
	constexpr Circle buttonB(560, 250, 20);
	constexpr Circle buttonX(460, 250, 20);
	constexpr Circle buttonY(510, 200, 20);
	constexpr Circle buttonBack(270, 250, 15);
	constexpr Circle buttonStart(410, 250, 15);



	// �U��

	buttonLB.draw(ColorF(controller.buttonLB.pressed() ? 1.0 : 0.7));
	buttonRB.draw(ColorF(controller.buttonRB.pressed() ? 1.0 : 0.7));

	//Ellipse(340 /* + 3.0 * Random(leftV + rightV) */, 480, 300, 440).draw(ColorF(0.9));
	//Ellipse(340, 40, 220, 120).draw(background);
	//Circle(340, 660, 240).draw(background);

	Circle(340, 250, 30).draw(ColorF(0.6));

	// �R���g���[���̐ڑ��̗L��
	if (controller.isConnected())
	{
		Circle(340, 250, 32).drawPie(-0.5_pi + 0.5_pi * controller.userIndex, 0.5_pi, ColorF(0.7, 1.0, 0.5));
	}

	Circle(340, 250, 25).draw(ColorF(0.6));

	leftTrigger.draw(AlphaF(0.25));
	leftTrigger.stretched((controller.leftTrigger - 1.0) * leftTrigger.h, 0, 0, 0).draw();

	rightTrigger.draw(AlphaF(0.25));
	rightTrigger.stretched((controller.rightTrigger - 1.0) * rightTrigger.h, 0, 0, 0).draw();

	buttonLThumb.draw(ColorF(controller.buttonLThumb.pressed() ? 0.85 : 0.5));
	Circle(buttonLThumb.center + Vec2(controller.leftThumbX, -controller.leftThumbY) * 25, 20).draw();

	buttonRThumb.draw(ColorF(controller.buttonRThumb.pressed() ? 0.85 : 0.5));
	Circle(buttonRThumb.center + Vec2(controller.rightThumbX, -controller.rightThumbY) * 25, 20).draw();

	buttonDPad.draw(ColorF(0.5));

	const Vec2 direction(
		controller.buttonRight.pressed() - controller.buttonLeft.pressed(),
		controller.buttonDown.pressed() - controller.buttonUp.pressed());

	if (!direction.isZero())
	{
		Circle(buttonDPad.center + direction.normalized() * 25, 15).draw();
	}

	buttonA.draw(ColorF(0.0, 1.0, 0.3).setA(controller.buttonA.pressed() ? 1.0 : 0.3));
	buttonB.draw(ColorF(1.0, 0.0, 0.3).setA(controller.buttonB.pressed() ? 1.0 : 0.3));
	buttonX.draw(ColorF(0.0, 0.3, 1.0).setA(controller.buttonX.pressed() ? 1.0 : 0.3));
	buttonY.draw(ColorF(1.0, 0.5, 0.0).setA(controller.buttonY.pressed() ? 1.0 : 0.3));

	buttonBack.draw(ColorF(controller.buttonBack.pressed() ? 1.0 : 0.7));
	buttonStart.draw(ColorF(controller.buttonStart.pressed() ? 1.0 : 0.7));
}

void DrawXInputInformation(CommonData& data)
{
	for (size_t i = 0; i < 2; ++i)
	{
		auto t = Transformer2D(Mat3x2::Translate(Vec2(0.0, i * 400)), true);

		DrawXInputController(data, i);
	}
}

void DrawMotorInfos(CommonData& data)
{
	for (const auto& i : data.motorData.motorDisplays)
	{
		auto& motorDisplay = i.second;
		auto& motorInfo = data.motorData.motorInfos[motorDisplay.name];

		RectF(motorDisplay.pos, Vec2(250.0, 100.0)).drawFrame(3.0);
		data.fontTable[U"main"](U"Name: ", motorInfo.name).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 0.0));
		data.fontTable[U"main"](U"ID: ", motorInfo.id).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 20.0));
		data.fontTable[U"main"](U"TYPE: ", motorInfo.type).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 40.0));
	}
}

void DrawBodyInfos(CommonData& data)
{
	for (const auto& i : data.bodyData.bodyPartDisplays)
	{
		auto& bodyPartDisplay = i.second;
		auto& bodyPartInfo = data.bodyData.bodyPartInfos[bodyPartDisplay.name];

		RectF(bodyPartDisplay.pos, Vec2(250.0, 100.0)).drawFrame(3.0);
		data.fontTable[U"main"](U"Name: ", bodyPartInfo.name).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 0.0));
		data.fontTable[U"main"](U"TYPE: ", bodyPartInfo.bindingType).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 20.0));
		data.fontTable[U"main"](U"SOURCE: ", bodyPartInfo.source).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 40.0));
		data.fontTable[U"main"](GetValueToString(data, bodyPartInfo.source)).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 60.0));
	}
}


void UpdateValueProcessor(CommonData& data)
{
	for (const auto& processName : data.valueProcessData.valueProcessSequence)
	{
		auto& valueProcess = data.valueProcessData.valueProcessInfos[processName];

		if (valueProcess.type == U"handFromController")
		{
			const double x = GetValue<double>(data, valueProcess.soureTable[U"X"]);
			const double y = GetValue<double>(data, valueProcess.soureTable[U"Y"]);
			const double z = GetValue<double>(data, valueProcess.soureTable[U"Z"]);
			const Vec3 zDisplacement = GetValue<Vec3>(data, valueProcess.soureTable[U"ZDisplacement"]);
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const Vec3 offset = GetValue<Vec3>(data, valueProcess.soureTable[U"Offset"]);
			const Vec2 controllVec2 = Vec2(xScale * x, yScale * y);
			Vec3 valueResult = offset + Vec3(controllVec2.x, controllVec2.y, 0.0) + zDisplacement * z;

			data.valueTable[valueProcess.to] = valueResult;
		}
		if (valueProcess.type == U"vec3FromDoubles")
		{
			const double x = GetValue<double>(data, valueProcess.soureTable[U"X"]);
			const double y = GetValue<double>(data, valueProcess.soureTable[U"Y"]);
			const double z = GetValue<double>(data, valueProcess.soureTable[U"Z"]);
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const double zScale = GetValue<double>(data, valueProcess.soureTable[U"ZScale"]);
			const Vec3 offset = GetValue<Vec3>(data, valueProcess.soureTable[U"Offset"]);
			Vec3 valueResult = offset + Vec3(xScale * x, yScale * y, zScale * z);

			data.valueTable[valueProcess.to] = valueResult;
		}
		if (valueProcess.type == U"threeDoublesFromVec3")
		{
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const double zScale = GetValue<double>(data, valueProcess.soureTable[U"ZScale"]);
			const double xOffset = GetValue<double>(data, valueProcess.soureTable[U"XOffset"]);
			const double yOffset = GetValue<double>(data, valueProcess.soureTable[U"YOffset"]);
			const double zOffset = GetValue<double>(data, valueProcess.soureTable[U"ZOffset"]);
			const Vec3 source = GetValue<Vec3>(data, valueProcess.soureTable[U"vec3"]);

			data.valueTable[valueProcess.soureTable[U"to1"]] = source.x * xScale + xOffset;
			data.valueTable[valueProcess.soureTable[U"to2"]] = source.y * yScale + yOffset;
			data.valueTable[valueProcess.soureTable[U"to3"]] = source.z * zScale + zOffset;
		}
		if (valueProcess.type == U"doubleFromTwoDoubles")
		{
			const double scale1 = GetValue<double>(data, valueProcess.soureTable[U"scale1"]);
			const double scale2 = GetValue<double>(data, valueProcess.soureTable[U"scale2"]);

			const double offset = GetValue<double>(data, valueProcess.soureTable[U"offset"]);

			const double source1 = GetValue<double>(data, valueProcess.soureTable[U"source1"]);
			const double source2 = GetValue<double>(data, valueProcess.soureTable[U"source2"]);

			data.valueTable[valueProcess.soureTable[U"to"]] = source1 * scale1 + source2 * scale2 + offset;
		}
	}
}


void LoadSystemSettings(CommonData& data)
{
	// TOML �t�@�C������f�[�^��ǂݍ���
	const TOMLReader toml(U"example/config/config.toml");

	if (!toml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.toml`");
	}

	// TOML �f�[�^�����ׂĕ\��
	//ShowTable(toml);

	//Print << U"-----";

	// �v�f�̃p�X�Œl���擾
	const String windowTitle = toml[U"Window.title"].getString();
	const int32 windowWidth = toml[U"Window.width"].get<int32>();
	const int32 windowHeight = toml[U"Window.height"].get<int32>();
	const bool windowSizable = toml[U"Window.sizable"].get<bool>();
	const ColorF sceneBackground = toml[U"Scene.mainBackground"].get<Color>();

	Window::SetTitle(windowTitle);
	Window::Resize(windowWidth, windowHeight);
	Window::SetStyle(windowSizable ? WindowStyle::Sizable : WindowStyle::Fixed);
	Scene::SetBackground(sceneBackground);


	//�e�p�̃I�u�W�F�N�g
	{
		Size size(windowWidth, windowHeight);

		data.gaussianA1 = RenderTexture(size);
		data.gaussianB1 = RenderTexture(size);
		data.gaussianA4 = RenderTexture(size / 4);
		data.gaussianB4 = RenderTexture(size / 4);
		//data.gaussianA8 = RenderTexture(size / 8);
		//data.gaussianB8 = RenderTexture(size / 8);
	}

	// ���l�̔z��� TOML �f�[�^����쐬

	// �A�C�e���̔z��� TOML �f�[�^����쐬

	// �A�C�e���`��p�̃t�H���g
	data.fontTable = GetFonts(toml[U"Fonts"].tableArrayView());
	data.textureTable = GetIconTextures(toml[U"IconTextures"].tableArrayView());

	data.printAllDataToConsole();

	HashTable<String, Button> buttons;
	{
		for (const auto& object : toml[U"Buttons"].tableArrayView())
		{
			Button button(object[U"textureName"].getString(), Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>()));
			button.setActive(object[U"isActive"].get<bool>());
			buttons[object[U"label"].getString()] = button;
		}
	}
	data.buttonTable = buttons;


	HashTable<String, Array<String>> buttonGroupTable;
	{
		for (const auto& object : toml[U"ButtonGroups"].tableArrayView())
		{
			Array<String> buttonNames;

			if (!object[U"ButtonNames"].isEmpty())
			{
				for (const auto& tObject : object[U"ButtonNames"].tableArrayView())
				{
					buttonNames << tObject[U"label"].getString();
				}
			}

			buttonGroupTable[object[U"label"].getString()] = buttonNames;
		}
	}
	data.buttonGroupTable = buttonGroupTable;

	data.valueTable[U"Menu.ButtonColor"] = toml[U"Menu.buttonColor"].get<Color>();
	data.valueTable[U"Menu.ButtonMouseOverColor"] = toml[U"Menu.buttonMouseOverColor"].get<Color>();
	data.valueTable[U"Menu.ButtonActiveColor"] = toml[U"Menu.buttonActiveColor"].get<Color>();
	data.valueTable[U"Menu.TransformerScale"] = toml[U"Menu.transformerScale"].get<double>();
	data.valueTable[U"Menu.SimpleGuiPos"] = toml[U"Menu.simpleGuiPos"].get<Vec2>();
	data.valueTable[U"Scene.MainBackground"] = sceneBackground;
	data.valueTable[U"Scene.ChannelWidth"] = toml[U"Scene.channelWidth"].get<int32>();
	data.valueTable[U"Scene.ChannelBackground"] = toml[U"Scene.channelBackground"].get<Color>();
	data.valueTable[U"Scene.MenuWidth"] = toml[U"Scene.menuWidth"].get<int32>();
	data.valueTable[U"Scene.MenuBackground"] = toml[U"Scene.menuBackground"].get<Color>();

	data.channelFunctionTable[U"gamepad"] = DrawXInputInformation;
	data.channelFunctionTable[U"robot"] = DrawMotorInfos;
	data.channelFunctionTable[U"body"] = DrawBodyInfos;
	data.channelFunctionTable[U"preview"] = DrawRobotData;


	constexpr double fov = 45_deg;
	constexpr Vec3 focusPosition(0, 5, 0);
	Vec3 eyePositon(0, 0, 0);
	data.camera = experimental::BasicCamera3D(Size(Scene::Size().x - toml[U"Scene.channelWidth"].get<int32>(), Scene::Size().y), fov, eyePositon, focusPosition);



}

void LoadUserSettings(CommonData& data)
{
	// TOML �t�@�C������f�[�^��ǂݍ���
	const TOMLReader settingsToml(U"settings.toml");

	if (!settingsToml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.toml`");
	}

	{
		HashTable<String, MotorConnectInformation> motorConnectInformations;
		for (const auto& object : settingsToml[U"Robot.COMs"].tableArrayView())
		{
			MotorConnectInformation motorConnectInformation;

			Dynamixel::BaudRate rate;
			if (object[U"baudRate"].getString() == U"1000000")
			{
				rate = Dynamixel::BaudRate::Rate1000000;
			}
			if (object[U"baudRate"].getString() == U"57600")
			{
				rate = Dynamixel::BaudRate::Rate57600;
			}

			motorConnectInformation.dyna = std::make_shared<Dynamixel>();
			motorConnectInformation.dyna->connectPort(object[U"comPort"].getString(), rate, ham::ProtocolType::Protocol2);

			for (const auto& names : object[U"Motors"].tableArrayView())
			{
				motorConnectInformation.names << names[U"label"].getString();
			}

			motorConnectInformations.emplace(object[U"label"].getString(), motorConnectInformation);
		}
		data.motorData.comTable = motorConnectInformations;

		HashTable<String, MotorInfo> motorInfos;
		for (const auto& object : settingsToml[U"Robot.Motors"].tableArrayView())
		{
			MotorInfo motorInfo;
			motorInfo.name = object[U"label"].getString();
			motorInfo.id = object[U"id"].get<size_t>();
			motorInfo.type = object[U"type"].getString();
			motorInfo.source = object[U"source"].getString();

			motorInfos[object[U"label"].getString()] = motorInfo;
		}
		data.motorData.motorInfos = motorInfos;

		HashTable<String, MotorDisplay> motorDisplays;
		for (const auto& object : settingsToml[U"Robot.MotorDisplays"].tableArrayView())
		{
			MotorDisplay motorDisplay;
			motorDisplay.name = object[U"label"].getString();
			motorDisplay.pos = Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>());

			motorDisplays[object[U"label"].getString()] = motorDisplay;
		}
		data.motorData.motorDisplays = motorDisplays;
	}


	HashTable<String, BodyPartInfo> bodyPartInfos;
	for (const auto& object : settingsToml[U"Body.Parts"].tableArrayView())
	{
		BodyPartInfo bodyPartInfo;
		bodyPartInfo.name = object[U"label"].getString();
		bodyPartInfo.bindingType = object[U"bindingType"].getString();
		bodyPartInfo.source = object[U"source"].getString();

		bodyPartInfos[object[U"label"].getString()] = bodyPartInfo;
	}
	data.bodyData.bodyPartInfos = bodyPartInfos;

	HashTable<String, BodyPartDisplay> bodyPartDisplays;
	for (const auto& object : settingsToml[U"Body.PartDisplays"].tableArrayView())
	{
		BodyPartDisplay bodyPartDisplay;
		bodyPartDisplay.name = object[U"label"].getString();
		bodyPartDisplay.pos = Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>());

		bodyPartDisplays[object[U"label"].getString()] = bodyPartDisplay;
	}
	data.bodyData.bodyPartDisplays = bodyPartDisplays;

	//------------------------------------------------

	for (const auto& object : settingsToml[U"ValueProcessor.values"].tableArrayView())
	{
		if (object[U"type"].getString() == U"vec3")
		{
			data.valueTable[Format(U"ValueProcessor.", object[U"label"].getString())]
				= Vec3(object[U"value.x"].get<double>(), object[U"value.y"].get<double>(), object[U"value.z"].get<double>());
		}
		if (object[U"type"].getString() == U"double")
		{
			data.valueTable[Format(U"ValueProcessor.", object[U"label"].getString())]
				= object[U"value"].get<double>();
		}
	}

	HashTable<String, ValueProcessInfo> valueProcessInfos;
	for (const auto& object : settingsToml[U"ValueProcessor.valueProcessInfos"].tableArrayView())
	{
		ValueProcessInfo valueProcessInfo;
		valueProcessInfo.name = object[U"label"].getString();
		valueProcessInfo.type = object[U"type"].getString();
		valueProcessInfo.to = object[U"to"].getString();

		if (valueProcessInfo.type == U"handFromController")
		{
			valueProcessInfo.soureTable[U"X"] = object[U"X"].getString();// U"XInput.Controller1.LeftThumbX";
			valueProcessInfo.soureTable[U"Y"] = object[U"Y"].getString(); //U"XInput.Controller1.LeftThumbY";
			valueProcessInfo.soureTable[U"Z"] = object[U"Z"].getString();
			valueProcessInfo.soureTable[U"ZDisplacement"] = object[U"ZDisplacement"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"Offset"] = object[U"Offset"].getString();
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
		}
		if (valueProcessInfo.type == U"vec3FromDoubles")
		{
			valueProcessInfo.soureTable[U"X"] = object[U"X"].getString();// U"XInput.Controller1.LeftThumbX";
			valueProcessInfo.soureTable[U"Y"] = object[U"Y"].getString(); //U"XInput.Controller1.LeftThumbY";
			valueProcessInfo.soureTable[U"Z"] = object[U"Z"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"ZScale"] = object[U"ZScale"].getString();
			valueProcessInfo.soureTable[U"Offset"] = object[U"Offset"].getString();
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
		}
		if (valueProcessInfo.type == U"threeDoublesFromVec3")
		{
			valueProcessInfo.soureTable[U"to1"] = object[U"to1"].getString();
			valueProcessInfo.soureTable[U"to2"] = object[U"to2"].getString();
			valueProcessInfo.soureTable[U"to3"] = object[U"to3"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"ZScale"] = object[U"ZScale"].getString();
			valueProcessInfo.soureTable[U"XOffset"] = object[U"XOffset"].getString();
			valueProcessInfo.soureTable[U"YOffset"] = object[U"YOffset"].getString();
			valueProcessInfo.soureTable[U"ZOffset"] = object[U"ZOffset"].getString();
			valueProcessInfo.soureTable[U"vec3"] = object[U"vec3"].getString();
		}
		if (valueProcessInfo.type == U"doubleFromTwoDoubles")
		{
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
			valueProcessInfo.soureTable[U"scale1"] = object[U"scale1"].getString();
			valueProcessInfo.soureTable[U"scale2"] = object[U"scale2"].getString();
			valueProcessInfo.soureTable[U"offset"] = object[U"offset"].getString();
			valueProcessInfo.soureTable[U"source1"] = object[U"source1"].getString();
			valueProcessInfo.soureTable[U"source2"] = object[U"source2"].getString();
		}

		valueProcessInfos[valueProcessInfo.name] = valueProcessInfo;
	}
	data.valueProcessData.valueProcessInfos = valueProcessInfos;


	Array<String> valueProcessSequence;
	for (const auto& object : settingsToml[U"ValueProcessor.valueProcessSequence"].tableArrayView())
	{
		valueProcessSequence << object[U"label"].getString();
	}
	data.valueProcessData.valueProcessSequence = valueProcessSequence;
}

void LoadData(CommonData& data)
{
	LoadSystemSettings(data);

	LoadUserSettings(data);
}

void Update(CommonData& data)
{
	UpdateInformations(data);

	UpdateValueProcessor(data);

	SendMotorInfo(data);

	ClearPrint();
	//Print << data.valueProcessData.valueProcessSequence;

	UpdateButtonGroup(data, U"menu");

	if (const auto value = GetActiveButtonNameFromButtonGroup(data, U"menu"))
	{
		auto name = value.value();

		UpdateButtonGroup(data, name);
	}
}

void Draw(CommonData& data)
{
	DrawButtonGroup(data, U"menu");

	if (const auto value = GetActiveButtonNameFromButtonGroup(data, U"menu"))
	{
		auto name = value.value();

		DrawButtonGroup(data, name);

		if (const auto channelValue = GetActiveButtonNameFromButtonGroup(data, name))
		{
			auto channelName = channelValue.value();

			if (data.channelFunctionTable.find(channelName) != data.channelFunctionTable.end())
			{
				auto t = Transformer2D(Mat3x2::Translate(Vec2(GetValue<int32>(data, U"Scene.ChannelWidth"), 0.0)), true);

				data.channelFunctionTable[channelName](data);
			}
		}
	}
}

void UpdateAndDraw(CommonData& data)
{
	Update(data);

	DrawBackground(data);

	{
		// �K�E�X�ڂ����p�e�N�X�`���ɂ�����x�V�[����`��
		data.gaussianA1.clear(ColorF(0.0));
		{
			ScopedRenderTarget2D target(data.gaussianA1);

			Draw(data);
		}

		// �I���W�i���T�C�Y�̃K�E�X�ڂ��� (A1)
		// A1 �� 1/4 �T�C�Y�ɂ��ăK�E�X�ڂ��� (A4)
		// A4 �� 1/2 �T�C�Y�ɂ��ăK�E�X�ڂ��� (A8)
		Shader::GaussianBlur(data.gaussianA1,data.gaussianB1, data.gaussianA1);
		//Shader::Downsample(data.gaussianA1, data.gaussianA4);
		//Shader::GaussianBlur(data.gaussianA4, data.gaussianB4, data.gaussianA4);
	}

	{
		ScopedRenderStates2D blend(BlendState::Subtractive);
		data.gaussianA1.resized(Scene::Size()).draw(ColorF(1.0, 0.3));
	}

	Draw(data);
}
#include <Siv3D.hpp>

#include "GUI.hpp"

void Main()
{
	CommonData data;

	LoadData(data);

	while (System::Update())
	{
		if (KeySpace.down())
		{
			TorqueEnableMotors(data);
		}

		UpdateAndDraw(data);
	}

	TorqueDisableMotors(data);
}

#pragma once

#include <Siv3D.hpp>

/// Spherical Linear Interpolation
	/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
	/// Reference: Shoemake at SIGGRAPH 89
	/// See also
	/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
Vec4 QuaternionSlerp(double t, const Vec4& from, const Vec4& to) {
	const double epsilon = 0.00001;
	double omega, cosomega, sinomega, scale_from, scale_to;

	Vec4 quatTo(to);
	// this is a dot product

	cosomega = from.dot(to);//from.asVec4().dot(to.asVec4());

	if (cosomega < 0.0)
	{
		cosomega = -cosomega;
		quatTo = -to;
	}

	if ((1.0 - cosomega) > epsilon)
	{
		omega = acos(cosomega); // 0 <= omega <= Pi (see man acos)
		sinomega = sin(omega);  // this sinomega should always be +ve so
		// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
		scale_from = sin((1.0 - t) * omega) / sinomega;
		scale_to = sin(t * omega) / sinomega;
	}
	else
	{
		/* --------------------------------------------------
		The ends of the vectors are very close
		we can use simple linear interpolation - no need
		to worry about the "spherical" interpolation
		-------------------------------------------------- */
		scale_from = 1.0 - t;
		scale_to = t;
	}

	return from * scale_from + quatTo * scale_to;
}


inline Mat4x4 GetRotatationMatrix(const Vec4& q)
{
	double length2 = q.lengthSq();
	Mat4x4 _mat = Mat4x4::Identity();

	const double QX = q.x;
	const double QY = q.y;
	const double QZ = q.z;
	const double QW = q.w;


	if (fabs(length2) <= std::numeric_limits<double>::min())
	{

	}
	else
	{
		double rlength2;
		// normalize quat if required.
		// We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
		// That is a square of a square root, so it is possible to avoid that
		if (length2 != 1.0)
		{
			rlength2 = 2.0 / length2;
		}
		else
		{
			rlength2 = 2.0;
		}

		// Source: Gamasutra, Rotating Objects Using Quaternions
		//
		//http://www.gamasutra.com/features/19980703/quaternions_01.htm

		double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		// calculate coefficients
		x2 = rlength2 * QX;
		y2 = rlength2 * QY;
		z2 = rlength2 * QZ;

		xx = QX * x2;
		xy = QX * y2;
		xz = QX * z2;

		yy = QY * y2;
		yz = QY * z2;
		zz = QZ * z2;

		wx = QW * x2;
		wy = QW * y2;
		wz = QW * z2;

		// Note.  Gamasutra gets the matrix assignments inverted, resulting
		// in left-handed rotations, which is contrary to OpenGL and OSG's
		// methodology.  The matrix assignment has been altered in the next
		// few lines of code to do the right thing.
		// Don Burns - Oct 13, 2001

		const double mat00 = 1.0 - (yy + zz);
		const double mat10 = xy - wz;
		const double mat20 = xz + wy;


		const double mat01 = xy + wz;
		const double mat11 = 1.0 - (xx + zz);
		const double mat21 = yz - wx;

		const double mat02 = xz - wy;
		const double mat12 = yz + wx;
		const double mat22 = 1.0 - (xx + yy);

		_mat = Mat4x4(mat00, mat01, mat02, 0.0,
			mat10, mat11, mat12, 0.0,
			mat20, mat21, mat22, 0.0,
			0.0, 0.0, 0.0, 1.0);
	}

	return _mat;
}



inline double InterpolateBezier(double x1, double x2, double y1, double y2, double x)
{
	double t = 0.5;
	double s = 0.5;

	for (size_t i = 0; i < 15; ++i)
	{
		const double ft = (3 * s * s * t * x1) + (3 * s * t * t * x2) + (t * t * t) - x;

		if (Math::Abs(ft) < 0.00001)
		{
			break;
		}

		if (ft > 0.0)
		{
			t -= 1 / Math::Pow(2.0, 2.0 + i);
		}
		else
		{
			t += 1 / Math::Pow(2.0, 2.0 + i);
		}

		s = 1.0 - t;
	}

	return (3 * s * s * t * y1) + (3 * s * t * t * y2) + (t * t * t);
}

struct BodyKeyFrame
{
	size_t time;
	Vec3 pos;
	Vec4 rotation;
	Optional<Vec4> rotationBezier;
};

struct MorphKeyFrame
{
	size_t time;
	double value;
};

struct BodySequence
{
	String name;
	Array<BodyKeyFrame> bodyKeyFrames;
};

struct MorphSequence
{
	String name;
	Array<MorphKeyFrame> morphKeyFrames;
};

struct AnimationData
{
	String name;
	HashTable<String, BodySequence> BodySequences;
	HashTable<String, MorphSequence> MorphSequences;
};


std::pair<size_t, size_t> GetTimeIndex(size_t timeFrame, const BodySequence& seq)
{
	if (seq.bodyKeyFrames.size() <= 1)
	{
		return std::pair<size_t, size_t>(0, 0);
	}

	size_t startIndex = 0;
	size_t endIndex = seq.bodyKeyFrames.size() - 1;
	size_t index = static_cast<size_t>(endIndex / 2);

	while (endIndex - startIndex != 1)
	{
		if (seq.bodyKeyFrames[index].time > timeFrame)
		{
			endIndex = index;
		}
		else
		{
			startIndex = index;
		}

		index = startIndex + (endIndex - startIndex) / 2;
	}

	if (index == 0)
	{
		return std::pair<size_t, size_t>(0, 0);
	}
	else
	{
		return std::pair<size_t, size_t>(startIndex, endIndex);
	}
}

std::pair<size_t, size_t> GetTimeIndex(size_t timeFrame, const MorphSequence& seq)
{
	if (seq.morphKeyFrames.size() <= 1)
	{
		return std::pair<size_t, size_t>(0, 0);
	}

	size_t startIndex = 0;
	size_t endIndex = seq.morphKeyFrames.size() - 1;
	size_t index = static_cast<size_t>(endIndex / 2);

	while (endIndex - startIndex != 1)
	{
		if (seq.morphKeyFrames[index].time > timeFrame)
		{
			endIndex = index;
		}
		else
		{
			startIndex = index;
		}

		index = startIndex + (endIndex - startIndex) / 2;
	}

	if (index == 0)
	{
		return std::pair<size_t, size_t>(0, 0);
	}
	else
	{
		return std::pair<size_t, size_t>(startIndex, endIndex);
	}
}

double GetCurrentMorphValue(size_t timeFrame, const MorphSequence& seq)
{
	const auto timeIndex = GetTimeIndex(timeFrame, seq);

	if (timeIndex.second == 0)
	{
		return 0.0;
	}
	else
	{
		const auto& startKeyFrame = seq.morphKeyFrames[timeIndex.first];
		const auto& endKeyFrame = seq.morphKeyFrames[timeIndex.second];

		const double startValue = startKeyFrame.value;
		const double endValue = endKeyFrame.value;

		const size_t offset = endKeyFrame.time - startKeyFrame.time;
		const double ratio = Clamp(static_cast<double>(timeFrame - startKeyFrame.time) / static_cast<double>(offset), 0.0, 1.0);

		Print << ratio;

		return  (endValue - startValue) * ratio + startValue;
	}
}

Vec4 GetCurrentRotationQuaternion(size_t timeFrame, const BodySequence& seq)
{
	const auto timeIndex = GetTimeIndex(timeFrame, seq);

	Print << timeIndex;

	if (timeIndex.second == 0)
	{
		return Vec4(0.0, 0.0, 0.0, 1.0);
	}
	else
	{
		const auto& startKeyFrame = seq.bodyKeyFrames[timeIndex.first];
		const auto& endKeyFrame = seq.bodyKeyFrames[timeIndex.second];

		const Vec4 startRotation = startKeyFrame.rotation;
		const Vec4 endRotation = endKeyFrame.rotation;

		const size_t offset = endKeyFrame.time - startKeyFrame.time;
		const double ratio = Clamp(static_cast<double>(timeFrame - startKeyFrame.time) / static_cast<double>(offset), 0.0, 1.0);


		if (startKeyFrame.rotationBezier)
		{
			const auto rotationBezier = startKeyFrame.rotationBezier.value();

			const double tRatio = InterpolateBezier(rotationBezier.x, rotationBezier.z, rotationBezier.y, rotationBezier.w, ratio);


			Print << tRatio;

			return QuaternionSlerp(tRatio, startRotation, endRotation);
		}
		else
		{
			return QuaternionSlerp(ratio, startRotation, endRotation);
		}
	}
}

Mat4x4 GetCurrentRotationMatrix(size_t timeFrame, const BodySequence& seq)
{
	return GetRotatationMatrix(GetCurrentRotationQuaternion(timeFrame, seq));
}


AnimationData GetAnimationData(const FilePathView& path)
{

	// XML �t�@�C������f�[�^��ǂݍ���
	const XMLReader xml(path);
	
	if (!xml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.xml`");
	}

	// XML �f�[�^�����ׂĕ\��
	//ShowElements(xml);

	AnimationData animationData;
	animationData.name = path;

	for (auto e = xml.firstChild(); e; e = e.nextSibling())
	{

		if (e.name() == U"boneMotionSequence")
		{
			//Console << U"Name: <{}>"_fmt(e.name());

			for (auto bodyPart = e.firstChild(); bodyPart; bodyPart = bodyPart.nextSibling())
			{

				const String bodyPartName = bodyPart.attributes()[0].second;

				if (bodyPartName == U"�Z���^�["
					|| bodyPartName == U"�㔼�g"
					|| bodyPartName == U"��"
					|| bodyPartName == U"��"
					|| bodyPartName == U"����"
					|| bodyPartName == U"���r"
					|| bodyPartName == U"���Ђ�"
					|| bodyPartName == U"�����"
					|| bodyPartName == U"�E��"
					|| bodyPartName == U"�E�r"
					|| bodyPartName == U"�E�Ђ�"
					|| bodyPartName == U"�E���")
				{
					BodySequence seq;
					seq.name = bodyPartName;

					//Console << U"Name: <{}>"_fmt(bodyPart.attributes()[0].second);

					for (auto boneMotion = bodyPart.firstChild(); boneMotion; boneMotion = boneMotion.nextSibling())
					{
						BodyKeyFrame keyFrame;

						const auto& attributes = boneMotion.attributes();

						//Console << U"Value: <{}>"_fmt(boneMotion.attributes());

						keyFrame.time = Parse<size_t>(attributes[0].second);

						for (auto info = boneMotion.firstChild(); info; info = info.nextSibling())
						{
							//Console << info.name() << U": <{}>"_fmt(info.attributes());

							const auto& infoAttributes = info.attributes();

							if (info.name() == U"bonePosition")
							{
								keyFrame.pos = Vec3(Parse<double>(infoAttributes[0].second)
									, Parse<double>(infoAttributes[1].second)
									, Parse<double>(infoAttributes[2].second));
							}

							if (info.name() == U"boneRotQuat")
							{
								keyFrame.rotation = Vec4(
									Parse<double>(infoAttributes[0].second)
									, Parse<double>(infoAttributes[1].second)
									, Parse<double>(infoAttributes[2].second)
									, Parse<double>(infoAttributes[3].second));

								const auto& interpolationInfo = info.firstChild();

								if (interpolationInfo)
								{
									const auto& interpolationAttributes = interpolationInfo.attributes();

									const Vec4 interpolationValues(
										Parse<double>(interpolationAttributes[0].second),
										Parse<double>(interpolationAttributes[1].second),
										Parse<double>(interpolationAttributes[2].second),
										Parse<double>(interpolationAttributes[3].second));

									keyFrame.rotationBezier = interpolationValues / 127.0;
								}

							}
						}

						seq.bodyKeyFrames << keyFrame;
					}


					animationData.BodySequences[seq.name] = seq;
				}
			}

		}

		if (e.name() == U"morphSequence")
		{
			for (auto morphPart = e.firstChild(); morphPart; morphPart = morphPart.nextSibling())
			{
				if (morphPart.attributes()[0].second == U"�܂΂���")
				{
					MorphSequence seq;
					seq.name = morphPart.attributes()[0].second;

					//Console << U"Name: <{}>"_fmt(bodyPart.attributes()[0].second);

					for (auto morphMotion = morphPart.firstChild(); morphMotion; morphMotion = morphMotion.nextSibling())
					{
						MorphKeyFrame keyFrame;

						//Console << U"Value: <{}>"_fmt(boneMotion.attributes());

						keyFrame.time = Parse<size_t>(morphMotion.attributes()[0].second);
						keyFrame.value = Parse<double>(morphMotion.attributes()[1].second);

						seq.morphKeyFrames << keyFrame;
					}


					animationData.MorphSequences[seq.name] = seq;
				}
			}
		}
	}

	return animationData;
}

#pragma once

#include "Solver.hpp"

#include <Siv3D.hpp>


namespace Codegen
{
	void computeNeckMechanismEnergy(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = -1;
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = (_i_var3) * (_i_var0);
		_i_var7 = std::sin(neckAngles(0, 0));
		_i_var8 = -(headConnectParameters(2, 0));
		_i_var9 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var10 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var11 = (_i_var4) * (motorConnectParameters(2, 0));
		_i_var12 = std::sin(neckAngles(1, 0));
		_i_var13 = (_i_var6)+(_i_var5);
		_i_var14 = std::cos(neckAngles(1, 0));
		_i_var15 = (_i_var8) * (_i_var7);
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(1, 0));
		_i_var18 = -(_i_var11);
		_i_var19 = (_i_var13) * (_i_var12);
		_i_var20 = (_i_var15) * (_i_var14);
		_i_var21 = -(_i_var15);
		_i_var22 = std::sin(motorAngles(0, 0));
		_i_var23 = -(motorConnectParameters(2, 0));
		_i_var24 = (_i_var16) * (_i_var12);
		_i_var25 = (_i_var1)-(_i_var14);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var12);
		_i_var30 = (_i_var13) * (_i_var14);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var14);
		_i_var34 = std::cos(motorAngles(1, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var14);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = -(motorConnectParameters(0, 0));
		_i_var40 = (_i_var30)+(_i_var29);
		_i_var41 = std::cos(motorAngles(0, 0));
		_i_var42 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var43 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var40)-(_i_var39);
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var41);
		_i_var50 = (_i_var43)-(_i_var42);
		_i_var51 = (_i_var44)-(motorConnectParameters(0, 0));
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var54)+(_i_var53);
		_i_var60 = (_i_var55) * (_i_var55);
		_i_var61 = (_i_var57)+(_i_var56);
		_i_var62 = (_i_var59)+(_i_var58);
		_i_var63 = (_i_var61)+(_i_var60);
		_i_var64 = std::sqrt(_i_var62);
		_i_var65 = std::sqrt(_i_var63);
		_i_var66 = (_i_var64)-(rodDistance);
		_i_var67 = (_i_var65)-(rodDistance);
		_i_var68 = (_i_var66) * (_i_var66);
		_i_var69 = (_i_var67) * (_i_var67);
		_i_var70 = (_i_var69)+(_i_var68);
		_i_var71 = 0.5;
		_i_var72 = (_i_var71) * (_i_var70);
		energy = _i_var72;
	}
	void computeNeckMechanismEnergyGradient(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		Eigen::Matrix<double, 2, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var5 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var6 = std::sin(neckAngles(0, 0));
		_i_var7 = -(headConnectParameters(2, 0));
		_i_var8 = -1;
		_i_var9 = (_i_var3) * (_i_var2);
		_i_var10 = (_i_var3) * (_i_var0);
		_i_var11 = std::sin(neckAngles(1, 0));
		_i_var12 = (_i_var5)+(_i_var4);
		_i_var13 = std::cos(neckAngles(1, 0));
		_i_var14 = (_i_var7) * (_i_var6);
		_i_var15 = (_i_var8) * (motorConnectParameters(2, 0));
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(0, 0));
		_i_var18 = -(motorConnectParameters(2, 0));
		_i_var19 = (_i_var12) * (_i_var11);
		_i_var20 = (_i_var14) * (_i_var13);
		_i_var21 = -(_i_var14);
		_i_var22 = std::sin(motorAngles(1, 0));
		_i_var23 = -(_i_var15);
		_i_var24 = (_i_var16) * (_i_var11);
		_i_var25 = (_i_var1)-(_i_var13);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var11);
		_i_var30 = (_i_var12) * (_i_var13);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var13);
		_i_var34 = std::cos(motorAngles(0, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var13);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = (_i_var30)+(_i_var29);
		_i_var40 = std::cos(motorAngles(1, 0));
		_i_var41 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var42 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var43 = -(motorConnectParameters(0, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var39)-(motorConnectParameters(0, 0));
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var40);
		_i_var50 = (_i_var42)-(_i_var41);
		_i_var51 = (_i_var44)-(_i_var43);
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var54)+(_i_var53);
		_i_var60 = (_i_var55) * (_i_var55);
		_i_var61 = (_i_var57)+(_i_var56);
		_i_var62 = (_i_var59)+(_i_var58);
		_i_var63 = (_i_var61)+(_i_var60);
		_i_var64 = std::sqrt(_i_var62);
		_i_var65 = std::sqrt(_i_var63);
		_i_var66 = 2;
		_i_var67 = (_i_var64)-(rodDistance);
		_i_var68 = 0.5;
		_i_var69 = (_i_var65)-(rodDistance);
		_i_var70 = (_i_var66) * (_i_var64);
		_i_var71 = (_i_var68) * (_i_var67);
		_i_var72 = (_i_var66) * (_i_var65);
		_i_var73 = (_i_var68) * (_i_var69);
		_i_var74 = (_i_var1) / (_i_var70);
		_i_var75 = (_i_var66) * (_i_var71);
		_i_var76 = (_i_var1) / (_i_var72);
		_i_var77 = (_i_var66) * (_i_var73);
		_i_var78 = (_i_var75) * (_i_var74);
		_i_var79 = (_i_var77) * (_i_var76);
		_i_var80 = (_i_var78) * (_i_var47);
		_i_var81 = (_i_var78) * (_i_var52);
		_i_var82 = (_i_var79) * (_i_var50);
		_i_var83 = (_i_var79) * (_i_var55);
		_i_var84 = (_i_var66) * (_i_var80);
		_i_var85 = (_i_var66) * (_i_var81);
		_i_var86 = (_i_var66) * (_i_var82);
		_i_var87 = (_i_var66) * (_i_var83);
		_i_var88 = (_i_var84) * (_i_var8);
		_i_var89 = (_i_var85) * (_i_var8);
		_i_var90 = (_i_var86) * (_i_var8);
		_i_var91 = (_i_var87) * (_i_var8);
		_i_var92 = (_i_var88) * (_i_var18);
		_i_var93 = -(_i_var17);
		_i_var94 = (_i_var89) * (motorConnectParameters(2, 0));
		_i_var95 = (_i_var90) * (_i_var23);
		_i_var96 = -(_i_var22);
		_i_var97 = (_i_var91) * (motorConnectParameters(2, 0));
		_i_var98 = (_i_var92) * (_i_var34);
		_i_var99 = (_i_var94) * (_i_var93);
		_i_var100 = (_i_var95) * (_i_var40);
		_i_var101 = (_i_var97) * (_i_var96);
		_i_var102 = (_i_var99)+(_i_var98);
		_i_var103 = (_i_var101)+(_i_var100);
		gradient(0, 0) = _i_var102;
		gradient(1, 0) = _i_var103;
	}
	void computeNeckMechanismEnergyHessian(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		Eigen::Matrix<double, 2, 2>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var5 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var6 = std::sin(neckAngles(0, 0));
		_i_var7 = -(headConnectParameters(2, 0));
		_i_var8 = -1;
		_i_var9 = (_i_var3) * (_i_var2);
		_i_var10 = (_i_var3) * (_i_var0);
		_i_var11 = std::sin(neckAngles(1, 0));
		_i_var12 = (_i_var5)+(_i_var4);
		_i_var13 = std::cos(neckAngles(1, 0));
		_i_var14 = (_i_var7) * (_i_var6);
		_i_var15 = (_i_var8) * (motorConnectParameters(2, 0));
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(0, 0));
		_i_var18 = -(motorConnectParameters(2, 0));
		_i_var19 = (_i_var12) * (_i_var11);
		_i_var20 = (_i_var14) * (_i_var13);
		_i_var21 = -(_i_var14);
		_i_var22 = std::sin(motorAngles(1, 0));
		_i_var23 = -(_i_var15);
		_i_var24 = (_i_var16) * (_i_var11);
		_i_var25 = (_i_var1)-(_i_var13);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var11);
		_i_var30 = (_i_var12) * (_i_var13);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var13);
		_i_var34 = std::cos(motorAngles(0, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var13);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = (_i_var30)+(_i_var29);
		_i_var40 = std::cos(motorAngles(1, 0));
		_i_var41 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var42 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var43 = -(motorConnectParameters(0, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var39)-(motorConnectParameters(0, 0));
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var40);
		_i_var50 = (_i_var42)-(_i_var41);
		_i_var51 = (_i_var44)-(_i_var43);
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = -(_i_var17);
		_i_var59 = (_i_var52) * (_i_var52);
		_i_var60 = (_i_var54)+(_i_var53);
		_i_var61 = -(_i_var22);
		_i_var62 = (_i_var55) * (_i_var55);
		_i_var63 = (_i_var57)+(_i_var56);
		_i_var64 = (_i_var34) * (_i_var18);
		_i_var65 = (_i_var58) * (motorConnectParameters(2, 0));
		_i_var66 = (_i_var60)+(_i_var59);
		_i_var67 = (_i_var40) * (_i_var23);
		_i_var68 = (_i_var61) * (motorConnectParameters(2, 0));
		_i_var69 = (_i_var63)+(_i_var62);
		_i_var70 = 2;
		_i_var71 = (_i_var64) * (_i_var8);
		_i_var72 = (_i_var65) * (_i_var8);
		_i_var73 = std::sqrt(_i_var66);
		_i_var74 = (_i_var67) * (_i_var8);
		_i_var75 = (_i_var68) * (_i_var8);
		_i_var76 = std::sqrt(_i_var69);
		_i_var77 = (_i_var71) * (_i_var70);
		_i_var78 = (_i_var72) * (_i_var70);
		_i_var79 = (_i_var70) * (_i_var73);
		_i_var80 = (_i_var73)-(rodDistance);
		_i_var81 = 0.5;
		_i_var82 = (_i_var74) * (_i_var70);
		_i_var83 = (_i_var75) * (_i_var70);
		_i_var84 = (_i_var70) * (_i_var76);
		_i_var85 = (_i_var76)-(rodDistance);
		_i_var86 = (_i_var77) * (_i_var47);
		_i_var87 = (_i_var78) * (_i_var52);
		_i_var88 = (_i_var79) * (_i_var79);
		_i_var89 = (_i_var81) * (_i_var80);
		_i_var90 = (_i_var82) * (_i_var50);
		_i_var91 = (_i_var83) * (_i_var55);
		_i_var92 = (_i_var84) * (_i_var84);
		_i_var93 = (_i_var81) * (_i_var85);
		_i_var94 = (_i_var1) / (_i_var79);
		_i_var95 = (_i_var87)+(_i_var86);
		_i_var96 = (_i_var1) / (_i_var88);
		_i_var97 = (_i_var70) * (_i_var89);
		_i_var98 = (_i_var1) / (_i_var84);
		_i_var99 = (_i_var91)+(_i_var90);
		_i_var100 = (_i_var1) / (_i_var92);
		_i_var101 = (_i_var70) * (_i_var93);
		_i_var102 = (_i_var95) * (_i_var94);
		_i_var103 = -(_i_var96);
		_i_var104 = (_i_var95) * (_i_var97);
		_i_var105 = (_i_var99) * (_i_var98);
		_i_var106 = -(_i_var100);
		_i_var107 = (_i_var99) * (_i_var101);
		_i_var108 = (_i_var102) * (_i_var70);
		_i_var109 = (_i_var104) * (_i_var103);
		_i_var110 = (_i_var105) * (_i_var70);
		_i_var111 = (_i_var107) * (_i_var106);
		_i_var112 = (_i_var108) * (_i_var81);
		_i_var113 = (_i_var109) * (_i_var70);
		_i_var114 = (_i_var110) * (_i_var81);
		_i_var115 = (_i_var111) * (_i_var70);
		_i_var116 = (_i_var113)+(_i_var112);
		_i_var117 = (_i_var115)+(_i_var114);
		_i_var118 = (_i_var116) * (_i_var94);
		_i_var119 = (_i_var97) * (_i_var94);
		_i_var120 = (_i_var117) * (_i_var98);
		_i_var121 = (_i_var101) * (_i_var98);
		_i_var122 = (_i_var118) * (_i_var47);
		_i_var123 = (_i_var77) * (_i_var119);
		_i_var124 = (_i_var119) * (_i_var52);
		_i_var125 = (_i_var118) * (_i_var52);
		_i_var126 = (_i_var78) * (_i_var119);
		_i_var127 = (_i_var120) * (_i_var50);
		_i_var128 = (_i_var82) * (_i_var121);
		_i_var129 = (_i_var121) * (_i_var55);
		_i_var130 = (_i_var120) * (_i_var55);
		_i_var131 = (_i_var83) * (_i_var121);
		_i_var132 = (_i_var123)+(_i_var122);
		_i_var133 = (_i_var70) * (_i_var124);
		_i_var134 = (_i_var126)+(_i_var125);
		_i_var135 = (_i_var119) * (_i_var47);
		_i_var136 = (_i_var128)+(_i_var127);
		_i_var137 = (_i_var70) * (_i_var129);
		_i_var138 = (_i_var131)+(_i_var130);
		_i_var139 = (_i_var121) * (_i_var50);
		_i_var140 = (_i_var132)+(_i_var122);
		_i_var141 = (_i_var133) * (_i_var8);
		_i_var142 = (_i_var134)+(_i_var125);
		_i_var143 = (_i_var70) * (_i_var135);
		_i_var144 = (_i_var136)+(_i_var127);
		_i_var145 = (_i_var137) * (_i_var8);
		_i_var146 = (_i_var138)+(_i_var130);
		_i_var147 = (_i_var70) * (_i_var139);
		_i_var148 = (_i_var140) * (_i_var8);
		_i_var149 = (_i_var141) * (motorConnectParameters(2, 0));
		_i_var150 = (_i_var142) * (_i_var8);
		_i_var151 = (_i_var143) * (_i_var8);
		_i_var152 = (_i_var144) * (_i_var8);
		_i_var153 = (_i_var145) * (motorConnectParameters(2, 0));
		_i_var154 = (_i_var146) * (_i_var8);
		_i_var155 = (_i_var147) * (_i_var8);
		_i_var156 = (_i_var148) * (_i_var18);
		_i_var157 = (_i_var149) * (_i_var8);
		_i_var158 = (_i_var150) * (motorConnectParameters(2, 0));
		_i_var159 = (_i_var151) * (_i_var18);
		_i_var160 = (_i_var152) * (_i_var23);
		_i_var161 = (_i_var153) * (_i_var8);
		_i_var162 = (_i_var154) * (motorConnectParameters(2, 0));
		_i_var163 = (_i_var155) * (_i_var23);
		_i_var164 = (_i_var157)+(_i_var156);
		_i_var165 = (_i_var159)+(_i_var158);
		_i_var166 = (_i_var161)+(_i_var160);
		_i_var167 = (_i_var163)+(_i_var162);
		_i_var168 = (_i_var164) * (_i_var34);
		_i_var169 = (_i_var165) * (_i_var58);
		_i_var170 = (_i_var166) * (_i_var40);
		_i_var171 = (_i_var167) * (_i_var61);
		_i_var172 = (_i_var169)+(_i_var168);
		_i_var173 = 0;
		_i_var174 = (_i_var171)+(_i_var170);
		hessian(0, 0) = _i_var172;
		hessian(1, 0) = _i_var173;
		hessian(0, 1) = _i_var173;
		hessian(1, 1) = _i_var174;
	}

}



namespace Codegen
{
	void compute3AxisKibouLeftArmEnergy(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = (_i_var16) * (_i_var14);
		_i_var24 = (_i_var10) * (_i_var13);
		_i_var25 = (_i_var20)+(_i_var19);
		_i_var26 = (_i_var22)+(_i_var21);
		_i_var27 = (_i_var24)+(_i_var23);
		_i_var28 = (_i_var25)+(offsets(1, 0));
		_i_var29 = (_i_var26)+(offsets(0, 0));
		_i_var30 = (_i_var27)+(offsets(2, 0));
		_i_var31 = (target(1, 0)) - (_i_var28);
		_i_var32 = (target(0, 0)) - (_i_var29);
		_i_var33 = (target(2, 0)) - (_i_var30);
		_i_var34 = (_i_var31) * (_i_var31);
		_i_var35 = (_i_var32) * (_i_var32);
		_i_var36 = (_i_var33) * (_i_var33);
		_i_var37 = (_i_var35)+(_i_var34);
		_i_var38 = (_i_var37)+(_i_var36);
		_i_var39 = std::sqrt(_i_var38);
		_i_var40 = 0.5;
		_i_var41 = (_i_var40) * (_i_var39);
		_i_var42 = (_i_var41) * (_i_var39);
		energy = _i_var42;
	}
	void compute3AxisKibouLeftArmEnergyGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = (_i_var16) * (_i_var14);
		_i_var24 = (_i_var10) * (_i_var13);
		_i_var25 = (_i_var20)+(_i_var19);
		_i_var26 = (_i_var22)+(_i_var21);
		_i_var27 = (_i_var24)+(_i_var23);
		_i_var28 = (_i_var25)+(offsets(1, 0));
		_i_var29 = (_i_var26)+(offsets(0, 0));
		_i_var30 = (_i_var27)+(offsets(2, 0));
		_i_var31 = (target(1, 0)) - (_i_var28);
		_i_var32 = (target(0, 0)) - (_i_var29);
		_i_var33 = (target(2, 0)) - (_i_var30);
		_i_var34 = (_i_var31) * (_i_var31);
		_i_var35 = (_i_var32) * (_i_var32);
		_i_var36 = (_i_var33) * (_i_var33);
		_i_var37 = (_i_var35)+(_i_var34);
		_i_var38 = (_i_var37)+(_i_var36);
		_i_var39 = std::sqrt(_i_var38);
		_i_var40 = 2;
		_i_var41 = 0.5;
		_i_var42 = (_i_var40) * (_i_var39);
		_i_var43 = (_i_var39) * (_i_var41);
		_i_var44 = (_i_var41) * (_i_var39);
		_i_var45 = (_i_var1) / (_i_var42);
		_i_var46 = (_i_var44)+(_i_var43);
		_i_var47 = (_i_var46) * (_i_var45);
		_i_var48 = (_i_var47) * (_i_var31);
		_i_var49 = -1;
		_i_var50 = (_i_var40) * (_i_var48);
		_i_var51 = (_i_var47) * (_i_var33);
		_i_var52 = (_i_var47) * (_i_var32);
		_i_var53 = (_i_var50) * (_i_var49);
		_i_var54 = (_i_var40) * (_i_var51);
		_i_var55 = (_i_var40) * (_i_var52);
		_i_var56 = (_i_var53) * (_i_var14);
		_i_var57 = (_i_var54) * (_i_var49);
		_i_var58 = (_i_var55) * (_i_var49);
		_i_var59 = (_i_var56) * (_i_var49);
		_i_var60 = (_i_var57) * (_i_var13);
		_i_var61 = (_i_var58) * (_i_var17);
		_i_var62 = (_i_var58) * (_i_var13);
		_i_var63 = (_i_var53) * (_i_var13);
		_i_var64 = (_i_var57) * (_i_var14);
		_i_var65 = (_i_var58) * (_i_var18);
		_i_var66 = (_i_var57) * (_i_var10);
		_i_var67 = (_i_var60)+(_i_var59);
		_i_var68 = (_i_var62)+(_i_var61);
		_i_var69 = (_i_var64)+(_i_var63);
		_i_var70 = (_i_var53) * (_i_var16);
		_i_var71 = (_i_var66)+(_i_var65);
		_i_var72 = (_i_var67) * (_i_var5);
		_i_var73 = (_i_var68) * (_i_var12);
		_i_var74 = (_i_var67) * (_i_var4);
		_i_var75 = (_i_var67) * (_i_var0);
		_i_var76 = (_i_var69) * (_i_var11);
		_i_var77 = (_i_var68) * (_i_var0);
		_i_var78 = (_i_var65) * (_i_var49);
		_i_var79 = (_i_var71)+(_i_var70);
		_i_var80 = (_i_var53) * (_i_var15);
		_i_var81 = (_i_var57) * (_i_var16);
		_i_var82 = (_i_var72) * (_i_var49);
		_i_var83 = (_i_var73)+(_i_var72);
		_i_var84 = (_i_var75)+(_i_var74);
		_i_var85 = (_i_var77)+(_i_var76);
		_i_var86 = -(_i_var14);
		_i_var87 = (_i_var79)+(_i_var78);
		_i_var88 = (_i_var81)+(_i_var80);
		_i_var89 = -(_i_var11);
		_i_var90 = (_i_var83)+(_i_var82);
		_i_var91 = (_i_var69) * (_i_var12);
		_i_var92 = (_i_var84) * (_i_var3);
		_i_var93 = -(_i_var2);
		_i_var94 = (_i_var85) * (offsets(4, 0));
		_i_var95 = (_i_var87) * (_i_var86);
		_i_var96 = (_i_var88) * (_i_var13);
		_i_var97 = (_i_var90) * (_i_var89);
		_i_var98 = (_i_var91) * (_i_var0);
		_i_var99 = (_i_var92) * (_i_var6);
		_i_var100 = (_i_var94) * (_i_var93);
		_i_var101 = (_i_var96)+(_i_var95);
		_i_var102 = (_i_var98)+(_i_var97);
		_i_var103 = (_i_var100)+(_i_var99);
		gradient(0, 0) = _i_var101;
		gradient(1, 0) = _i_var102;
		gradient(2, 0) = _i_var103;
	}
	void compute3AxisKibouLeftArmEnergyHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = -1;
		_i_var24 = -(_i_var11);
		_i_var25 = (_i_var16) * (_i_var14);
		_i_var26 = (_i_var10) * (_i_var13);
		_i_var27 = (_i_var20)+(_i_var19);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var6) * (_i_var3);
		_i_var30 = -(_i_var2);
		_i_var31 = (_i_var24) * (_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var27)+(offsets(1, 0));
		_i_var34 = (_i_var28)+(offsets(0, 0));
		_i_var35 = (_i_var29) * (_i_var4);
		_i_var36 = (_i_var29) * (_i_var0);
		_i_var37 = (_i_var30) * (offsets(4, 0));
		_i_var38 = (_i_var31)-(_i_var11);
		_i_var39 = (_i_var32)+(offsets(2, 0));
		_i_var40 = (target(1, 0)) - (_i_var33);
		_i_var41 = (target(0, 0)) - (_i_var34);
		_i_var42 = (_i_var36)+(_i_var35);
		_i_var43 = (_i_var37) * (_i_var11);
		_i_var44 = (_i_var37) * (_i_var0);
		_i_var45 = (_i_var38) * (_i_var5);
		_i_var46 = (_i_var0) * (_i_var12);
		_i_var47 = (_i_var24) * (_i_var12);
		_i_var48 = (target(2, 0)) - (_i_var39);
		_i_var49 = (_i_var40) * (_i_var40);
		_i_var50 = (_i_var41) * (_i_var41);
		_i_var51 = (_i_var42) * (_i_var23);
		_i_var52 = (_i_var42) * (_i_var13);
		_i_var53 = (_i_var43) * (_i_var14);
		_i_var54 = (_i_var44) * (_i_var17);
		_i_var55 = (_i_var44) * (_i_var13);
		_i_var56 = -(_i_var14);
		_i_var57 = (_i_var45) * (_i_var23);
		_i_var58 = (_i_var45) * (_i_var13);
		_i_var59 = (_i_var46) * (_i_var14);
		_i_var60 = (_i_var47) * (_i_var17);
		_i_var61 = (_i_var47) * (_i_var13);
		_i_var62 = (_i_var48) * (_i_var48);
		_i_var63 = (_i_var50)+(_i_var49);
		_i_var64 = (_i_var51) * (_i_var14);
		_i_var65 = (_i_var43) * (_i_var13);
		_i_var66 = (_i_var53)+(_i_var52);
		_i_var67 = (_i_var55)+(_i_var54);
		_i_var68 = (_i_var56) * (_i_var23);
		_i_var69 = (_i_var56) * (_i_var10);
		_i_var70 = (_i_var13) * (_i_var16);
		_i_var71 = (_i_var56) * (_i_var16);
		_i_var72 = (_i_var13) * (_i_var15);
		_i_var73 = (_i_var57) * (_i_var14);
		_i_var74 = (_i_var46) * (_i_var13);
		_i_var75 = (_i_var59)+(_i_var58);
		_i_var76 = (_i_var61)+(_i_var60);
		_i_var77 = (_i_var63)+(_i_var62);
		_i_var78 = (_i_var65)+(_i_var64);
		_i_var79 = 2;
		_i_var80 = (_i_var66) * (_i_var23);
		_i_var81 = (_i_var67) * (_i_var23);
		_i_var82 = (_i_var68)-(_i_var14);
		_i_var83 = (_i_var70)+(_i_var69);
		_i_var84 = (_i_var72)+(_i_var71);
		_i_var85 = (_i_var74)+(_i_var73);
		_i_var86 = (_i_var75) * (_i_var23);
		_i_var87 = (_i_var76) * (_i_var23);
		_i_var88 = std::sqrt(_i_var77);
		_i_var89 = (_i_var78) * (_i_var23);
		_i_var90 = (_i_var80) * (_i_var79);
		_i_var91 = (_i_var81) * (_i_var79);
		_i_var92 = (_i_var82) * (_i_var18);
		_i_var93 = (_i_var83) * (_i_var23);
		_i_var94 = (_i_var84) * (_i_var23);
		_i_var95 = (_i_var85) * (_i_var23);
		_i_var96 = (_i_var86) * (_i_var79);
		_i_var97 = (_i_var87) * (_i_var79);
		_i_var98 = (_i_var79) * (_i_var88);
		_i_var99 = 0.5;
		_i_var100 = (_i_var89) * (_i_var79);
		_i_var101 = (_i_var90) * (_i_var48);
		_i_var102 = (_i_var91) * (_i_var41);
		_i_var103 = (_i_var92) * (_i_var23);
		_i_var104 = (_i_var93) * (_i_var79);
		_i_var105 = (_i_var94) * (_i_var79);
		_i_var106 = (_i_var95) * (_i_var79);
		_i_var107 = (_i_var96) * (_i_var48);
		_i_var108 = (_i_var97) * (_i_var41);
		_i_var109 = (_i_var98) * (_i_var98);
		_i_var110 = (_i_var88) * (_i_var99);
		_i_var111 = (_i_var99) * (_i_var88);
		_i_var112 = (_i_var100) * (_i_var40);
		_i_var113 = (_i_var102)+(_i_var101);
		_i_var114 = (_i_var103) * (_i_var79);
		_i_var115 = (_i_var104) * (_i_var48);
		_i_var116 = (_i_var105) * (_i_var40);
		_i_var117 = (_i_var106) * (_i_var40);
		_i_var118 = (_i_var108)+(_i_var107);
		_i_var119 = (_i_var1) / (_i_var109);
		_i_var120 = (_i_var111)+(_i_var110);
		_i_var121 = (_i_var113)+(_i_var112);
		_i_var122 = (_i_var1) / (_i_var98);
		_i_var123 = (_i_var114) * (_i_var41);
		_i_var124 = (_i_var116)+(_i_var115);
		_i_var125 = (_i_var118)+(_i_var117);
		_i_var126 = -(_i_var119);
		_i_var127 = (_i_var121) * (_i_var120);
		_i_var128 = (_i_var121) * (_i_var122);
		_i_var129 = (_i_var124)+(_i_var123);
		_i_var130 = (_i_var125) * (_i_var120);
		_i_var131 = (_i_var125) * (_i_var122);
		_i_var132 = (_i_var127) * (_i_var126);
		_i_var133 = (_i_var128) * (_i_var99);
		_i_var134 = (_i_var129) * (_i_var120);
		_i_var135 = (_i_var129) * (_i_var122);
		_i_var136 = (_i_var130) * (_i_var126);
		_i_var137 = (_i_var131) * (_i_var99);
		_i_var138 = (_i_var132) * (_i_var79);
		_i_var139 = (_i_var79) * (_i_var133);
		_i_var140 = (_i_var134) * (_i_var126);
		_i_var141 = (_i_var135) * (_i_var99);
		_i_var142 = (_i_var136) * (_i_var79);
		_i_var143 = (_i_var79) * (_i_var137);
		_i_var144 = (_i_var139)+(_i_var138);
		_i_var145 = (_i_var140) * (_i_var79);
		_i_var146 = (_i_var79) * (_i_var141);
		_i_var147 = (_i_var143)+(_i_var142);
		_i_var148 = (_i_var120) * (_i_var122);
		_i_var149 = (_i_var144) * (_i_var122);
		_i_var150 = (_i_var146)+(_i_var145);
		_i_var151 = (_i_var147) * (_i_var122);
		_i_var152 = (_i_var148) * (_i_var40);
		_i_var153 = (_i_var149) * (_i_var40);
		_i_var154 = (_i_var100) * (_i_var148);
		_i_var155 = (_i_var150) * (_i_var122);
		_i_var156 = (_i_var148) * (_i_var41);
		_i_var157 = (_i_var148) * (_i_var48);
		_i_var158 = (_i_var151) * (_i_var40);
		_i_var159 = (_i_var106) * (_i_var148);
		_i_var160 = (_i_var151) * (_i_var41);
		_i_var161 = (_i_var97) * (_i_var148);
		_i_var162 = (_i_var79) * (_i_var152);
		_i_var163 = (_i_var154)+(_i_var153);
		_i_var164 = (_i_var149) * (_i_var48);
		_i_var165 = (_i_var90) * (_i_var148);
		_i_var166 = (_i_var149) * (_i_var41);
		_i_var167 = (_i_var91) * (_i_var148);
		_i_var168 = (_i_var155) * (_i_var48);
		_i_var169 = (_i_var104) * (_i_var148);
		_i_var170 = (_i_var79) * (_i_var156);
		_i_var171 = (_i_var79) * (_i_var157);
		_i_var172 = (_i_var151) * (_i_var48);
		_i_var173 = (_i_var96) * (_i_var148);
		_i_var174 = (_i_var159)+(_i_var158);
		_i_var175 = (_i_var161)+(_i_var160);
		_i_var176 = (_i_var162) * (_i_var23);
		_i_var177 = (_i_var163)+(_i_var153);
		_i_var178 = (_i_var165)+(_i_var164);
		_i_var179 = (_i_var167)+(_i_var166);
		_i_var180 = (_i_var155) * (_i_var41);
		_i_var181 = (_i_var114) * (_i_var148);
		_i_var182 = (_i_var169)+(_i_var168);
		_i_var183 = (_i_var170) * (_i_var23);
		_i_var184 = (_i_var171) * (_i_var23);
		_i_var185 = (_i_var173)+(_i_var172);
		_i_var186 = (_i_var174)+(_i_var158);
		_i_var187 = (_i_var175)+(_i_var160);
		_i_var188 = (_i_var176) * (_i_var14);
		_i_var189 = (_i_var177) * (_i_var23);
		_i_var190 = (_i_var178)+(_i_var164);
		_i_var191 = (_i_var179)+(_i_var166);
		_i_var192 = (_i_var155) * (_i_var40);
		_i_var193 = (_i_var105) * (_i_var148);
		_i_var194 = (_i_var181)+(_i_var180);
		_i_var195 = (_i_var182)+(_i_var168);
		_i_var196 = (_i_var183) * (_i_var18);
		_i_var197 = (_i_var184) * (_i_var10);
		_i_var198 = (_i_var185)+(_i_var172);
		_i_var199 = (_i_var47) * (_i_var183);
		_i_var200 = (_i_var46) * (_i_var176);
		_i_var201 = (_i_var43) * (_i_var176);
		_i_var202 = (_i_var44) * (_i_var183);
		_i_var203 = (_i_var186) * (_i_var23);
		_i_var204 = (_i_var187) * (_i_var23);
		_i_var205 = (_i_var188) * (_i_var23);
		_i_var206 = (_i_var184) * (_i_var13);
		_i_var207 = (_i_var183) * (_i_var17);
		_i_var208 = (_i_var183) * (_i_var13);
		_i_var209 = (_i_var189) * (_i_var14);
		_i_var210 = (_i_var190) * (_i_var23);
		_i_var211 = (_i_var191) * (_i_var23);
		_i_var212 = (_i_var176) * (_i_var13);
		_i_var213 = (_i_var184) * (_i_var14);
		_i_var214 = (_i_var193)+(_i_var192);
		_i_var215 = (_i_var194)+(_i_var180);
		_i_var216 = (_i_var195) * (_i_var23);
		_i_var217 = (_i_var176) * (_i_var15);
		_i_var218 = (_i_var184) * (_i_var16);
		_i_var219 = (_i_var176) * (_i_var16);
		_i_var220 = (_i_var197)+(_i_var196);
		_i_var221 = (_i_var198) * (_i_var23);
		_i_var222 = (_i_var45) * (_i_var184);
		_i_var223 = (_i_var200)+(_i_var199);
		_i_var224 = (_i_var42) * (_i_var184);
		_i_var225 = (_i_var202)+(_i_var201);
		_i_var226 = (_i_var203) * (_i_var14);
		_i_var227 = (_i_var204) * (_i_var17);
		_i_var228 = (_i_var204) * (_i_var13);
		_i_var229 = (_i_var206)+(_i_var205);
		_i_var230 = (_i_var208)+(_i_var207);
		_i_var231 = (_i_var209) * (_i_var23);
		_i_var232 = (_i_var210) * (_i_var13);
		_i_var233 = (_i_var211) * (_i_var17);
		_i_var234 = (_i_var211) * (_i_var13);
		_i_var235 = (_i_var213)+(_i_var212);
		_i_var236 = (_i_var189) * (_i_var13);
		_i_var237 = (_i_var210) * (_i_var14);
		_i_var238 = (_i_var214)+(_i_var192);
		_i_var239 = (_i_var215) * (_i_var23);
		_i_var240 = (_i_var216) * (_i_var10);
		_i_var241 = (_i_var218)+(_i_var217);
		_i_var242 = (_i_var196) * (_i_var23);
		_i_var243 = (_i_var220)+(_i_var219);
		_i_var244 = (_i_var221) * (_i_var10);
		_i_var245 = (_i_var223)+(_i_var222);
		_i_var246 = (_i_var210) * (_i_var10);
		_i_var247 = (_i_var225)+(_i_var224);
		_i_var248 = (_i_var226) * (_i_var23);
		_i_var249 = (_i_var221) * (_i_var13);
		_i_var250 = (_i_var228)+(_i_var227);
		_i_var251 = (_i_var229) * (_i_var5);
		_i_var252 = (_i_var230) * (_i_var12);
		_i_var253 = (_i_var232)+(_i_var231);
		_i_var254 = (_i_var234)+(_i_var233);
		_i_var255 = (_i_var29) * (_i_var229);
		_i_var256 = (_i_var37) * (_i_var230);
		_i_var257 = (_i_var235) * (_i_var11);
		_i_var258 = (_i_var230) * (_i_var0);
		_i_var259 = (_i_var237)+(_i_var236);
		_i_var260 = (_i_var238) * (_i_var23);
		_i_var261 = (_i_var239) * (_i_var18);
		_i_var262 = (_i_var241)+(_i_var240);
		_i_var263 = (_i_var243)+(_i_var242);
		_i_var264 = (_i_var204) * (_i_var18);
		_i_var265 = (_i_var245)+(_i_var244);
		_i_var266 = (_i_var57) * (_i_var176);
		_i_var267 = (_i_var46) * (_i_var184);
		_i_var268 = (_i_var211) * (_i_var18);
		_i_var269 = (_i_var247)+(_i_var246);
		_i_var270 = (_i_var51) * (_i_var176);
		_i_var271 = (_i_var43) * (_i_var184);
		_i_var272 = (_i_var249)+(_i_var248);
		_i_var273 = (_i_var250) * (_i_var12);
		_i_var274 = (_i_var235) * (_i_var12);
		_i_var275 = (_i_var203) * (_i_var13);
		_i_var276 = (_i_var221) * (_i_var14);
		_i_var277 = (_i_var251) * (_i_var23);
		_i_var278 = (_i_var252)+(_i_var251);
		_i_var279 = (_i_var253) * (_i_var5);
		_i_var280 = (_i_var254) * (_i_var12);
		_i_var281 = (_i_var256)+(_i_var255);
		_i_var282 = (_i_var253) * (_i_var4);
		_i_var283 = (_i_var253) * (_i_var0);
		_i_var284 = (_i_var258)+(_i_var257);
		_i_var285 = (_i_var259) * (_i_var11);
		_i_var286 = (_i_var254) * (_i_var0);
		_i_var287 = (_i_var229) * (_i_var4);
		_i_var288 = (_i_var229) * (_i_var0);
		_i_var289 = (_i_var260) * (_i_var16);
		_i_var290 = (_i_var262)+(_i_var261);
		_i_var291 = (_i_var216) * (_i_var16);
		_i_var292 = (_i_var263) * (_i_var23);
		_i_var293 = (_i_var199)+(_i_var264);
		_i_var294 = (_i_var203) * (_i_var16);
		_i_var295 = (_i_var265)+(_i_var264);
		_i_var296 = (_i_var221) * (_i_var16);
		_i_var297 = (_i_var267)+(_i_var266);
		_i_var298 = (_i_var202)+(_i_var268);
		_i_var299 = (_i_var189) * (_i_var16);
		_i_var300 = (_i_var269)+(_i_var268);
		_i_var301 = (_i_var210) * (_i_var16);
		_i_var302 = (_i_var271)+(_i_var270);
		_i_var303 = (_i_var272) * (_i_var5);
		_i_var304 = (_i_var274)+(_i_var273);
		_i_var305 = (_i_var276)+(_i_var275);
		_i_var306 = (_i_var278)+(_i_var277);
		_i_var307 = (_i_var255)+(_i_var279);
		_i_var308 = (_i_var281)+(_i_var280);
		_i_var309 = (_i_var283)+(_i_var282);
		_i_var310 = (_i_var284) * (offsets(4, 0));
		_i_var311 = (_i_var286)+(_i_var285);
		_i_var312 = (_i_var288)+(_i_var287);
		_i_var313 = (_i_var261) * (_i_var23);
		_i_var314 = (_i_var290)+(_i_var289);
		_i_var315 = (_i_var260) * (_i_var15);
		_i_var316 = (_i_var292)+(_i_var291);
		_i_var317 = (_i_var293) * (_i_var23);
		_i_var318 = (_i_var295)+(_i_var294);
		_i_var319 = (_i_var203) * (_i_var15);
		_i_var320 = (_i_var297)+(_i_var296);
		_i_var321 = (_i_var298) * (_i_var23);
		_i_var322 = (_i_var300)+(_i_var299);
		_i_var323 = (_i_var189) * (_i_var15);
		_i_var324 = (_i_var302)+(_i_var301);
		_i_var325 = (_i_var303) * (_i_var23);
		_i_var326 = (_i_var304)+(_i_var303);
		_i_var327 = (_i_var305) * (_i_var12);
		_i_var328 = (_i_var306) * (_i_var23);
		_i_var329 = (_i_var307) * (_i_var23);
		_i_var330 = (_i_var308)+(_i_var279);
		_i_var331 = (_i_var259) * (_i_var12);
		_i_var332 = (_i_var37) * (_i_var235);
		_i_var333 = (_i_var309) * (_i_var3);
		_i_var334 = (_i_var310) * (_i_var23);
		_i_var335 = (_i_var311) * (offsets(4, 0));
		_i_var336 = (_i_var312) * (_i_var3);
		_i_var337 = (_i_var314)+(_i_var313);
		_i_var338 = (_i_var316)+(_i_var315);
		_i_var339 = (_i_var318)+(_i_var317);
		_i_var340 = (_i_var320)+(_i_var319);
		_i_var341 = (_i_var322)+(_i_var321);
		_i_var342 = (_i_var324)+(_i_var323);
		_i_var343 = (_i_var326)+(_i_var325);
		_i_var344 = (_i_var328)+(_i_var327);
		_i_var345 = (_i_var330)+(_i_var329);
		_i_var346 = (_i_var332)+(_i_var331);
		_i_var347 = (_i_var334)+(_i_var333);
		_i_var348 = (_i_var336)+(_i_var335);
		_i_var349 = (_i_var337) * (_i_var56);
		_i_var350 = (_i_var338) * (_i_var13);
		_i_var351 = (_i_var339) * (_i_var56);
		_i_var352 = (_i_var340) * (_i_var13);
		_i_var353 = (_i_var341) * (_i_var56);
		_i_var354 = (_i_var342) * (_i_var13);
		_i_var355 = (_i_var343) * (_i_var24);
		_i_var356 = (_i_var344) * (_i_var0);
		_i_var357 = (_i_var345) * (_i_var24);
		_i_var358 = (_i_var346) * (_i_var0);
		_i_var359 = (_i_var347) * (_i_var6);
		_i_var360 = (_i_var348) * (_i_var30);
		_i_var361 = (_i_var350)+(_i_var349);
		_i_var362 = (_i_var352)+(_i_var351);
		_i_var363 = (_i_var354)+(_i_var353);
		_i_var364 = (_i_var356)+(_i_var355);
		_i_var365 = (_i_var358)+(_i_var357);
		_i_var366 = (_i_var360)+(_i_var359);
		hessian(0, 0) = _i_var361;
		hessian(1, 0) = _i_var362;
		hessian(2, 0) = _i_var363;
		hessian(0, 1) = _i_var362;
		hessian(1, 1) = _i_var364;
		hessian(2, 1) = _i_var365;
		hessian(0, 2) = _i_var363;
		hessian(1, 2) = _i_var365;
		hessian(2, 2) = _i_var366;
	}

}



namespace Codegen
{
	void compute3AxisKibouRightArmEnergy(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var12) * (_i_var19);
		_i_var24 = (_i_var21) * (_i_var20);
		_i_var25 = (_i_var19) * (_i_var13);
		_i_var26 = (_i_var12) * (_i_var22);
		_i_var27 = (_i_var14) * (_i_var13);
		_i_var28 = (_i_var23) * (_i_var20);
		_i_var29 = (_i_var11) * (_i_var13);
		_i_var30 = (_i_var25)+(_i_var24);
		_i_var31 = (_i_var27)+(_i_var26);
		_i_var32 = (_i_var29)+(_i_var28);
		_i_var33 = (_i_var30)+(offsets(1, 0));
		_i_var34 = (_i_var31)+(offsets(0, 0));
		_i_var35 = (_i_var32)+(offsets(2, 0));
		_i_var36 = (target(1, 0)) - (_i_var33);
		_i_var37 = (target(0, 0)) - (_i_var34);
		_i_var38 = (target(2, 0)) - (_i_var35);
		_i_var39 = (_i_var36) * (_i_var36);
		_i_var40 = (_i_var37) * (_i_var37);
		_i_var41 = (_i_var38) * (_i_var38);
		_i_var42 = (_i_var40)+(_i_var39);
		_i_var43 = (_i_var42)+(_i_var41);
		_i_var44 = std::sqrt(_i_var43);
		_i_var45 = 0.5;
		_i_var46 = (_i_var45) * (_i_var44);
		_i_var47 = (_i_var46) * (_i_var44);
		energy = _i_var47;
	}
	void compute3AxisKibouRightArmEnergyGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var12) * (_i_var19);
		_i_var24 = (_i_var21) * (_i_var20);
		_i_var25 = (_i_var19) * (_i_var13);
		_i_var26 = (_i_var12) * (_i_var22);
		_i_var27 = (_i_var14) * (_i_var13);
		_i_var28 = (_i_var23) * (_i_var20);
		_i_var29 = (_i_var11) * (_i_var13);
		_i_var30 = (_i_var25)+(_i_var24);
		_i_var31 = (_i_var27)+(_i_var26);
		_i_var32 = (_i_var29)+(_i_var28);
		_i_var33 = (_i_var30)+(offsets(1, 0));
		_i_var34 = (_i_var31)+(offsets(0, 0));
		_i_var35 = (_i_var32)+(offsets(2, 0));
		_i_var36 = (target(1, 0)) - (_i_var33);
		_i_var37 = (target(0, 0)) - (_i_var34);
		_i_var38 = (target(2, 0)) - (_i_var35);
		_i_var39 = (_i_var36) * (_i_var36);
		_i_var40 = (_i_var37) * (_i_var37);
		_i_var41 = (_i_var38) * (_i_var38);
		_i_var42 = (_i_var40)+(_i_var39);
		_i_var43 = (_i_var42)+(_i_var41);
		_i_var44 = std::sqrt(_i_var43);
		_i_var45 = 2;
		_i_var46 = 0.5;
		_i_var47 = (_i_var45) * (_i_var44);
		_i_var48 = (_i_var44) * (_i_var46);
		_i_var49 = (_i_var46) * (_i_var44);
		_i_var50 = (_i_var1) / (_i_var47);
		_i_var51 = (_i_var49)+(_i_var48);
		_i_var52 = (_i_var51) * (_i_var50);
		_i_var53 = (_i_var52) * (_i_var36);
		_i_var54 = (_i_var52) * (_i_var37);
		_i_var55 = (_i_var45) * (_i_var53);
		_i_var56 = (_i_var45) * (_i_var54);
		_i_var57 = (_i_var52) * (_i_var38);
		_i_var58 = (_i_var55) * (_i_var12);
		_i_var59 = (_i_var56) * (_i_var12);
		_i_var60 = (_i_var45) * (_i_var57);
		_i_var61 = (_i_var58) * (_i_var20);
		_i_var62 = (_i_var59) * (_i_var12);
		_i_var63 = (_i_var60) * (_i_var12);
		_i_var64 = (_i_var61) * (_i_var12);
		_i_var65 = (_i_var62) * (_i_var17);
		_i_var66 = (_i_var63) * (_i_var20);
		_i_var67 = (_i_var64) * (_i_var12);
		_i_var68 = (_i_var63) * (_i_var13);
		_i_var69 = (_i_var65) * (_i_var12);
		_i_var70 = (_i_var59) * (_i_var13);
		_i_var71 = (_i_var66) * (_i_var12);
		_i_var72 = (_i_var58) * (_i_var13);
		_i_var73 = (_i_var59) * (_i_var14);
		_i_var74 = (_i_var63) * (_i_var11);
		_i_var75 = (_i_var68)+(_i_var67);
		_i_var76 = (_i_var70)+(_i_var69);
		_i_var77 = (_i_var72)+(_i_var71);
		_i_var78 = (_i_var62) * (_i_var18);
		_i_var79 = (_i_var58) * (_i_var19);
		_i_var80 = (_i_var74)+(_i_var73);
		_i_var81 = (_i_var75) * (_i_var6);
		_i_var82 = (_i_var76) * (_i_var10);
		_i_var83 = (_i_var75) * (_i_var5);
		_i_var84 = (_i_var75) * (_i_var0);
		_i_var85 = (_i_var76) * (_i_var0);
		_i_var86 = (_i_var77) * (_i_var15);
		_i_var87 = (_i_var78) * (_i_var12);
		_i_var88 = (_i_var80)+(_i_var79);
		_i_var89 = (_i_var58) * (_i_var21);
		_i_var90 = (_i_var63) * (_i_var23);
		_i_var91 = (_i_var81) * (_i_var12);
		_i_var92 = (_i_var82)+(_i_var81);
		_i_var93 = (_i_var84)+(_i_var83);
		_i_var94 = (_i_var86)+(_i_var85);
		_i_var95 = -(_i_var20);
		_i_var96 = (_i_var88)+(_i_var87);
		_i_var97 = (_i_var90)+(_i_var89);
		_i_var98 = -(_i_var15);
		_i_var99 = (_i_var92)+(_i_var91);
		_i_var100 = (_i_var77) * (_i_var10);
		_i_var101 = (_i_var93) * (offsets(4, 0));
		_i_var102 = -(_i_var2);
		_i_var103 = (_i_var94) * (_i_var4);
		_i_var104 = (_i_var96) * (_i_var95);
		_i_var105 = (_i_var97) * (_i_var13);
		_i_var106 = (_i_var99) * (_i_var98);
		_i_var107 = (_i_var100) * (_i_var0);
		_i_var108 = (_i_var101) * (_i_var3);
		_i_var109 = (_i_var103) * (_i_var102);
		_i_var110 = (_i_var105)+(_i_var104);
		_i_var111 = (_i_var107)+(_i_var106);
		_i_var112 = (_i_var109)+(_i_var108);
		gradient(0, 0) = _i_var110;
		gradient(1, 0) = _i_var111;
		gradient(2, 0) = _i_var112;
	}
	void compute3AxisKibouRightArmEnergyHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = -(_i_var15);
		_i_var24 = -(_i_var2);
		_i_var25 = (_i_var12) * (_i_var19);
		_i_var26 = (_i_var21) * (_i_var20);
		_i_var27 = (_i_var19) * (_i_var13);
		_i_var28 = (_i_var12) * (_i_var22);
		_i_var29 = (_i_var14) * (_i_var13);
		_i_var30 = (_i_var23) * (_i_var12);
		_i_var31 = (_i_var3) * (offsets(4, 0));
		_i_var32 = (_i_var24) * (_i_var4);
		_i_var33 = (_i_var25) * (_i_var20);
		_i_var34 = (_i_var11) * (_i_var13);
		_i_var35 = (_i_var27)+(_i_var26);
		_i_var36 = (_i_var29)+(_i_var28);
		_i_var37 = (_i_var30)-(_i_var15);
		_i_var38 = (_i_var23) * (_i_var10);
		_i_var39 = (_i_var31) * (_i_var5);
		_i_var40 = (_i_var31) * (_i_var0);
		_i_var41 = (_i_var32) * (_i_var0);
		_i_var42 = (_i_var34)+(_i_var33);
		_i_var43 = (_i_var35)+(offsets(1, 0));
		_i_var44 = (_i_var36)+(offsets(0, 0));
		_i_var45 = (_i_var37) * (_i_var6);
		_i_var46 = (_i_var38) * (_i_var12);
		_i_var47 = (_i_var0) * (_i_var10);
		_i_var48 = (_i_var40)+(_i_var39);
		_i_var49 = (_i_var41) * (_i_var12);
		_i_var50 = (_i_var32) * (_i_var15);
		_i_var51 = (_i_var42)+(offsets(2, 0));
		_i_var52 = (target(1, 0)) - (_i_var43);
		_i_var53 = (target(0, 0)) - (_i_var44);
		_i_var54 = (_i_var45) * (_i_var12);
		_i_var55 = (_i_var46) * (_i_var17);
		_i_var56 = (_i_var47) * (_i_var12);
		_i_var57 = (_i_var48) * (_i_var12);
		_i_var58 = (_i_var49) * (_i_var17);
		_i_var59 = (_i_var50) * (_i_var12);
		_i_var60 = (target(2, 0)) - (_i_var51);
		_i_var61 = (_i_var52) * (_i_var52);
		_i_var62 = (_i_var53) * (_i_var53);
		_i_var63 = (_i_var54) * (_i_var12);
		_i_var64 = (_i_var55) * (_i_var12);
		_i_var65 = (_i_var38) * (_i_var13);
		_i_var66 = (_i_var45) * (_i_var13);
		_i_var67 = (_i_var56) * (_i_var20);
		_i_var68 = (_i_var57) * (_i_var12);
		_i_var69 = (_i_var58) * (_i_var12);
		_i_var70 = (_i_var41) * (_i_var13);
		_i_var71 = (_i_var59) * (_i_var20);
		_i_var72 = (_i_var48) * (_i_var13);
		_i_var73 = -(_i_var20);
		_i_var74 = (_i_var60) * (_i_var60);
		_i_var75 = (_i_var62)+(_i_var61);
		_i_var76 = (_i_var63) * (_i_var20);
		_i_var77 = (_i_var47) * (_i_var13);
		_i_var78 = (_i_var65)+(_i_var64);
		_i_var79 = (_i_var67)+(_i_var66);
		_i_var80 = (_i_var68) * (_i_var20);
		_i_var81 = (_i_var50) * (_i_var13);
		_i_var82 = (_i_var70)+(_i_var69);
		_i_var83 = (_i_var72)+(_i_var71);
		_i_var84 = (_i_var73) * (_i_var12);
		_i_var85 = (_i_var75)+(_i_var74);
		_i_var86 = (_i_var77)+(_i_var76);
		_i_var87 = 2;
		_i_var88 = (_i_var78) * (_i_var12);
		_i_var89 = (_i_var79) * (_i_var12);
		_i_var90 = (_i_var81)+(_i_var80);
		_i_var91 = (_i_var82) * (_i_var12);
		_i_var92 = (_i_var83) * (_i_var12);
		_i_var93 = (_i_var84) * (_i_var18);
		_i_var94 = (_i_var73) * (_i_var11);
		_i_var95 = (_i_var13) * (_i_var25);
		_i_var96 = (_i_var73) * (_i_var19);
		_i_var97 = (_i_var13) * (_i_var21);
		_i_var98 = std::sqrt(_i_var85);
		_i_var99 = (_i_var86) * (_i_var12);
		_i_var100 = (_i_var88) * (_i_var87);
		_i_var101 = (_i_var89) * (_i_var87);
		_i_var102 = (_i_var90) * (_i_var12);
		_i_var103 = (_i_var91) * (_i_var87);
		_i_var104 = (_i_var92) * (_i_var87);
		_i_var105 = (_i_var93) * (_i_var12);
		_i_var106 = (_i_var73) * (_i_var14);
		_i_var107 = (_i_var95)+(_i_var94);
		_i_var108 = (_i_var97)+(_i_var96);
		_i_var109 = (_i_var87) * (_i_var98);
		_i_var110 = 0.5;
		_i_var111 = (_i_var99) * (_i_var87);
		_i_var112 = (_i_var100) * (_i_var53);
		_i_var113 = (_i_var101) * (_i_var60);
		_i_var114 = (_i_var102) * (_i_var87);
		_i_var115 = (_i_var103) * (_i_var53);
		_i_var116 = (_i_var104) * (_i_var60);
		_i_var117 = (_i_var106)+(_i_var105);
		_i_var118 = (_i_var107) * (_i_var12);
		_i_var119 = (_i_var108) * (_i_var12);
		_i_var120 = (_i_var109) * (_i_var109);
		_i_var121 = (_i_var98) * (_i_var110);
		_i_var122 = (_i_var110) * (_i_var98);
		_i_var123 = (_i_var111) * (_i_var52);
		_i_var124 = (_i_var113)+(_i_var112);
		_i_var125 = (_i_var114) * (_i_var52);
		_i_var126 = (_i_var116)+(_i_var115);
		_i_var127 = (_i_var117) * (_i_var12);
		_i_var128 = (_i_var118) * (_i_var87);
		_i_var129 = (_i_var119) * (_i_var87);
		_i_var130 = (_i_var1) / (_i_var120);
		_i_var131 = (_i_var122)+(_i_var121);
		_i_var132 = (_i_var124)+(_i_var123);
		_i_var133 = (_i_var1) / (_i_var109);
		_i_var134 = (_i_var126)+(_i_var125);
		_i_var135 = (_i_var127) * (_i_var87);
		_i_var136 = (_i_var128) * (_i_var60);
		_i_var137 = (_i_var129) * (_i_var52);
		_i_var138 = -(_i_var130);
		_i_var139 = (_i_var132) * (_i_var131);
		_i_var140 = (_i_var132) * (_i_var133);
		_i_var141 = (_i_var134) * (_i_var131);
		_i_var142 = (_i_var134) * (_i_var133);
		_i_var143 = (_i_var135) * (_i_var53);
		_i_var144 = (_i_var137)+(_i_var136);
		_i_var145 = (_i_var139) * (_i_var138);
		_i_var146 = (_i_var140) * (_i_var110);
		_i_var147 = (_i_var141) * (_i_var138);
		_i_var148 = (_i_var142) * (_i_var110);
		_i_var149 = (_i_var144)+(_i_var143);
		_i_var150 = (_i_var145) * (_i_var87);
		_i_var151 = (_i_var87) * (_i_var146);
		_i_var152 = (_i_var147) * (_i_var87);
		_i_var153 = (_i_var87) * (_i_var148);
		_i_var154 = (_i_var149) * (_i_var131);
		_i_var155 = (_i_var149) * (_i_var133);
		_i_var156 = (_i_var151)+(_i_var150);
		_i_var157 = (_i_var153)+(_i_var152);
		_i_var158 = (_i_var154) * (_i_var138);
		_i_var159 = (_i_var155) * (_i_var110);
		_i_var160 = (_i_var156) * (_i_var133);
		_i_var161 = (_i_var131) * (_i_var133);
		_i_var162 = (_i_var157) * (_i_var133);
		_i_var163 = (_i_var158) * (_i_var87);
		_i_var164 = (_i_var87) * (_i_var159);
		_i_var165 = (_i_var160) * (_i_var53);
		_i_var166 = (_i_var100) * (_i_var161);
		_i_var167 = (_i_var161) * (_i_var52);
		_i_var168 = (_i_var161) * (_i_var53);
		_i_var169 = (_i_var162) * (_i_var52);
		_i_var170 = (_i_var114) * (_i_var161);
		_i_var171 = (_i_var162) * (_i_var53);
		_i_var172 = (_i_var103) * (_i_var161);
		_i_var173 = (_i_var164)+(_i_var163);
		_i_var174 = (_i_var160) * (_i_var52);
		_i_var175 = (_i_var111) * (_i_var161);
		_i_var176 = (_i_var166)+(_i_var165);
		_i_var177 = (_i_var87) * (_i_var167);
		_i_var178 = (_i_var87) * (_i_var168);
		_i_var179 = (_i_var170)+(_i_var169);
		_i_var180 = (_i_var172)+(_i_var171);
		_i_var181 = (_i_var161) * (_i_var60);
		_i_var182 = (_i_var162) * (_i_var60);
		_i_var183 = (_i_var104) * (_i_var161);
		_i_var184 = (_i_var173) * (_i_var133);
		_i_var185 = (_i_var175)+(_i_var174);
		_i_var186 = (_i_var176)+(_i_var165);
		_i_var187 = (_i_var177) * (_i_var12);
		_i_var188 = (_i_var178) * (_i_var12);
		_i_var189 = (_i_var179)+(_i_var169);
		_i_var190 = (_i_var180)+(_i_var171);
		_i_var191 = (_i_var87) * (_i_var181);
		_i_var192 = (_i_var183)+(_i_var182);
		_i_var193 = (_i_var184) * (_i_var60);
		_i_var194 = (_i_var128) * (_i_var161);
		_i_var195 = (_i_var160) * (_i_var60);
		_i_var196 = (_i_var101) * (_i_var161);
		_i_var197 = (_i_var185)+(_i_var174);
		_i_var198 = (_i_var186) * (_i_var12);
		_i_var199 = (_i_var187) * (_i_var20);
		_i_var200 = (_i_var188) * (_i_var12);
		_i_var201 = (_i_var189) * (_i_var12);
		_i_var202 = (_i_var190) * (_i_var12);
		_i_var203 = (_i_var191) * (_i_var12);
		_i_var204 = (_i_var192)+(_i_var182);
		_i_var205 = (_i_var184) * (_i_var53);
		_i_var206 = (_i_var135) * (_i_var161);
		_i_var207 = (_i_var194)+(_i_var193);
		_i_var208 = (_i_var196)+(_i_var195);
		_i_var209 = (_i_var197) * (_i_var12);
		_i_var210 = (_i_var198) * (_i_var12);
		_i_var211 = (_i_var199) * (_i_var12);
		_i_var212 = (_i_var200) * (_i_var17);
		_i_var213 = (_i_var201) * (_i_var20);
		_i_var214 = (_i_var202) * (_i_var12);
		_i_var215 = (_i_var203) * (_i_var20);
		_i_var216 = (_i_var204) * (_i_var12);
		_i_var217 = (_i_var206)+(_i_var205);
		_i_var218 = (_i_var184) * (_i_var52);
		_i_var219 = (_i_var129) * (_i_var161);
		_i_var220 = (_i_var207)+(_i_var193);
		_i_var221 = (_i_var188) * (_i_var14);
		_i_var222 = (_i_var203) * (_i_var11);
		_i_var223 = (_i_var208)+(_i_var195);
		_i_var224 = (_i_var38) * (_i_var188);
		_i_var225 = (_i_var47) * (_i_var187);
		_i_var226 = (_i_var41) * (_i_var188);
		_i_var227 = (_i_var50) * (_i_var187);
		_i_var228 = (_i_var209) * (_i_var20);
		_i_var229 = (_i_var210) * (_i_var17);
		_i_var230 = (_i_var211) * (_i_var12);
		_i_var231 = (_i_var203) * (_i_var13);
		_i_var232 = (_i_var212) * (_i_var12);
		_i_var233 = (_i_var188) * (_i_var13);
		_i_var234 = (_i_var213) * (_i_var12);
		_i_var235 = (_i_var214) * (_i_var17);
		_i_var236 = (_i_var215) * (_i_var12);
		_i_var237 = (_i_var187) * (_i_var13);
		_i_var238 = (_i_var216) * (_i_var20);
		_i_var239 = (_i_var217)+(_i_var205);
		_i_var240 = (_i_var219)+(_i_var218);
		_i_var241 = (_i_var220) * (_i_var12);
		_i_var242 = (_i_var187) * (_i_var21);
		_i_var243 = (_i_var203) * (_i_var25);
		_i_var244 = (_i_var200) * (_i_var18);
		_i_var245 = (_i_var187) * (_i_var19);
		_i_var246 = (_i_var222)+(_i_var221);
		_i_var247 = (_i_var223) * (_i_var12);
		_i_var248 = (_i_var45) * (_i_var203);
		_i_var249 = (_i_var225)+(_i_var224);
		_i_var250 = (_i_var48) * (_i_var203);
		_i_var251 = (_i_var227)+(_i_var226);
		_i_var252 = (_i_var228) * (_i_var12);
		_i_var253 = (_i_var229) * (_i_var12);
		_i_var254 = (_i_var198) * (_i_var13);
		_i_var255 = (_i_var231)+(_i_var230);
		_i_var256 = (_i_var233)+(_i_var232);
		_i_var257 = (_i_var234) * (_i_var12);
		_i_var258 = (_i_var216) * (_i_var13);
		_i_var259 = (_i_var235) * (_i_var12);
		_i_var260 = (_i_var202) * (_i_var13);
		_i_var261 = (_i_var237)+(_i_var236);
		_i_var262 = (_i_var238) * (_i_var12);
		_i_var263 = (_i_var201) * (_i_var13);
		_i_var264 = (_i_var239) * (_i_var12);
		_i_var265 = (_i_var240)+(_i_var218);
		_i_var266 = (_i_var241) * (_i_var11);
		_i_var267 = (_i_var243)+(_i_var242);
		_i_var268 = (_i_var244) * (_i_var12);
		_i_var269 = (_i_var246)+(_i_var245);
		_i_var270 = (_i_var247) * (_i_var11);
		_i_var271 = (_i_var249)+(_i_var248);
		_i_var272 = (_i_var216) * (_i_var11);
		_i_var273 = (_i_var251)+(_i_var250);
		_i_var274 = (_i_var252) * (_i_var12);
		_i_var275 = (_i_var247) * (_i_var13);
		_i_var276 = (_i_var254)+(_i_var253);
		_i_var277 = (_i_var247) * (_i_var20);
		_i_var278 = (_i_var255) * (_i_var6);
		_i_var279 = (_i_var256) * (_i_var10);
		_i_var280 = (_i_var258)+(_i_var257);
		_i_var281 = (_i_var260)+(_i_var259);
		_i_var282 = (_i_var31) * (_i_var255);
		_i_var283 = (_i_var32) * (_i_var256);
		_i_var284 = (_i_var256) * (_i_var0);
		_i_var285 = (_i_var261) * (_i_var15);
		_i_var286 = (_i_var263)+(_i_var262);
		_i_var287 = (_i_var264) * (_i_var12);
		_i_var288 = (_i_var265) * (_i_var12);
		_i_var289 = (_i_var264) * (_i_var14);
		_i_var290 = (_i_var267)+(_i_var266);
		_i_var291 = (_i_var269)+(_i_var268);
		_i_var292 = (_i_var210) * (_i_var18);
		_i_var293 = (_i_var46) * (_i_var200);
		_i_var294 = (_i_var198) * (_i_var14);
		_i_var295 = (_i_var271)+(_i_var270);
		_i_var296 = (_i_var63) * (_i_var187);
		_i_var297 = (_i_var56) * (_i_var203);
		_i_var298 = (_i_var214) * (_i_var18);
		_i_var299 = (_i_var49) * (_i_var200);
		_i_var300 = (_i_var202) * (_i_var14);
		_i_var301 = (_i_var273)+(_i_var272);
		_i_var302 = (_i_var68) * (_i_var187);
		_i_var303 = (_i_var59) * (_i_var203);
		_i_var304 = (_i_var275)+(_i_var274);
		_i_var305 = (_i_var276) * (_i_var10);
		_i_var306 = (_i_var261) * (_i_var10);
		_i_var307 = (_i_var277) * (_i_var12);
		_i_var308 = (_i_var209) * (_i_var13);
		_i_var309 = (_i_var278) * (_i_var12);
		_i_var310 = (_i_var279)+(_i_var278);
		_i_var311 = (_i_var280) * (_i_var6);
		_i_var312 = (_i_var281) * (_i_var10);
		_i_var313 = (_i_var283)+(_i_var282);
		_i_var314 = (_i_var280) * (_i_var5);
		_i_var315 = (_i_var280) * (_i_var0);
		_i_var316 = (_i_var285)+(_i_var284);
		_i_var317 = (_i_var281) * (_i_var0);
		_i_var318 = (_i_var286) * (_i_var15);
		_i_var319 = (_i_var255) * (_i_var5);
		_i_var320 = (_i_var255) * (_i_var0);
		_i_var321 = (_i_var287) * (_i_var18);
		_i_var322 = (_i_var288) * (_i_var19);
		_i_var323 = (_i_var290)+(_i_var289);
		_i_var324 = (_i_var241) * (_i_var25);
		_i_var325 = (_i_var291) * (_i_var12);
		_i_var326 = (_i_var293)+(_i_var292);
		_i_var327 = (_i_var209) * (_i_var19);
		_i_var328 = (_i_var295)+(_i_var294);
		_i_var329 = (_i_var247) * (_i_var25);
		_i_var330 = (_i_var297)+(_i_var296);
		_i_var331 = (_i_var299)+(_i_var298);
		_i_var332 = (_i_var201) * (_i_var19);
		_i_var333 = (_i_var301)+(_i_var300);
		_i_var334 = (_i_var216) * (_i_var25);
		_i_var335 = (_i_var303)+(_i_var302);
		_i_var336 = (_i_var304) * (_i_var6);
		_i_var337 = (_i_var306)+(_i_var305);
		_i_var338 = (_i_var308)+(_i_var307);
		_i_var339 = (_i_var310)+(_i_var309);
		_i_var340 = (_i_var282)+(_i_var311);
		_i_var341 = (_i_var313)+(_i_var312);
		_i_var342 = (_i_var315)+(_i_var314);
		_i_var343 = (_i_var316) * (_i_var4);
		_i_var344 = (_i_var318)+(_i_var317);
		_i_var345 = (_i_var320)+(_i_var319);
		_i_var346 = (_i_var321) * (_i_var12);
		_i_var347 = (_i_var323)+(_i_var322);
		_i_var348 = (_i_var288) * (_i_var21);
		_i_var349 = (_i_var325)+(_i_var324);
		_i_var350 = (_i_var326) * (_i_var12);
		_i_var351 = (_i_var328)+(_i_var327);
		_i_var352 = (_i_var209) * (_i_var21);
		_i_var353 = (_i_var330)+(_i_var329);
		_i_var354 = (_i_var331) * (_i_var12);
		_i_var355 = (_i_var333)+(_i_var332);
		_i_var356 = (_i_var201) * (_i_var21);
		_i_var357 = (_i_var335)+(_i_var334);
		_i_var358 = (_i_var336) * (_i_var12);
		_i_var359 = (_i_var337)+(_i_var336);
		_i_var360 = (_i_var338) * (_i_var10);
		_i_var361 = (_i_var339) * (_i_var12);
		_i_var362 = (_i_var340) * (_i_var12);
		_i_var363 = (_i_var341)+(_i_var311);
		_i_var364 = (_i_var286) * (_i_var10);
		_i_var365 = (_i_var32) * (_i_var261);
		_i_var366 = (_i_var342) * (offsets(4, 0));
		_i_var367 = (_i_var343) * (_i_var12);
		_i_var368 = (_i_var344) * (_i_var4);
		_i_var369 = (_i_var345) * (offsets(4, 0));
		_i_var370 = (_i_var347)+(_i_var346);
		_i_var371 = (_i_var349)+(_i_var348);
		_i_var372 = (_i_var351)+(_i_var350);
		_i_var373 = (_i_var353)+(_i_var352);
		_i_var374 = (_i_var355)+(_i_var354);
		_i_var375 = (_i_var357)+(_i_var356);
		_i_var376 = (_i_var359)+(_i_var358);
		_i_var377 = (_i_var361)+(_i_var360);
		_i_var378 = (_i_var363)+(_i_var362);
		_i_var379 = (_i_var365)+(_i_var364);
		_i_var380 = (_i_var367)+(_i_var366);
		_i_var381 = (_i_var369)+(_i_var368);
		_i_var382 = (_i_var370) * (_i_var73);
		_i_var383 = (_i_var371) * (_i_var13);
		_i_var384 = (_i_var372) * (_i_var73);
		_i_var385 = (_i_var373) * (_i_var13);
		_i_var386 = (_i_var374) * (_i_var73);
		_i_var387 = (_i_var375) * (_i_var13);
		_i_var388 = (_i_var376) * (_i_var23);
		_i_var389 = (_i_var377) * (_i_var0);
		_i_var390 = (_i_var378) * (_i_var23);
		_i_var391 = (_i_var379) * (_i_var0);
		_i_var392 = (_i_var380) * (_i_var3);
		_i_var393 = (_i_var381) * (_i_var24);
		_i_var394 = (_i_var383)+(_i_var382);
		_i_var395 = (_i_var385)+(_i_var384);
		_i_var396 = (_i_var387)+(_i_var386);
		_i_var397 = (_i_var389)+(_i_var388);
		_i_var398 = (_i_var391)+(_i_var390);
		_i_var399 = (_i_var393)+(_i_var392);
		hessian(0, 0) = _i_var394;
		hessian(1, 0) = _i_var395;
		hessian(2, 0) = _i_var396;
		hessian(0, 1) = _i_var395;
		hessian(1, 1) = _i_var397;
		hessian(2, 1) = _i_var398;
		hessian(0, 2) = _i_var396;
		hessian(1, 2) = _i_var398;
		hessian(2, 2) = _i_var399;
	}

}

namespace Codegen
{
	void compute3AxisKibouRightArmEnergyFromTwoTarget(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = -(offsets(3, 0));
		_i_var9 = (_i_var6) * (_i_var5);
		_i_var10 = (_i_var6) * (_i_var0);
		_i_var11 = (_i_var7)-(offsets(3, 0));
		_i_var12 = std::cos(angles(0, 0));
		_i_var13 = (_i_var8) * (_i_var0);
		_i_var14 = -1;
		_i_var15 = (_i_var10)+(_i_var9);
		_i_var16 = (_i_var11) * (_i_var0);
		_i_var17 = (_i_var1)-(_i_var12);
		_i_var18 = (_i_var14) * (_i_var13);
		_i_var19 = std::sin(angles(1, 0));
		_i_var20 = (_i_var14) * (_i_var15);
		_i_var21 = (_i_var14) * (_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var11) * (_i_var19);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var20);
		_i_var26 = (_i_var21) * (_i_var17);
		_i_var27 = (_i_var8) * (_i_var19);
		_i_var28 = (_i_var14) * (_i_var22);
		_i_var29 = (_i_var13) * (_i_var12);
		_i_var30 = (_i_var14) * (_i_var23);
		_i_var31 = (_i_var25) * (_i_var24);
		_i_var32 = (_i_var23) * (_i_var12);
		_i_var33 = (_i_var14) * (_i_var26);
		_i_var34 = (_i_var16) * (_i_var12);
		_i_var35 = (_i_var14) * (_i_var27);
		_i_var36 = (_i_var27) * (_i_var12);
		_i_var37 = (_i_var29)+(_i_var28);
		_i_var38 = (_i_var30) * (_i_var24);
		_i_var39 = (_i_var15) * (_i_var12);
		_i_var40 = (_i_var32)+(_i_var31);
		_i_var41 = (_i_var34)+(_i_var33);
		_i_var42 = (_i_var35) * (_i_var24);
		_i_var43 = (_i_var36)+(offsets(1, 0));
		_i_var44 = (_i_var37)+(offsets(0, 0));
		_i_var45 = (_i_var39)+(_i_var38);
		_i_var46 = (_i_var40)+(offsets(1, 0));
		_i_var47 = (_i_var41)+(offsets(0, 0));
		_i_var48 = (_i_var42)+(offsets(2, 0));
		_i_var49 = (targetElbow(1, 0)) - (_i_var43);
		_i_var50 = (targetElbow(0, 0)) - (_i_var44);
		_i_var51 = (_i_var45)+(offsets(2, 0));
		_i_var52 = (targetHand(1, 0)) - (_i_var46);
		_i_var53 = (targetHand(0, 0)) - (_i_var47);
		_i_var54 = (targetElbow(2, 0)) - (_i_var48);
		_i_var55 = (_i_var49) * (_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (targetHand(2, 0)) - (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var53) * (_i_var53);
		_i_var60 = (_i_var54) * (_i_var54);
		_i_var61 = (_i_var56)+(_i_var55);
		_i_var62 = (_i_var57) * (_i_var57);
		_i_var63 = (_i_var59)+(_i_var58);
		_i_var64 = (_i_var61)+(_i_var60);
		_i_var65 = (_i_var63)+(_i_var62);
		_i_var66 = std::sqrt(_i_var64);
		_i_var67 = std::sqrt(_i_var65);
		_i_var68 = (_i_var66) * (_i_var66);
		_i_var69 = (_i_var67) * (_i_var67);
		_i_var70 = (_i_var69)+(_i_var68);
		_i_var71 = 0.5;
		_i_var72 = (_i_var71) * (_i_var70);
		energy = _i_var72;
	}
	void compute3AxisKibouRightArmEnergyFromTwoTargetGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = -(offsets(3, 0));
		_i_var9 = (_i_var6) * (_i_var5);
		_i_var10 = (_i_var6) * (_i_var0);
		_i_var11 = (_i_var7)-(offsets(3, 0));
		_i_var12 = std::cos(angles(0, 0));
		_i_var13 = (_i_var8) * (_i_var0);
		_i_var14 = -1;
		_i_var15 = (_i_var10)+(_i_var9);
		_i_var16 = (_i_var11) * (_i_var0);
		_i_var17 = (_i_var1)-(_i_var12);
		_i_var18 = (_i_var14) * (_i_var13);
		_i_var19 = std::sin(angles(1, 0));
		_i_var20 = (_i_var14) * (_i_var15);
		_i_var21 = (_i_var14) * (_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var11) * (_i_var19);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var20);
		_i_var26 = (_i_var21) * (_i_var17);
		_i_var27 = (_i_var8) * (_i_var19);
		_i_var28 = (_i_var14) * (_i_var22);
		_i_var29 = (_i_var13) * (_i_var12);
		_i_var30 = (_i_var14) * (_i_var23);
		_i_var31 = (_i_var25) * (_i_var24);
		_i_var32 = (_i_var23) * (_i_var12);
		_i_var33 = (_i_var14) * (_i_var26);
		_i_var34 = (_i_var16) * (_i_var12);
		_i_var35 = (_i_var14) * (_i_var27);
		_i_var36 = (_i_var27) * (_i_var12);
		_i_var37 = (_i_var29)+(_i_var28);
		_i_var38 = (_i_var30) * (_i_var24);
		_i_var39 = (_i_var15) * (_i_var12);
		_i_var40 = (_i_var32)+(_i_var31);
		_i_var41 = (_i_var34)+(_i_var33);
		_i_var42 = (_i_var35) * (_i_var24);
		_i_var43 = (_i_var36)+(offsets(1, 0));
		_i_var44 = (_i_var37)+(offsets(0, 0));
		_i_var45 = (_i_var39)+(_i_var38);
		_i_var46 = (_i_var40)+(offsets(1, 0));
		_i_var47 = (_i_var41)+(offsets(0, 0));
		_i_var48 = (_i_var42)+(offsets(2, 0));
		_i_var49 = (targetElbow(1, 0)) - (_i_var43);
		_i_var50 = (targetElbow(0, 0)) - (_i_var44);
		_i_var51 = (_i_var45)+(offsets(2, 0));
		_i_var52 = (targetHand(1, 0)) - (_i_var46);
		_i_var53 = (targetHand(0, 0)) - (_i_var47);
		_i_var54 = (targetElbow(2, 0)) - (_i_var48);
		_i_var55 = (_i_var49) * (_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (targetHand(2, 0)) - (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var53) * (_i_var53);
		_i_var60 = (_i_var54) * (_i_var54);
		_i_var61 = (_i_var56)+(_i_var55);
		_i_var62 = (_i_var57) * (_i_var57);
		_i_var63 = (_i_var59)+(_i_var58);
		_i_var64 = (_i_var61)+(_i_var60);
		_i_var65 = (_i_var63)+(_i_var62);
		_i_var66 = std::sqrt(_i_var64);
		_i_var67 = 2;
		_i_var68 = 0.5;
		_i_var69 = std::sqrt(_i_var65);
		_i_var70 = (_i_var67) * (_i_var66);
		_i_var71 = (_i_var68) * (_i_var66);
		_i_var72 = (_i_var67) * (_i_var69);
		_i_var73 = (_i_var68) * (_i_var69);
		_i_var74 = (_i_var1) / (_i_var70);
		_i_var75 = (_i_var67) * (_i_var71);
		_i_var76 = (_i_var1) / (_i_var72);
		_i_var77 = (_i_var67) * (_i_var73);
		_i_var78 = (_i_var75) * (_i_var74);
		_i_var79 = (_i_var77) * (_i_var76);
		_i_var80 = (_i_var78) * (_i_var50);
		_i_var81 = (_i_var79) * (_i_var53);
		_i_var82 = (_i_var79) * (_i_var52);
		_i_var83 = (_i_var67) * (_i_var80);
		_i_var84 = (_i_var67) * (_i_var81);
		_i_var85 = (_i_var78) * (_i_var49);
		_i_var86 = (_i_var79) * (_i_var57);
		_i_var87 = (_i_var67) * (_i_var82);
		_i_var88 = (_i_var83) * (_i_var14);
		_i_var89 = (_i_var84) * (_i_var14);
		_i_var90 = (_i_var67) * (_i_var85);
		_i_var91 = (_i_var67) * (_i_var86);
		_i_var92 = (_i_var87) * (_i_var14);
		_i_var93 = (_i_var88) * (_i_var14);
		_i_var94 = (_i_var89) * (_i_var14);
		_i_var95 = (_i_var78) * (_i_var54);
		_i_var96 = (_i_var90) * (_i_var14);
		_i_var97 = (_i_var91) * (_i_var14);
		_i_var98 = (_i_var92) * (_i_var24);
		_i_var99 = (_i_var93) * (_i_var17);
		_i_var100 = (_i_var94) * (_i_var17);
		_i_var101 = (_i_var67) * (_i_var95);
		_i_var102 = (_i_var96) * (_i_var27);
		_i_var103 = (_i_var97) * (_i_var15);
		_i_var104 = (_i_var98) * (_i_var14);
		_i_var105 = (_i_var99) * (_i_var14);
		_i_var106 = (_i_var88) * (_i_var12);
		_i_var107 = (_i_var100) * (_i_var14);
		_i_var108 = (_i_var89) * (_i_var12);
		_i_var109 = (_i_var101) * (_i_var14);
		_i_var110 = (_i_var97) * (_i_var24);
		_i_var111 = (_i_var89) * (_i_var16);
		_i_var112 = (_i_var103)+(_i_var102);
		_i_var113 = (_i_var104) * (_i_var14);
		_i_var114 = (_i_var97) * (_i_var12);
		_i_var115 = (_i_var106)+(_i_var105);
		_i_var116 = (_i_var108)+(_i_var107);
		_i_var117 = (_i_var109) * (_i_var24);
		_i_var118 = (_i_var110) * (_i_var14);
		_i_var119 = (_i_var92) * (_i_var12);
		_i_var120 = (_i_var93) * (_i_var18);
		_i_var121 = (_i_var94) * (_i_var21);
		_i_var122 = (_i_var92) * (_i_var23);
		_i_var123 = (_i_var112)+(_i_var111);
		_i_var124 = (_i_var114)+(_i_var113);
		_i_var125 = (_i_var115) * (_i_var8);
		_i_var126 = (_i_var116) * (_i_var11);
		_i_var127 = (_i_var117) * (_i_var14);
		_i_var128 = (_i_var96) * (_i_var12);
		_i_var129 = (_i_var119)+(_i_var118);
		_i_var130 = (_i_var121)+(_i_var120);
		_i_var131 = (_i_var88) * (_i_var13);
		_i_var132 = (_i_var123)+(_i_var122);
		_i_var133 = (_i_var97) * (_i_var30);
		_i_var134 = (_i_var109) * (_i_var35);
		_i_var135 = (_i_var124) * (_i_var6);
		_i_var136 = (_i_var126)+(_i_var125);
		_i_var137 = (_i_var128)+(_i_var127);
		_i_var138 = (_i_var124) * (_i_var5);
		_i_var139 = (_i_var124) * (_i_var0);
		_i_var140 = (_i_var116) * (_i_var0);
		_i_var141 = (_i_var129) * (_i_var19);
		_i_var142 = (_i_var130) * (_i_var14);
		_i_var143 = (_i_var132)+(_i_var131);
		_i_var144 = (_i_var92) * (_i_var25);
		_i_var145 = (_i_var134)+(_i_var133);
		_i_var146 = (_i_var135) * (_i_var14);
		_i_var147 = (_i_var136)+(_i_var135);
		_i_var148 = (_i_var129) * (_i_var11);
		_i_var149 = (_i_var137) * (_i_var8);
		_i_var150 = (_i_var139)+(_i_var138);
		_i_var151 = (_i_var141)+(_i_var140);
		_i_var152 = -(_i_var24);
		_i_var153 = (_i_var143)+(_i_var142);
		_i_var154 = (_i_var145)+(_i_var144);
		_i_var155 = -(_i_var19);
		_i_var156 = (_i_var147)+(_i_var146);
		_i_var157 = (_i_var149)+(_i_var148);
		_i_var158 = (_i_var150) * (offsets(4, 0));
		_i_var159 = -(_i_var2);
		_i_var160 = (_i_var151) * (_i_var4);
		_i_var161 = (_i_var153) * (_i_var152);
		_i_var162 = (_i_var154) * (_i_var12);
		_i_var163 = (_i_var156) * (_i_var155);
		_i_var164 = (_i_var157) * (_i_var0);
		_i_var165 = (_i_var158) * (_i_var3);
		_i_var166 = (_i_var160) * (_i_var159);
		_i_var167 = (_i_var162)+(_i_var161);
		_i_var168 = (_i_var164)+(_i_var163);
		_i_var169 = (_i_var166)+(_i_var165);
		gradient(0, 0) = _i_var167;
		gradient(1, 0) = _i_var168;
		gradient(2, 0) = _i_var169;
	}
	void compute3AxisKibouRightArmEnergyFromTwoTargetHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399, _i_var400, _i_var401, _i_var402, _i_var403, _i_var404;
		double _i_var405, _i_var406, _i_var407, _i_var408, _i_var409, _i_var410, _i_var411, _i_var412, _i_var413, _i_var414, _i_var415, _i_var416, _i_var417, _i_var418, _i_var419;
		double _i_var420, _i_var421, _i_var422, _i_var423, _i_var424, _i_var425, _i_var426, _i_var427, _i_var428, _i_var429, _i_var430, _i_var431, _i_var432, _i_var433, _i_var434;
		double _i_var435, _i_var436, _i_var437, _i_var438, _i_var439, _i_var440, _i_var441, _i_var442, _i_var443, _i_var444, _i_var445, _i_var446, _i_var447, _i_var448, _i_var449;
		double _i_var450, _i_var451, _i_var452, _i_var453, _i_var454, _i_var455, _i_var456, _i_var457, _i_var458, _i_var459, _i_var460, _i_var461, _i_var462, _i_var463, _i_var464;
		double _i_var465, _i_var466, _i_var467, _i_var468, _i_var469, _i_var470, _i_var471, _i_var472, _i_var473, _i_var474, _i_var475, _i_var476, _i_var477, _i_var478, _i_var479;
		double _i_var480, _i_var481, _i_var482, _i_var483, _i_var484, _i_var485, _i_var486, _i_var487, _i_var488, _i_var489, _i_var490, _i_var491, _i_var492, _i_var493, _i_var494;
		double _i_var495, _i_var496, _i_var497, _i_var498, _i_var499, _i_var500, _i_var501, _i_var502, _i_var503, _i_var504, _i_var505, _i_var506, _i_var507, _i_var508, _i_var509;
		double _i_var510, _i_var511, _i_var512, _i_var513, _i_var514, _i_var515, _i_var516, _i_var517, _i_var518, _i_var519, _i_var520, _i_var521, _i_var522, _i_var523, _i_var524;
		double _i_var525, _i_var526, _i_var527, _i_var528, _i_var529, _i_var530, _i_var531, _i_var532, _i_var533, _i_var534, _i_var535, _i_var536, _i_var537, _i_var538, _i_var539;
		double _i_var540, _i_var541, _i_var542, _i_var543, _i_var544, _i_var545, _i_var546, _i_var547, _i_var548, _i_var549, _i_var550, _i_var551, _i_var552, _i_var553, _i_var554;
		double _i_var555, _i_var556, _i_var557, _i_var558, _i_var559, _i_var560, _i_var561, _i_var562, _i_var563, _i_var564, _i_var565, _i_var566, _i_var567, _i_var568, _i_var569;
		double _i_var570, _i_var571, _i_var572, _i_var573, _i_var574, _i_var575, _i_var576, _i_var577, _i_var578, _i_var579, _i_var580, _i_var581;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = -(offsets(3, 0));
		_i_var12 = (_i_var9)+(_i_var8);
		_i_var13 = -1;
		_i_var14 = std::cos(angles(0, 0));
		_i_var15 = (_i_var10) * (_i_var0);
		_i_var16 = std::sin(angles(1, 0));
		_i_var17 = (_i_var11) * (_i_var0);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var1)-(_i_var14);
		_i_var20 = (_i_var13) * (_i_var15);
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var13) * (_i_var17);
		_i_var23 = (_i_var10) * (_i_var16);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var18);
		_i_var26 = (_i_var20) * (_i_var19);
		_i_var27 = (_i_var21) * (_i_var13);
		_i_var28 = (_i_var22) * (_i_var19);
		_i_var29 = (_i_var13) * (_i_var23);
		_i_var30 = (_i_var25) * (_i_var24);
		_i_var31 = (_i_var23) * (_i_var14);
		_i_var32 = (_i_var13) * (_i_var26);
		_i_var33 = (_i_var15) * (_i_var14);
		_i_var34 = (_i_var27)-(_i_var16);
		_i_var35 = -(_i_var2);
		_i_var36 = (_i_var11) * (_i_var16);
		_i_var37 = (_i_var13) * (_i_var28);
		_i_var38 = (_i_var17) * (_i_var14);
		_i_var39 = (_i_var29) * (_i_var24);
		_i_var40 = (_i_var12) * (_i_var14);
		_i_var41 = (_i_var31)+(_i_var30);
		_i_var42 = (_i_var33)+(_i_var32);
		_i_var43 = (_i_var34) * (_i_var6);
		_i_var44 = (_i_var3) * (offsets(4, 0));
		_i_var45 = (_i_var35) * (_i_var4);
		_i_var46 = (_i_var13) * (_i_var36);
		_i_var47 = (_i_var36) * (_i_var14);
		_i_var48 = (_i_var38)+(_i_var37);
		_i_var49 = (_i_var40)+(_i_var39);
		_i_var50 = (_i_var41)+(offsets(1, 0));
		_i_var51 = (_i_var42)+(offsets(0, 0));
		_i_var52 = (_i_var21) * (_i_var10);
		_i_var53 = (_i_var43) * (_i_var13);
		_i_var54 = (_i_var0) * (_i_var10);
		_i_var55 = (_i_var44) * (_i_var5);
		_i_var56 = (_i_var44) * (_i_var0);
		_i_var57 = (_i_var45) * (_i_var0);
		_i_var58 = -(_i_var24);
		_i_var59 = (_i_var46) * (_i_var24);
		_i_var60 = (_i_var47)+(offsets(1, 0));
		_i_var61 = (_i_var48)+(offsets(0, 0));
		_i_var62 = (_i_var21) * (_i_var11);
		_i_var63 = (_i_var49)+(offsets(2, 0));
		_i_var64 = (targetHand(1, 0)) - (_i_var50);
		_i_var65 = (targetHand(0, 0)) - (_i_var51);
		_i_var66 = (_i_var52) * (_i_var13);
		_i_var67 = (_i_var53) * (_i_var13);
		_i_var68 = (_i_var54) * (_i_var13);
		_i_var69 = (_i_var56)+(_i_var55);
		_i_var70 = (_i_var57) * (_i_var13);
		_i_var71 = (_i_var45) * (_i_var16);
		_i_var72 = (_i_var58) * (_i_var13);
		_i_var73 = (_i_var59)+(offsets(2, 0));
		_i_var74 = (targetElbow(1, 0)) - (_i_var60);
		_i_var75 = (targetElbow(0, 0)) - (_i_var61);
		_i_var76 = (_i_var62) * (_i_var13);
		_i_var77 = (_i_var0) * (_i_var11);
		_i_var78 = (targetHand(2, 0)) - (_i_var63);
		_i_var79 = (_i_var64) * (_i_var64);
		_i_var80 = (_i_var65) * (_i_var65);
		_i_var81 = (_i_var66) * (_i_var19);
		_i_var82 = (_i_var67) * (_i_var24);
		_i_var83 = (_i_var54) * (_i_var14);
		_i_var84 = (_i_var43) * (_i_var14);
		_i_var85 = (_i_var68) * (_i_var24);
		_i_var86 = (_i_var69) * (_i_var13);
		_i_var87 = (_i_var70) * (_i_var19);
		_i_var88 = (_i_var71) * (_i_var13);
		_i_var89 = (_i_var72) * (_i_var20);
		_i_var90 = (targetElbow(2, 0)) - (_i_var73);
		_i_var91 = (_i_var74) * (_i_var74);
		_i_var92 = (_i_var75) * (_i_var75);
		_i_var93 = (_i_var76) * (_i_var19);
		_i_var94 = (_i_var77) * (_i_var13);
		_i_var95 = (_i_var78) * (_i_var78);
		_i_var96 = (_i_var80)+(_i_var79);
		_i_var97 = (_i_var81) * (_i_var13);
		_i_var98 = (_i_var52) * (_i_var14);
		_i_var99 = (_i_var83)+(_i_var82);
		_i_var100 = (_i_var85)+(_i_var84);
		_i_var101 = (_i_var86) * (_i_var13);
		_i_var102 = (_i_var87) * (_i_var13);
		_i_var103 = (_i_var57) * (_i_var14);
		_i_var104 = (_i_var88) * (_i_var24);
		_i_var105 = (_i_var69) * (_i_var14);
		_i_var106 = (_i_var72) * (_i_var22);
		_i_var107 = (_i_var89) * (_i_var13);
		_i_var108 = (_i_var58) * (_i_var15);
		_i_var109 = (_i_var58) * (_i_var23);
		_i_var110 = (_i_var14) * (_i_var25);
		_i_var111 = (_i_var90) * (_i_var90);
		_i_var112 = (_i_var92)+(_i_var91);
		_i_var113 = (_i_var93) * (_i_var13);
		_i_var114 = (_i_var62) * (_i_var14);
		_i_var115 = (_i_var94) * (_i_var24);
		_i_var116 = (_i_var77) * (_i_var14);
		_i_var117 = (_i_var96)+(_i_var95);
		_i_var118 = (_i_var98)+(_i_var97);
		_i_var119 = 2;
		_i_var120 = (_i_var99) * (_i_var13);
		_i_var121 = (_i_var100) * (_i_var13);
		_i_var122 = (_i_var101) * (_i_var24);
		_i_var123 = (_i_var71) * (_i_var14);
		_i_var124 = (_i_var103)+(_i_var102);
		_i_var125 = (_i_var105)+(_i_var104);
		_i_var126 = (_i_var106) * (_i_var13);
		_i_var127 = (_i_var58) * (_i_var17);
		_i_var128 = (_i_var58) * (_i_var12);
		_i_var129 = (_i_var14) * (_i_var29);
		_i_var130 = (_i_var108)+(_i_var107);
		_i_var131 = (_i_var110)+(_i_var109);
		_i_var132 = (_i_var112)+(_i_var111);
		_i_var133 = (_i_var114)+(_i_var113);
		_i_var134 = (_i_var115) * (_i_var13);
		_i_var135 = (_i_var116) * (_i_var13);
		_i_var136 = std::sqrt(_i_var117);
		_i_var137 = (_i_var118) * (_i_var13);
		_i_var138 = (_i_var120) * (_i_var119);
		_i_var139 = (_i_var121) * (_i_var119);
		_i_var140 = (_i_var123)+(_i_var122);
		_i_var141 = (_i_var124) * (_i_var13);
		_i_var142 = (_i_var125) * (_i_var13);
		_i_var143 = (_i_var127)+(_i_var126);
		_i_var144 = (_i_var14) * (_i_var46);
		_i_var145 = (_i_var129)+(_i_var128);
		_i_var146 = (_i_var130) * (_i_var13);
		_i_var147 = (_i_var131) * (_i_var13);
		_i_var148 = std::sqrt(_i_var132);
		_i_var149 = (_i_var133) * (_i_var13);
		_i_var150 = (_i_var134) * (_i_var119);
		_i_var151 = (_i_var135) * (_i_var119);
		_i_var152 = (_i_var119) * (_i_var136);
		_i_var153 = 0.5;
		_i_var154 = (_i_var137) * (_i_var119);
		_i_var155 = (_i_var138) * (_i_var64);
		_i_var156 = (_i_var139) * (_i_var78);
		_i_var157 = (_i_var140) * (_i_var13);
		_i_var158 = (_i_var141) * (_i_var119);
		_i_var159 = (_i_var142) * (_i_var119);
		_i_var160 = (_i_var58) * (_i_var36);
		_i_var161 = (_i_var143) * (_i_var13);
		_i_var162 = (_i_var144) * (_i_var13);
		_i_var163 = (_i_var145) * (_i_var13);
		_i_var164 = (_i_var146) * (_i_var119);
		_i_var165 = (_i_var147) * (_i_var119);
		_i_var166 = (_i_var119) * (_i_var148);
		_i_var167 = (_i_var149) * (_i_var119);
		_i_var168 = (_i_var150) * (_i_var90);
		_i_var169 = (_i_var151) * (_i_var74);
		_i_var170 = (_i_var152) * (_i_var152);
		_i_var171 = (_i_var153) * (_i_var136);
		_i_var172 = (_i_var154) * (_i_var65);
		_i_var173 = (_i_var156)+(_i_var155);
		_i_var174 = (_i_var157) * (_i_var119);
		_i_var175 = (_i_var158) * (_i_var65);
		_i_var176 = (_i_var159) * (_i_var78);
		_i_var177 = (_i_var160) * (_i_var13);
		_i_var178 = (_i_var161) * (_i_var119);
		_i_var179 = (_i_var162) * (_i_var119);
		_i_var180 = (_i_var163) * (_i_var119);
		_i_var181 = (_i_var164) * (_i_var65);
		_i_var182 = (_i_var165) * (_i_var64);
		_i_var183 = (_i_var166) * (_i_var166);
		_i_var184 = (_i_var153) * (_i_var148);
		_i_var185 = (_i_var167) * (_i_var75);
		_i_var186 = (_i_var169)+(_i_var168);
		_i_var187 = (_i_var1) / (_i_var170);
		_i_var188 = (_i_var119) * (_i_var171);
		_i_var189 = (_i_var173)+(_i_var172);
		_i_var190 = (_i_var1) / (_i_var152);
		_i_var191 = (_i_var174) * (_i_var64);
		_i_var192 = (_i_var176)+(_i_var175);
		_i_var193 = (_i_var177) * (_i_var119);
		_i_var194 = (_i_var178) * (_i_var75);
		_i_var195 = (_i_var179) * (_i_var90);
		_i_var196 = (_i_var180) * (_i_var78);
		_i_var197 = (_i_var182)+(_i_var181);
		_i_var198 = (_i_var1) / (_i_var183);
		_i_var199 = (_i_var119) * (_i_var184);
		_i_var200 = (_i_var186)+(_i_var185);
		_i_var201 = (_i_var1) / (_i_var166);
		_i_var202 = -(_i_var187);
		_i_var203 = (_i_var189) * (_i_var188);
		_i_var204 = (_i_var189) * (_i_var190);
		_i_var205 = (_i_var192)+(_i_var191);
		_i_var206 = (_i_var193) * (_i_var74);
		_i_var207 = (_i_var195)+(_i_var194);
		_i_var208 = (_i_var197)+(_i_var196);
		_i_var209 = -(_i_var198);
		_i_var210 = (_i_var200) * (_i_var199);
		_i_var211 = (_i_var200) * (_i_var201);
		_i_var212 = (_i_var203) * (_i_var202);
		_i_var213 = (_i_var204) * (_i_var119);
		_i_var214 = (_i_var205) * (_i_var188);
		_i_var215 = (_i_var205) * (_i_var190);
		_i_var216 = (_i_var207)+(_i_var206);
		_i_var217 = (_i_var208) * (_i_var188);
		_i_var218 = (_i_var208) * (_i_var190);
		_i_var219 = (_i_var210) * (_i_var209);
		_i_var220 = (_i_var211) * (_i_var119);
		_i_var221 = (_i_var212) * (_i_var119);
		_i_var222 = (_i_var213) * (_i_var153);
		_i_var223 = (_i_var214) * (_i_var202);
		_i_var224 = (_i_var215) * (_i_var119);
		_i_var225 = (_i_var216) * (_i_var199);
		_i_var226 = (_i_var216) * (_i_var201);
		_i_var227 = (_i_var217) * (_i_var202);
		_i_var228 = (_i_var218) * (_i_var119);
		_i_var229 = (_i_var188) * (_i_var190);
		_i_var230 = (_i_var199) * (_i_var201);
		_i_var231 = (_i_var219) * (_i_var119);
		_i_var232 = (_i_var220) * (_i_var153);
		_i_var233 = (_i_var222)+(_i_var221);
		_i_var234 = (_i_var223) * (_i_var119);
		_i_var235 = (_i_var224) * (_i_var153);
		_i_var236 = (_i_var225) * (_i_var209);
		_i_var237 = (_i_var226) * (_i_var119);
		_i_var238 = (_i_var227) * (_i_var119);
		_i_var239 = (_i_var228) * (_i_var153);
		_i_var240 = (_i_var229) * (_i_var64);
		_i_var241 = (_i_var230) * (_i_var74);
		_i_var242 = (_i_var232)+(_i_var231);
		_i_var243 = (_i_var233) * (_i_var190);
		_i_var244 = (_i_var230) * (_i_var75);
		_i_var245 = (_i_var229) * (_i_var65);
		_i_var246 = (_i_var235)+(_i_var234);
		_i_var247 = (_i_var236) * (_i_var119);
		_i_var248 = (_i_var237) * (_i_var153);
		_i_var249 = (_i_var239)+(_i_var238);
		_i_var250 = (_i_var229) * (_i_var78);
		_i_var251 = (_i_var119) * (_i_var240);
		_i_var252 = (_i_var119) * (_i_var241);
		_i_var253 = (_i_var242) * (_i_var201);
		_i_var254 = (_i_var243) * (_i_var65);
		_i_var255 = (_i_var154) * (_i_var229);
		_i_var256 = (_i_var119) * (_i_var244);
		_i_var257 = (_i_var119) * (_i_var245);
		_i_var258 = (_i_var246) * (_i_var190);
		_i_var259 = (_i_var248)+(_i_var247);
		_i_var260 = (_i_var249) * (_i_var190);
		_i_var261 = (_i_var230) * (_i_var90);
		_i_var262 = (_i_var119) * (_i_var250);
		_i_var263 = (_i_var251) * (_i_var13);
		_i_var264 = (_i_var252) * (_i_var13);
		_i_var265 = (_i_var253) * (_i_var75);
		_i_var266 = (_i_var167) * (_i_var230);
		_i_var267 = (_i_var255)+(_i_var254);
		_i_var268 = (_i_var256) * (_i_var13);
		_i_var269 = (_i_var257) * (_i_var13);
		_i_var270 = (_i_var258) * (_i_var64);
		_i_var271 = (_i_var174) * (_i_var229);
		_i_var272 = (_i_var258) * (_i_var65);
		_i_var273 = (_i_var158) * (_i_var229);
		_i_var274 = (_i_var259) * (_i_var201);
		_i_var275 = (_i_var260) * (_i_var78);
		_i_var276 = (_i_var180) * (_i_var229);
		_i_var277 = (_i_var119) * (_i_var261);
		_i_var278 = (_i_var262) * (_i_var13);
		_i_var279 = (_i_var243) * (_i_var78);
		_i_var280 = (_i_var139) * (_i_var229);
		_i_var281 = (_i_var54) * (_i_var263);
		_i_var282 = (_i_var77) * (_i_var264);
		_i_var283 = (_i_var243) * (_i_var64);
		_i_var284 = (_i_var138) * (_i_var229);
		_i_var285 = (_i_var266)+(_i_var265);
		_i_var286 = (_i_var267)+(_i_var254);
		_i_var287 = (_i_var268) * (_i_var13);
		_i_var288 = (_i_var269) * (_i_var13);
		_i_var289 = (_i_var271)+(_i_var270);
		_i_var290 = (_i_var273)+(_i_var272);
		_i_var291 = (_i_var258) * (_i_var78);
		_i_var292 = (_i_var159) * (_i_var229);
		_i_var293 = (_i_var274) * (_i_var74);
		_i_var294 = (_i_var193) * (_i_var230);
		_i_var295 = (_i_var276)+(_i_var275);
		_i_var296 = (_i_var277) * (_i_var13);
		_i_var297 = (_i_var264) * (_i_var36);
		_i_var298 = (_i_var278) * (_i_var12);
		_i_var299 = (_i_var253) * (_i_var74);
		_i_var300 = (_i_var151) * (_i_var230);
		_i_var301 = (_i_var280)+(_i_var279);
		_i_var302 = (_i_var43) * (_i_var278);
		_i_var303 = (_i_var282)+(_i_var281);
		_i_var304 = (_i_var284)+(_i_var283);
		_i_var305 = (_i_var285)+(_i_var265);
		_i_var306 = (_i_var286) * (_i_var13);
		_i_var307 = (_i_var253) * (_i_var90);
		_i_var308 = (_i_var150) * (_i_var230);
		_i_var309 = (_i_var263) * (_i_var24);
		_i_var310 = (_i_var287) * (_i_var19);
		_i_var311 = (_i_var288) * (_i_var19);
		_i_var312 = (_i_var289)+(_i_var270);
		_i_var313 = (_i_var290)+(_i_var272);
		_i_var314 = (_i_var292)+(_i_var291);
		_i_var315 = (_i_var274) * (_i_var75);
		_i_var316 = (_i_var178) * (_i_var230);
		_i_var317 = (_i_var260) * (_i_var65);
		_i_var318 = (_i_var164) * (_i_var229);
		_i_var319 = (_i_var294)+(_i_var293);
		_i_var320 = (_i_var295)+(_i_var275);
		_i_var321 = (_i_var278) * (_i_var29);
		_i_var322 = (_i_var296) * (_i_var46);
		_i_var323 = (_i_var269) * (_i_var15);
		_i_var324 = (_i_var298)+(_i_var297);
		_i_var325 = (_i_var300)+(_i_var299);
		_i_var326 = (_i_var301)+(_i_var279);
		_i_var327 = (_i_var52) * (_i_var269);
		_i_var328 = (_i_var303)+(_i_var302);
		_i_var329 = (_i_var304)+(_i_var283);
		_i_var330 = (_i_var305) * (_i_var13);
		_i_var331 = (_i_var306) * (_i_var13);
		_i_var332 = (_i_var278) * (_i_var24);
		_i_var333 = (_i_var296) * (_i_var24);
		_i_var334 = (_i_var308)+(_i_var307);
		_i_var335 = (_i_var309) * (_i_var13);
		_i_var336 = (_i_var310) * (_i_var13);
		_i_var337 = (_i_var268) * (_i_var14);
		_i_var338 = (_i_var311) * (_i_var13);
		_i_var339 = (_i_var269) * (_i_var14);
		_i_var340 = (_i_var312) * (_i_var13);
		_i_var341 = (_i_var313) * (_i_var13);
		_i_var342 = (_i_var314)+(_i_var291);
		_i_var343 = (_i_var316)+(_i_var315);
		_i_var344 = (_i_var318)+(_i_var317);
		_i_var345 = (_i_var260) * (_i_var64);
		_i_var346 = (_i_var165) * (_i_var229);
		_i_var347 = (_i_var319)+(_i_var293);
		_i_var348 = (_i_var320) * (_i_var13);
		_i_var349 = (_i_var263) * (_i_var25);
		_i_var350 = (_i_var322)+(_i_var321);
		_i_var351 = (_i_var274) * (_i_var90);
		_i_var352 = (_i_var179) * (_i_var230);
		_i_var353 = (_i_var287) * (_i_var22);
		_i_var354 = (_i_var288) * (_i_var20);
		_i_var355 = (_i_var263) * (_i_var23);
		_i_var356 = (_i_var324)+(_i_var323);
		_i_var357 = (_i_var325)+(_i_var299);
		_i_var358 = (_i_var326) * (_i_var13);
		_i_var359 = (_i_var62) * (_i_var268);
		_i_var360 = (_i_var328)+(_i_var327);
		_i_var361 = (_i_var329) * (_i_var13);
		_i_var362 = (_i_var330) * (_i_var13);
		_i_var363 = (_i_var331) * (_i_var19);
		_i_var364 = (_i_var332) * (_i_var13);
		_i_var365 = (_i_var263) * (_i_var14);
		_i_var366 = (_i_var333) * (_i_var13);
		_i_var367 = (_i_var264) * (_i_var14);
		_i_var368 = (_i_var334)+(_i_var307);
		_i_var369 = (_i_var335) * (_i_var13);
		_i_var370 = (_i_var278) * (_i_var14);
		_i_var371 = (_i_var337)+(_i_var336);
		_i_var372 = (_i_var339)+(_i_var338);
		_i_var373 = (_i_var340) * (_i_var24);
		_i_var374 = (_i_var341) * (_i_var13);
		_i_var375 = (_i_var342) * (_i_var13);
		_i_var376 = (_i_var343)+(_i_var315);
		_i_var377 = (_i_var344)+(_i_var317);
		_i_var378 = (_i_var346)+(_i_var345);
		_i_var379 = (_i_var347) * (_i_var13);
		_i_var380 = (_i_var348) * (_i_var12);
		_i_var381 = (_i_var350)+(_i_var349);
		_i_var382 = (_i_var352)+(_i_var351);
		_i_var383 = (_i_var354)+(_i_var353);
		_i_var384 = (_i_var268) * (_i_var17);
		_i_var385 = (_i_var356)+(_i_var355);
		_i_var386 = (_i_var357) * (_i_var13);
		_i_var387 = (_i_var358) * (_i_var12);
		_i_var388 = (_i_var360)+(_i_var359);
		_i_var389 = (_i_var57) * (_i_var269);
		_i_var390 = (_i_var71) * (_i_var263);
		_i_var391 = (_i_var361) * (_i_var24);
		_i_var392 = (_i_var362) * (_i_var19);
		_i_var393 = (_i_var363) * (_i_var13);
		_i_var394 = (_i_var306) * (_i_var14);
		_i_var395 = (_i_var365)+(_i_var364);
		_i_var396 = (_i_var367)+(_i_var366);
		_i_var397 = (_i_var368) * (_i_var13);
		_i_var398 = (_i_var370)+(_i_var369);
		_i_var399 = (_i_var371) * (_i_var11);
		_i_var400 = (_i_var372) * (_i_var10);
		_i_var401 = (_i_var373) * (_i_var13);
		_i_var402 = (_i_var374) * (_i_var19);
		_i_var403 = (_i_var375) * (_i_var24);
		_i_var404 = (_i_var376) * (_i_var13);
		_i_var405 = (_i_var377) * (_i_var13);
		_i_var406 = (_i_var378)+(_i_var345);
		_i_var407 = (_i_var379) * (_i_var36);
		_i_var408 = (_i_var381)+(_i_var380);
		_i_var409 = (_i_var382)+(_i_var351);
		_i_var410 = (_i_var383) * (_i_var13);
		_i_var411 = (_i_var385)+(_i_var384);
		_i_var412 = (_i_var76) * (_i_var287);
		_i_var413 = (_i_var66) * (_i_var288);
		_i_var414 = (_i_var386) * (_i_var36);
		_i_var415 = (_i_var388)+(_i_var387);
		_i_var416 = (_i_var68) * (_i_var278);
		_i_var417 = (_i_var94) * (_i_var296);
		_i_var418 = (_i_var69) * (_i_var278);
		_i_var419 = (_i_var390)+(_i_var389);
		_i_var420 = (_i_var391) * (_i_var13);
		_i_var421 = (_i_var392) * (_i_var13);
		_i_var422 = (_i_var330) * (_i_var14);
		_i_var423 = (_i_var394)+(_i_var393);
		_i_var424 = (_i_var395) * (_i_var10);
		_i_var425 = (_i_var396) * (_i_var11);
		_i_var426 = (_i_var397) * (_i_var24);
		_i_var427 = (_i_var398) * (_i_var6);
		_i_var428 = (_i_var400)+(_i_var399);
		_i_var429 = (_i_var401) * (_i_var13);
		_i_var430 = (_i_var375) * (_i_var14);
		_i_var431 = (_i_var402) * (_i_var13);
		_i_var432 = (_i_var341) * (_i_var14);
		_i_var433 = (_i_var403) * (_i_var13);
		_i_var434 = (_i_var340) * (_i_var14);
		_i_var435 = (_i_var404) * (_i_var13);
		_i_var436 = (_i_var405) * (_i_var13);
		_i_var437 = (_i_var406) * (_i_var13);
		_i_var438 = (_i_var405) * (_i_var15);
		_i_var439 = (_i_var408)+(_i_var407);
		_i_var440 = (_i_var409) * (_i_var13);
		_i_var441 = (_i_var411)+(_i_var410);
		_i_var442 = (_i_var331) * (_i_var20);
		_i_var443 = (_i_var413)+(_i_var412);
		_i_var444 = (_i_var306) * (_i_var15);
		_i_var445 = (_i_var415)+(_i_var414);
		_i_var446 = (_i_var67) * (_i_var263);
		_i_var447 = (_i_var417)+(_i_var416);
		_i_var448 = (_i_var375) * (_i_var12);
		_i_var449 = (_i_var419)+(_i_var418);
		_i_var450 = (_i_var420) * (_i_var13);
		_i_var451 = (_i_var358) * (_i_var14);
		_i_var452 = (_i_var422)+(_i_var421);
		_i_var453 = (_i_var423) * (_i_var10);
		_i_var454 = (_i_var425)+(_i_var424);
		_i_var455 = (_i_var358) * (_i_var24);
		_i_var456 = (_i_var426) * (_i_var13);
		_i_var457 = (_i_var386) * (_i_var14);
		_i_var458 = (_i_var427) * (_i_var13);
		_i_var459 = (_i_var428)+(_i_var427);
		_i_var460 = (_i_var430)+(_i_var429);
		_i_var461 = (_i_var432)+(_i_var431);
		_i_var462 = (_i_var44) * (_i_var398);
		_i_var463 = (_i_var45) * (_i_var372);
		_i_var464 = (_i_var372) * (_i_var0);
		_i_var465 = (_i_var395) * (_i_var16);
		_i_var466 = (_i_var434)+(_i_var433);
		_i_var467 = (_i_var435) * (_i_var22);
		_i_var468 = (_i_var436) * (_i_var20);
		_i_var469 = (_i_var437) * (_i_var23);
		_i_var470 = (_i_var439)+(_i_var438);
		_i_var471 = (_i_var440) * (_i_var46);
		_i_var472 = (_i_var441) * (_i_var13);
		_i_var473 = (_i_var362) * (_i_var22);
		_i_var474 = (_i_var443)+(_i_var442);
		_i_var475 = (_i_var361) * (_i_var23);
		_i_var476 = (_i_var445)+(_i_var444);
		_i_var477 = (_i_var397) * (_i_var46);
		_i_var478 = (_i_var447)+(_i_var446);
		_i_var479 = (_i_var374) * (_i_var20);
		_i_var480 = (_i_var70) * (_i_var288);
		_i_var481 = (_i_var341) * (_i_var15);
		_i_var482 = (_i_var449)+(_i_var448);
		_i_var483 = (_i_var101) * (_i_var263);
		_i_var484 = (_i_var88) * (_i_var278);
		_i_var485 = (_i_var451)+(_i_var450);
		_i_var486 = (_i_var452) * (_i_var11);
		_i_var487 = (_i_var454)+(_i_var453);
		_i_var488 = (_i_var455) * (_i_var13);
		_i_var489 = (_i_var361) * (_i_var14);
		_i_var490 = (_i_var457)+(_i_var456);
		_i_var491 = (_i_var459)+(_i_var458);
		_i_var492 = (_i_var460) * (_i_var6);
		_i_var493 = (_i_var461) * (_i_var10);
		_i_var494 = (_i_var463)+(_i_var462);
		_i_var495 = (_i_var460) * (_i_var5);
		_i_var496 = (_i_var460) * (_i_var0);
		_i_var497 = (_i_var465)+(_i_var464);
		_i_var498 = (_i_var461) * (_i_var0);
		_i_var499 = (_i_var466) * (_i_var16);
		_i_var500 = (_i_var398) * (_i_var5);
		_i_var501 = (_i_var398) * (_i_var0);
		_i_var502 = (_i_var468)+(_i_var467);
		_i_var503 = (_i_var404) * (_i_var17);
		_i_var504 = (_i_var470)+(_i_var469);
		_i_var505 = (_i_var348) * (_i_var29);
		_i_var506 = (_i_var472)+(_i_var471);
		_i_var507 = (_i_var474)+(_i_var473);
		_i_var508 = (_i_var330) * (_i_var17);
		_i_var509 = (_i_var476)+(_i_var475);
		_i_var510 = (_i_var358) * (_i_var29);
		_i_var511 = (_i_var478)+(_i_var477);
		_i_var512 = (_i_var480)+(_i_var479);
		_i_var513 = (_i_var340) * (_i_var23);
		_i_var514 = (_i_var482)+(_i_var481);
		_i_var515 = (_i_var375) * (_i_var29);
		_i_var516 = (_i_var484)+(_i_var483);
		_i_var517 = (_i_var485) * (_i_var6);
		_i_var518 = (_i_var487)+(_i_var486);
		_i_var519 = (_i_var489)+(_i_var488);
		_i_var520 = (_i_var490) * (_i_var11);
		_i_var521 = (_i_var491) * (_i_var13);
		_i_var522 = (_i_var462)+(_i_var492);
		_i_var523 = (_i_var494)+(_i_var493);
		_i_var524 = (_i_var496)+(_i_var495);
		_i_var525 = (_i_var497) * (_i_var4);
		_i_var526 = (_i_var499)+(_i_var498);
		_i_var527 = (_i_var501)+(_i_var500);
		_i_var528 = (_i_var502) * (_i_var13);
		_i_var529 = (_i_var504)+(_i_var503);
		_i_var530 = (_i_var437) * (_i_var25);
		_i_var531 = (_i_var506)+(_i_var505);
		_i_var532 = (_i_var507) * (_i_var13);
		_i_var533 = (_i_var509)+(_i_var508);
		_i_var534 = (_i_var361) * (_i_var25);
		_i_var535 = (_i_var511)+(_i_var510);
		_i_var536 = (_i_var512) * (_i_var13);
		_i_var537 = (_i_var514)+(_i_var513);
		_i_var538 = (_i_var340) * (_i_var25);
		_i_var539 = (_i_var516)+(_i_var515);
		_i_var540 = (_i_var517) * (_i_var13);
		_i_var541 = (_i_var518)+(_i_var517);
		_i_var542 = (_i_var519) * (_i_var10);
		_i_var543 = (_i_var521)+(_i_var520);
		_i_var544 = (_i_var522) * (_i_var13);
		_i_var545 = (_i_var523)+(_i_var492);
		_i_var546 = (_i_var466) * (_i_var10);
		_i_var547 = (_i_var45) * (_i_var395);
		_i_var548 = (_i_var524) * (offsets(4, 0));
		_i_var549 = (_i_var525) * (_i_var13);
		_i_var550 = (_i_var526) * (_i_var4);
		_i_var551 = (_i_var527) * (offsets(4, 0));
		_i_var552 = (_i_var529)+(_i_var528);
		_i_var553 = (_i_var531)+(_i_var530);
		_i_var554 = (_i_var533)+(_i_var532);
		_i_var555 = (_i_var535)+(_i_var534);
		_i_var556 = (_i_var537)+(_i_var536);
		_i_var557 = (_i_var539)+(_i_var538);
		_i_var558 = (_i_var541)+(_i_var540);
		_i_var559 = (_i_var543)+(_i_var542);
		_i_var560 = (_i_var545)+(_i_var544);
		_i_var561 = (_i_var547)+(_i_var546);
		_i_var562 = (_i_var549)+(_i_var548);
		_i_var563 = (_i_var551)+(_i_var550);
		_i_var564 = (_i_var552) * (_i_var58);
		_i_var565 = (_i_var553) * (_i_var14);
		_i_var566 = (_i_var554) * (_i_var58);
		_i_var567 = (_i_var555) * (_i_var14);
		_i_var568 = (_i_var556) * (_i_var58);
		_i_var569 = (_i_var557) * (_i_var14);
		_i_var570 = (_i_var558) * (_i_var21);
		_i_var571 = (_i_var559) * (_i_var0);
		_i_var572 = (_i_var560) * (_i_var21);
		_i_var573 = (_i_var561) * (_i_var0);
		_i_var574 = (_i_var562) * (_i_var3);
		_i_var575 = (_i_var563) * (_i_var35);
		_i_var576 = (_i_var565)+(_i_var564);
		_i_var577 = (_i_var567)+(_i_var566);
		_i_var578 = (_i_var569)+(_i_var568);
		_i_var579 = (_i_var571)+(_i_var570);
		_i_var580 = (_i_var573)+(_i_var572);
		_i_var581 = (_i_var575)+(_i_var574);
		hessian(0, 0) = _i_var576;
		hessian(1, 0) = _i_var577;
		hessian(2, 0) = _i_var578;
		hessian(0, 1) = _i_var577;
		hessian(1, 1) = _i_var579;
		hessian(2, 1) = _i_var580;
		hessian(0, 2) = _i_var578;
		hessian(1, 2) = _i_var580;
		hessian(2, 2) = _i_var581;
	}

}

namespace Codegen
{
	void compute3AxisKibouLeftArmEnergyFromTwoTarget(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = std::cos(angles(0, 0));
		_i_var11 = (_i_var8)+(_i_var7);
		_i_var12 = std::sin(angles(1, 0));
		_i_var13 = (_i_var9)+(offsets(3, 0));
		_i_var14 = (_i_var1)-(_i_var10);
		_i_var15 = (offsets(3, 0)) * (_i_var0);
		_i_var16 = std::sin(angles(0, 0));
		_i_var17 = -(_i_var11);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var13) * (_i_var0);
		_i_var20 = (offsets(3, 0)) * (_i_var12);
		_i_var21 = (_i_var15) * (_i_var14);
		_i_var22 = (_i_var15) * (_i_var10);
		_i_var23 = (_i_var17) * (_i_var16);
		_i_var24 = (_i_var18) * (_i_var10);
		_i_var25 = (_i_var19) * (_i_var14);
		_i_var26 = (_i_var19) * (_i_var10);
		_i_var27 = (_i_var20) * (_i_var10);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var18) * (_i_var16);
		_i_var30 = (_i_var11) * (_i_var10);
		_i_var31 = (_i_var24)+(_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var20) * (_i_var16);
		_i_var34 = (_i_var27)+(offsets(1, 0));
		_i_var35 = (_i_var28)+(offsets(0, 0));
		_i_var36 = (_i_var30)+(_i_var29);
		_i_var37 = (_i_var31)+(offsets(1, 0));
		_i_var38 = (_i_var32)+(offsets(0, 0));
		_i_var39 = (_i_var33)+(offsets(2, 0));
		_i_var40 = (targetElbow(1, 0)) - (_i_var34);
		_i_var41 = (targetElbow(0, 0)) - (_i_var35);
		_i_var42 = (_i_var36)+(offsets(2, 0));
		_i_var43 = (targetHand(1, 0)) - (_i_var37);
		_i_var44 = (targetHand(0, 0)) - (_i_var38);
		_i_var45 = (targetElbow(2, 0)) - (_i_var39);
		_i_var46 = (_i_var40) * (_i_var40);
		_i_var47 = (_i_var41) * (_i_var41);
		_i_var48 = (targetHand(2, 0)) - (_i_var42);
		_i_var49 = (_i_var43) * (_i_var43);
		_i_var50 = (_i_var44) * (_i_var44);
		_i_var51 = (_i_var45) * (_i_var45);
		_i_var52 = (_i_var47)+(_i_var46);
		_i_var53 = (_i_var48) * (_i_var48);
		_i_var54 = (_i_var50)+(_i_var49);
		_i_var55 = (_i_var52)+(_i_var51);
		_i_var56 = (_i_var54)+(_i_var53);
		_i_var57 = std::sqrt(_i_var55);
		_i_var58 = std::sqrt(_i_var56);
		_i_var59 = (_i_var57) * (_i_var57);
		_i_var60 = (_i_var58) * (_i_var58);
		_i_var61 = (_i_var60)+(_i_var59);
		_i_var62 = 0.5;
		_i_var63 = (_i_var62) * (_i_var61);
		energy = _i_var63;
	}
	void compute3AxisKibouLeftArmEnergyFromTwoTargetGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = std::cos(angles(0, 0));
		_i_var11 = (_i_var8)+(_i_var7);
		_i_var12 = std::sin(angles(1, 0));
		_i_var13 = (_i_var9)+(offsets(3, 0));
		_i_var14 = (_i_var1)-(_i_var10);
		_i_var15 = (offsets(3, 0)) * (_i_var0);
		_i_var16 = std::sin(angles(0, 0));
		_i_var17 = -(_i_var11);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var13) * (_i_var0);
		_i_var20 = (offsets(3, 0)) * (_i_var12);
		_i_var21 = (_i_var15) * (_i_var14);
		_i_var22 = (_i_var15) * (_i_var10);
		_i_var23 = (_i_var17) * (_i_var16);
		_i_var24 = (_i_var18) * (_i_var10);
		_i_var25 = (_i_var19) * (_i_var14);
		_i_var26 = (_i_var19) * (_i_var10);
		_i_var27 = (_i_var20) * (_i_var10);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var18) * (_i_var16);
		_i_var30 = (_i_var11) * (_i_var10);
		_i_var31 = (_i_var24)+(_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var20) * (_i_var16);
		_i_var34 = (_i_var27)+(offsets(1, 0));
		_i_var35 = (_i_var28)+(offsets(0, 0));
		_i_var36 = (_i_var30)+(_i_var29);
		_i_var37 = (_i_var31)+(offsets(1, 0));
		_i_var38 = (_i_var32)+(offsets(0, 0));
		_i_var39 = (_i_var33)+(offsets(2, 0));
		_i_var40 = (targetElbow(1, 0)) - (_i_var34);
		_i_var41 = (targetElbow(0, 0)) - (_i_var35);
		_i_var42 = (_i_var36)+(offsets(2, 0));
		_i_var43 = (targetHand(1, 0)) - (_i_var37);
		_i_var44 = (targetHand(0, 0)) - (_i_var38);
		_i_var45 = (targetElbow(2, 0)) - (_i_var39);
		_i_var46 = (_i_var40) * (_i_var40);
		_i_var47 = (_i_var41) * (_i_var41);
		_i_var48 = (targetHand(2, 0)) - (_i_var42);
		_i_var49 = (_i_var43) * (_i_var43);
		_i_var50 = (_i_var44) * (_i_var44);
		_i_var51 = (_i_var45) * (_i_var45);
		_i_var52 = (_i_var47)+(_i_var46);
		_i_var53 = (_i_var48) * (_i_var48);
		_i_var54 = (_i_var50)+(_i_var49);
		_i_var55 = (_i_var52)+(_i_var51);
		_i_var56 = (_i_var54)+(_i_var53);
		_i_var57 = std::sqrt(_i_var55);
		_i_var58 = 2;
		_i_var59 = 0.5;
		_i_var60 = std::sqrt(_i_var56);
		_i_var61 = (_i_var58) * (_i_var57);
		_i_var62 = (_i_var59) * (_i_var57);
		_i_var63 = (_i_var58) * (_i_var60);
		_i_var64 = (_i_var59) * (_i_var60);
		_i_var65 = (_i_var1) / (_i_var61);
		_i_var66 = (_i_var58) * (_i_var62);
		_i_var67 = (_i_var1) / (_i_var63);
		_i_var68 = (_i_var58) * (_i_var64);
		_i_var69 = (_i_var66) * (_i_var65);
		_i_var70 = (_i_var68) * (_i_var67);
		_i_var71 = (_i_var69) * (_i_var40);
		_i_var72 = (_i_var70) * (_i_var48);
		_i_var73 = (_i_var70) * (_i_var43);
		_i_var74 = (_i_var69) * (_i_var41);
		_i_var75 = (_i_var70) * (_i_var44);
		_i_var76 = -1;
		_i_var77 = (_i_var58) * (_i_var71);
		_i_var78 = (_i_var58) * (_i_var72);
		_i_var79 = (_i_var58) * (_i_var73);
		_i_var80 = (_i_var58) * (_i_var74);
		_i_var81 = (_i_var58) * (_i_var75);
		_i_var82 = (_i_var77) * (_i_var76);
		_i_var83 = (_i_var78) * (_i_var76);
		_i_var84 = (_i_var79) * (_i_var76);
		_i_var85 = (_i_var80) * (_i_var76);
		_i_var86 = (_i_var81) * (_i_var76);
		_i_var87 = (_i_var69) * (_i_var45);
		_i_var88 = (_i_var82) * (_i_var20);
		_i_var89 = (_i_var83) * (_i_var11);
		_i_var90 = (_i_var84) * (_i_var16);
		_i_var91 = (_i_var85) * (_i_var14);
		_i_var92 = (_i_var85) * (_i_var10);
		_i_var93 = (_i_var86) * (_i_var14);
		_i_var94 = (_i_var86) * (_i_var10);
		_i_var95 = (_i_var58) * (_i_var87);
		_i_var96 = (_i_var86) * (_i_var19);
		_i_var97 = (_i_var89)+(_i_var88);
		_i_var98 = (_i_var90) * (_i_var76);
		_i_var99 = (_i_var83) * (_i_var10);
		_i_var100 = (_i_var92)+(_i_var91);
		_i_var101 = (_i_var94)+(_i_var93);
		_i_var102 = (_i_var95) * (_i_var76);
		_i_var103 = (_i_var84) * (_i_var10);
		_i_var104 = (_i_var83) * (_i_var16);
		_i_var105 = (_i_var85) * (_i_var15);
		_i_var106 = (_i_var84) * (_i_var18);
		_i_var107 = (_i_var97)+(_i_var96);
		_i_var108 = (_i_var99)+(_i_var98);
		_i_var109 = (_i_var100) * (offsets(3, 0));
		_i_var110 = (_i_var101) * (_i_var13);
		_i_var111 = (_i_var82) * (_i_var10);
		_i_var112 = (_i_var102) * (_i_var16);
		_i_var113 = (_i_var104)+(_i_var103);
		_i_var114 = (_i_var96)+(_i_var105);
		_i_var115 = (_i_var107)+(_i_var106);
		_i_var116 = (_i_var83) * (_i_var18);
		_i_var117 = (_i_var102) * (_i_var20);
		_i_var118 = (_i_var108) * (_i_var5);
		_i_var119 = (_i_var110)+(_i_var109);
		_i_var120 = (_i_var112)+(_i_var111);
		_i_var121 = (_i_var108) * (_i_var4);
		_i_var122 = (_i_var108) * (_i_var0);
		_i_var123 = (_i_var113) * (_i_var12);
		_i_var124 = (_i_var101) * (_i_var0);
		_i_var125 = (_i_var114) * (_i_var76);
		_i_var126 = (_i_var115)+(_i_var105);
		_i_var127 = (_i_var84) * (_i_var17);
		_i_var128 = (_i_var117)+(_i_var116);
		_i_var129 = (_i_var118) * (_i_var76);
		_i_var130 = (_i_var119)+(_i_var118);
		_i_var131 = (_i_var113) * (_i_var13);
		_i_var132 = (_i_var120) * (offsets(3, 0));
		_i_var133 = (_i_var122)+(_i_var121);
		_i_var134 = (_i_var124)+(_i_var123);
		_i_var135 = -(_i_var16);
		_i_var136 = (_i_var126)+(_i_var125);
		_i_var137 = (_i_var128)+(_i_var127);
		_i_var138 = -(_i_var12);
		_i_var139 = (_i_var130)+(_i_var129);
		_i_var140 = (_i_var132)+(_i_var131);
		_i_var141 = (_i_var133) * (_i_var3);
		_i_var142 = -(_i_var2);
		_i_var143 = (_i_var134) * (offsets(4, 0));
		_i_var144 = (_i_var136) * (_i_var135);
		_i_var145 = (_i_var137) * (_i_var10);
		_i_var146 = (_i_var139) * (_i_var138);
		_i_var147 = (_i_var140) * (_i_var0);
		_i_var148 = (_i_var141) * (_i_var6);
		_i_var149 = (_i_var143) * (_i_var142);
		_i_var150 = (_i_var145)+(_i_var144);
		_i_var151 = (_i_var147)+(_i_var146);
		_i_var152 = (_i_var149)+(_i_var148);
		gradient(0, 0) = _i_var150;
		gradient(1, 0) = _i_var151;
		gradient(2, 0) = _i_var152;
	}
	void compute3AxisKibouLeftArmEnergyFromTwoTargetHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399, _i_var400, _i_var401, _i_var402, _i_var403, _i_var404;
		double _i_var405, _i_var406, _i_var407, _i_var408, _i_var409, _i_var410, _i_var411, _i_var412, _i_var413, _i_var414, _i_var415, _i_var416, _i_var417, _i_var418, _i_var419;
		double _i_var420, _i_var421, _i_var422, _i_var423, _i_var424, _i_var425, _i_var426, _i_var427, _i_var428, _i_var429, _i_var430, _i_var431, _i_var432, _i_var433, _i_var434;
		double _i_var435, _i_var436, _i_var437, _i_var438, _i_var439, _i_var440, _i_var441, _i_var442, _i_var443, _i_var444, _i_var445, _i_var446, _i_var447, _i_var448, _i_var449;
		double _i_var450, _i_var451, _i_var452, _i_var453, _i_var454, _i_var455, _i_var456, _i_var457, _i_var458, _i_var459, _i_var460, _i_var461, _i_var462, _i_var463, _i_var464;
		double _i_var465, _i_var466, _i_var467, _i_var468, _i_var469, _i_var470, _i_var471, _i_var472, _i_var473, _i_var474, _i_var475, _i_var476, _i_var477, _i_var478, _i_var479;
		double _i_var480, _i_var481, _i_var482, _i_var483, _i_var484, _i_var485, _i_var486, _i_var487, _i_var488, _i_var489, _i_var490, _i_var491, _i_var492, _i_var493, _i_var494;
		double _i_var495, _i_var496, _i_var497, _i_var498, _i_var499, _i_var500, _i_var501, _i_var502, _i_var503, _i_var504, _i_var505, _i_var506, _i_var507, _i_var508, _i_var509;
		double _i_var510, _i_var511, _i_var512, _i_var513, _i_var514, _i_var515, _i_var516, _i_var517, _i_var518, _i_var519, _i_var520, _i_var521, _i_var522, _i_var523, _i_var524;
		double _i_var525, _i_var526, _i_var527;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (offsets(3, 0)) * (_i_var0);
		_i_var20 = (_i_var15) * (_i_var14);
		_i_var21 = (_i_var16) * (_i_var13);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var18) * (_i_var13);
		_i_var24 = (offsets(3, 0)) * (_i_var11);
		_i_var25 = (_i_var19) * (_i_var17);
		_i_var26 = (_i_var19) * (_i_var13);
		_i_var27 = (_i_var16) * (_i_var14);
		_i_var28 = (_i_var10) * (_i_var13);
		_i_var29 = (_i_var21)+(_i_var20);
		_i_var30 = (_i_var23)+(_i_var22);
		_i_var31 = -1;
		_i_var32 = -(_i_var11);
		_i_var33 = (_i_var24) * (_i_var13);
		_i_var34 = (_i_var26)+(_i_var25);
		_i_var35 = (_i_var6) * (_i_var3);
		_i_var36 = -(_i_var2);
		_i_var37 = (_i_var28)+(_i_var27);
		_i_var38 = (_i_var29)+(offsets(1, 0));
		_i_var39 = (_i_var30)+(offsets(0, 0));
		_i_var40 = -(_i_var14);
		_i_var41 = (_i_var32) * (_i_var31);
		_i_var42 = (_i_var24) * (_i_var14);
		_i_var43 = (_i_var33)+(offsets(1, 0));
		_i_var44 = (_i_var34)+(offsets(0, 0));
		_i_var45 = (_i_var35) * (_i_var4);
		_i_var46 = (_i_var35) * (_i_var0);
		_i_var47 = (_i_var36) * (offsets(4, 0));
		_i_var48 = (_i_var37)+(offsets(2, 0));
		_i_var49 = (targetHand(1, 0)) - (_i_var38);
		_i_var50 = (targetHand(0, 0)) - (_i_var39);
		_i_var51 = (_i_var40) * (_i_var31);
		_i_var52 = (_i_var41)-(_i_var11);
		_i_var53 = (_i_var42)+(offsets(2, 0));
		_i_var54 = (targetElbow(1, 0)) - (_i_var43);
		_i_var55 = (targetElbow(0, 0)) - (_i_var44);
		_i_var56 = (_i_var46)+(_i_var45);
		_i_var57 = (_i_var47) * (_i_var11);
		_i_var58 = (_i_var47) * (_i_var0);
		_i_var59 = (targetHand(2, 0)) - (_i_var48);
		_i_var60 = (_i_var49) * (_i_var49);
		_i_var61 = (_i_var50) * (_i_var50);
		_i_var62 = (_i_var51)-(_i_var14);
		_i_var63 = (_i_var40) * (_i_var16);
		_i_var64 = (_i_var13) * (_i_var15);
		_i_var65 = (_i_var52) * (_i_var5);
		_i_var66 = (_i_var32) * (_i_var12);
		_i_var67 = (_i_var0) * (_i_var12);
		_i_var68 = (targetElbow(2, 0)) - (_i_var53);
		_i_var69 = (_i_var54) * (_i_var54);
		_i_var70 = (_i_var55) * (_i_var55);
		_i_var71 = (_i_var32) * (offsets(3, 0));
		_i_var72 = (_i_var0) * (offsets(3, 0));
		_i_var73 = (_i_var56) * (_i_var31);
		_i_var74 = (_i_var56) * (_i_var13);
		_i_var75 = (_i_var57) * (_i_var14);
		_i_var76 = (_i_var58) * (_i_var17);
		_i_var77 = (_i_var58) * (_i_var13);
		_i_var78 = (_i_var59) * (_i_var59);
		_i_var79 = (_i_var61)+(_i_var60);
		_i_var80 = (_i_var40) * (_i_var10);
		_i_var81 = (_i_var13) * (_i_var16);
		_i_var82 = (_i_var62) * (_i_var18);
		_i_var83 = (_i_var64)+(_i_var63);
		_i_var84 = (_i_var65) * (_i_var31);
		_i_var85 = (_i_var66) * (_i_var17);
		_i_var86 = (_i_var66) * (_i_var13);
		_i_var87 = (_i_var65) * (_i_var13);
		_i_var88 = (_i_var67) * (_i_var14);
		_i_var89 = (_i_var68) * (_i_var68);
		_i_var90 = (_i_var70)+(_i_var69);
		_i_var91 = (_i_var71) * (_i_var17);
		_i_var92 = (_i_var71) * (_i_var13);
		_i_var93 = (_i_var72) * (_i_var13);
		_i_var94 = (_i_var72) * (_i_var14);
		_i_var95 = (_i_var73) * (_i_var14);
		_i_var96 = (_i_var57) * (_i_var13);
		_i_var97 = (_i_var75)+(_i_var74);
		_i_var98 = (_i_var77)+(_i_var76);
		_i_var99 = (_i_var62) * (_i_var19);
		_i_var100 = (_i_var13) * (_i_var24);
		_i_var101 = (_i_var79)+(_i_var78);
		_i_var102 = (_i_var81)+(_i_var80);
		_i_var103 = 2;
		_i_var104 = (_i_var82) * (_i_var31);
		_i_var105 = (_i_var83) * (_i_var31);
		_i_var106 = (_i_var84) * (_i_var14);
		_i_var107 = (_i_var67) * (_i_var13);
		_i_var108 = (_i_var86)+(_i_var85);
		_i_var109 = (_i_var88)+(_i_var87);
		_i_var110 = (_i_var90)+(_i_var89);
		_i_var111 = (_i_var92)+(_i_var91);
		_i_var112 = (_i_var93) * (_i_var31);
		_i_var113 = (_i_var94) * (_i_var31);
		_i_var114 = (_i_var96)+(_i_var95);
		_i_var115 = (_i_var97) * (_i_var31);
		_i_var116 = (_i_var98) * (_i_var31);
		_i_var117 = (_i_var40) * (_i_var24);
		_i_var118 = (_i_var99) * (_i_var31);
		_i_var119 = (_i_var100) * (_i_var31);
		_i_var120 = std::sqrt(_i_var101);
		_i_var121 = (_i_var102) * (_i_var31);
		_i_var122 = (_i_var104) * (_i_var103);
		_i_var123 = (_i_var105) * (_i_var103);
		_i_var124 = (_i_var107)+(_i_var106);
		_i_var125 = (_i_var108) * (_i_var31);
		_i_var126 = (_i_var109) * (_i_var31);
		_i_var127 = std::sqrt(_i_var110);
		_i_var128 = (_i_var111) * (_i_var31);
		_i_var129 = (_i_var112) * (_i_var103);
		_i_var130 = (_i_var113) * (_i_var103);
		_i_var131 = (_i_var114) * (_i_var31);
		_i_var132 = (_i_var115) * (_i_var103);
		_i_var133 = (_i_var116) * (_i_var103);
		_i_var134 = (_i_var117) * (_i_var31);
		_i_var135 = (_i_var118) * (_i_var103);
		_i_var136 = (_i_var119) * (_i_var103);
		_i_var137 = (_i_var103) * (_i_var120);
		_i_var138 = 0.5;
		_i_var139 = (_i_var121) * (_i_var103);
		_i_var140 = (_i_var122) * (_i_var50);
		_i_var141 = (_i_var123) * (_i_var49);
		_i_var142 = (_i_var124) * (_i_var31);
		_i_var143 = (_i_var125) * (_i_var103);
		_i_var144 = (_i_var126) * (_i_var103);
		_i_var145 = (_i_var103) * (_i_var127);
		_i_var146 = (_i_var128) * (_i_var103);
		_i_var147 = (_i_var129) * (_i_var54);
		_i_var148 = (_i_var130) * (_i_var68);
		_i_var149 = (_i_var131) * (_i_var103);
		_i_var150 = (_i_var132) * (_i_var59);
		_i_var151 = (_i_var133) * (_i_var50);
		_i_var152 = (_i_var134) * (_i_var103);
		_i_var153 = (_i_var135) * (_i_var55);
		_i_var154 = (_i_var136) * (_i_var68);
		_i_var155 = (_i_var137) * (_i_var137);
		_i_var156 = (_i_var138) * (_i_var120);
		_i_var157 = (_i_var139) * (_i_var59);
		_i_var158 = (_i_var141)+(_i_var140);
		_i_var159 = (_i_var142) * (_i_var103);
		_i_var160 = (_i_var143) * (_i_var50);
		_i_var161 = (_i_var144) * (_i_var59);
		_i_var162 = (_i_var145) * (_i_var145);
		_i_var163 = (_i_var138) * (_i_var127);
		_i_var164 = (_i_var146) * (_i_var55);
		_i_var165 = (_i_var148)+(_i_var147);
		_i_var166 = (_i_var149) * (_i_var49);
		_i_var167 = (_i_var151)+(_i_var150);
		_i_var168 = (_i_var152) * (_i_var54);
		_i_var169 = (_i_var154)+(_i_var153);
		_i_var170 = (_i_var1) / (_i_var155);
		_i_var171 = (_i_var103) * (_i_var156);
		_i_var172 = (_i_var158)+(_i_var157);
		_i_var173 = (_i_var1) / (_i_var137);
		_i_var174 = (_i_var159) * (_i_var49);
		_i_var175 = (_i_var161)+(_i_var160);
		_i_var176 = (_i_var1) / (_i_var162);
		_i_var177 = (_i_var103) * (_i_var163);
		_i_var178 = (_i_var165)+(_i_var164);
		_i_var179 = (_i_var1) / (_i_var145);
		_i_var180 = (_i_var167)+(_i_var166);
		_i_var181 = (_i_var169)+(_i_var168);
		_i_var182 = -(_i_var170);
		_i_var183 = (_i_var172) * (_i_var171);
		_i_var184 = (_i_var172) * (_i_var173);
		_i_var185 = (_i_var175)+(_i_var174);
		_i_var186 = -(_i_var176);
		_i_var187 = (_i_var178) * (_i_var177);
		_i_var188 = (_i_var178) * (_i_var179);
		_i_var189 = (_i_var180) * (_i_var171);
		_i_var190 = (_i_var180) * (_i_var173);
		_i_var191 = (_i_var181) * (_i_var177);
		_i_var192 = (_i_var181) * (_i_var179);
		_i_var193 = (_i_var183) * (_i_var182);
		_i_var194 = (_i_var184) * (_i_var103);
		_i_var195 = (_i_var185) * (_i_var171);
		_i_var196 = (_i_var185) * (_i_var173);
		_i_var197 = (_i_var187) * (_i_var186);
		_i_var198 = (_i_var188) * (_i_var103);
		_i_var199 = (_i_var171) * (_i_var173);
		_i_var200 = (_i_var177) * (_i_var179);
		_i_var201 = (_i_var189) * (_i_var182);
		_i_var202 = (_i_var190) * (_i_var103);
		_i_var203 = (_i_var191) * (_i_var186);
		_i_var204 = (_i_var192) * (_i_var103);
		_i_var205 = (_i_var193) * (_i_var103);
		_i_var206 = (_i_var194) * (_i_var138);
		_i_var207 = (_i_var195) * (_i_var182);
		_i_var208 = (_i_var196) * (_i_var103);
		_i_var209 = (_i_var197) * (_i_var103);
		_i_var210 = (_i_var198) * (_i_var138);
		_i_var211 = (_i_var199) * (_i_var49);
		_i_var212 = (_i_var200) * (_i_var54);
		_i_var213 = (_i_var201) * (_i_var103);
		_i_var214 = (_i_var202) * (_i_var138);
		_i_var215 = (_i_var203) * (_i_var103);
		_i_var216 = (_i_var204) * (_i_var138);
		_i_var217 = (_i_var206)+(_i_var205);
		_i_var218 = (_i_var199) * (_i_var59);
		_i_var219 = (_i_var207) * (_i_var103);
		_i_var220 = (_i_var208) * (_i_var138);
		_i_var221 = (_i_var210)+(_i_var209);
		_i_var222 = (_i_var103) * (_i_var211);
		_i_var223 = (_i_var103) * (_i_var212);
		_i_var224 = (_i_var214)+(_i_var213);
		_i_var225 = (_i_var216)+(_i_var215);
		_i_var226 = (_i_var217) * (_i_var173);
		_i_var227 = (_i_var200) * (_i_var68);
		_i_var228 = (_i_var199) * (_i_var50);
		_i_var229 = (_i_var103) * (_i_var218);
		_i_var230 = (_i_var220)+(_i_var219);
		_i_var231 = (_i_var221) * (_i_var179);
		_i_var232 = (_i_var222) * (_i_var31);
		_i_var233 = (_i_var223) * (_i_var31);
		_i_var234 = (_i_var200) * (_i_var55);
		_i_var235 = (_i_var224) * (_i_var173);
		_i_var236 = (_i_var225) * (_i_var179);
		_i_var237 = (_i_var226) * (_i_var59);
		_i_var238 = (_i_var139) * (_i_var199);
		_i_var239 = (_i_var103) * (_i_var227);
		_i_var240 = (_i_var103) * (_i_var228);
		_i_var241 = (_i_var229) * (_i_var31);
		_i_var242 = (_i_var230) * (_i_var173);
		_i_var243 = (_i_var231) * (_i_var54);
		_i_var244 = (_i_var129) * (_i_var200);
		_i_var245 = (_i_var67) * (_i_var232);
		_i_var246 = (_i_var72) * (_i_var233);
		_i_var247 = (_i_var231) * (_i_var55);
		_i_var248 = (_i_var146) * (_i_var200);
		_i_var249 = (_i_var103) * (_i_var234);
		_i_var250 = (_i_var235) * (_i_var49);
		_i_var251 = (_i_var149) * (_i_var199);
		_i_var252 = (_i_var236) * (_i_var54);
		_i_var253 = (_i_var152) * (_i_var200);
		_i_var254 = (_i_var238)+(_i_var237);
		_i_var255 = (_i_var239) * (_i_var31);
		_i_var256 = (_i_var240) * (_i_var31);
		_i_var257 = (_i_var233) * (_i_var24);
		_i_var258 = (_i_var241) * (_i_var10);
		_i_var259 = (_i_var242) * (_i_var59);
		_i_var260 = (_i_var144) * (_i_var199);
		_i_var261 = (_i_var244)+(_i_var243);
		_i_var262 = (_i_var65) * (_i_var241);
		_i_var263 = (_i_var246)+(_i_var245);
		_i_var264 = (_i_var242) * (_i_var49);
		_i_var265 = (_i_var159) * (_i_var199);
		_i_var266 = (_i_var242) * (_i_var50);
		_i_var267 = (_i_var143) * (_i_var199);
		_i_var268 = (_i_var248)+(_i_var247);
		_i_var269 = (_i_var249) * (_i_var31);
		_i_var270 = (_i_var251)+(_i_var250);
		_i_var271 = (_i_var235) * (_i_var59);
		_i_var272 = (_i_var132) * (_i_var199);
		_i_var273 = (_i_var235) * (_i_var50);
		_i_var274 = (_i_var133) * (_i_var199);
		_i_var275 = (_i_var226) * (_i_var50);
		_i_var276 = (_i_var122) * (_i_var199);
		_i_var277 = (_i_var253)+(_i_var252);
		_i_var278 = (_i_var254)+(_i_var237);
		_i_var279 = (_i_var241) * (_i_var16);
		_i_var280 = (_i_var255) * (_i_var24);
		_i_var281 = (_i_var256) * (_i_var18);
		_i_var282 = (_i_var258)+(_i_var257);
		_i_var283 = (_i_var260)+(_i_var259);
		_i_var284 = (_i_var261)+(_i_var243);
		_i_var285 = (_i_var66) * (_i_var256);
		_i_var286 = (_i_var263)+(_i_var262);
		_i_var287 = (_i_var265)+(_i_var264);
		_i_var288 = (_i_var267)+(_i_var266);
		_i_var289 = (_i_var268)+(_i_var247);
		_i_var290 = (_i_var231) * (_i_var68);
		_i_var291 = (_i_var130) * (_i_var200);
		_i_var292 = (_i_var232) * (_i_var14);
		_i_var293 = (_i_var269) * (_i_var17);
		_i_var294 = (_i_var269) * (_i_var13);
		_i_var295 = (_i_var256) * (_i_var17);
		_i_var296 = (_i_var256) * (_i_var13);
		_i_var297 = (_i_var270)+(_i_var250);
		_i_var298 = (_i_var272)+(_i_var271);
		_i_var299 = (_i_var274)+(_i_var273);
		_i_var300 = (_i_var236) * (_i_var55);
		_i_var301 = (_i_var135) * (_i_var200);
		_i_var302 = (_i_var226) * (_i_var49);
		_i_var303 = (_i_var123) * (_i_var199);
		_i_var304 = (_i_var276)+(_i_var275);
		_i_var305 = (_i_var277)+(_i_var252);
		_i_var306 = (_i_var278) * (_i_var31);
		_i_var307 = (_i_var232) * (_i_var15);
		_i_var308 = (_i_var280)+(_i_var279);
		_i_var309 = (_i_var236) * (_i_var68);
		_i_var310 = (_i_var136) * (_i_var200);
		_i_var311 = (_i_var269) * (_i_var19);
		_i_var312 = (_i_var232) * (_i_var16);
		_i_var313 = (_i_var282)+(_i_var281);
		_i_var314 = (_i_var283)+(_i_var259);
		_i_var315 = (_i_var284) * (_i_var31);
		_i_var316 = (_i_var71) * (_i_var269);
		_i_var317 = (_i_var286)+(_i_var285);
		_i_var318 = (_i_var287)+(_i_var264);
		_i_var319 = (_i_var288)+(_i_var266);
		_i_var320 = (_i_var289) * (_i_var31);
		_i_var321 = (_i_var232) * (_i_var13);
		_i_var322 = (_i_var241) * (_i_var14);
		_i_var323 = (_i_var233) * (_i_var13);
		_i_var324 = (_i_var255) * (_i_var14);
		_i_var325 = (_i_var291)+(_i_var290);
		_i_var326 = (_i_var292) * (_i_var31);
		_i_var327 = (_i_var241) * (_i_var13);
		_i_var328 = (_i_var294)+(_i_var293);
		_i_var329 = (_i_var296)+(_i_var295);
		_i_var330 = (_i_var297) * (_i_var31);
		_i_var331 = (_i_var298)+(_i_var271);
		_i_var332 = (_i_var299)+(_i_var273);
		_i_var333 = (_i_var301)+(_i_var300);
		_i_var334 = (_i_var303)+(_i_var302);
		_i_var335 = (_i_var304)+(_i_var275);
		_i_var336 = (_i_var305) * (_i_var31);
		_i_var337 = (_i_var306) * (_i_var10);
		_i_var338 = (_i_var308)+(_i_var307);
		_i_var339 = (_i_var310)+(_i_var309);
		_i_var340 = (_i_var281)+(_i_var311);
		_i_var341 = (_i_var313)+(_i_var312);
		_i_var342 = (_i_var314) * (_i_var31);
		_i_var343 = (_i_var315) * (_i_var24);
		_i_var344 = (_i_var317)+(_i_var316);
		_i_var345 = (_i_var57) * (_i_var232);
		_i_var346 = (_i_var58) * (_i_var256);
		_i_var347 = (_i_var318) * (_i_var31);
		_i_var348 = (_i_var319) * (_i_var31);
		_i_var349 = (_i_var320) * (_i_var17);
		_i_var350 = (_i_var320) * (_i_var13);
		_i_var351 = (_i_var322)+(_i_var321);
		_i_var352 = (_i_var324)+(_i_var323);
		_i_var353 = (_i_var325)+(_i_var290);
		_i_var354 = (_i_var327)+(_i_var326);
		_i_var355 = (_i_var328) * (offsets(3, 0));
		_i_var356 = (_i_var329) * (_i_var12);
		_i_var357 = (_i_var330) * (_i_var14);
		_i_var358 = (_i_var331) * (_i_var31);
		_i_var359 = (_i_var332) * (_i_var31);
		_i_var360 = (_i_var333)+(_i_var300);
		_i_var361 = (_i_var334)+(_i_var302);
		_i_var362 = (_i_var335) * (_i_var31);
		_i_var363 = (_i_var336) * (_i_var24);
		_i_var364 = (_i_var338)+(_i_var337);
		_i_var365 = (_i_var339)+(_i_var309);
		_i_var366 = (_i_var340) * (_i_var31);
		_i_var367 = (_i_var341)+(_i_var311);
		_i_var368 = (_i_var342) * (_i_var10);
		_i_var369 = (_i_var344)+(_i_var343);
		_i_var370 = (_i_var67) * (_i_var241);
		_i_var371 = (_i_var72) * (_i_var255);
		_i_var372 = (_i_var56) * (_i_var241);
		_i_var373 = (_i_var346)+(_i_var345);
		_i_var374 = (_i_var347) * (_i_var14);
		_i_var375 = (_i_var348) * (_i_var17);
		_i_var376 = (_i_var348) * (_i_var13);
		_i_var377 = (_i_var350)+(_i_var349);
		_i_var378 = (_i_var351) * (_i_var12);
		_i_var379 = (_i_var352) * (offsets(3, 0));
		_i_var380 = (_i_var353) * (_i_var31);
		_i_var381 = (_i_var354) * (_i_var5);
		_i_var382 = (_i_var356)+(_i_var355);
		_i_var383 = (_i_var357) * (_i_var31);
		_i_var384 = (_i_var358) * (_i_var13);
		_i_var385 = (_i_var359) * (_i_var17);
		_i_var386 = (_i_var359) * (_i_var13);
		_i_var387 = (_i_var330) * (_i_var13);
		_i_var388 = (_i_var358) * (_i_var14);
		_i_var389 = (_i_var360) * (_i_var31);
		_i_var390 = (_i_var361) * (_i_var31);
		_i_var391 = (_i_var362) * (_i_var18);
		_i_var392 = (_i_var364)+(_i_var363);
		_i_var393 = (_i_var365) * (_i_var31);
		_i_var394 = (_i_var367)+(_i_var366);
		_i_var395 = (_i_var320) * (_i_var19);
		_i_var396 = (_i_var285)+(_i_var316);
		_i_var397 = (_i_var369)+(_i_var368);
		_i_var398 = (_i_var84) * (_i_var232);
		_i_var399 = (_i_var371)+(_i_var370);
		_i_var400 = (_i_var358) * (_i_var10);
		_i_var401 = (_i_var373)+(_i_var372);
		_i_var402 = (_i_var374) * (_i_var31);
		_i_var403 = (_i_var342) * (_i_var13);
		_i_var404 = (_i_var376)+(_i_var375);
		_i_var405 = (_i_var377) * (offsets(3, 0));
		_i_var406 = (_i_var379)+(_i_var378);
		_i_var407 = (_i_var315) * (_i_var13);
		_i_var408 = (_i_var380) * (_i_var14);
		_i_var409 = (_i_var381) * (_i_var31);
		_i_var410 = (_i_var382)+(_i_var381);
		_i_var411 = (_i_var384)+(_i_var383);
		_i_var412 = (_i_var386)+(_i_var385);
		_i_var413 = (_i_var35) * (_i_var354);
		_i_var414 = (_i_var47) * (_i_var329);
		_i_var415 = (_i_var351) * (_i_var11);
		_i_var416 = (_i_var329) * (_i_var0);
		_i_var417 = (_i_var388)+(_i_var387);
		_i_var418 = (_i_var389) * (_i_var19);
		_i_var419 = (_i_var390) * (_i_var16);
		_i_var420 = (_i_var392)+(_i_var391);
		_i_var421 = (_i_var393) * (_i_var24);
		_i_var422 = (_i_var394) * (_i_var31);
		_i_var423 = (_i_var348) * (_i_var18);
		_i_var424 = (_i_var396)+(_i_var395);
		_i_var425 = (_i_var397)+(_i_var395);
		_i_var426 = (_i_var380) * (_i_var24);
		_i_var427 = (_i_var399)+(_i_var398);
		_i_var428 = (_i_var359) * (_i_var18);
		_i_var429 = (_i_var401)+(_i_var400);
		_i_var430 = (_i_var73) * (_i_var232);
		_i_var431 = (_i_var57) * (_i_var241);
		_i_var432 = (_i_var403)+(_i_var402);
		_i_var433 = (_i_var404) * (_i_var12);
		_i_var434 = (_i_var406)+(_i_var405);
		_i_var435 = (_i_var347) * (_i_var13);
		_i_var436 = (_i_var342) * (_i_var14);
		_i_var437 = (_i_var408)+(_i_var407);
		_i_var438 = (_i_var410)+(_i_var409);
		_i_var439 = (_i_var411) * (_i_var5);
		_i_var440 = (_i_var412) * (_i_var12);
		_i_var441 = (_i_var414)+(_i_var413);
		_i_var442 = (_i_var411) * (_i_var4);
		_i_var443 = (_i_var411) * (_i_var0);
		_i_var444 = (_i_var416)+(_i_var415);
		_i_var445 = (_i_var417) * (_i_var11);
		_i_var446 = (_i_var412) * (_i_var0);
		_i_var447 = (_i_var354) * (_i_var4);
		_i_var448 = (_i_var354) * (_i_var0);
		_i_var449 = (_i_var391)+(_i_var418);
		_i_var450 = (_i_var420)+(_i_var419);
		_i_var451 = (_i_var306) * (_i_var16);
		_i_var452 = (_i_var422)+(_i_var421);
		_i_var453 = (_i_var424)+(_i_var423);
		_i_var454 = (_i_var347) * (_i_var16);
		_i_var455 = (_i_var425)+(_i_var423);
		_i_var456 = (_i_var342) * (_i_var16);
		_i_var457 = (_i_var427)+(_i_var426);
		_i_var458 = (_i_var346)+(_i_var428);
		_i_var459 = (_i_var330) * (_i_var16);
		_i_var460 = (_i_var429)+(_i_var428);
		_i_var461 = (_i_var358) * (_i_var16);
		_i_var462 = (_i_var431)+(_i_var430);
		_i_var463 = (_i_var432) * (_i_var5);
		_i_var464 = (_i_var434)+(_i_var433);
		_i_var465 = (_i_var436)+(_i_var435);
		_i_var466 = (_i_var437) * (offsets(3, 0));
		_i_var467 = (_i_var438) * (_i_var31);
		_i_var468 = (_i_var413)+(_i_var439);
		_i_var469 = (_i_var441)+(_i_var440);
		_i_var470 = (_i_var443)+(_i_var442);
		_i_var471 = (_i_var444) * (offsets(4, 0));
		_i_var472 = (_i_var446)+(_i_var445);
		_i_var473 = (_i_var448)+(_i_var447);
		_i_var474 = (_i_var449) * (_i_var31);
		_i_var475 = (_i_var450)+(_i_var418);
		_i_var476 = (_i_var390) * (_i_var15);
		_i_var477 = (_i_var452)+(_i_var451);
		_i_var478 = (_i_var453) * (_i_var31);
		_i_var479 = (_i_var455)+(_i_var454);
		_i_var480 = (_i_var347) * (_i_var15);
		_i_var481 = (_i_var457)+(_i_var456);
		_i_var482 = (_i_var458) * (_i_var31);
		_i_var483 = (_i_var460)+(_i_var459);
		_i_var484 = (_i_var330) * (_i_var15);
		_i_var485 = (_i_var462)+(_i_var461);
		_i_var486 = (_i_var463) * (_i_var31);
		_i_var487 = (_i_var464)+(_i_var463);
		_i_var488 = (_i_var465) * (_i_var12);
		_i_var489 = (_i_var467)+(_i_var466);
		_i_var490 = (_i_var468) * (_i_var31);
		_i_var491 = (_i_var469)+(_i_var439);
		_i_var492 = (_i_var417) * (_i_var12);
		_i_var493 = (_i_var47) * (_i_var351);
		_i_var494 = (_i_var470) * (_i_var3);
		_i_var495 = (_i_var471) * (_i_var31);
		_i_var496 = (_i_var472) * (offsets(4, 0));
		_i_var497 = (_i_var473) * (_i_var3);
		_i_var498 = (_i_var475)+(_i_var474);
		_i_var499 = (_i_var477)+(_i_var476);
		_i_var500 = (_i_var479)+(_i_var478);
		_i_var501 = (_i_var481)+(_i_var480);
		_i_var502 = (_i_var483)+(_i_var482);
		_i_var503 = (_i_var485)+(_i_var484);
		_i_var504 = (_i_var487)+(_i_var486);
		_i_var505 = (_i_var489)+(_i_var488);
		_i_var506 = (_i_var491)+(_i_var490);
		_i_var507 = (_i_var493)+(_i_var492);
		_i_var508 = (_i_var495)+(_i_var494);
		_i_var509 = (_i_var497)+(_i_var496);
		_i_var510 = (_i_var498) * (_i_var40);
		_i_var511 = (_i_var499) * (_i_var13);
		_i_var512 = (_i_var500) * (_i_var40);
		_i_var513 = (_i_var501) * (_i_var13);
		_i_var514 = (_i_var502) * (_i_var40);
		_i_var515 = (_i_var503) * (_i_var13);
		_i_var516 = (_i_var504) * (_i_var32);
		_i_var517 = (_i_var505) * (_i_var0);
		_i_var518 = (_i_var506) * (_i_var32);
		_i_var519 = (_i_var507) * (_i_var0);
		_i_var520 = (_i_var508) * (_i_var6);
		_i_var521 = (_i_var509) * (_i_var36);
		_i_var522 = (_i_var511)+(_i_var510);
		_i_var523 = (_i_var513)+(_i_var512);
		_i_var524 = (_i_var515)+(_i_var514);
		_i_var525 = (_i_var517)+(_i_var516);
		_i_var526 = (_i_var519)+(_i_var518);
		_i_var527 = (_i_var521)+(_i_var520);
		hessian(0, 0) = _i_var522;
		hessian(1, 0) = _i_var523;
		hessian(2, 0) = _i_var524;
		hessian(0, 1) = _i_var523;
		hessian(1, 1) = _i_var525;
		hessian(2, 1) = _i_var526;
		hessian(0, 2) = _i_var524;
		hessian(1, 2) = _i_var526;
		hessian(2, 2) = _i_var527;
	}

}


Vec2 SolveHeadAngles(const Vec2& neckAngles, double rodDistance
	, double headSideDistance, double headBackwardDistance, double headHeight
	, double motorSideDistance, double motorBackwardDistance, double motorHeight)
{
	dVector x(2);

	dVector headConnetParameters(3);
	headConnetParameters << headSideDistance, headHeight, headBackwardDistance;

	dVector motorConnectParameters(3);
	motorConnectParameters << motorSideDistance, motorHeight, motorBackwardDistance;

	dVector tNeckAngles(2);
	tNeckAngles << neckAngles.x, neckAngles.y;


	ModifiedNewton solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::computeNeckMechanismEnergy(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector2d gradient;
		Codegen::computeNeckMechanismEnergyGradient(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix2d hessian;
		Codegen::computeNeckMechanismEnergyHessian(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, hessian);
		result = hessian.sparseView();
	});

	return Vec2(x(0), x(1));
}

Vec3 SolveLeftArmAngles(const Vec3& shoulderPos, const Vec3& angles, const Vec3& targetPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector target(3);
	target << targetPos.x, targetPos.y, targetPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = -Math::HalfPi * 0.1;
	ci0(1) = Math::HalfPi;
	ci0(2) = Math::HalfPi * 0.8;
	ci0(3) = Math::HalfPi;
	ci0(4) = -0.1;
	ci0(5) = Math::HalfPi;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouLeftArmEnergy(parameters, tx, target, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouLeftArmEnergyGradient(parameters, tx, target, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouLeftArmEnergyHessian(parameters, tx, target, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

Vec3 SolveRightArmAngles(const Vec3& shoulderPos, const Vec3& angles, const Vec3& targetPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector target(3);
	target << targetPos.x, targetPos.y, targetPos.z;

	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = Math::HalfPi;
	ci0(1) = -Math::HalfPi * 0.1;
	ci0(2) = Math::HalfPi;
	ci0(3) = Math::HalfPi * 0.8;
	ci0(4) = Math::HalfPi;
	ci0(5) = -0.1;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouRightArmEnergy(parameters, tx, target, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouRightArmEnergyGradient(parameters, tx, target, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouRightArmEnergyHessian(parameters, tx, target, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}


Vec3 SolveLeftArmAnglesFromTwoTagets(const Vec3& shoulderPos, const Vec3& angles, const Vec3& handPos, const Vec3& elbowPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector targetHand(3);
	targetHand << handPos.x, handPos.y, handPos.z;

	dVector targetElbow(3);
	targetElbow << elbowPos.x, elbowPos.y, elbowPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = -Math::HalfPi * 0.1;
	ci0(1) = Math::HalfPi;
	ci0(2) = Math::HalfPi * 0.8;
	ci0(3) = Math::HalfPi;
	ci0(4) = -0.1;
	ci0(5) = Math::HalfPi;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTarget(parameters, tx, targetHand, targetElbow, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTargetGradient(parameters, tx, targetHand, targetElbow, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTargetHessian(parameters, tx, targetHand, targetElbow, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

Vec3 SolveRightArmAnglesFromTwoTagets(const Vec3& shoulderPos, const Vec3& angles, const Vec3& handPos, const Vec3& elbowPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector targetHand(3);
	targetHand << handPos.x, handPos.y, handPos.z;

	dVector targetElbow(3);
	targetElbow << elbowPos.x, elbowPos.y, elbowPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = Math::HalfPi;
	ci0(1) = -Math::HalfPi * 0.1;
	ci0(2) = Math::HalfPi;
	ci0(3) = Math::HalfPi * 0.8;
	ci0(4) = Math::HalfPi;
	ci0(5) = -0.1;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTarget(parameters, tx, targetHand, targetElbow, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTargetGradient(parameters, tx, targetHand, targetElbow, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTargetHessian(parameters, tx, targetHand, targetElbow, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

void Draw3DGrid(const Mat4x4& cameraMatrix)
{
	for (auto i : Range(-5, 5))
	{
		Line3D(Vec3(-5, 0, i), Vec3(5, 0, i)).draw(cameraMatrix, ColorF(0.5));
		Line3D(Vec3(i, 0, -5), Vec3(i, 0, 5)).draw(cameraMatrix, ColorF(0.5));
	}
}

void Draw3DPoint(const Vec3& pos, const ColorF& col, const Mat4x4& cameraMatrix)
{
	AABB(pos, Vec3(1.0, 1.0, 1.0) * 0.1).draw(cameraMatrix, col);
}

Vec3 MultiplyMatAndVec3(const Mat4x4& mat, const Vec3& vec)
{
	constexpr size_t vertexCount = 1;
	Float3 out;
	SIMD_Float4 tVec(vec, 0.0f);
	SIMD::Vector3TransformCoordStream(&out, &tVec, vertexCount, mat);
	return Vec3(out);
}

void DrawFace(const Mat4x4& transformMat, const Mat4x4& mat)
{
	const Vec3 P0 = MultiplyMatAndVec3(transformMat, Vec3(0.0, 0.0, 0.0));
	const Vec3 P1 = MultiplyMatAndVec3(transformMat, Vec3(1.0, 1.0, -1.0));
	const Vec3 P2 = MultiplyMatAndVec3(transformMat, Vec3(-1.0, 1.0, -1.0));
	const Vec3 P3 = MultiplyMatAndVec3(transformMat, Vec3(-1.0, -1.0, -1.0));
	const Vec3 P4 = MultiplyMatAndVec3(transformMat, Vec3(1.0, -1.0, -1.0));

	Line3D(Vec3(0.0, 0.0, 0.0), P0).draw(mat, Palette::Lightgreen);

	Line3D(P0, P1).draw(mat, Palette::Lightgreen);
	Line3D(P0, P2).draw(mat, Palette::Lightgreen);
	Line3D(P0, P3).draw(mat, Palette::Lightgreen);
	Line3D(P0, P4).draw(mat, Palette::Lightgreen);

	Line3D(P1, P2).draw(mat, Palette::Lightgreen);
	Line3D(P2, P3).draw(mat, Palette::Lightgreen);
	Line3D(P3, P4).draw(mat, Palette::Lightgreen);
	Line3D(P4, P1).draw(mat, Palette::Lightgreen);
}

void DrawEye(double eyeblinkValue, const Mat4x4& transformMat, const Mat4x4& mat)
{
	const Vec3 P1 = MultiplyMatAndVec3(transformMat, Vec3(0.1, 0.5 * eyeblinkValue, 0.0));
	const Vec3 P2 = MultiplyMatAndVec3(transformMat, Vec3(-0.1, 0.5 * eyeblinkValue, 0.0));
	const Vec3 P3 = MultiplyMatAndVec3(transformMat, Vec3(-0.1, -0.5 * eyeblinkValue, 0.0));
	const Vec3 P4 = MultiplyMatAndVec3(transformMat, Vec3(0.1, -0.5 * eyeblinkValue, 0.0));

	Line3D(P1, P2).draw(mat, Palette::Lightgreen);
	Line3D(P2, P3).draw(mat, Palette::Lightgreen);
	Line3D(P3, P4).draw(mat, Palette::Lightgreen);
	Line3D(P4, P1).draw(mat, Palette::Lightgreen);
}

void DrawBody(CommonData& data, Mat4x4 hipTranslation, Mat4x4 chestTranslation, Mat4x4 headTranslation, double eyeBlinkValue, const Mat4x4& mat)
{
	DrawFace(hipTranslation, mat);
	DrawFace(chestTranslation, mat);
	DrawFace(headTranslation, mat);
	DrawEye(eyeBlinkValue, Mat4x4::Translation(0.3, 0.0, -1.0) * headTranslation, mat);
	DrawEye(eyeBlinkValue, Mat4x4::Translation(-0.3, 0.0, -1.0) * headTranslation, mat);
}

Vec3 RotateVec3FromAxisAndAngle(const Vec3& x, const Vec3& normalizedAxis, double angle)
{
	using Math::Cos;
	using Math::Sin;

	return Cos(angle) * x + Sin(angle) * (normalizedAxis.cross(x)) + (normalizedAxis.dot(x) * (1.0 - Cos(angle))) * normalizedAxis;
}


#include "CommonData.hpp"


Vec3 TranslateVec3ByMat4x4(const Vec3& value, const Mat4x4& translation)
{
	return SIMD::Vector3TransformCoord(SIMD_Float4(value, 0.0), translation).xyz();
}

void DrawArms(CommonData& data, const Mat4x4& chestTranslation, const Mat4x4& mat)
{
	const Vec3 leftArmAngles = GetValue<Vec3>(data, U"ValueProcessor.leftArmAngles");
	const Vec3 rightArmAngles = GetValue<Vec3>(data, U"ValueProcessor.rightArmAngles");
	const Vec3 leftShoulderArmPos = GetValue<Vec3>(data, U"ValueProcessor.leftShoulderArmPos");
	const Vec3 rightShoulderArmPos = GetValue<Vec3>(data, U"ValueProcessor.rightShoulderArmPos");

	const double length = GetValue<double>(data, U"ValueProcessor.armLength");

	{
		const Vec3 tArmPos = RotateVec3FromAxisAndAngle(Vec3(length, 0.0, 0.0), Vec3(0.0, 1.0, 0.0), leftArmAngles.z) + Vec3(length, 0.0, 0.0);
		const Vec3 ttArmPos = RotateVec3FromAxisAndAngle(tArmPos, Vec3(0.0, 0.0, 1.0), leftArmAngles.y);
		const Vec3 armPos = RotateVec3FromAxisAndAngle(ttArmPos, Vec3(1.0, 0.0, 0.0), leftArmAngles.x) + leftShoulderArmPos;

		const Vec3 ttElbowPos = RotateVec3FromAxisAndAngle(Vec3(length, 0.0, 0.0), Vec3(0.0, 0.0, 1.0), leftArmAngles.y);
		const Vec3 elbowPos = RotateVec3FromAxisAndAngle(ttElbowPos, Vec3(1.0, 0.0, 0.0), leftArmAngles.x) + leftShoulderArmPos;

		Line3D(TranslateVec3ByMat4x4(leftShoulderArmPos, chestTranslation), TranslateVec3ByMat4x4(elbowPos, chestTranslation)).draw(mat, Palette::Yellow);
		Line3D(TranslateVec3ByMat4x4(elbowPos, chestTranslation), TranslateVec3ByMat4x4(armPos, chestTranslation)).draw(mat, Palette::Yellow);
	}

	{
		const Vec3 tArmPos = RotateVec3FromAxisAndAngle(Vec3(-length, 0.0, 0.0), Vec3(0.0, 1.0, 0.0), rightArmAngles.z) + Vec3(-length, 0.0, 0.0);
		const Vec3 ttArmPos = RotateVec3FromAxisAndAngle(tArmPos, Vec3(0.0, 0.0, 1.0), rightArmAngles.y);
		const Vec3 armPos = RotateVec3FromAxisAndAngle(ttArmPos, Vec3(-1.0, 0.0, 0.0), rightArmAngles.x) + rightShoulderArmPos;

		const Vec3 ttElbowPos = RotateVec3FromAxisAndAngle(Vec3(-length, 0.0, 0.0), Vec3(0.0, 0.0, 1.0), rightArmAngles.y);
		const Vec3 elbowPos = RotateVec3FromAxisAndAngle(ttElbowPos, Vec3(-1.0, 0.0, 0.0), rightArmAngles.x) + rightShoulderArmPos;

		Line3D(TranslateVec3ByMat4x4(rightShoulderArmPos, chestTranslation), TranslateVec3ByMat4x4(elbowPos, chestTranslation)).draw(mat, Palette::Yellow);
		Line3D(TranslateVec3ByMat4x4(elbowPos, chestTranslation), TranslateVec3ByMat4x4(armPos, chestTranslation)).draw(mat, Palette::Yellow);
	}
}

void UpdateRobotData(CommonData& data)
{
	const double length = GetValue<double>(data, U"ValueProcessor.armLength");

	const auto leftArmAngles = GetValue<Vec3>(data, U"ValueProcessor.leftArmAngles");
	const auto rightArmAngles = GetValue<Vec3>(data, U"ValueProcessor.rightArmAngles");
	const auto targetLeftArmPos = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"leftArm"].source);
	const auto targetRightArmPos = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"rightArm"].source);

	data.valueTable[U"ValueProcessor.leftArmAngles"] = SolveLeftArmAngles(GetValue<Vec3>(data, U"ValueProcessor.leftShoulderArmPos")
		, leftArmAngles
		, targetLeftArmPos
		, length, length);
	data.valueTable[U"ValueProcessor.rightArmAngles"] = SolveRightArmAngles(GetValue<Vec3>(data, U"ValueProcessor.rightShoulderArmPos")
		, rightArmAngles
		, targetRightArmPos
		, length, length);
}

void TorqueEnableMotors(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		for (auto& name : connection.names)
		{
			auto& motorInfo = data.motorData.motorInfos[name];

			const double value = GetValue<double>(data, motorInfo.source);

			ham::Request request;

			if (motorInfo.type == U"Dynamixel.XL320")
			{
				request = ham::XL320_TorqueEnable(motorInfo.id);
			}
			else if (motorInfo.type == U"Dynamixel.XM430")
			{
				request = ham::XM430W350R_TorqueEnable(motorInfo.id);
			}
			else
			{
				Error();
			}

			connection.dyna->setWriteRequest(request, 1);
		}

		connection.dyna->sendData();
	}
}

void TorqueDisableMotors(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		if (connection.dyna->isConnecting())
		{
			for (auto& name : connection.names)
			{
				auto& motorInfo = data.motorData.motorInfos[name];

				const double value = GetValue<double>(data, motorInfo.source);

				ham::Request request;

				if (motorInfo.type == U"Dynamixel.XL320")
				{
					request = ham::XL320_TorqueEnable(motorInfo.id);
				}
				else if (motorInfo.type == U"Dynamixel.XM430")
				{
					request = ham::XM430W350R_TorqueEnable(motorInfo.id);
				}
				else
				{
					Error();
				}

				connection.dyna->setWriteRequest(request, 0);
			}

			connection.dyna->sendData();
		}
	}
}

double GetXL320ValueFromRad(double radian)
{
	const double digree = -(radian * 180.0 / Math::Pi);
	return Clamp(digree / 150.0, -1.0, 1.0) * 512 + 512;
}

double GetXM430ValueFromRad(double radian)
{
	return (-radian + Math::Pi) * 4096 / Math::TwoPi;
}

void SendMotorInfo(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		if (connection.dyna->isConnecting())
		{
			if (connection.state == MotorConnectInformation::State::Sending)
			{
				for (auto& name : connection.names)
				{
					auto& motorInfo = data.motorData.motorInfos[name];

					const double value = GetValue<double>(data, motorInfo.source);

					ham::Request request;

					if (motorInfo.type == U"Dynamixel.XL320")
					{
						request = ham::XL320_GoalPosition(motorInfo.id);
						connection.dyna->setWriteRequest(request, GetXL320ValueFromRad(value));
					}
					else if (motorInfo.type == U"Dynamixel.XM430")
					{
						request = ham::XM430W350R_GoalPosition(motorInfo.id);
						connection.dyna->setWriteRequest(request, GetXM430ValueFromRad(value));
					}
					else
					{
						Error();
					}
				}

				connection.dyna->sendData();
			}
		}
	}
}

void DrawRobotData(CommonData& data)
{

	//3D Camera
	auto eyePositon = Cylindrical(20, GetValue<double>(data, U"ValueProcessor.cameraAngle"), 4);
	data.camera.setView(eyePositon, GetValue<Vec3>(data, U"ValueProcessor.focusPosition"));
	const Mat4x4 mat = data.camera.getMat4x4();

	//Draw 3D Objects
	{
		ScopedRenderStates2D culling(RasterizerState::SolidCullBack);

		Draw3DGrid(mat);



		//Draw3DPoint(GetValue<Vec3>(data, U"Robot.subTargetPosLeft"), Palette::Yellow, mat);
		//Draw3DPoint(GetValue<Vec3>(data, U"Robot.subTargetPosRight"), Palette::Yellow, mat);

		auto chestRotation = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"chest"].source);
		auto headRotation = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"head"].source);
		const Mat4x4 hipRot = Mat4x4::Identity();
		const Mat4x4 chestRot = Mat4x4::RotationX(chestRotation.x) * Mat4x4::RotationZ(chestRotation.z) * Mat4x4::RotationY(chestRotation.y);
		const Mat4x4 headRot = Mat4x4::RotationX(headRotation.x) * Mat4x4::RotationZ(headRotation.z) * Mat4x4::RotationY(headRotation.y);
		double eyeBlinkValue = 1.0;

		const Vec3 pos = Vec3(0.0, 0.0, 0.0);
		const auto hipTranslation = hipRot * Mat4x4::Translation(pos.x, pos.y, pos.z);
		const auto chestTranslation = chestRot * Mat4x4::Translation(0.0, 2.0, 0.0) * hipTranslation;
		const auto headTranslation = headRot * Mat4x4::Translation(0.0, 5.0, 0.0) * chestTranslation;

		DrawBody(data, hipTranslation, chestTranslation, headTranslation, eyeBlinkValue, mat);
		DrawArms(data, chestTranslation, mat);

		const auto leftTarget = TranslateVec3ByMat4x4(GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"leftArm"].source), chestTranslation);
		const auto rightTarget = TranslateVec3ByMat4x4(GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"rightArm"].source), chestTranslation);
		Draw3DPoint(leftTarget, Palette::Red, mat);
		Draw3DPoint(rightTarget, Palette::Red, mat);
	}
}
#pragma once
#include <iostream>
# include <Eigen/Eigen>

#include "EigQuadProg.hpp"


using V3D = Eigen::Vector3d;
using V2D = Eigen::Vector2d;
using Triplet = Eigen::Triplet<double>;
using TripVec = std::vector<Eigen::Triplet<double>>;
using SpMat = Eigen::SparseMatrix<double>;

class dVector : public Eigen::VectorXd
{
public:

	dVector()
		: Eigen::VectorXd()
	{
		setZero();
	}

	dVector(int n)
		: Eigen::VectorXd(n)
	{
		setZero();
	}

	dVector(const Eigen::VectorXd& v)
		: Eigen::VectorXd(v)
	{

	}

};

class SolverBase
{
protected:

	Eigen::IOFormat HeavyFmt = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
	size_t m_maxIteratinos = 50;
	size_t m_maxLinesearchIterations = 15;
	double m_residual = 1e-8;
	bool m_debugPrint = false;

public:

	SolverBase()
	{

	}

	virtual size_t step(
		dVector& parameter,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const std::vector<size_t>& fixedIndices = {}) = 0;

	virtual ~SolverBase()
	{

	}

	void setMaxIterations(size_t maxIterations)
	{
		m_maxIteratinos = maxIterations;
	}

	size_t getMaxIterations() const
	{
		return m_maxIteratinos;
	}

	void setMaxLinesearchIterations(size_t maxLinesearchIterations)
	{
		m_maxLinesearchIterations = maxLinesearchIterations;
	}

	size_t getLinesearchIterations() const
	{
		return m_maxLinesearchIterations;
	}

	void setResidual(double residual)
	{
		m_residual = residual;
	}

	double getResidual() const
	{
		return m_residual;
	}

	void setDebugPrint(bool debugPrint)
	{
		m_debugPrint = debugPrint;
	}

	bool getDebugPrint() const
	{
		return m_debugPrint;
	}

protected:

	void filter(dVector& v, const std::vector<size_t>& fixedIndices)
	{
		for (int i = 0; i < fixedIndices.size(); ++i)
		{
			const int index = fixedIndices[i];

			v(index) = 0.0;
		}
	}

	void filterMat(SpMat& A, dVector& b, const std::vector<size_t>& fixedIndices, double diagValue = 1.0)
	{
		for (int i = 0; i < fixedIndices.size(); ++i)
		{
			const int index = fixedIndices[i];

			for (SpMat::InnerIterator it(A, index); it; ++it)
			{
				const int ii = static_cast<int>(it.row());
				const int jj = static_cast<int>(it.col());

				if (ii == jj)
					A.coeffRef(ii, jj) = diagValue;
				else
				{
					A.coeffRef(ii, jj) = 0.0;
					A.coeffRef(jj, ii) = 0.0;
				}
			}
		}
	}

};


class ModifiedNewton : public SolverBase
{
public:

	ModifiedNewton()
		: SolverBase()
	{

	}

	virtual ~ModifiedNewton()
	{

	}

	size_t step(
		dVector& x,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const std::vector<size_t>& fixedIndices = {}) override
	{
		const int dim = static_cast<int>(x.size());
		size_t iteration = 0;
		dVector tx = x;
		dVector dx = dVector(dim);
		dVector grad = dVector(dim);
		SpMat hessian(dim, dim);

		for (; iteration < m_maxIteratinos; ++iteration)
		{
			//Get gradient
			getGrad(tx, grad);

			//filter gradient
			filter(grad, fixedIndices);

			const double gradNorm = grad.norm();

			if (m_debugPrint)
			{
				std::cout << "ITERATION: ";
				std::cout << iteration << std::endl;
				std::cout << "\tVALUE: ";
				std::cout << getValue(tx);
				std::cout << "\tX: ";
				std::cout << tx.transpose().format(HeavyFmt);
				std::cout << "\tGRADIENT: ";
				std::cout << grad.format(HeavyFmt) << std::endl;
				std::cout << "\tGRADIENT NORM: ";
				std::cout << gradNorm << std::endl;
			}

			//Decide escape
			if (std::isnan(gradNorm) || gradNorm < m_residual)
			{
				break;
			}

			//Get Hessian
			getHessian(tx, hessian);

			//filter hessian
			filterMat(hessian, tx, fixedIndices);

			//Get Dx with regularization
			getDxWithReguralizeHessian(dx, grad, hessian);

			//filterDx
			filter(dx, fixedIndices);

			//Determine alpha (indirectly) using line search
			lineSearch(dx, tx, getValue);
		}


		x = tx;


		if (m_debugPrint)
		{
			std::cout << "ITERATION" << std::endl;
			std::cout << iteration << std::endl;

			std::cout << "GRADIENT" << std::endl;
			std::cout << grad.format(HeavyFmt) << std::endl;

			std::cout << "HESSIAN" << std::endl;
			std::cout << hessian.toDense().format(HeavyFmt) << std::endl;

			std::cout << "DX" << std::endl;
			std::cout << dx.format(HeavyFmt) << std::endl;

			std::cout << "X" << std::endl;
			std::cout << x.format(HeavyFmt) << std::endl;

			std::cout << "VALUE" << std::endl;
			std::cout << getValue(x) << std::endl;
		}

		return iteration;
	}


protected:

	void getDxWithReguralizeHessian(dVector& dx, dVector& grad, SpMat& hessian)
	{
		bool solved = false;
		Eigen::VectorXd dmydv;

		for (int j = 0;; ++j)
		{
			const Eigen::SimplicialCholesky<SpMat, Eigen::Upper> chol(hessian);
			dmydv = chol.solve(-grad);

			solved = true;

			//check factorization
			if (chol.info() != Eigen::Success)
			{
				solved = false;
			}

			//check redsiduum
			const Eigen::VectorXd r = hessian * dmydv + grad;

			if (r.norm() > 10e-8)
			{
				solved = false;
			}

			//check dot product between gradient and search direction
			const double dot_lin = dmydv.dot(grad);

			if (dot_lin > 0.0)
			{
				solved = false;
			}

			if (solved)
			{
				break;
			}

			{
				const double reg = (1e-4) * pow(3, j);
				if (reg > 1e10)
				{
					std::cout << " regularization is way too high: reg = " << reg << std::endl;
				}
				for (int k = 0; k < hessian.rows(); ++k)
				{
					hessian.coeffRef(k, k) += reg;

					if (m_debugPrint)
					{
						std::cout << "\tREGULARIZE: reg = " << reg << std::endl;
					}
				}
			}
		}

		dx = dmydv;
	}

	void lineSearch(dVector& dx, dVector& x, std::function<double(dVector&)> getValue)
	{
		const double beta = 0.5;
		const double currentValue = getValue(x);
		double nextValue = currentValue;

		if (m_debugPrint)
		{
			std::cout << "\tLINE SEARCH" << std::endl;
		}

		for (int j = 0; ; ++j)
		{
			dVector nextX = x;

			nextX += dx;

			nextValue = getValue(nextX);

			if (m_debugPrint)
			{
				std::cout << "\tNEXT X CANDIDATE: " << nextX.transpose().format(HeavyFmt) << "\tVALUE: " << nextValue << std::endl;
			}

			if ((nextValue < currentValue) || (j == m_maxLinesearchIterations))
			{
				x = nextX;
				break;
			}

			dx *= beta;
		}
	}

};


class SQPSolver : public ModifiedNewton
{
public:

	SQPSolver()
		: ModifiedNewton()
	{

	}

	virtual ~SQPSolver()
	{

	}

	size_t step(
		dVector& x,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const Eigen::MatrixXd& CE, const dVector& ce0,
		const Eigen::MatrixXd& CI, const dVector& ci0,
		const std::vector<size_t>& fixedIndices = {})
	{
		const int dim = static_cast<int>(x.size());
		size_t iteration = 0;
		dVector tx = x;
		dVector dx = dVector(dim);
		dVector ttx = dVector(dim);
		dVector grad = dVector(dim);
		SpMat hessian(dim, dim);

		for (; iteration < m_maxIteratinos; ++iteration)
		{
			ttx = tx;

			//Get gradient
			getGrad(tx, grad);

			//filter gradient
			filter(grad, fixedIndices);

			const double gradNorm = grad.norm();

			if (m_debugPrint)
			{
				std::cout << "ITERATION: ";
				std::cout << iteration << std::endl;
				std::cout << "\tVALUE: ";
				std::cout << getValue(ttx);
				std::cout << "\tX: ";
				std::cout << ttx.transpose().format(HeavyFmt);
				std::cout << "\tGRADIENT NORM: ";
				std::cout << gradNorm << std::endl;
			}

			//Decide escape
			if (std::isnan(gradNorm) || gradNorm < m_residual)
			{
				x = tx;

				break;
			}

			//Get Hessian
			getHessian(tx, hessian);

			//filter hessian
			filterMat(hessian, tx, fixedIndices);

			//Get Dx with regularization
			getDxWithReguralizeHessian(dx, grad, hessian);

			//SolveQP
			const double value = solveQP(ttx, grad, hessian, CE, ce0, CI, ci0);

			dx = dVector(ttx - tx);

			//filterDx
			//filter(dx, fixedIndices);

			//tParameter += dx;
			//Determine alpha (indirectly) using line search
			lineSearch(dx, tx, getValue);
		}

		x = tx;

		if (m_debugPrint)
		{
			std::cout << "ITERATION" << std::endl;
			std::cout << iteration << std::endl;

			std::cout << "GRADIENT" << std::endl;
			std::cout << grad.format(HeavyFmt) << std::endl;

			std::cout << "HESSIAN" << std::endl;
			std::cout << hessian.toDense().format(HeavyFmt) << std::endl;

			std::cout << "DX" << std::endl;
			std::cout << dx.format(HeavyFmt) << std::endl;

			std::cout << "X" << std::endl;
			std::cout << x.format(HeavyFmt) << std::endl;

			std::cout << "VALUE" << std::endl;
			std::cout << getValue(x) << std::endl;
		}

		return iteration;
	}

private:

	inline double solveQP(dVector& x, const dVector& grad, const SpMat& hessian,
		const Eigen::MatrixXd& CE, const dVector& ce0,
		const Eigen::MatrixXd& CI, const dVector& ci0)
	{
		Eigen::MatrixXd G = hessian.toDense();
		Eigen::VectorXd g0 = grad - G * x;

		return solveEigenQuadProg(G, g0, CE, ce0, CI, ci0, x);
	}

};
#pragma once

#include <Siv3D.hpp>
#include "dynamixel_sdk/DynamixelManager.hpp"


template<typename T>
Array<T> GetArray(const TOMLArrayView& arrayView)
{
	Array<T> values;

	for (const auto& object : arrayView)
	{
		values << object.get<T>();
	}

	return values;
}

HashTable<String, Font> GetFonts(const TOMLTableArrayView& arrayView)
{
	HashTable<String, Font> fontTable;
	{
		for (const auto& object : arrayView)
		{
			fontTable[object[U"label"].getString()] = Font(object[U"size"].get<int32>(),
				static_cast<Typeface>(object[U"typeface"].get<int32>()));
		}
	}

	return fontTable;
}

HashTable<String, Texture> GetIconTextures(const TOMLTableArrayView& arrayView)
{
	HashTable<String, Texture> textureTable;
	{
		for (const auto& object : arrayView)
		{
			textureTable[object[U"label"].getString()] = Texture(Icon(object[U"code"].get<uint32>(), object[U"size"].get<int32>()));
		}
	}

	return textureTable;
}

class Button
{
public:

	enum class State
	{
		LeftClicked,
		RightClicked
	};

	Button()
	{

	}

	Button(const String& textureName, const Vec2& pos)
		: m_textureName(textureName)
		, m_pos(pos)
	{

	}

	void draw(const HashTable<String, Texture>& textures
		, const ColorF& color, const ColorF& mouseOverColor, const ColorF& activeColor) const
	{
		ColorF col;

		if (m_isActive)
		{
			col = activeColor;
		}
		else
		{
			col = isMouseOver() ? mouseOverColor : color;
		}

		textures.at(m_textureName).drawAt(m_pos, col);
	}

	Optional<State> update()
	{
		Optional<State> result;

		auto region = getMouseRegion();

		if (region.leftClicked())
		{
			result = State::LeftClicked;
		}
		if (region.rightClicked())
		{
			result = State::RightClicked;
		}

		return result;
	}

	void setActive(bool isActive)
	{
		m_isActive = isActive;
	}

	bool isActive() const
	{
		return m_isActive;
	}

	bool isMouseOver() const
	{
		return getMouseRegion().mouseOver();
	}

	Circle getMouseRegion() const
	{
		return Circle(m_pos, m_radius);
	}

private:

	double m_radius = 30.0;
	bool m_isActive = false;
	String m_textureName;
	Vec2 m_pos;

};

struct MotorInfo
{
	String name;
	size_t id;
	String type;
	String source;
};

struct MotorDisplay
{
	String name;
	Vec2 pos;
};

struct MotorConnectInformation
{
	enum class State
	{
		Stop,
		Sending
	};

	std::shared_ptr<Dynamixel> dyna;
	Array<String> names;
	State state = State::Stop;
};

struct MotorData
{
	HashTable<String, MotorInfo> motorInfos;
	HashTable<String, MotorDisplay> motorDisplays;
	HashTable<String, MotorConnectInformation> comTable;
};

struct BodyPartInfo
{
	String name;
	String bindingType;
	String source;
};

struct BodyPartDisplay
{
	String name;
	Vec2 pos;
};

struct BodyData
{
	HashTable<String, BodyPartInfo> bodyPartInfos;
	HashTable<String, BodyPartDisplay> bodyPartDisplays;
};

struct ValueProcessInfo
{

	String name;
	String type;
	String to;
	HashTable<String, String> soureTable;
};

struct ValueProcessData
{
	HashTable<String, ValueProcessInfo> valueProcessInfos;
	Array<String> valueProcessSequence;
};

struct CommonData
{
	using Value = std::variant<bool, size_t, int32, double, Vec2, Vec3, Vec4, Size, String, Color>;

	struct ValuePrintVisitor
	{
		String operator()(const bool& value)
		{
			return Format(value);
		}
		String operator()(const size_t& value)
		{
			return Format(value);
		}
		String operator()(const int32& value)
		{
			return Format(value);
		}
		String operator()(const double& value)
		{
			return Format(value);
		}
		String operator()(const Vec2& value)
		{
			return Format(value);
		}
		String operator()(const Vec3& value)
		{
			return Format(value);
		}
		String operator()(const Vec4& value)
		{
			return Format(value);
		}
		String operator()(const Size& value)
		{
			return Format(value);
		}
		String operator()(const String& value)
		{
			return Format(value);
		}
		String operator()(const Color& value)
		{
			return Format(value);
		}
	};

	struct ValueTypePrintVisitor
	{
		String operator()(const bool&)
		{
			return U"TYPE: bool";
		}
		String operator()(const size_t&)
		{
			return U"TYPE: size_t";
		}
		String operator()(const int32&)
		{
			return U"TYPE: int32";
		}
		String operator()(const double&)
		{
			return U"TYPE: double";
		}
		String operator()(const Vec2&)
		{
			return U"TYPE: Vec2";
		}
		String operator()(const Vec3&)
		{
			return U"TYPE: Vec3";
		}
		String operator()(const Vec4&)
		{
			return U"TYPE: Vec4";
		}
		String operator()(const Size&)
		{
			return U"TYPE: Size";
		}
		String operator()(const String&)
		{
			return U"TYPE: String";
		}
		String operator()(const Color&)
		{
			return U"TYPE: Color";
		}
	};

	void printAllDataToConsole()
	{
		for (const auto& value : valueTable)
		{
			String text = U"KEY: " + value.first;
			text += U" " + std::visit(ValuePrintVisitor{}, value.second);

			Console << text;
		}
	}

	void printAllDataToWindnow()
	{
		for (const auto& value : valueTable)
		{
			String text = U"KEY: " + value.first;
			text += U" " + std::visit(ValuePrintVisitor{}, value.second);

			Print << text;
		}
	}


	HashTable<String, Value> valueTable;
	HashTable<String, Font> fontTable;
	HashTable<String, Texture> textureTable;
	HashTable<String, Button> buttonTable;
	HashTable<String, Array<String>> buttonGroupTable;
	HashTable <String, std::function<void(CommonData&)>> channelFunctionTable;

	MotorData motorData;
	BodyData bodyData;
	ValueProcessData valueProcessData;

	experimental::BasicCamera3D camera{};


	RenderTexture gaussianA1, gaussianB1;
	RenderTexture gaussianA4, gaussianB4;
	//RenderTexture gaussianA8, gaussianB8;

	double a8 = 1.0;
	double a4 = 1.0;
	double a1 = 1.0;
};


template<class T>
T GetValue(CommonData& data, const String& name)
{
	try
	{
		auto& value = data.valueTable[name];
		return std::get<T>(value);
	}
	catch (const ParseError & e)
	{
		// ��O�̏ڍׂ��擾���ĕ\��
		Print << e.what();
	}
}

String GetValueToString(CommonData& data, const String& name)
{
	try
	{
		return std::visit(CommonData::ValuePrintVisitor{}, data.valueTable[name]);
	}
	catch (const ParseError & e)
	{
		// ��O�̏ڍׂ��擾���ĕ\��
		Print << e.what();
	}
}
#include "EigQuadProg.hpp"

#include <vector>

using namespace Eigen;

template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
	using std::abs;
	using std::sqrt;
	Scalar a1, b1, t;
	a1 = abs(a);
	b1 = abs(b);
	if (a1 > b1)
	{
		t = (b1 / a1);
		return a1 * sqrt(1.0 + t * t);
	}
	else
		if (b1 > a1)
		{
			t = (a1 / b1);
			return b1 * sqrt(1.0 + t * t);
		}
	return a1 * sqrt(2.0);
}

// }

inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np)
{
	d = J.adjoint() * np;
}

inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq)
{
	z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
}

inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
{
	r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Upper>().solve(d.head(iq));
}

bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int p, int& iq, int l);

double solveEigenQuadProg(MatrixXd& G, VectorXd& g0,
	const MatrixXd& CE, const VectorXd& ce0,
	const MatrixXd& CI, const VectorXd& ci0,
	VectorXd& x)
{
	using std::abs;
	int i, j, k, l; /* indices */
	int ip, me, mi;
	int n = g0.size();
	int p = ce0.size();
	int m = ci0.size();
	MatrixXd R(G.rows(), G.cols()), J(G.rows(), G.cols());

	LLT<MatrixXd, Lower> chol(G.cols());

	VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
	VectorXd x_old(n), u_old(m + p);
	double f_value, psi, c1, c2, sum, ss, R_norm;
	const double inf = std::numeric_limits<double>::infinity();
	double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
	  * and the full step length t2 */
	VectorXi A(m + p), A_old(m + p), iai(m + p);
	int q;
	int iq, iter = 0;
	std::vector<bool> iaexcl(m + p);

	me = p; /* number of equality constraints */
	mi = m; /* number of inequality constraints */
	q = 0;  /* size of the active set A (containing the indices of the active constraints) */

	/*
	 * Preprocessing phase
	 */

	 /* compute the trace of the original matrix G */
	c1 = G.trace();

	/* decompose the matrix G in the form LL^T */
	chol.compute(G);

	/* initialize the matrix R */
	d.setZero();
	R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */

	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
	J.setIdentity();
	J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
	print_matrix("J", J, n);
#endif

	/* c1 * c2 is an estimate for cond(G) */

	/*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
	x = chol.solve(g0);
	x = -x;
	/* and compute the current solution value */
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
	std::cerr << "Unconstrained solution: " << f_value << std::endl;
	print_vector("x", x, n);
#endif

	/* Add equality constraints to the working set A */
	iq = 0;
	for (i = 0; i < me; i++)
	{
		np = CE.col(i);
		compute_d(d, J, np);
		update_z(z, J, d, iq);
		update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
		print_matrix("R", R, iq);
		print_vector("z", z, n);
		print_vector("r", r, iq);
		print_vector("d", d, n);
#endif

		/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
		  becomes feasible */
		t2 = 0.0;
		if (abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
			t2 = (-np.dot(x) - ce0(i)) / z.dot(np);

		x += t2 * z;

		/* set u = u+ */
		u(iq) = t2;
		u.head(iq) -= t2 * r.head(iq);

		/* compute the new solution value */
		f_value += 0.5 * (t2 * t2) * z.dot(np);
		A(i) = -i - 1;

		if (!add_constraint(R, J, d, iq, R_norm))
		{
			// FIXME: it should raise an error
			// Equality constraints are linearly dependent
			return f_value;
		}
	}

	/* set iai = K \ A */
	for (i = 0; i < mi; i++)
		iai(i) = i;

l1:	iter++;
#ifdef TRACE_SOLVER
	print_vector("x", x, n);
#endif
	/* step 1: choose a violated constraint */
	for (i = me; i < iq; i++)
	{
		ip = A(i);
		iai(ip) = -1;
	}

	/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
	ss = 0.0;
	psi = 0.0; /* this value will contain the sum of all infeasibilities */
	ip = 0; /* ip will be the index of the chosen violated constraint */
	for (i = 0; i < mi; i++)
	{
		iaexcl[i] = true;
		sum = CI.col(i).dot(x) + ci0(i);
		s(i) = sum;
		psi += std::min(0.0, sum);
	}
#ifdef TRACE_SOLVER
	print_vector("s", s, mi);
#endif


	if (abs(psi) <= mi * std::numeric_limits<double>::epsilon()* c1* c2 * 100.0)
	{
		/* numerically there are not infeasibilities anymore */
		q = iq;
		return f_value;
	}

	/* save old values for u, x and A */
	u_old.head(iq) = u.head(iq);
	A_old.head(iq) = A.head(iq);
	x_old = x;

l2: /* Step 2: check for feasibility and determine a new S-pair */
	for (i = 0; i < mi; i++)
	{
		if (s(i) < ss && iai(i) != -1 && iaexcl[i])
		{
			ss = s(i);
			ip = i;
		}
	}
	if (ss >= 0.0)
	{
		q = iq;
		return f_value;
	}

	/* set np = n(ip) */
	np = CI.col(ip);
	/* set u = (u 0)^T */
	u(iq) = 0.0;
	/* add ip to the active set A */
	A(iq) = ip;

#ifdef TRACE_SOLVER
	std::cerr << "Trying with constraint " << ip << std::endl;
	print_vector("np", np, n);
#endif

l2a:/* Step 2a: determine step direction */
  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
	compute_d(d, J, np);
	update_z(z, J, d, iq);
	/* compute N* np (if q > 0): the negative of the step direction in the dual space */
	update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
	std::cerr << "Step direction z" << std::endl;
	print_vector("z", z, n);
	print_vector("r", r, iq + 1);
	print_vector("u", u, iq + 1);
	print_vector("d", d, n);
	print_ivector("A", A, iq + 1);
#endif

	/* Step 2b: compute step length */
	l = 0;
	/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
	t1 = inf; /* +inf */
	/* find the index l s.t. it reaches the minimum of u+(x) / r */
	for (k = me; k < iq; k++)
	{
		double tmp;
		if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1))
		{
			t1 = tmp;
			l = A(k);
		}
	}
	/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
	if (abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
		t2 = -s(ip) / z.dot(np);
	else
		t2 = inf; /* +inf */

	  /* the step is chosen as the minimum of t1 and t2 */
	t = std::min(t1, t2);
#ifdef TRACE_SOLVER
	std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif

	/* Step 2c: determine new S-pair and take step: */

	/* case (i): no step in primal or dual space */
	if (t >= inf)
	{
		/* QPP is infeasible */
		// FIXME: unbounded to raise
		q = iq;
		return inf;
	}
	/* case (ii): step in dual space */
	if (t2 >= inf)
	{
		/* set u = u +  t * [-r 1) and drop constraint l from the active set A */
		u.head(iq) -= t * r.head(iq);
		u(iq) += t;
		iai(l) = l;
		delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
		std::cerr << " in dual space: "
			<< f_value << std::endl;
		print_vector("x", x, n);
		print_vector("z", z, n);
		print_ivector("A", A, iq + 1);
#endif
		goto l2a;
	}

	/* case (iii): step in primal and dual space */

	x += t * z;
	/* update the solution value */
	f_value += t * z.dot(np) * (0.5 * t + u(iq));

	u.head(iq) -= t * r.head(iq);
	u(iq) += t;
#ifdef TRACE_SOLVER
	std::cerr << " in both spaces: "
		<< f_value << std::endl;
	print_vector("x", x, n);
	print_vector("u", u, iq + 1);
	print_vector("r", r, iq + 1);
	print_ivector("A", A, iq + 1);
#endif

	if (t == t2)
	{
#ifdef TRACE_SOLVER
		std::cerr << "Full step has taken " << t << std::endl;
		print_vector("x", x, n);
#endif
		/* full step has taken */
		/* add constraint ip to the active set*/
		if (!add_constraint(R, J, d, iq, R_norm))
		{
			iaexcl[ip] = false;
			delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
			print_matrix("R", R, n);
			print_ivector("A", A, iq);
#endif
			for (i = 0; i < m; i++)
				iai(i) = i;
			for (i = 0; i < iq; i++)
			{
				A(i) = A_old(i);
				iai(A(i)) = -1;
				u(i) = u_old(i);
			}
			x = x_old;
			goto l2; /* go to step 2 */
		}
		else
			iai(ip) = -1;
#ifdef TRACE_SOLVER
		print_matrix("R", R, n);
		print_ivector("A", A, iq);
#endif
		goto l1;
	}

	/* a patial step has taken */
#ifdef TRACE_SOLVER
	std::cerr << "Partial step has taken " << t << std::endl;
	print_vector("x", x, n);
#endif
	/* drop constraint l */
	iai(l) = l;
	delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
	print_matrix("R", R, n);
	print_ivector("A", A, iq);
#endif

	s(ip) = CI.col(ip).dot(x) + ci0(ip);

#ifdef TRACE_SOLVER
	print_vector("s", s, mi);
#endif
	goto l2a;
}


inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm)
{
	using std::abs;
	int n = J.rows();
#ifdef TRACE_SOLVER
	std::cerr << "Add constraint " << iq << '/';
#endif
	int i, j, k;
	double cc, ss, h, t1, t2, xny;

	/* we have to find the Givens rotation which will reduce the element
		  d(j) to zero.
		  if it is already zero we don't have to do anything, except of
		  decreasing j */
	for (j = n - 1; j >= iq + 1; j--)
	{
		/* The Givens rotation is done with the matrix (cc cs, cs -cc).
				 If cc is one, then element (j) of d is zero compared with element
				 (j - 1). Hence we don't have to do anything.
				 If cc is zero, then we just have to switch column (j) and column (j - 1)
				 of J. Since we only switch columns in J, we have to be careful how we
				 update d depending on the sign of gs.
				 Otherwise we have to apply the Givens rotation to these columns.
				 The i - 1 element of d has to be updated to h. */
		cc = d(j - 1);
		ss = d(j);
		h = distance(cc, ss);
		if (h == 0.0)
			continue;
		d(j) = 0.0;
		ss = ss / h;
		cc = cc / h;
		if (cc < 0.0)
		{
			cc = -cc;
			ss = -ss;
			d(j - 1) = -h;
		}
		else
			d(j - 1) = h;
		xny = ss / (1.0 + cc);
		for (k = 0; k < n; k++)
		{
			t1 = J(k, j - 1);
			t2 = J(k, j);
			J(k, j - 1) = t1 * cc + t2 * ss;
			J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
		}
	}
	/* update the number of constraints added*/
	iq++;
	/* To update R we have to put the iq components of the d vector
	  into column iq - 1 of R
	  */
	R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
	std::cerr << iq << std::endl;
#endif

	if (abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon()* R_norm)
		// problem degenerate
		return false;
	R_norm = std::max<double>(R_norm, abs(d(iq - 1)));
	return true;
}


inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int p, int& iq, int l)
{

	int n = R.rows();
#ifdef TRACE_SOLVER
	std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
	int i, j, k, qq;
	double cc, ss, h, xny, t1, t2;

	/* Find the index qq for active constraint l to be removed */
	for (i = p; i < iq; i++)
		if (A(i) == l)
		{
			qq = i;
			break;
		}

	/* remove the constraint from the active set and the duals */
	for (i = qq; i < iq - 1; i++)
	{
		A(i) = A(i + 1);
		u(i) = u(i + 1);
		R.col(i) = R.col(i + 1);
	}

	A(iq - 1) = A(iq);
	u(iq - 1) = u(iq);
	A(iq) = 0;
	u(iq) = 0.0;
	for (j = 0; j < iq; j++)
		R(j, iq - 1) = 0.0;
	/* constraint has been fully removed */
	iq--;
#ifdef TRACE_SOLVER
	std::cerr << '/' << iq << std::endl;
#endif 

	if (iq == 0)
		return;

	for (j = qq; j < iq; j++)
	{
		cc = R(j, j);
		ss = R(j + 1, j);
		h = distance(cc, ss);
		if (h == 0.0)
			continue;
		cc = cc / h;
		ss = ss / h;
		R(j + 1, j) = 0.0;
		if (cc < 0.0)
		{
			R(j, j) = -h;
			cc = -cc;
			ss = -ss;
		}
		else
			R(j, j) = h;

		xny = ss / (1.0 + cc);
		for (k = j + 1; k < iq; k++)
		{
			t1 = R(j, k);
			t2 = R(j + 1, k);
			R(j, k) = t1 * cc + t2 * ss;
			R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
		}
		for (k = 0; k < n; k++)
		{
			t1 = J(k, j);
			t2 = J(k, j + 1);
			J(k, j) = t1 * cc + t2 * ss;
			J(k, j + 1) = xny * (J(k, j) + t1) - t2;
		}
	}
}
#ifndef _EIGEN_QUADSOLVE_HPP_
#define _EIGEN_QUADSOLVE_HPP_

//source http://www.labri.fr/perso/guenneba/code/QuadProg/eiquadprog.hpp
/*
 FILE eiquadprog.hh

 NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
	   uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
	   Luca Di Gaspero, working with ublas data structures.

 The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
 for the solution of a (convex) Quadratic Programming problem
by means of a dual method.

The problem is in the form:

min 0.5 * x G x + g0 x
s.t.
	CE^T x + ce0 = 0
	CI^T x + ci0 >= 0

 The matrix and vectors dimensions are as follows:
	 G: n * n
		g0: n

		CE: n * p
	 ce0: p

	  CI: n * m
   ci0: m

	 x: n

 The function will return the cost of the solution written in the x vector or
 std::numeric_limits::infinity() if the problem is infeasible. In the latter case
 the value of the x vector is not correct.

 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
			 strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.

 Notes:
  1. pay attention in setting up the vectors ce0 and ci0.
	   If the constraints of your problem are specified in the form
	   A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
  2. The matrix G is modified within the function since it is used to compute
	 the G = L^T L cholesky factorization for further computations inside the function.
	 If you need the original matrix G you should make a copy of it and pass the copy
	 to the function.


 The author will be grateful if the researchers using this software will
 acknowledge the contribution of this modified function and of Di Gaspero's
 original version in their research papers.


LICENSE

Copyright (2010) Gael Guennebaud
Copyright (2008) Angelo Furfaro
Copyright (2006) Luca Di Gaspero


This file is a porting of QuadProg++ routine, originally developed
by Luca Di Gaspero, exploiting uBlas data structures for vectors and
matrices instead of native C++ array.

uquadprog is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

uquadprog is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with uquadprog; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include <Eigen/Dense>

double solveEigenQuadProg(Eigen::MatrixXd& G, Eigen::VectorXd& g0,
	const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
	const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
	Eigen::VectorXd& x);

#endif

GUI.hpp

#pragma once

#include <Siv3D.hpp>
#include <variant>
#include "CommonData.hpp"
#include "Robot.hpp"

void DrawBackground(CommonData& data)
{
	Scene::Rect().draw(GetValue<Color>(data, U"Scene.MainBackground"));
	Rect(0, 0, GetValue<int32>(data, U"Scene.ChannelWidth"), Scene::Height()).draw(GetValue<Color>(data, U"Scene.ChannelBackground"));
	Rect(0, 0, GetValue<int32>(data, U"Scene.MenuWidth"), Scene::Height()).draw(GetValue<Color>(data, U"Scene.MenuBackground"));
}

void UpdateButtonGroup(CommonData& data, const String& groupName)
{
	String buttonActionName;
	Optional<Button::State> buttonActionResult;

	if (data.buttonGroupTable[groupName].size() == 0)
	{
		return;
	}

	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		buttonActionResult = data.buttonTable[buttonName].update();

		if (buttonActionResult.has_value())
		{
			buttonActionName = buttonName;
			break;
		}
	}

	if (buttonActionResult.has_value())
	{
		for (auto& buttonName : data.buttonGroupTable[groupName])
		{
			data.buttonTable[buttonName].setActive(false);
		}

		data.buttonTable[buttonActionName].setActive(true);
	}
}

void DrawButtonGroup(CommonData& data, const String& groupName)
{
	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		data.buttonTable[buttonName].draw(data.textureTable
			, GetValue<Color>(data, U"Menu.ButtonColor")
			, GetValue<Color>(data, U"Menu.ButtonMouseOverColor")
			, GetValue<Color>(data, U"Menu.ButtonActiveColor"));
	}
}

Optional<String> GetActiveButtonNameFromButtonGroup(CommonData& data, const String& groupName)
{
	Optional<String> result;

	for (auto& buttonName : data.buttonGroupTable[groupName])
	{
		if (data.buttonTable[buttonName].isActive())
		{
			result = buttonName;
			break;
		}
	}

	return result;
}



void UpdateMouseInformation(CommonData& data)
{
	data.valueTable[U"Mouse.Pos"] = Cursor::Pos();
	data.valueTable[U"Mouse.PosF"] = Cursor::PosF();
	data.valueTable[U"Mouse.Delta"] = Cursor::Delta();
	data.valueTable[U"Mouse.ScreenPos"] = Cursor::ScreenPos();
}

void UpdateXInputInformation(CommonData& data)
{
	for (size_t userIndex = 0; userIndex < 4; ++userIndex)
	{
		auto controller = XInput(userIndex);

		//if (controller.isConnected())
		{
			String prefix = Format(U"XInput.Controller", userIndex + 1);

			data.valueTable[Format(prefix, U".LeftTrigger")] = controller.leftTrigger;
			data.valueTable[Format(prefix, U".RightTrigger")] = controller.rightTrigger;

			data.valueTable[Format(prefix, U".ButtonLThumb.Pressed")] = controller.buttonLThumb.pressed();
			data.valueTable[Format(prefix, U".ButtonLThumb.Down")] = controller.buttonLThumb.down();
			data.valueTable[Format(prefix, U".LeftThumbX")] = controller.leftThumbX;
			data.valueTable[Format(prefix, U".LeftThumbY")] = controller.leftThumbY;


			data.valueTable[Format(prefix, U".ButtonRThumb.Pressed")] = controller.buttonRThumb.pressed();
			data.valueTable[Format(prefix, U".ButtonRThumb.Down")] = controller.buttonRThumb.down();
			data.valueTable[Format(prefix, U".RightThumbX")] = controller.rightThumbX;
			data.valueTable[Format(prefix, U".RightThumbY")] = controller.rightThumbY;

			data.valueTable[Format(prefix, U".ButtonRight.Pressed")] = controller.buttonRight.pressed();
			data.valueTable[Format(prefix, U".ButtonLeft.Pressed")] = controller.buttonLeft.pressed();
			data.valueTable[Format(prefix, U".ButtonUp.Pressed")] = controller.buttonUp.pressed();
			data.valueTable[Format(prefix, U".ButtonDown.Pressed")] = controller.buttonDown.pressed();

			data.valueTable[Format(prefix, U".ButtonA.Pressed")] = controller.buttonA.pressed();
			data.valueTable[Format(prefix, U".ButtonB.Pressed")] = controller.buttonB.pressed();
			data.valueTable[Format(prefix, U".ButtonX.Pressed")] = controller.buttonX.pressed();
			data.valueTable[Format(prefix, U".ButtonY.Pressed")] = controller.buttonY.pressed();

			data.valueTable[Format(prefix, U".ButtonBack.Pressed")] = controller.buttonBack.pressed();
			data.valueTable[Format(prefix, U".ButtonStart.Pressed")] = controller.buttonStart.pressed();
		}
	}
}

void UpdateInformations(CommonData& data)
{
	UpdateMouseInformation(data);
	UpdateXInputInformation(data);
	UpdateRobotData(data);
}



void DrawXInputController(CommonData& data, size_t userIndex)
{
	auto controller = XInput(userIndex);

	ColorF background = GetValue<Color>(data, U"Scene.MainBackground");

	constexpr Ellipse buttonLB(160, 140, 50, 24);
	constexpr Ellipse buttonRB(520, 140, 50, 24);

	constexpr RectF leftTrigger(150, 16, 40, 100);
	constexpr RectF rightTrigger(500, 16, 40, 100);

	constexpr Circle buttonLThumb(170, 250, 35);
	constexpr Circle buttonRThumb(420, 350, 35);
	constexpr Circle buttonDPad(260, 350, 40);

	constexpr Circle buttonA(510, 300, 20);
	constexpr Circle buttonB(560, 250, 20);
	constexpr Circle buttonX(460, 250, 20);
	constexpr Circle buttonY(510, 200, 20);
	constexpr Circle buttonBack(270, 250, 15);
	constexpr Circle buttonStart(410, 250, 15);



	// �U��

	buttonLB.draw(ColorF(controller.buttonLB.pressed() ? 1.0 : 0.7));
	buttonRB.draw(ColorF(controller.buttonRB.pressed() ? 1.0 : 0.7));

	//Ellipse(340 /* + 3.0 * Random(leftV + rightV) */, 480, 300, 440).draw(ColorF(0.9));
	//Ellipse(340, 40, 220, 120).draw(background);
	//Circle(340, 660, 240).draw(background);

	Circle(340, 250, 30).draw(ColorF(0.6));

	// �R���g���[���̐ڑ��̗L��
	if (controller.isConnected())
	{
		Circle(340, 250, 32).drawPie(-0.5_pi + 0.5_pi * controller.userIndex, 0.5_pi, ColorF(0.7, 1.0, 0.5));
	}

	Circle(340, 250, 25).draw(ColorF(0.6));

	leftTrigger.draw(AlphaF(0.25));
	leftTrigger.stretched((controller.leftTrigger - 1.0) * leftTrigger.h, 0, 0, 0).draw();

	rightTrigger.draw(AlphaF(0.25));
	rightTrigger.stretched((controller.rightTrigger - 1.0) * rightTrigger.h, 0, 0, 0).draw();

	buttonLThumb.draw(ColorF(controller.buttonLThumb.pressed() ? 0.85 : 0.5));
	Circle(buttonLThumb.center + Vec2(controller.leftThumbX, -controller.leftThumbY) * 25, 20).draw();

	buttonRThumb.draw(ColorF(controller.buttonRThumb.pressed() ? 0.85 : 0.5));
	Circle(buttonRThumb.center + Vec2(controller.rightThumbX, -controller.rightThumbY) * 25, 20).draw();

	buttonDPad.draw(ColorF(0.5));

	const Vec2 direction(
		controller.buttonRight.pressed() - controller.buttonLeft.pressed(),
		controller.buttonDown.pressed() - controller.buttonUp.pressed());

	if (!direction.isZero())
	{
		Circle(buttonDPad.center + direction.normalized() * 25, 15).draw();
	}

	buttonA.draw(ColorF(0.0, 1.0, 0.3).setA(controller.buttonA.pressed() ? 1.0 : 0.3));
	buttonB.draw(ColorF(1.0, 0.0, 0.3).setA(controller.buttonB.pressed() ? 1.0 : 0.3));
	buttonX.draw(ColorF(0.0, 0.3, 1.0).setA(controller.buttonX.pressed() ? 1.0 : 0.3));
	buttonY.draw(ColorF(1.0, 0.5, 0.0).setA(controller.buttonY.pressed() ? 1.0 : 0.3));

	buttonBack.draw(ColorF(controller.buttonBack.pressed() ? 1.0 : 0.7));
	buttonStart.draw(ColorF(controller.buttonStart.pressed() ? 1.0 : 0.7));
}

void DrawXInputInformation(CommonData& data)
{
	for (size_t i = 0; i < 2; ++i)
	{
		auto t = Transformer2D(Mat3x2::Translate(Vec2(0.0, i * 400)), true);

		DrawXInputController(data, i);
	}
}

void DrawMotorInfos(CommonData& data)
{
	for (const auto& i : data.motorData.motorDisplays)
	{
		auto& motorDisplay = i.second;
		auto& motorInfo = data.motorData.motorInfos[motorDisplay.name];

		RectF(motorDisplay.pos, Vec2(250.0, 100.0)).drawFrame(3.0);
		data.fontTable[U"main"](U"Name: ", motorInfo.name).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 0.0));
		data.fontTable[U"main"](U"ID: ", motorInfo.id).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 20.0));
		data.fontTable[U"main"](U"TYPE: ", motorInfo.type).draw(Arg::topLeft = motorDisplay.pos + Vec2(5.0, 40.0));
	}
}

void DrawBodyInfos(CommonData& data)
{
	for (const auto& i : data.bodyData.bodyPartDisplays)
	{
		auto& bodyPartDisplay = i.second;
		auto& bodyPartInfo = data.bodyData.bodyPartInfos[bodyPartDisplay.name];

		RectF(bodyPartDisplay.pos, Vec2(250.0, 100.0)).drawFrame(3.0);
		data.fontTable[U"main"](U"Name: ", bodyPartInfo.name).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 0.0));
		data.fontTable[U"main"](U"TYPE: ", bodyPartInfo.bindingType).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 20.0));
		data.fontTable[U"main"](U"SOURCE: ", bodyPartInfo.source).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 40.0));
		data.fontTable[U"main"](GetValueToString(data, bodyPartInfo.source)).draw(Arg::topLeft = bodyPartDisplay.pos + Vec2(5.0, 60.0));
	}
}


void UpdateValueProcessor(CommonData& data)
{
	for (const auto& processName : data.valueProcessData.valueProcessSequence)
	{
		auto& valueProcess = data.valueProcessData.valueProcessInfos[processName];

		if (valueProcess.type == U"handFromController")
		{
			const double x = GetValue<double>(data, valueProcess.soureTable[U"X"]);
			const double y = GetValue<double>(data, valueProcess.soureTable[U"Y"]);
			const double z = GetValue<double>(data, valueProcess.soureTable[U"Z"]);
			const Vec3 zDisplacement = GetValue<Vec3>(data, valueProcess.soureTable[U"ZDisplacement"]);
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const Vec3 offset = GetValue<Vec3>(data, valueProcess.soureTable[U"Offset"]);
			const Vec2 controllVec2 = Vec2(xScale * x, yScale * y);
			Vec3 valueResult = offset + Vec3(controllVec2.x, controllVec2.y, 0.0) + zDisplacement * z;

			data.valueTable[valueProcess.to] = valueResult;
		}
		if (valueProcess.type == U"vec3FromDoubles")
		{
			const double x = GetValue<double>(data, valueProcess.soureTable[U"X"]);
			const double y = GetValue<double>(data, valueProcess.soureTable[U"Y"]);
			const double z = GetValue<double>(data, valueProcess.soureTable[U"Z"]);
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const double zScale = GetValue<double>(data, valueProcess.soureTable[U"ZScale"]);
			const Vec3 offset = GetValue<Vec3>(data, valueProcess.soureTable[U"Offset"]);
			Vec3 valueResult = offset + Vec3(xScale * x, yScale * y, zScale * z);

			data.valueTable[valueProcess.to] = valueResult;
		}
		if (valueProcess.type == U"threeDoublesFromVec3")
		{
			const double xScale = GetValue<double>(data, valueProcess.soureTable[U"XScale"]);
			const double yScale = GetValue<double>(data, valueProcess.soureTable[U"YScale"]);
			const double zScale = GetValue<double>(data, valueProcess.soureTable[U"ZScale"]);
			const double xOffset = GetValue<double>(data, valueProcess.soureTable[U"XOffset"]);
			const double yOffset = GetValue<double>(data, valueProcess.soureTable[U"YOffset"]);
			const double zOffset = GetValue<double>(data, valueProcess.soureTable[U"ZOffset"]);
			const Vec3 source = GetValue<Vec3>(data, valueProcess.soureTable[U"vec3"]);

			data.valueTable[valueProcess.soureTable[U"to1"]] = source.x * xScale + xOffset;
			data.valueTable[valueProcess.soureTable[U"to2"]] = source.y * yScale + yOffset;
			data.valueTable[valueProcess.soureTable[U"to3"]] = source.z * zScale + zOffset;
		}
		if (valueProcess.type == U"doubleFromTwoDoubles")
		{
			const double scale1 = GetValue<double>(data, valueProcess.soureTable[U"scale1"]);
			const double scale2 = GetValue<double>(data, valueProcess.soureTable[U"scale2"]);

			const double offset = GetValue<double>(data, valueProcess.soureTable[U"offset"]);

			const double source1 = GetValue<double>(data, valueProcess.soureTable[U"source1"]);
			const double source2 = GetValue<double>(data, valueProcess.soureTable[U"source2"]);

			data.valueTable[valueProcess.soureTable[U"to"]] = source1 * scale1 + source2 * scale2 + offset;
		}
	}
}


void LoadSystemSettings(CommonData& data)
{
	// TOML �t�@�C������f�[�^��ǂݍ���
	const TOMLReader toml(U"example/config/config.toml");

	if (!toml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.toml`");
	}

	// TOML �f�[�^�����ׂĕ\��
	//ShowTable(toml);

	//Print << U"-----";

	// �v�f�̃p�X�Œl���擾
	const String windowTitle = toml[U"Window.title"].getString();
	const int32 windowWidth = toml[U"Window.width"].get<int32>();
	const int32 windowHeight = toml[U"Window.height"].get<int32>();
	const bool windowSizable = toml[U"Window.sizable"].get<bool>();
	const ColorF sceneBackground = toml[U"Scene.mainBackground"].get<Color>();

	Window::SetTitle(windowTitle);
	Window::Resize(windowWidth, windowHeight);
	Window::SetStyle(windowSizable ? WindowStyle::Sizable : WindowStyle::Fixed);
	Scene::SetBackground(sceneBackground);


	//�e�p�̃I�u�W�F�N�g
	{
		Size size(windowWidth, windowHeight);

		data.gaussianA1 = RenderTexture(size);
		data.gaussianB1 = RenderTexture(size);
		data.gaussianA4 = RenderTexture(size / 4);
		data.gaussianB4 = RenderTexture(size / 4);
		//data.gaussianA8 = RenderTexture(size / 8);
		//data.gaussianB8 = RenderTexture(size / 8);
	}

	// ���l�̔z��� TOML �f�[�^����쐬

	// �A�C�e���̔z��� TOML �f�[�^����쐬

	// �A�C�e���`��p�̃t�H���g
	data.fontTable = GetFonts(toml[U"Fonts"].tableArrayView());
	data.textureTable = GetIconTextures(toml[U"IconTextures"].tableArrayView());

	data.printAllDataToConsole();

	HashTable<String, Button> buttons;
	{
		for (const auto& object : toml[U"Buttons"].tableArrayView())
		{
			Button button(object[U"textureName"].getString(), Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>()));
			button.setActive(object[U"isActive"].get<bool>());
			buttons[object[U"label"].getString()] = button;
		}
	}
	data.buttonTable = buttons;


	HashTable<String, Array<String>> buttonGroupTable;
	{
		for (const auto& object : toml[U"ButtonGroups"].tableArrayView())
		{
			Array<String> buttonNames;

			if (!object[U"ButtonNames"].isEmpty())
			{
				for (const auto& tObject : object[U"ButtonNames"].tableArrayView())
				{
					buttonNames << tObject[U"label"].getString();
				}
			}

			buttonGroupTable[object[U"label"].getString()] = buttonNames;
		}
	}
	data.buttonGroupTable = buttonGroupTable;

	data.valueTable[U"Menu.ButtonColor"] = toml[U"Menu.buttonColor"].get<Color>();
	data.valueTable[U"Menu.ButtonMouseOverColor"] = toml[U"Menu.buttonMouseOverColor"].get<Color>();
	data.valueTable[U"Menu.ButtonActiveColor"] = toml[U"Menu.buttonActiveColor"].get<Color>();
	data.valueTable[U"Menu.TransformerScale"] = toml[U"Menu.transformerScale"].get<double>();
	data.valueTable[U"Menu.SimpleGuiPos"] = toml[U"Menu.simpleGuiPos"].get<Vec2>();
	data.valueTable[U"Scene.MainBackground"] = sceneBackground;
	data.valueTable[U"Scene.ChannelWidth"] = toml[U"Scene.channelWidth"].get<int32>();
	data.valueTable[U"Scene.ChannelBackground"] = toml[U"Scene.channelBackground"].get<Color>();
	data.valueTable[U"Scene.MenuWidth"] = toml[U"Scene.menuWidth"].get<int32>();
	data.valueTable[U"Scene.MenuBackground"] = toml[U"Scene.menuBackground"].get<Color>();

	data.channelFunctionTable[U"gamepad"] = DrawXInputInformation;
	data.channelFunctionTable[U"robot"] = DrawMotorInfos;
	data.channelFunctionTable[U"body"] = DrawBodyInfos;
	data.channelFunctionTable[U"preview"] = DrawRobotData;


	constexpr double fov = 45_deg;
	constexpr Vec3 focusPosition(0, 5, 0);
	Vec3 eyePositon(0, 0, 0);
	data.camera = experimental::BasicCamera3D(Size(Scene::Size().x - toml[U"Scene.channelWidth"].get<int32>(), Scene::Size().y), fov, eyePositon, focusPosition);



}

void LoadUserSettings(CommonData& data)
{
	// TOML �t�@�C������f�[�^��ǂݍ���
	const TOMLReader settingsToml(U"settings.toml");

	if (!settingsToml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.toml`");
	}

	{
		HashTable<String, MotorConnectInformation> motorConnectInformations;
		for (const auto& object : settingsToml[U"Robot.COMs"].tableArrayView())
		{
			MotorConnectInformation motorConnectInformation;

			Dynamixel::BaudRate rate;
			if (object[U"baudRate"].getString() == U"1000000")
			{
				rate = Dynamixel::BaudRate::Rate1000000;
			}
			if (object[U"baudRate"].getString() == U"57600")
			{
				rate = Dynamixel::BaudRate::Rate57600;
			}

			motorConnectInformation.dyna = std::make_shared<Dynamixel>();
			motorConnectInformation.dyna->connectPort(object[U"comPort"].getString(), rate, ham::ProtocolType::Protocol2);

			for (const auto& names : object[U"Motors"].tableArrayView())
			{
				motorConnectInformation.names << names[U"label"].getString();
			}

			motorConnectInformations.emplace(object[U"label"].getString(), motorConnectInformation);
		}
		data.motorData.comTable = motorConnectInformations;

		HashTable<String, MotorInfo> motorInfos;
		for (const auto& object : settingsToml[U"Robot.Motors"].tableArrayView())
		{
			MotorInfo motorInfo;
			motorInfo.name = object[U"label"].getString();
			motorInfo.id = object[U"id"].get<size_t>();
			motorInfo.type = object[U"type"].getString();
			motorInfo.source = object[U"source"].getString();

			motorInfos[object[U"label"].getString()] = motorInfo;
		}
		data.motorData.motorInfos = motorInfos;

		HashTable<String, MotorDisplay> motorDisplays;
		for (const auto& object : settingsToml[U"Robot.MotorDisplays"].tableArrayView())
		{
			MotorDisplay motorDisplay;
			motorDisplay.name = object[U"label"].getString();
			motorDisplay.pos = Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>());

			motorDisplays[object[U"label"].getString()] = motorDisplay;
		}
		data.motorData.motorDisplays = motorDisplays;
	}


	HashTable<String, BodyPartInfo> bodyPartInfos;
	for (const auto& object : settingsToml[U"Body.Parts"].tableArrayView())
	{
		BodyPartInfo bodyPartInfo;
		bodyPartInfo.name = object[U"label"].getString();
		bodyPartInfo.bindingType = object[U"bindingType"].getString();
		bodyPartInfo.source = object[U"source"].getString();

		bodyPartInfos[object[U"label"].getString()] = bodyPartInfo;
	}
	data.bodyData.bodyPartInfos = bodyPartInfos;

	HashTable<String, BodyPartDisplay> bodyPartDisplays;
	for (const auto& object : settingsToml[U"Body.PartDisplays"].tableArrayView())
	{
		BodyPartDisplay bodyPartDisplay;
		bodyPartDisplay.name = object[U"label"].getString();
		bodyPartDisplay.pos = Vec2(object[U"pos.x"].get<double>(), object[U"pos.y"].get<double>());

		bodyPartDisplays[object[U"label"].getString()] = bodyPartDisplay;
	}
	data.bodyData.bodyPartDisplays = bodyPartDisplays;

	//------------------------------------------------

	for (const auto& object : settingsToml[U"ValueProcessor.values"].tableArrayView())
	{
		if (object[U"type"].getString() == U"vec3")
		{
			data.valueTable[Format(U"ValueProcessor.", object[U"label"].getString())]
				= Vec3(object[U"value.x"].get<double>(), object[U"value.y"].get<double>(), object[U"value.z"].get<double>());
		}
		if (object[U"type"].getString() == U"double")
		{
			data.valueTable[Format(U"ValueProcessor.", object[U"label"].getString())]
				= object[U"value"].get<double>();
		}
	}

	HashTable<String, ValueProcessInfo> valueProcessInfos;
	for (const auto& object : settingsToml[U"ValueProcessor.valueProcessInfos"].tableArrayView())
	{
		ValueProcessInfo valueProcessInfo;
		valueProcessInfo.name = object[U"label"].getString();
		valueProcessInfo.type = object[U"type"].getString();
		valueProcessInfo.to = object[U"to"].getString();

		if (valueProcessInfo.type == U"handFromController")
		{
			valueProcessInfo.soureTable[U"X"] = object[U"X"].getString();// U"XInput.Controller1.LeftThumbX";
			valueProcessInfo.soureTable[U"Y"] = object[U"Y"].getString(); //U"XInput.Controller1.LeftThumbY";
			valueProcessInfo.soureTable[U"Z"] = object[U"Z"].getString();
			valueProcessInfo.soureTable[U"ZDisplacement"] = object[U"ZDisplacement"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"Offset"] = object[U"Offset"].getString();
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
		}
		if (valueProcessInfo.type == U"vec3FromDoubles")
		{
			valueProcessInfo.soureTable[U"X"] = object[U"X"].getString();// U"XInput.Controller1.LeftThumbX";
			valueProcessInfo.soureTable[U"Y"] = object[U"Y"].getString(); //U"XInput.Controller1.LeftThumbY";
			valueProcessInfo.soureTable[U"Z"] = object[U"Z"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"ZScale"] = object[U"ZScale"].getString();
			valueProcessInfo.soureTable[U"Offset"] = object[U"Offset"].getString();
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
		}
		if (valueProcessInfo.type == U"threeDoublesFromVec3")
		{
			valueProcessInfo.soureTable[U"to1"] = object[U"to1"].getString();
			valueProcessInfo.soureTable[U"to2"] = object[U"to2"].getString();
			valueProcessInfo.soureTable[U"to3"] = object[U"to3"].getString();
			valueProcessInfo.soureTable[U"XScale"] = object[U"XScale"].getString();
			valueProcessInfo.soureTable[U"YScale"] = object[U"YScale"].getString();
			valueProcessInfo.soureTable[U"ZScale"] = object[U"ZScale"].getString();
			valueProcessInfo.soureTable[U"XOffset"] = object[U"XOffset"].getString();
			valueProcessInfo.soureTable[U"YOffset"] = object[U"YOffset"].getString();
			valueProcessInfo.soureTable[U"ZOffset"] = object[U"ZOffset"].getString();
			valueProcessInfo.soureTable[U"vec3"] = object[U"vec3"].getString();
		}
		if (valueProcessInfo.type == U"doubleFromTwoDoubles")
		{
			valueProcessInfo.soureTable[U"to"] = object[U"to"].getString();
			valueProcessInfo.soureTable[U"scale1"] = object[U"scale1"].getString();
			valueProcessInfo.soureTable[U"scale2"] = object[U"scale2"].getString();
			valueProcessInfo.soureTable[U"offset"] = object[U"offset"].getString();
			valueProcessInfo.soureTable[U"source1"] = object[U"source1"].getString();
			valueProcessInfo.soureTable[U"source2"] = object[U"source2"].getString();
		}

		valueProcessInfos[valueProcessInfo.name] = valueProcessInfo;
	}
	data.valueProcessData.valueProcessInfos = valueProcessInfos;


	Array<String> valueProcessSequence;
	for (const auto& object : settingsToml[U"ValueProcessor.valueProcessSequence"].tableArrayView())
	{
		valueProcessSequence << object[U"label"].getString();
	}
	data.valueProcessData.valueProcessSequence = valueProcessSequence;
}

void LoadData(CommonData& data)
{
	LoadSystemSettings(data);

	LoadUserSettings(data);
}

void Update(CommonData& data)
{
	UpdateInformations(data);

	UpdateValueProcessor(data);

	SendMotorInfo(data);

	ClearPrint();
	//Print << data.valueProcessData.valueProcessSequence;

	UpdateButtonGroup(data, U"menu");

	if (const auto value = GetActiveButtonNameFromButtonGroup(data, U"menu"))
	{
		auto name = value.value();

		UpdateButtonGroup(data, name);
	}
}

void Draw(CommonData& data)
{
	DrawButtonGroup(data, U"menu");

	if (const auto value = GetActiveButtonNameFromButtonGroup(data, U"menu"))
	{
		auto name = value.value();

		DrawButtonGroup(data, name);

		if (const auto channelValue = GetActiveButtonNameFromButtonGroup(data, name))
		{
			auto channelName = channelValue.value();

			if (data.channelFunctionTable.find(channelName) != data.channelFunctionTable.end())
			{
				auto t = Transformer2D(Mat3x2::Translate(Vec2(GetValue<int32>(data, U"Scene.ChannelWidth"), 0.0)), true);

				data.channelFunctionTable[channelName](data);
			}
		}
	}
}

void UpdateAndDraw(CommonData& data)
{
	Update(data);

	DrawBackground(data);

	{
		// �K�E�X�ڂ����p�e�N�X�`���ɂ�����x�V�[����`��
		data.gaussianA1.clear(ColorF(0.0));
		{
			ScopedRenderTarget2D target(data.gaussianA1);

			Draw(data);
		}

		// �I���W�i���T�C�Y�̃K�E�X�ڂ��� (A1)
		// A1 �� 1/4 �T�C�Y�ɂ��ăK�E�X�ڂ��� (A4)
		// A4 �� 1/2 �T�C�Y�ɂ��ăK�E�X�ڂ��� (A8)
		Shader::GaussianBlur(data.gaussianA1,data.gaussianB1, data.gaussianA1);
		//Shader::Downsample(data.gaussianA1, data.gaussianA4);
		//Shader::GaussianBlur(data.gaussianA4, data.gaussianB4, data.gaussianA4);
	}

	{
		ScopedRenderStates2D blend(BlendState::Subtractive);
		data.gaussianA1.resized(Scene::Size()).draw(ColorF(1.0, 0.3));
	}

	Draw(data);
}

Main.cpp

#include <Siv3D.hpp>

#include "GUI.hpp"

void Main()
{
	CommonData data;

	LoadData(data);

	while (System::Update())
	{
		if (KeySpace.down())
		{
			TorqueEnableMotors(data);
		}

		UpdateAndDraw(data);
	}

	TorqueDisableMotors(data);
}

MMD.hpp

#pragma once

#include <Siv3D.hpp>

/// Spherical Linear Interpolation
	/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
	/// Reference: Shoemake at SIGGRAPH 89
	/// See also
	/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
Vec4 QuaternionSlerp(double t, const Vec4& from, const Vec4& to) {
	const double epsilon = 0.00001;
	double omega, cosomega, sinomega, scale_from, scale_to;

	Vec4 quatTo(to);
	// this is a dot product

	cosomega = from.dot(to);//from.asVec4().dot(to.asVec4());

	if (cosomega < 0.0)
	{
		cosomega = -cosomega;
		quatTo = -to;
	}

	if ((1.0 - cosomega) > epsilon)
	{
		omega = acos(cosomega); // 0 <= omega <= Pi (see man acos)
		sinomega = sin(omega);  // this sinomega should always be +ve so
		// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
		scale_from = sin((1.0 - t) * omega) / sinomega;
		scale_to = sin(t * omega) / sinomega;
	}
	else
	{
		/* --------------------------------------------------
		The ends of the vectors are very close
		we can use simple linear interpolation - no need
		to worry about the "spherical" interpolation
		-------------------------------------------------- */
		scale_from = 1.0 - t;
		scale_to = t;
	}

	return from * scale_from + quatTo * scale_to;
}


inline Mat4x4 GetRotatationMatrix(const Vec4& q)
{
	double length2 = q.lengthSq();
	Mat4x4 _mat = Mat4x4::Identity();

	const double QX = q.x;
	const double QY = q.y;
	const double QZ = q.z;
	const double QW = q.w;


	if (fabs(length2) <= std::numeric_limits<double>::min())
	{

	}
	else
	{
		double rlength2;
		// normalize quat if required.
		// We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
		// That is a square of a square root, so it is possible to avoid that
		if (length2 != 1.0)
		{
			rlength2 = 2.0 / length2;
		}
		else
		{
			rlength2 = 2.0;
		}

		// Source: Gamasutra, Rotating Objects Using Quaternions
		//
		//http://www.gamasutra.com/features/19980703/quaternions_01.htm

		double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		// calculate coefficients
		x2 = rlength2 * QX;
		y2 = rlength2 * QY;
		z2 = rlength2 * QZ;

		xx = QX * x2;
		xy = QX * y2;
		xz = QX * z2;

		yy = QY * y2;
		yz = QY * z2;
		zz = QZ * z2;

		wx = QW * x2;
		wy = QW * y2;
		wz = QW * z2;

		// Note.  Gamasutra gets the matrix assignments inverted, resulting
		// in left-handed rotations, which is contrary to OpenGL and OSG's
		// methodology.  The matrix assignment has been altered in the next
		// few lines of code to do the right thing.
		// Don Burns - Oct 13, 2001

		const double mat00 = 1.0 - (yy + zz);
		const double mat10 = xy - wz;
		const double mat20 = xz + wy;


		const double mat01 = xy + wz;
		const double mat11 = 1.0 - (xx + zz);
		const double mat21 = yz - wx;

		const double mat02 = xz - wy;
		const double mat12 = yz + wx;
		const double mat22 = 1.0 - (xx + yy);

		_mat = Mat4x4(mat00, mat01, mat02, 0.0,
			mat10, mat11, mat12, 0.0,
			mat20, mat21, mat22, 0.0,
			0.0, 0.0, 0.0, 1.0);
	}

	return _mat;
}



inline double InterpolateBezier(double x1, double x2, double y1, double y2, double x)
{
	double t = 0.5;
	double s = 0.5;

	for (size_t i = 0; i < 15; ++i)
	{
		const double ft = (3 * s * s * t * x1) + (3 * s * t * t * x2) + (t * t * t) - x;

		if (Math::Abs(ft) < 0.00001)
		{
			break;
		}

		if (ft > 0.0)
		{
			t -= 1 / Math::Pow(2.0, 2.0 + i);
		}
		else
		{
			t += 1 / Math::Pow(2.0, 2.0 + i);
		}

		s = 1.0 - t;
	}

	return (3 * s * s * t * y1) + (3 * s * t * t * y2) + (t * t * t);
}

struct BodyKeyFrame
{
	size_t time;
	Vec3 pos;
	Vec4 rotation;
	Optional<Vec4> rotationBezier;
};

struct MorphKeyFrame
{
	size_t time;
	double value;
};

struct BodySequence
{
	String name;
	Array<BodyKeyFrame> bodyKeyFrames;
};

struct MorphSequence
{
	String name;
	Array<MorphKeyFrame> morphKeyFrames;
};

struct AnimationData
{
	String name;
	HashTable<String, BodySequence> BodySequences;
	HashTable<String, MorphSequence> MorphSequences;
};


std::pair<size_t, size_t> GetTimeIndex(size_t timeFrame, const BodySequence& seq)
{
	if (seq.bodyKeyFrames.size() <= 1)
	{
		return std::pair<size_t, size_t>(0, 0);
	}

	size_t startIndex = 0;
	size_t endIndex = seq.bodyKeyFrames.size() - 1;
	size_t index = static_cast<size_t>(endIndex / 2);

	while (endIndex - startIndex != 1)
	{
		if (seq.bodyKeyFrames[index].time > timeFrame)
		{
			endIndex = index;
		}
		else
		{
			startIndex = index;
		}

		index = startIndex + (endIndex - startIndex) / 2;
	}

	if (index == 0)
	{
		return std::pair<size_t, size_t>(0, 0);
	}
	else
	{
		return std::pair<size_t, size_t>(startIndex, endIndex);
	}
}

std::pair<size_t, size_t> GetTimeIndex(size_t timeFrame, const MorphSequence& seq)
{
	if (seq.morphKeyFrames.size() <= 1)
	{
		return std::pair<size_t, size_t>(0, 0);
	}

	size_t startIndex = 0;
	size_t endIndex = seq.morphKeyFrames.size() - 1;
	size_t index = static_cast<size_t>(endIndex / 2);

	while (endIndex - startIndex != 1)
	{
		if (seq.morphKeyFrames[index].time > timeFrame)
		{
			endIndex = index;
		}
		else
		{
			startIndex = index;
		}

		index = startIndex + (endIndex - startIndex) / 2;
	}

	if (index == 0)
	{
		return std::pair<size_t, size_t>(0, 0);
	}
	else
	{
		return std::pair<size_t, size_t>(startIndex, endIndex);
	}
}

double GetCurrentMorphValue(size_t timeFrame, const MorphSequence& seq)
{
	const auto timeIndex = GetTimeIndex(timeFrame, seq);

	if (timeIndex.second == 0)
	{
		return 0.0;
	}
	else
	{
		const auto& startKeyFrame = seq.morphKeyFrames[timeIndex.first];
		const auto& endKeyFrame = seq.morphKeyFrames[timeIndex.second];

		const double startValue = startKeyFrame.value;
		const double endValue = endKeyFrame.value;

		const size_t offset = endKeyFrame.time - startKeyFrame.time;
		const double ratio = Clamp(static_cast<double>(timeFrame - startKeyFrame.time) / static_cast<double>(offset), 0.0, 1.0);

		Print << ratio;

		return  (endValue - startValue) * ratio + startValue;
	}
}

Vec4 GetCurrentRotationQuaternion(size_t timeFrame, const BodySequence& seq)
{
	const auto timeIndex = GetTimeIndex(timeFrame, seq);

	Print << timeIndex;

	if (timeIndex.second == 0)
	{
		return Vec4(0.0, 0.0, 0.0, 1.0);
	}
	else
	{
		const auto& startKeyFrame = seq.bodyKeyFrames[timeIndex.first];
		const auto& endKeyFrame = seq.bodyKeyFrames[timeIndex.second];

		const Vec4 startRotation = startKeyFrame.rotation;
		const Vec4 endRotation = endKeyFrame.rotation;

		const size_t offset = endKeyFrame.time - startKeyFrame.time;
		const double ratio = Clamp(static_cast<double>(timeFrame - startKeyFrame.time) / static_cast<double>(offset), 0.0, 1.0);


		if (startKeyFrame.rotationBezier)
		{
			const auto rotationBezier = startKeyFrame.rotationBezier.value();

			const double tRatio = InterpolateBezier(rotationBezier.x, rotationBezier.z, rotationBezier.y, rotationBezier.w, ratio);


			Print << tRatio;

			return QuaternionSlerp(tRatio, startRotation, endRotation);
		}
		else
		{
			return QuaternionSlerp(ratio, startRotation, endRotation);
		}
	}
}

Mat4x4 GetCurrentRotationMatrix(size_t timeFrame, const BodySequence& seq)
{
	return GetRotatationMatrix(GetCurrentRotationQuaternion(timeFrame, seq));
}


AnimationData GetAnimationData(const FilePathView& path)
{

	// XML �t�@�C������f�[�^��ǂݍ���
	const XMLReader xml(path);
	
	if (!xml) // �����ǂݍ��݂Ɏ��s������
	{
		throw Error(U"Failed to load `config.xml`");
	}

	// XML �f�[�^�����ׂĕ\��
	//ShowElements(xml);

	AnimationData animationData;
	animationData.name = path;

	for (auto e = xml.firstChild(); e; e = e.nextSibling())
	{

		if (e.name() == U"boneMotionSequence")
		{
			//Console << U"Name: <{}>"_fmt(e.name());

			for (auto bodyPart = e.firstChild(); bodyPart; bodyPart = bodyPart.nextSibling())
			{

				const String bodyPartName = bodyPart.attributes()[0].second;

				if (bodyPartName == U"�Z���^�["
					|| bodyPartName == U"�㔼�g"
					|| bodyPartName == U"��"
					|| bodyPartName == U"��"
					|| bodyPartName == U"����"
					|| bodyPartName == U"���r"
					|| bodyPartName == U"���Ђ�"
					|| bodyPartName == U"�����"
					|| bodyPartName == U"�E��"
					|| bodyPartName == U"�E�r"
					|| bodyPartName == U"�E�Ђ�"
					|| bodyPartName == U"�E���")
				{
					BodySequence seq;
					seq.name = bodyPartName;

					//Console << U"Name: <{}>"_fmt(bodyPart.attributes()[0].second);

					for (auto boneMotion = bodyPart.firstChild(); boneMotion; boneMotion = boneMotion.nextSibling())
					{
						BodyKeyFrame keyFrame;

						const auto& attributes = boneMotion.attributes();

						//Console << U"Value: <{}>"_fmt(boneMotion.attributes());

						keyFrame.time = Parse<size_t>(attributes[0].second);

						for (auto info = boneMotion.firstChild(); info; info = info.nextSibling())
						{
							//Console << info.name() << U": <{}>"_fmt(info.attributes());

							const auto& infoAttributes = info.attributes();

							if (info.name() == U"bonePosition")
							{
								keyFrame.pos = Vec3(Parse<double>(infoAttributes[0].second)
									, Parse<double>(infoAttributes[1].second)
									, Parse<double>(infoAttributes[2].second));
							}

							if (info.name() == U"boneRotQuat")
							{
								keyFrame.rotation = Vec4(
									Parse<double>(infoAttributes[0].second)
									, Parse<double>(infoAttributes[1].second)
									, Parse<double>(infoAttributes[2].second)
									, Parse<double>(infoAttributes[3].second));

								const auto& interpolationInfo = info.firstChild();

								if (interpolationInfo)
								{
									const auto& interpolationAttributes = interpolationInfo.attributes();

									const Vec4 interpolationValues(
										Parse<double>(interpolationAttributes[0].second),
										Parse<double>(interpolationAttributes[1].second),
										Parse<double>(interpolationAttributes[2].second),
										Parse<double>(interpolationAttributes[3].second));

									keyFrame.rotationBezier = interpolationValues / 127.0;
								}

							}
						}

						seq.bodyKeyFrames << keyFrame;
					}


					animationData.BodySequences[seq.name] = seq;
				}
			}

		}

		if (e.name() == U"morphSequence")
		{
			for (auto morphPart = e.firstChild(); morphPart; morphPart = morphPart.nextSibling())
			{
				if (morphPart.attributes()[0].second == U"�܂΂���")
				{
					MorphSequence seq;
					seq.name = morphPart.attributes()[0].second;

					//Console << U"Name: <{}>"_fmt(bodyPart.attributes()[0].second);

					for (auto morphMotion = morphPart.firstChild(); morphMotion; morphMotion = morphMotion.nextSibling())
					{
						MorphKeyFrame keyFrame;

						//Console << U"Value: <{}>"_fmt(boneMotion.attributes());

						keyFrame.time = Parse<size_t>(morphMotion.attributes()[0].second);
						keyFrame.value = Parse<double>(morphMotion.attributes()[1].second);

						seq.morphKeyFrames << keyFrame;
					}


					animationData.MorphSequences[seq.name] = seq;
				}
			}
		}
	}

	return animationData;
}

Robot.hpp

#pragma once

#include "Solver.hpp"

#include <Siv3D.hpp>


namespace Codegen
{
	void computeNeckMechanismEnergy(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = -1;
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = (_i_var3) * (_i_var0);
		_i_var7 = std::sin(neckAngles(0, 0));
		_i_var8 = -(headConnectParameters(2, 0));
		_i_var9 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var10 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var11 = (_i_var4) * (motorConnectParameters(2, 0));
		_i_var12 = std::sin(neckAngles(1, 0));
		_i_var13 = (_i_var6)+(_i_var5);
		_i_var14 = std::cos(neckAngles(1, 0));
		_i_var15 = (_i_var8) * (_i_var7);
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(1, 0));
		_i_var18 = -(_i_var11);
		_i_var19 = (_i_var13) * (_i_var12);
		_i_var20 = (_i_var15) * (_i_var14);
		_i_var21 = -(_i_var15);
		_i_var22 = std::sin(motorAngles(0, 0));
		_i_var23 = -(motorConnectParameters(2, 0));
		_i_var24 = (_i_var16) * (_i_var12);
		_i_var25 = (_i_var1)-(_i_var14);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var12);
		_i_var30 = (_i_var13) * (_i_var14);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var14);
		_i_var34 = std::cos(motorAngles(1, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var14);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = -(motorConnectParameters(0, 0));
		_i_var40 = (_i_var30)+(_i_var29);
		_i_var41 = std::cos(motorAngles(0, 0));
		_i_var42 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var43 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var40)-(_i_var39);
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var41);
		_i_var50 = (_i_var43)-(_i_var42);
		_i_var51 = (_i_var44)-(motorConnectParameters(0, 0));
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var54)+(_i_var53);
		_i_var60 = (_i_var55) * (_i_var55);
		_i_var61 = (_i_var57)+(_i_var56);
		_i_var62 = (_i_var59)+(_i_var58);
		_i_var63 = (_i_var61)+(_i_var60);
		_i_var64 = std::sqrt(_i_var62);
		_i_var65 = std::sqrt(_i_var63);
		_i_var66 = (_i_var64)-(rodDistance);
		_i_var67 = (_i_var65)-(rodDistance);
		_i_var68 = (_i_var66) * (_i_var66);
		_i_var69 = (_i_var67) * (_i_var67);
		_i_var70 = (_i_var69)+(_i_var68);
		_i_var71 = 0.5;
		_i_var72 = (_i_var71) * (_i_var70);
		energy = _i_var72;
	}
	void computeNeckMechanismEnergyGradient(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		Eigen::Matrix<double, 2, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var5 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var6 = std::sin(neckAngles(0, 0));
		_i_var7 = -(headConnectParameters(2, 0));
		_i_var8 = -1;
		_i_var9 = (_i_var3) * (_i_var2);
		_i_var10 = (_i_var3) * (_i_var0);
		_i_var11 = std::sin(neckAngles(1, 0));
		_i_var12 = (_i_var5)+(_i_var4);
		_i_var13 = std::cos(neckAngles(1, 0));
		_i_var14 = (_i_var7) * (_i_var6);
		_i_var15 = (_i_var8) * (motorConnectParameters(2, 0));
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(0, 0));
		_i_var18 = -(motorConnectParameters(2, 0));
		_i_var19 = (_i_var12) * (_i_var11);
		_i_var20 = (_i_var14) * (_i_var13);
		_i_var21 = -(_i_var14);
		_i_var22 = std::sin(motorAngles(1, 0));
		_i_var23 = -(_i_var15);
		_i_var24 = (_i_var16) * (_i_var11);
		_i_var25 = (_i_var1)-(_i_var13);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var11);
		_i_var30 = (_i_var12) * (_i_var13);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var13);
		_i_var34 = std::cos(motorAngles(0, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var13);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = (_i_var30)+(_i_var29);
		_i_var40 = std::cos(motorAngles(1, 0));
		_i_var41 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var42 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var43 = -(motorConnectParameters(0, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var39)-(motorConnectParameters(0, 0));
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var40);
		_i_var50 = (_i_var42)-(_i_var41);
		_i_var51 = (_i_var44)-(_i_var43);
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var54)+(_i_var53);
		_i_var60 = (_i_var55) * (_i_var55);
		_i_var61 = (_i_var57)+(_i_var56);
		_i_var62 = (_i_var59)+(_i_var58);
		_i_var63 = (_i_var61)+(_i_var60);
		_i_var64 = std::sqrt(_i_var62);
		_i_var65 = std::sqrt(_i_var63);
		_i_var66 = 2;
		_i_var67 = (_i_var64)-(rodDistance);
		_i_var68 = 0.5;
		_i_var69 = (_i_var65)-(rodDistance);
		_i_var70 = (_i_var66) * (_i_var64);
		_i_var71 = (_i_var68) * (_i_var67);
		_i_var72 = (_i_var66) * (_i_var65);
		_i_var73 = (_i_var68) * (_i_var69);
		_i_var74 = (_i_var1) / (_i_var70);
		_i_var75 = (_i_var66) * (_i_var71);
		_i_var76 = (_i_var1) / (_i_var72);
		_i_var77 = (_i_var66) * (_i_var73);
		_i_var78 = (_i_var75) * (_i_var74);
		_i_var79 = (_i_var77) * (_i_var76);
		_i_var80 = (_i_var78) * (_i_var47);
		_i_var81 = (_i_var78) * (_i_var52);
		_i_var82 = (_i_var79) * (_i_var50);
		_i_var83 = (_i_var79) * (_i_var55);
		_i_var84 = (_i_var66) * (_i_var80);
		_i_var85 = (_i_var66) * (_i_var81);
		_i_var86 = (_i_var66) * (_i_var82);
		_i_var87 = (_i_var66) * (_i_var83);
		_i_var88 = (_i_var84) * (_i_var8);
		_i_var89 = (_i_var85) * (_i_var8);
		_i_var90 = (_i_var86) * (_i_var8);
		_i_var91 = (_i_var87) * (_i_var8);
		_i_var92 = (_i_var88) * (_i_var18);
		_i_var93 = -(_i_var17);
		_i_var94 = (_i_var89) * (motorConnectParameters(2, 0));
		_i_var95 = (_i_var90) * (_i_var23);
		_i_var96 = -(_i_var22);
		_i_var97 = (_i_var91) * (motorConnectParameters(2, 0));
		_i_var98 = (_i_var92) * (_i_var34);
		_i_var99 = (_i_var94) * (_i_var93);
		_i_var100 = (_i_var95) * (_i_var40);
		_i_var101 = (_i_var97) * (_i_var96);
		_i_var102 = (_i_var99)+(_i_var98);
		_i_var103 = (_i_var101)+(_i_var100);
		gradient(0, 0) = _i_var102;
		gradient(1, 0) = _i_var103;
	}
	void computeNeckMechanismEnergyHessian(const Eigen::Matrix<double, 3, 1>& headConnectParameters, const Eigen::Matrix<double, 3, 1>& motorConnectParameters, double rodDistance, const Eigen::Matrix<double, 2, 1>& neckAngles, const Eigen::Matrix<double, 2, 1>& motorAngles,
		Eigen::Matrix<double, 2, 2>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174;
		_i_var0 = std::cos(neckAngles(0, 0));
		_i_var1 = 1;
		_i_var2 = (_i_var1)-(_i_var0);
		_i_var3 = -(headConnectParameters(0, 0));
		_i_var4 = (headConnectParameters(0, 0)) * (_i_var2);
		_i_var5 = (headConnectParameters(0, 0)) * (_i_var0);
		_i_var6 = std::sin(neckAngles(0, 0));
		_i_var7 = -(headConnectParameters(2, 0));
		_i_var8 = -1;
		_i_var9 = (_i_var3) * (_i_var2);
		_i_var10 = (_i_var3) * (_i_var0);
		_i_var11 = std::sin(neckAngles(1, 0));
		_i_var12 = (_i_var5)+(_i_var4);
		_i_var13 = std::cos(neckAngles(1, 0));
		_i_var14 = (_i_var7) * (_i_var6);
		_i_var15 = (_i_var8) * (motorConnectParameters(2, 0));
		_i_var16 = (_i_var10)+(_i_var9);
		_i_var17 = std::sin(motorAngles(0, 0));
		_i_var18 = -(motorConnectParameters(2, 0));
		_i_var19 = (_i_var12) * (_i_var11);
		_i_var20 = (_i_var14) * (_i_var13);
		_i_var21 = -(_i_var14);
		_i_var22 = std::sin(motorAngles(1, 0));
		_i_var23 = -(_i_var15);
		_i_var24 = (_i_var16) * (_i_var11);
		_i_var25 = (_i_var1)-(_i_var13);
		_i_var26 = (headConnectParameters(2, 0)) * (_i_var0);
		_i_var27 = (_i_var18) * (_i_var17);
		_i_var28 = (_i_var20)+(_i_var19);
		_i_var29 = (_i_var21) * (_i_var11);
		_i_var30 = (_i_var12) * (_i_var13);
		_i_var31 = (_i_var23) * (_i_var22);
		_i_var32 = (_i_var20)+(_i_var24);
		_i_var33 = (_i_var16) * (_i_var13);
		_i_var34 = std::cos(motorAngles(0, 0));
		_i_var35 = (_i_var26) * (_i_var25);
		_i_var36 = (_i_var26) * (_i_var13);
		_i_var37 = (_i_var27)+(motorConnectParameters(1, 0));
		_i_var38 = (_i_var28)+(headConnectParameters(1, 0));
		_i_var39 = (_i_var30)+(_i_var29);
		_i_var40 = std::cos(motorAngles(1, 0));
		_i_var41 = (_i_var31)+(motorConnectParameters(1, 0));
		_i_var42 = (_i_var32)+(headConnectParameters(1, 0));
		_i_var43 = -(motorConnectParameters(0, 0));
		_i_var44 = (_i_var33)+(_i_var29);
		_i_var45 = (motorConnectParameters(2, 0)) * (_i_var34);
		_i_var46 = (_i_var36)+(_i_var35);
		_i_var47 = (_i_var38)-(_i_var37);
		_i_var48 = (_i_var39)-(motorConnectParameters(0, 0));
		_i_var49 = (motorConnectParameters(2, 0)) * (_i_var40);
		_i_var50 = (_i_var42)-(_i_var41);
		_i_var51 = (_i_var44)-(_i_var43);
		_i_var52 = (_i_var46)-(_i_var45);
		_i_var53 = (_i_var47) * (_i_var47);
		_i_var54 = (_i_var48) * (_i_var48);
		_i_var55 = (_i_var46)-(_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (_i_var51) * (_i_var51);
		_i_var58 = -(_i_var17);
		_i_var59 = (_i_var52) * (_i_var52);
		_i_var60 = (_i_var54)+(_i_var53);
		_i_var61 = -(_i_var22);
		_i_var62 = (_i_var55) * (_i_var55);
		_i_var63 = (_i_var57)+(_i_var56);
		_i_var64 = (_i_var34) * (_i_var18);
		_i_var65 = (_i_var58) * (motorConnectParameters(2, 0));
		_i_var66 = (_i_var60)+(_i_var59);
		_i_var67 = (_i_var40) * (_i_var23);
		_i_var68 = (_i_var61) * (motorConnectParameters(2, 0));
		_i_var69 = (_i_var63)+(_i_var62);
		_i_var70 = 2;
		_i_var71 = (_i_var64) * (_i_var8);
		_i_var72 = (_i_var65) * (_i_var8);
		_i_var73 = std::sqrt(_i_var66);
		_i_var74 = (_i_var67) * (_i_var8);
		_i_var75 = (_i_var68) * (_i_var8);
		_i_var76 = std::sqrt(_i_var69);
		_i_var77 = (_i_var71) * (_i_var70);
		_i_var78 = (_i_var72) * (_i_var70);
		_i_var79 = (_i_var70) * (_i_var73);
		_i_var80 = (_i_var73)-(rodDistance);
		_i_var81 = 0.5;
		_i_var82 = (_i_var74) * (_i_var70);
		_i_var83 = (_i_var75) * (_i_var70);
		_i_var84 = (_i_var70) * (_i_var76);
		_i_var85 = (_i_var76)-(rodDistance);
		_i_var86 = (_i_var77) * (_i_var47);
		_i_var87 = (_i_var78) * (_i_var52);
		_i_var88 = (_i_var79) * (_i_var79);
		_i_var89 = (_i_var81) * (_i_var80);
		_i_var90 = (_i_var82) * (_i_var50);
		_i_var91 = (_i_var83) * (_i_var55);
		_i_var92 = (_i_var84) * (_i_var84);
		_i_var93 = (_i_var81) * (_i_var85);
		_i_var94 = (_i_var1) / (_i_var79);
		_i_var95 = (_i_var87)+(_i_var86);
		_i_var96 = (_i_var1) / (_i_var88);
		_i_var97 = (_i_var70) * (_i_var89);
		_i_var98 = (_i_var1) / (_i_var84);
		_i_var99 = (_i_var91)+(_i_var90);
		_i_var100 = (_i_var1) / (_i_var92);
		_i_var101 = (_i_var70) * (_i_var93);
		_i_var102 = (_i_var95) * (_i_var94);
		_i_var103 = -(_i_var96);
		_i_var104 = (_i_var95) * (_i_var97);
		_i_var105 = (_i_var99) * (_i_var98);
		_i_var106 = -(_i_var100);
		_i_var107 = (_i_var99) * (_i_var101);
		_i_var108 = (_i_var102) * (_i_var70);
		_i_var109 = (_i_var104) * (_i_var103);
		_i_var110 = (_i_var105) * (_i_var70);
		_i_var111 = (_i_var107) * (_i_var106);
		_i_var112 = (_i_var108) * (_i_var81);
		_i_var113 = (_i_var109) * (_i_var70);
		_i_var114 = (_i_var110) * (_i_var81);
		_i_var115 = (_i_var111) * (_i_var70);
		_i_var116 = (_i_var113)+(_i_var112);
		_i_var117 = (_i_var115)+(_i_var114);
		_i_var118 = (_i_var116) * (_i_var94);
		_i_var119 = (_i_var97) * (_i_var94);
		_i_var120 = (_i_var117) * (_i_var98);
		_i_var121 = (_i_var101) * (_i_var98);
		_i_var122 = (_i_var118) * (_i_var47);
		_i_var123 = (_i_var77) * (_i_var119);
		_i_var124 = (_i_var119) * (_i_var52);
		_i_var125 = (_i_var118) * (_i_var52);
		_i_var126 = (_i_var78) * (_i_var119);
		_i_var127 = (_i_var120) * (_i_var50);
		_i_var128 = (_i_var82) * (_i_var121);
		_i_var129 = (_i_var121) * (_i_var55);
		_i_var130 = (_i_var120) * (_i_var55);
		_i_var131 = (_i_var83) * (_i_var121);
		_i_var132 = (_i_var123)+(_i_var122);
		_i_var133 = (_i_var70) * (_i_var124);
		_i_var134 = (_i_var126)+(_i_var125);
		_i_var135 = (_i_var119) * (_i_var47);
		_i_var136 = (_i_var128)+(_i_var127);
		_i_var137 = (_i_var70) * (_i_var129);
		_i_var138 = (_i_var131)+(_i_var130);
		_i_var139 = (_i_var121) * (_i_var50);
		_i_var140 = (_i_var132)+(_i_var122);
		_i_var141 = (_i_var133) * (_i_var8);
		_i_var142 = (_i_var134)+(_i_var125);
		_i_var143 = (_i_var70) * (_i_var135);
		_i_var144 = (_i_var136)+(_i_var127);
		_i_var145 = (_i_var137) * (_i_var8);
		_i_var146 = (_i_var138)+(_i_var130);
		_i_var147 = (_i_var70) * (_i_var139);
		_i_var148 = (_i_var140) * (_i_var8);
		_i_var149 = (_i_var141) * (motorConnectParameters(2, 0));
		_i_var150 = (_i_var142) * (_i_var8);
		_i_var151 = (_i_var143) * (_i_var8);
		_i_var152 = (_i_var144) * (_i_var8);
		_i_var153 = (_i_var145) * (motorConnectParameters(2, 0));
		_i_var154 = (_i_var146) * (_i_var8);
		_i_var155 = (_i_var147) * (_i_var8);
		_i_var156 = (_i_var148) * (_i_var18);
		_i_var157 = (_i_var149) * (_i_var8);
		_i_var158 = (_i_var150) * (motorConnectParameters(2, 0));
		_i_var159 = (_i_var151) * (_i_var18);
		_i_var160 = (_i_var152) * (_i_var23);
		_i_var161 = (_i_var153) * (_i_var8);
		_i_var162 = (_i_var154) * (motorConnectParameters(2, 0));
		_i_var163 = (_i_var155) * (_i_var23);
		_i_var164 = (_i_var157)+(_i_var156);
		_i_var165 = (_i_var159)+(_i_var158);
		_i_var166 = (_i_var161)+(_i_var160);
		_i_var167 = (_i_var163)+(_i_var162);
		_i_var168 = (_i_var164) * (_i_var34);
		_i_var169 = (_i_var165) * (_i_var58);
		_i_var170 = (_i_var166) * (_i_var40);
		_i_var171 = (_i_var167) * (_i_var61);
		_i_var172 = (_i_var169)+(_i_var168);
		_i_var173 = 0;
		_i_var174 = (_i_var171)+(_i_var170);
		hessian(0, 0) = _i_var172;
		hessian(1, 0) = _i_var173;
		hessian(0, 1) = _i_var173;
		hessian(1, 1) = _i_var174;
	}

}



namespace Codegen
{
	void compute3AxisKibouLeftArmEnergy(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = (_i_var16) * (_i_var14);
		_i_var24 = (_i_var10) * (_i_var13);
		_i_var25 = (_i_var20)+(_i_var19);
		_i_var26 = (_i_var22)+(_i_var21);
		_i_var27 = (_i_var24)+(_i_var23);
		_i_var28 = (_i_var25)+(offsets(1, 0));
		_i_var29 = (_i_var26)+(offsets(0, 0));
		_i_var30 = (_i_var27)+(offsets(2, 0));
		_i_var31 = (target(1, 0)) - (_i_var28);
		_i_var32 = (target(0, 0)) - (_i_var29);
		_i_var33 = (target(2, 0)) - (_i_var30);
		_i_var34 = (_i_var31) * (_i_var31);
		_i_var35 = (_i_var32) * (_i_var32);
		_i_var36 = (_i_var33) * (_i_var33);
		_i_var37 = (_i_var35)+(_i_var34);
		_i_var38 = (_i_var37)+(_i_var36);
		_i_var39 = std::sqrt(_i_var38);
		_i_var40 = 0.5;
		_i_var41 = (_i_var40) * (_i_var39);
		_i_var42 = (_i_var41) * (_i_var39);
		energy = _i_var42;
	}
	void compute3AxisKibouLeftArmEnergyGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = (_i_var16) * (_i_var14);
		_i_var24 = (_i_var10) * (_i_var13);
		_i_var25 = (_i_var20)+(_i_var19);
		_i_var26 = (_i_var22)+(_i_var21);
		_i_var27 = (_i_var24)+(_i_var23);
		_i_var28 = (_i_var25)+(offsets(1, 0));
		_i_var29 = (_i_var26)+(offsets(0, 0));
		_i_var30 = (_i_var27)+(offsets(2, 0));
		_i_var31 = (target(1, 0)) - (_i_var28);
		_i_var32 = (target(0, 0)) - (_i_var29);
		_i_var33 = (target(2, 0)) - (_i_var30);
		_i_var34 = (_i_var31) * (_i_var31);
		_i_var35 = (_i_var32) * (_i_var32);
		_i_var36 = (_i_var33) * (_i_var33);
		_i_var37 = (_i_var35)+(_i_var34);
		_i_var38 = (_i_var37)+(_i_var36);
		_i_var39 = std::sqrt(_i_var38);
		_i_var40 = 2;
		_i_var41 = 0.5;
		_i_var42 = (_i_var40) * (_i_var39);
		_i_var43 = (_i_var39) * (_i_var41);
		_i_var44 = (_i_var41) * (_i_var39);
		_i_var45 = (_i_var1) / (_i_var42);
		_i_var46 = (_i_var44)+(_i_var43);
		_i_var47 = (_i_var46) * (_i_var45);
		_i_var48 = (_i_var47) * (_i_var31);
		_i_var49 = -1;
		_i_var50 = (_i_var40) * (_i_var48);
		_i_var51 = (_i_var47) * (_i_var33);
		_i_var52 = (_i_var47) * (_i_var32);
		_i_var53 = (_i_var50) * (_i_var49);
		_i_var54 = (_i_var40) * (_i_var51);
		_i_var55 = (_i_var40) * (_i_var52);
		_i_var56 = (_i_var53) * (_i_var14);
		_i_var57 = (_i_var54) * (_i_var49);
		_i_var58 = (_i_var55) * (_i_var49);
		_i_var59 = (_i_var56) * (_i_var49);
		_i_var60 = (_i_var57) * (_i_var13);
		_i_var61 = (_i_var58) * (_i_var17);
		_i_var62 = (_i_var58) * (_i_var13);
		_i_var63 = (_i_var53) * (_i_var13);
		_i_var64 = (_i_var57) * (_i_var14);
		_i_var65 = (_i_var58) * (_i_var18);
		_i_var66 = (_i_var57) * (_i_var10);
		_i_var67 = (_i_var60)+(_i_var59);
		_i_var68 = (_i_var62)+(_i_var61);
		_i_var69 = (_i_var64)+(_i_var63);
		_i_var70 = (_i_var53) * (_i_var16);
		_i_var71 = (_i_var66)+(_i_var65);
		_i_var72 = (_i_var67) * (_i_var5);
		_i_var73 = (_i_var68) * (_i_var12);
		_i_var74 = (_i_var67) * (_i_var4);
		_i_var75 = (_i_var67) * (_i_var0);
		_i_var76 = (_i_var69) * (_i_var11);
		_i_var77 = (_i_var68) * (_i_var0);
		_i_var78 = (_i_var65) * (_i_var49);
		_i_var79 = (_i_var71)+(_i_var70);
		_i_var80 = (_i_var53) * (_i_var15);
		_i_var81 = (_i_var57) * (_i_var16);
		_i_var82 = (_i_var72) * (_i_var49);
		_i_var83 = (_i_var73)+(_i_var72);
		_i_var84 = (_i_var75)+(_i_var74);
		_i_var85 = (_i_var77)+(_i_var76);
		_i_var86 = -(_i_var14);
		_i_var87 = (_i_var79)+(_i_var78);
		_i_var88 = (_i_var81)+(_i_var80);
		_i_var89 = -(_i_var11);
		_i_var90 = (_i_var83)+(_i_var82);
		_i_var91 = (_i_var69) * (_i_var12);
		_i_var92 = (_i_var84) * (_i_var3);
		_i_var93 = -(_i_var2);
		_i_var94 = (_i_var85) * (offsets(4, 0));
		_i_var95 = (_i_var87) * (_i_var86);
		_i_var96 = (_i_var88) * (_i_var13);
		_i_var97 = (_i_var90) * (_i_var89);
		_i_var98 = (_i_var91) * (_i_var0);
		_i_var99 = (_i_var92) * (_i_var6);
		_i_var100 = (_i_var94) * (_i_var93);
		_i_var101 = (_i_var96)+(_i_var95);
		_i_var102 = (_i_var98)+(_i_var97);
		_i_var103 = (_i_var100)+(_i_var99);
		gradient(0, 0) = _i_var101;
		gradient(1, 0) = _i_var102;
		gradient(2, 0) = _i_var103;
	}
	void compute3AxisKibouLeftArmEnergyHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (_i_var15) * (_i_var14);
		_i_var20 = (_i_var16) * (_i_var13);
		_i_var21 = (_i_var18) * (_i_var17);
		_i_var22 = (_i_var18) * (_i_var13);
		_i_var23 = -1;
		_i_var24 = -(_i_var11);
		_i_var25 = (_i_var16) * (_i_var14);
		_i_var26 = (_i_var10) * (_i_var13);
		_i_var27 = (_i_var20)+(_i_var19);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var6) * (_i_var3);
		_i_var30 = -(_i_var2);
		_i_var31 = (_i_var24) * (_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var27)+(offsets(1, 0));
		_i_var34 = (_i_var28)+(offsets(0, 0));
		_i_var35 = (_i_var29) * (_i_var4);
		_i_var36 = (_i_var29) * (_i_var0);
		_i_var37 = (_i_var30) * (offsets(4, 0));
		_i_var38 = (_i_var31)-(_i_var11);
		_i_var39 = (_i_var32)+(offsets(2, 0));
		_i_var40 = (target(1, 0)) - (_i_var33);
		_i_var41 = (target(0, 0)) - (_i_var34);
		_i_var42 = (_i_var36)+(_i_var35);
		_i_var43 = (_i_var37) * (_i_var11);
		_i_var44 = (_i_var37) * (_i_var0);
		_i_var45 = (_i_var38) * (_i_var5);
		_i_var46 = (_i_var0) * (_i_var12);
		_i_var47 = (_i_var24) * (_i_var12);
		_i_var48 = (target(2, 0)) - (_i_var39);
		_i_var49 = (_i_var40) * (_i_var40);
		_i_var50 = (_i_var41) * (_i_var41);
		_i_var51 = (_i_var42) * (_i_var23);
		_i_var52 = (_i_var42) * (_i_var13);
		_i_var53 = (_i_var43) * (_i_var14);
		_i_var54 = (_i_var44) * (_i_var17);
		_i_var55 = (_i_var44) * (_i_var13);
		_i_var56 = -(_i_var14);
		_i_var57 = (_i_var45) * (_i_var23);
		_i_var58 = (_i_var45) * (_i_var13);
		_i_var59 = (_i_var46) * (_i_var14);
		_i_var60 = (_i_var47) * (_i_var17);
		_i_var61 = (_i_var47) * (_i_var13);
		_i_var62 = (_i_var48) * (_i_var48);
		_i_var63 = (_i_var50)+(_i_var49);
		_i_var64 = (_i_var51) * (_i_var14);
		_i_var65 = (_i_var43) * (_i_var13);
		_i_var66 = (_i_var53)+(_i_var52);
		_i_var67 = (_i_var55)+(_i_var54);
		_i_var68 = (_i_var56) * (_i_var23);
		_i_var69 = (_i_var56) * (_i_var10);
		_i_var70 = (_i_var13) * (_i_var16);
		_i_var71 = (_i_var56) * (_i_var16);
		_i_var72 = (_i_var13) * (_i_var15);
		_i_var73 = (_i_var57) * (_i_var14);
		_i_var74 = (_i_var46) * (_i_var13);
		_i_var75 = (_i_var59)+(_i_var58);
		_i_var76 = (_i_var61)+(_i_var60);
		_i_var77 = (_i_var63)+(_i_var62);
		_i_var78 = (_i_var65)+(_i_var64);
		_i_var79 = 2;
		_i_var80 = (_i_var66) * (_i_var23);
		_i_var81 = (_i_var67) * (_i_var23);
		_i_var82 = (_i_var68)-(_i_var14);
		_i_var83 = (_i_var70)+(_i_var69);
		_i_var84 = (_i_var72)+(_i_var71);
		_i_var85 = (_i_var74)+(_i_var73);
		_i_var86 = (_i_var75) * (_i_var23);
		_i_var87 = (_i_var76) * (_i_var23);
		_i_var88 = std::sqrt(_i_var77);
		_i_var89 = (_i_var78) * (_i_var23);
		_i_var90 = (_i_var80) * (_i_var79);
		_i_var91 = (_i_var81) * (_i_var79);
		_i_var92 = (_i_var82) * (_i_var18);
		_i_var93 = (_i_var83) * (_i_var23);
		_i_var94 = (_i_var84) * (_i_var23);
		_i_var95 = (_i_var85) * (_i_var23);
		_i_var96 = (_i_var86) * (_i_var79);
		_i_var97 = (_i_var87) * (_i_var79);
		_i_var98 = (_i_var79) * (_i_var88);
		_i_var99 = 0.5;
		_i_var100 = (_i_var89) * (_i_var79);
		_i_var101 = (_i_var90) * (_i_var48);
		_i_var102 = (_i_var91) * (_i_var41);
		_i_var103 = (_i_var92) * (_i_var23);
		_i_var104 = (_i_var93) * (_i_var79);
		_i_var105 = (_i_var94) * (_i_var79);
		_i_var106 = (_i_var95) * (_i_var79);
		_i_var107 = (_i_var96) * (_i_var48);
		_i_var108 = (_i_var97) * (_i_var41);
		_i_var109 = (_i_var98) * (_i_var98);
		_i_var110 = (_i_var88) * (_i_var99);
		_i_var111 = (_i_var99) * (_i_var88);
		_i_var112 = (_i_var100) * (_i_var40);
		_i_var113 = (_i_var102)+(_i_var101);
		_i_var114 = (_i_var103) * (_i_var79);
		_i_var115 = (_i_var104) * (_i_var48);
		_i_var116 = (_i_var105) * (_i_var40);
		_i_var117 = (_i_var106) * (_i_var40);
		_i_var118 = (_i_var108)+(_i_var107);
		_i_var119 = (_i_var1) / (_i_var109);
		_i_var120 = (_i_var111)+(_i_var110);
		_i_var121 = (_i_var113)+(_i_var112);
		_i_var122 = (_i_var1) / (_i_var98);
		_i_var123 = (_i_var114) * (_i_var41);
		_i_var124 = (_i_var116)+(_i_var115);
		_i_var125 = (_i_var118)+(_i_var117);
		_i_var126 = -(_i_var119);
		_i_var127 = (_i_var121) * (_i_var120);
		_i_var128 = (_i_var121) * (_i_var122);
		_i_var129 = (_i_var124)+(_i_var123);
		_i_var130 = (_i_var125) * (_i_var120);
		_i_var131 = (_i_var125) * (_i_var122);
		_i_var132 = (_i_var127) * (_i_var126);
		_i_var133 = (_i_var128) * (_i_var99);
		_i_var134 = (_i_var129) * (_i_var120);
		_i_var135 = (_i_var129) * (_i_var122);
		_i_var136 = (_i_var130) * (_i_var126);
		_i_var137 = (_i_var131) * (_i_var99);
		_i_var138 = (_i_var132) * (_i_var79);
		_i_var139 = (_i_var79) * (_i_var133);
		_i_var140 = (_i_var134) * (_i_var126);
		_i_var141 = (_i_var135) * (_i_var99);
		_i_var142 = (_i_var136) * (_i_var79);
		_i_var143 = (_i_var79) * (_i_var137);
		_i_var144 = (_i_var139)+(_i_var138);
		_i_var145 = (_i_var140) * (_i_var79);
		_i_var146 = (_i_var79) * (_i_var141);
		_i_var147 = (_i_var143)+(_i_var142);
		_i_var148 = (_i_var120) * (_i_var122);
		_i_var149 = (_i_var144) * (_i_var122);
		_i_var150 = (_i_var146)+(_i_var145);
		_i_var151 = (_i_var147) * (_i_var122);
		_i_var152 = (_i_var148) * (_i_var40);
		_i_var153 = (_i_var149) * (_i_var40);
		_i_var154 = (_i_var100) * (_i_var148);
		_i_var155 = (_i_var150) * (_i_var122);
		_i_var156 = (_i_var148) * (_i_var41);
		_i_var157 = (_i_var148) * (_i_var48);
		_i_var158 = (_i_var151) * (_i_var40);
		_i_var159 = (_i_var106) * (_i_var148);
		_i_var160 = (_i_var151) * (_i_var41);
		_i_var161 = (_i_var97) * (_i_var148);
		_i_var162 = (_i_var79) * (_i_var152);
		_i_var163 = (_i_var154)+(_i_var153);
		_i_var164 = (_i_var149) * (_i_var48);
		_i_var165 = (_i_var90) * (_i_var148);
		_i_var166 = (_i_var149) * (_i_var41);
		_i_var167 = (_i_var91) * (_i_var148);
		_i_var168 = (_i_var155) * (_i_var48);
		_i_var169 = (_i_var104) * (_i_var148);
		_i_var170 = (_i_var79) * (_i_var156);
		_i_var171 = (_i_var79) * (_i_var157);
		_i_var172 = (_i_var151) * (_i_var48);
		_i_var173 = (_i_var96) * (_i_var148);
		_i_var174 = (_i_var159)+(_i_var158);
		_i_var175 = (_i_var161)+(_i_var160);
		_i_var176 = (_i_var162) * (_i_var23);
		_i_var177 = (_i_var163)+(_i_var153);
		_i_var178 = (_i_var165)+(_i_var164);
		_i_var179 = (_i_var167)+(_i_var166);
		_i_var180 = (_i_var155) * (_i_var41);
		_i_var181 = (_i_var114) * (_i_var148);
		_i_var182 = (_i_var169)+(_i_var168);
		_i_var183 = (_i_var170) * (_i_var23);
		_i_var184 = (_i_var171) * (_i_var23);
		_i_var185 = (_i_var173)+(_i_var172);
		_i_var186 = (_i_var174)+(_i_var158);
		_i_var187 = (_i_var175)+(_i_var160);
		_i_var188 = (_i_var176) * (_i_var14);
		_i_var189 = (_i_var177) * (_i_var23);
		_i_var190 = (_i_var178)+(_i_var164);
		_i_var191 = (_i_var179)+(_i_var166);
		_i_var192 = (_i_var155) * (_i_var40);
		_i_var193 = (_i_var105) * (_i_var148);
		_i_var194 = (_i_var181)+(_i_var180);
		_i_var195 = (_i_var182)+(_i_var168);
		_i_var196 = (_i_var183) * (_i_var18);
		_i_var197 = (_i_var184) * (_i_var10);
		_i_var198 = (_i_var185)+(_i_var172);
		_i_var199 = (_i_var47) * (_i_var183);
		_i_var200 = (_i_var46) * (_i_var176);
		_i_var201 = (_i_var43) * (_i_var176);
		_i_var202 = (_i_var44) * (_i_var183);
		_i_var203 = (_i_var186) * (_i_var23);
		_i_var204 = (_i_var187) * (_i_var23);
		_i_var205 = (_i_var188) * (_i_var23);
		_i_var206 = (_i_var184) * (_i_var13);
		_i_var207 = (_i_var183) * (_i_var17);
		_i_var208 = (_i_var183) * (_i_var13);
		_i_var209 = (_i_var189) * (_i_var14);
		_i_var210 = (_i_var190) * (_i_var23);
		_i_var211 = (_i_var191) * (_i_var23);
		_i_var212 = (_i_var176) * (_i_var13);
		_i_var213 = (_i_var184) * (_i_var14);
		_i_var214 = (_i_var193)+(_i_var192);
		_i_var215 = (_i_var194)+(_i_var180);
		_i_var216 = (_i_var195) * (_i_var23);
		_i_var217 = (_i_var176) * (_i_var15);
		_i_var218 = (_i_var184) * (_i_var16);
		_i_var219 = (_i_var176) * (_i_var16);
		_i_var220 = (_i_var197)+(_i_var196);
		_i_var221 = (_i_var198) * (_i_var23);
		_i_var222 = (_i_var45) * (_i_var184);
		_i_var223 = (_i_var200)+(_i_var199);
		_i_var224 = (_i_var42) * (_i_var184);
		_i_var225 = (_i_var202)+(_i_var201);
		_i_var226 = (_i_var203) * (_i_var14);
		_i_var227 = (_i_var204) * (_i_var17);
		_i_var228 = (_i_var204) * (_i_var13);
		_i_var229 = (_i_var206)+(_i_var205);
		_i_var230 = (_i_var208)+(_i_var207);
		_i_var231 = (_i_var209) * (_i_var23);
		_i_var232 = (_i_var210) * (_i_var13);
		_i_var233 = (_i_var211) * (_i_var17);
		_i_var234 = (_i_var211) * (_i_var13);
		_i_var235 = (_i_var213)+(_i_var212);
		_i_var236 = (_i_var189) * (_i_var13);
		_i_var237 = (_i_var210) * (_i_var14);
		_i_var238 = (_i_var214)+(_i_var192);
		_i_var239 = (_i_var215) * (_i_var23);
		_i_var240 = (_i_var216) * (_i_var10);
		_i_var241 = (_i_var218)+(_i_var217);
		_i_var242 = (_i_var196) * (_i_var23);
		_i_var243 = (_i_var220)+(_i_var219);
		_i_var244 = (_i_var221) * (_i_var10);
		_i_var245 = (_i_var223)+(_i_var222);
		_i_var246 = (_i_var210) * (_i_var10);
		_i_var247 = (_i_var225)+(_i_var224);
		_i_var248 = (_i_var226) * (_i_var23);
		_i_var249 = (_i_var221) * (_i_var13);
		_i_var250 = (_i_var228)+(_i_var227);
		_i_var251 = (_i_var229) * (_i_var5);
		_i_var252 = (_i_var230) * (_i_var12);
		_i_var253 = (_i_var232)+(_i_var231);
		_i_var254 = (_i_var234)+(_i_var233);
		_i_var255 = (_i_var29) * (_i_var229);
		_i_var256 = (_i_var37) * (_i_var230);
		_i_var257 = (_i_var235) * (_i_var11);
		_i_var258 = (_i_var230) * (_i_var0);
		_i_var259 = (_i_var237)+(_i_var236);
		_i_var260 = (_i_var238) * (_i_var23);
		_i_var261 = (_i_var239) * (_i_var18);
		_i_var262 = (_i_var241)+(_i_var240);
		_i_var263 = (_i_var243)+(_i_var242);
		_i_var264 = (_i_var204) * (_i_var18);
		_i_var265 = (_i_var245)+(_i_var244);
		_i_var266 = (_i_var57) * (_i_var176);
		_i_var267 = (_i_var46) * (_i_var184);
		_i_var268 = (_i_var211) * (_i_var18);
		_i_var269 = (_i_var247)+(_i_var246);
		_i_var270 = (_i_var51) * (_i_var176);
		_i_var271 = (_i_var43) * (_i_var184);
		_i_var272 = (_i_var249)+(_i_var248);
		_i_var273 = (_i_var250) * (_i_var12);
		_i_var274 = (_i_var235) * (_i_var12);
		_i_var275 = (_i_var203) * (_i_var13);
		_i_var276 = (_i_var221) * (_i_var14);
		_i_var277 = (_i_var251) * (_i_var23);
		_i_var278 = (_i_var252)+(_i_var251);
		_i_var279 = (_i_var253) * (_i_var5);
		_i_var280 = (_i_var254) * (_i_var12);
		_i_var281 = (_i_var256)+(_i_var255);
		_i_var282 = (_i_var253) * (_i_var4);
		_i_var283 = (_i_var253) * (_i_var0);
		_i_var284 = (_i_var258)+(_i_var257);
		_i_var285 = (_i_var259) * (_i_var11);
		_i_var286 = (_i_var254) * (_i_var0);
		_i_var287 = (_i_var229) * (_i_var4);
		_i_var288 = (_i_var229) * (_i_var0);
		_i_var289 = (_i_var260) * (_i_var16);
		_i_var290 = (_i_var262)+(_i_var261);
		_i_var291 = (_i_var216) * (_i_var16);
		_i_var292 = (_i_var263) * (_i_var23);
		_i_var293 = (_i_var199)+(_i_var264);
		_i_var294 = (_i_var203) * (_i_var16);
		_i_var295 = (_i_var265)+(_i_var264);
		_i_var296 = (_i_var221) * (_i_var16);
		_i_var297 = (_i_var267)+(_i_var266);
		_i_var298 = (_i_var202)+(_i_var268);
		_i_var299 = (_i_var189) * (_i_var16);
		_i_var300 = (_i_var269)+(_i_var268);
		_i_var301 = (_i_var210) * (_i_var16);
		_i_var302 = (_i_var271)+(_i_var270);
		_i_var303 = (_i_var272) * (_i_var5);
		_i_var304 = (_i_var274)+(_i_var273);
		_i_var305 = (_i_var276)+(_i_var275);
		_i_var306 = (_i_var278)+(_i_var277);
		_i_var307 = (_i_var255)+(_i_var279);
		_i_var308 = (_i_var281)+(_i_var280);
		_i_var309 = (_i_var283)+(_i_var282);
		_i_var310 = (_i_var284) * (offsets(4, 0));
		_i_var311 = (_i_var286)+(_i_var285);
		_i_var312 = (_i_var288)+(_i_var287);
		_i_var313 = (_i_var261) * (_i_var23);
		_i_var314 = (_i_var290)+(_i_var289);
		_i_var315 = (_i_var260) * (_i_var15);
		_i_var316 = (_i_var292)+(_i_var291);
		_i_var317 = (_i_var293) * (_i_var23);
		_i_var318 = (_i_var295)+(_i_var294);
		_i_var319 = (_i_var203) * (_i_var15);
		_i_var320 = (_i_var297)+(_i_var296);
		_i_var321 = (_i_var298) * (_i_var23);
		_i_var322 = (_i_var300)+(_i_var299);
		_i_var323 = (_i_var189) * (_i_var15);
		_i_var324 = (_i_var302)+(_i_var301);
		_i_var325 = (_i_var303) * (_i_var23);
		_i_var326 = (_i_var304)+(_i_var303);
		_i_var327 = (_i_var305) * (_i_var12);
		_i_var328 = (_i_var306) * (_i_var23);
		_i_var329 = (_i_var307) * (_i_var23);
		_i_var330 = (_i_var308)+(_i_var279);
		_i_var331 = (_i_var259) * (_i_var12);
		_i_var332 = (_i_var37) * (_i_var235);
		_i_var333 = (_i_var309) * (_i_var3);
		_i_var334 = (_i_var310) * (_i_var23);
		_i_var335 = (_i_var311) * (offsets(4, 0));
		_i_var336 = (_i_var312) * (_i_var3);
		_i_var337 = (_i_var314)+(_i_var313);
		_i_var338 = (_i_var316)+(_i_var315);
		_i_var339 = (_i_var318)+(_i_var317);
		_i_var340 = (_i_var320)+(_i_var319);
		_i_var341 = (_i_var322)+(_i_var321);
		_i_var342 = (_i_var324)+(_i_var323);
		_i_var343 = (_i_var326)+(_i_var325);
		_i_var344 = (_i_var328)+(_i_var327);
		_i_var345 = (_i_var330)+(_i_var329);
		_i_var346 = (_i_var332)+(_i_var331);
		_i_var347 = (_i_var334)+(_i_var333);
		_i_var348 = (_i_var336)+(_i_var335);
		_i_var349 = (_i_var337) * (_i_var56);
		_i_var350 = (_i_var338) * (_i_var13);
		_i_var351 = (_i_var339) * (_i_var56);
		_i_var352 = (_i_var340) * (_i_var13);
		_i_var353 = (_i_var341) * (_i_var56);
		_i_var354 = (_i_var342) * (_i_var13);
		_i_var355 = (_i_var343) * (_i_var24);
		_i_var356 = (_i_var344) * (_i_var0);
		_i_var357 = (_i_var345) * (_i_var24);
		_i_var358 = (_i_var346) * (_i_var0);
		_i_var359 = (_i_var347) * (_i_var6);
		_i_var360 = (_i_var348) * (_i_var30);
		_i_var361 = (_i_var350)+(_i_var349);
		_i_var362 = (_i_var352)+(_i_var351);
		_i_var363 = (_i_var354)+(_i_var353);
		_i_var364 = (_i_var356)+(_i_var355);
		_i_var365 = (_i_var358)+(_i_var357);
		_i_var366 = (_i_var360)+(_i_var359);
		hessian(0, 0) = _i_var361;
		hessian(1, 0) = _i_var362;
		hessian(2, 0) = _i_var363;
		hessian(0, 1) = _i_var362;
		hessian(1, 1) = _i_var364;
		hessian(2, 1) = _i_var365;
		hessian(0, 2) = _i_var363;
		hessian(1, 2) = _i_var365;
		hessian(2, 2) = _i_var366;
	}

}



namespace Codegen
{
	void compute3AxisKibouRightArmEnergy(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var12) * (_i_var19);
		_i_var24 = (_i_var21) * (_i_var20);
		_i_var25 = (_i_var19) * (_i_var13);
		_i_var26 = (_i_var12) * (_i_var22);
		_i_var27 = (_i_var14) * (_i_var13);
		_i_var28 = (_i_var23) * (_i_var20);
		_i_var29 = (_i_var11) * (_i_var13);
		_i_var30 = (_i_var25)+(_i_var24);
		_i_var31 = (_i_var27)+(_i_var26);
		_i_var32 = (_i_var29)+(_i_var28);
		_i_var33 = (_i_var30)+(offsets(1, 0));
		_i_var34 = (_i_var31)+(offsets(0, 0));
		_i_var35 = (_i_var32)+(offsets(2, 0));
		_i_var36 = (target(1, 0)) - (_i_var33);
		_i_var37 = (target(0, 0)) - (_i_var34);
		_i_var38 = (target(2, 0)) - (_i_var35);
		_i_var39 = (_i_var36) * (_i_var36);
		_i_var40 = (_i_var37) * (_i_var37);
		_i_var41 = (_i_var38) * (_i_var38);
		_i_var42 = (_i_var40)+(_i_var39);
		_i_var43 = (_i_var42)+(_i_var41);
		_i_var44 = std::sqrt(_i_var43);
		_i_var45 = 0.5;
		_i_var46 = (_i_var45) * (_i_var44);
		_i_var47 = (_i_var46) * (_i_var44);
		energy = _i_var47;
	}
	void compute3AxisKibouRightArmEnergyGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var12) * (_i_var19);
		_i_var24 = (_i_var21) * (_i_var20);
		_i_var25 = (_i_var19) * (_i_var13);
		_i_var26 = (_i_var12) * (_i_var22);
		_i_var27 = (_i_var14) * (_i_var13);
		_i_var28 = (_i_var23) * (_i_var20);
		_i_var29 = (_i_var11) * (_i_var13);
		_i_var30 = (_i_var25)+(_i_var24);
		_i_var31 = (_i_var27)+(_i_var26);
		_i_var32 = (_i_var29)+(_i_var28);
		_i_var33 = (_i_var30)+(offsets(1, 0));
		_i_var34 = (_i_var31)+(offsets(0, 0));
		_i_var35 = (_i_var32)+(offsets(2, 0));
		_i_var36 = (target(1, 0)) - (_i_var33);
		_i_var37 = (target(0, 0)) - (_i_var34);
		_i_var38 = (target(2, 0)) - (_i_var35);
		_i_var39 = (_i_var36) * (_i_var36);
		_i_var40 = (_i_var37) * (_i_var37);
		_i_var41 = (_i_var38) * (_i_var38);
		_i_var42 = (_i_var40)+(_i_var39);
		_i_var43 = (_i_var42)+(_i_var41);
		_i_var44 = std::sqrt(_i_var43);
		_i_var45 = 2;
		_i_var46 = 0.5;
		_i_var47 = (_i_var45) * (_i_var44);
		_i_var48 = (_i_var44) * (_i_var46);
		_i_var49 = (_i_var46) * (_i_var44);
		_i_var50 = (_i_var1) / (_i_var47);
		_i_var51 = (_i_var49)+(_i_var48);
		_i_var52 = (_i_var51) * (_i_var50);
		_i_var53 = (_i_var52) * (_i_var36);
		_i_var54 = (_i_var52) * (_i_var37);
		_i_var55 = (_i_var45) * (_i_var53);
		_i_var56 = (_i_var45) * (_i_var54);
		_i_var57 = (_i_var52) * (_i_var38);
		_i_var58 = (_i_var55) * (_i_var12);
		_i_var59 = (_i_var56) * (_i_var12);
		_i_var60 = (_i_var45) * (_i_var57);
		_i_var61 = (_i_var58) * (_i_var20);
		_i_var62 = (_i_var59) * (_i_var12);
		_i_var63 = (_i_var60) * (_i_var12);
		_i_var64 = (_i_var61) * (_i_var12);
		_i_var65 = (_i_var62) * (_i_var17);
		_i_var66 = (_i_var63) * (_i_var20);
		_i_var67 = (_i_var64) * (_i_var12);
		_i_var68 = (_i_var63) * (_i_var13);
		_i_var69 = (_i_var65) * (_i_var12);
		_i_var70 = (_i_var59) * (_i_var13);
		_i_var71 = (_i_var66) * (_i_var12);
		_i_var72 = (_i_var58) * (_i_var13);
		_i_var73 = (_i_var59) * (_i_var14);
		_i_var74 = (_i_var63) * (_i_var11);
		_i_var75 = (_i_var68)+(_i_var67);
		_i_var76 = (_i_var70)+(_i_var69);
		_i_var77 = (_i_var72)+(_i_var71);
		_i_var78 = (_i_var62) * (_i_var18);
		_i_var79 = (_i_var58) * (_i_var19);
		_i_var80 = (_i_var74)+(_i_var73);
		_i_var81 = (_i_var75) * (_i_var6);
		_i_var82 = (_i_var76) * (_i_var10);
		_i_var83 = (_i_var75) * (_i_var5);
		_i_var84 = (_i_var75) * (_i_var0);
		_i_var85 = (_i_var76) * (_i_var0);
		_i_var86 = (_i_var77) * (_i_var15);
		_i_var87 = (_i_var78) * (_i_var12);
		_i_var88 = (_i_var80)+(_i_var79);
		_i_var89 = (_i_var58) * (_i_var21);
		_i_var90 = (_i_var63) * (_i_var23);
		_i_var91 = (_i_var81) * (_i_var12);
		_i_var92 = (_i_var82)+(_i_var81);
		_i_var93 = (_i_var84)+(_i_var83);
		_i_var94 = (_i_var86)+(_i_var85);
		_i_var95 = -(_i_var20);
		_i_var96 = (_i_var88)+(_i_var87);
		_i_var97 = (_i_var90)+(_i_var89);
		_i_var98 = -(_i_var15);
		_i_var99 = (_i_var92)+(_i_var91);
		_i_var100 = (_i_var77) * (_i_var10);
		_i_var101 = (_i_var93) * (offsets(4, 0));
		_i_var102 = -(_i_var2);
		_i_var103 = (_i_var94) * (_i_var4);
		_i_var104 = (_i_var96) * (_i_var95);
		_i_var105 = (_i_var97) * (_i_var13);
		_i_var106 = (_i_var99) * (_i_var98);
		_i_var107 = (_i_var100) * (_i_var0);
		_i_var108 = (_i_var101) * (_i_var3);
		_i_var109 = (_i_var103) * (_i_var102);
		_i_var110 = (_i_var105)+(_i_var104);
		_i_var111 = (_i_var107)+(_i_var106);
		_i_var112 = (_i_var109)+(_i_var108);
		gradient(0, 0) = _i_var110;
		gradient(1, 0) = _i_var111;
		gradient(2, 0) = _i_var112;
	}
	void compute3AxisKibouRightArmEnergyHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& target, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = (_i_var9)+(_i_var8);
		_i_var12 = -1;
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = (_i_var10) * (_i_var0);
		_i_var15 = std::sin(angles(1, 0));
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var14);
		_i_var19 = (_i_var10) * (_i_var15);
		_i_var20 = std::sin(angles(0, 0));
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = -(_i_var15);
		_i_var24 = -(_i_var2);
		_i_var25 = (_i_var12) * (_i_var19);
		_i_var26 = (_i_var21) * (_i_var20);
		_i_var27 = (_i_var19) * (_i_var13);
		_i_var28 = (_i_var12) * (_i_var22);
		_i_var29 = (_i_var14) * (_i_var13);
		_i_var30 = (_i_var23) * (_i_var12);
		_i_var31 = (_i_var3) * (offsets(4, 0));
		_i_var32 = (_i_var24) * (_i_var4);
		_i_var33 = (_i_var25) * (_i_var20);
		_i_var34 = (_i_var11) * (_i_var13);
		_i_var35 = (_i_var27)+(_i_var26);
		_i_var36 = (_i_var29)+(_i_var28);
		_i_var37 = (_i_var30)-(_i_var15);
		_i_var38 = (_i_var23) * (_i_var10);
		_i_var39 = (_i_var31) * (_i_var5);
		_i_var40 = (_i_var31) * (_i_var0);
		_i_var41 = (_i_var32) * (_i_var0);
		_i_var42 = (_i_var34)+(_i_var33);
		_i_var43 = (_i_var35)+(offsets(1, 0));
		_i_var44 = (_i_var36)+(offsets(0, 0));
		_i_var45 = (_i_var37) * (_i_var6);
		_i_var46 = (_i_var38) * (_i_var12);
		_i_var47 = (_i_var0) * (_i_var10);
		_i_var48 = (_i_var40)+(_i_var39);
		_i_var49 = (_i_var41) * (_i_var12);
		_i_var50 = (_i_var32) * (_i_var15);
		_i_var51 = (_i_var42)+(offsets(2, 0));
		_i_var52 = (target(1, 0)) - (_i_var43);
		_i_var53 = (target(0, 0)) - (_i_var44);
		_i_var54 = (_i_var45) * (_i_var12);
		_i_var55 = (_i_var46) * (_i_var17);
		_i_var56 = (_i_var47) * (_i_var12);
		_i_var57 = (_i_var48) * (_i_var12);
		_i_var58 = (_i_var49) * (_i_var17);
		_i_var59 = (_i_var50) * (_i_var12);
		_i_var60 = (target(2, 0)) - (_i_var51);
		_i_var61 = (_i_var52) * (_i_var52);
		_i_var62 = (_i_var53) * (_i_var53);
		_i_var63 = (_i_var54) * (_i_var12);
		_i_var64 = (_i_var55) * (_i_var12);
		_i_var65 = (_i_var38) * (_i_var13);
		_i_var66 = (_i_var45) * (_i_var13);
		_i_var67 = (_i_var56) * (_i_var20);
		_i_var68 = (_i_var57) * (_i_var12);
		_i_var69 = (_i_var58) * (_i_var12);
		_i_var70 = (_i_var41) * (_i_var13);
		_i_var71 = (_i_var59) * (_i_var20);
		_i_var72 = (_i_var48) * (_i_var13);
		_i_var73 = -(_i_var20);
		_i_var74 = (_i_var60) * (_i_var60);
		_i_var75 = (_i_var62)+(_i_var61);
		_i_var76 = (_i_var63) * (_i_var20);
		_i_var77 = (_i_var47) * (_i_var13);
		_i_var78 = (_i_var65)+(_i_var64);
		_i_var79 = (_i_var67)+(_i_var66);
		_i_var80 = (_i_var68) * (_i_var20);
		_i_var81 = (_i_var50) * (_i_var13);
		_i_var82 = (_i_var70)+(_i_var69);
		_i_var83 = (_i_var72)+(_i_var71);
		_i_var84 = (_i_var73) * (_i_var12);
		_i_var85 = (_i_var75)+(_i_var74);
		_i_var86 = (_i_var77)+(_i_var76);
		_i_var87 = 2;
		_i_var88 = (_i_var78) * (_i_var12);
		_i_var89 = (_i_var79) * (_i_var12);
		_i_var90 = (_i_var81)+(_i_var80);
		_i_var91 = (_i_var82) * (_i_var12);
		_i_var92 = (_i_var83) * (_i_var12);
		_i_var93 = (_i_var84) * (_i_var18);
		_i_var94 = (_i_var73) * (_i_var11);
		_i_var95 = (_i_var13) * (_i_var25);
		_i_var96 = (_i_var73) * (_i_var19);
		_i_var97 = (_i_var13) * (_i_var21);
		_i_var98 = std::sqrt(_i_var85);
		_i_var99 = (_i_var86) * (_i_var12);
		_i_var100 = (_i_var88) * (_i_var87);
		_i_var101 = (_i_var89) * (_i_var87);
		_i_var102 = (_i_var90) * (_i_var12);
		_i_var103 = (_i_var91) * (_i_var87);
		_i_var104 = (_i_var92) * (_i_var87);
		_i_var105 = (_i_var93) * (_i_var12);
		_i_var106 = (_i_var73) * (_i_var14);
		_i_var107 = (_i_var95)+(_i_var94);
		_i_var108 = (_i_var97)+(_i_var96);
		_i_var109 = (_i_var87) * (_i_var98);
		_i_var110 = 0.5;
		_i_var111 = (_i_var99) * (_i_var87);
		_i_var112 = (_i_var100) * (_i_var53);
		_i_var113 = (_i_var101) * (_i_var60);
		_i_var114 = (_i_var102) * (_i_var87);
		_i_var115 = (_i_var103) * (_i_var53);
		_i_var116 = (_i_var104) * (_i_var60);
		_i_var117 = (_i_var106)+(_i_var105);
		_i_var118 = (_i_var107) * (_i_var12);
		_i_var119 = (_i_var108) * (_i_var12);
		_i_var120 = (_i_var109) * (_i_var109);
		_i_var121 = (_i_var98) * (_i_var110);
		_i_var122 = (_i_var110) * (_i_var98);
		_i_var123 = (_i_var111) * (_i_var52);
		_i_var124 = (_i_var113)+(_i_var112);
		_i_var125 = (_i_var114) * (_i_var52);
		_i_var126 = (_i_var116)+(_i_var115);
		_i_var127 = (_i_var117) * (_i_var12);
		_i_var128 = (_i_var118) * (_i_var87);
		_i_var129 = (_i_var119) * (_i_var87);
		_i_var130 = (_i_var1) / (_i_var120);
		_i_var131 = (_i_var122)+(_i_var121);
		_i_var132 = (_i_var124)+(_i_var123);
		_i_var133 = (_i_var1) / (_i_var109);
		_i_var134 = (_i_var126)+(_i_var125);
		_i_var135 = (_i_var127) * (_i_var87);
		_i_var136 = (_i_var128) * (_i_var60);
		_i_var137 = (_i_var129) * (_i_var52);
		_i_var138 = -(_i_var130);
		_i_var139 = (_i_var132) * (_i_var131);
		_i_var140 = (_i_var132) * (_i_var133);
		_i_var141 = (_i_var134) * (_i_var131);
		_i_var142 = (_i_var134) * (_i_var133);
		_i_var143 = (_i_var135) * (_i_var53);
		_i_var144 = (_i_var137)+(_i_var136);
		_i_var145 = (_i_var139) * (_i_var138);
		_i_var146 = (_i_var140) * (_i_var110);
		_i_var147 = (_i_var141) * (_i_var138);
		_i_var148 = (_i_var142) * (_i_var110);
		_i_var149 = (_i_var144)+(_i_var143);
		_i_var150 = (_i_var145) * (_i_var87);
		_i_var151 = (_i_var87) * (_i_var146);
		_i_var152 = (_i_var147) * (_i_var87);
		_i_var153 = (_i_var87) * (_i_var148);
		_i_var154 = (_i_var149) * (_i_var131);
		_i_var155 = (_i_var149) * (_i_var133);
		_i_var156 = (_i_var151)+(_i_var150);
		_i_var157 = (_i_var153)+(_i_var152);
		_i_var158 = (_i_var154) * (_i_var138);
		_i_var159 = (_i_var155) * (_i_var110);
		_i_var160 = (_i_var156) * (_i_var133);
		_i_var161 = (_i_var131) * (_i_var133);
		_i_var162 = (_i_var157) * (_i_var133);
		_i_var163 = (_i_var158) * (_i_var87);
		_i_var164 = (_i_var87) * (_i_var159);
		_i_var165 = (_i_var160) * (_i_var53);
		_i_var166 = (_i_var100) * (_i_var161);
		_i_var167 = (_i_var161) * (_i_var52);
		_i_var168 = (_i_var161) * (_i_var53);
		_i_var169 = (_i_var162) * (_i_var52);
		_i_var170 = (_i_var114) * (_i_var161);
		_i_var171 = (_i_var162) * (_i_var53);
		_i_var172 = (_i_var103) * (_i_var161);
		_i_var173 = (_i_var164)+(_i_var163);
		_i_var174 = (_i_var160) * (_i_var52);
		_i_var175 = (_i_var111) * (_i_var161);
		_i_var176 = (_i_var166)+(_i_var165);
		_i_var177 = (_i_var87) * (_i_var167);
		_i_var178 = (_i_var87) * (_i_var168);
		_i_var179 = (_i_var170)+(_i_var169);
		_i_var180 = (_i_var172)+(_i_var171);
		_i_var181 = (_i_var161) * (_i_var60);
		_i_var182 = (_i_var162) * (_i_var60);
		_i_var183 = (_i_var104) * (_i_var161);
		_i_var184 = (_i_var173) * (_i_var133);
		_i_var185 = (_i_var175)+(_i_var174);
		_i_var186 = (_i_var176)+(_i_var165);
		_i_var187 = (_i_var177) * (_i_var12);
		_i_var188 = (_i_var178) * (_i_var12);
		_i_var189 = (_i_var179)+(_i_var169);
		_i_var190 = (_i_var180)+(_i_var171);
		_i_var191 = (_i_var87) * (_i_var181);
		_i_var192 = (_i_var183)+(_i_var182);
		_i_var193 = (_i_var184) * (_i_var60);
		_i_var194 = (_i_var128) * (_i_var161);
		_i_var195 = (_i_var160) * (_i_var60);
		_i_var196 = (_i_var101) * (_i_var161);
		_i_var197 = (_i_var185)+(_i_var174);
		_i_var198 = (_i_var186) * (_i_var12);
		_i_var199 = (_i_var187) * (_i_var20);
		_i_var200 = (_i_var188) * (_i_var12);
		_i_var201 = (_i_var189) * (_i_var12);
		_i_var202 = (_i_var190) * (_i_var12);
		_i_var203 = (_i_var191) * (_i_var12);
		_i_var204 = (_i_var192)+(_i_var182);
		_i_var205 = (_i_var184) * (_i_var53);
		_i_var206 = (_i_var135) * (_i_var161);
		_i_var207 = (_i_var194)+(_i_var193);
		_i_var208 = (_i_var196)+(_i_var195);
		_i_var209 = (_i_var197) * (_i_var12);
		_i_var210 = (_i_var198) * (_i_var12);
		_i_var211 = (_i_var199) * (_i_var12);
		_i_var212 = (_i_var200) * (_i_var17);
		_i_var213 = (_i_var201) * (_i_var20);
		_i_var214 = (_i_var202) * (_i_var12);
		_i_var215 = (_i_var203) * (_i_var20);
		_i_var216 = (_i_var204) * (_i_var12);
		_i_var217 = (_i_var206)+(_i_var205);
		_i_var218 = (_i_var184) * (_i_var52);
		_i_var219 = (_i_var129) * (_i_var161);
		_i_var220 = (_i_var207)+(_i_var193);
		_i_var221 = (_i_var188) * (_i_var14);
		_i_var222 = (_i_var203) * (_i_var11);
		_i_var223 = (_i_var208)+(_i_var195);
		_i_var224 = (_i_var38) * (_i_var188);
		_i_var225 = (_i_var47) * (_i_var187);
		_i_var226 = (_i_var41) * (_i_var188);
		_i_var227 = (_i_var50) * (_i_var187);
		_i_var228 = (_i_var209) * (_i_var20);
		_i_var229 = (_i_var210) * (_i_var17);
		_i_var230 = (_i_var211) * (_i_var12);
		_i_var231 = (_i_var203) * (_i_var13);
		_i_var232 = (_i_var212) * (_i_var12);
		_i_var233 = (_i_var188) * (_i_var13);
		_i_var234 = (_i_var213) * (_i_var12);
		_i_var235 = (_i_var214) * (_i_var17);
		_i_var236 = (_i_var215) * (_i_var12);
		_i_var237 = (_i_var187) * (_i_var13);
		_i_var238 = (_i_var216) * (_i_var20);
		_i_var239 = (_i_var217)+(_i_var205);
		_i_var240 = (_i_var219)+(_i_var218);
		_i_var241 = (_i_var220) * (_i_var12);
		_i_var242 = (_i_var187) * (_i_var21);
		_i_var243 = (_i_var203) * (_i_var25);
		_i_var244 = (_i_var200) * (_i_var18);
		_i_var245 = (_i_var187) * (_i_var19);
		_i_var246 = (_i_var222)+(_i_var221);
		_i_var247 = (_i_var223) * (_i_var12);
		_i_var248 = (_i_var45) * (_i_var203);
		_i_var249 = (_i_var225)+(_i_var224);
		_i_var250 = (_i_var48) * (_i_var203);
		_i_var251 = (_i_var227)+(_i_var226);
		_i_var252 = (_i_var228) * (_i_var12);
		_i_var253 = (_i_var229) * (_i_var12);
		_i_var254 = (_i_var198) * (_i_var13);
		_i_var255 = (_i_var231)+(_i_var230);
		_i_var256 = (_i_var233)+(_i_var232);
		_i_var257 = (_i_var234) * (_i_var12);
		_i_var258 = (_i_var216) * (_i_var13);
		_i_var259 = (_i_var235) * (_i_var12);
		_i_var260 = (_i_var202) * (_i_var13);
		_i_var261 = (_i_var237)+(_i_var236);
		_i_var262 = (_i_var238) * (_i_var12);
		_i_var263 = (_i_var201) * (_i_var13);
		_i_var264 = (_i_var239) * (_i_var12);
		_i_var265 = (_i_var240)+(_i_var218);
		_i_var266 = (_i_var241) * (_i_var11);
		_i_var267 = (_i_var243)+(_i_var242);
		_i_var268 = (_i_var244) * (_i_var12);
		_i_var269 = (_i_var246)+(_i_var245);
		_i_var270 = (_i_var247) * (_i_var11);
		_i_var271 = (_i_var249)+(_i_var248);
		_i_var272 = (_i_var216) * (_i_var11);
		_i_var273 = (_i_var251)+(_i_var250);
		_i_var274 = (_i_var252) * (_i_var12);
		_i_var275 = (_i_var247) * (_i_var13);
		_i_var276 = (_i_var254)+(_i_var253);
		_i_var277 = (_i_var247) * (_i_var20);
		_i_var278 = (_i_var255) * (_i_var6);
		_i_var279 = (_i_var256) * (_i_var10);
		_i_var280 = (_i_var258)+(_i_var257);
		_i_var281 = (_i_var260)+(_i_var259);
		_i_var282 = (_i_var31) * (_i_var255);
		_i_var283 = (_i_var32) * (_i_var256);
		_i_var284 = (_i_var256) * (_i_var0);
		_i_var285 = (_i_var261) * (_i_var15);
		_i_var286 = (_i_var263)+(_i_var262);
		_i_var287 = (_i_var264) * (_i_var12);
		_i_var288 = (_i_var265) * (_i_var12);
		_i_var289 = (_i_var264) * (_i_var14);
		_i_var290 = (_i_var267)+(_i_var266);
		_i_var291 = (_i_var269)+(_i_var268);
		_i_var292 = (_i_var210) * (_i_var18);
		_i_var293 = (_i_var46) * (_i_var200);
		_i_var294 = (_i_var198) * (_i_var14);
		_i_var295 = (_i_var271)+(_i_var270);
		_i_var296 = (_i_var63) * (_i_var187);
		_i_var297 = (_i_var56) * (_i_var203);
		_i_var298 = (_i_var214) * (_i_var18);
		_i_var299 = (_i_var49) * (_i_var200);
		_i_var300 = (_i_var202) * (_i_var14);
		_i_var301 = (_i_var273)+(_i_var272);
		_i_var302 = (_i_var68) * (_i_var187);
		_i_var303 = (_i_var59) * (_i_var203);
		_i_var304 = (_i_var275)+(_i_var274);
		_i_var305 = (_i_var276) * (_i_var10);
		_i_var306 = (_i_var261) * (_i_var10);
		_i_var307 = (_i_var277) * (_i_var12);
		_i_var308 = (_i_var209) * (_i_var13);
		_i_var309 = (_i_var278) * (_i_var12);
		_i_var310 = (_i_var279)+(_i_var278);
		_i_var311 = (_i_var280) * (_i_var6);
		_i_var312 = (_i_var281) * (_i_var10);
		_i_var313 = (_i_var283)+(_i_var282);
		_i_var314 = (_i_var280) * (_i_var5);
		_i_var315 = (_i_var280) * (_i_var0);
		_i_var316 = (_i_var285)+(_i_var284);
		_i_var317 = (_i_var281) * (_i_var0);
		_i_var318 = (_i_var286) * (_i_var15);
		_i_var319 = (_i_var255) * (_i_var5);
		_i_var320 = (_i_var255) * (_i_var0);
		_i_var321 = (_i_var287) * (_i_var18);
		_i_var322 = (_i_var288) * (_i_var19);
		_i_var323 = (_i_var290)+(_i_var289);
		_i_var324 = (_i_var241) * (_i_var25);
		_i_var325 = (_i_var291) * (_i_var12);
		_i_var326 = (_i_var293)+(_i_var292);
		_i_var327 = (_i_var209) * (_i_var19);
		_i_var328 = (_i_var295)+(_i_var294);
		_i_var329 = (_i_var247) * (_i_var25);
		_i_var330 = (_i_var297)+(_i_var296);
		_i_var331 = (_i_var299)+(_i_var298);
		_i_var332 = (_i_var201) * (_i_var19);
		_i_var333 = (_i_var301)+(_i_var300);
		_i_var334 = (_i_var216) * (_i_var25);
		_i_var335 = (_i_var303)+(_i_var302);
		_i_var336 = (_i_var304) * (_i_var6);
		_i_var337 = (_i_var306)+(_i_var305);
		_i_var338 = (_i_var308)+(_i_var307);
		_i_var339 = (_i_var310)+(_i_var309);
		_i_var340 = (_i_var282)+(_i_var311);
		_i_var341 = (_i_var313)+(_i_var312);
		_i_var342 = (_i_var315)+(_i_var314);
		_i_var343 = (_i_var316) * (_i_var4);
		_i_var344 = (_i_var318)+(_i_var317);
		_i_var345 = (_i_var320)+(_i_var319);
		_i_var346 = (_i_var321) * (_i_var12);
		_i_var347 = (_i_var323)+(_i_var322);
		_i_var348 = (_i_var288) * (_i_var21);
		_i_var349 = (_i_var325)+(_i_var324);
		_i_var350 = (_i_var326) * (_i_var12);
		_i_var351 = (_i_var328)+(_i_var327);
		_i_var352 = (_i_var209) * (_i_var21);
		_i_var353 = (_i_var330)+(_i_var329);
		_i_var354 = (_i_var331) * (_i_var12);
		_i_var355 = (_i_var333)+(_i_var332);
		_i_var356 = (_i_var201) * (_i_var21);
		_i_var357 = (_i_var335)+(_i_var334);
		_i_var358 = (_i_var336) * (_i_var12);
		_i_var359 = (_i_var337)+(_i_var336);
		_i_var360 = (_i_var338) * (_i_var10);
		_i_var361 = (_i_var339) * (_i_var12);
		_i_var362 = (_i_var340) * (_i_var12);
		_i_var363 = (_i_var341)+(_i_var311);
		_i_var364 = (_i_var286) * (_i_var10);
		_i_var365 = (_i_var32) * (_i_var261);
		_i_var366 = (_i_var342) * (offsets(4, 0));
		_i_var367 = (_i_var343) * (_i_var12);
		_i_var368 = (_i_var344) * (_i_var4);
		_i_var369 = (_i_var345) * (offsets(4, 0));
		_i_var370 = (_i_var347)+(_i_var346);
		_i_var371 = (_i_var349)+(_i_var348);
		_i_var372 = (_i_var351)+(_i_var350);
		_i_var373 = (_i_var353)+(_i_var352);
		_i_var374 = (_i_var355)+(_i_var354);
		_i_var375 = (_i_var357)+(_i_var356);
		_i_var376 = (_i_var359)+(_i_var358);
		_i_var377 = (_i_var361)+(_i_var360);
		_i_var378 = (_i_var363)+(_i_var362);
		_i_var379 = (_i_var365)+(_i_var364);
		_i_var380 = (_i_var367)+(_i_var366);
		_i_var381 = (_i_var369)+(_i_var368);
		_i_var382 = (_i_var370) * (_i_var73);
		_i_var383 = (_i_var371) * (_i_var13);
		_i_var384 = (_i_var372) * (_i_var73);
		_i_var385 = (_i_var373) * (_i_var13);
		_i_var386 = (_i_var374) * (_i_var73);
		_i_var387 = (_i_var375) * (_i_var13);
		_i_var388 = (_i_var376) * (_i_var23);
		_i_var389 = (_i_var377) * (_i_var0);
		_i_var390 = (_i_var378) * (_i_var23);
		_i_var391 = (_i_var379) * (_i_var0);
		_i_var392 = (_i_var380) * (_i_var3);
		_i_var393 = (_i_var381) * (_i_var24);
		_i_var394 = (_i_var383)+(_i_var382);
		_i_var395 = (_i_var385)+(_i_var384);
		_i_var396 = (_i_var387)+(_i_var386);
		_i_var397 = (_i_var389)+(_i_var388);
		_i_var398 = (_i_var391)+(_i_var390);
		_i_var399 = (_i_var393)+(_i_var392);
		hessian(0, 0) = _i_var394;
		hessian(1, 0) = _i_var395;
		hessian(2, 0) = _i_var396;
		hessian(0, 1) = _i_var395;
		hessian(1, 1) = _i_var397;
		hessian(2, 1) = _i_var398;
		hessian(0, 2) = _i_var396;
		hessian(1, 2) = _i_var398;
		hessian(2, 2) = _i_var399;
	}

}

namespace Codegen
{
	void compute3AxisKibouRightArmEnergyFromTwoTarget(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = -(offsets(3, 0));
		_i_var9 = (_i_var6) * (_i_var5);
		_i_var10 = (_i_var6) * (_i_var0);
		_i_var11 = (_i_var7)-(offsets(3, 0));
		_i_var12 = std::cos(angles(0, 0));
		_i_var13 = (_i_var8) * (_i_var0);
		_i_var14 = -1;
		_i_var15 = (_i_var10)+(_i_var9);
		_i_var16 = (_i_var11) * (_i_var0);
		_i_var17 = (_i_var1)-(_i_var12);
		_i_var18 = (_i_var14) * (_i_var13);
		_i_var19 = std::sin(angles(1, 0));
		_i_var20 = (_i_var14) * (_i_var15);
		_i_var21 = (_i_var14) * (_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var11) * (_i_var19);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var20);
		_i_var26 = (_i_var21) * (_i_var17);
		_i_var27 = (_i_var8) * (_i_var19);
		_i_var28 = (_i_var14) * (_i_var22);
		_i_var29 = (_i_var13) * (_i_var12);
		_i_var30 = (_i_var14) * (_i_var23);
		_i_var31 = (_i_var25) * (_i_var24);
		_i_var32 = (_i_var23) * (_i_var12);
		_i_var33 = (_i_var14) * (_i_var26);
		_i_var34 = (_i_var16) * (_i_var12);
		_i_var35 = (_i_var14) * (_i_var27);
		_i_var36 = (_i_var27) * (_i_var12);
		_i_var37 = (_i_var29)+(_i_var28);
		_i_var38 = (_i_var30) * (_i_var24);
		_i_var39 = (_i_var15) * (_i_var12);
		_i_var40 = (_i_var32)+(_i_var31);
		_i_var41 = (_i_var34)+(_i_var33);
		_i_var42 = (_i_var35) * (_i_var24);
		_i_var43 = (_i_var36)+(offsets(1, 0));
		_i_var44 = (_i_var37)+(offsets(0, 0));
		_i_var45 = (_i_var39)+(_i_var38);
		_i_var46 = (_i_var40)+(offsets(1, 0));
		_i_var47 = (_i_var41)+(offsets(0, 0));
		_i_var48 = (_i_var42)+(offsets(2, 0));
		_i_var49 = (targetElbow(1, 0)) - (_i_var43);
		_i_var50 = (targetElbow(0, 0)) - (_i_var44);
		_i_var51 = (_i_var45)+(offsets(2, 0));
		_i_var52 = (targetHand(1, 0)) - (_i_var46);
		_i_var53 = (targetHand(0, 0)) - (_i_var47);
		_i_var54 = (targetElbow(2, 0)) - (_i_var48);
		_i_var55 = (_i_var49) * (_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (targetHand(2, 0)) - (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var53) * (_i_var53);
		_i_var60 = (_i_var54) * (_i_var54);
		_i_var61 = (_i_var56)+(_i_var55);
		_i_var62 = (_i_var57) * (_i_var57);
		_i_var63 = (_i_var59)+(_i_var58);
		_i_var64 = (_i_var61)+(_i_var60);
		_i_var65 = (_i_var63)+(_i_var62);
		_i_var66 = std::sqrt(_i_var64);
		_i_var67 = std::sqrt(_i_var65);
		_i_var68 = (_i_var66) * (_i_var66);
		_i_var69 = (_i_var67) * (_i_var67);
		_i_var70 = (_i_var69)+(_i_var68);
		_i_var71 = 0.5;
		_i_var72 = (_i_var71) * (_i_var70);
		energy = _i_var72;
	}
	void compute3AxisKibouRightArmEnergyFromTwoTargetGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = -(offsets(3, 0));
		_i_var9 = (_i_var6) * (_i_var5);
		_i_var10 = (_i_var6) * (_i_var0);
		_i_var11 = (_i_var7)-(offsets(3, 0));
		_i_var12 = std::cos(angles(0, 0));
		_i_var13 = (_i_var8) * (_i_var0);
		_i_var14 = -1;
		_i_var15 = (_i_var10)+(_i_var9);
		_i_var16 = (_i_var11) * (_i_var0);
		_i_var17 = (_i_var1)-(_i_var12);
		_i_var18 = (_i_var14) * (_i_var13);
		_i_var19 = std::sin(angles(1, 0));
		_i_var20 = (_i_var14) * (_i_var15);
		_i_var21 = (_i_var14) * (_i_var16);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var11) * (_i_var19);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var20);
		_i_var26 = (_i_var21) * (_i_var17);
		_i_var27 = (_i_var8) * (_i_var19);
		_i_var28 = (_i_var14) * (_i_var22);
		_i_var29 = (_i_var13) * (_i_var12);
		_i_var30 = (_i_var14) * (_i_var23);
		_i_var31 = (_i_var25) * (_i_var24);
		_i_var32 = (_i_var23) * (_i_var12);
		_i_var33 = (_i_var14) * (_i_var26);
		_i_var34 = (_i_var16) * (_i_var12);
		_i_var35 = (_i_var14) * (_i_var27);
		_i_var36 = (_i_var27) * (_i_var12);
		_i_var37 = (_i_var29)+(_i_var28);
		_i_var38 = (_i_var30) * (_i_var24);
		_i_var39 = (_i_var15) * (_i_var12);
		_i_var40 = (_i_var32)+(_i_var31);
		_i_var41 = (_i_var34)+(_i_var33);
		_i_var42 = (_i_var35) * (_i_var24);
		_i_var43 = (_i_var36)+(offsets(1, 0));
		_i_var44 = (_i_var37)+(offsets(0, 0));
		_i_var45 = (_i_var39)+(_i_var38);
		_i_var46 = (_i_var40)+(offsets(1, 0));
		_i_var47 = (_i_var41)+(offsets(0, 0));
		_i_var48 = (_i_var42)+(offsets(2, 0));
		_i_var49 = (targetElbow(1, 0)) - (_i_var43);
		_i_var50 = (targetElbow(0, 0)) - (_i_var44);
		_i_var51 = (_i_var45)+(offsets(2, 0));
		_i_var52 = (targetHand(1, 0)) - (_i_var46);
		_i_var53 = (targetHand(0, 0)) - (_i_var47);
		_i_var54 = (targetElbow(2, 0)) - (_i_var48);
		_i_var55 = (_i_var49) * (_i_var49);
		_i_var56 = (_i_var50) * (_i_var50);
		_i_var57 = (targetHand(2, 0)) - (_i_var51);
		_i_var58 = (_i_var52) * (_i_var52);
		_i_var59 = (_i_var53) * (_i_var53);
		_i_var60 = (_i_var54) * (_i_var54);
		_i_var61 = (_i_var56)+(_i_var55);
		_i_var62 = (_i_var57) * (_i_var57);
		_i_var63 = (_i_var59)+(_i_var58);
		_i_var64 = (_i_var61)+(_i_var60);
		_i_var65 = (_i_var63)+(_i_var62);
		_i_var66 = std::sqrt(_i_var64);
		_i_var67 = 2;
		_i_var68 = 0.5;
		_i_var69 = std::sqrt(_i_var65);
		_i_var70 = (_i_var67) * (_i_var66);
		_i_var71 = (_i_var68) * (_i_var66);
		_i_var72 = (_i_var67) * (_i_var69);
		_i_var73 = (_i_var68) * (_i_var69);
		_i_var74 = (_i_var1) / (_i_var70);
		_i_var75 = (_i_var67) * (_i_var71);
		_i_var76 = (_i_var1) / (_i_var72);
		_i_var77 = (_i_var67) * (_i_var73);
		_i_var78 = (_i_var75) * (_i_var74);
		_i_var79 = (_i_var77) * (_i_var76);
		_i_var80 = (_i_var78) * (_i_var50);
		_i_var81 = (_i_var79) * (_i_var53);
		_i_var82 = (_i_var79) * (_i_var52);
		_i_var83 = (_i_var67) * (_i_var80);
		_i_var84 = (_i_var67) * (_i_var81);
		_i_var85 = (_i_var78) * (_i_var49);
		_i_var86 = (_i_var79) * (_i_var57);
		_i_var87 = (_i_var67) * (_i_var82);
		_i_var88 = (_i_var83) * (_i_var14);
		_i_var89 = (_i_var84) * (_i_var14);
		_i_var90 = (_i_var67) * (_i_var85);
		_i_var91 = (_i_var67) * (_i_var86);
		_i_var92 = (_i_var87) * (_i_var14);
		_i_var93 = (_i_var88) * (_i_var14);
		_i_var94 = (_i_var89) * (_i_var14);
		_i_var95 = (_i_var78) * (_i_var54);
		_i_var96 = (_i_var90) * (_i_var14);
		_i_var97 = (_i_var91) * (_i_var14);
		_i_var98 = (_i_var92) * (_i_var24);
		_i_var99 = (_i_var93) * (_i_var17);
		_i_var100 = (_i_var94) * (_i_var17);
		_i_var101 = (_i_var67) * (_i_var95);
		_i_var102 = (_i_var96) * (_i_var27);
		_i_var103 = (_i_var97) * (_i_var15);
		_i_var104 = (_i_var98) * (_i_var14);
		_i_var105 = (_i_var99) * (_i_var14);
		_i_var106 = (_i_var88) * (_i_var12);
		_i_var107 = (_i_var100) * (_i_var14);
		_i_var108 = (_i_var89) * (_i_var12);
		_i_var109 = (_i_var101) * (_i_var14);
		_i_var110 = (_i_var97) * (_i_var24);
		_i_var111 = (_i_var89) * (_i_var16);
		_i_var112 = (_i_var103)+(_i_var102);
		_i_var113 = (_i_var104) * (_i_var14);
		_i_var114 = (_i_var97) * (_i_var12);
		_i_var115 = (_i_var106)+(_i_var105);
		_i_var116 = (_i_var108)+(_i_var107);
		_i_var117 = (_i_var109) * (_i_var24);
		_i_var118 = (_i_var110) * (_i_var14);
		_i_var119 = (_i_var92) * (_i_var12);
		_i_var120 = (_i_var93) * (_i_var18);
		_i_var121 = (_i_var94) * (_i_var21);
		_i_var122 = (_i_var92) * (_i_var23);
		_i_var123 = (_i_var112)+(_i_var111);
		_i_var124 = (_i_var114)+(_i_var113);
		_i_var125 = (_i_var115) * (_i_var8);
		_i_var126 = (_i_var116) * (_i_var11);
		_i_var127 = (_i_var117) * (_i_var14);
		_i_var128 = (_i_var96) * (_i_var12);
		_i_var129 = (_i_var119)+(_i_var118);
		_i_var130 = (_i_var121)+(_i_var120);
		_i_var131 = (_i_var88) * (_i_var13);
		_i_var132 = (_i_var123)+(_i_var122);
		_i_var133 = (_i_var97) * (_i_var30);
		_i_var134 = (_i_var109) * (_i_var35);
		_i_var135 = (_i_var124) * (_i_var6);
		_i_var136 = (_i_var126)+(_i_var125);
		_i_var137 = (_i_var128)+(_i_var127);
		_i_var138 = (_i_var124) * (_i_var5);
		_i_var139 = (_i_var124) * (_i_var0);
		_i_var140 = (_i_var116) * (_i_var0);
		_i_var141 = (_i_var129) * (_i_var19);
		_i_var142 = (_i_var130) * (_i_var14);
		_i_var143 = (_i_var132)+(_i_var131);
		_i_var144 = (_i_var92) * (_i_var25);
		_i_var145 = (_i_var134)+(_i_var133);
		_i_var146 = (_i_var135) * (_i_var14);
		_i_var147 = (_i_var136)+(_i_var135);
		_i_var148 = (_i_var129) * (_i_var11);
		_i_var149 = (_i_var137) * (_i_var8);
		_i_var150 = (_i_var139)+(_i_var138);
		_i_var151 = (_i_var141)+(_i_var140);
		_i_var152 = -(_i_var24);
		_i_var153 = (_i_var143)+(_i_var142);
		_i_var154 = (_i_var145)+(_i_var144);
		_i_var155 = -(_i_var19);
		_i_var156 = (_i_var147)+(_i_var146);
		_i_var157 = (_i_var149)+(_i_var148);
		_i_var158 = (_i_var150) * (offsets(4, 0));
		_i_var159 = -(_i_var2);
		_i_var160 = (_i_var151) * (_i_var4);
		_i_var161 = (_i_var153) * (_i_var152);
		_i_var162 = (_i_var154) * (_i_var12);
		_i_var163 = (_i_var156) * (_i_var155);
		_i_var164 = (_i_var157) * (_i_var0);
		_i_var165 = (_i_var158) * (_i_var3);
		_i_var166 = (_i_var160) * (_i_var159);
		_i_var167 = (_i_var162)+(_i_var161);
		_i_var168 = (_i_var164)+(_i_var163);
		_i_var169 = (_i_var166)+(_i_var165);
		gradient(0, 0) = _i_var167;
		gradient(1, 0) = _i_var168;
		gradient(2, 0) = _i_var169;
	}
	void compute3AxisKibouRightArmEnergyFromTwoTargetHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399, _i_var400, _i_var401, _i_var402, _i_var403, _i_var404;
		double _i_var405, _i_var406, _i_var407, _i_var408, _i_var409, _i_var410, _i_var411, _i_var412, _i_var413, _i_var414, _i_var415, _i_var416, _i_var417, _i_var418, _i_var419;
		double _i_var420, _i_var421, _i_var422, _i_var423, _i_var424, _i_var425, _i_var426, _i_var427, _i_var428, _i_var429, _i_var430, _i_var431, _i_var432, _i_var433, _i_var434;
		double _i_var435, _i_var436, _i_var437, _i_var438, _i_var439, _i_var440, _i_var441, _i_var442, _i_var443, _i_var444, _i_var445, _i_var446, _i_var447, _i_var448, _i_var449;
		double _i_var450, _i_var451, _i_var452, _i_var453, _i_var454, _i_var455, _i_var456, _i_var457, _i_var458, _i_var459, _i_var460, _i_var461, _i_var462, _i_var463, _i_var464;
		double _i_var465, _i_var466, _i_var467, _i_var468, _i_var469, _i_var470, _i_var471, _i_var472, _i_var473, _i_var474, _i_var475, _i_var476, _i_var477, _i_var478, _i_var479;
		double _i_var480, _i_var481, _i_var482, _i_var483, _i_var484, _i_var485, _i_var486, _i_var487, _i_var488, _i_var489, _i_var490, _i_var491, _i_var492, _i_var493, _i_var494;
		double _i_var495, _i_var496, _i_var497, _i_var498, _i_var499, _i_var500, _i_var501, _i_var502, _i_var503, _i_var504, _i_var505, _i_var506, _i_var507, _i_var508, _i_var509;
		double _i_var510, _i_var511, _i_var512, _i_var513, _i_var514, _i_var515, _i_var516, _i_var517, _i_var518, _i_var519, _i_var520, _i_var521, _i_var522, _i_var523, _i_var524;
		double _i_var525, _i_var526, _i_var527, _i_var528, _i_var529, _i_var530, _i_var531, _i_var532, _i_var533, _i_var534, _i_var535, _i_var536, _i_var537, _i_var538, _i_var539;
		double _i_var540, _i_var541, _i_var542, _i_var543, _i_var544, _i_var545, _i_var546, _i_var547, _i_var548, _i_var549, _i_var550, _i_var551, _i_var552, _i_var553, _i_var554;
		double _i_var555, _i_var556, _i_var557, _i_var558, _i_var559, _i_var560, _i_var561, _i_var562, _i_var563, _i_var564, _i_var565, _i_var566, _i_var567, _i_var568, _i_var569;
		double _i_var570, _i_var571, _i_var572, _i_var573, _i_var574, _i_var575, _i_var576, _i_var577, _i_var578, _i_var579, _i_var580, _i_var581;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = std::cos(angles(2, 0));
		_i_var4 = -(offsets(4, 0));
		_i_var5 = (_i_var1)-(_i_var0);
		_i_var6 = (offsets(4, 0)) * (_i_var2);
		_i_var7 = (_i_var4) * (_i_var3);
		_i_var8 = (_i_var6) * (_i_var5);
		_i_var9 = (_i_var6) * (_i_var0);
		_i_var10 = (_i_var7)-(offsets(3, 0));
		_i_var11 = -(offsets(3, 0));
		_i_var12 = (_i_var9)+(_i_var8);
		_i_var13 = -1;
		_i_var14 = std::cos(angles(0, 0));
		_i_var15 = (_i_var10) * (_i_var0);
		_i_var16 = std::sin(angles(1, 0));
		_i_var17 = (_i_var11) * (_i_var0);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var1)-(_i_var14);
		_i_var20 = (_i_var13) * (_i_var15);
		_i_var21 = -(_i_var16);
		_i_var22 = (_i_var13) * (_i_var17);
		_i_var23 = (_i_var10) * (_i_var16);
		_i_var24 = std::sin(angles(0, 0));
		_i_var25 = -(_i_var18);
		_i_var26 = (_i_var20) * (_i_var19);
		_i_var27 = (_i_var21) * (_i_var13);
		_i_var28 = (_i_var22) * (_i_var19);
		_i_var29 = (_i_var13) * (_i_var23);
		_i_var30 = (_i_var25) * (_i_var24);
		_i_var31 = (_i_var23) * (_i_var14);
		_i_var32 = (_i_var13) * (_i_var26);
		_i_var33 = (_i_var15) * (_i_var14);
		_i_var34 = (_i_var27)-(_i_var16);
		_i_var35 = -(_i_var2);
		_i_var36 = (_i_var11) * (_i_var16);
		_i_var37 = (_i_var13) * (_i_var28);
		_i_var38 = (_i_var17) * (_i_var14);
		_i_var39 = (_i_var29) * (_i_var24);
		_i_var40 = (_i_var12) * (_i_var14);
		_i_var41 = (_i_var31)+(_i_var30);
		_i_var42 = (_i_var33)+(_i_var32);
		_i_var43 = (_i_var34) * (_i_var6);
		_i_var44 = (_i_var3) * (offsets(4, 0));
		_i_var45 = (_i_var35) * (_i_var4);
		_i_var46 = (_i_var13) * (_i_var36);
		_i_var47 = (_i_var36) * (_i_var14);
		_i_var48 = (_i_var38)+(_i_var37);
		_i_var49 = (_i_var40)+(_i_var39);
		_i_var50 = (_i_var41)+(offsets(1, 0));
		_i_var51 = (_i_var42)+(offsets(0, 0));
		_i_var52 = (_i_var21) * (_i_var10);
		_i_var53 = (_i_var43) * (_i_var13);
		_i_var54 = (_i_var0) * (_i_var10);
		_i_var55 = (_i_var44) * (_i_var5);
		_i_var56 = (_i_var44) * (_i_var0);
		_i_var57 = (_i_var45) * (_i_var0);
		_i_var58 = -(_i_var24);
		_i_var59 = (_i_var46) * (_i_var24);
		_i_var60 = (_i_var47)+(offsets(1, 0));
		_i_var61 = (_i_var48)+(offsets(0, 0));
		_i_var62 = (_i_var21) * (_i_var11);
		_i_var63 = (_i_var49)+(offsets(2, 0));
		_i_var64 = (targetHand(1, 0)) - (_i_var50);
		_i_var65 = (targetHand(0, 0)) - (_i_var51);
		_i_var66 = (_i_var52) * (_i_var13);
		_i_var67 = (_i_var53) * (_i_var13);
		_i_var68 = (_i_var54) * (_i_var13);
		_i_var69 = (_i_var56)+(_i_var55);
		_i_var70 = (_i_var57) * (_i_var13);
		_i_var71 = (_i_var45) * (_i_var16);
		_i_var72 = (_i_var58) * (_i_var13);
		_i_var73 = (_i_var59)+(offsets(2, 0));
		_i_var74 = (targetElbow(1, 0)) - (_i_var60);
		_i_var75 = (targetElbow(0, 0)) - (_i_var61);
		_i_var76 = (_i_var62) * (_i_var13);
		_i_var77 = (_i_var0) * (_i_var11);
		_i_var78 = (targetHand(2, 0)) - (_i_var63);
		_i_var79 = (_i_var64) * (_i_var64);
		_i_var80 = (_i_var65) * (_i_var65);
		_i_var81 = (_i_var66) * (_i_var19);
		_i_var82 = (_i_var67) * (_i_var24);
		_i_var83 = (_i_var54) * (_i_var14);
		_i_var84 = (_i_var43) * (_i_var14);
		_i_var85 = (_i_var68) * (_i_var24);
		_i_var86 = (_i_var69) * (_i_var13);
		_i_var87 = (_i_var70) * (_i_var19);
		_i_var88 = (_i_var71) * (_i_var13);
		_i_var89 = (_i_var72) * (_i_var20);
		_i_var90 = (targetElbow(2, 0)) - (_i_var73);
		_i_var91 = (_i_var74) * (_i_var74);
		_i_var92 = (_i_var75) * (_i_var75);
		_i_var93 = (_i_var76) * (_i_var19);
		_i_var94 = (_i_var77) * (_i_var13);
		_i_var95 = (_i_var78) * (_i_var78);
		_i_var96 = (_i_var80)+(_i_var79);
		_i_var97 = (_i_var81) * (_i_var13);
		_i_var98 = (_i_var52) * (_i_var14);
		_i_var99 = (_i_var83)+(_i_var82);
		_i_var100 = (_i_var85)+(_i_var84);
		_i_var101 = (_i_var86) * (_i_var13);
		_i_var102 = (_i_var87) * (_i_var13);
		_i_var103 = (_i_var57) * (_i_var14);
		_i_var104 = (_i_var88) * (_i_var24);
		_i_var105 = (_i_var69) * (_i_var14);
		_i_var106 = (_i_var72) * (_i_var22);
		_i_var107 = (_i_var89) * (_i_var13);
		_i_var108 = (_i_var58) * (_i_var15);
		_i_var109 = (_i_var58) * (_i_var23);
		_i_var110 = (_i_var14) * (_i_var25);
		_i_var111 = (_i_var90) * (_i_var90);
		_i_var112 = (_i_var92)+(_i_var91);
		_i_var113 = (_i_var93) * (_i_var13);
		_i_var114 = (_i_var62) * (_i_var14);
		_i_var115 = (_i_var94) * (_i_var24);
		_i_var116 = (_i_var77) * (_i_var14);
		_i_var117 = (_i_var96)+(_i_var95);
		_i_var118 = (_i_var98)+(_i_var97);
		_i_var119 = 2;
		_i_var120 = (_i_var99) * (_i_var13);
		_i_var121 = (_i_var100) * (_i_var13);
		_i_var122 = (_i_var101) * (_i_var24);
		_i_var123 = (_i_var71) * (_i_var14);
		_i_var124 = (_i_var103)+(_i_var102);
		_i_var125 = (_i_var105)+(_i_var104);
		_i_var126 = (_i_var106) * (_i_var13);
		_i_var127 = (_i_var58) * (_i_var17);
		_i_var128 = (_i_var58) * (_i_var12);
		_i_var129 = (_i_var14) * (_i_var29);
		_i_var130 = (_i_var108)+(_i_var107);
		_i_var131 = (_i_var110)+(_i_var109);
		_i_var132 = (_i_var112)+(_i_var111);
		_i_var133 = (_i_var114)+(_i_var113);
		_i_var134 = (_i_var115) * (_i_var13);
		_i_var135 = (_i_var116) * (_i_var13);
		_i_var136 = std::sqrt(_i_var117);
		_i_var137 = (_i_var118) * (_i_var13);
		_i_var138 = (_i_var120) * (_i_var119);
		_i_var139 = (_i_var121) * (_i_var119);
		_i_var140 = (_i_var123)+(_i_var122);
		_i_var141 = (_i_var124) * (_i_var13);
		_i_var142 = (_i_var125) * (_i_var13);
		_i_var143 = (_i_var127)+(_i_var126);
		_i_var144 = (_i_var14) * (_i_var46);
		_i_var145 = (_i_var129)+(_i_var128);
		_i_var146 = (_i_var130) * (_i_var13);
		_i_var147 = (_i_var131) * (_i_var13);
		_i_var148 = std::sqrt(_i_var132);
		_i_var149 = (_i_var133) * (_i_var13);
		_i_var150 = (_i_var134) * (_i_var119);
		_i_var151 = (_i_var135) * (_i_var119);
		_i_var152 = (_i_var119) * (_i_var136);
		_i_var153 = 0.5;
		_i_var154 = (_i_var137) * (_i_var119);
		_i_var155 = (_i_var138) * (_i_var64);
		_i_var156 = (_i_var139) * (_i_var78);
		_i_var157 = (_i_var140) * (_i_var13);
		_i_var158 = (_i_var141) * (_i_var119);
		_i_var159 = (_i_var142) * (_i_var119);
		_i_var160 = (_i_var58) * (_i_var36);
		_i_var161 = (_i_var143) * (_i_var13);
		_i_var162 = (_i_var144) * (_i_var13);
		_i_var163 = (_i_var145) * (_i_var13);
		_i_var164 = (_i_var146) * (_i_var119);
		_i_var165 = (_i_var147) * (_i_var119);
		_i_var166 = (_i_var119) * (_i_var148);
		_i_var167 = (_i_var149) * (_i_var119);
		_i_var168 = (_i_var150) * (_i_var90);
		_i_var169 = (_i_var151) * (_i_var74);
		_i_var170 = (_i_var152) * (_i_var152);
		_i_var171 = (_i_var153) * (_i_var136);
		_i_var172 = (_i_var154) * (_i_var65);
		_i_var173 = (_i_var156)+(_i_var155);
		_i_var174 = (_i_var157) * (_i_var119);
		_i_var175 = (_i_var158) * (_i_var65);
		_i_var176 = (_i_var159) * (_i_var78);
		_i_var177 = (_i_var160) * (_i_var13);
		_i_var178 = (_i_var161) * (_i_var119);
		_i_var179 = (_i_var162) * (_i_var119);
		_i_var180 = (_i_var163) * (_i_var119);
		_i_var181 = (_i_var164) * (_i_var65);
		_i_var182 = (_i_var165) * (_i_var64);
		_i_var183 = (_i_var166) * (_i_var166);
		_i_var184 = (_i_var153) * (_i_var148);
		_i_var185 = (_i_var167) * (_i_var75);
		_i_var186 = (_i_var169)+(_i_var168);
		_i_var187 = (_i_var1) / (_i_var170);
		_i_var188 = (_i_var119) * (_i_var171);
		_i_var189 = (_i_var173)+(_i_var172);
		_i_var190 = (_i_var1) / (_i_var152);
		_i_var191 = (_i_var174) * (_i_var64);
		_i_var192 = (_i_var176)+(_i_var175);
		_i_var193 = (_i_var177) * (_i_var119);
		_i_var194 = (_i_var178) * (_i_var75);
		_i_var195 = (_i_var179) * (_i_var90);
		_i_var196 = (_i_var180) * (_i_var78);
		_i_var197 = (_i_var182)+(_i_var181);
		_i_var198 = (_i_var1) / (_i_var183);
		_i_var199 = (_i_var119) * (_i_var184);
		_i_var200 = (_i_var186)+(_i_var185);
		_i_var201 = (_i_var1) / (_i_var166);
		_i_var202 = -(_i_var187);
		_i_var203 = (_i_var189) * (_i_var188);
		_i_var204 = (_i_var189) * (_i_var190);
		_i_var205 = (_i_var192)+(_i_var191);
		_i_var206 = (_i_var193) * (_i_var74);
		_i_var207 = (_i_var195)+(_i_var194);
		_i_var208 = (_i_var197)+(_i_var196);
		_i_var209 = -(_i_var198);
		_i_var210 = (_i_var200) * (_i_var199);
		_i_var211 = (_i_var200) * (_i_var201);
		_i_var212 = (_i_var203) * (_i_var202);
		_i_var213 = (_i_var204) * (_i_var119);
		_i_var214 = (_i_var205) * (_i_var188);
		_i_var215 = (_i_var205) * (_i_var190);
		_i_var216 = (_i_var207)+(_i_var206);
		_i_var217 = (_i_var208) * (_i_var188);
		_i_var218 = (_i_var208) * (_i_var190);
		_i_var219 = (_i_var210) * (_i_var209);
		_i_var220 = (_i_var211) * (_i_var119);
		_i_var221 = (_i_var212) * (_i_var119);
		_i_var222 = (_i_var213) * (_i_var153);
		_i_var223 = (_i_var214) * (_i_var202);
		_i_var224 = (_i_var215) * (_i_var119);
		_i_var225 = (_i_var216) * (_i_var199);
		_i_var226 = (_i_var216) * (_i_var201);
		_i_var227 = (_i_var217) * (_i_var202);
		_i_var228 = (_i_var218) * (_i_var119);
		_i_var229 = (_i_var188) * (_i_var190);
		_i_var230 = (_i_var199) * (_i_var201);
		_i_var231 = (_i_var219) * (_i_var119);
		_i_var232 = (_i_var220) * (_i_var153);
		_i_var233 = (_i_var222)+(_i_var221);
		_i_var234 = (_i_var223) * (_i_var119);
		_i_var235 = (_i_var224) * (_i_var153);
		_i_var236 = (_i_var225) * (_i_var209);
		_i_var237 = (_i_var226) * (_i_var119);
		_i_var238 = (_i_var227) * (_i_var119);
		_i_var239 = (_i_var228) * (_i_var153);
		_i_var240 = (_i_var229) * (_i_var64);
		_i_var241 = (_i_var230) * (_i_var74);
		_i_var242 = (_i_var232)+(_i_var231);
		_i_var243 = (_i_var233) * (_i_var190);
		_i_var244 = (_i_var230) * (_i_var75);
		_i_var245 = (_i_var229) * (_i_var65);
		_i_var246 = (_i_var235)+(_i_var234);
		_i_var247 = (_i_var236) * (_i_var119);
		_i_var248 = (_i_var237) * (_i_var153);
		_i_var249 = (_i_var239)+(_i_var238);
		_i_var250 = (_i_var229) * (_i_var78);
		_i_var251 = (_i_var119) * (_i_var240);
		_i_var252 = (_i_var119) * (_i_var241);
		_i_var253 = (_i_var242) * (_i_var201);
		_i_var254 = (_i_var243) * (_i_var65);
		_i_var255 = (_i_var154) * (_i_var229);
		_i_var256 = (_i_var119) * (_i_var244);
		_i_var257 = (_i_var119) * (_i_var245);
		_i_var258 = (_i_var246) * (_i_var190);
		_i_var259 = (_i_var248)+(_i_var247);
		_i_var260 = (_i_var249) * (_i_var190);
		_i_var261 = (_i_var230) * (_i_var90);
		_i_var262 = (_i_var119) * (_i_var250);
		_i_var263 = (_i_var251) * (_i_var13);
		_i_var264 = (_i_var252) * (_i_var13);
		_i_var265 = (_i_var253) * (_i_var75);
		_i_var266 = (_i_var167) * (_i_var230);
		_i_var267 = (_i_var255)+(_i_var254);
		_i_var268 = (_i_var256) * (_i_var13);
		_i_var269 = (_i_var257) * (_i_var13);
		_i_var270 = (_i_var258) * (_i_var64);
		_i_var271 = (_i_var174) * (_i_var229);
		_i_var272 = (_i_var258) * (_i_var65);
		_i_var273 = (_i_var158) * (_i_var229);
		_i_var274 = (_i_var259) * (_i_var201);
		_i_var275 = (_i_var260) * (_i_var78);
		_i_var276 = (_i_var180) * (_i_var229);
		_i_var277 = (_i_var119) * (_i_var261);
		_i_var278 = (_i_var262) * (_i_var13);
		_i_var279 = (_i_var243) * (_i_var78);
		_i_var280 = (_i_var139) * (_i_var229);
		_i_var281 = (_i_var54) * (_i_var263);
		_i_var282 = (_i_var77) * (_i_var264);
		_i_var283 = (_i_var243) * (_i_var64);
		_i_var284 = (_i_var138) * (_i_var229);
		_i_var285 = (_i_var266)+(_i_var265);
		_i_var286 = (_i_var267)+(_i_var254);
		_i_var287 = (_i_var268) * (_i_var13);
		_i_var288 = (_i_var269) * (_i_var13);
		_i_var289 = (_i_var271)+(_i_var270);
		_i_var290 = (_i_var273)+(_i_var272);
		_i_var291 = (_i_var258) * (_i_var78);
		_i_var292 = (_i_var159) * (_i_var229);
		_i_var293 = (_i_var274) * (_i_var74);
		_i_var294 = (_i_var193) * (_i_var230);
		_i_var295 = (_i_var276)+(_i_var275);
		_i_var296 = (_i_var277) * (_i_var13);
		_i_var297 = (_i_var264) * (_i_var36);
		_i_var298 = (_i_var278) * (_i_var12);
		_i_var299 = (_i_var253) * (_i_var74);
		_i_var300 = (_i_var151) * (_i_var230);
		_i_var301 = (_i_var280)+(_i_var279);
		_i_var302 = (_i_var43) * (_i_var278);
		_i_var303 = (_i_var282)+(_i_var281);
		_i_var304 = (_i_var284)+(_i_var283);
		_i_var305 = (_i_var285)+(_i_var265);
		_i_var306 = (_i_var286) * (_i_var13);
		_i_var307 = (_i_var253) * (_i_var90);
		_i_var308 = (_i_var150) * (_i_var230);
		_i_var309 = (_i_var263) * (_i_var24);
		_i_var310 = (_i_var287) * (_i_var19);
		_i_var311 = (_i_var288) * (_i_var19);
		_i_var312 = (_i_var289)+(_i_var270);
		_i_var313 = (_i_var290)+(_i_var272);
		_i_var314 = (_i_var292)+(_i_var291);
		_i_var315 = (_i_var274) * (_i_var75);
		_i_var316 = (_i_var178) * (_i_var230);
		_i_var317 = (_i_var260) * (_i_var65);
		_i_var318 = (_i_var164) * (_i_var229);
		_i_var319 = (_i_var294)+(_i_var293);
		_i_var320 = (_i_var295)+(_i_var275);
		_i_var321 = (_i_var278) * (_i_var29);
		_i_var322 = (_i_var296) * (_i_var46);
		_i_var323 = (_i_var269) * (_i_var15);
		_i_var324 = (_i_var298)+(_i_var297);
		_i_var325 = (_i_var300)+(_i_var299);
		_i_var326 = (_i_var301)+(_i_var279);
		_i_var327 = (_i_var52) * (_i_var269);
		_i_var328 = (_i_var303)+(_i_var302);
		_i_var329 = (_i_var304)+(_i_var283);
		_i_var330 = (_i_var305) * (_i_var13);
		_i_var331 = (_i_var306) * (_i_var13);
		_i_var332 = (_i_var278) * (_i_var24);
		_i_var333 = (_i_var296) * (_i_var24);
		_i_var334 = (_i_var308)+(_i_var307);
		_i_var335 = (_i_var309) * (_i_var13);
		_i_var336 = (_i_var310) * (_i_var13);
		_i_var337 = (_i_var268) * (_i_var14);
		_i_var338 = (_i_var311) * (_i_var13);
		_i_var339 = (_i_var269) * (_i_var14);
		_i_var340 = (_i_var312) * (_i_var13);
		_i_var341 = (_i_var313) * (_i_var13);
		_i_var342 = (_i_var314)+(_i_var291);
		_i_var343 = (_i_var316)+(_i_var315);
		_i_var344 = (_i_var318)+(_i_var317);
		_i_var345 = (_i_var260) * (_i_var64);
		_i_var346 = (_i_var165) * (_i_var229);
		_i_var347 = (_i_var319)+(_i_var293);
		_i_var348 = (_i_var320) * (_i_var13);
		_i_var349 = (_i_var263) * (_i_var25);
		_i_var350 = (_i_var322)+(_i_var321);
		_i_var351 = (_i_var274) * (_i_var90);
		_i_var352 = (_i_var179) * (_i_var230);
		_i_var353 = (_i_var287) * (_i_var22);
		_i_var354 = (_i_var288) * (_i_var20);
		_i_var355 = (_i_var263) * (_i_var23);
		_i_var356 = (_i_var324)+(_i_var323);
		_i_var357 = (_i_var325)+(_i_var299);
		_i_var358 = (_i_var326) * (_i_var13);
		_i_var359 = (_i_var62) * (_i_var268);
		_i_var360 = (_i_var328)+(_i_var327);
		_i_var361 = (_i_var329) * (_i_var13);
		_i_var362 = (_i_var330) * (_i_var13);
		_i_var363 = (_i_var331) * (_i_var19);
		_i_var364 = (_i_var332) * (_i_var13);
		_i_var365 = (_i_var263) * (_i_var14);
		_i_var366 = (_i_var333) * (_i_var13);
		_i_var367 = (_i_var264) * (_i_var14);
		_i_var368 = (_i_var334)+(_i_var307);
		_i_var369 = (_i_var335) * (_i_var13);
		_i_var370 = (_i_var278) * (_i_var14);
		_i_var371 = (_i_var337)+(_i_var336);
		_i_var372 = (_i_var339)+(_i_var338);
		_i_var373 = (_i_var340) * (_i_var24);
		_i_var374 = (_i_var341) * (_i_var13);
		_i_var375 = (_i_var342) * (_i_var13);
		_i_var376 = (_i_var343)+(_i_var315);
		_i_var377 = (_i_var344)+(_i_var317);
		_i_var378 = (_i_var346)+(_i_var345);
		_i_var379 = (_i_var347) * (_i_var13);
		_i_var380 = (_i_var348) * (_i_var12);
		_i_var381 = (_i_var350)+(_i_var349);
		_i_var382 = (_i_var352)+(_i_var351);
		_i_var383 = (_i_var354)+(_i_var353);
		_i_var384 = (_i_var268) * (_i_var17);
		_i_var385 = (_i_var356)+(_i_var355);
		_i_var386 = (_i_var357) * (_i_var13);
		_i_var387 = (_i_var358) * (_i_var12);
		_i_var388 = (_i_var360)+(_i_var359);
		_i_var389 = (_i_var57) * (_i_var269);
		_i_var390 = (_i_var71) * (_i_var263);
		_i_var391 = (_i_var361) * (_i_var24);
		_i_var392 = (_i_var362) * (_i_var19);
		_i_var393 = (_i_var363) * (_i_var13);
		_i_var394 = (_i_var306) * (_i_var14);
		_i_var395 = (_i_var365)+(_i_var364);
		_i_var396 = (_i_var367)+(_i_var366);
		_i_var397 = (_i_var368) * (_i_var13);
		_i_var398 = (_i_var370)+(_i_var369);
		_i_var399 = (_i_var371) * (_i_var11);
		_i_var400 = (_i_var372) * (_i_var10);
		_i_var401 = (_i_var373) * (_i_var13);
		_i_var402 = (_i_var374) * (_i_var19);
		_i_var403 = (_i_var375) * (_i_var24);
		_i_var404 = (_i_var376) * (_i_var13);
		_i_var405 = (_i_var377) * (_i_var13);
		_i_var406 = (_i_var378)+(_i_var345);
		_i_var407 = (_i_var379) * (_i_var36);
		_i_var408 = (_i_var381)+(_i_var380);
		_i_var409 = (_i_var382)+(_i_var351);
		_i_var410 = (_i_var383) * (_i_var13);
		_i_var411 = (_i_var385)+(_i_var384);
		_i_var412 = (_i_var76) * (_i_var287);
		_i_var413 = (_i_var66) * (_i_var288);
		_i_var414 = (_i_var386) * (_i_var36);
		_i_var415 = (_i_var388)+(_i_var387);
		_i_var416 = (_i_var68) * (_i_var278);
		_i_var417 = (_i_var94) * (_i_var296);
		_i_var418 = (_i_var69) * (_i_var278);
		_i_var419 = (_i_var390)+(_i_var389);
		_i_var420 = (_i_var391) * (_i_var13);
		_i_var421 = (_i_var392) * (_i_var13);
		_i_var422 = (_i_var330) * (_i_var14);
		_i_var423 = (_i_var394)+(_i_var393);
		_i_var424 = (_i_var395) * (_i_var10);
		_i_var425 = (_i_var396) * (_i_var11);
		_i_var426 = (_i_var397) * (_i_var24);
		_i_var427 = (_i_var398) * (_i_var6);
		_i_var428 = (_i_var400)+(_i_var399);
		_i_var429 = (_i_var401) * (_i_var13);
		_i_var430 = (_i_var375) * (_i_var14);
		_i_var431 = (_i_var402) * (_i_var13);
		_i_var432 = (_i_var341) * (_i_var14);
		_i_var433 = (_i_var403) * (_i_var13);
		_i_var434 = (_i_var340) * (_i_var14);
		_i_var435 = (_i_var404) * (_i_var13);
		_i_var436 = (_i_var405) * (_i_var13);
		_i_var437 = (_i_var406) * (_i_var13);
		_i_var438 = (_i_var405) * (_i_var15);
		_i_var439 = (_i_var408)+(_i_var407);
		_i_var440 = (_i_var409) * (_i_var13);
		_i_var441 = (_i_var411)+(_i_var410);
		_i_var442 = (_i_var331) * (_i_var20);
		_i_var443 = (_i_var413)+(_i_var412);
		_i_var444 = (_i_var306) * (_i_var15);
		_i_var445 = (_i_var415)+(_i_var414);
		_i_var446 = (_i_var67) * (_i_var263);
		_i_var447 = (_i_var417)+(_i_var416);
		_i_var448 = (_i_var375) * (_i_var12);
		_i_var449 = (_i_var419)+(_i_var418);
		_i_var450 = (_i_var420) * (_i_var13);
		_i_var451 = (_i_var358) * (_i_var14);
		_i_var452 = (_i_var422)+(_i_var421);
		_i_var453 = (_i_var423) * (_i_var10);
		_i_var454 = (_i_var425)+(_i_var424);
		_i_var455 = (_i_var358) * (_i_var24);
		_i_var456 = (_i_var426) * (_i_var13);
		_i_var457 = (_i_var386) * (_i_var14);
		_i_var458 = (_i_var427) * (_i_var13);
		_i_var459 = (_i_var428)+(_i_var427);
		_i_var460 = (_i_var430)+(_i_var429);
		_i_var461 = (_i_var432)+(_i_var431);
		_i_var462 = (_i_var44) * (_i_var398);
		_i_var463 = (_i_var45) * (_i_var372);
		_i_var464 = (_i_var372) * (_i_var0);
		_i_var465 = (_i_var395) * (_i_var16);
		_i_var466 = (_i_var434)+(_i_var433);
		_i_var467 = (_i_var435) * (_i_var22);
		_i_var468 = (_i_var436) * (_i_var20);
		_i_var469 = (_i_var437) * (_i_var23);
		_i_var470 = (_i_var439)+(_i_var438);
		_i_var471 = (_i_var440) * (_i_var46);
		_i_var472 = (_i_var441) * (_i_var13);
		_i_var473 = (_i_var362) * (_i_var22);
		_i_var474 = (_i_var443)+(_i_var442);
		_i_var475 = (_i_var361) * (_i_var23);
		_i_var476 = (_i_var445)+(_i_var444);
		_i_var477 = (_i_var397) * (_i_var46);
		_i_var478 = (_i_var447)+(_i_var446);
		_i_var479 = (_i_var374) * (_i_var20);
		_i_var480 = (_i_var70) * (_i_var288);
		_i_var481 = (_i_var341) * (_i_var15);
		_i_var482 = (_i_var449)+(_i_var448);
		_i_var483 = (_i_var101) * (_i_var263);
		_i_var484 = (_i_var88) * (_i_var278);
		_i_var485 = (_i_var451)+(_i_var450);
		_i_var486 = (_i_var452) * (_i_var11);
		_i_var487 = (_i_var454)+(_i_var453);
		_i_var488 = (_i_var455) * (_i_var13);
		_i_var489 = (_i_var361) * (_i_var14);
		_i_var490 = (_i_var457)+(_i_var456);
		_i_var491 = (_i_var459)+(_i_var458);
		_i_var492 = (_i_var460) * (_i_var6);
		_i_var493 = (_i_var461) * (_i_var10);
		_i_var494 = (_i_var463)+(_i_var462);
		_i_var495 = (_i_var460) * (_i_var5);
		_i_var496 = (_i_var460) * (_i_var0);
		_i_var497 = (_i_var465)+(_i_var464);
		_i_var498 = (_i_var461) * (_i_var0);
		_i_var499 = (_i_var466) * (_i_var16);
		_i_var500 = (_i_var398) * (_i_var5);
		_i_var501 = (_i_var398) * (_i_var0);
		_i_var502 = (_i_var468)+(_i_var467);
		_i_var503 = (_i_var404) * (_i_var17);
		_i_var504 = (_i_var470)+(_i_var469);
		_i_var505 = (_i_var348) * (_i_var29);
		_i_var506 = (_i_var472)+(_i_var471);
		_i_var507 = (_i_var474)+(_i_var473);
		_i_var508 = (_i_var330) * (_i_var17);
		_i_var509 = (_i_var476)+(_i_var475);
		_i_var510 = (_i_var358) * (_i_var29);
		_i_var511 = (_i_var478)+(_i_var477);
		_i_var512 = (_i_var480)+(_i_var479);
		_i_var513 = (_i_var340) * (_i_var23);
		_i_var514 = (_i_var482)+(_i_var481);
		_i_var515 = (_i_var375) * (_i_var29);
		_i_var516 = (_i_var484)+(_i_var483);
		_i_var517 = (_i_var485) * (_i_var6);
		_i_var518 = (_i_var487)+(_i_var486);
		_i_var519 = (_i_var489)+(_i_var488);
		_i_var520 = (_i_var490) * (_i_var11);
		_i_var521 = (_i_var491) * (_i_var13);
		_i_var522 = (_i_var462)+(_i_var492);
		_i_var523 = (_i_var494)+(_i_var493);
		_i_var524 = (_i_var496)+(_i_var495);
		_i_var525 = (_i_var497) * (_i_var4);
		_i_var526 = (_i_var499)+(_i_var498);
		_i_var527 = (_i_var501)+(_i_var500);
		_i_var528 = (_i_var502) * (_i_var13);
		_i_var529 = (_i_var504)+(_i_var503);
		_i_var530 = (_i_var437) * (_i_var25);
		_i_var531 = (_i_var506)+(_i_var505);
		_i_var532 = (_i_var507) * (_i_var13);
		_i_var533 = (_i_var509)+(_i_var508);
		_i_var534 = (_i_var361) * (_i_var25);
		_i_var535 = (_i_var511)+(_i_var510);
		_i_var536 = (_i_var512) * (_i_var13);
		_i_var537 = (_i_var514)+(_i_var513);
		_i_var538 = (_i_var340) * (_i_var25);
		_i_var539 = (_i_var516)+(_i_var515);
		_i_var540 = (_i_var517) * (_i_var13);
		_i_var541 = (_i_var518)+(_i_var517);
		_i_var542 = (_i_var519) * (_i_var10);
		_i_var543 = (_i_var521)+(_i_var520);
		_i_var544 = (_i_var522) * (_i_var13);
		_i_var545 = (_i_var523)+(_i_var492);
		_i_var546 = (_i_var466) * (_i_var10);
		_i_var547 = (_i_var45) * (_i_var395);
		_i_var548 = (_i_var524) * (offsets(4, 0));
		_i_var549 = (_i_var525) * (_i_var13);
		_i_var550 = (_i_var526) * (_i_var4);
		_i_var551 = (_i_var527) * (offsets(4, 0));
		_i_var552 = (_i_var529)+(_i_var528);
		_i_var553 = (_i_var531)+(_i_var530);
		_i_var554 = (_i_var533)+(_i_var532);
		_i_var555 = (_i_var535)+(_i_var534);
		_i_var556 = (_i_var537)+(_i_var536);
		_i_var557 = (_i_var539)+(_i_var538);
		_i_var558 = (_i_var541)+(_i_var540);
		_i_var559 = (_i_var543)+(_i_var542);
		_i_var560 = (_i_var545)+(_i_var544);
		_i_var561 = (_i_var547)+(_i_var546);
		_i_var562 = (_i_var549)+(_i_var548);
		_i_var563 = (_i_var551)+(_i_var550);
		_i_var564 = (_i_var552) * (_i_var58);
		_i_var565 = (_i_var553) * (_i_var14);
		_i_var566 = (_i_var554) * (_i_var58);
		_i_var567 = (_i_var555) * (_i_var14);
		_i_var568 = (_i_var556) * (_i_var58);
		_i_var569 = (_i_var557) * (_i_var14);
		_i_var570 = (_i_var558) * (_i_var21);
		_i_var571 = (_i_var559) * (_i_var0);
		_i_var572 = (_i_var560) * (_i_var21);
		_i_var573 = (_i_var561) * (_i_var0);
		_i_var574 = (_i_var562) * (_i_var3);
		_i_var575 = (_i_var563) * (_i_var35);
		_i_var576 = (_i_var565)+(_i_var564);
		_i_var577 = (_i_var567)+(_i_var566);
		_i_var578 = (_i_var569)+(_i_var568);
		_i_var579 = (_i_var571)+(_i_var570);
		_i_var580 = (_i_var573)+(_i_var572);
		_i_var581 = (_i_var575)+(_i_var574);
		hessian(0, 0) = _i_var576;
		hessian(1, 0) = _i_var577;
		hessian(2, 0) = _i_var578;
		hessian(0, 1) = _i_var577;
		hessian(1, 1) = _i_var579;
		hessian(2, 1) = _i_var580;
		hessian(0, 2) = _i_var578;
		hessian(1, 2) = _i_var580;
		hessian(2, 2) = _i_var581;
	}

}

namespace Codegen
{
	void compute3AxisKibouLeftArmEnergyFromTwoTarget(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, double& energy) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = std::cos(angles(0, 0));
		_i_var11 = (_i_var8)+(_i_var7);
		_i_var12 = std::sin(angles(1, 0));
		_i_var13 = (_i_var9)+(offsets(3, 0));
		_i_var14 = (_i_var1)-(_i_var10);
		_i_var15 = (offsets(3, 0)) * (_i_var0);
		_i_var16 = std::sin(angles(0, 0));
		_i_var17 = -(_i_var11);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var13) * (_i_var0);
		_i_var20 = (offsets(3, 0)) * (_i_var12);
		_i_var21 = (_i_var15) * (_i_var14);
		_i_var22 = (_i_var15) * (_i_var10);
		_i_var23 = (_i_var17) * (_i_var16);
		_i_var24 = (_i_var18) * (_i_var10);
		_i_var25 = (_i_var19) * (_i_var14);
		_i_var26 = (_i_var19) * (_i_var10);
		_i_var27 = (_i_var20) * (_i_var10);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var18) * (_i_var16);
		_i_var30 = (_i_var11) * (_i_var10);
		_i_var31 = (_i_var24)+(_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var20) * (_i_var16);
		_i_var34 = (_i_var27)+(offsets(1, 0));
		_i_var35 = (_i_var28)+(offsets(0, 0));
		_i_var36 = (_i_var30)+(_i_var29);
		_i_var37 = (_i_var31)+(offsets(1, 0));
		_i_var38 = (_i_var32)+(offsets(0, 0));
		_i_var39 = (_i_var33)+(offsets(2, 0));
		_i_var40 = (targetElbow(1, 0)) - (_i_var34);
		_i_var41 = (targetElbow(0, 0)) - (_i_var35);
		_i_var42 = (_i_var36)+(offsets(2, 0));
		_i_var43 = (targetHand(1, 0)) - (_i_var37);
		_i_var44 = (targetHand(0, 0)) - (_i_var38);
		_i_var45 = (targetElbow(2, 0)) - (_i_var39);
		_i_var46 = (_i_var40) * (_i_var40);
		_i_var47 = (_i_var41) * (_i_var41);
		_i_var48 = (targetHand(2, 0)) - (_i_var42);
		_i_var49 = (_i_var43) * (_i_var43);
		_i_var50 = (_i_var44) * (_i_var44);
		_i_var51 = (_i_var45) * (_i_var45);
		_i_var52 = (_i_var47)+(_i_var46);
		_i_var53 = (_i_var48) * (_i_var48);
		_i_var54 = (_i_var50)+(_i_var49);
		_i_var55 = (_i_var52)+(_i_var51);
		_i_var56 = (_i_var54)+(_i_var53);
		_i_var57 = std::sqrt(_i_var55);
		_i_var58 = std::sqrt(_i_var56);
		_i_var59 = (_i_var57) * (_i_var57);
		_i_var60 = (_i_var58) * (_i_var58);
		_i_var61 = (_i_var60)+(_i_var59);
		_i_var62 = 0.5;
		_i_var63 = (_i_var62) * (_i_var61);
		energy = _i_var63;
	}
	void compute3AxisKibouLeftArmEnergyFromTwoTargetGradient(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 1>& gradient) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = std::cos(angles(0, 0));
		_i_var11 = (_i_var8)+(_i_var7);
		_i_var12 = std::sin(angles(1, 0));
		_i_var13 = (_i_var9)+(offsets(3, 0));
		_i_var14 = (_i_var1)-(_i_var10);
		_i_var15 = (offsets(3, 0)) * (_i_var0);
		_i_var16 = std::sin(angles(0, 0));
		_i_var17 = -(_i_var11);
		_i_var18 = (_i_var13) * (_i_var12);
		_i_var19 = (_i_var13) * (_i_var0);
		_i_var20 = (offsets(3, 0)) * (_i_var12);
		_i_var21 = (_i_var15) * (_i_var14);
		_i_var22 = (_i_var15) * (_i_var10);
		_i_var23 = (_i_var17) * (_i_var16);
		_i_var24 = (_i_var18) * (_i_var10);
		_i_var25 = (_i_var19) * (_i_var14);
		_i_var26 = (_i_var19) * (_i_var10);
		_i_var27 = (_i_var20) * (_i_var10);
		_i_var28 = (_i_var22)+(_i_var21);
		_i_var29 = (_i_var18) * (_i_var16);
		_i_var30 = (_i_var11) * (_i_var10);
		_i_var31 = (_i_var24)+(_i_var23);
		_i_var32 = (_i_var26)+(_i_var25);
		_i_var33 = (_i_var20) * (_i_var16);
		_i_var34 = (_i_var27)+(offsets(1, 0));
		_i_var35 = (_i_var28)+(offsets(0, 0));
		_i_var36 = (_i_var30)+(_i_var29);
		_i_var37 = (_i_var31)+(offsets(1, 0));
		_i_var38 = (_i_var32)+(offsets(0, 0));
		_i_var39 = (_i_var33)+(offsets(2, 0));
		_i_var40 = (targetElbow(1, 0)) - (_i_var34);
		_i_var41 = (targetElbow(0, 0)) - (_i_var35);
		_i_var42 = (_i_var36)+(offsets(2, 0));
		_i_var43 = (targetHand(1, 0)) - (_i_var37);
		_i_var44 = (targetHand(0, 0)) - (_i_var38);
		_i_var45 = (targetElbow(2, 0)) - (_i_var39);
		_i_var46 = (_i_var40) * (_i_var40);
		_i_var47 = (_i_var41) * (_i_var41);
		_i_var48 = (targetHand(2, 0)) - (_i_var42);
		_i_var49 = (_i_var43) * (_i_var43);
		_i_var50 = (_i_var44) * (_i_var44);
		_i_var51 = (_i_var45) * (_i_var45);
		_i_var52 = (_i_var47)+(_i_var46);
		_i_var53 = (_i_var48) * (_i_var48);
		_i_var54 = (_i_var50)+(_i_var49);
		_i_var55 = (_i_var52)+(_i_var51);
		_i_var56 = (_i_var54)+(_i_var53);
		_i_var57 = std::sqrt(_i_var55);
		_i_var58 = 2;
		_i_var59 = 0.5;
		_i_var60 = std::sqrt(_i_var56);
		_i_var61 = (_i_var58) * (_i_var57);
		_i_var62 = (_i_var59) * (_i_var57);
		_i_var63 = (_i_var58) * (_i_var60);
		_i_var64 = (_i_var59) * (_i_var60);
		_i_var65 = (_i_var1) / (_i_var61);
		_i_var66 = (_i_var58) * (_i_var62);
		_i_var67 = (_i_var1) / (_i_var63);
		_i_var68 = (_i_var58) * (_i_var64);
		_i_var69 = (_i_var66) * (_i_var65);
		_i_var70 = (_i_var68) * (_i_var67);
		_i_var71 = (_i_var69) * (_i_var40);
		_i_var72 = (_i_var70) * (_i_var48);
		_i_var73 = (_i_var70) * (_i_var43);
		_i_var74 = (_i_var69) * (_i_var41);
		_i_var75 = (_i_var70) * (_i_var44);
		_i_var76 = -1;
		_i_var77 = (_i_var58) * (_i_var71);
		_i_var78 = (_i_var58) * (_i_var72);
		_i_var79 = (_i_var58) * (_i_var73);
		_i_var80 = (_i_var58) * (_i_var74);
		_i_var81 = (_i_var58) * (_i_var75);
		_i_var82 = (_i_var77) * (_i_var76);
		_i_var83 = (_i_var78) * (_i_var76);
		_i_var84 = (_i_var79) * (_i_var76);
		_i_var85 = (_i_var80) * (_i_var76);
		_i_var86 = (_i_var81) * (_i_var76);
		_i_var87 = (_i_var69) * (_i_var45);
		_i_var88 = (_i_var82) * (_i_var20);
		_i_var89 = (_i_var83) * (_i_var11);
		_i_var90 = (_i_var84) * (_i_var16);
		_i_var91 = (_i_var85) * (_i_var14);
		_i_var92 = (_i_var85) * (_i_var10);
		_i_var93 = (_i_var86) * (_i_var14);
		_i_var94 = (_i_var86) * (_i_var10);
		_i_var95 = (_i_var58) * (_i_var87);
		_i_var96 = (_i_var86) * (_i_var19);
		_i_var97 = (_i_var89)+(_i_var88);
		_i_var98 = (_i_var90) * (_i_var76);
		_i_var99 = (_i_var83) * (_i_var10);
		_i_var100 = (_i_var92)+(_i_var91);
		_i_var101 = (_i_var94)+(_i_var93);
		_i_var102 = (_i_var95) * (_i_var76);
		_i_var103 = (_i_var84) * (_i_var10);
		_i_var104 = (_i_var83) * (_i_var16);
		_i_var105 = (_i_var85) * (_i_var15);
		_i_var106 = (_i_var84) * (_i_var18);
		_i_var107 = (_i_var97)+(_i_var96);
		_i_var108 = (_i_var99)+(_i_var98);
		_i_var109 = (_i_var100) * (offsets(3, 0));
		_i_var110 = (_i_var101) * (_i_var13);
		_i_var111 = (_i_var82) * (_i_var10);
		_i_var112 = (_i_var102) * (_i_var16);
		_i_var113 = (_i_var104)+(_i_var103);
		_i_var114 = (_i_var96)+(_i_var105);
		_i_var115 = (_i_var107)+(_i_var106);
		_i_var116 = (_i_var83) * (_i_var18);
		_i_var117 = (_i_var102) * (_i_var20);
		_i_var118 = (_i_var108) * (_i_var5);
		_i_var119 = (_i_var110)+(_i_var109);
		_i_var120 = (_i_var112)+(_i_var111);
		_i_var121 = (_i_var108) * (_i_var4);
		_i_var122 = (_i_var108) * (_i_var0);
		_i_var123 = (_i_var113) * (_i_var12);
		_i_var124 = (_i_var101) * (_i_var0);
		_i_var125 = (_i_var114) * (_i_var76);
		_i_var126 = (_i_var115)+(_i_var105);
		_i_var127 = (_i_var84) * (_i_var17);
		_i_var128 = (_i_var117)+(_i_var116);
		_i_var129 = (_i_var118) * (_i_var76);
		_i_var130 = (_i_var119)+(_i_var118);
		_i_var131 = (_i_var113) * (_i_var13);
		_i_var132 = (_i_var120) * (offsets(3, 0));
		_i_var133 = (_i_var122)+(_i_var121);
		_i_var134 = (_i_var124)+(_i_var123);
		_i_var135 = -(_i_var16);
		_i_var136 = (_i_var126)+(_i_var125);
		_i_var137 = (_i_var128)+(_i_var127);
		_i_var138 = -(_i_var12);
		_i_var139 = (_i_var130)+(_i_var129);
		_i_var140 = (_i_var132)+(_i_var131);
		_i_var141 = (_i_var133) * (_i_var3);
		_i_var142 = -(_i_var2);
		_i_var143 = (_i_var134) * (offsets(4, 0));
		_i_var144 = (_i_var136) * (_i_var135);
		_i_var145 = (_i_var137) * (_i_var10);
		_i_var146 = (_i_var139) * (_i_var138);
		_i_var147 = (_i_var140) * (_i_var0);
		_i_var148 = (_i_var141) * (_i_var6);
		_i_var149 = (_i_var143) * (_i_var142);
		_i_var150 = (_i_var145)+(_i_var144);
		_i_var151 = (_i_var147)+(_i_var146);
		_i_var152 = (_i_var149)+(_i_var148);
		gradient(0, 0) = _i_var150;
		gradient(1, 0) = _i_var151;
		gradient(2, 0) = _i_var152;
	}
	void compute3AxisKibouLeftArmEnergyFromTwoTargetHessian(const Eigen::Matrix<double, 5, 1>& offsets, const Eigen::Matrix<double, 3, 1>& angles, const Eigen::Matrix<double, 3, 1>& targetHand, const Eigen::Matrix<double, 3, 1>& targetElbow, Eigen::Matrix<double, 3, 3>& hessian) {
		double _i_var0, _i_var1, _i_var2, _i_var3, _i_var4, _i_var5, _i_var6, _i_var7, _i_var8, _i_var9, _i_var10, _i_var11, _i_var12, _i_var13, _i_var14;
		double _i_var15, _i_var16, _i_var17, _i_var18, _i_var19, _i_var20, _i_var21, _i_var22, _i_var23, _i_var24, _i_var25, _i_var26, _i_var27, _i_var28, _i_var29;
		double _i_var30, _i_var31, _i_var32, _i_var33, _i_var34, _i_var35, _i_var36, _i_var37, _i_var38, _i_var39, _i_var40, _i_var41, _i_var42, _i_var43, _i_var44;
		double _i_var45, _i_var46, _i_var47, _i_var48, _i_var49, _i_var50, _i_var51, _i_var52, _i_var53, _i_var54, _i_var55, _i_var56, _i_var57, _i_var58, _i_var59;
		double _i_var60, _i_var61, _i_var62, _i_var63, _i_var64, _i_var65, _i_var66, _i_var67, _i_var68, _i_var69, _i_var70, _i_var71, _i_var72, _i_var73, _i_var74;
		double _i_var75, _i_var76, _i_var77, _i_var78, _i_var79, _i_var80, _i_var81, _i_var82, _i_var83, _i_var84, _i_var85, _i_var86, _i_var87, _i_var88, _i_var89;
		double _i_var90, _i_var91, _i_var92, _i_var93, _i_var94, _i_var95, _i_var96, _i_var97, _i_var98, _i_var99, _i_var100, _i_var101, _i_var102, _i_var103, _i_var104;
		double _i_var105, _i_var106, _i_var107, _i_var108, _i_var109, _i_var110, _i_var111, _i_var112, _i_var113, _i_var114, _i_var115, _i_var116, _i_var117, _i_var118, _i_var119;
		double _i_var120, _i_var121, _i_var122, _i_var123, _i_var124, _i_var125, _i_var126, _i_var127, _i_var128, _i_var129, _i_var130, _i_var131, _i_var132, _i_var133, _i_var134;
		double _i_var135, _i_var136, _i_var137, _i_var138, _i_var139, _i_var140, _i_var141, _i_var142, _i_var143, _i_var144, _i_var145, _i_var146, _i_var147, _i_var148, _i_var149;
		double _i_var150, _i_var151, _i_var152, _i_var153, _i_var154, _i_var155, _i_var156, _i_var157, _i_var158, _i_var159, _i_var160, _i_var161, _i_var162, _i_var163, _i_var164;
		double _i_var165, _i_var166, _i_var167, _i_var168, _i_var169, _i_var170, _i_var171, _i_var172, _i_var173, _i_var174, _i_var175, _i_var176, _i_var177, _i_var178, _i_var179;
		double _i_var180, _i_var181, _i_var182, _i_var183, _i_var184, _i_var185, _i_var186, _i_var187, _i_var188, _i_var189, _i_var190, _i_var191, _i_var192, _i_var193, _i_var194;
		double _i_var195, _i_var196, _i_var197, _i_var198, _i_var199, _i_var200, _i_var201, _i_var202, _i_var203, _i_var204, _i_var205, _i_var206, _i_var207, _i_var208, _i_var209;
		double _i_var210, _i_var211, _i_var212, _i_var213, _i_var214, _i_var215, _i_var216, _i_var217, _i_var218, _i_var219, _i_var220, _i_var221, _i_var222, _i_var223, _i_var224;
		double _i_var225, _i_var226, _i_var227, _i_var228, _i_var229, _i_var230, _i_var231, _i_var232, _i_var233, _i_var234, _i_var235, _i_var236, _i_var237, _i_var238, _i_var239;
		double _i_var240, _i_var241, _i_var242, _i_var243, _i_var244, _i_var245, _i_var246, _i_var247, _i_var248, _i_var249, _i_var250, _i_var251, _i_var252, _i_var253, _i_var254;
		double _i_var255, _i_var256, _i_var257, _i_var258, _i_var259, _i_var260, _i_var261, _i_var262, _i_var263, _i_var264, _i_var265, _i_var266, _i_var267, _i_var268, _i_var269;
		double _i_var270, _i_var271, _i_var272, _i_var273, _i_var274, _i_var275, _i_var276, _i_var277, _i_var278, _i_var279, _i_var280, _i_var281, _i_var282, _i_var283, _i_var284;
		double _i_var285, _i_var286, _i_var287, _i_var288, _i_var289, _i_var290, _i_var291, _i_var292, _i_var293, _i_var294, _i_var295, _i_var296, _i_var297, _i_var298, _i_var299;
		double _i_var300, _i_var301, _i_var302, _i_var303, _i_var304, _i_var305, _i_var306, _i_var307, _i_var308, _i_var309, _i_var310, _i_var311, _i_var312, _i_var313, _i_var314;
		double _i_var315, _i_var316, _i_var317, _i_var318, _i_var319, _i_var320, _i_var321, _i_var322, _i_var323, _i_var324, _i_var325, _i_var326, _i_var327, _i_var328, _i_var329;
		double _i_var330, _i_var331, _i_var332, _i_var333, _i_var334, _i_var335, _i_var336, _i_var337, _i_var338, _i_var339, _i_var340, _i_var341, _i_var342, _i_var343, _i_var344;
		double _i_var345, _i_var346, _i_var347, _i_var348, _i_var349, _i_var350, _i_var351, _i_var352, _i_var353, _i_var354, _i_var355, _i_var356, _i_var357, _i_var358, _i_var359;
		double _i_var360, _i_var361, _i_var362, _i_var363, _i_var364, _i_var365, _i_var366, _i_var367, _i_var368, _i_var369, _i_var370, _i_var371, _i_var372, _i_var373, _i_var374;
		double _i_var375, _i_var376, _i_var377, _i_var378, _i_var379, _i_var380, _i_var381, _i_var382, _i_var383, _i_var384, _i_var385, _i_var386, _i_var387, _i_var388, _i_var389;
		double _i_var390, _i_var391, _i_var392, _i_var393, _i_var394, _i_var395, _i_var396, _i_var397, _i_var398, _i_var399, _i_var400, _i_var401, _i_var402, _i_var403, _i_var404;
		double _i_var405, _i_var406, _i_var407, _i_var408, _i_var409, _i_var410, _i_var411, _i_var412, _i_var413, _i_var414, _i_var415, _i_var416, _i_var417, _i_var418, _i_var419;
		double _i_var420, _i_var421, _i_var422, _i_var423, _i_var424, _i_var425, _i_var426, _i_var427, _i_var428, _i_var429, _i_var430, _i_var431, _i_var432, _i_var433, _i_var434;
		double _i_var435, _i_var436, _i_var437, _i_var438, _i_var439, _i_var440, _i_var441, _i_var442, _i_var443, _i_var444, _i_var445, _i_var446, _i_var447, _i_var448, _i_var449;
		double _i_var450, _i_var451, _i_var452, _i_var453, _i_var454, _i_var455, _i_var456, _i_var457, _i_var458, _i_var459, _i_var460, _i_var461, _i_var462, _i_var463, _i_var464;
		double _i_var465, _i_var466, _i_var467, _i_var468, _i_var469, _i_var470, _i_var471, _i_var472, _i_var473, _i_var474, _i_var475, _i_var476, _i_var477, _i_var478, _i_var479;
		double _i_var480, _i_var481, _i_var482, _i_var483, _i_var484, _i_var485, _i_var486, _i_var487, _i_var488, _i_var489, _i_var490, _i_var491, _i_var492, _i_var493, _i_var494;
		double _i_var495, _i_var496, _i_var497, _i_var498, _i_var499, _i_var500, _i_var501, _i_var502, _i_var503, _i_var504, _i_var505, _i_var506, _i_var507, _i_var508, _i_var509;
		double _i_var510, _i_var511, _i_var512, _i_var513, _i_var514, _i_var515, _i_var516, _i_var517, _i_var518, _i_var519, _i_var520, _i_var521, _i_var522, _i_var523, _i_var524;
		double _i_var525, _i_var526, _i_var527;
		_i_var0 = std::cos(angles(1, 0));
		_i_var1 = 1;
		_i_var2 = std::sin(angles(2, 0));
		_i_var3 = -(offsets(4, 0));
		_i_var4 = (_i_var1)-(_i_var0);
		_i_var5 = (_i_var3) * (_i_var2);
		_i_var6 = std::cos(angles(2, 0));
		_i_var7 = (_i_var5) * (_i_var4);
		_i_var8 = (_i_var5) * (_i_var0);
		_i_var9 = (offsets(4, 0)) * (_i_var6);
		_i_var10 = (_i_var8)+(_i_var7);
		_i_var11 = std::sin(angles(1, 0));
		_i_var12 = (_i_var9)+(offsets(3, 0));
		_i_var13 = std::cos(angles(0, 0));
		_i_var14 = std::sin(angles(0, 0));
		_i_var15 = -(_i_var10);
		_i_var16 = (_i_var12) * (_i_var11);
		_i_var17 = (_i_var1)-(_i_var13);
		_i_var18 = (_i_var12) * (_i_var0);
		_i_var19 = (offsets(3, 0)) * (_i_var0);
		_i_var20 = (_i_var15) * (_i_var14);
		_i_var21 = (_i_var16) * (_i_var13);
		_i_var22 = (_i_var18) * (_i_var17);
		_i_var23 = (_i_var18) * (_i_var13);
		_i_var24 = (offsets(3, 0)) * (_i_var11);
		_i_var25 = (_i_var19) * (_i_var17);
		_i_var26 = (_i_var19) * (_i_var13);
		_i_var27 = (_i_var16) * (_i_var14);
		_i_var28 = (_i_var10) * (_i_var13);
		_i_var29 = (_i_var21)+(_i_var20);
		_i_var30 = (_i_var23)+(_i_var22);
		_i_var31 = -1;
		_i_var32 = -(_i_var11);
		_i_var33 = (_i_var24) * (_i_var13);
		_i_var34 = (_i_var26)+(_i_var25);
		_i_var35 = (_i_var6) * (_i_var3);
		_i_var36 = -(_i_var2);
		_i_var37 = (_i_var28)+(_i_var27);
		_i_var38 = (_i_var29)+(offsets(1, 0));
		_i_var39 = (_i_var30)+(offsets(0, 0));
		_i_var40 = -(_i_var14);
		_i_var41 = (_i_var32) * (_i_var31);
		_i_var42 = (_i_var24) * (_i_var14);
		_i_var43 = (_i_var33)+(offsets(1, 0));
		_i_var44 = (_i_var34)+(offsets(0, 0));
		_i_var45 = (_i_var35) * (_i_var4);
		_i_var46 = (_i_var35) * (_i_var0);
		_i_var47 = (_i_var36) * (offsets(4, 0));
		_i_var48 = (_i_var37)+(offsets(2, 0));
		_i_var49 = (targetHand(1, 0)) - (_i_var38);
		_i_var50 = (targetHand(0, 0)) - (_i_var39);
		_i_var51 = (_i_var40) * (_i_var31);
		_i_var52 = (_i_var41)-(_i_var11);
		_i_var53 = (_i_var42)+(offsets(2, 0));
		_i_var54 = (targetElbow(1, 0)) - (_i_var43);
		_i_var55 = (targetElbow(0, 0)) - (_i_var44);
		_i_var56 = (_i_var46)+(_i_var45);
		_i_var57 = (_i_var47) * (_i_var11);
		_i_var58 = (_i_var47) * (_i_var0);
		_i_var59 = (targetHand(2, 0)) - (_i_var48);
		_i_var60 = (_i_var49) * (_i_var49);
		_i_var61 = (_i_var50) * (_i_var50);
		_i_var62 = (_i_var51)-(_i_var14);
		_i_var63 = (_i_var40) * (_i_var16);
		_i_var64 = (_i_var13) * (_i_var15);
		_i_var65 = (_i_var52) * (_i_var5);
		_i_var66 = (_i_var32) * (_i_var12);
		_i_var67 = (_i_var0) * (_i_var12);
		_i_var68 = (targetElbow(2, 0)) - (_i_var53);
		_i_var69 = (_i_var54) * (_i_var54);
		_i_var70 = (_i_var55) * (_i_var55);
		_i_var71 = (_i_var32) * (offsets(3, 0));
		_i_var72 = (_i_var0) * (offsets(3, 0));
		_i_var73 = (_i_var56) * (_i_var31);
		_i_var74 = (_i_var56) * (_i_var13);
		_i_var75 = (_i_var57) * (_i_var14);
		_i_var76 = (_i_var58) * (_i_var17);
		_i_var77 = (_i_var58) * (_i_var13);
		_i_var78 = (_i_var59) * (_i_var59);
		_i_var79 = (_i_var61)+(_i_var60);
		_i_var80 = (_i_var40) * (_i_var10);
		_i_var81 = (_i_var13) * (_i_var16);
		_i_var82 = (_i_var62) * (_i_var18);
		_i_var83 = (_i_var64)+(_i_var63);
		_i_var84 = (_i_var65) * (_i_var31);
		_i_var85 = (_i_var66) * (_i_var17);
		_i_var86 = (_i_var66) * (_i_var13);
		_i_var87 = (_i_var65) * (_i_var13);
		_i_var88 = (_i_var67) * (_i_var14);
		_i_var89 = (_i_var68) * (_i_var68);
		_i_var90 = (_i_var70)+(_i_var69);
		_i_var91 = (_i_var71) * (_i_var17);
		_i_var92 = (_i_var71) * (_i_var13);
		_i_var93 = (_i_var72) * (_i_var13);
		_i_var94 = (_i_var72) * (_i_var14);
		_i_var95 = (_i_var73) * (_i_var14);
		_i_var96 = (_i_var57) * (_i_var13);
		_i_var97 = (_i_var75)+(_i_var74);
		_i_var98 = (_i_var77)+(_i_var76);
		_i_var99 = (_i_var62) * (_i_var19);
		_i_var100 = (_i_var13) * (_i_var24);
		_i_var101 = (_i_var79)+(_i_var78);
		_i_var102 = (_i_var81)+(_i_var80);
		_i_var103 = 2;
		_i_var104 = (_i_var82) * (_i_var31);
		_i_var105 = (_i_var83) * (_i_var31);
		_i_var106 = (_i_var84) * (_i_var14);
		_i_var107 = (_i_var67) * (_i_var13);
		_i_var108 = (_i_var86)+(_i_var85);
		_i_var109 = (_i_var88)+(_i_var87);
		_i_var110 = (_i_var90)+(_i_var89);
		_i_var111 = (_i_var92)+(_i_var91);
		_i_var112 = (_i_var93) * (_i_var31);
		_i_var113 = (_i_var94) * (_i_var31);
		_i_var114 = (_i_var96)+(_i_var95);
		_i_var115 = (_i_var97) * (_i_var31);
		_i_var116 = (_i_var98) * (_i_var31);
		_i_var117 = (_i_var40) * (_i_var24);
		_i_var118 = (_i_var99) * (_i_var31);
		_i_var119 = (_i_var100) * (_i_var31);
		_i_var120 = std::sqrt(_i_var101);
		_i_var121 = (_i_var102) * (_i_var31);
		_i_var122 = (_i_var104) * (_i_var103);
		_i_var123 = (_i_var105) * (_i_var103);
		_i_var124 = (_i_var107)+(_i_var106);
		_i_var125 = (_i_var108) * (_i_var31);
		_i_var126 = (_i_var109) * (_i_var31);
		_i_var127 = std::sqrt(_i_var110);
		_i_var128 = (_i_var111) * (_i_var31);
		_i_var129 = (_i_var112) * (_i_var103);
		_i_var130 = (_i_var113) * (_i_var103);
		_i_var131 = (_i_var114) * (_i_var31);
		_i_var132 = (_i_var115) * (_i_var103);
		_i_var133 = (_i_var116) * (_i_var103);
		_i_var134 = (_i_var117) * (_i_var31);
		_i_var135 = (_i_var118) * (_i_var103);
		_i_var136 = (_i_var119) * (_i_var103);
		_i_var137 = (_i_var103) * (_i_var120);
		_i_var138 = 0.5;
		_i_var139 = (_i_var121) * (_i_var103);
		_i_var140 = (_i_var122) * (_i_var50);
		_i_var141 = (_i_var123) * (_i_var49);
		_i_var142 = (_i_var124) * (_i_var31);
		_i_var143 = (_i_var125) * (_i_var103);
		_i_var144 = (_i_var126) * (_i_var103);
		_i_var145 = (_i_var103) * (_i_var127);
		_i_var146 = (_i_var128) * (_i_var103);
		_i_var147 = (_i_var129) * (_i_var54);
		_i_var148 = (_i_var130) * (_i_var68);
		_i_var149 = (_i_var131) * (_i_var103);
		_i_var150 = (_i_var132) * (_i_var59);
		_i_var151 = (_i_var133) * (_i_var50);
		_i_var152 = (_i_var134) * (_i_var103);
		_i_var153 = (_i_var135) * (_i_var55);
		_i_var154 = (_i_var136) * (_i_var68);
		_i_var155 = (_i_var137) * (_i_var137);
		_i_var156 = (_i_var138) * (_i_var120);
		_i_var157 = (_i_var139) * (_i_var59);
		_i_var158 = (_i_var141)+(_i_var140);
		_i_var159 = (_i_var142) * (_i_var103);
		_i_var160 = (_i_var143) * (_i_var50);
		_i_var161 = (_i_var144) * (_i_var59);
		_i_var162 = (_i_var145) * (_i_var145);
		_i_var163 = (_i_var138) * (_i_var127);
		_i_var164 = (_i_var146) * (_i_var55);
		_i_var165 = (_i_var148)+(_i_var147);
		_i_var166 = (_i_var149) * (_i_var49);
		_i_var167 = (_i_var151)+(_i_var150);
		_i_var168 = (_i_var152) * (_i_var54);
		_i_var169 = (_i_var154)+(_i_var153);
		_i_var170 = (_i_var1) / (_i_var155);
		_i_var171 = (_i_var103) * (_i_var156);
		_i_var172 = (_i_var158)+(_i_var157);
		_i_var173 = (_i_var1) / (_i_var137);
		_i_var174 = (_i_var159) * (_i_var49);
		_i_var175 = (_i_var161)+(_i_var160);
		_i_var176 = (_i_var1) / (_i_var162);
		_i_var177 = (_i_var103) * (_i_var163);
		_i_var178 = (_i_var165)+(_i_var164);
		_i_var179 = (_i_var1) / (_i_var145);
		_i_var180 = (_i_var167)+(_i_var166);
		_i_var181 = (_i_var169)+(_i_var168);
		_i_var182 = -(_i_var170);
		_i_var183 = (_i_var172) * (_i_var171);
		_i_var184 = (_i_var172) * (_i_var173);
		_i_var185 = (_i_var175)+(_i_var174);
		_i_var186 = -(_i_var176);
		_i_var187 = (_i_var178) * (_i_var177);
		_i_var188 = (_i_var178) * (_i_var179);
		_i_var189 = (_i_var180) * (_i_var171);
		_i_var190 = (_i_var180) * (_i_var173);
		_i_var191 = (_i_var181) * (_i_var177);
		_i_var192 = (_i_var181) * (_i_var179);
		_i_var193 = (_i_var183) * (_i_var182);
		_i_var194 = (_i_var184) * (_i_var103);
		_i_var195 = (_i_var185) * (_i_var171);
		_i_var196 = (_i_var185) * (_i_var173);
		_i_var197 = (_i_var187) * (_i_var186);
		_i_var198 = (_i_var188) * (_i_var103);
		_i_var199 = (_i_var171) * (_i_var173);
		_i_var200 = (_i_var177) * (_i_var179);
		_i_var201 = (_i_var189) * (_i_var182);
		_i_var202 = (_i_var190) * (_i_var103);
		_i_var203 = (_i_var191) * (_i_var186);
		_i_var204 = (_i_var192) * (_i_var103);
		_i_var205 = (_i_var193) * (_i_var103);
		_i_var206 = (_i_var194) * (_i_var138);
		_i_var207 = (_i_var195) * (_i_var182);
		_i_var208 = (_i_var196) * (_i_var103);
		_i_var209 = (_i_var197) * (_i_var103);
		_i_var210 = (_i_var198) * (_i_var138);
		_i_var211 = (_i_var199) * (_i_var49);
		_i_var212 = (_i_var200) * (_i_var54);
		_i_var213 = (_i_var201) * (_i_var103);
		_i_var214 = (_i_var202) * (_i_var138);
		_i_var215 = (_i_var203) * (_i_var103);
		_i_var216 = (_i_var204) * (_i_var138);
		_i_var217 = (_i_var206)+(_i_var205);
		_i_var218 = (_i_var199) * (_i_var59);
		_i_var219 = (_i_var207) * (_i_var103);
		_i_var220 = (_i_var208) * (_i_var138);
		_i_var221 = (_i_var210)+(_i_var209);
		_i_var222 = (_i_var103) * (_i_var211);
		_i_var223 = (_i_var103) * (_i_var212);
		_i_var224 = (_i_var214)+(_i_var213);
		_i_var225 = (_i_var216)+(_i_var215);
		_i_var226 = (_i_var217) * (_i_var173);
		_i_var227 = (_i_var200) * (_i_var68);
		_i_var228 = (_i_var199) * (_i_var50);
		_i_var229 = (_i_var103) * (_i_var218);
		_i_var230 = (_i_var220)+(_i_var219);
		_i_var231 = (_i_var221) * (_i_var179);
		_i_var232 = (_i_var222) * (_i_var31);
		_i_var233 = (_i_var223) * (_i_var31);
		_i_var234 = (_i_var200) * (_i_var55);
		_i_var235 = (_i_var224) * (_i_var173);
		_i_var236 = (_i_var225) * (_i_var179);
		_i_var237 = (_i_var226) * (_i_var59);
		_i_var238 = (_i_var139) * (_i_var199);
		_i_var239 = (_i_var103) * (_i_var227);
		_i_var240 = (_i_var103) * (_i_var228);
		_i_var241 = (_i_var229) * (_i_var31);
		_i_var242 = (_i_var230) * (_i_var173);
		_i_var243 = (_i_var231) * (_i_var54);
		_i_var244 = (_i_var129) * (_i_var200);
		_i_var245 = (_i_var67) * (_i_var232);
		_i_var246 = (_i_var72) * (_i_var233);
		_i_var247 = (_i_var231) * (_i_var55);
		_i_var248 = (_i_var146) * (_i_var200);
		_i_var249 = (_i_var103) * (_i_var234);
		_i_var250 = (_i_var235) * (_i_var49);
		_i_var251 = (_i_var149) * (_i_var199);
		_i_var252 = (_i_var236) * (_i_var54);
		_i_var253 = (_i_var152) * (_i_var200);
		_i_var254 = (_i_var238)+(_i_var237);
		_i_var255 = (_i_var239) * (_i_var31);
		_i_var256 = (_i_var240) * (_i_var31);
		_i_var257 = (_i_var233) * (_i_var24);
		_i_var258 = (_i_var241) * (_i_var10);
		_i_var259 = (_i_var242) * (_i_var59);
		_i_var260 = (_i_var144) * (_i_var199);
		_i_var261 = (_i_var244)+(_i_var243);
		_i_var262 = (_i_var65) * (_i_var241);
		_i_var263 = (_i_var246)+(_i_var245);
		_i_var264 = (_i_var242) * (_i_var49);
		_i_var265 = (_i_var159) * (_i_var199);
		_i_var266 = (_i_var242) * (_i_var50);
		_i_var267 = (_i_var143) * (_i_var199);
		_i_var268 = (_i_var248)+(_i_var247);
		_i_var269 = (_i_var249) * (_i_var31);
		_i_var270 = (_i_var251)+(_i_var250);
		_i_var271 = (_i_var235) * (_i_var59);
		_i_var272 = (_i_var132) * (_i_var199);
		_i_var273 = (_i_var235) * (_i_var50);
		_i_var274 = (_i_var133) * (_i_var199);
		_i_var275 = (_i_var226) * (_i_var50);
		_i_var276 = (_i_var122) * (_i_var199);
		_i_var277 = (_i_var253)+(_i_var252);
		_i_var278 = (_i_var254)+(_i_var237);
		_i_var279 = (_i_var241) * (_i_var16);
		_i_var280 = (_i_var255) * (_i_var24);
		_i_var281 = (_i_var256) * (_i_var18);
		_i_var282 = (_i_var258)+(_i_var257);
		_i_var283 = (_i_var260)+(_i_var259);
		_i_var284 = (_i_var261)+(_i_var243);
		_i_var285 = (_i_var66) * (_i_var256);
		_i_var286 = (_i_var263)+(_i_var262);
		_i_var287 = (_i_var265)+(_i_var264);
		_i_var288 = (_i_var267)+(_i_var266);
		_i_var289 = (_i_var268)+(_i_var247);
		_i_var290 = (_i_var231) * (_i_var68);
		_i_var291 = (_i_var130) * (_i_var200);
		_i_var292 = (_i_var232) * (_i_var14);
		_i_var293 = (_i_var269) * (_i_var17);
		_i_var294 = (_i_var269) * (_i_var13);
		_i_var295 = (_i_var256) * (_i_var17);
		_i_var296 = (_i_var256) * (_i_var13);
		_i_var297 = (_i_var270)+(_i_var250);
		_i_var298 = (_i_var272)+(_i_var271);
		_i_var299 = (_i_var274)+(_i_var273);
		_i_var300 = (_i_var236) * (_i_var55);
		_i_var301 = (_i_var135) * (_i_var200);
		_i_var302 = (_i_var226) * (_i_var49);
		_i_var303 = (_i_var123) * (_i_var199);
		_i_var304 = (_i_var276)+(_i_var275);
		_i_var305 = (_i_var277)+(_i_var252);
		_i_var306 = (_i_var278) * (_i_var31);
		_i_var307 = (_i_var232) * (_i_var15);
		_i_var308 = (_i_var280)+(_i_var279);
		_i_var309 = (_i_var236) * (_i_var68);
		_i_var310 = (_i_var136) * (_i_var200);
		_i_var311 = (_i_var269) * (_i_var19);
		_i_var312 = (_i_var232) * (_i_var16);
		_i_var313 = (_i_var282)+(_i_var281);
		_i_var314 = (_i_var283)+(_i_var259);
		_i_var315 = (_i_var284) * (_i_var31);
		_i_var316 = (_i_var71) * (_i_var269);
		_i_var317 = (_i_var286)+(_i_var285);
		_i_var318 = (_i_var287)+(_i_var264);
		_i_var319 = (_i_var288)+(_i_var266);
		_i_var320 = (_i_var289) * (_i_var31);
		_i_var321 = (_i_var232) * (_i_var13);
		_i_var322 = (_i_var241) * (_i_var14);
		_i_var323 = (_i_var233) * (_i_var13);
		_i_var324 = (_i_var255) * (_i_var14);
		_i_var325 = (_i_var291)+(_i_var290);
		_i_var326 = (_i_var292) * (_i_var31);
		_i_var327 = (_i_var241) * (_i_var13);
		_i_var328 = (_i_var294)+(_i_var293);
		_i_var329 = (_i_var296)+(_i_var295);
		_i_var330 = (_i_var297) * (_i_var31);
		_i_var331 = (_i_var298)+(_i_var271);
		_i_var332 = (_i_var299)+(_i_var273);
		_i_var333 = (_i_var301)+(_i_var300);
		_i_var334 = (_i_var303)+(_i_var302);
		_i_var335 = (_i_var304)+(_i_var275);
		_i_var336 = (_i_var305) * (_i_var31);
		_i_var337 = (_i_var306) * (_i_var10);
		_i_var338 = (_i_var308)+(_i_var307);
		_i_var339 = (_i_var310)+(_i_var309);
		_i_var340 = (_i_var281)+(_i_var311);
		_i_var341 = (_i_var313)+(_i_var312);
		_i_var342 = (_i_var314) * (_i_var31);
		_i_var343 = (_i_var315) * (_i_var24);
		_i_var344 = (_i_var317)+(_i_var316);
		_i_var345 = (_i_var57) * (_i_var232);
		_i_var346 = (_i_var58) * (_i_var256);
		_i_var347 = (_i_var318) * (_i_var31);
		_i_var348 = (_i_var319) * (_i_var31);
		_i_var349 = (_i_var320) * (_i_var17);
		_i_var350 = (_i_var320) * (_i_var13);
		_i_var351 = (_i_var322)+(_i_var321);
		_i_var352 = (_i_var324)+(_i_var323);
		_i_var353 = (_i_var325)+(_i_var290);
		_i_var354 = (_i_var327)+(_i_var326);
		_i_var355 = (_i_var328) * (offsets(3, 0));
		_i_var356 = (_i_var329) * (_i_var12);
		_i_var357 = (_i_var330) * (_i_var14);
		_i_var358 = (_i_var331) * (_i_var31);
		_i_var359 = (_i_var332) * (_i_var31);
		_i_var360 = (_i_var333)+(_i_var300);
		_i_var361 = (_i_var334)+(_i_var302);
		_i_var362 = (_i_var335) * (_i_var31);
		_i_var363 = (_i_var336) * (_i_var24);
		_i_var364 = (_i_var338)+(_i_var337);
		_i_var365 = (_i_var339)+(_i_var309);
		_i_var366 = (_i_var340) * (_i_var31);
		_i_var367 = (_i_var341)+(_i_var311);
		_i_var368 = (_i_var342) * (_i_var10);
		_i_var369 = (_i_var344)+(_i_var343);
		_i_var370 = (_i_var67) * (_i_var241);
		_i_var371 = (_i_var72) * (_i_var255);
		_i_var372 = (_i_var56) * (_i_var241);
		_i_var373 = (_i_var346)+(_i_var345);
		_i_var374 = (_i_var347) * (_i_var14);
		_i_var375 = (_i_var348) * (_i_var17);
		_i_var376 = (_i_var348) * (_i_var13);
		_i_var377 = (_i_var350)+(_i_var349);
		_i_var378 = (_i_var351) * (_i_var12);
		_i_var379 = (_i_var352) * (offsets(3, 0));
		_i_var380 = (_i_var353) * (_i_var31);
		_i_var381 = (_i_var354) * (_i_var5);
		_i_var382 = (_i_var356)+(_i_var355);
		_i_var383 = (_i_var357) * (_i_var31);
		_i_var384 = (_i_var358) * (_i_var13);
		_i_var385 = (_i_var359) * (_i_var17);
		_i_var386 = (_i_var359) * (_i_var13);
		_i_var387 = (_i_var330) * (_i_var13);
		_i_var388 = (_i_var358) * (_i_var14);
		_i_var389 = (_i_var360) * (_i_var31);
		_i_var390 = (_i_var361) * (_i_var31);
		_i_var391 = (_i_var362) * (_i_var18);
		_i_var392 = (_i_var364)+(_i_var363);
		_i_var393 = (_i_var365) * (_i_var31);
		_i_var394 = (_i_var367)+(_i_var366);
		_i_var395 = (_i_var320) * (_i_var19);
		_i_var396 = (_i_var285)+(_i_var316);
		_i_var397 = (_i_var369)+(_i_var368);
		_i_var398 = (_i_var84) * (_i_var232);
		_i_var399 = (_i_var371)+(_i_var370);
		_i_var400 = (_i_var358) * (_i_var10);
		_i_var401 = (_i_var373)+(_i_var372);
		_i_var402 = (_i_var374) * (_i_var31);
		_i_var403 = (_i_var342) * (_i_var13);
		_i_var404 = (_i_var376)+(_i_var375);
		_i_var405 = (_i_var377) * (offsets(3, 0));
		_i_var406 = (_i_var379)+(_i_var378);
		_i_var407 = (_i_var315) * (_i_var13);
		_i_var408 = (_i_var380) * (_i_var14);
		_i_var409 = (_i_var381) * (_i_var31);
		_i_var410 = (_i_var382)+(_i_var381);
		_i_var411 = (_i_var384)+(_i_var383);
		_i_var412 = (_i_var386)+(_i_var385);
		_i_var413 = (_i_var35) * (_i_var354);
		_i_var414 = (_i_var47) * (_i_var329);
		_i_var415 = (_i_var351) * (_i_var11);
		_i_var416 = (_i_var329) * (_i_var0);
		_i_var417 = (_i_var388)+(_i_var387);
		_i_var418 = (_i_var389) * (_i_var19);
		_i_var419 = (_i_var390) * (_i_var16);
		_i_var420 = (_i_var392)+(_i_var391);
		_i_var421 = (_i_var393) * (_i_var24);
		_i_var422 = (_i_var394) * (_i_var31);
		_i_var423 = (_i_var348) * (_i_var18);
		_i_var424 = (_i_var396)+(_i_var395);
		_i_var425 = (_i_var397)+(_i_var395);
		_i_var426 = (_i_var380) * (_i_var24);
		_i_var427 = (_i_var399)+(_i_var398);
		_i_var428 = (_i_var359) * (_i_var18);
		_i_var429 = (_i_var401)+(_i_var400);
		_i_var430 = (_i_var73) * (_i_var232);
		_i_var431 = (_i_var57) * (_i_var241);
		_i_var432 = (_i_var403)+(_i_var402);
		_i_var433 = (_i_var404) * (_i_var12);
		_i_var434 = (_i_var406)+(_i_var405);
		_i_var435 = (_i_var347) * (_i_var13);
		_i_var436 = (_i_var342) * (_i_var14);
		_i_var437 = (_i_var408)+(_i_var407);
		_i_var438 = (_i_var410)+(_i_var409);
		_i_var439 = (_i_var411) * (_i_var5);
		_i_var440 = (_i_var412) * (_i_var12);
		_i_var441 = (_i_var414)+(_i_var413);
		_i_var442 = (_i_var411) * (_i_var4);
		_i_var443 = (_i_var411) * (_i_var0);
		_i_var444 = (_i_var416)+(_i_var415);
		_i_var445 = (_i_var417) * (_i_var11);
		_i_var446 = (_i_var412) * (_i_var0);
		_i_var447 = (_i_var354) * (_i_var4);
		_i_var448 = (_i_var354) * (_i_var0);
		_i_var449 = (_i_var391)+(_i_var418);
		_i_var450 = (_i_var420)+(_i_var419);
		_i_var451 = (_i_var306) * (_i_var16);
		_i_var452 = (_i_var422)+(_i_var421);
		_i_var453 = (_i_var424)+(_i_var423);
		_i_var454 = (_i_var347) * (_i_var16);
		_i_var455 = (_i_var425)+(_i_var423);
		_i_var456 = (_i_var342) * (_i_var16);
		_i_var457 = (_i_var427)+(_i_var426);
		_i_var458 = (_i_var346)+(_i_var428);
		_i_var459 = (_i_var330) * (_i_var16);
		_i_var460 = (_i_var429)+(_i_var428);
		_i_var461 = (_i_var358) * (_i_var16);
		_i_var462 = (_i_var431)+(_i_var430);
		_i_var463 = (_i_var432) * (_i_var5);
		_i_var464 = (_i_var434)+(_i_var433);
		_i_var465 = (_i_var436)+(_i_var435);
		_i_var466 = (_i_var437) * (offsets(3, 0));
		_i_var467 = (_i_var438) * (_i_var31);
		_i_var468 = (_i_var413)+(_i_var439);
		_i_var469 = (_i_var441)+(_i_var440);
		_i_var470 = (_i_var443)+(_i_var442);
		_i_var471 = (_i_var444) * (offsets(4, 0));
		_i_var472 = (_i_var446)+(_i_var445);
		_i_var473 = (_i_var448)+(_i_var447);
		_i_var474 = (_i_var449) * (_i_var31);
		_i_var475 = (_i_var450)+(_i_var418);
		_i_var476 = (_i_var390) * (_i_var15);
		_i_var477 = (_i_var452)+(_i_var451);
		_i_var478 = (_i_var453) * (_i_var31);
		_i_var479 = (_i_var455)+(_i_var454);
		_i_var480 = (_i_var347) * (_i_var15);
		_i_var481 = (_i_var457)+(_i_var456);
		_i_var482 = (_i_var458) * (_i_var31);
		_i_var483 = (_i_var460)+(_i_var459);
		_i_var484 = (_i_var330) * (_i_var15);
		_i_var485 = (_i_var462)+(_i_var461);
		_i_var486 = (_i_var463) * (_i_var31);
		_i_var487 = (_i_var464)+(_i_var463);
		_i_var488 = (_i_var465) * (_i_var12);
		_i_var489 = (_i_var467)+(_i_var466);
		_i_var490 = (_i_var468) * (_i_var31);
		_i_var491 = (_i_var469)+(_i_var439);
		_i_var492 = (_i_var417) * (_i_var12);
		_i_var493 = (_i_var47) * (_i_var351);
		_i_var494 = (_i_var470) * (_i_var3);
		_i_var495 = (_i_var471) * (_i_var31);
		_i_var496 = (_i_var472) * (offsets(4, 0));
		_i_var497 = (_i_var473) * (_i_var3);
		_i_var498 = (_i_var475)+(_i_var474);
		_i_var499 = (_i_var477)+(_i_var476);
		_i_var500 = (_i_var479)+(_i_var478);
		_i_var501 = (_i_var481)+(_i_var480);
		_i_var502 = (_i_var483)+(_i_var482);
		_i_var503 = (_i_var485)+(_i_var484);
		_i_var504 = (_i_var487)+(_i_var486);
		_i_var505 = (_i_var489)+(_i_var488);
		_i_var506 = (_i_var491)+(_i_var490);
		_i_var507 = (_i_var493)+(_i_var492);
		_i_var508 = (_i_var495)+(_i_var494);
		_i_var509 = (_i_var497)+(_i_var496);
		_i_var510 = (_i_var498) * (_i_var40);
		_i_var511 = (_i_var499) * (_i_var13);
		_i_var512 = (_i_var500) * (_i_var40);
		_i_var513 = (_i_var501) * (_i_var13);
		_i_var514 = (_i_var502) * (_i_var40);
		_i_var515 = (_i_var503) * (_i_var13);
		_i_var516 = (_i_var504) * (_i_var32);
		_i_var517 = (_i_var505) * (_i_var0);
		_i_var518 = (_i_var506) * (_i_var32);
		_i_var519 = (_i_var507) * (_i_var0);
		_i_var520 = (_i_var508) * (_i_var6);
		_i_var521 = (_i_var509) * (_i_var36);
		_i_var522 = (_i_var511)+(_i_var510);
		_i_var523 = (_i_var513)+(_i_var512);
		_i_var524 = (_i_var515)+(_i_var514);
		_i_var525 = (_i_var517)+(_i_var516);
		_i_var526 = (_i_var519)+(_i_var518);
		_i_var527 = (_i_var521)+(_i_var520);
		hessian(0, 0) = _i_var522;
		hessian(1, 0) = _i_var523;
		hessian(2, 0) = _i_var524;
		hessian(0, 1) = _i_var523;
		hessian(1, 1) = _i_var525;
		hessian(2, 1) = _i_var526;
		hessian(0, 2) = _i_var524;
		hessian(1, 2) = _i_var526;
		hessian(2, 2) = _i_var527;
	}

}


Vec2 SolveHeadAngles(const Vec2& neckAngles, double rodDistance
	, double headSideDistance, double headBackwardDistance, double headHeight
	, double motorSideDistance, double motorBackwardDistance, double motorHeight)
{
	dVector x(2);

	dVector headConnetParameters(3);
	headConnetParameters << headSideDistance, headHeight, headBackwardDistance;

	dVector motorConnectParameters(3);
	motorConnectParameters << motorSideDistance, motorHeight, motorBackwardDistance;

	dVector tNeckAngles(2);
	tNeckAngles << neckAngles.x, neckAngles.y;


	ModifiedNewton solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::computeNeckMechanismEnergy(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector2d gradient;
		Codegen::computeNeckMechanismEnergyGradient(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix2d hessian;
		Codegen::computeNeckMechanismEnergyHessian(headConnetParameters, motorConnectParameters, rodDistance, tNeckAngles, tx, hessian);
		result = hessian.sparseView();
	});

	return Vec2(x(0), x(1));
}

Vec3 SolveLeftArmAngles(const Vec3& shoulderPos, const Vec3& angles, const Vec3& targetPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector target(3);
	target << targetPos.x, targetPos.y, targetPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = -Math::HalfPi * 0.1;
	ci0(1) = Math::HalfPi;
	ci0(2) = Math::HalfPi * 0.8;
	ci0(3) = Math::HalfPi;
	ci0(4) = -0.1;
	ci0(5) = Math::HalfPi;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouLeftArmEnergy(parameters, tx, target, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouLeftArmEnergyGradient(parameters, tx, target, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouLeftArmEnergyHessian(parameters, tx, target, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

Vec3 SolveRightArmAngles(const Vec3& shoulderPos, const Vec3& angles, const Vec3& targetPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector target(3);
	target << targetPos.x, targetPos.y, targetPos.z;

	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = Math::HalfPi;
	ci0(1) = -Math::HalfPi * 0.1;
	ci0(2) = Math::HalfPi;
	ci0(3) = Math::HalfPi * 0.8;
	ci0(4) = Math::HalfPi;
	ci0(5) = -0.1;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouRightArmEnergy(parameters, tx, target, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouRightArmEnergyGradient(parameters, tx, target, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouRightArmEnergyHessian(parameters, tx, target, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}


Vec3 SolveLeftArmAnglesFromTwoTagets(const Vec3& shoulderPos, const Vec3& angles, const Vec3& handPos, const Vec3& elbowPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector targetHand(3);
	targetHand << handPos.x, handPos.y, handPos.z;

	dVector targetElbow(3);
	targetElbow << elbowPos.x, elbowPos.y, elbowPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = -Math::HalfPi * 0.1;
	ci0(1) = Math::HalfPi;
	ci0(2) = Math::HalfPi * 0.8;
	ci0(3) = Math::HalfPi;
	ci0(4) = -0.1;
	ci0(5) = Math::HalfPi;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTarget(parameters, tx, targetHand, targetElbow, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTargetGradient(parameters, tx, targetHand, targetElbow, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouLeftArmEnergyFromTwoTargetHessian(parameters, tx, targetHand, targetElbow, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

Vec3 SolveRightArmAnglesFromTwoTagets(const Vec3& shoulderPos, const Vec3& angles, const Vec3& handPos, const Vec3& elbowPos, double upperArmLength, double lowerArmLength)
{
	dVector x(3);
	x << angles.x, angles.y, angles.z;

	dVector parameters(5);
	parameters << shoulderPos.x, shoulderPos.y, shoulderPos.z, upperArmLength, lowerArmLength;

	dVector targetHand(3);
	targetHand << handPos.x, handPos.y, handPos.z;

	dVector targetElbow(3);
	targetElbow << elbowPos.x, elbowPos.y, elbowPos.z;


	Eigen::MatrixXd CI(3, 6);
	Eigen::MatrixXd CE(3, 0);

	CI(0, 0) = 1.0;
	CI(1, 0) = 0.0;
	CI(2, 0) = 0.0;

	CI(0, 1) = -1.0;
	CI(1, 1) = 0.0;
	CI(2, 1) = 0.0;

	CI(0, 2) = 0.0;
	CI(1, 2) = 1.0;
	CI(2, 2) = 0.0;

	CI(0, 3) = 0.0;
	CI(1, 3) = -1.0;
	CI(2, 3) = 0.0;

	CI(0, 4) = 0.0;
	CI(1, 4) = 0.0;
	CI(2, 4) = 1.0;

	CI(0, 5) = 0.0;
	CI(1, 5) = 0.0;
	CI(2, 5) = -1.0;


	dVector ci0(6);
	dVector ce0(0);

	ci0(0) = Math::HalfPi;
	ci0(1) = -Math::HalfPi * 0.1;
	ci0(2) = Math::HalfPi;
	ci0(3) = Math::HalfPi * 0.8;
	ci0(4) = Math::HalfPi;
	ci0(5) = -0.1;


	SQPSolver solver;
	solver.step(x,
		[&](const dVector& tx) {
		double result;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTarget(parameters, tx, targetHand, targetElbow, result);
		return result;
	},
		[&](const dVector& tx, dVector& result) {
		Eigen::Vector3d gradient;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTargetGradient(parameters, tx, targetHand, targetElbow, gradient);
		result = dVector(gradient);
	},
		[&](const dVector& tx, SpMat& result) {
		Eigen::Matrix3d hessian;
		Codegen::compute3AxisKibouRightArmEnergyFromTwoTargetHessian(parameters, tx, targetHand, targetElbow, hessian);
		result = hessian.sparseView();
	}, CE, ce0, CI, ci0);

	return Vec3(x(0), x(1), x(2));
}

void Draw3DGrid(const Mat4x4& cameraMatrix)
{
	for (auto i : Range(-5, 5))
	{
		Line3D(Vec3(-5, 0, i), Vec3(5, 0, i)).draw(cameraMatrix, ColorF(0.5));
		Line3D(Vec3(i, 0, -5), Vec3(i, 0, 5)).draw(cameraMatrix, ColorF(0.5));
	}
}

void Draw3DPoint(const Vec3& pos, const ColorF& col, const Mat4x4& cameraMatrix)
{
	AABB(pos, Vec3(1.0, 1.0, 1.0) * 0.1).draw(cameraMatrix, col);
}

Vec3 MultiplyMatAndVec3(const Mat4x4& mat, const Vec3& vec)
{
	constexpr size_t vertexCount = 1;
	Float3 out;
	SIMD_Float4 tVec(vec, 0.0f);
	SIMD::Vector3TransformCoordStream(&out, &tVec, vertexCount, mat);
	return Vec3(out);
}

void DrawFace(const Mat4x4& transformMat, const Mat4x4& mat)
{
	const Vec3 P0 = MultiplyMatAndVec3(transformMat, Vec3(0.0, 0.0, 0.0));
	const Vec3 P1 = MultiplyMatAndVec3(transformMat, Vec3(1.0, 1.0, -1.0));
	const Vec3 P2 = MultiplyMatAndVec3(transformMat, Vec3(-1.0, 1.0, -1.0));
	const Vec3 P3 = MultiplyMatAndVec3(transformMat, Vec3(-1.0, -1.0, -1.0));
	const Vec3 P4 = MultiplyMatAndVec3(transformMat, Vec3(1.0, -1.0, -1.0));

	Line3D(Vec3(0.0, 0.0, 0.0), P0).draw(mat, Palette::Lightgreen);

	Line3D(P0, P1).draw(mat, Palette::Lightgreen);
	Line3D(P0, P2).draw(mat, Palette::Lightgreen);
	Line3D(P0, P3).draw(mat, Palette::Lightgreen);
	Line3D(P0, P4).draw(mat, Palette::Lightgreen);

	Line3D(P1, P2).draw(mat, Palette::Lightgreen);
	Line3D(P2, P3).draw(mat, Palette::Lightgreen);
	Line3D(P3, P4).draw(mat, Palette::Lightgreen);
	Line3D(P4, P1).draw(mat, Palette::Lightgreen);
}

void DrawEye(double eyeblinkValue, const Mat4x4& transformMat, const Mat4x4& mat)
{
	const Vec3 P1 = MultiplyMatAndVec3(transformMat, Vec3(0.1, 0.5 * eyeblinkValue, 0.0));
	const Vec3 P2 = MultiplyMatAndVec3(transformMat, Vec3(-0.1, 0.5 * eyeblinkValue, 0.0));
	const Vec3 P3 = MultiplyMatAndVec3(transformMat, Vec3(-0.1, -0.5 * eyeblinkValue, 0.0));
	const Vec3 P4 = MultiplyMatAndVec3(transformMat, Vec3(0.1, -0.5 * eyeblinkValue, 0.0));

	Line3D(P1, P2).draw(mat, Palette::Lightgreen);
	Line3D(P2, P3).draw(mat, Palette::Lightgreen);
	Line3D(P3, P4).draw(mat, Palette::Lightgreen);
	Line3D(P4, P1).draw(mat, Palette::Lightgreen);
}

void DrawBody(CommonData& data, Mat4x4 hipTranslation, Mat4x4 chestTranslation, Mat4x4 headTranslation, double eyeBlinkValue, const Mat4x4& mat)
{
	DrawFace(hipTranslation, mat);
	DrawFace(chestTranslation, mat);
	DrawFace(headTranslation, mat);
	DrawEye(eyeBlinkValue, Mat4x4::Translation(0.3, 0.0, -1.0) * headTranslation, mat);
	DrawEye(eyeBlinkValue, Mat4x4::Translation(-0.3, 0.0, -1.0) * headTranslation, mat);
}

Vec3 RotateVec3FromAxisAndAngle(const Vec3& x, const Vec3& normalizedAxis, double angle)
{
	using Math::Cos;
	using Math::Sin;

	return Cos(angle) * x + Sin(angle) * (normalizedAxis.cross(x)) + (normalizedAxis.dot(x) * (1.0 - Cos(angle))) * normalizedAxis;
}


#include "CommonData.hpp"


Vec3 TranslateVec3ByMat4x4(const Vec3& value, const Mat4x4& translation)
{
	return SIMD::Vector3TransformCoord(SIMD_Float4(value, 0.0), translation).xyz();
}

void DrawArms(CommonData& data, const Mat4x4& chestTranslation, const Mat4x4& mat)
{
	const Vec3 leftArmAngles = GetValue<Vec3>(data, U"ValueProcessor.leftArmAngles");
	const Vec3 rightArmAngles = GetValue<Vec3>(data, U"ValueProcessor.rightArmAngles");
	const Vec3 leftShoulderArmPos = GetValue<Vec3>(data, U"ValueProcessor.leftShoulderArmPos");
	const Vec3 rightShoulderArmPos = GetValue<Vec3>(data, U"ValueProcessor.rightShoulderArmPos");

	const double length = GetValue<double>(data, U"ValueProcessor.armLength");

	{
		const Vec3 tArmPos = RotateVec3FromAxisAndAngle(Vec3(length, 0.0, 0.0), Vec3(0.0, 1.0, 0.0), leftArmAngles.z) + Vec3(length, 0.0, 0.0);
		const Vec3 ttArmPos = RotateVec3FromAxisAndAngle(tArmPos, Vec3(0.0, 0.0, 1.0), leftArmAngles.y);
		const Vec3 armPos = RotateVec3FromAxisAndAngle(ttArmPos, Vec3(1.0, 0.0, 0.0), leftArmAngles.x) + leftShoulderArmPos;

		const Vec3 ttElbowPos = RotateVec3FromAxisAndAngle(Vec3(length, 0.0, 0.0), Vec3(0.0, 0.0, 1.0), leftArmAngles.y);
		const Vec3 elbowPos = RotateVec3FromAxisAndAngle(ttElbowPos, Vec3(1.0, 0.0, 0.0), leftArmAngles.x) + leftShoulderArmPos;

		Line3D(TranslateVec3ByMat4x4(leftShoulderArmPos, chestTranslation), TranslateVec3ByMat4x4(elbowPos, chestTranslation)).draw(mat, Palette::Yellow);
		Line3D(TranslateVec3ByMat4x4(elbowPos, chestTranslation), TranslateVec3ByMat4x4(armPos, chestTranslation)).draw(mat, Palette::Yellow);
	}

	{
		const Vec3 tArmPos = RotateVec3FromAxisAndAngle(Vec3(-length, 0.0, 0.0), Vec3(0.0, 1.0, 0.0), rightArmAngles.z) + Vec3(-length, 0.0, 0.0);
		const Vec3 ttArmPos = RotateVec3FromAxisAndAngle(tArmPos, Vec3(0.0, 0.0, 1.0), rightArmAngles.y);
		const Vec3 armPos = RotateVec3FromAxisAndAngle(ttArmPos, Vec3(-1.0, 0.0, 0.0), rightArmAngles.x) + rightShoulderArmPos;

		const Vec3 ttElbowPos = RotateVec3FromAxisAndAngle(Vec3(-length, 0.0, 0.0), Vec3(0.0, 0.0, 1.0), rightArmAngles.y);
		const Vec3 elbowPos = RotateVec3FromAxisAndAngle(ttElbowPos, Vec3(-1.0, 0.0, 0.0), rightArmAngles.x) + rightShoulderArmPos;

		Line3D(TranslateVec3ByMat4x4(rightShoulderArmPos, chestTranslation), TranslateVec3ByMat4x4(elbowPos, chestTranslation)).draw(mat, Palette::Yellow);
		Line3D(TranslateVec3ByMat4x4(elbowPos, chestTranslation), TranslateVec3ByMat4x4(armPos, chestTranslation)).draw(mat, Palette::Yellow);
	}
}

void UpdateRobotData(CommonData& data)
{
	const double length = GetValue<double>(data, U"ValueProcessor.armLength");

	const auto leftArmAngles = GetValue<Vec3>(data, U"ValueProcessor.leftArmAngles");
	const auto rightArmAngles = GetValue<Vec3>(data, U"ValueProcessor.rightArmAngles");
	const auto targetLeftArmPos = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"leftArm"].source);
	const auto targetRightArmPos = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"rightArm"].source);

	data.valueTable[U"ValueProcessor.leftArmAngles"] = SolveLeftArmAngles(GetValue<Vec3>(data, U"ValueProcessor.leftShoulderArmPos")
		, leftArmAngles
		, targetLeftArmPos
		, length, length);
	data.valueTable[U"ValueProcessor.rightArmAngles"] = SolveRightArmAngles(GetValue<Vec3>(data, U"ValueProcessor.rightShoulderArmPos")
		, rightArmAngles
		, targetRightArmPos
		, length, length);
}

void TorqueEnableMotors(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		for (auto& name : connection.names)
		{
			auto& motorInfo = data.motorData.motorInfos[name];

			const double value = GetValue<double>(data, motorInfo.source);

			ham::Request request;

			if (motorInfo.type == U"Dynamixel.XL320")
			{
				request = ham::XL320_TorqueEnable(motorInfo.id);
			}
			else if (motorInfo.type == U"Dynamixel.XM430")
			{
				request = ham::XM430W350R_TorqueEnable(motorInfo.id);
			}
			else
			{
				Error();
			}

			connection.dyna->setWriteRequest(request, 1);
		}

		connection.dyna->sendData();
	}
}

void TorqueDisableMotors(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		if (connection.dyna->isConnecting())
		{
			for (auto& name : connection.names)
			{
				auto& motorInfo = data.motorData.motorInfos[name];

				const double value = GetValue<double>(data, motorInfo.source);

				ham::Request request;

				if (motorInfo.type == U"Dynamixel.XL320")
				{
					request = ham::XL320_TorqueEnable(motorInfo.id);
				}
				else if (motorInfo.type == U"Dynamixel.XM430")
				{
					request = ham::XM430W350R_TorqueEnable(motorInfo.id);
				}
				else
				{
					Error();
				}

				connection.dyna->setWriteRequest(request, 0);
			}

			connection.dyna->sendData();
		}
	}
}

double GetXL320ValueFromRad(double radian)
{
	const double digree = -(radian * 180.0 / Math::Pi);
	return Clamp(digree / 150.0, -1.0, 1.0) * 512 + 512;
}

double GetXM430ValueFromRad(double radian)
{
	return (-radian + Math::Pi) * 4096 / Math::TwoPi;
}

void SendMotorInfo(CommonData& data)
{
	for (auto it = data.motorData.comTable.begin(); it != data.motorData.comTable.end(); ++it)
	{
		auto& connection = it.value();

		if (connection.dyna->isConnecting())
		{
			if (connection.state == MotorConnectInformation::State::Sending)
			{
				for (auto& name : connection.names)
				{
					auto& motorInfo = data.motorData.motorInfos[name];

					const double value = GetValue<double>(data, motorInfo.source);

					ham::Request request;

					if (motorInfo.type == U"Dynamixel.XL320")
					{
						request = ham::XL320_GoalPosition(motorInfo.id);
						connection.dyna->setWriteRequest(request, GetXL320ValueFromRad(value));
					}
					else if (motorInfo.type == U"Dynamixel.XM430")
					{
						request = ham::XM430W350R_GoalPosition(motorInfo.id);
						connection.dyna->setWriteRequest(request, GetXM430ValueFromRad(value));
					}
					else
					{
						Error();
					}
				}

				connection.dyna->sendData();
			}
		}
	}
}

void DrawRobotData(CommonData& data)
{

	//3D Camera
	auto eyePositon = Cylindrical(20, GetValue<double>(data, U"ValueProcessor.cameraAngle"), 4);
	data.camera.setView(eyePositon, GetValue<Vec3>(data, U"ValueProcessor.focusPosition"));
	const Mat4x4 mat = data.camera.getMat4x4();

	//Draw 3D Objects
	{
		ScopedRenderStates2D culling(RasterizerState::SolidCullBack);

		Draw3DGrid(mat);



		//Draw3DPoint(GetValue<Vec3>(data, U"Robot.subTargetPosLeft"), Palette::Yellow, mat);
		//Draw3DPoint(GetValue<Vec3>(data, U"Robot.subTargetPosRight"), Palette::Yellow, mat);

		auto chestRotation = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"chest"].source);
		auto headRotation = GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"head"].source);
		const Mat4x4 hipRot = Mat4x4::Identity();
		const Mat4x4 chestRot = Mat4x4::RotationX(chestRotation.x) * Mat4x4::RotationZ(chestRotation.z) * Mat4x4::RotationY(chestRotation.y);
		const Mat4x4 headRot = Mat4x4::RotationX(headRotation.x) * Mat4x4::RotationZ(headRotation.z) * Mat4x4::RotationY(headRotation.y);
		double eyeBlinkValue = 1.0;

		const Vec3 pos = Vec3(0.0, 0.0, 0.0);
		const auto hipTranslation = hipRot * Mat4x4::Translation(pos.x, pos.y, pos.z);
		const auto chestTranslation = chestRot * Mat4x4::Translation(0.0, 2.0, 0.0) * hipTranslation;
		const auto headTranslation = headRot * Mat4x4::Translation(0.0, 5.0, 0.0) * chestTranslation;

		DrawBody(data, hipTranslation, chestTranslation, headTranslation, eyeBlinkValue, mat);
		DrawArms(data, chestTranslation, mat);

		const auto leftTarget = TranslateVec3ByMat4x4(GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"leftArm"].source), chestTranslation);
		const auto rightTarget = TranslateVec3ByMat4x4(GetValue<Vec3>(data, data.bodyData.bodyPartInfos[U"rightArm"].source), chestTranslation);
		Draw3DPoint(leftTarget, Palette::Red, mat);
		Draw3DPoint(rightTarget, Palette::Red, mat);
	}
}

Solver.hpp

#pragma once
#include <iostream>
# include <Eigen/Eigen>

#include "EigQuadProg.hpp"


using V3D = Eigen::Vector3d;
using V2D = Eigen::Vector2d;
using Triplet = Eigen::Triplet<double>;
using TripVec = std::vector<Eigen::Triplet<double>>;
using SpMat = Eigen::SparseMatrix<double>;

class dVector : public Eigen::VectorXd
{
public:

	dVector()
		: Eigen::VectorXd()
	{
		setZero();
	}

	dVector(int n)
		: Eigen::VectorXd(n)
	{
		setZero();
	}

	dVector(const Eigen::VectorXd& v)
		: Eigen::VectorXd(v)
	{

	}

};

class SolverBase
{
protected:

	Eigen::IOFormat HeavyFmt = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
	size_t m_maxIteratinos = 50;
	size_t m_maxLinesearchIterations = 15;
	double m_residual = 1e-8;
	bool m_debugPrint = false;

public:

	SolverBase()
	{

	}

	virtual size_t step(
		dVector& parameter,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const std::vector<size_t>& fixedIndices = {}) = 0;

	virtual ~SolverBase()
	{

	}

	void setMaxIterations(size_t maxIterations)
	{
		m_maxIteratinos = maxIterations;
	}

	size_t getMaxIterations() const
	{
		return m_maxIteratinos;
	}

	void setMaxLinesearchIterations(size_t maxLinesearchIterations)
	{
		m_maxLinesearchIterations = maxLinesearchIterations;
	}

	size_t getLinesearchIterations() const
	{
		return m_maxLinesearchIterations;
	}

	void setResidual(double residual)
	{
		m_residual = residual;
	}

	double getResidual() const
	{
		return m_residual;
	}

	void setDebugPrint(bool debugPrint)
	{
		m_debugPrint = debugPrint;
	}

	bool getDebugPrint() const
	{
		return m_debugPrint;
	}

protected:

	void filter(dVector& v, const std::vector<size_t>& fixedIndices)
	{
		for (int i = 0; i < fixedIndices.size(); ++i)
		{
			const int index = fixedIndices[i];

			v(index) = 0.0;
		}
	}

	void filterMat(SpMat& A, dVector& b, const std::vector<size_t>& fixedIndices, double diagValue = 1.0)
	{
		for (int i = 0; i < fixedIndices.size(); ++i)
		{
			const int index = fixedIndices[i];

			for (SpMat::InnerIterator it(A, index); it; ++it)
			{
				const int ii = static_cast<int>(it.row());
				const int jj = static_cast<int>(it.col());

				if (ii == jj)
					A.coeffRef(ii, jj) = diagValue;
				else
				{
					A.coeffRef(ii, jj) = 0.0;
					A.coeffRef(jj, ii) = 0.0;
				}
			}
		}
	}

};


class ModifiedNewton : public SolverBase
{
public:

	ModifiedNewton()
		: SolverBase()
	{

	}

	virtual ~ModifiedNewton()
	{

	}

	size_t step(
		dVector& x,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const std::vector<size_t>& fixedIndices = {}) override
	{
		const int dim = static_cast<int>(x.size());
		size_t iteration = 0;
		dVector tx = x;
		dVector dx = dVector(dim);
		dVector grad = dVector(dim);
		SpMat hessian(dim, dim);

		for (; iteration < m_maxIteratinos; ++iteration)
		{
			//Get gradient
			getGrad(tx, grad);

			//filter gradient
			filter(grad, fixedIndices);

			const double gradNorm = grad.norm();

			if (m_debugPrint)
			{
				std::cout << "ITERATION: ";
				std::cout << iteration << std::endl;
				std::cout << "\tVALUE: ";
				std::cout << getValue(tx);
				std::cout << "\tX: ";
				std::cout << tx.transpose().format(HeavyFmt);
				std::cout << "\tGRADIENT: ";
				std::cout << grad.format(HeavyFmt) << std::endl;
				std::cout << "\tGRADIENT NORM: ";
				std::cout << gradNorm << std::endl;
			}

			//Decide escape
			if (std::isnan(gradNorm) || gradNorm < m_residual)
			{
				break;
			}

			//Get Hessian
			getHessian(tx, hessian);

			//filter hessian
			filterMat(hessian, tx, fixedIndices);

			//Get Dx with regularization
			getDxWithReguralizeHessian(dx, grad, hessian);

			//filterDx
			filter(dx, fixedIndices);

			//Determine alpha (indirectly) using line search
			lineSearch(dx, tx, getValue);
		}


		x = tx;


		if (m_debugPrint)
		{
			std::cout << "ITERATION" << std::endl;
			std::cout << iteration << std::endl;

			std::cout << "GRADIENT" << std::endl;
			std::cout << grad.format(HeavyFmt) << std::endl;

			std::cout << "HESSIAN" << std::endl;
			std::cout << hessian.toDense().format(HeavyFmt) << std::endl;

			std::cout << "DX" << std::endl;
			std::cout << dx.format(HeavyFmt) << std::endl;

			std::cout << "X" << std::endl;
			std::cout << x.format(HeavyFmt) << std::endl;

			std::cout << "VALUE" << std::endl;
			std::cout << getValue(x) << std::endl;
		}

		return iteration;
	}


protected:

	void getDxWithReguralizeHessian(dVector& dx, dVector& grad, SpMat& hessian)
	{
		bool solved = false;
		Eigen::VectorXd dmydv;

		for (int j = 0;; ++j)
		{
			const Eigen::SimplicialCholesky<SpMat, Eigen::Upper> chol(hessian);
			dmydv = chol.solve(-grad);

			solved = true;

			//check factorization
			if (chol.info() != Eigen::Success)
			{
				solved = false;
			}

			//check redsiduum
			const Eigen::VectorXd r = hessian * dmydv + grad;

			if (r.norm() > 10e-8)
			{
				solved = false;
			}

			//check dot product between gradient and search direction
			const double dot_lin = dmydv.dot(grad);

			if (dot_lin > 0.0)
			{
				solved = false;
			}

			if (solved)
			{
				break;
			}

			{
				const double reg = (1e-4) * pow(3, j);
				if (reg > 1e10)
				{
					std::cout << " regularization is way too high: reg = " << reg << std::endl;
				}
				for (int k = 0; k < hessian.rows(); ++k)
				{
					hessian.coeffRef(k, k) += reg;

					if (m_debugPrint)
					{
						std::cout << "\tREGULARIZE: reg = " << reg << std::endl;
					}
				}
			}
		}

		dx = dmydv;
	}

	void lineSearch(dVector& dx, dVector& x, std::function<double(dVector&)> getValue)
	{
		const double beta = 0.5;
		const double currentValue = getValue(x);
		double nextValue = currentValue;

		if (m_debugPrint)
		{
			std::cout << "\tLINE SEARCH" << std::endl;
		}

		for (int j = 0; ; ++j)
		{
			dVector nextX = x;

			nextX += dx;

			nextValue = getValue(nextX);

			if (m_debugPrint)
			{
				std::cout << "\tNEXT X CANDIDATE: " << nextX.transpose().format(HeavyFmt) << "\tVALUE: " << nextValue << std::endl;
			}

			if ((nextValue < currentValue) || (j == m_maxLinesearchIterations))
			{
				x = nextX;
				break;
			}

			dx *= beta;
		}
	}

};


class SQPSolver : public ModifiedNewton
{
public:

	SQPSolver()
		: ModifiedNewton()
	{

	}

	virtual ~SQPSolver()
	{

	}

	size_t step(
		dVector& x,
		std::function<double(const dVector&)> getValue,
		std::function<void(const dVector&, dVector&)> getGrad,
		std::function<void(const dVector&, SpMat&)> getHessian,
		const Eigen::MatrixXd& CE, const dVector& ce0,
		const Eigen::MatrixXd& CI, const dVector& ci0,
		const std::vector<size_t>& fixedIndices = {})
	{
		const int dim = static_cast<int>(x.size());
		size_t iteration = 0;
		dVector tx = x;
		dVector dx = dVector(dim);
		dVector ttx = dVector(dim);
		dVector grad = dVector(dim);
		SpMat hessian(dim, dim);

		for (; iteration < m_maxIteratinos; ++iteration)
		{
			ttx = tx;

			//Get gradient
			getGrad(tx, grad);

			//filter gradient
			filter(grad, fixedIndices);

			const double gradNorm = grad.norm();

			if (m_debugPrint)
			{
				std::cout << "ITERATION: ";
				std::cout << iteration << std::endl;
				std::cout << "\tVALUE: ";
				std::cout << getValue(ttx);
				std::cout << "\tX: ";
				std::cout << ttx.transpose().format(HeavyFmt);
				std::cout << "\tGRADIENT NORM: ";
				std::cout << gradNorm << std::endl;
			}

			//Decide escape
			if (std::isnan(gradNorm) || gradNorm < m_residual)
			{
				x = tx;

				break;
			}

			//Get Hessian
			getHessian(tx, hessian);

			//filter hessian
			filterMat(hessian, tx, fixedIndices);

			//Get Dx with regularization
			getDxWithReguralizeHessian(dx, grad, hessian);

			//SolveQP
			const double value = solveQP(ttx, grad, hessian, CE, ce0, CI, ci0);

			dx = dVector(ttx - tx);

			//filterDx
			//filter(dx, fixedIndices);

			//tParameter += dx;
			//Determine alpha (indirectly) using line search
			lineSearch(dx, tx, getValue);
		}

		x = tx;

		if (m_debugPrint)
		{
			std::cout << "ITERATION" << std::endl;
			std::cout << iteration << std::endl;

			std::cout << "GRADIENT" << std::endl;
			std::cout << grad.format(HeavyFmt) << std::endl;

			std::cout << "HESSIAN" << std::endl;
			std::cout << hessian.toDense().format(HeavyFmt) << std::endl;

			std::cout << "DX" << std::endl;
			std::cout << dx.format(HeavyFmt) << std::endl;

			std::cout << "X" << std::endl;
			std::cout << x.format(HeavyFmt) << std::endl;

			std::cout << "VALUE" << std::endl;
			std::cout << getValue(x) << std::endl;
		}

		return iteration;
	}

private:

	inline double solveQP(dVector& x, const dVector& grad, const SpMat& hessian,
		const Eigen::MatrixXd& CE, const dVector& ce0,
		const Eigen::MatrixXd& CI, const dVector& ci0)
	{
		Eigen::MatrixXd G = hessian.toDense();
		Eigen::VectorXd g0 = grad - G * x;

		return solveEigenQuadProg(G, g0, CE, ce0, CI, ci0, x);
	}

};