/*! This is a helper function for grasp force optimization routines. Given a
set of contacts, and a matrix of contact wrenches expressed in *world*
coordinates and relative to object origin (as computed in gfo routines)
this function will convert them to object coordinate system and accumulate
them in the object's external wrench accumulator. This can serve as an
output of the gfo functions and also for rendering purposes, as the
external wrench accumulator can then be rendered.
*/
void
Grasp::accumulateAndDisplayObjectWrenches(std::list<Contact*> *contacts,
const Matrix &objectWrenches)
{
int count = 0;
std::list<Contact*>::iterator it;
for (it=contacts->begin(); it!=contacts->end(); it++, count++) {
Contact *contact = *it;
vec3 force(objectWrenches.elem(6*count+0,0),
objectWrenches.elem(6*count+1,0),
objectWrenches.elem(6*count+2,0));
vec3 torque(objectWrenches.elem(6*count+3,0),
objectWrenches.elem(6*count+4,0),
objectWrenches.elem(6*count+5,0));
if (contact->getBody2()->isDynamic()) {
DynamicBody *object = (DynamicBody*)(contact->getBody2());
//compute force and torque in body reference frame
//and scale them down for now for rendering and output purposes
force = 1.0e-6 * force * object->getTran().inverse();
//torque is also scaled by maxRadius in conversion matrix
torque = 1.0e-6 * torque * object->getTran().inverse();
//accumulate them on object
object->addForce(force);
object->addTorque(torque);
}
}
}
/*!
Returns the correct coefficient of friction for this contact. If either
body is dynamic, and the relative velocity between them is greater than
1.0 mm/sec (should be made a parameter), then it returns the kinetic COF,
otherwise it returns the static COF.
*/
double
Contact::getCof() const
{
DynamicBody *db;
vec3 radius,vel1(vec3::ZERO),vel2(vec3::ZERO),rotvel;
if (body1->isDynamic()) {
db = (DynamicBody *)body1;
radius = db->getTran().rotation() * (loc - db->getCoG());
vel1.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
vel1 += radius * rotvel;
}
if (body2->isDynamic()) {
db = (DynamicBody *)body2;
radius = db->getTran().rotation() * (mate->loc - db->getCoG());
vel2.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
vel2 += radius * rotvel;
}
if ((vel1 - vel2).len() > 1.0) {
DBGP("SLIDING!");
return kcof;
}
else return cof;
}
