/*! This computes the wrenches of the virtual contact as used for grasp
quality computations. The important aspect to take into account is
wether we are using an object or not.
If we are using an object, we assume that contact centroid and maxradius
have already been set to match those of the object (hopefully, the grasp
has done this). Also, the force applied be the contact is scaled by a
function of the distance between the contact and the actual object.
If there is no object, we assume that contact centroid and maxradius
have been preset dependign only on the set of virtual contacts that
make up the grasp (again, we hope the grasp has done this). There is no
scaling involved, all forces are handled normally as if the contact was
actually on the object.
*/
void
VirtualContact::computeWrenches(bool useObjectData, bool simplify)
{
int i;
//double alpha;
vec3 radius, forceVec,torqueVec,tangentX,tangentY;
//we need everything to be wrt world coordinates
position worldLoc;
vec3 worldNormal;
if (!useObjectData) {
worldLoc = getWorldLocation();
worldNormal = getWorldNormal();
transf worldFrame = frame * body1->getTran();
tangentX = worldFrame.affine().row(0);
tangentY = worldFrame.affine().row(1);
} else {
// LOOK AT VIRTUAL CONTACT ON THE HAND
worldLoc = getWorldLocation();
worldNormal = getWorldNormal();
tangentX = vec3(0,1,0) * worldNormal;
tangentX = normalise(tangentX);
tangentY = worldNormal * tangentX;
}
//mCenter needs to be already set to object cog if needed as such
radius = worldLoc - mCenter;
if (wrench) delete [] wrench;
if (simplify) {
numFCWrenches = 1; //this is hack-ish, should be fixed
wrench = new Wrench[1];
wrench[0].force = worldNormal;
wrench[0].torque = (radius*worldNormal) / mMaxRadius;
return;
}
numFCWrenches = numFrictionEdges;
wrench = new Wrench[numFrictionEdges];
//SHOULD SET UP COEFFICIENT BASED ON MATERIALS!
for (i=0;i<numFCWrenches;i++) {
forceVec = worldNormal + (tangentX*cof*frictionEdges[6*i])+ (tangentY*cof*frictionEdges[6*i+1]);
//max friction is normal_force_magnitude (which is 1) * coeff_of_friction
//possible friction for this wrench is (friction_edge * max_friction) in the X and Y direction of the contact
wrench[i].force = forceVec;
wrench[i].torque = ( (radius*forceVec) + worldNormal*cof*frictionEdges[6*i+5] )/mMaxRadius;
//max torque is contact_radius * normal_force_magnitude (which is 1) * coeff_of_friction
//possible torque for this wrench is (friction_edge * max_torque) in the direction of the contact normal
//the contact_radius coefficient is already taken into account in the friction_edge
}
}
LocalFrame SceneObject::getAutoGenWorldLocalFrame(unsigned fi, const vec3f& position, bool flat) const
{
LocalFrame lf;
lf.buildFromNormal(getWorldNormal(fi, position, flat));
/*
lf.n = getWorldNormal(fi, position, flat);
vec3f tmpT;
tmpT = (std::abs(lf.n.z) > 0.99f) ? vec3f(1,0,0) : vec3f(0,0,1);
lf.s = lf.n.cross(tmpT);
lf.s.normalize();
lf.t = lf.s.cross(lf.n);
lf.t.normalize();
*/
/*
vec3f axis = up.cross(lf.n);
float angle = acos(clampf(up.dot(lf.n), -1, 1));
if (!(axis.length() >= 1e-6f || (axis.length() < 1e-6f && n.dot(lf.n) == 1.f)))
{
printf("error , %.8f\n" , n.dot(lf.n));
}
axis.normalize();
//if (axis.length() < 1e-6f)
//{
// lf.s = vec3f(1,0,0);
// lf.t = vec3f(0,0,1);
//}
//else
{
lf.s = vec3f(rotMat(axis, angle)*vec4<float>(vec3f(1,0,0), 0));
lf.t = vec3f(rotMat(axis, angle)*vec4<float>(vec3f(0,0,1), 0));
}
*/
return lf;
}
