C++ math::Vector类代码示例

    /*! \brief A ray-rod intersection test.
      
      A rod is a cylinder which is not infinite, but of limited
      length. The cylinder is defined using a single base vertex at
      the center of the bottom circular face and an axial vector
      pointing from the base vertex to the top vertex. This test
      ignores the back face of the rod. It is used to detect when a
      ray will enter a rod.
     
      \param T The origin of the ray relative to the base vertex.
      \param D The direction/velocity of the ray.
      \param A The axial vector of the rod.
      \param r Radius of the rod.
      \return The time until the intersection, or HUGE_VAL if no intersection.
    */
    inline double ray_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
    {
      double t = ray_cylinder(T, D, A / A.nrm(), r);
      double Tproj = ((T + t * D) | A);
      
      if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;

      return t;
    }

void ALSound::SetListener(const Math::Vector &eye, const Math::Vector &lookat)
{
    m_eye = eye;
    m_lookat = lookat;
    Math::Vector forward = lookat - eye;
    forward.Normalize();
    float orientation[] = {forward.x, forward.y, forward.z, 0.f, -1.0f, 0.0f};

    alListener3f(AL_POSITION, eye.x, eye.y, eye.z);
    alListenerfv(AL_ORIENTATION, orientation);
}

    /*! \brief A ray-inverse_rod intersection test.
    
      A rod is a cylinder which is not infinite, but of limited
      length. An inverse rod is used to test when a ray will exit a
      rod. The cylinder is defined using a single base vertex at the
      center of the bottom circular face and an axial vector
      pointing from the base vertex to the top vertex. This test
      ignores the back face of the rod.
     
      \param T The origin of the ray relative to the base vertex.
      \param D The direction/velocity of the ray.
      \param A The axial vector of the inverse rod.
      \param r Radius of the inverse rod.

      \tparam always_intersect If true, this will ensure that glancing
      ray's never escape the enclosing sphere by returning the time
      when the ray is nearest the sphere if the ray does not intersect
      the sphere.

      \return The time until the intersection, or HUGE_VAL if no intersection.
    */
    inline double ray_inv_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
    {
      double t = ray_inv_cylinder(T, D, A / A.nrm(), r);

      M_throw() << "Confirm that this function is correct";

      double Tproj = ((T + t * D) | A);
      
      if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;

      return t;
    }

    //! \brief A ray-sphere intersection test with backface culling.
    //!
    //! \param T The origin of the ray relative to the sphere center.
    //! \param D The direction/velocity of the ray.
    //! \param r The radius of the sphere.
    //! \return The time until the intersection, or HUGE_VAL if no intersection.
    inline double ray_sphere_bfc(const math::Vector& T,
				 const math::Vector& D,
				 const double& r)
    {
      double TD = (T | D);

      if (TD >= 0) return HUGE_VAL;
      
      double c = T.nrm2() - r * r;
      double arg = TD * TD - D.nrm2() * c;
      
      if (arg < 0) return HUGE_VAL;

      return  - c / (TD - std::sqrt(arg));
    }

    //! \brief A ray-inverse_sphere intersection test with backface culling.
    //!
    //! An inverse sphere means an "enclosing" sphere.
    //!
    //! \param T The origin of the ray relative to the inverse sphere
    //! center.
    //! \param D The direction/velocity of the ray.
    //! \param d The diameter of the inverse sphere.
    //! \return The time until the intersection, or HUGE_VAL if no intersection.
    inline double ray_inv_sphere_bfc(const math::Vector& T,
				     const math::Vector& D,
				     const double& r)
    {
      double D2 = D.nrm2();

      if (D2 == 0) return HUGE_VAL;
      
      double TD = T | D;
      double arg = TD * TD - D2 * (T.nrm2() - r * r);

      if (arg < 0) return HUGE_VAL;

      return (std::sqrt(arg) - TD) / D2;
    }

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _v_att.rollspeed;
	rates(1) = _v_att.pitchspeed;
	rates(2) = _v_att.yawspeed;

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit */
	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

void math::Matrix::assignToBucket(math::Vector &vector) {
	math::Space *space = vector.getSpace();

	int column = std::floor(vector.get(math::Dimension::X) / bucketSize[math::Dimension::X]);
	int row = std::floor(vector.get(math::Dimension::Y) / bucketSize[math::Dimension::Y]);
	int dimension = std::floor(std::min<double>(vector.get(math::Dimension::Z), Vector::getSpace()->getMax(math::Dimension::Z)) /
		bucketSize[math::Dimension::Z]);

	if (column == space->getMax(math::Dimension::X) / bucketSize[math::Dimension::X])
		column--;
	if (row == space->getMax(math::Dimension::Y) / bucketSize[math::Dimension::Y])
		row--;
	if (dimension == space->getMax(math::Dimension::Z) / bucketSize[math::Dimension::Z])
		dimension--;

	values[row][column][dimension]++;
}

template<UnsignedInt dimensions> std::size_t AbstractImage::dataSize(Math::Vector<dimensions, Int> size) const {
    /** @todo Code this properly when all @fn_gl{PixelStore} parameters are implemented */
    /* Row size, rounded to multiple of 4 bytes */
    const std::size_t rowSize = ((size[0]*pixelSize() + 3)/4)*4;

    /** @todo Can't this be done somewhat nicer? */
    size[0] = 1;
    return rowSize*size.product();
}

	OffcentreSpheresOverlapFunction(const math::Vector& rij, const math::Vector& vij, const math::Vector& omegai, const math::Vector& omegaj,
					const math::Vector& nu1, const math::Vector& nu2, const double diameter1, const double diameter2, 
					const double maxdist, const double t, const double invgamma, const double t_min, const double t_max):
	  w1(omegai), w2(omegaj), u1(nu1), u2(nu2), r12(rij), v12(vij), _diameter1(diameter1), _diameter2(diameter2), _invgamma(invgamma), _t(t), _t_min(t_min), _t_max(t_max)
	{
	  double Gmax = std::max(1 + t * invgamma, 1 + (t + t_max) * invgamma);
	  const double sigmaij = 0.5 * (_diameter1 + _diameter2);
	  const double sigmaij2 = sigmaij * sigmaij;
	  double magw1 = w1.nrm(), magw2 = w2.nrm();
	  double rijmax = Gmax * std::max(maxdist, rij.nrm());
#ifdef MAGNET_DEBUG
	  if (rij.nrm() > 1.0001 * maxdist)
	    std::cout << "WARNING!: Particle separation is larger than the maximum specified. " <<rij.nrm() << ">" << maxdist << "\n";
#endif
	  double magu1 = u1.nrm(), magu2 = u2.nrm();
	  double vijmax = v12.nrm() + Gmax * (magu1 * magw1 + magu2 * magw2) + std::abs(invgamma) * (magu1 + magu2);
	  double aijmax = Gmax * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2) + 2 * std::abs(invgamma) * (magu1 * magw1 + magu2 * magw2);
	  double dotaijmax = Gmax * (magu1 * magw1 * magw1 * magw1 + magu2 * magw2 * magw2 * magw2) + 3 * std::abs(invgamma) * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2);

	  _f1max = 2 * rijmax * vijmax + 2 * Gmax * std::abs(invgamma) * sigmaij2;
	  _f2max = 2 * vijmax * vijmax + 2 * rijmax * aijmax + 2 * invgamma * invgamma * sigmaij2;
	  _f3max = 6 * vijmax * aijmax + 2 * rijmax * dotaijmax;
	}

	std::array<double, nderivs> eval(const double dt = 0) const
	{
	  math::Vector u1new = Rodrigues(w1 * dt) * math::Vector(u1);
	  math::Vector u2new = Rodrigues(w2 * dt) * math::Vector(u2);

	  const double colldiam = 0.5 * (_diameter1 + _diameter2);
	  const double growthfactor = 1 + _invgamma * (_t + dt);
	  const math::Vector rij = r12 + dt * v12 + growthfactor * (u1new - u2new);
	  const math::Vector vij = v12 + growthfactor * ((w1 ^ u1new) - (w2 ^ u2new)) + _invgamma * (u1new - u2new);
	  const math::Vector aij = growthfactor * (-w1.nrm2() * u1new + w2.nrm2() * u2new) + 2 * _invgamma * ((w1 ^ u1new) - (w2 ^ u2new));
	  const math::Vector dotaij = growthfactor * (-w1.nrm2() * (w1 ^ u1new) + w2.nrm2() * (w2 ^ u2new)) + 3 * _invgamma * (-w1.nrm2() * u1new + w2.nrm2() * u2new);

	  std::array<double, nderivs> retval;
	  for (size_t i(0); i < nderivs; ++i)
	    switch (first_deriv + i) {
	    case 0: retval[i] = (rij | rij) - growthfactor * growthfactor * colldiam * colldiam; break;
	    case 1: retval[i] = 2 * (rij | vij) - 2 * _invgamma * growthfactor * colldiam * colldiam; break;
	    case 2: retval[i] = 2 * vij.nrm2() + 2 * (rij | aij) - 2 * _invgamma * _invgamma * colldiam * colldiam; break;
	    case 3: retval[i] = 6 * (vij | aij) + 2 * (rij | dotaij); break;
	    default:
	      M_throw() << "Invalid access";
	    }
	  return retval;
	}

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* throttle pid attenuation factor */
	float tpa =  fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;

	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp);

	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
				    /* if the axis is the yaw axis, do not update the integral if the limit is hit */
				    !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

void
MulticopterPositionControl::control_manual(float dt)
{
	_sp_move_rate.zero();

	if (_control_mode.flag_control_altitude_enabled) {
		if(_reset_mission)
		{
			_reset_mission = false;
			_mode_mission = 1 ;
			_hover_time = 0.0 ;
		}
		float height_hover_constant=-1.0;
		float hover_time_constant = 20.0;
		switch(_mode_mission)
		{	
			case 1:
				_sp_move_rate(2) = -0.8;
				if(_pos_sp(2)<=height_hover_constant)
					_mode_mission=2;
				break;
			case 2:
				_hover_time += dt;
				if(_hover_time>hover_time_constant)
				{
					_hover_time=0.0;
					_mode_mission=3;
				}
				break;
			case 3:
				_pos_sp_triplet.current.type =position_setpoint_s::SETPOINT_TYPE_LAND;
				break;
			default:
				/* move altitude setpoint with throttle stick */
				_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
				break;
		}
	}

	if (_control_mode.flag_control_position_enabled) {
		/* move position setpoint with roll/pitch stick */
		_sp_move_rate(0) = _manual.x;
		_sp_move_rate(1) = _manual.y;
	}

	/* limit setpoint move rate */
	float sp_move_norm = _sp_move_rate.length();

	if (sp_move_norm > 1.0f) {
		_sp_move_rate /= sp_move_norm;
	}

	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
	math::Matrix<3, 3> R_yaw_sp;
	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);

	if (_control_mode.flag_control_altitude_enabled) {
		/* reset alt setpoint to current altitude if needed */
		reset_alt_sp();
	}

	if (_control_mode.flag_control_position_enabled) {
		/* reset position setpoint to current position if needed */
		reset_pos_sp();
	}

	/* feed forward setpoint move rate with weight vel_ff */
	_vel_ff = _sp_move_rate.emult(_params.vel_ff);

	/* move position setpoint */
	_pos_sp += _sp_move_rate * dt;

	/* check if position setpoint is too far from actual position */
	math::Vector<3> pos_sp_offs;
	pos_sp_offs.zero();

	if (_control_mode.flag_control_position_enabled) {
		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
	}

	if (_control_mode.flag_control_altitude_enabled) {
		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
	}

	float pos_sp_offs_norm = pos_sp_offs.length();

	if (pos_sp_offs_norm > 1.0f) {
		pos_sp_offs /= pos_sp_offs_norm;
		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
	}
}

void
MulticopterPositionControl::task_main()
{
	warnx("started");

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	mavlink_log_info(_mavlink_fd, "[mpc] started");

	/*
	 * do subscriptions
	 */
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));

	parameters_update(true);

	/* initialize values of critical structs until first regular update */
	_arming.armed = false;

	/* get an initial update for all sensor and status data */
	poll_subscriptions();

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;

	const float alt_ctl_dz = 0.2f;
	const float pos_ctl_dz = 0.05f;

	math::Vector<3> sp_move_rate;
	sp_move_rate.zero();
	math::Vector<3> thrust_int;
	thrust_int.zero();
	math::Matrix<3, 3> R;
	R.identity();

	/* wakeup source */
	struct pollfd fds[1];

	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {
		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		poll_subscriptions();
		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
		t_prev = t;

		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			reset_int_z = true;
			reset_int_xy = true;
		}

		was_armed = _control_mode.flag_armed;

		update_ref();

		if (_control_mode.flag_control_altitude_enabled ||
		    _control_mode.flag_control_position_enabled ||
		    _control_mode.flag_control_climb_rate_enabled ||
		    _control_mode.flag_control_velocity_enabled) {

			_pos(0) = _local_pos.x;
			_pos(1) = _local_pos.y;
			_pos(2) = _local_pos.z;

			_vel(0) = _local_pos.vx;
			_vel(1) = _local_pos.vy;
			_vel(2) = _local_pos.vz;

			sp_move_rate.zero();

//.........这里部分代码省略.........

//==================================================
//! Dot Product
//==================================================
float Math::Vector::operator*( const Math::Vector& rhs ) const {
	return x*rhs.X() + y*rhs.Y() + z*rhs.Z();
}