bool World::IsReachable(const Robot& robot, const Tree& tree, const Obstacle& obstacle) const { Vector3 treePos = tree.GetPosition(); int room = ( tree.GetPosition()(1) > 0 ) ? 1 : - 1; const matrices::Matrix4& transform( robot.ToRobotCoordinates()); numeric p1robPos = matrices::matrix4TimesVect3( transform, obstacle.GetP1())(1) + 0.05 * room; numeric p3robPos = matrices::matrix4TimesVect3( transform, obstacle.GetP3())(1) + 0.05 * room; if(! (p1robPos * room >= 0 || p3robPos * room >= 0)) { return false; } //assert(pImpl_->instantiated_); return pImpl_->intersection_.Intersect(robot, tree, obstacle); }
FilterDistance::FilterDistance(NUMBER treshold, const Tree& tree, const Vector3& target) : Filter_ABC() , treshold_(treshold) , target_(target) , treePos_(tree.GetPosition()) { // NOTHING }
Sample::Sample(Tree& tree) { Joint * j = tree.GetRoot(); while(j) { angles_.push_back(j->GetTheta()); position_ = j->GetS(); j = j->pChild_; } position_ -= tree.GetPosition(); jacobianProd_ = tree.GetJacobian()->GetJacobianProduct(); jacobianProdInverse_ = tree.GetJacobian()->GetJacobianProductInverse(); }