//-----------------------------------------------------------------------------
// startMotion()
//-----------------------------------------------------------------------------
BOOL LLMotionController::startMotion(const LLUUID &id, F32 start_offset)
{
// look for motion in our list of created motions
LLMotion *motion = createMotion(id);
if (!motion)
{
return FALSE;
}
//if the motion is already active, then we're done
else if (isMotionActive(motion)) // motion is playing and...
{
if (motion->isStopped()) // motion has been stopped
{
deactivateMotion(motion, false);
}
else if (mTime < motion->mSendStopTimestamp) // motion is still active
{
return TRUE;
}
}
// llinfos << "Starting motion " << name << llendl;
return activateMotion(motion, mTime - start_offset);
}
//-----------------------------------------------------------------------------
// startMotion()
//-----------------------------------------------------------------------------
BOOL LLMotionController::startMotion(const LLUUID &id, F32 start_offset)
{
// do we have an instance of this motion for this character?
LLMotion *motion = findMotion(id);
// motion that is stopping will be allowed to stop but
// replaced by a new instance of that motion
if (motion
&& !mPaused
&& motion->canDeprecate()
&& motion->isActive() // singu: do not deprecate motions that are not active.
&& motion->getFadeWeight() > 0.01f // not LOD-ed out
&& (motion->isBlending() || motion->getStopTime() != 0.f))
{
deprecateMotionInstance(motion);
// force creation of new instance
motion = NULL;
}
// create new motion instance
if (!motion)
{
motion = createMotion(id);
}
if (!motion)
{
return FALSE;
}
//if the motion is already active and allows deprecation, then let it keep playing
else if (motion->canDeprecate() && isMotionActive(motion))
{
return TRUE;
}
// llinfos << "Starting motion " << name << llendl;
//<singu>
F32 start_time = mAnimTime - start_offset;
if (!mDisableSyncing)
{
start_time = motion->syncActivationTime(start_time);
}
++mDisableSyncing;
//</singu>
BOOL res = activateMotionInstance(motion, start_time);
//<singu>
--mDisableSyncing;
//</singu>
return res;
}
ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
// update goal and check validity
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
if (goal == nullptr)
{
OMPL_ERROR("%s: Goal undefined", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
while (const base::State *st = pis_.nextStart())
{
startMotion_ = createMotion(goal_s, st);
break;
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
bool solved = false;
auto *rmotion = new Motion(si_);
base::State *xstate = si_->allocState();
goalMotion_ = createGoalMotion(goal_s);
CostEstimatorLb costEstimatorLb(goal, idToMotionMap_);
LPAstarLb_ = new LPAstarLb(startMotion_->id_, goalMotion_->id_, graphLb_, costEstimatorLb); // rooted at source
CostEstimatorApx costEstimatorApx(this);
LPAstarApx_ = new LPAstarApx(goalMotion_->id_, startMotion_->id_, graphApx_, costEstimatorApx); // rooted at target
double approxdif = std::numeric_limits<double>::infinity();
// e+e/d. K-nearest RRT*
double k_rrg = boost::math::constants::e<double>() +
boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension();
////////////////////////////////////////////
// step (1) - RRT
////////////////////////////////////////////
bestCost_ = std::numeric_limits<double>::infinity();
rrt(ptc, goal_s, xstate, rmotion, approxdif);
if (!ptc())
{
solved = true;
////////////////////////////////////////////
// step (2) - Lazy construction of G_lb
////////////////////////////////////////////
idToMotionMap_.push_back(goalMotion_);
int k = getK(idToMotionMap_.size(), k_rrg);
std::vector<Motion *> nnVec;
nnVec.reserve(k);
BOOST_FOREACH (Motion *motion, idToMotionMap_)
{
nn_->nearestK(motion, k, nnVec);
BOOST_FOREACH (Motion *neighbor, nnVec)
if (neighbor->id_ != motion->id_ && !edgeExistsLb(motion, neighbor))
addEdgeLb(motion, neighbor, distanceFunction(motion, neighbor));
}
