void Jacobian::ComputeJacobian(const Tree& tree)
{
	Invalidate();
	jacobian_ = MatrixX(3,tree.GetNumJoint()-1); // à cause de son incrémentation débile
	// Traverse this to find all end effectors
	Vector3 temp;
	Joint* n = tree.GetRoot();
	while (n) {	
		if (n->IsEffector())
		{
			int i = n->GetEffectorNum();
			// Find all ancestors (they will usually all be joints)
			// Set the corresponding entries in the Jacobian J
			Joint* m = tree.GetParent(n);
			while (m) {
				int j = m->GetJointNum();
				assert (0 <=i && i<tree.GetNumEffector() && 0<=j && j<tree.GetNumJoint());
				
				temp = m->GetS();			// joint pos.
				temp -= n->GetS();			// -(end effector pos. - joint pos.)
				Vector3 tmp2 = temp.cross(m->GetW());			// cross product with joint rotation axis
				jacobian_.col(j-1) = tmp2;
				m = tree.GetParent(m);
			}
		}
		n = (n->IsEffector() ? 0 : tree.GetSuccessor(n));
	}
}
Sample::Sample(Tree& tree)
{
	Joint * j = tree.GetRoot();
	while(j)
	{
		angles_.push_back(j->GetTheta());
		position_ = j->GetS();
		j = j->pChild_;
	}
	position_ -= tree.GetPosition();
	jacobianProd_ = tree.GetJacobian()->GetJacobianProduct();
	jacobianProdInverse_ = tree.GetJacobian()->GetJacobianProductInverse();
}