C++ std::acos方法代碼示例

TEST(AgradFwdAcos,FvarFvarVar_2ndDeriv) {
  using stan::agrad::fvar;
  using stan::agrad::var;
  using std::acos;

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

  fvar<fvar<var> > b = acos(z);

  AVEC y = createAVEC(z.val_.val_);
  VEC g;
  b.val_.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.5 * 2.0 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), g[0]);

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

  fvar<fvar<var> > c = acos(w);

  AVEC p = createAVEC(w.val_.val_);
  VEC q;
  c.d_.val_.grad(p,q);
  EXPECT_FLOAT_EQ(-0.5 * 2.0 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), q[0]);
}

TEST(AgradFwdAcos,FvarFvarDouble) {
  using stan::math::fvar;
  using std::acos;

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

  fvar<fvar<double> > a = acos(x);

  EXPECT_FLOAT_EQ(acos(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), a.val_.d_);
  EXPECT_FLOAT_EQ(0, a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);

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

  a = acos(y);
  EXPECT_FLOAT_EQ(acos(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.val_.d_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);
}

template<typename T> T bounded_acos(T v)
{
  using std::acos;
  using std::min;
  using std::max;
  return acos((max)(T(-1),(min)(v,T(1))));
}

void VKDrone::aimAt(double x, double y) {
    // Angle from NORTH to "ang" attribute restricted to [-2PI, 2PI].
    double facingAngle = fmod(myShip->ang, 360) * (PI / 180.0);

    // Ajust facingAngle to [-PI, PI]. West -> negative, East -> positive.
    if (facingAngle > PI) {
        facingAngle -= 2 * PI;
    } else if (facingAngle < -PI) {
        facingAngle += 2 * PI;
    }

    // Unitary vector of facing direction from NORTH.
    double facingX = -1 * sin(facingAngle);
    double facingY = cos(facingAngle);

    // Vector to target with respect to myShip.
    double targetX = x - myShip->posx;
    double targetY = y - myShip->posy;

    // Find the angle [0,PI] between the vectors using "dot (or inner) product".
    double dotProduct = facingX * targetX + facingY * targetY;
    double norm = sqrt(facingX * facingX + facingY * facingY) *
                  sqrt(targetX * targetX + targetY * targetY);
    double result = max(min(dotProduct / norm, 1.0), -1.0);
    double angleToTarget = acos(result);

    // Cross product sign tells if target is at right (negative angleToTarget).
    if (facingX * targetY - facingY * targetX < 0) {
        angleToTarget *= -1.0;
    }

    sideThrustFront = rotationPID.compute(0.0, angleToTarget);
    sideThrustBack = -sideThrustFront;
}

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

    inline
    fvar<T>
    acos(const fvar<T>& x) {
      using std::acos;
      using std::sqrt;
      using stan::math::square;

      return fvar<T>(acos(x.val_), x.d_ / -sqrt(1 - square(x.val_)));
    }

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

TEST(AgradFwdAcos,FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::acos;

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

  fvar<fvar<var> > b = acos(z);

  EXPECT_FLOAT_EQ(acos(0.5), b.val_.val_.val());
  EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), b.val_.d_.val());
  EXPECT_FLOAT_EQ(0, b.d_.val_.val());
  EXPECT_FLOAT_EQ(0, b.d_.d_.val());

  AVEC y = createAVEC(z.val_.val_);
  VEC g;
  b.val_.val_.grad(y,g);
  EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]);

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

  b = acos(w);
  EXPECT_FLOAT_EQ(acos(0.5), b.val_.val_.val());
  EXPECT_FLOAT_EQ(-2.0 / sqrt(1.0 - 0.5 * 0.5), b.d_.val_.val());
  EXPECT_FLOAT_EQ(0, b.val_.d_.val());
  EXPECT_FLOAT_EQ(0, b.d_.d_.val());

  AVEC p = createAVEC(w.val_.val_);
  VEC q;
  b.val_.val_.grad(p,q);
  EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), q[0]);
}

void Compensator::motion_compensation(sensor_msgs::PointCloud2::Ptr& msg,
                                      const double timestamp_min,
                                      const double timestamp_max,
                                      const Eigen::Affine3d& pose_min_time,
                                      const Eigen::Affine3d& pose_max_time) {
  using std::abs;
  using std::sin;
  using std::acos;

  Eigen::Vector3d translation =
      pose_min_time.translation() - pose_max_time.translation();
  Eigen::Quaterniond q_max(pose_max_time.linear());
  Eigen::Quaterniond q_min(pose_min_time.linear());
  Eigen::Quaterniond q1(q_max.conjugate() * q_min);
  Eigen::Quaterniond q0(Eigen::Quaterniond::Identity());
  q1.normalize();
  translation = q_max.conjugate() * translation;

  int total = msg->width * msg->height;

  double d = q0.dot(q1);
  double abs_d = abs(d);
  double f = 1.0 / (timestamp_max - timestamp_min);

  // Threshold for a "significant" rotation from min_time to max_time:
  // The LiDAR range accuracy is ~2 cm. Over 70 meters range, it means an angle
  // of 0.02 / 70 =
  // 0.0003 rad. So, we consider a rotation "significant" only if the scalar
  // part of quaternion is
  // less than cos(0.0003 / 2) = 1 - 1e-8.
  if (abs_d < 1.0 - 1.0e-8) {
    double theta = acos(abs_d);
    double sin_theta = sin(theta);
    double c1_sign = (d > 0) ? 1 : -1;
    for (int i = 0; i < total; ++i) {
      size_t offset = i * msg->point_step;
      Scalar* x_scalar =
          reinterpret_cast<Scalar*>(&msg->data[offset + x_offset_]);
      if (std::isnan(*x_scalar)) {
        ROS_DEBUG_STREAM("nan point do not need motion compensation");
        continue;
      }
      Scalar* y_scalar =
          reinterpret_cast<Scalar*>(&msg->data[offset + y_offset_]);
      Scalar* z_scalar =
          reinterpret_cast<Scalar*>(&msg->data[offset + z_offset_]);
      Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);

      double tp = 0.0;
      memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
             timestamp_data_size_);
      double t = (timestamp_max - tp) * f;

      Eigen::Translation3d ti(t * translation);

      double c0 = sin((1 - t) * theta) / sin_theta;
      double c1 = sin(t * theta) / sin_theta * c1_sign;
      Eigen::Quaterniond qi(c0 * q0.coeffs() + c1 * q1.coeffs());

      Eigen::Affine3d trans = ti * qi;
      p = trans * p;
      *x_scalar = p.x();
      *y_scalar = p.y();
      *z_scalar = p.z();
    }
    return;
  }
  // Not a "significant" rotation. Do translation only.
  for (int i = 0; i < total; ++i) {
    Scalar* x_scalar =
        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + x_offset_]);
    if (std::isnan(*x_scalar)) {
      ROS_DEBUG_STREAM("nan point do not need motion compensation");
      continue;
    }
    Scalar* y_scalar =
        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + y_offset_]);
    Scalar* z_scalar =
        reinterpret_cast<Scalar*>(&msg->data[i * msg->point_step + z_offset_]);
    Eigen::Vector3d p(*x_scalar, *y_scalar, *z_scalar);

    double tp = 0.0;
    memcpy(&tp, &msg->data[i * msg->point_step + timestamp_offset_],
           timestamp_data_size_);
    double t = (timestamp_max - tp) * f;
    Eigen::Translation3d ti(t * translation);

    p = ti * p;
    *x_scalar = p.x();
    *y_scalar = p.y();
    *z_scalar = p.z();
  }
}

 inline T0
 operator()(const T0& arg1) const {
   return acos(arg1);
 }

/** Vector Angle
 *
 * cos <a> = (a * b) / (|a|*|b|)
 */ 
double angle(const Point& p1, const Point& p2, const Point& p3){
    Segment s1 = Segment(p2,p1);
    Segment s2 = Segment(p2,p3);

    return acos(scalar_product(s1,s2) / (s2.magnitude() * s1.magnitude()));
}