C++ position类代码示例

inline constexpr
bool
operator>=(const position<TTag, TDistance1>& x,
           const position<TTag, TDistance2>& y)
{
    return x.distance_from_origin() >= y.distance_from_origin();
}

void init(int       argc,
          char      **argv,
          uint      displayMode,
          position  initPos,
          size      initSize,
          string    windowTitle
          )
{
    glutInit(&argc, argv);
    //Simple buffer
    glutInitDisplayMode(displayMode);
    glutInitWindowPosition(initPos.getX(),initPos.getY());
    glutInitWindowSize(initSize.getWidth(),initSize.getHeight());
    glutCreateWindow(windowTitle.c_str());
    initRendering();

    glutDisplayFunc(draw);
    glutIdleFunc(animate);

    glutKeyboardFunc(keyboardListener);
    glutSpecialFunc(specialKeyListener);


    glutMouseFunc(mouseListener);

    //Call to the drawing function


}

Shell::Shell(position startPos) // Constructor
{
	pos.set(startPos.getX(), startPos.getY(), startPos.getTh());
	seeTank = false;
	seeAi = false;
	float x,y;
	x = pos.getX();
	y = pos.getY();
	updateBb();
}

	position treeSearch(object e,position P) //return a position after searching an element
	{
		if (isExternal(P))
			return P;

		if (P.element()==e)
			return P;
		else if (e<P.element())
			return treeSearch(e,leftChild(P));
		else 
			return treeSearch(e,rightChild(P));
	}

inline constexpr
position<TTag, TToDistance>
position_cast(const position<TTag, TFromDistance>& pos)
{
    return position<TTag, TToDistance>(distance_cast<TToDistance>(
                            pos.distance_from_origin()));
}

bool
RegionFilteredSensor::filterContact(const position & boundaryPos0, const position & boundaryPos1, const position & ps){
	double p0x = boundaryPos0.x();
	double p0y = boundaryPos0.y();
	double p0z = boundaryPos0.z();
	double p1x = boundaryPos1.x();
	double p1y = boundaryPos1.y();
	double p1z = boundaryPos1.z();

	if((p0x <= ps[0] && p0y <= ps[1] && p0z <= ps[2]) &&
		(p1x >= ps.x() && p1y >= ps.y() && p1z >= ps.z())){
		return true;
	}
	return false;
}

/// Returns true if the specified position is inside (inclusive)
/// the region.
///
/// @param[in] p Point to test.
/// @retval true Point is in the region or on regions boundary.
/// @retval false Point outside the region.
///
bool region::inside(const position & p) const
{
	// testing latitude is easy, there is no date line, no wrap-arround
	if (p.lat() > p0_.lat())
		return false;
	if (p.lat() < p1_.lat())
		return false;

	// testint longitude

	// shifted longitudes, now between 0..360 (date line, eastwards)
	const double plon_s = 180.0 + p.lon();
	const double p0lon_s = 180.0 + p0_.lon();
	const double p1lon_s = 180.0 + p1_.lon();

	// p is west of p0, but not west enough to reach p1
	if ((plon_s < p0lon_s) && (plon_s > p1lon_s))
		return false;

	return true;
}

bool operator<(const position & a, const position & b)
{
    if(a.x() < b.x() || a.y() < b.y() || a.z() < b.z())
        return true;
    return false;

}

/// Initializes the region by the specified corners
///
/// @code
/// a0
/// +--------------+
/// |              |
/// |              |
/// |              |
/// +--------------+ a1
/// @endcode
///
/// @note There is no sorting of coordinates, the positions a0/a1 must
///       already be top/left,bottom/right ordering. There is no automatic
///       way to sort them because of the international date line.
///
/// @param[in] a0 Position top/left
/// @param[in] a1 Position bottom/right
/// @exception std::invalid_argument Positions are not correct. The latitude
///   of the second point <tt>p1</tt> is northerly than <tt>p0</tt>. Or positions
///   are party or fully the same.
///
region::region(const position & a0, const position & a1)
	: p0_(a0)
	, p1_(a1)
{
	if ((a0.lat() == a1.lat()) || (a0.lon() == a1.lon()))
		throw std::invalid_argument{"specified region lacks a dimension"};
	if (a0.lat() < a1.lat())
		throw std::invalid_argument{"specified region is upside down"};
}

void add(){
  node *prevlast = 0;
  while (last.pos == len && last.to != root)
    last = go(last.to->p->lnk,last.to->L,last.pos);
  while (!last.can(s[len]-'a')) {
    node* lastnode = split(last.to, last.pos);
    addleaf(lastnode,len);
    if (prevlast) prevlast->lnk = lastnode;
    prevlast = lastnode;
    if (lastnode == root) {
      last = position(root,0);
      break;
    }
    assert(lastnode->p->lnk);
    last = go(lastnode->p->lnk,lastnode->L,last.pos);
  }
  if (prevlast && !prevlast->lnk){
    assert(last.to && last.pos == last.to->R);
    prevlast->lnk = last.to;
  }
  assert(last.can(s[len]-'a'));
  last.next(s[len]-'a');
  len++;
}

std::vector<int> person::choose_move (position &pos, bool p)
{
	in_out[options[0]]->draw(pos);
	in_out[options[0]]->message("You are ");
	if (p)
	{	
		in_out[options[0]]->message("red");
	}
	else
	{
		in_out[options[0]]->message("black");
	}
	in_out[options[0]]->message(".\n\nWhich piece do you want to move?\n");
	std::vector<int> o = in_out[options[0]]->choose_point();
	if (pos.board[o[0]][o[1]] * (2 *!p - 1) <= 0)
	{
		in_out[options[0]]->message("Please choose one of your pieces.\n\n");
		return choose_move(pos, p);
	}
	in_out[options[0]]->message("\nWhere do you want to move it?\n");
	std::vector<int> n = in_out[options[0]]->choose_point();
	std::vector<int> m;
	bool is_valid = false;
	std::vector<std::vector<int> > z;
	pos.valid_moves(z, p);
	for (unsigned int i = 0; i < z.size(); i ++)
	{
		if ((o[0] == z[i][0]) && (o[1] == z[i][1]) && (n[0] == z[i][2]) && (n[1] == z[i][3]))
		{
			is_valid = true;
		}
	}
	if (is_valid)
	{
		m.push_back(o[0]);
		m.push_back(o[1]);
		m.push_back(n[0]);
		m.push_back(n[1]);
		return m;
	}
	else
	{
		in_out[options[0]]->message("\nPlease choose a valid move.\n\n");
		return choose_move(pos, p);
	}
}

void navCallback(const ardrone_autonomy::Navdata &msg)
{
	static bool start=true;
	static float last_time = 0;
	if(start){
		start = false;
		pos.pos_f(0) = -1.95;
		pos.pos_f(1) = 0;
		pos.pos_b(0) = -1.95;
		pos.pos_b(1) = 0;
		last_time = msg.tm;
	}
	geometry_msgs::PoseStamped rawpos_b;
	geometry_msgs::PoseStamped rawpos_f;
	Vector3f raw_v ;

	
	raw_v(0) = msg.vx;
	raw_v(1) = msg.vy;
	raw_v(2) = msg.vz;

	float dt = (msg.tm - last_time)/1000000.0;
	last_time = msg.tm;
	pos.pos_b += raw_v * dt / 1000.0;
	pos.get_R_field_body(pos.yaw/57.3);
	pos.pos_f = pos.R_field_body * pos.pos_b;

	rawpos_b.pose.position.x = pos.pos_b(0);
	rawpos_b.pose.position.y = pos.pos_b(1);
	rawpos_b.pose.position.z = pos.pos_b(2);
	rawpos_f.pose.position.x = pos.pos_f(0);
	rawpos_f.pose.position.y = pos.pos_f(1);
	rawpos_f.pose.position.z = pos.pos_f(2);

	rawpos_b_pub.publish(rawpos_b);
	rawpos_f_pub.publish(rawpos_f);

	// 0: Unknown
	// 1: Inited
	// 2: Landed
	// 3,7: Flying
	// 4: Hovering
	// 5: Test (?)
	// 6: Taking off
	// 8: Landing
	// 9: Looping (?)
	flight.drone_state = msg.state;
}

location location::span(position b, unsigned n) {
    return location(b, position(b.row(), b.col() + n));
}

bool position::operator<(const position &other) const {
    if (row() < other.row()) return true;
    if (row() > other.row()) return false;
    return col() < other.col();
}

	nodeptr toPointer(position P) //return the pointer saved in position P
	{
		return P.pointer();
	}