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)))); }
TEST(AgradFwdAcos,FvarVar_1stDeriv) { using stan::agrad::fvar; using stan::agrad::var; using std::acos; fvar<var> x(0.5,0.3); fvar<var> a = acos(x); EXPECT_FLOAT_EQ(acos(0.5), a.val_.val()); EXPECT_FLOAT_EQ(-0.3 / sqrt(1.0 - 0.5 * 0.5), a.d_.val()); AVEC y = createAVEC(x.val_); VEC g; a.val_.grad(y,g); EXPECT_FLOAT_EQ(-1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]); }
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,FvarVar_2ndDeriv) { using stan::agrad::fvar; using stan::agrad::var; using std::acos; fvar<var> x(0.5,0.3); fvar<var> a = acos(x); AVEC z = createAVEC(x.val_); VEC h; a.d_.grad(z,h); EXPECT_FLOAT_EQ(-0.5 * 0.3 / (sqrt(1.0 - 0.5 * 0.5) * 0.75), h[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]); }
TEST(AgradFwdAcos,FvarFvarVar_3rdDeriv) { using stan::agrad::fvar; using stan::agrad::var; using std::acos; fvar<fvar<var> > z; z.val_.val_ = 0.5; z.val_.d_ = 1.0; z.d_.val_ = 1.0; fvar<fvar<var> > b = acos(z); AVEC y = createAVEC(z.val_.val_); VEC g; b.d_.d_.grad(y,g); EXPECT_FLOAT_EQ(-3.07920143567800, g[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())); }
TEST(AgradFwdAcos,Fvar) { using stan::math::fvar; using std::acos; using std::sqrt; using std::isnan; using stan::math::NEGATIVE_INFTY; fvar<double> x(0.5,1.0); fvar<double> a = acos(x); EXPECT_FLOAT_EQ(acos(0.5), a.val_); EXPECT_FLOAT_EQ(1 / -sqrt(1 - 0.5 * 0.5), a.d_); fvar<double> b = 2 * acos(x) + 4; EXPECT_FLOAT_EQ(2 * acos(0.5) + 4, b.val_); EXPECT_FLOAT_EQ(2 / -sqrt(1 - 0.5 * 0.5), b.d_); fvar<double> c = -acos(x) + 5; EXPECT_FLOAT_EQ(-acos(0.5) + 5, c.val_); EXPECT_FLOAT_EQ(-1 / -sqrt(1 - 0.5 * 0.5), c.d_); fvar<double> d = -3 * acos(x) + 5 * x; EXPECT_FLOAT_EQ(-3 * acos(0.5) + 5 * 0.5, d.val_); EXPECT_FLOAT_EQ(-3 / -sqrt(1 - 0.5 * 0.5) + 5, d.d_); fvar<double> y(3.4); y.d_ = 1.0; fvar<double> e = acos(y); isnan(e.val_); isnan(e.d_); fvar<double> z(1.0); z.d_ = 1.0; fvar<double> f = acos(z); EXPECT_FLOAT_EQ(acos(1.0), f.val_); EXPECT_FLOAT_EQ(NEGATIVE_INFTY, f.d_); fvar<double> z2(1.0+stan::math::EPSILON,1.0); fvar<double> f2 = acos(z2); EXPECT_TRUE(boost::math::isnan(f2.val_)); EXPECT_TRUE(boost::math::isnan(f2.d_)); fvar<double> z3(-1.0-stan::math::EPSILON,1.0); fvar<double> f3 = acos(z3); EXPECT_TRUE(boost::math::isnan(f3.val_)); EXPECT_TRUE(boost::math::isnan(f3.d_)); }