Exemple #1
0
void submit(int usecs,Fn_0<void> *fn)
  {
  Ttask *t=new Ttask();
  Ttask *u;
  t->usecs=usecs;
  t->fn=fn;
  for(u=timer_tasks.next;u && usecs>u->usecs;u=u->link.next);
  if (u)
    timer_tasks.insert_before(u,t);
  else
    timer_tasks.push_back(t);
  }
Exemple #2
0
Link* Link::set_neighbours()
{
	Link* p = this;
	Link* mark = this;

	if (p->prev && p->succ) return p;

	p = p->to_first_total();

	while (p->value < this->value) // Если есть элемент на схожем уровне с нашим, то запоминаем его в маркер.
	{ // Следуем вперёд до тех пор, пока не найдём самые ближние элементы (prev) и (succ).
		while (p->upper && p->level < level) p = p->upper;
		if (p->level == level) // Если найден элемент на этом уровне, запомним его и продолжим поиск дальше.
		{
			mark = p;
			while (p->lower)
			{
				p = p->lower;
			}
		}

		if (p->succ) p = p->succ;
	}
	if (mark && mark != this)
	{
		this->prev = mark;
		this->prev->succ = this;
	}

	mark = 0;

	p = p->to_first_total();

	while (p->level != this->level && p->succ) // Тоже самое для элемента succ
	{
		if (p->succ) p = p->succ;
		while (p->upper && p->value > value && p->level < level) p = p->upper;
		if (p->level == level && p->value > this->value) // Если найден элемент на этом уровне, запомним его и продолжим поиск дальше.
		{
			mark = p;

		}
	}

	if (mark && mark != this)
	{
		this->succ = mark;
		this->succ->prev = this;
	}

	return this;
}
EditableSceneBodyImpl::EditableSceneBodyImpl(EditableSceneBody* self, BodyItemPtr& bodyItem)
    : self(self),
      bodyItem(bodyItem),
      kinematicsBar(KinematicsBar::instance()),
      modified(SgUpdate::MODIFIED)
{
    pointedSceneLink = 0;
    outlinedLink = 0;
    targetLink = 0;

    positionDragger = new PositionDragger;
    positionDragger->setDraggerAlwaysShown(true);
    positionDragger->sigDragStarted().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKstarted, this));
    positionDragger->sigPositionDragged().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKdragged, this));
    
    dragMode = DRAG_NONE;
    isDragging = false;
    isEditMode = false;

    markerGroup = new SgGroup;
    markerGroup->setName("Marker");
    self->addChild(markerGroup);

    double radius = 0;
    const int n = self->numSceneLinks();
    for(int i=0; i < n; ++i){
        SceneLink* sLink = self->sceneLink(i);
        BoundingBox bb = sLink->boundingBox();
        double radius0 = (bb.max() - bb.center()).norm();
        if(radius0 > radius){
            radius = radius0;
        }
    }
    cmMarker = new CrossMarker(0.25, Vector3f(0.0f, 1.0f, 0.0f), 2.0);
    cmMarker->setName("centerOfMass");
    cmMarker->setSize(radius);
    isCmVisible = false;

    LeggedBodyHelperPtr legged = getLeggedBodyHelper(self->body());
    if(legged->isValid() && legged->numFeet() > 0){
        Link* footLink = legged->footLink(0);
        const double r = calcLinkMarkerRadius(self->sceneLink(footLink->index()));
        zmpMarker = new SphereMarker(r, Vector3f(0.0f, 1.0f, 0.0f), 0.3);
        zmpMarker->setName("ZMP");
        zmpMarker->addChild(new CrossMarker(r * 2.5, Vector3f(0.0f, 1.0f, 0.0f), 2.0f));
    } else {
        zmpMarker = new SphereMarker(0.1, Vector3f(0.0f, 1.0f, 0.0f), 0.3);
    }
    isZmpVisible = false;

    self->sigGraphConnection().connect(boost::bind(&EditableSceneBodyImpl::onSceneGraphConnection, this, _1));
}
Exemple #4
0
void DBWorker::updateTab(int tabId, QString url, QString title, QString path)
{
    Link currentLink = getCurrentLink(tabId);
    if (!currentLink.isValid()) {
        qWarning() << "attempt to update url that is not stored in db." << tabId << title << url << path << currentLink.linkId() << currentLink.url();
        return;
    }
#ifdef DEBUG_LOGS
    qDebug() << tabId << title << url << path;
#endif
    updateLink(currentLink.linkId(), url, title, path);
    emit tabChanged(getTabData(tabId));
}
Exemple #5
0
void BodyLinkViewImpl::addSelfCollision(const CollisionLinkPair& collisionPair, QString& collisionString)
{
    Link* oppositeLink;
    if(collisionPair.link[0] == currentLink){
        oppositeLink = collisionPair.link[1];
    } else {
        oppositeLink = collisionPair.link[0];
    }
    if(!collisionString.isEmpty()){
        collisionString += " ";
    }
    collisionString += oppositeLink->name().c_str();
}
Exemple #6
0
//----------------------------------------------------------------------------
Object* Stream::GetFromMap (Object* pkLinkID)
{
    map<Object*,void*>::iterator kOIter = m_kMap.find(pkLinkID);
    if ( kOIter != m_kMap.end() )
    {
        Link* pkLink = (Link*) kOIter->second;
        return pkLink->GetObject();
    }
    else
    {
        return NULL;
    }
}
Exemple #7
0
void Linktest::test_sendError_connected()
{
    CommSocket_flush_called = false;

    m_encoder = new Atlas::Objects::ObjectsEncoder(*m_bridge);
    m_link->setEncoder(m_encoder);

    Operation op;

    m_link->sendError(op, "test error message", "");

    ASSERT_TRUE(CommSocket_flush_called);
}
Exemple #8
0
void Signature::changeLinkIds(int idFrom, int idTo)
{
	std::map<int, Link>::iterator iter = _links.find(idFrom);
	if(iter != _links.end())
	{
		Link link = iter->second;
		_links.erase(iter);
		link.setTo(idTo);
		_links.insert(std::make_pair(idTo, link));
		_linksModified = true;
		UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
	}
}
Exemple #9
0
std::ostream& operator<<(std::ostream& os, JointPath& path)
{
    int n = path.numJoints();
    for(int i=0; i < n; ++i){
        Link* link = path.joint(i);
        os << link->name();
        if(i != n){
            os << (path.isJointDownward(i) ? " => " : " <= ");
        }
    }
    os << std::endl;
    return os;
}
Exemple #10
0
Link* Link::reverse() // Шагаем назад на один шаг
{
	if (!prev && !lower) return this; // Если идти назад некуда
	Link* p = this;

	if (p->value == 1 && p->level > 1)
	{
		if (p->lower) p = p->lower;
		p = p->to_last();
	}
	if (p->prev) p = p->prev;

	return p;
}
std::ostream& operator<<(std::ostream& os, LinkTraverse& traverse)
{
    int n = traverse.numLinks();
    for(int i=0; i < n; ++i){
        Link* link = traverse[i];
        os << link->name();
        if(i != n){
            os << (traverse.isDownward(i) ? " => " : " <= ");
        }
    }
    os << std::endl;

    return os;
}
Exemple #12
0
void TestCreate() {
  ScopedPass pass("Link::Create()");
  OrNeuron neuron1;
  OrNeuron neuron2;
  Link * link = Link::Create(neuron1, neuron2);
  assert(neuron1.GetOutputCount() == 1);
  assert(neuron1.GetInputCount() == 0);
  assert(neuron2.GetOutputCount() == 0);
  assert(neuron2.GetInputCount() == 1);
  assert(&link->GetSender() == &neuron1);
  assert(&link->GetReceiver() == &neuron2);
  assert(link->GetSenderNext() == nullptr);
  assert(link->GetReceiverNext() == nullptr);
  
  Link * link1 = Link::Create(neuron1, neuron2);
  assert(neuron1.GetOutputCount() == 2);
  assert(neuron1.GetInputCount() == 0);
  assert(neuron2.GetOutputCount() == 0);
  assert(neuron2.GetInputCount() == 2);
  assert(&link1->GetSender() == &neuron1);
  assert(&link1->GetReceiver() == &neuron2);
  assert(link1->GetSenderNext() == link);
  assert(link1->GetReceiverNext() == link);
  assert(link->GetSenderNext() == nullptr);
  assert(link->GetReceiverNext() == nullptr);
  
  link->Remove();
  link1->Remove();
}
Exemple #13
0
void init_links(int num, const char *ip, int port){
	fdes = new Fdevents();
	free_links = new std::vector<Link *>();

	for(int i=0; i<num; i++){
		Link *link = Link::connect(ip, port);
		if(!link){
			fprintf(stderr, "connect error! %s\n", strerror(errno));
			exit(0);
		}
		fdes->set(link->fd(), FDEVENT_IN, 0, link);
		free_links->push_back(link);
	}
}
Exemple #14
0
Link* Link::find(Link* n)
{
    Link* p = n;

    p = p->to_first();

    while (p)
    {
        if (p->level == level && p->value == value) return p;
        p = p->succ;
    }

    return 0;
}
Exemple #15
0
Link* Link::find(Link* n)
{
	Link* p = this;

	p = p->to_first();

	while (p)
	{
		if (n->level == p->level && n->value == p->value) return p;
		p = p->succ;
	}

	return 0;
}
Get_Instance_Status Link_Provider::get_instance(
    const Link* model,
    Link*& instance)
{
    Link* link = _map.lookup(model);

    if (link)
    {
        instance = link->clone();
        return GET_INSTANCE_OK;
    }

    return GET_INSTANCE_NOT_FOUND;
}
Exemple #17
0
JointPath& JointPath::setGoal
(const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R)
{
    targetTranslationGoal = end_p;
    targetRotationGoal = end_R;
    
    Link* baseLink = linkPath.baseLink();
    baseLink->p() = base_p;
    baseLink->R() = base_R;

    needForwardKinematicsBeforeIK = true;

    return *this;
}
Exemple #18
0
Link* Link::find_lower(Link *n)
{
	Link* p = this;

	while (p->level > n->level - 1 && p->level > 1) p = p->to_first_total(); // Ведём поиск на уровне ниже, p->level - 1

	while (p)
	{
		if (p->level == n->level && p->value == n->value) return p;
		p = p->advance();
	}

	return 0;
}
Exemple #19
0
Link* Link::find_duplicate(Link *n)
{
	Link* p = this;

	p = p->to_first_total();

	while (p)
	{
		if (p->level == n->level && p->value == n->value) return p;
		p = p->advance();
	}

	return 0;
}
Exemple #20
0
Link * FirstFitAlgorithm::createDummyLink(Link * virtualLink, Assignment * assignment)
{
    if ( virtualLink == 0 )
        return 0;

    Element * first = getCastedAssignment(virtualLink->getFirst(), assignment);
    Element * second = getCastedAssignment(virtualLink->getSecond(), assignment);

    if ( first == 0 || second == 0 )
        return 0;

    Link * dummyLink = new Link("dummy_link", virtualLink->getCapacity(), virtualLink->getMaxCapacity());
    dummyLink->bindElements(first, second);
    return dummyLink;
}
	static void Add(Link** firstLink, int value, int pos)
	{
		Link* node = new Link(value);
		Link* headRoom = new Link(-1);
		headRoom->setNext(*firstLink);
		Link* cur = headRoom;
		while (cur->getNext() && pos > 0)
		{
			cur = cur->getNext();
			pos--;
		}
		node->setNext(cur->getNext());
		cur->setNext(node);
		*firstLink = headRoom->getNext();
	}
void PrefsLoadManager::Cleanup(BOOL force)
{
	PLoader *q;
	Link *p = m_activeLoaders.First();
	while(p)
	{
		q = static_cast<PLoader *>(p);
		p = p->Suc();
		if(q->IsDead() || force)
		{
			q->Out();
			OP_DELETE(q);
		}
	}
}
Exemple #23
0
int main(int argc, char *argv[])
{
    Link a;
    a.creat_link();
    
    Iterator i(a.getHead());
    
    for (; i.get_ptr() != NULL; i++)
    {
        cout << *i << endl;    
    }

    a.free();
    return 0;
}
bool BodyRosJointControllerItem::control()
{
  controlTime_ = controllerTarget->currentTime();
  double updateSince = controlTime_ - joint_state_last_update_;

  if (updateSince > joint_state_update_period_) {
    // publish current joint states
    joint_state_.header.stamp.fromSec(controlTime_);

    for (int i = 0; i < body()->numJoints(); i++) {
      Link* joint = body()->joint(i);
      joint_state_.position[i] = joint->q();
      joint_state_.velocity[i] = joint->dq();
      joint_state_.effort[i] = joint->u();
    }

    joint_state_publisher_.publish(joint_state_);
    joint_state_last_update_ += joint_state_update_period_;
  }

  // apply joint force based on the trajectory message
  if (has_trajectory_ && controlTime_ >= trajectory_start_) {
    if (trajectory_index_ < points_.size()) {
      unsigned int joint_size = joint_names_.size();

      for (size_t i = 0; i < joint_size; ++i) {
        std::map<std::string, int>::const_iterator it = joint_number_map_.find(joint_names_[i]);

        if (it != joint_number_map_.end()) {
          apply_message(body()->joint((*it).second), i, &points_[ trajectory_index_ ]);
        } else {
          ROS_WARN("Unknown joint name: %s", joint_names_[i].c_str());
        }
      }

      ros::Duration duration(points_[trajectory_index_].time_from_start.sec,
                             points_[trajectory_index_].time_from_start.nsec);
      trajectory_start_ += duration.toSec();
      trajectory_index_++;
    } else {
      has_trajectory_ = false;
    }
  }

  keep_attitude();

  return true;
}
int main()
{
    Link* norse_gods = new Link("Thor");
    norse_gods = norse_gods->insert(new Link("Odin"));
    norse_gods = norse_gods->insert(new Link("Zeus"));
    norse_gods = norse_gods->insert(new Link("Freia"));

    Link* greek_gods = new Link("Hera");
    greek_gods = greek_gods->insert(new Link("Athena"));
    greek_gods = greek_gods->insert(new Link("Mars"));
    greek_gods = greek_gods->insert(new Link("Poseidon"));

    Link* p = greek_gods->find("Mars");
    if (p) p->value = "Ares";

    // Move Zeus into his correct Pantheon:
    {
        Link* p = norse_gods->find("Zeus");
        if (p) {
            if (p==norse_gods)
                norse_gods = p->next();
            p->erase();
            greek_gods = greek_gods->insert(p);
        }
    }

	norse_gods->add(new Link("loki"));
    // Finally, let's print out those lists:

	print_all(norse_gods);
    cout<<"\n";
	norse_gods=norse_gods->advance(2); // test advance
    print_all(norse_gods);
    cout<<"\n";

    print_all(greek_gods);
    cout<<"\n";
	cout<<greek_gods->node_count()<<endl; // test node count

	Link* god_list=new Link("Apollo");
	god_list=god_list->add(new Link("Aphrodite"));
	god_list=god_list->add(new Link("Artemis"));
	god_list=god_list->add(new Link("Hades"));
	god_list=god_list->first();
	print_all(god_list);
	cout<<endl;
	keep_window_open();
}
Exemple #26
0
Link* Link::to_last_total()
{
	Link* p = this;

	p = p->to_first();

	while (p->upper)
	{
		p = p->upper;
	}
	while (p->succ)
	{
		p = p->succ;
	}
	return p;
}
    void insert(double key)
    {
	Link* pCurrent = pFirst;
	Link* pPrevious = NULL;
	Link* pNewLink = new Link(key);
	while(pCurrent != NULL && key > pCurrent->getData())
	{
	    pPrevious = pCurrent;
	    pCurrent = pCurrent->pNext;
	}
	if(pPrevious == NULL)
	    pFirst = pNewLink;
	else
	    pPrevious->pNext = pNewLink;
	pNewLink->pNext = pCurrent;
    } // end insert()
Exemple #28
0
Link* Link::advance()
{
	Link* p = this;

	if (p->value == system_length && p->level < system_levels)
	{
		p = p->to_first();
		if (p->upper) 
		{
			p = p->upper;
			return p;
		}
	}
	p = p->succ;
	return p;
}
Exemple #29
0
static void copyFrameToBodyState(FrameType& frame, Body& body)
{
    const BodyMotion& motion = frame.motion();
    int numJoints = std::min(body.numJoints(), motion.numJoints());
    const MultiValueSeq::Frame q = motion.jointPosSeq()->frame(frame.frame());
    for(int i=0; i < numJoints; ++i){
        body.joint(i)->q() = q[i];
    }
    int numLinks =  std::min(body.numLinks(), motion.numLinks());
    const MultiSE3Seq::Frame p = motion.linkPosSeq()->frame(frame.frame());
    for(int i=0; i < numLinks; ++i){
        Link* link = body.link(i);
        link->p() = p[i].translation();
        link->R() = p[i].rotation().toRotationMatrix();
    }
}
Exemple #30
0
Link* Link::find_total(Link *n)
{
	if (n == 0) return this;

	Link* p = this;

	p = p->to_first_total();

	while (p)
	{
		if (n->level == p->level && n->value == p->value) return n;
		p = p->advance();
	}

	return 0;
}