Matrix doubleTouchThread::findH0(skinContact &sc)
{
// Set the proper orientation for the touching end-effector
Matrix H0(4,4);
Vector x(3,0.0), z(3,0.0), y(3,0.0);
x = sc.getNormalDir();
z[0] = -x[2]/x[0]; z[2] = 1;
y = -1*(cross(x,z));
// Let's make them unitary vectors:
x = x / norm(x);
y = y / norm(y);
z = z / norm(z);
H0.zero();
H0(3,3) = 1;
H0.setSubcol(x,0,0);
H0.setSubcol(y,0,1);
H0.setSubcol(z,0,2);
H0.setSubcol(sc.getGeoCenter(),0,3);
return H0;
}
Matrix doubleTouchThread::findH0(skinContact &sc)
{
// Set the proper orientation for the touching end-effector
Matrix H0(4,4);
Vector x(3,0.0), z(3,0.0), y(3,0.0);
x = sc.getNormalDir();
if (x[0] == 0.0)
{
x[0] = 0.00000001; // Avoid the division by 0
}
if (curTaskType!="LHtoR" && curTaskType!="RHtoL")
{
z[0] = -x[2]/x[0]; z[2] = 1;
y = -1*(cross(x,z));
}
else
{
// When x[0]==+-1, We can exploit an easier rule:
z[1] = x[2];
y = -1*(cross(x,z));
}
// Let's make them unitary vectors:
x = x / norm(x);
y = y / norm(y);
z = z / norm(z);
H0 = eye(4);
H0.setSubcol(x,0,0);
H0.setSubcol(y,0,1);
H0.setSubcol(z,0,2);
H0.setSubcol(sc.getCoP(),0,3);
return H0;
}
