text: /******************************************************************************
   * Copyright 2017 The Apollo Authors. All Rights Reserved.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *****************************************************************************/
  
  // apollo/modules/common/math/math_utils.cc
  
  #include "utils/apollo_math_utils.h"
  
  #include <utility>
  
  namespace bubblefs {
  namespace myapollo {
  namespace common {
  namespace math {
  
  double Sqr(const double x) { return x * x; }
  
  double CrossProd(const Vec2d& start_point, const Vec2d& end_point_1,
                   const Vec2d& end_point_2) {
    return (end_point_1 - start_point).CrossProd(end_point_2 - start_point);
  }
  
  double InnerProd(const Vec2d& start_point, const Vec2d& end_point_1,
                   const Vec2d& end_point_2) {
    return (end_point_1 - start_point).InnerProd(end_point_2 - start_point);
  }
  
  double CrossProd(const double x0, const double y0, const double x1,
                   const double y1) {
    return x0 * y1 - x1 * y0;
  }
  
  double InnerProd(const double x0, const double y0, const double x1,
                   const double y1) {
    return x0 * x1 + y0 * y1;
  }
  
  double WrapAngle(const double angle) {
    const double new_angle = std::fmod(angle, M_PI * 2.0);
    return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle;
  }
  
  double NormalizeAngle(const double angle) {
    double a = std::fmod(angle + M_PI, 2.0 * M_PI);
    if (a < 0.0) {
      a += (2.0 * M_PI);
    }
    return a - M_PI;
  }
  
  double AngleDiff(const double from, const double to) {
    return NormalizeAngle(to - from);
  }
  
  int RandomInt(const int s, const int t, unsigned int rand_seed) {
    if (s >= t) {
      return s;
    }
    return s + rand_r(&rand_seed) % (t - s + 1);
  }
  
  double RandomDouble(const double s, const double t, unsigned int rand_seed) {
    return s + (t - s) / 16383.0 * (rand_r(&rand_seed) & 16383);
  }
  
  // Gaussian
  double Gaussian(const double u, const double std, const double x) {
    return (1.0 / std::sqrt(2 * M_PI * std * std)) *
           std::exp(-(x - u) * (x - u) / (2 * std * std));
  }
  
  // Sigmoid
  double Sigmoid(const double x) { return 1.0 / (1.0 + std::exp(-x)); }
  
  }  // namespace math
  }  // namespace common
  }  // namespace myapollo
  }  // namespace bubblefs