/*! 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]);
}
}
}
}
void InterpolatedVector::doInterpolate(float dt)
{
InterpolatedVectorData *data = ensureData();
//errorLog ("gothere");
/*
// old method
if (data->ease)
{
float diff = data->timePassed / data->timePeriod;
if (diff > 0.5f)
diff = 1.0f - diff;
diff /= 0.5f;
diff *= 2;
//diff += 0.5f;
data->fakeTimePassed += dt*diff;
}
*/
data->timePassed += dt;
if (data->timePassed >= data->timePeriod)
{
this->x = data->target.x;
this->y = data->target.y;
this->z = data->target.z;
data->interpolating = false;
if (data->loopType != 0)
{
if (data->loopType > 0)
data->loopType -= 1;
if (data->pingPong)
{
interpolateTo (data->from, data->timePeriod, data->loopType, data->pingPong, data->ease, IS_LOOPING);
}
else
{
this->x = data->from.x;
this->y = data->from.y;
this->z = data->from.z;
interpolateTo (data->target, data->timePeriod, data->loopType, data->pingPong, data->ease, IS_LOOPING);
}
}
}
else
{
Vector v;
/*
// old method
if (data->ease)
{
v = lerp(data->from, data->target, (data->timePassed / data->timePeriod), data->ease);
//v = (data->target - data->from) *
//v = (data->target - data->from) * (data->fakeTimePassed / data->timePeriod);
}
else
{
float perc = data->timePassed / data->timePeriod;
v = (data->target - data->from) * perc;
}
v += data->from;
*/
v = lerp(data->from, data->target, (data->timePassed / data->timePeriod), data->ease ? LERP_EASE : LERP_LINEAR);
this->x = v.x;
this->y = v.y;
this->z = v.z;
//*updatee += data->from;
}
}
void InterpolatedVector::update(float dt)
{
if (pendingInterpolation && trigger)
{
if (trigger->interpolating == triggerFlag)
{
interpolating = true;
pendingInterpolation = false;
}
else
return;
}
if (isFollowingPath())
{
updatePath(dt);
}
if (isInterpolating())
{
//errorLog ("gothere");
/*
// old method
if (ease)
{
float diff = timePassed / timePeriod;
if (diff > 0.5)
diff = 1.0 - diff;
diff /= 0.5;
diff *= 2;
//diff += 0.5f;
fakeTimePassed += dt*diff;
}
*/
timePassed += dt;
if (timePassed >= timePeriod)
{
this->x = target.x;
this->y = target.y;
this->z = target.z;
interpolating = false;
if (loopType != 0)
{
if (loopType > 0)
loopType -= 1;
if (pingPong)
{
interpolateTo (from, timePeriod, loopType, pingPong, ease, IS_LOOPING);
}
else
{
this->x = from.x;
this->y = from.y;
this->z = from.z;
interpolateTo (target, timePeriod, loopType, pingPong, ease, IS_LOOPING);
}
}
else
{
this->endOfInterpolationEvent.call();
this->endOfInterpolationEvent.set(0);
}
}
else
{
Vector v;
/*
// old method
if (ease)
{
v = lerp(from, target, (timePassed / timePeriod), ease);
//v = (target - from) *
//v = (target - from) * (fakeTimePassed / timePeriod);
}
else
{
float perc = timePassed / timePeriod;
v = (target - from) * perc;
}
v += from;
*/
v = lerp(from, target, (timePassed / timePeriod), ease ? LERP_EASE : LERP_LINEAR);
this->x = v.x;
this->y = v.y;
this->z = v.z;
//*updatee += from;
}
}
}
void InterpolatedVector::updatePath(float dt)
{
if (!speedPath)
{
if (pathTimer > pathTime)
{
Vector value = path.getPathNode(path.getNumPathNodes()-1)->value;
this->x = value.x;
this->y = value.y;
this->z = value.z;
if (loopType != 0)
{
if (loopType > 0)
loopType -= 1;
int oldLoopType = loopType;
if (pingPong)
{
// flip path
path.flip();
startPath(pathTime);
loopType = oldLoopType;
}
else
{
startPath(pathTime);
loopType = oldLoopType;
}
}
else
{
stopPath();
endOfPathEvent.call();
}
}
else
{
pathTimer += dt * pathTimeMultiplier;
// ;//dt*timeSpeedMultiplier;
float perc = pathTimer/pathTime;
Vector value = path.getValue(perc);
this->x = value.x;
this->y = value.y;
this->z = value.z;
/*
std::ostringstream os;
os << "nodes: " << path.getNumPathNodes() << " pathTimer: " << pathTimer << " pathTime: " << pathTime << " perc: " << perc << " p(" << x << ", " << y << ")";
debugLog(os.str());
*/
/*
float diff = pathTime - pathTimer;
if (timeSpeedEase > 0)
{
float secs = 1.0/timeSpeedEase;
if (diff <= secs)
{
timeSpeedMultiplier -= dt*timeSpeedEase;
if (timeSpeedMultiplier < 0.1)
timeSpeedMultiplier = 0.1;
}
}
if (timeSpeedMultiplier < 1)
{
timeSpeedMultiplier += dt*timeSpeedEase;
if (timeSpeedMultiplier >= 1)
timeSpeedMultiplier = 1;
}
*/
}
}
else
{
if (!isInterpolating())
{
currentPathNode++;
VectorPathNode *node = path.getPathNode(currentPathNode);
/*
if (node)
{
}
else
{
stopPath();
endOfPathEvent.call();
}
*/
if (node)
{
interpolateTo(node->value, (node->value - Vector(this->x, this->y, this->z)).getLength3D()*(1.0/pathSpeed));
}
else
{
// handle looping etc
stopPath();
endOfPathEvent.call();
}
}
}
}
