Apollo 10.0
自动驾驶开放平台
math_utils.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include <cmath>
20#include <utility>
21
22namespace apollo {
23namespace common {
24namespace math {
25
26double Sqr(const double x) { return x * x; }
27
28double CrossProd(const Vec2d& start_point, const Vec2d& end_point_1,
29 const Vec2d& end_point_2) {
30 return (end_point_1 - start_point).CrossProd(end_point_2 - start_point);
31}
32
33double InnerProd(const Vec2d& start_point, const Vec2d& end_point_1,
34 const Vec2d& end_point_2) {
35 return (end_point_1 - start_point).InnerProd(end_point_2 - start_point);
36}
37
38double CrossProd(const double x0, const double y0, const double x1,
39 const double y1) {
40 return x0 * y1 - x1 * y0;
41}
42
43double InnerProd(const double x0, const double y0, const double x1,
44 const double y1) {
45 return x0 * x1 + y0 * y1;
46}
47
48double WrapAngle(const double angle) {
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;
51}
52
53double NormalizeAngle(const double angle) {
54 double a = std::fmod(angle + M_PI, 2.0 * M_PI);
55 if (a < 0.0) {
56 a += (2.0 * M_PI);
57 }
58 return a - M_PI;
59}
60
61double AngleDiff(const double from, const double to) {
62 return NormalizeAngle(to - from);
63}
64
65int RandomInt(const int s, const int t, unsigned int rand_seed) {
66 if (s >= t) {
67 return s;
68 }
69 return s + rand_r(&rand_seed) % (t - s + 1);
70}
71
72double RandomDouble(const double s, const double t, unsigned int rand_seed) {
73 return s + (t - s) / 16383.0 * (rand_r(&rand_seed) & 16383);
74}
75
76// Gaussian
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));
80}
81
82Eigen::Vector2d RotateVector2d(const Eigen::Vector2d& v_in,
83 const double theta) {
84 const double cos_theta = std::cos(theta);
85 const double sin_theta = std::sin(theta);
86
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();
89
90 return {x, y};
91}
92
93std::pair<double, double> Cartesian2Polar(double x, double y) {
94 double r = std::sqrt(x * x + y * y);
95 double theta = std::atan2(y, x);
96 return std::make_pair(r, theta);
97}
98
99double check_negative(double input_data) {
100 if (std::signbit(input_data)) {
101 input_data = -input_data;
102 }
103 return input_data;
104}
105
106} // namespace math
107} // namespace common
108} // namespace apollo
Implements a class of 2-dimensional vectors.
Definition vec2d.h:42
Math-related util functions.
double Sqr(const double x)
Definition math_utils.cc:26
int RandomInt(const int s, const int t, unsigned int rand_seed)
Get a random integer between two integer values by a random seed.
Definition math_utils.cc:65
double Gaussian(const double u, const double std, const double x)
Definition math_utils.cc:77
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.
Definition math_utils.cc:28
double RandomDouble(const double s, const double t, unsigned int rand_seed)
Get a random double between two integer values by a random seed.
Definition math_utils.cc:72
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
Definition math_utils.cc:48
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.
Definition math_utils.cc:33
std::pair< double, double > Cartesian2Polar(double x, double y)
Definition math_utils.cc:93
double AngleDiff(const double from, const double to)
Calculate the difference between angle from and to
Definition math_utils.cc:61
double check_negative(double input_data)
Definition math_utils.cc:99
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53
Eigen::Vector2d RotateVector2d(const Eigen::Vector2d &v_in, const double theta)
Definition math_utils.cc:82
class register implement
Definition arena_queue.h:37
Definition future.h:29