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

TEST(AgradFwdAsin,FvarFvarVar_2ndDeriv) {
    using stan::math::fvar;
    using stan::math::var;
    using std::asin;

    fvar<fvar<var> > x;
    x.val_.val_ = 0.5;
    x.val_.d_ = 2.0;

    fvar<fvar<var> > a = asin(x);

    AVEC p = createAVEC(x.val_.val_);
    VEC g;
    a.val_.d_.grad(p,g);
    EXPECT_FLOAT_EQ(2.0 * 0.76980033, g[0]);

    fvar<fvar<var> > y;
    y.val_.val_ = 0.5;
    y.d_.val_ = 2.0;

    fvar<fvar<var> > b = asin(y);

    AVEC q = createAVEC(y.val_.val_);
    VEC r;
    b.d_.val_.grad(q,r);
    EXPECT_FLOAT_EQ(2.0 * 0.76980033, r[0]);
}

    // 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);
    }

    // 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;
        }
    }

inline GAUSS gauss_ini(double e, double phi0, double &chi, double &rc)
{
    using std::asin;
    using std::cos;
    using std::sin;
    using std::sqrt;
    using std::tan;

    double sphi = 0;
    double cphi = 0;
    double es = 0;

    GAUSS en;
    es = e * e;
    en.e = e;
    sphi = sin(phi0);
    cphi = cos(phi0);
    cphi *= cphi;

    rc = sqrt(1.0 - es) / (1.0 - es * sphi * sphi);
    en.C = sqrt(1.0 + es * cphi * cphi / (1.0 - es));
    chi = asin(sphi / en.C);
    en.ratexp = 0.5 * en.C * e;
    en.K = tan(0.5 * chi + detail::FORTPI)
           / (pow(tan(0.5 * phi0 + detail::FORTPI), en.C) * srat(en.e * sphi, en.ratexp));

    return en;
}

// 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 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);
		}
}

void complex_number_examples()
{
    Complex z1{0, 1};
    std::cout << std::setprecision(std::numeric_limits<typename Complex::value_type>::digits10);
    std::cout << std::scientific << std::fixed;
    std::cout << "Print a complex number: " << z1 << std::endl;
    std::cout << "Square it             : " << z1*z1 << std::endl;
    std::cout << "Real part             : " << z1.real() << " = " << real(z1) << std::endl;
    std::cout << "Imaginary part        : " << z1.imag() << " = " << imag(z1) << std::endl;
    using std::abs;
    std::cout << "Absolute value        : " << abs(z1) << std::endl;
    std::cout << "Argument              : " << arg(z1) << std::endl;
    std::cout << "Norm                  : " << norm(z1) << std::endl;
    std::cout << "Complex conjugate     : " << conj(z1) << std::endl;
    std::cout << "Projection onto Riemann sphere: " <<  proj(z1) << std::endl;
    typename Complex::value_type r = 1;
    typename Complex::value_type theta = 0.8;
    using std::polar;
    std::cout << "Polar coordinates (phase = 0)    : " << polar(r) << std::endl;
    std::cout << "Polar coordinates (phase !=0)    : " << polar(r, theta) << std::endl;

    std::cout << "\nElementary special functions:\n";
    using std::exp;
    std::cout << "exp(z1) = " << exp(z1) << std::endl;
    using std::log;
    std::cout << "log(z1) = " << log(z1) << std::endl;
    using std::log10;
    std::cout << "log10(z1) = " << log10(z1) << std::endl;
    using std::pow;
    std::cout << "pow(z1, z1) = " << pow(z1, z1) << std::endl;
    using std::sqrt;
    std::cout << "Take its square root  : " << sqrt(z1) << std::endl;
    using std::sin;
    std::cout << "sin(z1) = " << sin(z1) << std::endl;
    using std::cos;
    std::cout << "cos(z1) = " << cos(z1) << std::endl;
    using std::tan;
    std::cout << "tan(z1) = " << tan(z1) << std::endl;
    using std::asin;
    std::cout << "asin(z1) = " << asin(z1) << std::endl;
    using std::acos;
    std::cout << "acos(z1) = " << acos(z1) << std::endl;
    using std::atan;
    std::cout << "atan(z1) = " << atan(z1) << std::endl;
    using std::sinh;
    std::cout << "sinh(z1) = " << sinh(z1) << std::endl;
    using std::cosh;
    std::cout << "cosh(z1) = " << cosh(z1) << std::endl;
    using std::tanh;
    std::cout << "tanh(z1) = " << tanh(z1) << std::endl;
    using std::asinh;
    std::cout << "asinh(z1) = " << asinh(z1) << std::endl;
    using std::acosh;
    std::cout << "acosh(z1) = " << acosh(z1) << std::endl;
    using std::atanh;
    std::cout << "atanh(z1) = " << atanh(z1) << std::endl;
}

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));
		}
    }

void Rotation::inverseTransform(double& lat, double& lon) const {
    const double u = toRadians(lon);
    const double v = toRadians(lat);

    const double w = cos(v);
    const double x = cos(u) * w;
    const double y = sin(u) * w;
    const double z = sin(v);

    const double x2 = a11 * x + a21 * y + a31 * z;
    const double y2 = a12 * x + a22 * y + a32 * z;
    const double z2 = a13 * x + a23 * y + a33 * z;

    lat = toDegrees(asin(z2));
    lon = toDegrees(atan2(y2, x2));
}

/*
 * Compute the central angle, in radians, between two lat/lon pairs, in
 * radians. Uses the haversine formula with a spherical geoid approximation.
 */
double
central_angle(const double lat_a, const double lon_a, const double lat_b,
        const double lon_b)
{
    using std::cos;
    using std::asin;
    using std::sqrt;

    const double delta_lat = lat_b - lat_a;
    const double delta_lon = lon_b - lon_a;

    const double h = haversin(delta_lat) +
        cos(lat_a) * cos(lat_b) * haversin(delta_lon);
    const double c = 2.0 * asin( sqrt(h) );

    return c;
}

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);
}

int vectester (Vector zerovec)
{
  using std::abs;
  using std::acos;
  using std::asin;
  using std::atan;
  using std::floor;
  using std::pow;
  using std::sin;
  using std::sqrt;
  using std::tan;

  typedef typename Vector::value_type Scalar;

  Vector random_vec = zerovec;

  Vector error_vec = zerovec;

  std::srand(12345); // Fixed seed for reproduceability of failures

  // Avoid divide by zero errors or acos(x>1) NaNs later
  for (unsigned int i=0; i != random_vec.size(); ++i)
    random_vec.raw_at(i) = .25 + (static_cast<Scalar>(std::rand())/RAND_MAX/2);

  int returnval = 0;

  one_test(2*random_vec - random_vec - random_vec);

  one_test(3*random_vec - random_vec*3);

  one_test((random_vec + random_vec)/2 - random_vec);

  one_test(sqrt(random_vec) * sqrt(random_vec) - random_vec);
  one_test(random_vec*random_vec - pow(random_vec,2));
  one_test(sqrt(random_vec) - pow(random_vec,Scalar(.5)));

  one_test(random_vec - sin(asin(random_vec)));
  one_test(random_vec - tan(atan(random_vec)));

  one_test(floor(random_vec / 2));
  one_test(abs(random_vec) - random_vec);

  return returnval;
}

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);

		}