/*! 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(); } } } }