[C++][Major] Added JointModelDense and JointDataDense. Both templated by the dimension of configuration space and tangential space. Each joint can be converted to a Dense one