26double Sqr(
const double x) {
return x * x; }
29 const Vec2d& end_point_2) {
30 return (end_point_1 - start_point).CrossProd(end_point_2 - start_point);
34 const Vec2d& end_point_2) {
35 return (end_point_1 - start_point).InnerProd(end_point_2 - start_point);
38double CrossProd(
const double x0,
const double y0,
const double x1,
40 return x0 * y1 - x1 * y0;
43double InnerProd(
const double x0,
const double y0,
const double x1,
45 return x0 * x1 + y0 * y1;
49 const double new_angle = std::fmod(angle, M_PI * 2.0);
50 return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle;
54 double a = std::fmod(angle + M_PI, 2.0 * M_PI);
61double AngleDiff(
const double from,
const double to) {
65int RandomInt(
const int s,
const int t,
unsigned int rand_seed) {
69 return s + rand_r(&rand_seed) % (t - s + 1);
72double RandomDouble(
const double s,
const double t,
unsigned int rand_seed) {
73 return s + (t - s) / 16383.0 * (rand_r(&rand_seed) & 16383);
77double Gaussian(
const double u,
const double std,
const double x) {
78 return (1.0 / std::sqrt(2 * M_PI *
std *
std)) *
79 std::exp(-(x - u) * (x - u) / (2 *
std *
std));
84 const double cos_theta = std::cos(theta);
85 const double sin_theta = std::sin(theta);
87 auto x = cos_theta * v_in.x() - sin_theta * v_in.y();
88 auto y = sin_theta * v_in.x() + cos_theta * v_in.y();
94 double r = std::sqrt(x * x + y * y);
95 double theta = std::atan2(y, x);
96 return std::make_pair(r, theta);
100 if (std::signbit(input_data)) {
101 input_data = -input_data;
Implements a class of 2-dimensional vectors.
Math-related util functions.
double Sqr(const double x)
int RandomInt(const int s, const int t, unsigned int rand_seed)
Get a random integer between two integer values by a random seed.
double Gaussian(const double u, const double std, const double x)
double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1, const Vec2d &end_point_2)
Cross product between two 2-D vectors from the common start point, and end at two other points.
double RandomDouble(const double s, const double t, unsigned int rand_seed)
Get a random double between two integer values by a random seed.
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1, const Vec2d &end_point_2)
Inner product between two 2-D vectors from the common start point, and end at two other points.
std::pair< double, double > Cartesian2Polar(double x, double y)
double AngleDiff(const double from, const double to)
Calculate the difference between angle from and to
double check_negative(double input_data)
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Eigen::Vector2d RotateVector2d(const Eigen::Vector2d &v_in, const double theta)