/*! Attempts to move the element from its current pose to the new pose
in \a newTran in a single step. If the final pose if collision-free
it is done. If not, it interpolates to find the exact moment of contact.
Returns \a false if the interpolation fails.
*/
bool WorldElement::jumpTo(transf newTran, CollisionReport *contactReport)
{
int i, numCols;
bool success;
CollisionReport colReport;
transf lastTran = getTran();
if (setTran(newTran) == FAILURE) { return false; }
//we are only interested in collisions involving this body
std::vector<Body *> interestList;
//a robot will place all of its links in here
getBodyList(&interestList);
contactReport->clear();
while (1) {
numCols = myWorld->getCollisionReport(&colReport, &interestList);
if (!numCols) {
return true;
}
#ifdef GRASPITDBG
for (i = 0; i < numCols; i++) {
std::cerr << colReport[i].first->getName().latin1() << " -- "
<< colReport[i].second->getName().latin1() << std::endl;
}
DBGP("I am " << myName.latin1());
#endif
success = interpolateTo(lastTran, getTran(), colReport);
if (!success) {
return false;
}
contactReport->clear();
for (i = 0; i < numCols; i++) {
if (myWorld->getDist(colReport[i].first, colReport[i].second) < Contact::THRESHOLD) {
contactReport->push_back(colReport[i]);
}
}
}
}
/*! Given a start position which is expected to be collision-free, and a
new position which which causes inter-penetration, it interpolates
between the two to find the exact moment of contact. Returns false if
the interpolation fails (usually because the starting point is also in
collision).
Only looks at possible collisions in \a colReport, which the caller must
determine before calling this.
*/
bool
WorldElement::interpolateTo(transf lastTran, transf newTran, const CollisionReport &colReport)
{
vec3 nextTranslation;
Quaternion nextRotation;
transf nextTran;
int numCols = colReport.size();
//this causes the interpolation to first check the original transform
//since in many cases the original position is actually the one in contact, this can save a lot of computation
//as well as machine precision problems
//technically, it allows t to become negative, but in practice this should never happen as long as the initial position
//is collision free
double t = 0.0, deltat = 1.0, minDist;
bool done = false;
while (!done && deltat > 1.0e-20 && t >= 0) {
DBGP("move interpolation cycle")
deltat /= 2.0;
nextTranslation = (1.0-t)*lastTran.translation() + t*newTran.translation();
nextRotation = Quaternion::Slerp(t,lastTran.rotation(),newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
DBGP("moving to time : " << t);
if (setTran(nextTran) == FAILURE) {
deltat = 0;
break;
}
minDist = myWorld->getDist(colReport[0].first,colReport[0].second);
for (int i=1; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
minDist = MIN(minDist,dist);
}
DBGP("minDist: " << minDist);
if (minDist > 0) {
if (minDist < Contact::THRESHOLD * 0.5)
break;
t += deltat;
}
else
t -= deltat;
//debug code
if ( deltat <= 1.0e-20 || t < 0) {
for (int i=0; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
DBGA(colReport[i].first->getName().latin1() << " -- " <<
colReport[i].second->getName().latin1() << " is " << dist);
}
}
}
if (deltat < 1.0e-20 || t < 0) {
DBGP("deltat failsafe or negative t hit; interpolate failure");
fprintf(stdout,"WorldElement interpolation error!\n");
return false;
} else {
DBGP("deltat: " << deltat << "; minDist: " << minDist <<"; interpolate success.");
return true;
}
}