C++ std::atan2方法代码示例

 /**
  *  @brief Extract angles corresponding to an YXZ transformation
  *  @param angles Resulting angles, in order: phi,theta,psi about Y,X,Z
  *
  *  @note In the case where cos(phi)==0, we set psi = 0 (gimbal lock).
  */
 void extractYXZ(float angles[3]) const
 {
     static_assert(N == 3 && M == 3, "Matrix must be a member of SO3 group");
     
     using std::cos;
     using std::atan2;
     
     const matrix <3,3>& R = *this;
     
     //  numerical rounding may cause this value to drift out of bounds
     float nsin_phi = R(1,2);
     if (nsin_phi < -1.0f) {
         nsin_phi = -1.0f;
     } else if (nsin_phi > 1.0f) {
         nsin_phi = 1.0f;
     }
     
     angles[0] = asinf( -nsin_phi );   //  phi
     if (cos(angles[0]) < 1.0e-5f)
     {
         angles[1] = atan2(R(0,1), R(0,0));  //  theta
         angles[2] = 0.0f;                   //  psi
     }
     else
     {
         angles[1] = atan2(R(0,2), R(2,2));  //  theta
         angles[2] = atan2(R(1,0), R(1,1));  //  psi
     }
 }

void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    using std::atan2;
    using std::asin;
    using std::sqrt;
    using std::max;
    using std::min;
	controlSystem = controller->getControlSystem();
	systemUnlocked = controller->getSystemStatus();
	if (controlSystem==2 && systemUnlocked){
		roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                       1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
		roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18);		   
		controller->setRoll(roll_w);	
	} else {
		pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
		pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
		controller->setPitch(pitch_w);
		//cout << "SPEED: " << controller->longSpeed[pitch_w] << endl;
	}
    if (controlSystem == 0 && systemUnlocked){
		yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                    1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
		yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		controller->setYaw(yaw_w);
		}
}

    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                           1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
        float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
        float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                          1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));

        //hacky globals
        ROLL = roll;
        PITCH = pitch;
        YAW = yaw;

        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);

        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
        if (CURRENTPOSE == myo::Pose::waveIn) {
            WAVEINCOUNTER += 2;
        }
        if (CURRENTPOSE == myo::Pose::fingersSpread) {
            FINGSERSPREADCOUNTER += 2;
        }
        if (WAVEINCOUNTER != 1) {
            WAVEINCOUNTER -= 1;
        }
    }

    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        std::cout<<"orientation fucntion called"<<std::endl;
        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                           1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
        float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
        float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                        1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));

        osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
				p << osc::BeginMessage("/myo/orientation")
        << MAC
        << quat.x() << quat.y() << quat.z() << quat.w() << roll << pitch << yaw << osc::EndMessage;
		transmitSocket->Send(p.Data(), p.Size());
    
        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    }

/*!
 * Convert X, Y, Z in wgs84 to longitude, latitude, height
 * @param[in] x in meter
 * @param[in] y in meter
 * @param[in] z in meter
 * @return llh vector<double> contains longitude, latitude, height
 */
vector<double> ecef2llh(const double& x, const double& y, const double& z)
{
        double longitude {0.0}; /*< longitude in radian */
        double latitude {0.0}; /*< latitude in radian */
        double height {0.0}; /*< height in meter */

        double p = sqrt((x * x) + (y * y));
        double theta = atan2((z * A), (p * B));

        /* Avoid 0 division error */
        if(x == 0.0 && y == 0.0) {
                vector<double>
                llh {0.0, copysign(90.0,z), z - copysign(B,z)};
                return llh;
        } else {

                latitude = atan2(
                                   (z + (Er * Er * B * pow(sin(theta), 3))),
                                   (p - (E * E * A * pow(cos(theta), 3)))
                           );
                longitude = atan2(y, x);
                double n = A / sqrt(1.0 - E * E * sin(latitude) * sin(latitude));
                height = p / cos(latitude) - n;

                vector<double>
                llh {toDeg(longitude), toDeg(latitude), height};

                return llh;
        }
}

TEST(AgradFwdAtan2,FvarFvarDouble) {
  using stan::math::fvar;
  using std::atan2;

  fvar<fvar<double> > x;
  x.val_.val_ = 1.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1.5;
  y.d_.val_ = 1.0;

  double z = 1.5;

  fvar<fvar<double> > a = atan2(x,y);

  EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.val_.d_);
  EXPECT_FLOAT_EQ(-1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.d_.val_);
  EXPECT_FLOAT_EQ((1.5 * 1.5 - 1.5 * 1.5) / ((1.5 * 1.5 + 1.5 * 1.5) * (1.5 * 1.5 + 1.5 * 1.5)), a.d_.d_);

  a = atan2(x,z);
  EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.val_.d_);
  EXPECT_FLOAT_EQ(0.0, a.d_.val_);
  EXPECT_FLOAT_EQ(0.0, a.d_.d_);

  a = atan2(z, y);

  EXPECT_FLOAT_EQ(atan(1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(0.0, a.val_.d_);
  EXPECT_FLOAT_EQ(-1.5 / (1.5 * 1.5 + 1.5 * 1.5), a.d_.val_);
  EXPECT_FLOAT_EQ(0.0, a.d_.d_);
}

// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    using std::atan2;
    using std::asin;
    using std::sqrt;
    
    // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
    float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                       1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
    float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
    float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                      1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
    
    // Convert the floating point angles in radians to a scale from 0 to 20.
    float roll_f = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
    float pitch_f = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
    float yaw_f = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
    
    int roll_w = static_cast<int>(roll_f);
    int pitch_w = static_cast<int>(pitch_f);
    int yaw_w = static_cast<int>(yaw_f);
    
    MyoData &data = knownMyosData[myo];
    data.roll = roll_f/18.0*100.0;
    data.pitch = pitch_f/18.0*100.0;
    data.yaw = yaw_f/18.0*100.0;
    
    sendOrientation(myo, data.roll, data.pitch, data.yaw);
}

void DeviceCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
    Device * device = findDevice(myo);
    if ( device ) {
        using std::atan2;
        using std::asin;
        using std::sqrt;
        
        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                           1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
        float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
        float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                          1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
        
        device->q = quat;//set(quat.x(), quat.y(), quat.z(), quat.w());
        
        
        device->roll = roll;
        device->pitch = pitch;
        device->yaw = yaw;
        
        
        // Convert the floating point angles in radians to a scale from 0 to 20.
        device->roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        device->pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        device->yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
        
        float gyoro_g = sqrt(device->gyro.x()*device->gyro.x() + device->gyro.y()*device->gyro.y() + device->gyro.z()*device->gyro.z());
        float g = sqrt(device->accel.x()*device->accel.x() + device->accel.y()*device->accel.y() + device->accel.z()*device->accel.z());
        //            cout << gyoro_g << endl;
        //            cout << g << endl;
        if ( gyoro_g <= 0.2 ) device->gravity = g;
        
//        Quaternion4f q;
//        q = quat;
//        //.set(quat.x(), quat.z(), quat.y(), quat.w());
//        Vector3f linear_accel;
//        ofMatrix4x4 mat;
//        mat.translate(ofVec3f(0,device->gravity,0));
//        mat.rotate(q);
//        ofVec3f trans = mat.getTranslation();
//        
//        linear_accel = device->getAccel();
//        linear_accel.x = linear_accel.x - trans.x;
//        linear_accel.y = linear_accel.y - trans.z;
//        linear_accel.z = linear_accel.z - trans.y;
//
//        device->linear_accel.set(linear_accel);
        //            cout << device->getAccel() << endl;
        //            cout << mat.getTranslation() << endl;
        //            cout << linear_accel << endl;
    }
}

    // onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
    // as a unit quaternion.
    void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
    {
        using std::atan2;
        using std::asin;
        using std::sqrt;

        // Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
        float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
                           1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
        float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
        float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
                        1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
		
        // Convert the floating point angles in radians to a scale from 0 to 20.
        roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
        yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		float drop = 1.1;
		roll = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
        pitch = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
		if(pitch > 10)
		{	if(pitch < 20)
			{	pitch *= (drop*(20-pitch));
			}
		}
		else
		{	if(pitch != 10)
			{	pitch *= (drop*(10-pitch));
			}
		}	
        yaw = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
		if(yaw > 10.0000)
		{	if(yaw < 20.0000)
			{	yaw *= (drop*(20-yaw));
			}
			else
			{	yaw = 19.9999;
			}
		}
		else
		{	if(yaw != 10.0000)
			{	yaw *= (drop*(10-yaw));
			}
		}	
		if(color_input == true)
		{	float x = (GetSystemMetrics(SM_CXSCREEN));
			float y = (GetSystemMetrics(SM_CYSCREEN));
			SetCursorPos((((((20)-yaw))/(20))*x), ((pitch/(20))*y));
		}
    }

 typename stan::return_type<T_y, T_loc, T_scale>::type 
 cdf_function(const T_y& y, const T_loc& mu, const T_scale& sigma,
              const T3&, const T4&, const T5&) {
   using std::atan2;
   using stan::math::pi;
   return atan2(y-mu, sigma) / pi() + 0.5;
 }

   inline
   typename quan::where_<
      quan::meta::and_<
         quan::meta::dimensionally_equivalent<StaticUnit_L, StaticUnit_R>,
         quan::meta::or_<
            quan::meta::and_<
               quan::meta::allow_implicit_unit_conversions<StaticUnit_L>,
               quan::meta::allow_implicit_unit_conversions<StaticUnit_R>
            >,
            quan::meta::is_math_same_conversion_factor<
               typename quan::meta::get_conversion_factor<StaticUnit_L>::type,
               typename quan::meta::get_conversion_factor<StaticUnit_R>::type
            >
         >
      >,
      typename quan::angle_<
         typename quan::meta::binary_op<
            fixed_quantity<StaticUnit_L, NumericType_L>,
            quan::meta::divides,
            fixed_quantity<StaticUnit_R, NumericType_R>
         >::type
      >::rad
   >::type
   atan2(fixed_quantity<StaticUnit_L, NumericType_L> const & y,
         fixed_quantity<StaticUnit_R, NumericType_R> const & x)
   {
       typedef fixed_quantity<StaticUnit_L, NumericType_L> arg_type1;
       typedef fixed_quantity<StaticUnit_R, NumericType_R> arg_type2;

       // need to convert both quantities to a common unit
       // which this does, and if necessary promotes value_type
       typedef typename quan::meta::binary_op<
         arg_type1, quan::meta::minus, arg_type2
       >::type finest_grained_type;

       finest_grained_type ty = y;
       finest_grained_type tx = x;

       // 'type' of result of division must be a numeric
       typedef typename quan::meta::binary_op<
          finest_grained_type,
          quan::meta::divides,
          finest_grained_type
       >::type result_value_type;

       // To show this ... check it is a numeric
       static_assert(quan::meta::is_numeric<
                     result_value_type
                     >::value," result not numeric");

       typedef typename quan::angle_<result_value_type>::rad result_type;
   #ifndef QUAN_AVR_NO_CPP_STDLIB
       using std::atan2;
   #else
       using ::atan2;
   #endif
       return result_type {atan2(ty.numeric_value(), tx.numeric_value())};
   }

 /** Cartesian to spherical coordinates */
 inline static
 void cart2sph(real_type& r, real_type& theta, real_type& phi,
               const point_type& x) {
   using std::acos;
   using std::atan2;
   r = norm_2(x);
   theta = acos(x[2] / (r + 1e-100));
   phi = atan2(x[1], x[0]);
 }

void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
	using std::atan2;
	using std::asin;
	using std::sqrt;

	// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
	float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
					   1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
	float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
	float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
					1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));

	// Convert the floating point angles in radians to a scale from 0 to 20.
	roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 100);
	pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 100);
	yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 100);
}

void myouse::MyouseListener::onOrientationData(myo::Myo * myo, uint64_t timestamp,
	const myo::Quaternion<float> & rotation)
{
	using std::atan2;
	using std::asin;
	using std::sin;

	double newRoll = atan2(2.0f * (rotation.w() * rotation.x() + rotation.y() * rotation.z()),
		1.0f - 2.0f * (rotation.x() * rotation.x() + rotation.y() * rotation.y()));
	double newPitch = asin(2.0f * (rotation.w() * rotation.y() - rotation.z() * rotation.x()));
	double newYaw = atan2(2.0f * (rotation.w() * rotation.z() + rotation.x() * rotation.y()),
		1.0f - 2.0f * (rotation.y() * rotation.y() + rotation.z() * rotation.z()));

	double roll = newRoll - rollOffset;
	double pitch = newPitch - pitchOffset;
	double yaw = newYaw - yawOffset;

	if (xDir == myo::xDirectionTowardElbow) pitch *= -1;

	if (isScrolling)
	{
		scroll(-pitch * SCROLL_SPEED);
	}
	else
	{
		int x = SCREEN_WIDTH * (0.5 - X_SPEED * yaw);
		int y = SCREEN_HEIGHT * (0.5 + Y_SPEED * pitch);

		bool dragging = leftDown || rightDown || middleDown;
		float dist = sqrt((x - lastX) * (x - lastX) + (y - lastY) * (y - lastY));

		if (!dragging || dist > DRAG_THRESHOLD)
		{
			moveMouse(x, y);

			lastX = x;
			lastY = y;
		}
	}

	rawRoll = newRoll;
	rawPitch = newPitch;
	rawYaw = newYaw;
}

		// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
		// as a unit quaternion.
		void onOrientationData(Myo* myo, uint64_t timestamp, const Quaternion<float>& quat)
		{
			using std::atan2;
			using std::asin;
			using std::sqrt;
			using std::max;
			using std::min;
			// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
			float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
							   1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
			float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
			float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
							1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
			// Convert the floating point angles in radians to a scale from 0 to 18.
		  //  roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
		  //  pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
		  //  yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);

		}