double rotationMatrixFromContactsPositionKine(const Vector3 vLFootPos, const Vector3 vRFootPos, Matrix3& R )
{
Vector3 Vrl, axis, theta;
AngleAxis j;
double h, thetay, thetaz;
// Definition of the length and the unit vector between the two foots.
Vrl=vLFootPos-vRFootPos;
j=kine::rotationVectorToAngleAxis(Vrl);
h = j.angle(); // length between the ankles
axis = j.axis(); // unit vector between the ankles
//Definition of the transformation (rotation) between (x,y,z) and (perpendicular of j, j, z).
//theta=theta(0,0,atan(axis[0]/axis[1]));
thetay = atan2(axis[0],axis[2]);//theta[1];
thetaz = atan2(axis[0],axis[1]);//theta[2];
R << cos(thetay)*cos(thetaz), -cos(thetay)*sin(thetaz), sin(thetay),
sin(thetaz), cos(thetaz), 0,
-sin(thetay)*cos(thetaz), sin(thetay)*sin(thetaz), cos(thetay);
return h;
}
void LinkPropertyViewImpl::addProperty(const string& name, const AngleAxis& rotation)
{
static const QString format("%1 %2 %3 %4");
const Vector3 a = rotation.axis();
addProperty(name, format.arg(a[0]).arg(a[1]).arg(a[2]).arg(rotation.angle()));
}