freego first

src/common/3rd/CMakeLists.txt

@@ -0,0 +1 @@
+cmake_minimum_required(VERSION 3.8)

src/common/3rd/x86/math/include/math/aabox2d.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class AABox2d {
+public:
+    AABox2d() = default;
+    AABox2d(const Vec2d& center, const double length, const double width);
+    AABox2d(const Vec2d& one_corner, const Vec2d& opponent_corner);
+    AABox2d(const std::vector<Vec2d>& points);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double area() const { return _length * _width; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const AABox2d& box) const;
+
+    bool has_overlap(const AABox2d& box) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec2d& shift_vec);
+
+    void merge_from(const AABox2d& other_box);
+    void merge_from(const Vec2d& other_point);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/aabox3d.h

@@ -0,0 +1,57 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: aabox for 3d
+
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class AABox3d {
+public:
+    AABox3d() = default;
+    AABox3d(const Vec3d& center, const double length, const double width, const double height);
+    AABox3d(const Vec3d& one_corner, const Vec3d& opponent_corner);
+    AABox3d(const std::vector<Vec3d>& points);
+
+    AABox2d aabox2d() const;
+    const Vec3d& center() const { return _center; }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double height() const { return _height; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+    double min_z() const { return _center.z() - _half_height; }
+    double max_z() const { return _center.z() + _half_height; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec3d>* const corners) const;
+
+    bool is_point_in(const Vec3d& point) const;
+    bool is_point_on_boundary(const Vec3d& point) const;
+
+    double distance_to(const Vec3d& point) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec3d& shift_vec);
+
+    void merge_from(const AABox3d& other_box);
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _height = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _half_height = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/aaboxkdtree2d.h

@@ -0,0 +1,387 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABoxKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree2dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2dNode(const std::vector<ObjectPtr>& objects,
+        const AABoxKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return AABox2d({ _min_x, _min_y }, { _max_x, _max_y });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : obj1->aabox().min_y() < obj2->aabox().min_y();
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : obj1->aabox().max_y() > obj2->aabox().max_y();
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x() : object->aabox().min_y());
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x() : object->aabox().max_y());
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABoxKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        if (params.max_leaf_dimension >= 0.0 && std::max(_max_x - _min_x, _max_y - _min_y) <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        return dx * dx + dy * dy;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        return dx * dx + dy * dy;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec2d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec2d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        if (_max_x - _min_x >= _max_y - _min_y) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree2d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2d(const std::vector<ObjectType>& objects, const AABoxKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree2dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox2d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _root = nullptr;
+};
+
+}

src/common/3rd/x86/math/include/math/aaboxkdtree3d.h

@@ -0,0 +1,441 @@
+
+// Copyright 2018 Baidu. Inc All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+// Date: 2018-06-19 14:44
+//
+// Description: aaboxkdtree for 3d element
+
+#ifndef _MATH_COMMON_AABOXKDTREE3D_H_
+#define _MATH_COMMON_AABOXKDTREE3D_H_
+
+#pragma once
+
+#include "math/aabox3d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABox3dKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree3dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3dNode(const std::vector<ObjectPtr>& objects,
+        const AABox3dKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return AABox3d({ _min_x, _min_y, _min_z }, { _max_x, _max_y, _max_z });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().min_y() < obj2->aabox().min_y()
+                                                                              : obj1->aabox().min_z() < obj2->aabox().min_z());
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().max_y() > obj2->aabox().max_y()
+                                                                              : obj1->aabox().max_z() > obj2->aabox().max_z());
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().min_y()
+                                                                       : object->aabox().min_z()));
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().max_y()
+                                                                       : object->aabox().max_z()));
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        double max_dim = std::max(_max_x - _min_x, _max_y - _min_y);
+        max_dim = std::max(max_dim, _max_z - _min_z);
+        if (params.max_leaf_dimension >= 0.0 && max_dim <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        double dz = 0.0;
+        if (point.z() < _min_z) {
+            dz = _min_z - point.z();
+        } else if (point.z() > _max_z) {
+            dz = point.z() - _max_z;
+        }
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        const double dz = (point.z() > _mid_z ? (point.z() - _min_z) : (point.z() - _max_z));
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec3d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec3d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _min_z = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        _max_z = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+            _min_z = std::min(_min_z, object->aabox().min_z());
+            _max_z = std::max(_max_z, object->aabox().max_z());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+        _mid_z = (_min_z + _max_z) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        double dx = _max_x - _min_x;
+        double dy = _max_y - _min_y;
+        double dz = _max_z - _min_z;
+        if (dx >= dy && dx >= dz) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else if (dy >= dx && dy >= dz) {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        } else {
+            _partition = PARTITION_Z;
+            _partition_position = (_min_z + _max_z) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else if (_partition == PARTITION_Y) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_z() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_z() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _min_z = 0.0;
+    double _max_z = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+    double _mid_z = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+        PARTITION_Z = 3,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree3d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3d(const std::vector<ObjectType>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree3dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox3d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _root = nullptr;
+};
+
+}
+
+#endif // _MATH_COMMON_AABOXKDTREE3D_H_

src/common/3rd/x86/math/include/math/angle.h

@@ -0,0 +1,173 @@
+// Copyright (c) 2016 Baidu.com, Inc. All Rights Reserved
+// Author: hengliang
+
+// This file defines the Angle class, which uses an integer to represent an angle, and supports
+// commonly-used operations such as addition, subtraction, and trigonometry. Using an integer gives
+// the following advantages over the traditional floating-point representations:
+// - A signed integer automatically wraps an angle to the interval [-pi, pi).
+// - It makes the delta angle calculation easier and safer. For example, the difference between -179
+//   degrees and 179 degrees longitude is automatically 2 degrees.
+// - It uses less storage to achieve similar level of accuracy: Angle8 < Angle16 < float < Angle32 <
+//   double < Angle64, where < means "less precise than."
+// - Integer operations are more efficient.
+// - Angle8 and Angle16 allow super fast trigonometric functions via a 64-KiB lookup table.
+//
+// It is recommended to use the Angle class as much as possible, especially for the following
+// scenarios:
+// - Latitude/longitude: Angle32 can achieve < 1 cm resolution.
+// - Euler angles: Angle16 is precise enough for localization/object detection.
+//
+// Usage examples: Please refer to angle_test.cpp.
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <limits>
+
+namespace decision::math {
+
+template <typename T>
+class Angle {
+public:
+    static_assert(std::numeric_limits<T>::is_integer && std::numeric_limits<T>::is_signed,
+        "T must be a signed integer type");
+
+    static constexpr T RAW_PI = std::numeric_limits<T>::min();
+    static constexpr T RAW_PI_2 = -(std::numeric_limits<T>::min() >> 1);
+    static constexpr double DEG_TO_RAW = RAW_PI / -180.0;
+    static constexpr double RAD_TO_RAW = RAW_PI * -M_1_PI;
+    static constexpr double RAW_TO_DEG = -180.0 / RAW_PI;
+    static constexpr double RAW_TO_RAD = -M_PI / RAW_PI;
+
+    // Construct an object from an angle in degrees.
+    static Angle from_deg(double value)
+    {
+        Angle a(lround(value * DEG_TO_RAW));
+        return a;
+    }
+
+    // Construct an object from an angle in radians.
+    static Angle from_rad(double value)
+    {
+        Angle a(lround(value * RAD_TO_RAW));
+        return a;
+    }
+
+    explicit Angle(T value = 0)
+        : _value(value)
+    {
+    }
+
+    double to_deg() const
+    {
+        return _value * RAW_TO_DEG;
+    }
+
+    double to_rad() const
+    {
+        return _value * RAW_TO_RAD;
+    }
+
+    T raw() const
+    {
+        return _value;
+    }
+
+    Angle operator+=(Angle other)
+    {
+        _value += other._value;
+        return *this;
+    }
+
+    Angle operator-=(Angle other)
+    {
+        _value -= other._value;
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator*=(Scalar s)
+    {
+        _value = lround(_value * s);
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator/=(Scalar s)
+    {
+        _value = lround(_value / s);
+        return *this;
+    }
+
+private:
+    T _value;
+};
+
+using Angle8 = Angle<int8_t>;
+using Angle16 = Angle<int16_t>;
+using Angle32 = Angle<int32_t>;
+using Angle64 = Angle<int64_t>;
+
+template <typename T>
+Angle<T> operator+(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs += rhs;
+    return lhs;
+}
+
+template <typename T>
+Angle<T> operator-(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs -= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Angle<T> lhs, Scalar rhs)
+{
+    lhs *= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Scalar lhs, Angle<T> rhs)
+{
+    rhs *= lhs;
+    return rhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator/(Angle<T> lhs, Scalar rhs)
+{
+    lhs /= rhs;
+    return lhs;
+}
+
+template <typename T>
+double operator/(Angle<T> lhs, Angle<T> rhs)
+{
+    return double(lhs.raw()) / rhs.raw();
+}
+
+template <typename T>
+bool operator==(Angle<T> lhs, Angle<T> rhs)
+{
+    return lhs.raw() == rhs.raw();
+}
+
+template <typename T>
+bool operator!=(Angle<T> lhs, Angle<T> rhs)
+{
+    return !(lhs == rhs);
+}
+
+// Fast trigonometric functions. Single precision is sufficient for Angle16 and Angle8.
+float sin(Angle16 a);
+float cos(Angle16 a);
+float tan(Angle16 a);
+float sin(Angle8 a);
+float cos(Angle8 a);
+float tan(Angle8 a);
+
+}

src/common/3rd/x86/math/include/math/box2d.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class Box2d {
+public:
+    Box2d(const Vec2d& center, const double heading,
+        const double length, const double width);
+    Box2d(const Segment2d& axis, const double width);
+    explicit Box2d(const AABox2d& aabox);
+    static Box2d create_aa_box(const Vec2d& one_corner, const Vec2d& opponent_corner);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _cos_heading; }
+    double sin_heading() const { return _sin_heading; }
+    double area() const { return _length * _width; }
+    double diagonal() const { return hypot(_length, _width); }
+
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool has_overlap(const Box2d& box) const;
+
+    AABox2d get_aa_box() const;
+
+    void rotate_from_center(double rotate_angle);
+    void shift(const Vec2d& shift_vec);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _heading = 0.0;
+    double _cos_heading = 1.0;
+    double _sin_heading = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/euler_angles_zxy.h

@@ -0,0 +1,118 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// A class of Euler angles with the intrinsic sequence ZXY.
+//
+// Our vehicle reference frame follows NovAtel's convention right/forward/up. The Euler angles
+// represents the rotation from the local frame (east/north/up) to the vehicle frame, as explained
+// below:
+//   roll: is zero when the car is level, and positive when the left side of the car is up;
+//   pitch: is zero when the car is level, and positive when the car is nose up;
+//   yaw: is zero when the car is pointing to north, and positive when the car turns left.
+// When using Euler angles to represent the rotation, we should use the intrinsic rotations ZXY. See
+// below for more explanations.
+//   http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html
+
+#ifndef ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+#define ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+
+#include "Eigen/Geometry"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace adu {
+namespace common {
+namespace math {
+
+template <typename T>
+struct EulerAnglesZXY {
+    T roll, pitch, yaw;
+
+    // Constructs an identity rotation.
+    EulerAnglesZXY()
+        : roll(0)
+        , pitch(0)
+        , yaw(0)
+    {
+    }
+
+    // Constructs a rotation using only yaw.
+    explicit EulerAnglesZXY(T yaw)
+        : roll(0)
+        , pitch(0)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using roll, pitch, and yaw.
+    EulerAnglesZXY(T roll, T pitch, T yaw)
+        : roll(roll)
+        , pitch(pitch)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using components of a quaternion.
+    EulerAnglesZXY(T qw, T qx, T qy, T qz)
+        : roll(atan2(2.0 * (qw * qy - qx * qz), 2.0 * (sqr<T>(qw) + sqr<T>(qz)) - 1.0))
+        , pitch(asin(2.0 * (qw * qx + qy * qz)))
+        , yaw(atan2(2.0 * (qw * qz - qx * qy), 2.0 * (sqr<T>(qw) + sqr<T>(qy)) - 1.0))
+    {
+    }
+
+    // Constructs a rotation using a quaternion.
+    explicit EulerAnglesZXY(const Eigen::Quaternion<T>& q)
+        : EulerAnglesZXY(q.w(), q.x(), q.y(), q.z())
+    {
+    }
+
+    // Normalizes roll, pitch, and yaw to [-PI, PI).
+    void normalize()
+    {
+        roll = normalize_angle(roll);
+        pitch = normalize_angle(pitch);
+        yaw = normalize_angle(yaw);
+    }
+
+    // Check if the rotation is valid. A valid rotation must have -PI/2 < pitch < PI/2.
+    bool is_valid()
+    {
+        normalize();
+        return pitch < M_PI_2 && pitch > -M_PI_2;
+    }
+
+    // Converts to a quaternion. The scalar part is guarantee to be non-negative.
+    Eigen::Quaternion<T> to_quaternion() const
+    {
+        // Uses double for internal calculation for better precision.
+        double r = roll * 0.5;
+        double p = pitch * 0.5;
+        double y = yaw * 0.5;
+
+        double sr = sin(r);
+        double sp = sin(p);
+        double sy = sin(y);
+
+        double cr = cos(r);
+        double cp = cos(p);
+        double cy = cos(y);
+
+        T qw = cr * cp * cy - sr * sp * sy;
+        T qx = cr * sp * cy - sr * cp * sy;
+        T qy = cr * sp * sy + sr * cp * cy;
+        T qz = cr * cp * sy + sr * sp * cy;
+        if (qw < 0.0)
+            return { -qw, -qx, -qy, -qz };
+        return { qw, qx, qy, qz };
+    }
+};
+
+using EulerAnglesZXYf = EulerAnglesZXY<float>;
+using EulerAnglesZXYd = EulerAnglesZXY<double>;
+
+} // namespace math
+} // namespace common
+} // namespace adu
+
+#endif // ADU_COMMON_MATH_EULER_ANGLES_ZXY_H

src/common/3rd/x86/math/include/math/heading.h

@@ -0,0 +1,45 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// Helper functions that converts heading from/to a quaternion.
+//
+// We have permanently defined that the heading of car is zero when the car is pointing to east, and
+// positive when the car turns left. This function guarantees correct heading calculation,
+// regardless any future changes in the definition of vehicle reference frame.
+
+#pragma once
+
+#include "Eigen/Geometry"
+#include "math/euler_angles_zxy.h"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace decision::math {
+
+// Returns heading (in radians) in [-PI, PI).
+inline double quaternion_to_heading(double qw, double qx, double qy, double qz)
+{
+    EulerAnglesZXYd euler_angles(qw, qx, qy, qz);
+    // Needs adding pi/2 to yaw because when the vehicle reference frame is RFU, yaw is zero when
+    // the car is pointing to north.
+    return normalize_angle(euler_angles.yaw + M_PI_2);
+}
+
+// Similar to above but takes a Quaternion object.
+template <typename T>
+inline double quaternion_to_heading(const Eigen::Quaternion<T>& q)
+{
+    return quaternion_to_heading(q.w(), q.x(), q.y(), q.z());
+}
+
+// Returns a quaternion with zero roll, zero pitch, and the specified heading.
+template <typename T>
+inline Eigen::Quaternion<T> heading_to_quaternion(double heading)
+{
+    // Needs deducting pi/2 from heading because the vehicle reference frame is RFU.
+    EulerAnglesZXY<T> euler_angles(heading - M_PI_2);
+    return euler_angles.to_quaternion();
+}
+
+}

src/common/3rd/x86/math/include/math/math_utils.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/polygon2d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+constexpr double kMathEpsilon = 1e-10;
+
+inline double cross_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.y() - point1.y()) - (point3.x() - point1.x()) * (point2.y() - point1.y());
+}
+
+inline Vec3d cross_prod(const Vec3d& point1, const Vec3d& point2, const Vec3d& point3)
+{
+    Vec3d v12 = point2 - point1;
+    Vec3d v13 = point3 - point1;
+    return v12.cross_prod(v13);
+}
+
+inline double inner_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.x() - point1.x()) + (point2.y() - point1.y()) * (point3.y() - point1.y());
+}
+
+// Wrap angle to [0, 2 * PI).
+inline double wrap_angle(const double angle)
+{
+    const double new_angle = fmod(angle, M_PI * 2.0);
+    return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle;
+}
+
+// Normalize angle to [-PI, PI).
+inline double normalize_angle(const double angle)
+{
+    const double new_angle = fmod(angle + M_PI, M_PI * 2.0);
+    return (new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle) - M_PI;
+}
+
+template <typename T>
+inline T sqr(const T value)
+{
+    return value * value;
+}
+
+}
+
+#include <iostream>
+#define CHECK(v) (void)(v)
+#define CHECK_NOTNULL(v) (void)(v)
+#define CHECK_GE(a, b)
+#define CHECK_GT(a, b)

src/common/3rd/x86/math/include/math/polygon2d.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math{
+
+class Polygon2d {
+public:
+    Polygon2d() = default;
+    explicit Polygon2d(const Box2d& box);
+    explicit Polygon2d(std::vector<Vec2d> points);
+
+    const std::vector<Vec2d>& points() const { return _points; }
+    const std::vector<Segment2d>& segments() const { return _segments; }
+    int num_points() const { return _num_points; }
+    bool is_convex() const { return _is_convex; }
+    double area() const { return _area; }
+
+    double distance_to_boundary(const Vec2d& point) const;
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+    double distance_to(const Polygon2d& polygon) const;
+    double distance_sqr_to(const Vec2d& point) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    bool is_contain(const Segment2d& segment) const;
+    bool is_contain(const Polygon2d& polygon) const;
+
+    static bool compute_convex_hull(const std::vector<Vec2d>& points, Polygon2d* const polygon);
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool get_overlap(const Segment2d& segment, Vec2d* const first, Vec2d* const last) const;
+    std::vector<Segment2d> get_all_overlaps(const Segment2d& segment) const;
+
+    bool has_overlap(const Polygon2d& polygon) const;
+    // Only compute overlaps between two convex polygons.
+    bool compute_overlap(const Polygon2d& other_polygon, Polygon2d* const overlap_polygon) const;
+
+    AABox2d aa_bounding_box() const;
+    Box2d bounding_box_with_heading(const double heading) const;
+    Box2d min_area_bounding_box() const;
+    void extreme_points(const double heading, Vec2d* const first, Vec2d* const last) const;
+
+    Polygon2d expand_by_distance(const double distance) const;
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    int next(int at) const;
+    int prev(int at) const;
+
+    // Remove point p if and only if outer_prod(segment.start, segment.end, p) < 0.
+    static bool clip_convex_hull(const Segment2d& segment, std::vector<Vec2d>* const points);
+
+    std::vector<Vec2d> _points;
+    int _num_points = 0;
+    std::vector<Segment2d> _segments;
+    bool _is_convex = false;
+    double _area = 0.0;
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/polygon3d.h

@@ -0,0 +1,38 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: polygon for 3d point, the polygon is constructed on 2d space, but the point is 3d
+
+#pragma once
+
+#include "math/polygon2d.h"
+#include "math/segment3d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Polygon3d {
+public:
+    Polygon3d() = default;
+    explicit Polygon3d(const std::vector<Vec3d>& points);
+
+    Polygon2d polygon2d() const;
+    const std::vector<Vec3d>& points() const { return _points; }
+    const std::vector<Segment3d>& segments() const { return _segments; }
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    inline size_t next(size_t at) const;
+    inline size_t prev(size_t at) const;
+
+    std::vector<Vec3d> _points;
+    std::vector<Vec2d> _points2d;
+    std::vector<Segment3d> _segments;
+};
+
+}

src/common/3rd/x86/math/include/math/segment2d.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+
+namespace decision::math {
+
+class Segment2d {
+public:
+    Segment2d();
+    Segment2d(const Vec2d& start, const Vec2d& end);
+
+    const Vec2d& start() const { return _start; }
+    const Vec2d& end() const { return _end; }
+    const Vec2d& unit_direction() const { return _unit_direction; }
+    Vec2d center() const { return (_start + _end) / 2.0; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _unit_direction.x(); }
+    double sin_heading() const { return _unit_direction.y(); }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+    double distance_sqr_to(const Vec2d& point) const;
+    double distance_sqr_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+
+    bool is_point_in(const Vec2d& point) const;
+
+    bool has_intersect(const Segment2d& other_segment) const;
+    bool get_intersect(const Segment2d& other_segment, Vec2d* const point) const;
+
+    double project_onto_unit(const Vec2d& point) const;
+    double product_onto_unit(const Vec2d& point) const;
+    double get_perpendicular_foot(const Vec2d& point, Vec2d* const foot_point) const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _start;
+    Vec2d _end;
+    Vec2d _unit_direction;
+    double _heading = 0.0;
+    double _length = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/segment3d.h

@@ -0,0 +1,51 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: segment for 3d
+
+#pragma once
+
+#include "math/segment2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Segment3d {
+public:
+    Segment3d();
+    Segment3d(const Vec3d& start, const Vec3d& end);
+
+    const Vec3d& start() const { return _start; }
+    const Vec3d& end() const { return _end; }
+    const Vec3d& unit_direction() const { return _unit_direction; }
+    Vec3d center() const { return (_start + _end) / 2.0; }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec3d& point) const;
+    double distance_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+    double distance_sqr_to(const Vec3d& point) const;
+    double distance_sqr_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+
+    bool is_point_in(const Vec3d& point) const;
+
+    bool has_intersect(const Segment3d& other_segment) const;
+    bool get_intersect(const Segment3d& other_segment, Vec3d* const point) const;
+
+    double project_onto_unit(const Vec3d& point) const;
+    double product_onto_unit(const Vec3d& point) const;
+    double get_perpendicular_foot(const Vec3d& point, Vec3d* const foot_point) const;
+
+    Segment2d segment2d() const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _start;
+    Vec3d _end;
+    Vec3d _unit_direction;
+    double _length = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/sin_table.h

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace decision::math {
+
+extern const float SIN_TABLE[16385];
+
+}

src/common/3rd/x86/math/include/math/vec2d.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <cmath>
+#include <string>
+
+namespace decision::math {
+
+class Vec2d {
+public:
+    constexpr Vec2d() noexcept
+        : Vec2d(0, 0)
+    {
+    }
+    constexpr Vec2d(const double x, const double y) noexcept
+        : _x(x)
+        , _y(y)
+    {
+    }
+    static Vec2d create_unit_vec(const double angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec2d& other) const;
+    double distance_sqr_to(const Vec2d& other) const;
+
+    Vec2d operator+(const Vec2d& other) const;
+    Vec2d operator-(const Vec2d& other) const;
+    Vec2d operator*(const double ratio) const;
+    Vec2d operator/(const double ratio) const;
+    Vec2d& operator+=(const Vec2d& other);
+    Vec2d& operator-=(const Vec2d& other);
+    Vec2d& operator*=(const double ratio);
+    Vec2d& operator/=(const double ratio);
+    bool operator==(const Vec2d& other) const;
+
+    //! rotate the vector by angle.
+    Vec2d rotate(const double angle) const;
+
+    double cross_prod(const Vec2d& other) const;
+    double inner_prod(const Vec2d& other) const;
+
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+};
+
+Vec2d operator*(const double ratio, const Vec2d& vec);
+
+}

src/common/3rd/x86/math/include/math/vec3d.h

@@ -0,0 +1,66 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: supporting for 3D element
+
+#pragma once
+
+// #include <cmath>
+// #include <string>
+#include "math/vec2d.h"
+
+namespace decision::math {
+
+class Vec3d {
+public:
+    constexpr Vec3d() noexcept
+        : Vec3d(0, 0, 0)
+    {
+    }
+    constexpr Vec3d(const double x, const double y, const double z) noexcept
+        : _x(x)
+        , _y(y)
+        , _z(z)
+    {
+    }
+    static Vec3d create_unit_vec(const double yaw_angle, const double pitch_angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double z() const { return _z; }
+    double yaw_angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void set_z(const double z) { _z = z; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec3d& other) const;
+    double distance_sqr_to(const Vec3d& other) const;
+
+    Vec3d operator+(const Vec3d& other) const;
+    Vec3d operator-(const Vec3d& other) const;
+    Vec3d operator*(const double ratio) const;
+    Vec3d operator/(const double ratio) const;
+    Vec3d& operator+=(const Vec3d& other);
+    Vec3d& operator-=(const Vec3d& other);
+    Vec3d& operator*=(const double ratio);
+    Vec3d& operator/=(const double ratio);
+    bool operator==(const Vec3d& other) const;
+
+    Vec3d cross_prod(const Vec3d& other) const;
+    double inner_prod(const Vec3d& other) const;
+
+    Vec2d vec2d() const { return Vec2d(_x, _y); }
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+    double _z = 0.0;
+};
+
+Vec3d operator*(const double ratio, const Vec3d& vec);
+
+}

src/common/3rd/x86/mmw/include/mmw/mmwabstract.h

@@ -0,0 +1,47 @@
+#pragma once
+#include "muduo/base/noncopyable.h"
+#include "muduo/net/EventLoop.h"
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+
+namespace muduo::net {
+class EventLoopThread;
+};
+
+namespace mmw {
+
+class MMWAbstract : public muduo::noncopyable {
+public:
+    MMWAbstract(void) = default;
+    virtual ~MMWAbstract(void) = default;
+    virtual bool init(void) = 0;
+};
+
+class PublisherAbstract : public MMWAbstract {
+public:
+    virtual bool init(void) = 0;
+    virtual bool publish(const char* topic, size_t topicSize, const char* content, size_t contentSize) = 0;
+    bool publish(const std::string& topic, const std::string& content);
+};
+
+class SubscriberAbstract : public MMWAbstract {
+public:
+    using ReadCallBack = std::function<void(const char*, size_t, const char*, size_t)>;
+    virtual ~SubscriberAbstract(void);
+    bool readStart(ReadCallBack func);
+    virtual bool init(void) = 0;
+    virtual bool subscribe(const std::string& topic) = 0;
+    virtual bool unSubscribe(const std::string& topic) = 0;
+
+protected:
+    virtual void readyMessage(ReadCallBack& func) = 0;
+
+private:
+    std::once_flag onceFlag_;
+    std::atomic<bool> workFlags_;
+    std::shared_ptr<muduo::net::EventLoopThread> eventLoopthreadPtr_ { nullptr };
+};
+
+}

src/common/3rd/x86/mmw/include/mmw/mmwfactory.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "zmqmmw.h"
+namespace mmw {
+
+enum class MMWSelector : uint32_t {
+    ZMQ = 0
+};
+
+class MMWFactory {
+public:
+    template <typename... Args>
+    std::shared_ptr<PublisherAbstract> getPublisher(MMWSelector mmwSelector, Args&&... args)
+    {
+        switch (mmwSelector) {
+        case MMWSelector::ZMQ:
+            return std::make_shared<ZmqPublisher>(std::forward<Args>(args)...);
+        default:
+            return nullptr;
+        }
+    }
+
+    template <typename... Args>
+    std::shared_ptr<SubscriberAbstract> getSubscriber(MMWSelector mmwSelector, Args&&... args)
+    {
+        switch (mmwSelector) {
+        case MMWSelector::ZMQ:
+            return std::make_shared<ZmqSubscriber>(std::forward<Args>(args)...);
+        default:
+            return nullptr;
+        }
+    }
+};
+
+}

src/common/3rd/x86/mmw/include/mmw/zmqmmw.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "../utils/vl_common.h"
+#include "mmwabstract.h"
+#include <atomic>
+#include <set>
+#include <zmq.hpp>
+using namespace utils;
+namespace mmw {
+
+class ZmqPublisher : public PublisherAbstract {
+public:
+    ZmqPublisher(std::string localIp = "");
+    virtual bool init(void) override final;
+    virtual bool publish(const char* topic, size_t topicSize, const char* content, size_t contentSize) override final;
+
+private:
+    std::string localAddr_;
+    zmq::context_t context_ { 1 };
+    zmq::socket_t publisher_ { context_, ZMQ_PUB };
+};
+
+class ZmqSubscriber : public SubscriberAbstract {
+public:
+    ZmqSubscriber(std::set<std::string> connectAddrList);
+    virtual bool init(void) override final;
+    virtual bool subscribe(const std::string& topic) override final;
+    virtual bool unSubscribe(const std::string& topic) override final;
+
+protected:
+    virtual void readyMessage(ReadCallBack& func) override final;
+
+private:
+    std::set<std::string> connectAddrSet_;
+    zmq::context_t context_ { 1 };
+    zmq::socket_t subscriber_ { context_, ZMQ_SUB };
+};
+
+}

src/common/3rd/x86/utils/include/utils.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include "string"
+
+namespace utils {
+
+extern std::string getProcessPath(void);
+extern std::string getProcessName(void);
+
+}

src/common/3rd/x86/utils/include/utils/bevdetConfig.h

@@ -0,0 +1,6 @@
+#pragma once
+
+constexpr const char* bevDetConfigurePath = "/cfgs/configure.yaml";
+constexpr const char* bevDetImgStageEngine = "model/img_stage_lt_d_fp16.engine";
+constexpr const char* bevDetBEVStageEngine = "model/bev_stage_lt_d_fp16.engine";
+constexpr const char* bevDetDepth = "cfgs/bevdet_lt_depth.yaml";

src/common/3rd/x86/utils/include/utils/common.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <string>
+
+#define UNUSED(name) ((void)name)
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])
+
+namespace afl {
+
+/**
+ * @brief
+ * @param pidDirPath [in]存放pid的路径,如果运行中出错会强制退出程序
+ */
+extern void instanceRun(const std::string& pidDirPath);
+/**
+ * @brief get the process path
+ * @return success:name fault:empty
+ */
+extern std::string getProcessPath(void);
+/**
+ * @brief get the process name
+ * @return success:name fault:empty
+ */
+extern std::string getProcessName(void);
+/**
+ * @brief Check if the folder exists
+ * @param filePath[in] file path
+ * @return true
+ * @return false
+ */
+extern bool isFileExist(const std::string& filePath);
+/**
+ * @brief Read all data from the file into a string
+ * @param path[in] file path
+ * @param out[out] result
+ * @return true
+ * @return false
+ */
+extern bool readFileAllDataToString(const std::string& path, std::string& out);
+/**
+ * @brief write all data to path
+ * @param in[in] content
+ * @param path[in] file path
+ * @param isApp[in] is append
+ * @return true
+ * @return false
+ */
+extern bool writeStringAllDataToFile(const std::string& in, const std::string& path, bool isApp = false);
+/**
+ * @brief 返回uuid
+ * @return std::string
+ */
+extern std::string getUUID(void);
+/**
+ * @brief 随机生成字符串
+ * @param len 随机生成字符串的长度
+ * @return std::string
+ */
+extern std::string generateRandomString(size_t len);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ * @param size[in] 需要打印数据的字节个数
+ */
+extern void hexPrint(const char* data, size_t size);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ */
+extern void hexPrint(const std::string& data);
+/**
+ * @brief 时间字符串转unix时间戳
+ * @param timeStamp[in]时间字符串,格式为%d-%d-%d %d:%d:%d
+ * @return time_t
+ */
+extern time_t strTimeToUnixTimestamp(const std::string& timeStamp);
+
+}

src/common/3rd/x86/utils/include/utils/defer.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <functional>
+
+class defer_t final {
+public:
+    template <typename F>
+    explicit defer_t(F&& f) noexcept
+        : f_ {
+            std::forward<F>(f)
+        }
+    {
+    }
+
+    ~defer_t()
+    {
+
+        if (f_) {
+
+            f_();
+        }
+    }
+
+    defer_t(const defer_t&) = delete;
+    defer_t& operator=(const defer_t&) = delete;
+
+    defer_t(defer_t&& rhs) noexcept
+        : f_ {
+            std::move(rhs.f_)
+        }
+    {
+    }
+
+    defer_t& operator=(defer_t&& rhs) noexcept
+    {
+
+        f_ = std::move(rhs.f_);
+        return *this;
+    }
+
+private:
+    std::function<void()> f_;
+};
+
+class defer_maker final {
+
+public:
+    template <typename F>
+    defer_t operator+(F&& f)
+    {
+
+        return defer_t {
+            std::forward<F>(f)
+        };
+    }
+};
+
+#define DEFER_CONCAT_NAME(l, r) l##r
+#define DEFER_CREATE_NAME(l, r) DEFER_CONCAT_NAME(l, r)
+#define defer auto DEFER_CREATE_NAME(defer_, __COUNTER__) = defer_maker {} +
+#define defer_scope defer[&]

src/common/3rd/x86/utils/include/utils/error_event.h

@@ -0,0 +1,135 @@
+#pragma once
+
+/**
+0x01	系统基础	SYS
+0x02	can0	CAN0
+0x03	can1	CAN1
+0x04	can2	CAN2
+0x10	IMU	IMU
+0x11	GPS	GPS
+0x12	单线雷达 #1 左	LIDAR1
+0x13	单线雷达 #2 右	LIDAR2
+0x14	单线雷达 #3 左斜	LIDAR3
+0x15	单线雷达 #4 右斜	LIDAR4
+0x16	单线雷达 #5 后	LIDAR5
+0x17	相机 #1（RGB）	CAM1
+0x18	相机 #2（RGB-D/后）	CAM2
+0x19	屏幕	TFT
+
+0x20	电机驱动器	MOTOR
+0x21	上控	TOP
+ */
+enum class ErrorEventCategory {
+    SYS = 0x01,
+    CAN0 = 0x02,
+    CAN1 = 0x03,
+    CAN2 = 0x04,
+    IMU = 0x10,
+    GPS = 0x11,
+    LIDAR1 = 0x12,
+    LIDAR2 = 0x13,
+    LIDAR3 = 0x14,
+    LIDAR4 = 0x15,
+    LIDAR5 = 0x16,
+    CAM1 = 0x17,
+    CAM2 = 0x18,
+    TFT = 0x19,
+    MOTOR = 0x20,
+    TOP = 0x21,
+};
+//  01 发生  02 恢复
+enum class ErrorEventLevel {
+    OCCUR = 0x01,
+    RECOVER = 0x02,
+};
+
+#define SYS_CONFIG_ERROR 0x010101 // 配置读取失败
+#define SYS_INIT_FAIL 0x010102 // 初始化失败
+#define SYS_FILE_ERROR 0x010103 // 文件 IO 异常
+#define SYS_NETWORK_FAIL 0x010201 // 4G / 网络错误
+#define SYS_TIME_SYNC_ERROR 0x010501 // 系统时间异常
+
+#define CAN0_NOT_FOUND 0x020201 // 未检测到 CAN0
+#define CAN0_DISCONNECTED 0x020202 // CAN0 断连
+
+#define CAN1_NOT_FOUND 0x030201 // 未检测到 CAN1
+#define CAN1_DISCONNECTED 0x030202 // CAN1 断连
+
+#define CAN2_NOT_FOUND 0x040201 // 未检测到 CAN2
+#define CAN2_DISCONNECTED 0x040202 // CAN2 断连
+
+#define IMU_NOT_FOUND 0x100201 // 未检测到 IMU
+#define IMU_DISCONNECTED 0x100202 // IMU 断连
+#define IMU_TIMEOUT 0x100303 // 超时无数据
+#define IMU_DATA_INVALID 0x100304 // 数据异常（NaN/饱和）
+#define IMU_CALIB_FAIL 0x100407 // 校准失败
+#define IMU_TIME_SYNC_ERROR 0x100508 // 时间戳跳变
+#define IMU_DRIVER_ERROR 0x100609 // SDK/协议解析失败
+
+#define GPS_NOT_FOUND 0x110201 // 未检测到 GPS
+#define GPS_DISCONNECTED 0x110202 // 断连
+#define GPS_NO_SIGNAL 0x110304 // 无卫星 / SNR 太低
+#define GPS_DATA_LOSS 0x110305 // 数据丢包
+#define GPS_TIME_SYNC_ERROR 0x110508 // PPS/时间异常
+#define GPS_DRIVER_ERROR 0x110609 // 协议解析失败（NMEA/UBlox）
+
+// 左雷达
+#define LIDAR1_NOT_FOUND 0x120201 // 未检测到雷达
+#define LIDAR1_DISCONNECTED 0x120202 // 断连
+#define LIDAR1_TIMEOUT 0x120303 // 无数据
+#define LIDAR1_DATA_INVALID 0x120304 // 数据无效
+#define LIDAR1_TIME_SYNC_ERROR 0x120508 // 时间戳异常
+#define LIDAR1_DRIVER_ERROR 0x120609 // 协议解析失败
+// 右雷达
+#define LIDAR2_NOT_FOUND 0x130201 // 未检测到雷达
+#define LIDAR2_DISCONNECTED 0x130202 // 断连
+#define LIDAR2_TIMEOUT 0x130303 // 无数据
+#define LIDAR2_DATA_INVALID 0x130304 // 数据无效
+#define LIDAR2_TIME_SYNC_ERROR 0x130508 // 时间戳异常
+#define LIDAR2_DRIVER_ERROR 0x130609 // 协议解析失败
+// 左斜雷达
+#define LIDAR3_NOT_FOUND 0x140201 // 未检测到雷达
+#define LIDAR3_DISCONNECTED 0x140202 // 断连
+#define LIDAR3_TIMEOUT 0x140303 // 无数据
+#define LIDAR3_DATA_INVALID 0x140304 // 数据无效
+#define LIDAR3_TIME_SYNC_ERROR 0x140508 // 时间戳异常
+#define LIDAR3_DRIVER_ERROR 0x140609 // 协议解析失败
+// 右斜雷达
+#define LIDAR4_NOT_FOUND 0x150201 // 未检测到雷达
+#define LIDAR4_DISCONNECTED 0x150202 // 断连
+#define LIDAR4_TIMEOUT 0x150303 // 无数据
+#define LIDAR4_DATA_INVALID 0x150304 // 数据无效
+#define LIDAR4_TIME_SYNC_ERROR 0x150508 // 时间戳异常
+#define LIDAR4_DRIVER_ERROR 0x150609 // 协议解析失败
+// 后雷达
+#define LIDAR5_NOT_FOUND 0x160201 // 未检测到雷达
+#define LIDAR5_DISCONNECTED 0x160202 // 断连
+#define LIDAR5_TIMEOUT 0x160303 // 无数据
+#define LIDAR5_DATA_INVALID 0x160304 // 数据无效
+#define LIDAR5_TIME_SYNC_ERROR 0x160508 // 时间戳异常
+#define LIDAR5_DRIVER_ERROR 0x160609 // 协议解析失败
+
+#define CAM1_NOT_FOUND 0x170201 // 未检测到相机
+#define CAM1_DISCONNECTED 0x170202 // 断连
+#define CAM1_FRAME_DROP 0x170305 // 丢帧过多
+#define CAM1_DATA_INVALID 0x170304 // 图像损坏/格式错误
+#define CAM1_TIME_SYNC_ERROR 0x170508 // 时间异常
+#define CAM1_DRIVER_ERROR 0x170609 // SDK/驱动错误
+
+#define CAM2_NOT_FOUND 0x180201 // 未检测到相机
+#define CAM2_DISCONNECTED 0x180202 // 断连
+#define CAM2_DEPTH_INVALID 0x180304 // 深度数据异常
+#define CAM2_FRAME_DROP 0x180305 // 丢帧过多
+#define CAM2_TIME_SYNC_ERROR 0x180508 // 时间异常
+#define CAM2_DRIVER_ERROR 0x180609 // SDK/驱动错误
+
+#define TFT_NOT_FOUND 0x190201 // 未检测到TFT
+#define TFT_DISCONNECTED 0x190202 // 断连
+#define TFT_DEPTH_INVALID 0x190304 //
+#define TFT_FRAME_DROP 0x190305 //
+#define TFT_TIME_SYNC_ERROR 0x190508 // 时间异常
+#define TFT_DRIVER_ERROR 0x190609 // SDK/驱动错误
+
+inline uint32_t encodeErrorLevel(uint32_t error_code, ErrorEventLevel level) {
+    return (error_code << 8) | (static_cast<uint32_t>(level) & 0xFF);
+}

src/common/3rd/x86/utils/include/utils/non_timer.h

@@ -0,0 +1,62 @@
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <thread>
+namespace utils {
+// 定义定时器类
+class NonBlockingTimer {
+public:
+    // 构造函数，接受定时器时间（毫秒）和回调函数
+    NonBlockingTimer(int milliseconds, std::function<void()> callback)
+        : duration(std::chrono::milliseconds(milliseconds))
+        , callback(callback)
+        , is_running(false)
+    {
+    }
+
+    // 启动定时器
+    void start()
+    {
+        if (is_running)
+            return; // 如果定时器已经在运行，返回
+        is_running = true;
+        timer_thread = std::thread([this]() {
+            while (is_running) {
+                std::this_thread::sleep_for(duration); // 睡眠指定的时间
+                if (is_running) {
+                    callback(); // 时间到后执行回调函数
+                }
+            }
+        });
+        timer_thread.detach(); // 分离线程，非阻塞
+    }
+
+    // 停止定时器
+    void stop()
+    {
+        is_running = false;
+    }
+
+    // 检查定时器是否正在运行
+    bool running() const
+    {
+        return is_running;
+    }
+
+    ~NonBlockingTimer()
+    {
+        stop(); // 确保在销毁时停止定时器
+        if (timer_thread.joinable()) {
+            timer_thread.join(); // 如果线程可加入，加入线程
+        }
+    }
+
+private:
+    std::chrono::milliseconds duration; // 定时器的时间（毫秒）
+    std::function<void()> callback; // 回调函数
+    std::atomic<bool> is_running; // 定时器是否正在运行
+    std::thread timer_thread; // 定时器线程
+};
+}

src/common/3rd/x86/utils/include/utils/noncopyable.h

@@ -0,0 +1,15 @@
+#pragma once
+
+namespace utils {
+
+class Noncopyable {
+public:
+    Noncopyable(const Noncopyable&) = delete;
+    void operator=(const Noncopyable&) = delete;
+
+protected:
+    Noncopyable() = default;
+    ~Noncopyable() = default;
+};
+
+}

src/common/3rd/x86/utils/include/utils/serial_port.h

@@ -0,0 +1,102 @@
+/*
+ * @copyright: Copyright (c) 2024 zhengqi.com, Inc. All Rights Reserved
+ * @Author: likang(likang@baidu.com)
+ * @Date: 2024-01-05 16:32:38
+ * @FilePath: mem/src/sensor/serial_port/serial_port.h
+ * @brief:
+ */
+#ifndef SERIALPORT_H
+#define SERIALPORT_H
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+struct termios;
+namespace utils {
+
+enum BaudRate {
+    BR0 = 0000000,
+    BR50 = 0000001,
+    BR75 = 0000002,
+    BR110 = 0000003,
+    BR134 = 0000004,
+    BR150 = 0000005,
+    BR200 = 0000006,
+    BR300 = 0000007,
+    BR600 = 0000010,
+    BR1200 = 0000011,
+    BR1800 = 0000012,
+    BR2400 = 0000013,
+    BR4800 = 0000014,
+    BR9600 = 0000015,
+    BR19200 = 0000016,
+    BR38400 = 0000017,
+    BR57600 = 0010001,
+    BR115200 = 0010002,
+    BR230400 = 0010003,
+    BR460800 = 0010004,
+    BR500000 = 0010005,
+    BR576000 = 0010006,
+    BR921600 = 0010007,
+    BR1000000 = 0010010,
+    BR1152000 = 0010011,
+    BR1500000 = 0010012,
+    BR2000000 = 0010013,
+    BR2500000 = 0010014,
+    BR3000000 = 0010015,
+    BR3500000 = 0010016,
+    BR4000000 = 0010017
+};
+
+enum DataBits {
+    DATA_BITS_5,
+    DATA_BITS_6,
+    DATA_BITS_7,
+    DATA_BITS_8,
+};
+
+enum StopBits { STOP_BITS_1,
+    STOP_BITS_2 };
+
+enum Parity { PARITY_NONE,
+    PARITY_EVEN,
+    PARITY_MARK,
+    PARITY_ODD,
+    PARITY_SPACE };
+
+struct OpenOptions {
+    bool auto_open;
+    BaudRate baud_rate;
+    DataBits data_bits;
+    StopBits stop_bits;
+    Parity parity;
+    bool xon; // 当一端的数据连接不再能接受更多数据（或者接近这个状态），它发送XOFF字节给另一端。另一端收到XOFF字节，挂起数据发送。
+    bool xoff; // 一端如果准备好继续接收数据，它发送XON字节给另一端，另一端恢复数据发送。
+    bool xany;
+    int vmin;
+    int vtime;
+};
+
+class SerialPort {
+public:
+    static BaudRate baud_rate_make(unsigned long baud_rate);
+    static const OpenOptions default_options;
+    SerialPort(const std::string& devpath, const OpenOptions& options, bool isblock = false);
+    ~SerialPort();
+    bool start();
+    int read(char* buffer, int buf_len);
+    int write(char* buffer, int buf_len);
+    int get_tty_fd();
+
+private:
+    bool _is_block;
+    bool serial_port_init();
+    void termios_options(termios& tios, const OpenOptions& options);
+    std::string _path;
+    OpenOptions _open_options;
+    int _tty_fd = 0;
+};
+
+} // namespace sensor
+#endif // SERIALPORT_H

src/common/3rd/x86/utils/include/utils/singleton.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "noncopyable.h"
+#include <memory>
+
+namespace utils {
+
+template <typename T>
+class Singleton : public Noncopyable {
+public:
+    template <typename... Args>
+    static T& getInstance(Args&&... args)
+    {
+        static T t(std::forward<Args>(args)...);
+        return t;
+    }
+};
+
+}

src/common/3rd/x86/utils/include/utils/timer.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "string"
+#include <list>
+#include <muduo/base/Timestamp.h>
+#include <muduo/net/EventLoop.h>
+#include <muduo/net/EventLoopThread.h>
+
+namespace utils {
+
+class Timer {
+public:
+    Timer(muduo::net::EventLoop* loop);
+
+    // 开始计时器，指定秒数后触发事件
+    void startTimer(double seconds, const muduo::net::TimerCallback& callback);
+
+private:
+    muduo::net::EventLoop* loop_; // 事件循环
+};
+
+}

src/common/3rd/x86/utils/include/utils/topic.h

@@ -0,0 +1,147 @@
+#pragma once
+
+constexpr const char *imagesTopic = "/sensor/camera/images";
+constexpr const char *cameraFusedImagesTopic = "/perception/cameraFused/images";
+
+constexpr const char *cameraFusedObjectsTopic = "/perception/cameraFused/objects";
+constexpr const char *multiFusedObjectsTopic = "/perception/multiFused/objects";
+constexpr const char *multiFusedObjectsTopicDecision = "/perception/decision/multiFused/objects";
+constexpr const char *obstacleMapTopicDecision = "/perception/obstacle_map";
+constexpr const char *eventTopic = "/decision/event";
+
+constexpr const char *danceStatusTopic = "/xhi/status";
+
+constexpr const char *backupCarTopic = "/sensor/backup/car";
+constexpr const char *handleControlTopic = "/handle/control";              // 手柄控制
+constexpr const char *nav2ControlTopic = "/nav/control";                   // nva2控制
+constexpr const char *appControlTopic = "/app/control";                    // app控制
+constexpr const char *carWheelSpeedTopic = "/car/speed";                   // 车辆底盘信息
+constexpr const char *carWheelSpeedStaTopic = "/wheel/twist";              // 车辆底盘信息
+constexpr const char *carEncoderTopic = "/car/encoder";                    // 车辆电机圈数
+constexpr const char *healthDataTopic = "/sensor/health/data";             // 健康计信息
+constexpr const char *healthTftDataTopic = "/sensor/tft/health/data";      // tft健康计信息
+constexpr const char *healthStatusTopic = "/sensor/health/start";          // 健康计启动
+constexpr const char *sensorhandleControlTopic = "/sensor/handle/control"; // 上控健康控制
+constexpr const char *gpsTopic = "/sensor/gps/data";
+constexpr const char *imuTopic = "/sensor/imu/data";
+constexpr const char *hrvStartTopic = "/sensor/hrv/start";
+constexpr const char *ultrasoundLeftTopic = "/sensor/left/ultrasound";
+constexpr const char *ultrasoundRightTopic = "/sensor/right/ultrasound";
+constexpr const char *ultrasoundBackTopic = "/sensor/back/ultrasound";
+constexpr const char *joystickTopic = "/sensor/joystick";
+constexpr const char *carInfoTopic = "/vlago/car/info";
+constexpr const char *operationSceTopic = "/service/operation/scenario";
+constexpr const char *serviceColourScreenTopic = "/service/colour_screen";
+constexpr const char *serviceGearTopic = "/service/gear";
+constexpr const char *modeChangeTopic = "/mode/change";
+constexpr const char *cameraH264Topic = "/sensor/backup/camera/h264";
+constexpr const char *leftLaserTopic = "/scan/left";
+constexpr const char *rightLaserTopic = "/scan/right";
+constexpr const char *leftTiltLaserTopic = "/scan/left_tilt";
+constexpr const char *rightTiltLaserTopic = "/scan/right_tilt";
+constexpr const char *backLaserTopic = "/scan/back";
+constexpr const char *leftTiltPointTopice = "/perception/left_tilt/pointcloud";
+constexpr const char *rightTiltPointTopice = "/perception/right_tilt/pointcloud";
+
+// 场景辅助测试相关topic
+constexpr const char *leftLaserROITopic = "/scene_auxiliary/left_laser/roi_filtered";
+constexpr const char *rightLaserROITopic = "/scene_auxiliary/right_laser/roi_filtered";
+constexpr const char *leftLaserClusterTopic = "/scene_auxiliary/left_laser/clusters";
+constexpr const char *rightLaserClusterTopic = "/scene_auxiliary/right_laser/clusters";
+constexpr const char *leftTiltClusterTopic = "/scene_auxiliary/left_tilt/clusters";
+constexpr const char *rightTiltClusterTopic = "/scene_auxiliary/right_tilt/clusters";
+constexpr const char *backClusterTopic = "/scene_auxiliary/back_laser/clusters";
+
+// base_link坐标系转换后的点云话题
+constexpr const char *leftLaserBaseLinkTopic = "/scene_auxiliary/left_laser/base_link";
+constexpr const char *rightLaserBaseLinkTopic = "/scene_auxiliary/right_laser/base_link";
+constexpr const char *leftTiltBaseLinkTopic = "/scene_auxiliary/left_tilt/base_link";
+constexpr const char *rightTiltBaseLinkTopic = "/scene_auxiliary/right_tilt/base_link";
+constexpr const char *backLaserBaseLinkTopic = "/scene_auxiliary/back_laser/base_link";
+
+constexpr const char *modeStatusTopic = "/vlago/mode/status";
+constexpr const char *rechargeStatusTopic = "/vlago/recharge/status";  //废弃
+constexpr const char *rechargeStateTopic = "/vlago/recharge/state";
+constexpr const char *sceneServiceModeTopic = "/scene_services/mode";
+constexpr const char *rgbPointCloudTopic = "/rgb/pointcloud";
+
+// 自车
+// constexpr const char* carInfo = "/sensor/car/information";
+
+// service
+constexpr const char *hrvStartService = "/vlago/hrv/start";
+constexpr const char *hrvInfoService = "/vlago/hrv/info";
+constexpr const char *operationSceService = "/vlago/operation/scenario";
+constexpr const char *operationStatusService = "/vlago/operation/status";
+constexpr const char *carControlService = "/vlago/car/control";
+// constexpr const char* carInfoService = "/vlago/car/info";
+constexpr const char *followControlService = "/vlago/follow_me";
+constexpr const char *followControlStatusService = "/vlago/follow_me/status";
+constexpr const char *summonService = "/valgo/car/summon";
+constexpr const char *summonStatusService = "/valgo/car/summon/status";
+constexpr const char *highService = "/vaglo/car/high";
+constexpr const char *gearsControlService = "/vlago/gear";
+constexpr const char *sensorGearControlService = "/sensor/vlago/gear";
+constexpr const char *idControlService = "/vlago/car_id";
+constexpr const char *narrowControlService = "/vlago/narrow_assistant_mode";
+constexpr const char *standardControlService = "/vlago/standard_mode";
+constexpr const char *cruiseControlService = "/vlago/cruise_mode";
+constexpr const char *auxiliaryControlService = "/vlago/safe_mode";
+constexpr const char *tftControlService = "/vlago/lamp_language_screen";
+constexpr const char *sensorTftControlService = "/sensor/vlago/lamp_language_screen";
+constexpr const char *danceControlService = "/vlago/dance_show";
+constexpr const char *modeSwitchService = "/mode/switch";
+constexpr const char *vlagoSettingService = "/vlago/setting";
+constexpr const char *vlagoModeEnableService = "/vlago/mode/enable";
+constexpr const char *rechargeService = "/vlago/auto_charge";
+constexpr const char *manageFollowmeService = "/manage/followme";
+constexpr const char *manageCruiseService = "/manage/cruise";
+constexpr const char *autoindoorService = "/vlago/autoDriveMode";
+constexpr const char *autoindoorNavGotoService = "/vlago/autoindoor/goto";
+constexpr const char *autoindoorNavCannelService = "/vlago/autoindoor/cannel";
+
+constexpr const char *feedNarrowService = "/feed/narrow/ctrl";
+constexpr const char *feedRechargeService = "/feed/recharge/ctrl";
+constexpr const char *feedFollwmeService = "/feed/follwme/ctrl";
+constexpr const char *feedDanceService = "/feed/dance/ctrl";
+//
+constexpr const char *ServiceFollowControlTopic = "/service/follow/control";
+// 模式
+constexpr const char *sceneAuxiliaryService = "/scene/auxiliary";
+constexpr const char *sceneNarrowService = "/scene/narrow";
+constexpr const char *sceneStandardService = "/scene/standard";
+constexpr const char *sceneCruiseService = "/scene/cruise";
+constexpr const char *sceneHelpcarService = "/scene/helpcar";
+constexpr const char *sceneDanceService = "/scene/dance";
+constexpr const char *sceneRechargeService = "/scene/recharge";
+constexpr const char *sceneFollowmeService = "/scene/followme";
+// 德国版本健康健 启动舞蹈秀
+constexpr const char *sceneKeyDanceTopic = "/scene/dance/key";
+
+constexpr const char *manageFollowMe = "/manage/followme";
+constexpr const char *manageDance = "/manage/dance";
+constexpr const char *manageStandard = "/manage/standard";
+
+// 模型
+constexpr const char *perTracekerTopic = "/perception/tracker/data";
+constexpr const char *perFeatuerTopic = "/perception/localization/data";
+constexpr const char *perDetctorTopic = "/perception/detector/data";
+constexpr const char *perMonnaiTopic = "/perception/end_points";
+constexpr const char *perSpeedbumpTopic = "/speedbump_marker_array";
+constexpr const char *perPoDaoTopic = "/perception/podao_detect_status";
+// 预警
+constexpr const char *auxiliaryAlarmTopic = "/vlago/auxiliary/alarm";
+constexpr const char *auxiliaryAlarmBackTopic = "/vlago/backcar/alarm";
+constexpr const char *cursorAlarmTopic = "/vlago/cruise/status";
+constexpr const char *speedbumpAlarmTopic = "/speed_bump/status";
+constexpr const char *downhillAlarmTopic = "/downhill/status";
+constexpr const char *uphillAlarmTopic = "/uphill/status";
+constexpr const char *footLimiterTopic = "/foot/limiter/status";
+
+// tft模式
+constexpr const char *tftModeTopic = "/vlago/tft/mode";
+
+constexpr const char *audioTopic = "/vlago/audio/play";
+
+// 错误码topic
+constexpr const char* errorEventTopic = "/error_events";

src/common/3rd/x86/utils/include/utils/trigger_logger.h

@@ -0,0 +1,174 @@
+#pragma once
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <string>
+#include <thread>
+#include <atomic>
+
+namespace utils {
+
+/**
+ * @brief 埋点数据统一格式
+ * 
+ * 统一的埋点数据格式，用于云端上传
+ */
+struct TriggerData {
+    // 基础字段
+    std::string from;              // 数据来源: "pad" 或 "vlago"
+    std::string type;              // 事件类型: "trigger" 固定值
+    std::string event_name;        // 事件名称: 如 "mode_switch_start"
+    int64_t timestamp;             // 时间戳(毫秒)
+    std::string timestamp_str;     // 时间戳字符串: "YYYY-MM-DD HH:MM:SS"
+    
+    // 位置信息
+    std::string node_name;         // ROS节点名称
+    std::string file_path;        // 源文件路径
+    int line_number;               // 源代码行号
+    std::string function_name;     // 函数名称
+    
+    // 业务数据
+    std::map<std::string, std::string> data;  // 业务数据键值对
+    
+    // 系统信息
+    std::string vehicle_id;        // 车辆ID (可选)
+    std::string version;           // 软件版本 (可选)
+};
+
+/**
+ * @brief 埋点日志收集器
+ * 
+ * 单例模式，负责收集埋点数据并上传到云端
+ */
+class TriggerLogger {
+public:
+    /**
+     * @brief 获取单例实例
+     */
+    static TriggerLogger& getInstance();
+    
+    /**
+     * @brief 初始化埋点日志器
+     * @param websocket_url WebSocket服务器地址
+     * @param vehicle_id 车辆ID
+     * @param version 软件版本
+     */
+    void initialize(const std::string& websocket_url = "ws://60.205.235.193:3009",
+                   const std::string& vehicle_id = "",
+                   const std::string& version = "");
+    
+    /**
+     * @brief 记录埋点事件
+     * @param event_name 事件名称
+     * @param data 业务数据
+     * @param file_path 源文件路径
+     * @param line_number 源代码行号
+     * @param function_name 函数名称
+     * @param node_name ROS节点名称
+     */
+    void log(const std::string& event_name,
+             const std::map<std::string, std::string>& data,
+             const std::string& file_path = "",
+             int line_number = 0,
+             const std::string& function_name = "",
+             const std::string& node_name = "");
+    
+    /**
+     * @brief 设置车辆ID
+     */
+    void setVehicleId(const std::string& vehicle_id);
+    
+    /**
+     * @brief 设置软件版本
+     */
+    void setVersion(const std::string& version);
+    
+    /**
+     * @brief 停止日志器
+     */
+    void shutdown();
+    
+    /**
+     * @brief 检查是否已初始化
+     */
+    bool isInitialized() const { return initialized_.load(); }
+
+private:
+    TriggerLogger() = default;
+    ~TriggerLogger();
+    TriggerLogger(const TriggerLogger&) = delete;
+    TriggerLogger& operator=(const TriggerLogger&) = delete;
+    
+    /**
+     * @brief 将TriggerData转换为JSON字符串
+     */
+    std::string toJson(const TriggerData& trigger_data);
+    
+    /**
+     * @brief 上传线程函数
+     */
+    void uploadThread();
+    
+    /**
+     * @brief 获取当前时间戳(毫秒)
+     */
+    int64_t getCurrentTimestamp();
+    
+    /**
+     * @brief 格式化时间戳字符串
+     */
+    std::string formatTimestamp(int64_t timestamp_ms);
+    
+    // WebSocket客户端前向声明
+    class WebSocketClient;
+    
+    std::unique_ptr<WebSocketClient> ws_client_;
+    std::string websocket_url_;
+    std::string vehicle_id_;
+    std::string version_;
+    
+    std::mutex queue_mutex_;
+    std::queue<TriggerData> data_queue_;
+    std::atomic<bool> initialized_{false};
+    std::atomic<bool> should_stop_{false};
+    std::thread upload_thread_;
+    
+    // 队列大小限制
+    static constexpr size_t MAX_QUEUE_SIZE = 10000;
+};
+
+/**
+ * @brief 便捷宏定义，用于快速埋点
+ * 
+ * 使用示例:
+ *   TRIGGER_LOG("mode_switch_start", {
+ *       {"old_mode", "standard"},
+ *       {"new_mode", "auxiliary"}
+ *   });
+ */
+#define TRIGGER_LOG(event_name, data) \
+    utils::TriggerLogger::getInstance().log( \
+        event_name, \
+        data, \
+        __FILE__, \
+        __LINE__, \
+        __FUNCTION__, \
+        "")
+
+/**
+ * @brief 带节点名称的埋点宏
+ */
+#define TRIGGER_LOG_WITH_NODE(event_name, data, node_name) \
+    utils::TriggerLogger::getInstance().log( \
+        event_name, \
+        data, \
+        __FILE__, \
+        __LINE__, \
+        __FUNCTION__, \
+        node_name)
+
+} // namespace utils
+

src/common/3rd/x86/utils/include/utils/utils.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "string"
+
+#include <list>
+
+namespace utils {
+
+extern double legalizeAngle(double angle);
+extern double angleDiff(double a, double b);
+extern double angleAverage(std::list<double> angles);
+extern uint64_t format_time();
+}

src/common/3rd/x86/utils/include/utils/vl_common.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <string>
+
+#define UNUSED(name) ((void)name)
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])
+
+namespace utils {
+
+/**
+ * @brief
+ * @param pidDirPath [in]存放pid的路径,如果运行中出错会强制退出程序
+ */
+extern void instanceRun(const std::string& pidDirPath);
+/**
+ * @brief get the process path
+ * @return success:name fault:empty
+ */
+extern std::string getProcessPath(void);
+/**
+ * @brief get the process name
+ * @return success:name fault:empty
+ */
+extern std::string getProcessName(void);
+/**
+ * @brief Check if the folder exists
+ * @param filePath[in] file path
+ * @return true
+ * @return false
+ */
+extern bool isFileExist(const std::string& filePath);
+/**
+ * @brief Read all data from the file into a string
+ * @param path[in] file path
+ * @param out[out] result
+ * @return true
+ * @return false
+ */
+extern bool readFileAllDataToString(const std::string& path, std::string& out);
+/**
+ * @brief write all data to path
+ * @param in[in] content
+ * @param path[in] file path
+ * @param isApp[in] is append
+ * @return true
+ * @return false
+ */
+extern bool writeStringAllDataToFile(const std::string& in, const std::string& path, bool isApp = false);
+/**
+ * @brief 返回uuid
+ * @return std::string
+ */
+extern std::string getUUID(void);
+/**
+ * @brief 随机生成字符串
+ * @param len 随机生成字符串的长度
+ * @return std::string
+ */
+extern std::string generateRandomString(size_t len);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ * @param size[in] 需要打印数据的字节个数
+ */
+extern void hexPrint(const char* data, size_t size);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ */
+extern void hexPrint(const std::string& data);
+/**
+ * @brief 时间字符串转unix时间戳
+ * @param timeStamp[in]时间字符串,格式为%d-%d-%d %d:%d:%d
+ * @return time_t
+ */
+extern time_t strTimeToUnixTimestamp(const std::string& timeStamp);
+
+}

src/common/3rd/x86/utils/include/utils/warning_type.h

@@ -0,0 +1,45 @@
+#pragma once
+//告警等级  (地面以上) 1 前向减速 2 侧向减速 3 后向减速 4 前方刹停 5 侧向刹停 6 后向刹停 7 驶离减速区 8 驶离刹停区
+//          (地面以下) 9 前向减速区 10 侧向减速区 11 前方刹停区 12 侧向刹停区  13 驶离减速区 14 驶离刹停区
+enum class WarningType {
+    //正常
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+
+    FORWARD_NEGATIVE_PLANE_WARNING = 9, // 前向负平面减速区
+    LATERAL_NEGATIVE_PLANE_WARNING = 10, // 侧向负平面减速区
+    FORWARD_NEGATIVE_PLANE_BRAKE_WARNING = 11, // 前向负平面刹停区
+    LATERAL_NEGATIVE_PLANE_BRAKE_WARNING = 12, // 侧向负平面刹停区
+    FORWARD_NEGATIVE_PLANE_LEAVE_WARNING = 13, // 驶离负平面减速区
+    LATERAL_NEGATIVE_PLANE_LEAVE_WARNING = 14, // 驶离负平面刹停区
+};
+
+// 对应灯带效果
+enum class LightEffect {
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+};
+
+// 对应语音效果
+enum class VoiceEffect {
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+};

src/common/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(common)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function -std=c++17)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+add_subdirectory(utils)
+add_subdirectory(math)
+#add_subdirectory(mmw)
+
+ament_package()
+
+

src/common/cmake/add3rd.cmake

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+
+function(add3rd arg1)
+  include_directories("${PROJECT_SOURCE_DIR}/../common/3rd/x86/${arg1}/include")
+  link_directories("${PROJECT_SOURCE_DIR}/../common/3rd/x86/${arg1}/lib")
+endfunction()
+
+function(add3rds)
+    foreach(v IN LISTS ARGV)
+      add3rd(${v})
+    endforeach()
+endfunction()

src/common/math/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+
+file(GLOB SRCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.cpp)
+
+add_library(math STATIC ${SRCS})
+
+target_include_directories(math PUBLIC ..)
+
+file(GLOB INCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.h *.hpp)
+install(FILES ${INCS} DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/math/include/math)
+install(TARGETS math DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/math/lib)

src/common/math/aabox2d.cpp

@@ -0,0 +1,140 @@
+#include "math/aabox2d.h"
+#include "math/math_utils.h"
+
+#include <cmath>
+#include <sstream>
+
+using namespace decision::math;
+
+AABox2d::AABox2d(const Vec2d& center, const double length, const double width)
+    : _center(center)
+    , _length(length)
+    , _width(width)
+    , _half_length(length / 2.0)
+    , _half_width(width / 2.0)
+{
+    CHECK_GT(_length, -kMathEpsilon);
+    CHECK_GT(_width, -kMathEpsilon);
+}
+
+AABox2d::AABox2d(const Vec2d& one_corner, const Vec2d& opponent_corner)
+    : AABox2d((one_corner + opponent_corner) / 2.0,
+        std::abs(one_corner.x() - opponent_corner.x()),
+        std::abs(one_corner.y() - opponent_corner.y()))
+{
+}
+
+AABox2d::AABox2d(const std::vector<Vec2d>& points)
+{
+    CHECK(!points.empty());
+    double min_x = points[0].x();
+    double max_x = points[0].x();
+    double min_y = points[0].y();
+    double max_y = points[0].y();
+    for (const auto& point : points) {
+        min_x = std::min(min_x, point.x());
+        max_x = std::max(max_x, point.x());
+        min_y = std::min(min_y, point.y());
+        max_y = std::max(max_y, point.y());
+    }
+
+    _center = { (min_x + max_x) / 2.0, (min_y + max_y) / 2.0 };
+    _length = max_x - min_x;
+    _width = max_y - min_y;
+    _half_length = _length / 2.0;
+    _half_width = _width / 2.0;
+}
+
+void AABox2d::get_all_corners(std::vector<Vec2d>* const corners) const
+{
+    CHECK_NOTNULL(corners)->clear();
+    corners->reserve(4);
+    corners->emplace_back(_center.x() + _half_length, _center.y() - _half_width);
+    corners->emplace_back(_center.x() + _half_length, _center.y() + _half_width);
+    corners->emplace_back(_center.x() - _half_length, _center.y() + _half_width);
+    corners->emplace_back(_center.x() - _half_length, _center.y() - _half_width);
+}
+
+bool AABox2d::is_point_in(const Vec2d& point) const
+{
+    return std::abs(point.x() - _center.x()) <= _half_length + kMathEpsilon && std::abs(point.y() - _center.y()) <= _half_width + kMathEpsilon;
+}
+
+bool AABox2d::is_point_on_boundary(const Vec2d& point) const
+{
+    const double dx = std::abs(point.x() - _center.x());
+    const double dy = std::abs(point.y() - _center.y());
+    return (std::abs(dx - _half_length) <= kMathEpsilon && dy <= _half_width + kMathEpsilon) || (std::abs(dy - _half_width) <= kMathEpsilon && dx <= _half_length + kMathEpsilon);
+}
+
+double AABox2d::distance_to(const Vec2d& point) const
+{
+    const double dx = std::abs(point.x() - _center.x()) - _half_length;
+    const double dy = std::abs(point.y() - _center.y()) - _half_width;
+    if (dx <= 0.0) {
+        return std::max(0.0, dy);
+    }
+    if (dy <= 0.0) {
+        return dx;
+    }
+    return hypot(dx, dy);
+}
+
+double AABox2d::distance_to(const AABox2d& box) const
+{
+    const double dx = std::abs(box.center_x() - _center.x()) - box.half_length() - _half_length;
+    const double dy = std::abs(box.center_y() - _center.y()) - box.half_width() - _half_width;
+    if (dx <= 0.0) {
+        return std::max(0.0, dy);
+    }
+    if (dy <= 0.0) {
+        return dx;
+    }
+    return hypot(dx, dy);
+}
+
+bool AABox2d::has_overlap(const AABox2d& box) const
+{
+    return std::abs(box.center_x() - _center.x()) <= box.half_length() + _half_length && std::abs(box.center_y() - _center.y()) <= box.half_width() + _half_width;
+}
+
+void AABox2d::shift(const Vec2d& shift_vec)
+{
+    _center += shift_vec;
+}
+
+void AABox2d::merge_from(const AABox2d& other_box)
+{
+    const double x1 = std::min(min_x(), other_box.min_x());
+    const double x2 = std::max(max_x(), other_box.max_x());
+    const double y1 = std::min(min_y(), other_box.min_y());
+    const double y2 = std::max(max_y(), other_box.max_y());
+    _center = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0);
+    _length = x2 - x1;
+    _width = y2 - y1;
+    _half_length = _length / 2.0;
+    _half_width = _width / 2.0;
+}
+
+void AABox2d::merge_from(const Vec2d& other_point)
+{
+    const double x1 = std::min(min_x(), other_point.x());
+    const double x2 = std::max(max_x(), other_point.x());
+    const double y1 = std::min(min_y(), other_point.y());
+    const double y2 = std::max(max_y(), other_point.y());
+    _center = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0);
+    _length = x2 - x1;
+    _width = y2 - y1;
+    _half_length = _length / 2.0;
+    _half_width = _width / 2.0;
+}
+
+std::string AABox2d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "aabox2d ( center = " << _center.debug_string()
+         << "  length = " << _length
+         << "  width = " << _width << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/aabox2d.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class AABox2d {
+public:
+    AABox2d() = default;
+    AABox2d(const Vec2d& center, const double length, const double width);
+    AABox2d(const Vec2d& one_corner, const Vec2d& opponent_corner);
+    AABox2d(const std::vector<Vec2d>& points);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double area() const { return _length * _width; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const AABox2d& box) const;
+
+    bool has_overlap(const AABox2d& box) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec2d& shift_vec);
+
+    void merge_from(const AABox2d& other_box);
+    void merge_from(const Vec2d& other_point);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+};
+
+}

src/common/math/aabox3d.cpp

@@ -0,0 +1,145 @@
+#include "math/aabox3d.h"
+#include "math/math_utils.h"
+
+#include <sstream>
+
+using namespace decision::math;
+
+AABox3d::AABox3d(const Vec3d& center, const double length, const double width, const double height)
+    : _center(center)
+    , _length(length)
+    , _width(width)
+    , _height(height)
+    , _half_length(length / 2.0)
+    , _half_width(width / 2.0)
+    , _half_height(height / 2.0)
+{
+    CHECK_GT(_length, -kMathEpsilon);
+    CHECK_GT(_width, -kMathEpsilon);
+    CHECK_GT(_height, -kMathEpsilon);
+}
+
+AABox3d::AABox3d(const Vec3d& one_corner, const Vec3d& opponent_corner)
+    : AABox3d((one_corner + opponent_corner) / 2.0,
+        std::abs(one_corner.x() - opponent_corner.x()),
+        std::abs(one_corner.y() - opponent_corner.y()),
+        std::abs(one_corner.z() - opponent_corner.z()))
+{
+}
+
+AABox3d::AABox3d(const std::vector<Vec3d>& points)
+{
+    CHECK(!points.empty());
+    double min_x = points[0].x();
+    double max_x = points[0].x();
+    double min_y = points[0].y();
+    double max_y = points[0].y();
+    double min_z = points[0].z();
+    double max_z = points[0].z();
+    for (const auto& point : points) {
+        min_x = std::min(min_x, point.x());
+        max_x = std::max(max_x, point.x());
+        min_y = std::min(min_y, point.y());
+        max_y = std::max(max_y, point.y());
+        min_z = std::min(min_z, point.z());
+        max_z = std::max(max_z, point.z());
+    }
+
+    _center = { (min_x + max_x) / 2.0, (min_y + max_y) / 2.0, (min_z + max_z) / 2.0 };
+    _length = max_x - min_x;
+    _width = max_y - min_y;
+    _height = max_z - min_z;
+    _half_length = _length / 2.0;
+    _half_width = _width / 2.0;
+    _half_height = _height / 2.0;
+}
+
+AABox2d AABox3d::aabox2d() const
+{
+    return AABox2d({ _center.x(), _center.y() }, _length, _width);
+}
+
+void AABox3d::get_all_corners(std::vector<Vec3d>* const corners) const
+{
+    CHECK_NOTNULL(corners)->clear();
+    corners->reserve(8);
+    corners->emplace_back(_center.x() - _half_length,
+        _center.y() - _half_width,
+        _center.z() - _half_height);
+    corners->emplace_back(_center.x() + _half_length,
+        _center.y() - _half_width,
+        _center.z() - _half_height);
+    corners->emplace_back(_center.x() + _half_length,
+        _center.y() + _half_width,
+        _center.z() - _half_height);
+    corners->emplace_back(_center.x() - _half_length,
+        _center.y() + _half_width,
+        _center.z() - _half_height);
+
+    corners->emplace_back(_center.x() - _half_length,
+        _center.y() - _half_width,
+        _center.z() + _half_height);
+    corners->emplace_back(_center.x() + _half_length,
+        _center.y() - _half_width,
+        _center.z() + _half_height);
+    corners->emplace_back(_center.x() + _half_length,
+        _center.y() + _half_width,
+        _center.z() + _half_height);
+    corners->emplace_back(_center.x() - _half_length,
+        _center.y() + _half_width,
+        _center.z() + _half_height);
+}
+
+bool AABox3d::is_point_in(const Vec3d& point) const
+{
+    return std::abs(point.x() - _center.x()) <= _half_length + kMathEpsilon && std::abs(point.y() - _center.y()) <= _half_width + kMathEpsilon && std::abs(point.z() - _center.z()) <= _half_height + kMathEpsilon;
+}
+
+bool AABox3d::is_point_on_boundary(const Vec3d& point) const
+{
+    const double dx = std::abs(point.x() - _center.x());
+    const double dy = std::abs(point.y() - _center.y());
+    const double dz = std::abs(point.z() - _center.z());
+    return (std::abs(dx - _half_length) <= kMathEpsilon && std::abs(dz - _half_height) <= kMathEpsilon && dy <= _half_width + kMathEpsilon) || (std::abs(dy - _half_width) <= kMathEpsilon && std::abs(dz - _half_height) <= kMathEpsilon && dx <= _half_length + kMathEpsilon) || (std::abs(dx - _half_length) <= kMathEpsilon && std::abs(dy - _half_width) <= kMathEpsilon && dz <= _half_height + kMathEpsilon);
+}
+
+double AABox3d::distance_to(const Vec3d& point) const
+{
+    const double dx = std::abs(point.x() - _center.x()) - _half_length;
+    const double dy = std::abs(point.y() - _center.y()) - _half_width;
+    const double dz = std::abs(point.z() - _center.z()) - _half_height;
+    return std::sqrt(std::max(0.0, dx) * std::max(0.0, dx) + std::max(0.0, dy) * std::max(0.0, dy) + std::max(0.0, dz) * std::max(0.0, dz));
+}
+
+void AABox3d::shift(const Vec3d& shift_vec)
+{
+    _center += shift_vec;
+}
+
+void AABox3d::merge_from(const AABox3d& other_box)
+{
+    const double x1 = std::min(min_x(), other_box.min_x());
+    const double x2 = std::max(max_x(), other_box.max_x());
+    const double y1 = std::min(min_y(), other_box.min_y());
+    const double y2 = std::max(max_y(), other_box.max_y());
+    const double z1 = std::min(min_z(), other_box.min_z());
+    const double z2 = std::max(max_z(), other_box.max_z());
+    _center = Vec3d((x1 + x2) / 2.0, (y1 + y2) / 2.0, (z1 + z2) / 2.0);
+    _length = x2 - x1;
+    _width = y2 - y1;
+    _height = z2 - z1;
+    _half_length = _length / 2.0;
+    _half_width = _width / 2.0;
+    _half_height = _height / 2.0;
+}
+
+std::string AABox3d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "aabox3d ( center = " << _center.debug_string()
+         << "  length = " << _length
+         << "  width = " << _width
+         << "  height = " << _height << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/aabox3d.h

@@ -0,0 +1,57 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: aabox for 3d
+
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class AABox3d {
+public:
+    AABox3d() = default;
+    AABox3d(const Vec3d& center, const double length, const double width, const double height);
+    AABox3d(const Vec3d& one_corner, const Vec3d& opponent_corner);
+    AABox3d(const std::vector<Vec3d>& points);
+
+    AABox2d aabox2d() const;
+    const Vec3d& center() const { return _center; }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double height() const { return _height; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+    double min_z() const { return _center.z() - _half_height; }
+    double max_z() const { return _center.z() + _half_height; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec3d>* const corners) const;
+
+    bool is_point_in(const Vec3d& point) const;
+    bool is_point_on_boundary(const Vec3d& point) const;
+
+    double distance_to(const Vec3d& point) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec3d& shift_vec);
+
+    void merge_from(const AABox3d& other_box);
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _height = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _half_height = 0.0;
+};
+
+}

src/common/math/aaboxkdtree2d.cpp

@@ -0,0 +1 @@
+#include "math/aaboxkdtree2d.h"

src/common/math/aaboxkdtree2d.h

@@ -0,0 +1,387 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABoxKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree2dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2dNode(const std::vector<ObjectPtr>& objects,
+        const AABoxKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return AABox2d({ _min_x, _min_y }, { _max_x, _max_y });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : obj1->aabox().min_y() < obj2->aabox().min_y();
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : obj1->aabox().max_y() > obj2->aabox().max_y();
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x() : object->aabox().min_y());
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x() : object->aabox().max_y());
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABoxKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        if (params.max_leaf_dimension >= 0.0 && std::max(_max_x - _min_x, _max_y - _min_y) <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        return dx * dx + dy * dy;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        return dx * dx + dy * dy;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec2d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec2d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        if (_max_x - _min_x >= _max_y - _min_y) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree2d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2d(const std::vector<ObjectType>& objects, const AABoxKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree2dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox2d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _root = nullptr;
+};
+
+}

src/common/math/aaboxkdtree3d.h

@@ -0,0 +1,441 @@
+
+// Copyright 2018 Baidu. Inc All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+// Date: 2018-06-19 14:44
+//
+// Description: aaboxkdtree for 3d element
+
+#ifndef _MATH_COMMON_AABOXKDTREE3D_H_
+#define _MATH_COMMON_AABOXKDTREE3D_H_
+
+#pragma once
+
+#include "math/aabox3d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABox3dKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree3dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3dNode(const std::vector<ObjectPtr>& objects,
+        const AABox3dKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return AABox3d({ _min_x, _min_y, _min_z }, { _max_x, _max_y, _max_z });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().min_y() < obj2->aabox().min_y()
+                                                                              : obj1->aabox().min_z() < obj2->aabox().min_z());
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().max_y() > obj2->aabox().max_y()
+                                                                              : obj1->aabox().max_z() > obj2->aabox().max_z());
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().min_y()
+                                                                       : object->aabox().min_z()));
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().max_y()
+                                                                       : object->aabox().max_z()));
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        double max_dim = std::max(_max_x - _min_x, _max_y - _min_y);
+        max_dim = std::max(max_dim, _max_z - _min_z);
+        if (params.max_leaf_dimension >= 0.0 && max_dim <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        double dz = 0.0;
+        if (point.z() < _min_z) {
+            dz = _min_z - point.z();
+        } else if (point.z() > _max_z) {
+            dz = point.z() - _max_z;
+        }
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        const double dz = (point.z() > _mid_z ? (point.z() - _min_z) : (point.z() - _max_z));
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec3d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec3d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _min_z = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        _max_z = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+            _min_z = std::min(_min_z, object->aabox().min_z());
+            _max_z = std::max(_max_z, object->aabox().max_z());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+        _mid_z = (_min_z + _max_z) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        double dx = _max_x - _min_x;
+        double dy = _max_y - _min_y;
+        double dz = _max_z - _min_z;
+        if (dx >= dy && dx >= dz) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else if (dy >= dx && dy >= dz) {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        } else {
+            _partition = PARTITION_Z;
+            _partition_position = (_min_z + _max_z) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else if (_partition == PARTITION_Y) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_z() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_z() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _min_z = 0.0;
+    double _max_z = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+    double _mid_z = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+        PARTITION_Z = 3,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree3d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3d(const std::vector<ObjectType>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree3dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox3d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _root = nullptr;
+};
+
+}
+
+#endif // _MATH_COMMON_AABOXKDTREE3D_H_

src/common/math/angle.cpp

@@ -0,0 +1,52 @@
+#include "math/angle.h"
+#include "math/sin_table.h"
+
+namespace decision::math {
+
+float sin(Angle16 a)
+{
+    int16_t idx = a.raw();
+    if (idx < -Angle16::RAW_PI_2) {
+        idx += Angle16::RAW_PI;
+        return -SIN_TABLE[idx];
+    }
+    if (idx < 0) {
+        return -SIN_TABLE[-idx];
+    }
+    if (idx < Angle16::RAW_PI_2) {
+        return SIN_TABLE[idx];
+    }
+    idx = Angle16::RAW_PI - idx;
+    return SIN_TABLE[idx];
+}
+
+float cos(Angle16 a)
+{
+    Angle16 b(Angle16::RAW_PI_2 - a.raw());
+    return sin(b);
+}
+
+float tan(Angle16 a)
+{
+    return sin(a) / cos(a);
+}
+
+float sin(Angle8 a)
+{
+    Angle16 b(a.raw() << 8);
+    return sin(b);
+}
+
+float cos(Angle8 a)
+{
+    Angle16 b(a.raw() << 8);
+    return cos(b);
+}
+
+float tan(Angle8 a)
+{
+    Angle16 b(a.raw() << 8);
+    return tan(b);
+}
+
+}

src/common/math/angle.h

@@ -0,0 +1,173 @@
+// Copyright (c) 2016 Baidu.com, Inc. All Rights Reserved
+// Author: hengliang
+
+// This file defines the Angle class, which uses an integer to represent an angle, and supports
+// commonly-used operations such as addition, subtraction, and trigonometry. Using an integer gives
+// the following advantages over the traditional floating-point representations:
+// - A signed integer automatically wraps an angle to the interval [-pi, pi).
+// - It makes the delta angle calculation easier and safer. For example, the difference between -179
+//   degrees and 179 degrees longitude is automatically 2 degrees.
+// - It uses less storage to achieve similar level of accuracy: Angle8 < Angle16 < float < Angle32 <
+//   double < Angle64, where < means "less precise than."
+// - Integer operations are more efficient.
+// - Angle8 and Angle16 allow super fast trigonometric functions via a 64-KiB lookup table.
+//
+// It is recommended to use the Angle class as much as possible, especially for the following
+// scenarios:
+// - Latitude/longitude: Angle32 can achieve < 1 cm resolution.
+// - Euler angles: Angle16 is precise enough for localization/object detection.
+//
+// Usage examples: Please refer to angle_test.cpp.
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <limits>
+
+namespace decision::math {
+
+template <typename T>
+class Angle {
+public:
+    static_assert(std::numeric_limits<T>::is_integer && std::numeric_limits<T>::is_signed,
+        "T must be a signed integer type");
+
+    static constexpr T RAW_PI = std::numeric_limits<T>::min();
+    static constexpr T RAW_PI_2 = -(std::numeric_limits<T>::min() >> 1);
+    static constexpr double DEG_TO_RAW = RAW_PI / -180.0;
+    static constexpr double RAD_TO_RAW = RAW_PI * -M_1_PI;
+    static constexpr double RAW_TO_DEG = -180.0 / RAW_PI;
+    static constexpr double RAW_TO_RAD = -M_PI / RAW_PI;
+
+    // Construct an object from an angle in degrees.
+    static Angle from_deg(double value)
+    {
+        Angle a(lround(value * DEG_TO_RAW));
+        return a;
+    }
+
+    // Construct an object from an angle in radians.
+    static Angle from_rad(double value)
+    {
+        Angle a(lround(value * RAD_TO_RAW));
+        return a;
+    }
+
+    explicit Angle(T value = 0)
+        : _value(value)
+    {
+    }
+
+    double to_deg() const
+    {
+        return _value * RAW_TO_DEG;
+    }
+
+    double to_rad() const
+    {
+        return _value * RAW_TO_RAD;
+    }
+
+    T raw() const
+    {
+        return _value;
+    }
+
+    Angle operator+=(Angle other)
+    {
+        _value += other._value;
+        return *this;
+    }
+
+    Angle operator-=(Angle other)
+    {
+        _value -= other._value;
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator*=(Scalar s)
+    {
+        _value = lround(_value * s);
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator/=(Scalar s)
+    {
+        _value = lround(_value / s);
+        return *this;
+    }
+
+private:
+    T _value;
+};
+
+using Angle8 = Angle<int8_t>;
+using Angle16 = Angle<int16_t>;
+using Angle32 = Angle<int32_t>;
+using Angle64 = Angle<int64_t>;
+
+template <typename T>
+Angle<T> operator+(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs += rhs;
+    return lhs;
+}
+
+template <typename T>
+Angle<T> operator-(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs -= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Angle<T> lhs, Scalar rhs)
+{
+    lhs *= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Scalar lhs, Angle<T> rhs)
+{
+    rhs *= lhs;
+    return rhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator/(Angle<T> lhs, Scalar rhs)
+{
+    lhs /= rhs;
+    return lhs;
+}
+
+template <typename T>
+double operator/(Angle<T> lhs, Angle<T> rhs)
+{
+    return double(lhs.raw()) / rhs.raw();
+}
+
+template <typename T>
+bool operator==(Angle<T> lhs, Angle<T> rhs)
+{
+    return lhs.raw() == rhs.raw();
+}
+
+template <typename T>
+bool operator!=(Angle<T> lhs, Angle<T> rhs)
+{
+    return !(lhs == rhs);
+}
+
+// Fast trigonometric functions. Single precision is sufficient for Angle16 and Angle8.
+float sin(Angle16 a);
+float cos(Angle16 a);
+float tan(Angle16 a);
+float sin(Angle8 a);
+float cos(Angle8 a);
+float tan(Angle8 a);
+
+}

src/common/math/box2d.cpp

@@ -0,0 +1,279 @@
+#include "math/box2d.h"
+#include "math/math_utils.h"
+#include "math/polygon2d.h"
+
+#include <cmath>
+#include <sstream>
+
+using namespace decision::math;
+
+namespace {
+
+double outer_prod(double x0, double y0, double x1, double y1, double x2, double y2)
+{
+    return (x1 - x0) * (y2 - y0) - (x2 - x0) * (y1 - y0);
+}
+
+double pt_seg_distance(double query_x, double query_y, double start_x, double start_y,
+    double end_x, double end_y, double length)
+{
+    const double x0 = query_x - start_x;
+    const double y0 = query_y - start_y;
+    const double dx = end_x - start_x;
+    const double dy = end_y - start_y;
+    const double proj = x0 * dx + y0 * dy;
+    if (proj <= 0.0) {
+        return hypot(x0, y0);
+    }
+    if (proj >= length * length) {
+        return hypot(x0 - dx, y0 - dy);
+    }
+    return std::abs(x0 * dy - y0 * dx) / length;
+}
+
+} // namespace
+
+Box2d::Box2d(const Vec2d& center, const double heading,
+    const double length, const double width)
+    : _center(center)
+    , _length(length)
+    , _width(width)
+    , _half_length(length / 2.0)
+    , _half_width(width / 2.0)
+    , _heading(heading)
+    , _cos_heading(cos(heading))
+    , _sin_heading(sin(heading))
+{
+    CHECK_GT(_length, -kMathEpsilon);
+    CHECK_GT(_width, -kMathEpsilon);
+}
+
+Box2d::Box2d(const Segment2d& axis, const double width)
+    : _center(axis.center())
+    , _length(axis.length())
+    , _width(width)
+    , _half_length(axis.length() / 2.0)
+    , _half_width(width / 2.0)
+    , _heading(axis.heading())
+    , _cos_heading(axis.cos_heading())
+    , _sin_heading(axis.sin_heading())
+{
+    CHECK_GT(_length, -kMathEpsilon);
+    CHECK_GT(_width, -kMathEpsilon);
+}
+
+Box2d::Box2d(const AABox2d& aabox)
+    : _center(aabox.center())
+    , _length(aabox.length())
+    , _width(aabox.width())
+    , _half_length(aabox.half_length())
+    , _half_width(aabox.half_width())
+    , _heading(0.0)
+    , _cos_heading(1.0)
+    , _sin_heading(0.0)
+{
+    CHECK_GT(_length, -kMathEpsilon);
+    CHECK_GT(_width, -kMathEpsilon);
+}
+
+Box2d Box2d::create_aa_box(const Vec2d& one_corner, const Vec2d& opponent_corner)
+{
+    const double x1 = std::min(one_corner.x(), opponent_corner.x());
+    const double x2 = std::max(one_corner.x(), opponent_corner.x());
+    const double y1 = std::min(one_corner.y(), opponent_corner.y());
+    const double y2 = std::max(one_corner.y(), opponent_corner.y());
+    return Box2d({ (x1 + x2) / 2.0, (y1 + y2) / 2.0 }, 0.0, x2 - x1, y2 - y1);
+}
+
+void Box2d::get_all_corners(std::vector<Vec2d>* const corners) const
+{
+    if (corners == nullptr) {
+        return;
+    }
+    const double dx1 = _cos_heading * _half_length;
+    const double dy1 = _sin_heading * _half_length;
+    const double dx2 = _sin_heading * _half_width;
+    const double dy2 = -_cos_heading * _half_width;
+    corners->clear();
+    corners->reserve(4);
+    corners->emplace_back(_center.x() + dx1 + dx2, _center.y() + dy1 + dy2);
+    corners->emplace_back(_center.x() + dx1 - dx2, _center.y() + dy1 - dy2);
+    corners->emplace_back(_center.x() - dx1 - dx2, _center.y() - dy1 - dy2);
+    corners->emplace_back(_center.x() - dx1 + dx2, _center.y() - dy1 + dy2);
+}
+
+bool Box2d::is_point_in(const Vec2d& point) const
+{
+    const double x0 = point.x() - _center.x();
+    const double y0 = point.y() - _center.y();
+    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading);
+    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading);
+    return dx <= _half_length + kMathEpsilon && dy <= _half_width + kMathEpsilon;
+}
+
+bool Box2d::is_point_on_boundary(const Vec2d& point) const
+{
+    const double x0 = point.x() - _center.x();
+    const double y0 = point.y() - _center.y();
+    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading);
+    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading);
+    return (std::abs(dx - _half_length) <= kMathEpsilon && dy <= _half_width + kMathEpsilon) || (std::abs(dy - _half_width) <= kMathEpsilon && dx <= _half_length + kMathEpsilon);
+}
+
+double Box2d::distance_to(const Vec2d& point) const
+{
+    const double x0 = point.x() - _center.x();
+    const double y0 = point.y() - _center.y();
+    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading) - _half_length;
+    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading) - _half_width;
+    if (dx <= 0.0) {
+        return std::max(0.0, dy);
+    }
+    if (dy <= 0.0) {
+        return dx;
+    }
+    return hypot(dx, dy);
+}
+
+bool Box2d::has_overlap(const Segment2d& segment) const
+{
+    if (segment.length() <= kMathEpsilon) {
+        return is_point_in(segment.start());
+    }
+    return distance_to(segment) <= kMathEpsilon;
+}
+
+double Box2d::distance_to(const Segment2d& segment) const
+{
+    if (segment.length() <= kMathEpsilon) {
+        return distance_to(segment.start());
+    }
+    const double ref_x1 = segment.start().x() - _center.x();
+    const double ref_y1 = segment.start().y() - _center.y();
+    double x1 = ref_x1 * _cos_heading + ref_y1 * _sin_heading;
+    double y1 = ref_x1 * _sin_heading - ref_y1 * _cos_heading;
+    double box_x = _half_length;
+    double box_y = _half_width;
+    int gx1 = (x1 >= box_x ? 1 : (x1 <= -box_x ? -1 : 0));
+    int gy1 = (y1 >= box_y ? 1 : (y1 <= -box_y ? -1 : 0));
+    if (gx1 == 0 && gy1 == 0) {
+        return 0.0;
+    }
+    const double ref_x2 = segment.end().x() - _center.x();
+    const double ref_y2 = segment.end().y() - _center.y();
+    double x2 = ref_x2 * _cos_heading + ref_y2 * _sin_heading;
+    double y2 = ref_x2 * _sin_heading - ref_y2 * _cos_heading;
+    int gx2 = (x2 >= box_x ? 1 : (x2 <= -box_x ? -1 : 0));
+    int gy2 = (y2 >= box_y ? 1 : (y2 <= -box_y ? -1 : 0));
+    if (gx2 == 0 && gy2 == 0) {
+        return 0.0;
+    }
+    if (gx1 < 0 || (gx1 == 0 && gx2 < 0)) {
+        x1 = -x1;
+        gx1 = -gx1;
+        x2 = -x2;
+        gx2 = -gx2;
+    }
+    if (gy1 < 0 || (gy1 == 0 && gy2 < 0)) {
+        y1 = -y1;
+        gy1 = -gy1;
+        y2 = -y2;
+        gy2 = -gy2;
+    }
+    if (gx1 < gy1 || (gx1 == gy1 && gx2 < gy2)) {
+        std::swap(x1, y1);
+        std::swap(gx1, gy1);
+        std::swap(x2, y2);
+        std::swap(gx2, gy2);
+        std::swap(box_x, box_y);
+    }
+    if (gx1 == 1 && gy1 == 1) {
+        switch (gx2 * 3 + gy2) {
+        case 4:
+            return pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
+        case 3:
+            return (x1 > x2) ? (x2 - box_x)
+                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
+        case 2:
+            return (x1 > x2) ? pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length())
+                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
+        case -1:
+            return outer_prod(x1, y1, x2, y2, box_x, -box_y) >= 0.0 ? 0.0
+                                                                    : pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length());
+        case -4:
+            return outer_prod(x1, y1, x2, y2, box_x, -box_y) <= 0.0 ? pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length())
+                                                                    : (outer_prod(x1, y1, x2, y2, -box_x, box_y) <= 0.0 ? 0.0
+                                                                                                                        : pt_seg_distance(-box_x, box_y, x1, y1, x2, y2, segment.length()));
+        }
+    } else {
+        switch (gx2 * 3 + gy2) {
+        case 4:
+            return (x1 < x2) ? (x1 - box_x)
+                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
+        case 3:
+            return std::min(x1, x2) - box_x;
+        case 1:
+        case -2:
+            return outer_prod(x1, y1, x2, y2, box_x, box_y) <= 0.0 ? 0.0
+                                                                   : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
+        case -3:
+            return 0.0;
+        }
+    }
+    // CHECK(0) << "unimplemented state: " << gx1 << " " << gy1 << " " << gx2 << " " << gy2;
+    return 0.0;
+}
+
+double Box2d::distance_to(const Box2d& box) const
+{
+    return Polygon2d(box).distance_to(*this);
+}
+
+bool Box2d::has_overlap(const Box2d& box) const
+{
+    const double shift_x = box.center_x() - _center.x();
+    const double shift_y = box.center_y() - _center.y();
+
+    const double dx1 = _cos_heading * _half_length;
+    const double dy1 = _sin_heading * _half_length;
+    const double dx2 = _sin_heading * _half_width;
+    const double dy2 = -_cos_heading * _half_width;
+    const double dx3 = box.cos_heading() * box.half_length();
+    const double dy3 = box.sin_heading() * box.half_length();
+    const double dx4 = box.sin_heading() * box.half_width();
+    const double dy4 = -box.cos_heading() * box.half_width();
+
+    return std::abs(shift_x * _cos_heading + shift_y * _sin_heading) <= std::abs(dx3 * _cos_heading + dy3 * _sin_heading) + std::abs(dx4 * _cos_heading + dy4 * _sin_heading) + _half_length && std::abs(shift_x * _sin_heading - shift_y * _cos_heading) <= std::abs(dx3 * _sin_heading - dy3 * _cos_heading) + std::abs(dx4 * _sin_heading - dy4 * _cos_heading) + _half_width && std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <= std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) + std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) + box.half_length() && std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <= std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) + std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) + box.half_width();
+}
+
+AABox2d Box2d::get_aa_box() const
+{
+    const double dx1 = std::abs(_cos_heading * _half_length);
+    const double dy1 = std::abs(_sin_heading * _half_length);
+    const double dx2 = std::abs(_sin_heading * _half_width);
+    const double dy2 = std::abs(_cos_heading * _half_width);
+    return AABox2d(_center, (dx1 + dx2) * 2.0, (dy1 + dy2) * 2.0);
+}
+
+void Box2d::rotate_from_center(double rotate_angle)
+{
+    _heading = normalize_angle(_heading + rotate_angle);
+    _cos_heading = cos(_heading);
+    _sin_heading = sin(_heading);
+}
+
+void Box2d::shift(const Vec2d& shift_vec)
+{
+    _center += shift_vec;
+}
+
+std::string Box2d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "box2d ( center = " << _center.debug_string()
+         << "  heading = " << _heading
+         << "  length = " << _length
+         << "  width = " << _width << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/box2d.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class Box2d {
+public:
+    Box2d(const Vec2d& center, const double heading,
+        const double length, const double width);
+    Box2d(const Segment2d& axis, const double width);
+    explicit Box2d(const AABox2d& aabox);
+    static Box2d create_aa_box(const Vec2d& one_corner, const Vec2d& opponent_corner);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _cos_heading; }
+    double sin_heading() const { return _sin_heading; }
+    double area() const { return _length * _width; }
+    double diagonal() const { return hypot(_length, _width); }
+
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool has_overlap(const Box2d& box) const;
+
+    AABox2d get_aa_box() const;
+
+    void rotate_from_center(double rotate_angle);
+    void shift(const Vec2d& shift_vec);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _heading = 0.0;
+    double _cos_heading = 1.0;
+    double _sin_heading = 0.0;
+};
+
+}

src/common/math/euler_angles_zxy.h

@@ -0,0 +1,118 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// A class of Euler angles with the intrinsic sequence ZXY.
+//
+// Our vehicle reference frame follows NovAtel's convention right/forward/up. The Euler angles
+// represents the rotation from the local frame (east/north/up) to the vehicle frame, as explained
+// below:
+//   roll: is zero when the car is level, and positive when the left side of the car is up;
+//   pitch: is zero when the car is level, and positive when the car is nose up;
+//   yaw: is zero when the car is pointing to north, and positive when the car turns left.
+// When using Euler angles to represent the rotation, we should use the intrinsic rotations ZXY. See
+// below for more explanations.
+//   http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html
+
+#ifndef ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+#define ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+
+#include "Eigen/Geometry"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace adu {
+namespace common {
+namespace math {
+
+template <typename T>
+struct EulerAnglesZXY {
+    T roll, pitch, yaw;
+
+    // Constructs an identity rotation.
+    EulerAnglesZXY()
+        : roll(0)
+        , pitch(0)
+        , yaw(0)
+    {
+    }
+
+    // Constructs a rotation using only yaw.
+    explicit EulerAnglesZXY(T yaw)
+        : roll(0)
+        , pitch(0)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using roll, pitch, and yaw.
+    EulerAnglesZXY(T roll, T pitch, T yaw)
+        : roll(roll)
+        , pitch(pitch)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using components of a quaternion.
+    EulerAnglesZXY(T qw, T qx, T qy, T qz)
+        : roll(atan2(2.0 * (qw * qy - qx * qz), 2.0 * (sqr<T>(qw) + sqr<T>(qz)) - 1.0))
+        , pitch(asin(2.0 * (qw * qx + qy * qz)))
+        , yaw(atan2(2.0 * (qw * qz - qx * qy), 2.0 * (sqr<T>(qw) + sqr<T>(qy)) - 1.0))
+    {
+    }
+
+    // Constructs a rotation using a quaternion.
+    explicit EulerAnglesZXY(const Eigen::Quaternion<T>& q)
+        : EulerAnglesZXY(q.w(), q.x(), q.y(), q.z())
+    {
+    }
+
+    // Normalizes roll, pitch, and yaw to [-PI, PI).
+    void normalize()
+    {
+        roll = normalize_angle(roll);
+        pitch = normalize_angle(pitch);
+        yaw = normalize_angle(yaw);
+    }
+
+    // Check if the rotation is valid. A valid rotation must have -PI/2 < pitch < PI/2.
+    bool is_valid()
+    {
+        normalize();
+        return pitch < M_PI_2 && pitch > -M_PI_2;
+    }
+
+    // Converts to a quaternion. The scalar part is guarantee to be non-negative.
+    Eigen::Quaternion<T> to_quaternion() const
+    {
+        // Uses double for internal calculation for better precision.
+        double r = roll * 0.5;
+        double p = pitch * 0.5;
+        double y = yaw * 0.5;
+
+        double sr = sin(r);
+        double sp = sin(p);
+        double sy = sin(y);
+
+        double cr = cos(r);
+        double cp = cos(p);
+        double cy = cos(y);
+
+        T qw = cr * cp * cy - sr * sp * sy;
+        T qx = cr * sp * cy - sr * cp * sy;
+        T qy = cr * sp * sy + sr * cp * cy;
+        T qz = cr * cp * sy + sr * sp * cy;
+        if (qw < 0.0)
+            return { -qw, -qx, -qy, -qz };
+        return { qw, qx, qy, qz };
+    }
+};
+
+using EulerAnglesZXYf = EulerAnglesZXY<float>;
+using EulerAnglesZXYd = EulerAnglesZXY<double>;
+
+} // namespace math
+} // namespace common
+} // namespace adu
+
+#endif // ADU_COMMON_MATH_EULER_ANGLES_ZXY_H

src/common/math/heading.h

@@ -0,0 +1,45 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// Helper functions that converts heading from/to a quaternion.
+//
+// We have permanently defined that the heading of car is zero when the car is pointing to east, and
+// positive when the car turns left. This function guarantees correct heading calculation,
+// regardless any future changes in the definition of vehicle reference frame.
+
+#pragma once
+
+#include "Eigen/Geometry"
+#include "math/euler_angles_zxy.h"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace decision::math {
+
+// Returns heading (in radians) in [-PI, PI).
+inline double quaternion_to_heading(double qw, double qx, double qy, double qz)
+{
+    EulerAnglesZXYd euler_angles(qw, qx, qy, qz);
+    // Needs adding pi/2 to yaw because when the vehicle reference frame is RFU, yaw is zero when
+    // the car is pointing to north.
+    return normalize_angle(euler_angles.yaw + M_PI_2);
+}
+
+// Similar to above but takes a Quaternion object.
+template <typename T>
+inline double quaternion_to_heading(const Eigen::Quaternion<T>& q)
+{
+    return quaternion_to_heading(q.w(), q.x(), q.y(), q.z());
+}
+
+// Returns a quaternion with zero roll, zero pitch, and the specified heading.
+template <typename T>
+inline Eigen::Quaternion<T> heading_to_quaternion(double heading)
+{
+    // Needs deducting pi/2 from heading because the vehicle reference frame is RFU.
+    EulerAnglesZXY<T> euler_angles(heading - M_PI_2);
+    return euler_angles.to_quaternion();
+}
+
+}

src/common/math/math_utils.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/polygon2d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+constexpr double kMathEpsilon = 1e-10;
+
+inline double cross_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.y() - point1.y()) - (point3.x() - point1.x()) * (point2.y() - point1.y());
+}
+
+inline Vec3d cross_prod(const Vec3d& point1, const Vec3d& point2, const Vec3d& point3)
+{
+    Vec3d v12 = point2 - point1;
+    Vec3d v13 = point3 - point1;
+    return v12.cross_prod(v13);
+}
+
+inline double inner_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.x() - point1.x()) + (point2.y() - point1.y()) * (point3.y() - point1.y());
+}
+
+// Wrap angle to [0, 2 * PI).
+inline double wrap_angle(const double angle)
+{
+    const double new_angle = fmod(angle, M_PI * 2.0);
+    return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle;
+}
+
+// Normalize angle to [-PI, PI).
+inline double normalize_angle(const double angle)
+{
+    const double new_angle = fmod(angle + M_PI, M_PI * 2.0);
+    return (new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle) - M_PI;
+}
+
+template <typename T>
+inline T sqr(const T value)
+{
+    return value * value;
+}
+
+}
+
+#include <iostream>
+#define CHECK(v) (void)(v)
+#define CHECK_NOTNULL(v) (void)(v)
+#define CHECK_GE(a, b)
+#define CHECK_GT(a, b)

src/common/math/polygon2d.cpp

@@ -0,0 +1,592 @@
+#include "math/polygon2d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <sstream>
+
+using namespace decision::math;
+
+Polygon2d::Polygon2d(const Box2d& box)
+{
+    box.get_all_corners(&_points);
+    build_from_points();
+}
+
+Polygon2d::Polygon2d(std::vector<Vec2d> points)
+    : _points(std::move(points))
+{
+    build_from_points();
+}
+
+double Polygon2d::distance_to(const Vec2d& point) const
+{
+    CHECK_GE(_points.size(), 3u);
+    if (is_point_in(point)) {
+        return 0.0;
+    }
+    double distance = std::numeric_limits<double>::infinity();
+    for (int i = 0; i < _num_points; ++i) {
+        distance = std::min(distance, _segments[i].distance_to(point));
+    }
+    return distance;
+}
+
+double Polygon2d::distance_sqr_to(const Vec2d& point) const
+{
+    CHECK_GE(_points.size(), 3u);
+    if (is_point_in(point)) {
+        return 0.0;
+    }
+    double distance_sqr = std::numeric_limits<double>::infinity();
+    for (int i = 0; i < _num_points; ++i) {
+        distance_sqr = std::min(distance_sqr, _segments[i].distance_sqr_to(point));
+    }
+    return distance_sqr;
+}
+
+double Polygon2d::distance_to(const Segment2d& segment) const
+{
+    if (segment.length() <= kMathEpsilon) {
+        return distance_to(segment.start());
+    }
+    CHECK_GE(_points.size(), 3u);
+    if (is_point_in(segment.center())) {
+        return 0.0;
+    }
+    if (std::any_of(_segments.begin(), _segments.end(),
+            [&](const Segment2d& poly_seg) {
+                return poly_seg.has_intersect(segment);
+            })) {
+        return 0.0;
+    }
+
+    double distance = std::min(distance_to(segment.start()), distance_to(segment.end()));
+    for (int i = 0; i < _num_points; ++i) {
+        distance = std::min(distance, segment.distance_to(_points[i]));
+    }
+    return distance;
+}
+
+double Polygon2d::distance_to(const Box2d& box) const
+{
+    CHECK_GE(_points.size(), 3u);
+    return distance_to(Polygon2d(box));
+}
+
+double Polygon2d::distance_to(const Polygon2d& polygon) const
+{
+    CHECK_GE(_points.size(), 3u);
+    CHECK_GE(polygon.num_points(), 3);
+
+    if (is_point_in(polygon.points()[0])) {
+        return 0.0;
+    }
+    if (polygon.is_point_in(_points[0])) {
+        return 0.0;
+    }
+    double distance = std::numeric_limits<double>::infinity();
+    for (int i = 0; i < _num_points; ++i) {
+        distance = std::min(distance, polygon.distance_to(_segments[i]));
+    }
+    return distance;
+}
+
+double Polygon2d::distance_to_boundary(const Vec2d& point) const
+{
+    double distance = std::numeric_limits<double>::infinity();
+    for (int i = 0; i < _num_points; ++i) {
+        distance = std::min(distance, _segments[i].distance_to(point));
+    }
+    return distance;
+}
+
+bool Polygon2d::is_point_on_boundary(const Vec2d& point) const
+{
+    CHECK_GE(_points.size(), 3u);
+    return std::any_of(_segments.begin(), _segments.end(),
+        [&](const Segment2d& poly_seg) {
+            return poly_seg.is_point_in(point);
+        });
+}
+
+bool Polygon2d::is_point_in(const Vec2d& point) const
+{
+    CHECK_GE(_points.size(), 3u);
+    if (is_point_on_boundary(point)) {
+        return true;
+    }
+    int j = _num_points - 1;
+    int c = 0;
+    for (int i = 0; i < _num_points; ++i) {
+        if ((_points[i].y() > point.y()) != (_points[j].y() > point.y())) {
+            const double side = cross_prod(point, _points[i], _points[j]);
+            if (_points[i].y() < _points[j].y() ? side > 0.0 : side < 0.0) {
+                ++c;
+            }
+        }
+        j = i;
+    }
+    return c & 1;
+}
+
+bool Polygon2d::has_overlap(const Polygon2d& polygon) const
+{
+    CHECK_GE(_points.size(), 3u);
+    return distance_to(polygon) <= kMathEpsilon;
+}
+
+bool Polygon2d::is_contain(const Segment2d& segment) const
+{
+    if (segment.length() <= kMathEpsilon) {
+        return is_point_in(segment.start());
+    }
+    CHECK_GE(_points.size(), 3u);
+    if (!is_point_in(segment.start())) {
+        return false;
+    }
+    if (!is_point_in(segment.end())) {
+        return false;
+    }
+    if (!_is_convex) {
+        std::vector<Segment2d> overlaps = get_all_overlaps(segment);
+        double total_length = 0;
+        for (const auto& overlap_seg : overlaps) {
+            total_length += overlap_seg.length();
+        }
+        return total_length >= segment.length() - kMathEpsilon;
+    }
+    return true;
+}
+
+bool Polygon2d::is_contain(const Polygon2d& polygon) const
+{
+    CHECK_GE(_points.size(), 3u);
+    if (_area < polygon.area() - kMathEpsilon) {
+        return false;
+    }
+    if (!is_point_in(polygon.points()[0])) {
+        return false;
+    }
+    const auto& segments = polygon.segments();
+    return std::all_of(segments.begin(), segments.end(),
+        [&](const Segment2d& segment) {
+            return is_contain(segment);
+        });
+}
+
+int Polygon2d::next(int at) const
+{
+    return at >= _num_points - 1 ? 0 : at + 1;
+}
+
+int Polygon2d::prev(int at) const
+{
+    return at == 0 ? _num_points - 1 : at - 1;
+}
+
+void Polygon2d::build_from_points()
+{
+    _num_points = _points.size();
+    CHECK_GE(_num_points, 3);
+
+    // Make sure the points are in ccw order.
+    _area = 0.0;
+    for (int i = 1; i < _num_points; ++i) {
+        _area += cross_prod(_points[0], _points[i - 1], _points[i]);
+    }
+    if (_area < 0) {
+        _area = -_area;
+        std::reverse(_points.begin(), _points.end());
+    }
+    _area /= 2.0;
+    CHECK_GT(_area, kMathEpsilon);
+
+    // Construct segments.
+    _segments.reserve(_num_points);
+    for (int i = 0; i < _num_points; ++i) {
+        _segments.emplace_back(_points[i], _points[next(i)]);
+    }
+
+    // Check convexity.
+    _is_convex = true;
+    for (int i = 0; i < _num_points; ++i) {
+        if (cross_prod(_points[prev(i)], _points[i], _points[next(i)]) <= -kMathEpsilon) {
+            _is_convex = false;
+            break;
+        }
+    }
+
+    // Compute aabox.
+    _min_x = _points[0].x();
+    _max_x = _points[0].x();
+    _min_y = _points[0].y();
+    _max_y = _points[0].y();
+    for (const auto& point : _points) {
+        _min_x = std::min(_min_x, point.x());
+        _max_x = std::max(_max_x, point.x());
+        _min_y = std::min(_min_y, point.y());
+        _max_y = std::max(_max_y, point.y());
+    }
+}
+
+bool Polygon2d::compute_convex_hull(const std::vector<Vec2d>& points,
+    Polygon2d* const polygon)
+{
+    CHECK_NOTNULL(polygon);
+    const int n = points.size();
+    if (n < 3) {
+        return false;
+    }
+    std::vector<int> sorted_indices(n);
+    for (int i = 0; i < n; ++i) {
+        sorted_indices[i] = i;
+    }
+    std::sort(sorted_indices.begin(), sorted_indices.end(),
+        [&](const int idx1, const int idx2) {
+            const Vec2d& pt1 = points[idx1];
+            const Vec2d& pt2 = points[idx2];
+            const double dx = pt1.x() - pt2.x();
+            if (std::abs(dx) > kMathEpsilon) {
+                return dx < 0.0;
+            }
+            return pt1.y() < pt2.y();
+        });
+    int count = 0;
+    std::vector<int> results;
+    results.reserve(n);
+    int last_count = 1;
+    for (int i = 0; i < n + n; ++i) {
+        if (i == n) {
+            last_count = count;
+        }
+        const int idx = sorted_indices[(i < n) ? i : (n + n - 1 - i)];
+        const Vec2d& pt = points[idx];
+        while (count > last_count && cross_prod(points[results[count - 2]], points[results[count - 1]], pt) <= kMathEpsilon) {
+            results.pop_back();
+            --count;
+        }
+        results.push_back(idx);
+        ++count;
+    }
+    --count;
+    if (count < 3) {
+        return false;
+    }
+    std::vector<Vec2d> result_points;
+    result_points.reserve(count);
+    for (int i = 0; i < count; ++i) {
+        result_points.push_back(points[results[i]]);
+    }
+
+    // check if area > kMathEpsilon
+    double area = 0.0;
+    for (int i = 1; i < count; ++i) {
+        area += cross_prod(result_points[0], result_points[i - 1], result_points[i]);
+    }
+    if (area < 0) {
+        area = -area;
+    }
+    area /= 2.0;
+    if (area > kMathEpsilon) {
+        *polygon = Polygon2d(result_points);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Polygon2d::clip_convex_hull(const Segment2d& segment,
+    std::vector<Vec2d>* const points)
+{
+    if (segment.length() <= kMathEpsilon) {
+        return true;
+    }
+    CHECK_NOTNULL(points);
+    const int n = points->size();
+    if (n < 3) {
+        return false;
+    }
+    std::vector<double> prod(n);
+    std::vector<int> side(n);
+    for (int i = 0; i < n; ++i) {
+        prod[i] = cross_prod(segment.start(), segment.end(), (*points)[i]);
+        if (std::abs(prod[i]) <= kMathEpsilon) {
+            side[i] = 0;
+        } else {
+            side[i] = ((prod[i] < 0) ? -1 : 1);
+        }
+    }
+
+    std::vector<Vec2d> new_points;
+    for (int i = 0; i < n; ++i) {
+        if (side[i] >= 0) {
+            new_points.push_back((*points)[i]);
+        }
+        const int j = ((i == n - 1) ? 0 : (i + 1));
+        if (side[i] * side[j] < 0) {
+            const double ratio = prod[j] / (prod[j] - prod[i]);
+            new_points.emplace_back((*points)[i].x() * ratio + (*points)[j].x() * (1.0 - ratio),
+                (*points)[i].y() * ratio + (*points)[j].y() * (1.0 - ratio));
+        }
+    }
+
+    points->swap(new_points);
+    return points->size() >= 3;
+}
+
+bool Polygon2d::compute_overlap(const Polygon2d& other_polygon, Polygon2d* const overlap_polygon) const
+{
+    CHECK_GE(_points.size(), 3u);
+    CHECK_NOTNULL(overlap_polygon);
+    CHECK(_is_convex && other_polygon.is_convex());
+    std::vector<Vec2d> points = other_polygon.points();
+    for (int i = 0; i < _num_points; ++i) {
+        if (!clip_convex_hull(_segments[i], &points)) {
+            return false;
+        }
+    }
+    return compute_convex_hull(points, overlap_polygon);
+}
+
+bool Polygon2d::has_overlap(const Segment2d& segment) const
+{
+    CHECK_GE(_points.size(), 3u);
+    Vec2d first;
+    Vec2d last;
+    return get_overlap(segment, &first, &last);
+}
+
+bool Polygon2d::get_overlap(const Segment2d& segment, Vec2d* const first, Vec2d* const last) const
+{
+    CHECK_GE(_points.size(), 3u);
+    CHECK_NOTNULL(first);
+    CHECK_NOTNULL(last);
+
+    if (segment.length() <= kMathEpsilon) {
+        if (!is_point_in(segment.start())) {
+            return false;
+        }
+        *first = segment.start();
+        *last = segment.start();
+        return true;
+    }
+
+    double min_proj = segment.length();
+    double max_proj = 0;
+    if (is_point_in(segment.start())) {
+        *first = segment.start();
+        min_proj = 0.0;
+    }
+    if (is_point_in(segment.end())) {
+        *last = segment.end();
+        max_proj = segment.length();
+    }
+    for (const auto& poly_seg : _segments) {
+        Vec2d pt;
+        if (poly_seg.get_intersect(segment, &pt)) {
+            const double proj = segment.project_onto_unit(pt);
+            if (proj < min_proj) {
+                min_proj = proj;
+                *first = pt;
+            }
+            if (proj > max_proj) {
+                max_proj = proj;
+                *last = pt;
+            }
+        }
+    }
+    return min_proj <= max_proj + kMathEpsilon;
+}
+
+std::vector<Segment2d> Polygon2d::get_all_overlaps(const Segment2d& segment) const
+{
+    CHECK_GE(_points.size(), 3u);
+
+    if (segment.length() <= kMathEpsilon) {
+        std::vector<Segment2d> overlaps;
+        if (is_point_in(segment.start())) {
+            overlaps.push_back(segment);
+        }
+        return overlaps;
+    }
+    std::vector<double> projections;
+    if (is_point_in(segment.start())) {
+        projections.push_back(0.0);
+    }
+    if (is_point_in(segment.end())) {
+        projections.push_back(segment.length());
+    }
+    for (const auto& poly_seg : _segments) {
+        Vec2d pt;
+        if (poly_seg.get_intersect(segment, &pt)) {
+            projections.push_back(segment.project_onto_unit(pt));
+        }
+    }
+    std::sort(projections.begin(), projections.end());
+    std::vector<std::pair<double, double>> overlaps;
+    for (size_t i = 0; i + 1 < projections.size(); ++i) {
+        const double start_proj = projections[i];
+        const double end_proj = projections[i + 1];
+        if (end_proj - start_proj <= kMathEpsilon) {
+            continue;
+        }
+        const Vec2d reference_point = segment.start() + (start_proj + end_proj) / 2.0 * segment.unit_direction();
+        if (!is_point_in(reference_point)) {
+            continue;
+        }
+        if (overlaps.empty() || start_proj > overlaps.back().second + kMathEpsilon) {
+            overlaps.emplace_back(start_proj, end_proj);
+        } else {
+            overlaps.back().second = end_proj;
+        }
+    }
+    std::vector<Segment2d> overlap_segments;
+    for (const auto& overlap : overlaps) {
+        overlap_segments.emplace_back(
+            segment.start() + overlap.first * segment.unit_direction(),
+            segment.start() + overlap.second * segment.unit_direction());
+    }
+    return overlap_segments;
+}
+
+void Polygon2d::extreme_points(const double heading, Vec2d* const first, Vec2d* const last) const
+{
+    CHECK_GE(_points.size(), 3u);
+    CHECK_NOTNULL(first);
+    CHECK_NOTNULL(last);
+
+    const Vec2d direction_vec = Vec2d::create_unit_vec(heading);
+    double min_proj = std::numeric_limits<double>::infinity();
+    double max_proj = -std::numeric_limits<double>::infinity();
+    for (const auto& pt : _points) {
+        const double proj = pt.inner_prod(direction_vec);
+        if (proj < min_proj) {
+            min_proj = proj;
+            *first = pt;
+        }
+        if (proj > max_proj) {
+            max_proj = proj;
+            *last = pt;
+        }
+    }
+}
+
+AABox2d Polygon2d::aa_bounding_box() const
+{
+    return AABox2d({ _min_x, _min_y }, { _max_x, _max_y });
+}
+
+Box2d Polygon2d::bounding_box_with_heading(const double heading) const
+{
+    CHECK_GE(_points.size(), 3u);
+    const Vec2d direction_vec = Vec2d::create_unit_vec(heading);
+    Vec2d px1;
+    Vec2d px2;
+    Vec2d py1;
+    Vec2d py2;
+    extreme_points(heading, &px1, &px2);
+    extreme_points(heading - M_PI_2, &py1, &py2);
+    const double x1 = px1.inner_prod(direction_vec);
+    const double x2 = px2.inner_prod(direction_vec);
+    const double y1 = py1.cross_prod(direction_vec);
+    const double y2 = py2.cross_prod(direction_vec);
+    return Box2d((x1 + x2) / 2.0 * direction_vec + (y1 + y2) / 2.0 * Vec2d(direction_vec.y(), -direction_vec.x()),
+        heading, x2 - x1, y2 - y1);
+}
+
+Box2d Polygon2d::min_area_bounding_box() const
+{
+    CHECK_GE(_points.size(), 3u);
+    if (!_is_convex) {
+        Polygon2d convex_polygon;
+        compute_convex_hull(_points, &convex_polygon);
+        CHECK(convex_polygon.is_convex());
+        return convex_polygon.min_area_bounding_box();
+    }
+    double min_area = std::numeric_limits<double>::infinity();
+    double min_area_at_heading = 0.0;
+    int left_most = 0;
+    int right_most = 0;
+    int top_most = 0;
+    for (int i = 0; i < _num_points; ++i) {
+        const auto& segment = _segments[i];
+        double proj = 0.0;
+        double min_proj = segment.project_onto_unit(_points[left_most]);
+        while ((proj = segment.project_onto_unit(_points[prev(left_most)])) < min_proj) {
+            min_proj = proj;
+            left_most = prev(left_most);
+        }
+        while ((proj = segment.project_onto_unit(_points[next(left_most)])) < min_proj) {
+            min_proj = proj;
+            left_most = next(left_most);
+        }
+        double max_proj = segment.project_onto_unit(_points[right_most]);
+        while ((proj = segment.project_onto_unit(_points[prev(right_most)])) > max_proj) {
+            max_proj = proj;
+            right_most = prev(right_most);
+        }
+        while ((proj = segment.project_onto_unit(_points[next(right_most)])) > max_proj) {
+            max_proj = proj;
+            right_most = next(right_most);
+        }
+        double prod = 0.0;
+        double max_prod = segment.product_onto_unit(_points[top_most]);
+        while ((prod = segment.product_onto_unit(_points[prev(top_most)])) > max_prod) {
+            max_prod = prod;
+            top_most = prev(top_most);
+        }
+        while ((prod = segment.product_onto_unit(_points[next(top_most)])) > max_prod) {
+            max_prod = prod;
+            top_most = next(top_most);
+        }
+        const double area = max_prod * (max_proj - min_proj);
+        if (area < min_area) {
+            min_area = area;
+            min_area_at_heading = segment.heading();
+        }
+    }
+    return bounding_box_with_heading(min_area_at_heading);
+}
+
+Polygon2d Polygon2d::expand_by_distance(const double distance) const
+{
+    if (!_is_convex) {
+        Polygon2d convex_polygon;
+        compute_convex_hull(_points, &convex_polygon);
+        CHECK(convex_polygon.is_convex());
+        return convex_polygon.expand_by_distance(distance);
+    }
+    const double kMinAngle = 0.1;
+    std::vector<Vec2d> points;
+    for (int i = 0; i < _num_points; ++i) {
+        const double start_angle = _segments[prev(i)].heading() - M_PI_2;
+        const double end_angle = _segments[i].heading() - M_PI_2;
+        const double diff = wrap_angle(end_angle - start_angle);
+        if (diff <= kMathEpsilon) {
+            points.push_back(_points[i] + Vec2d::create_unit_vec(start_angle) * distance);
+        } else {
+            const int count = static_cast<int>(diff / kMinAngle) + 1;
+            for (int k = 0; k <= count; ++k) {
+                const double angle = start_angle + diff * static_cast<double>(k) / static_cast<double>(count);
+                points.push_back(_points[i] + Vec2d::create_unit_vec(angle) * distance);
+            }
+        }
+    }
+    Polygon2d new_polygon;
+    CHECK(compute_convex_hull(points, &new_polygon));
+    return new_polygon;
+}
+
+std::string Polygon2d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "polygon2d (  num_points = " << _num_points << "  points = (";
+    for (const auto& pt : _points) {
+        sout << " " << pt.debug_string();
+    }
+    sout << " )  " << (_is_convex ? "convex" : "non-convex") << "  area = " << _area << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/polygon2d.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math{
+
+class Polygon2d {
+public:
+    Polygon2d() = default;
+    explicit Polygon2d(const Box2d& box);
+    explicit Polygon2d(std::vector<Vec2d> points);
+
+    const std::vector<Vec2d>& points() const { return _points; }
+    const std::vector<Segment2d>& segments() const { return _segments; }
+    int num_points() const { return _num_points; }
+    bool is_convex() const { return _is_convex; }
+    double area() const { return _area; }
+
+    double distance_to_boundary(const Vec2d& point) const;
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+    double distance_to(const Polygon2d& polygon) const;
+    double distance_sqr_to(const Vec2d& point) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    bool is_contain(const Segment2d& segment) const;
+    bool is_contain(const Polygon2d& polygon) const;
+
+    static bool compute_convex_hull(const std::vector<Vec2d>& points, Polygon2d* const polygon);
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool get_overlap(const Segment2d& segment, Vec2d* const first, Vec2d* const last) const;
+    std::vector<Segment2d> get_all_overlaps(const Segment2d& segment) const;
+
+    bool has_overlap(const Polygon2d& polygon) const;
+    // Only compute overlaps between two convex polygons.
+    bool compute_overlap(const Polygon2d& other_polygon, Polygon2d* const overlap_polygon) const;
+
+    AABox2d aa_bounding_box() const;
+    Box2d bounding_box_with_heading(const double heading) const;
+    Box2d min_area_bounding_box() const;
+    void extreme_points(const double heading, Vec2d* const first, Vec2d* const last) const;
+
+    Polygon2d expand_by_distance(const double distance) const;
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    int next(int at) const;
+    int prev(int at) const;
+
+    // Remove point p if and only if outer_prod(segment.start, segment.end, p) < 0.
+    static bool clip_convex_hull(const Segment2d& segment, std::vector<Vec2d>* const points);
+
+    std::vector<Vec2d> _points;
+    int _num_points = 0;
+    std::vector<Segment2d> _segments;
+    bool _is_convex = false;
+    double _area = 0.0;
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+};
+
+}

src/common/math/polygon3d.cpp

@@ -0,0 +1,78 @@
+#include "math/polygon3d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <sstream>
+
+using namespace decision::math;
+
+Polygon3d::Polygon3d(const std::vector<Vec3d>& points)
+    : _points(points)
+{
+    build_from_points();
+}
+
+Polygon2d Polygon3d::polygon2d() const
+{
+    return Polygon2d(_points2d);
+}
+
+inline size_t Polygon3d::next(size_t at) const
+{
+    return at >= _points.size() - 1 ? 0 : at + 1;
+}
+
+inline size_t Polygon3d::prev(size_t at) const
+{
+    return at == 0 ? _points.size() - 1 : at - 1;
+}
+
+void Polygon3d::build_from_points()
+{
+    auto num_points = _points.size();
+    CHECK_GE(num_points, 3u);
+
+    // remove duplicates
+    std::vector<Vec3d> points_tmp(std::move(_points));
+    _points.clear();
+    _points2d.clear();
+    for (size_t i = 0; i < num_points; ++i) {
+        Vec3d vec3d(points_tmp[i].x(), points_tmp[i].y(), points_tmp[i].z());
+        auto pos = std::find(_points.begin(), _points.end(), vec3d);
+        if (pos == _points.end()) {
+            _points2d.emplace_back(vec3d.x(), vec3d.y());
+            _points.emplace_back(points_tmp[i]);
+        }
+    }
+    num_points = _points.size();
+    CHECK_GE(num_points, 3u);
+
+    // Make sure the points are in ccw order.
+    double area = 0.0;
+    for (size_t i = 1; i < num_points; ++i) {
+        area += cross_prod(_points2d[0], _points2d[i - 1], _points2d[i]);
+    }
+    if (area < 0) {
+        area = -area;
+        std::reverse(_points2d.begin(), _points2d.end());
+        std::reverse(_points.begin(), _points.end());
+    }
+    CHECK_GT(area, kMathEpsilon);
+
+    // Construct segments.
+    _segments.reserve(num_points);
+    for (size_t i = 0; i < num_points; ++i) {
+        _segments.emplace_back(_points[i], _points[next(i)]);
+    }
+}
+
+std::string Polygon3d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "polygon3d (  num_points = " << _points.size() << "  points = (";
+    for (const auto& pt : _points) {
+        sout << " " << pt.debug_string();
+    }
+    sout.flush();
+    return sout.str();
+}

src/common/math/polygon3d.h

@@ -0,0 +1,38 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: polygon for 3d point, the polygon is constructed on 2d space, but the point is 3d
+
+#pragma once
+
+#include "math/polygon2d.h"
+#include "math/segment3d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Polygon3d {
+public:
+    Polygon3d() = default;
+    explicit Polygon3d(const std::vector<Vec3d>& points);
+
+    Polygon2d polygon2d() const;
+    const std::vector<Vec3d>& points() const { return _points; }
+    const std::vector<Segment3d>& segments() const { return _segments; }
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    inline size_t next(size_t at) const;
+    inline size_t prev(size_t at) const;
+
+    std::vector<Vec3d> _points;
+    std::vector<Vec2d> _points2d;
+    std::vector<Segment3d> _segments;
+};
+
+}

src/common/math/segment2d.cpp

@@ -0,0 +1,215 @@
+#include "math/segment2d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <cmath>
+#include <sstream>
+
+using namespace decision::math;
+
+namespace {
+
+bool is_within(double val, double bound1, double bound2)
+{
+    if (bound1 > bound2) {
+        std::swap(bound1, bound2);
+    }
+    return val >= bound1 - kMathEpsilon && val <= bound2 + kMathEpsilon;
+}
+
+} // namespace
+
+Segment2d::Segment2d()
+{
+    _unit_direction = Vec2d(1, 0);
+}
+
+Segment2d::Segment2d(const Vec2d& start, const Vec2d& end)
+    : _start(start)
+    , _end(end)
+{
+
+    const double dx = _end.x() - _start.x();
+    const double dy = _end.y() - _start.y();
+    _length = hypot(dx, dy);
+    _unit_direction = (_length <= kMathEpsilon ? Vec2d(0, 0) : Vec2d(dx / _length, dy / _length));
+    _heading = _unit_direction.angle();
+}
+
+double Segment2d::length() const
+{
+    return _length;
+}
+
+double Segment2d::length_sqr() const
+{
+    return _length * _length;
+}
+
+double Segment2d::distance_to(const Vec2d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y();
+    if (proj <= 0.0) {
+        return hypot(x0, y0);
+    }
+    if (proj >= _length) {
+        return point.distance_to(_end);
+    }
+    return std::abs(x0 * _unit_direction.y() - y0 * _unit_direction.x());
+}
+
+double Segment2d::distance_to(const Vec2d& point, Vec2d* const nearest_pt) const
+{
+    CHECK_NOTNULL(nearest_pt);
+    if (_length <= kMathEpsilon) {
+        *nearest_pt = _start;
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y();
+    if (proj < 0.0) {
+        *nearest_pt = _start;
+        return hypot(x0, y0);
+    }
+    if (proj > _length) {
+        *nearest_pt = _end;
+        return point.distance_to(_end);
+    }
+    *nearest_pt = _start + _unit_direction * proj;
+    return std::abs(x0 * _unit_direction.y() - y0 * _unit_direction.x());
+}
+
+double Segment2d::distance_sqr_to(const Vec2d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return point.distance_sqr_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y();
+    if (proj <= 0.0) {
+        return sqr(x0) + sqr(y0);
+    }
+    if (proj >= _length) {
+        return point.distance_sqr_to(_end);
+    }
+    return sqr(x0 * _unit_direction.y() - y0 * _unit_direction.x());
+}
+
+double Segment2d::distance_sqr_to(const Vec2d& point, Vec2d* const nearest_pt) const
+{
+    CHECK_NOTNULL(nearest_pt);
+    if (_length <= kMathEpsilon) {
+        *nearest_pt = _start;
+        return point.distance_sqr_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y();
+    if (proj <= 0.0) {
+        *nearest_pt = _start;
+        return sqr(x0) + sqr(y0);
+    }
+    if (proj >= _length) {
+        *nearest_pt = _end;
+        return point.distance_sqr_to(_end);
+    }
+    *nearest_pt = _start + _unit_direction * proj;
+    return sqr(x0 * _unit_direction.y() - y0 * _unit_direction.x());
+}
+
+bool Segment2d::is_point_in(const Vec2d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return std::abs(point.x() - _start.x()) <= kMathEpsilon && std::abs(point.y() - _start.y()) <= kMathEpsilon;
+    }
+    const double prod = cross_prod(point, _start, _end);
+    if (std::abs(prod) > kMathEpsilon) {
+        return false;
+    }
+    return is_within(point.x(), _start.x(), _end.x()) && is_within(point.y(), _start.y(), _end.y());
+}
+
+double Segment2d::project_onto_unit(const Vec2d& point) const
+{
+    return _unit_direction.inner_prod(point - _start);
+}
+
+double Segment2d::product_onto_unit(const Vec2d& point) const
+{
+    return _unit_direction.cross_prod(point - _start);
+}
+
+bool Segment2d::has_intersect(const Segment2d& other_segment) const
+{
+    Vec2d point;
+    return get_intersect(other_segment, &point);
+}
+
+bool Segment2d::get_intersect(const Segment2d& other_segment, Vec2d* const point) const
+{
+    CHECK_NOTNULL(point);
+    if (is_point_in(other_segment.start())) {
+        *point = other_segment.start();
+        return true;
+    }
+    if (is_point_in(other_segment.end())) {
+        *point = other_segment.end();
+        return true;
+    }
+    if (other_segment.is_point_in(_start)) {
+        *point = _start;
+        return true;
+    }
+    if (other_segment.is_point_in(_end)) {
+        *point = _end;
+        return true;
+    }
+    if (_length <= kMathEpsilon || other_segment.length() <= kMathEpsilon) {
+        return false;
+    }
+    const double cc1 = cross_prod(_start, _end, other_segment.start());
+    const double cc2 = cross_prod(_start, _end, other_segment.end());
+    if (cc1 * cc2 >= -kMathEpsilon) {
+        return false;
+    }
+    const double cc3 = cross_prod(other_segment.start(), other_segment.end(), _start);
+    const double cc4 = cross_prod(other_segment.start(), other_segment.end(), _end);
+    if (cc3 * cc4 >= -kMathEpsilon) {
+        return false;
+    }
+    const double ratio = cc4 / (cc4 - cc3);
+    *point = Vec2d(_start.x() * ratio + _end.x() * (1.0 - ratio),
+        _start.y() * ratio + _end.y() * (1.0 - ratio));
+    return true;
+}
+
+// return distance with perpendicular foot point.
+double Segment2d::get_perpendicular_foot(const Vec2d& point, Vec2d* const foot_point) const
+{
+    CHECK_NOTNULL(foot_point);
+    if (_length <= kMathEpsilon) {
+        *foot_point = _start;
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y();
+    *foot_point = _start + _unit_direction * proj;
+    return std::abs(x0 * _unit_direction.y() - y0 * _unit_direction.x());
+}
+
+std::string Segment2d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "segment2d ( start = " << _start.debug_string()
+         << "  end = " << _end.debug_string() << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/segment2d.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+
+namespace decision::math {
+
+class Segment2d {
+public:
+    Segment2d();
+    Segment2d(const Vec2d& start, const Vec2d& end);
+
+    const Vec2d& start() const { return _start; }
+    const Vec2d& end() const { return _end; }
+    const Vec2d& unit_direction() const { return _unit_direction; }
+    Vec2d center() const { return (_start + _end) / 2.0; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _unit_direction.x(); }
+    double sin_heading() const { return _unit_direction.y(); }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+    double distance_sqr_to(const Vec2d& point) const;
+    double distance_sqr_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+
+    bool is_point_in(const Vec2d& point) const;
+
+    bool has_intersect(const Segment2d& other_segment) const;
+    bool get_intersect(const Segment2d& other_segment, Vec2d* const point) const;
+
+    double project_onto_unit(const Vec2d& point) const;
+    double product_onto_unit(const Vec2d& point) const;
+    double get_perpendicular_foot(const Vec2d& point, Vec2d* const foot_point) const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _start;
+    Vec2d _end;
+    Vec2d _unit_direction;
+    double _heading = 0.0;
+    double _length = 0.0;
+};
+
+}

src/common/math/segment3d.cpp

@@ -0,0 +1,248 @@
+#include "math/segment3d.h"
+#include "math/math_utils.h"
+
+#include <sstream>
+
+using namespace decision::math;
+
+namespace {
+
+bool is_within(double val, double bound1, double bound2)
+{
+    if (bound1 > bound2) {
+        std::swap(bound1, bound2);
+    }
+    return val >= bound1 - kMathEpsilon && val <= bound2 + kMathEpsilon;
+}
+
+} // namespace
+
+Segment3d::Segment3d()
+{
+    _unit_direction = Vec3d(1, 0, 0);
+}
+
+Segment3d::Segment3d(const Vec3d& start, const Vec3d& end)
+    : _start(start)
+    , _end(end)
+{
+
+    const double dx = _end.x() - _start.x();
+    const double dy = _end.y() - _start.y();
+    const double dz = _end.z() - _start.z();
+    _length = hypot(hypot(dx, dy), dz);
+    _unit_direction = (_length <= kMathEpsilon ? Vec3d(0, 0, 0) : Vec3d(dx / _length, dy / _length, dz / _length));
+}
+
+double Segment3d::length() const
+{
+    return _length;
+}
+
+double Segment3d::length_sqr() const
+{
+    return _length * _length;
+}
+
+double Segment3d::distance_to(const Vec3d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double z0 = point.z() - _start.z();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y() + z0 * _unit_direction.z();
+    if (proj <= 0.0) {
+        return hypot(hypot(x0, y0), z0);
+    }
+    if (proj >= _length) {
+        return point.distance_to(_end);
+    }
+    Vec3d vec1 = Vec3d(x0, y0, z0);
+    Vec3d c_p = _unit_direction.cross_prod(vec1);
+    return c_p.length();
+}
+
+double Segment3d::distance_to(const Vec3d& point, Vec3d* const nearest_pt) const
+{
+    CHECK_NOTNULL(nearest_pt);
+    if (_length <= kMathEpsilon) {
+        *nearest_pt = _start;
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double z0 = point.z() - _start.z();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y() + z0 * _unit_direction.z();
+    if (proj < 0.0) {
+        *nearest_pt = _start;
+        return hypot(hypot(x0, y0), z0);
+    }
+    if (proj > _length) {
+        *nearest_pt = _end;
+        return point.distance_to(_end);
+    }
+    *nearest_pt = _start + _unit_direction * proj;
+    Vec3d vec1 = Vec3d(x0, y0, z0);
+    Vec3d c_p = _unit_direction.cross_prod(vec1);
+    return c_p.length();
+}
+
+double Segment3d::distance_sqr_to(const Vec3d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return point.distance_sqr_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double z0 = point.z() - _start.z();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y() + z0 * _unit_direction.z();
+    if (proj <= 0.0) {
+        return sqr(x0) + sqr(y0) + sqr(z0);
+    }
+    if (proj >= _length) {
+        return point.distance_sqr_to(_end);
+    }
+    Vec3d vec1 = Vec3d(x0, y0, z0);
+    Vec3d c_p = _unit_direction.cross_prod(vec1);
+
+    return c_p.length_sqr();
+}
+
+double Segment3d::distance_sqr_to(const Vec3d& point, Vec3d* const nearest_pt) const
+{
+    CHECK_NOTNULL(nearest_pt);
+    if (_length <= kMathEpsilon) {
+        *nearest_pt = _start;
+        return point.distance_sqr_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double z0 = point.z() - _start.z();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y() + z0 * _unit_direction.z();
+    if (proj < 0.0) {
+        *nearest_pt = _start;
+        return sqr(x0) + sqr(y0) + sqr(z0);
+    }
+    if (proj > _length) {
+        *nearest_pt = _end;
+        return point.distance_sqr_to(_end);
+    }
+    *nearest_pt = _start + _unit_direction * proj;
+    Vec3d vec1 = Vec3d(x0, y0, z0);
+    Vec3d c_p = _unit_direction.cross_prod(vec1);
+    return c_p.length_sqr();
+}
+
+bool Segment3d::is_point_in(const Vec3d& point) const
+{
+    if (_length <= kMathEpsilon) {
+        return std::abs(point.x() - _start.x()) <= kMathEpsilon && std::abs(point.y() - _start.y()) <= kMathEpsilon && std::abs(point.z() - _start.z()) <= kMathEpsilon;
+    }
+    const double prod = (_unit_direction.cross_prod(point - _start)).length();
+    if (std::abs(prod) > kMathEpsilon) {
+        return false;
+    }
+    return is_within(point.x(), _start.x(), _end.x()) && is_within(point.y(), _start.y(), _end.y()) && is_within(point.z(), _start.z(), _end.z());
+}
+
+double Segment3d::project_onto_unit(const Vec3d& point) const
+{
+    return _unit_direction.inner_prod(point - _start);
+}
+
+double Segment3d::product_onto_unit(const Vec3d& point) const
+{
+    Vec3d c_p = _unit_direction.cross_prod(point - _start);
+    return c_p.length();
+}
+
+bool Segment3d::has_intersect(const Segment3d& other_segment) const
+{
+    Vec3d point;
+    return get_intersect(other_segment, &point);
+}
+
+bool Segment3d::get_intersect(const Segment3d& other_segment, Vec3d* const point) const
+{
+    CHECK_NOTNULL(point);
+    if (is_point_in(other_segment.start())) {
+        *point = other_segment.start();
+        return true;
+    }
+    if (is_point_in(other_segment.end())) {
+        *point = other_segment.end();
+        return true;
+    }
+    if (other_segment.is_point_in(_start)) {
+        *point = _start;
+        return true;
+    }
+    if (other_segment.is_point_in(_end)) {
+        *point = _end;
+        return true;
+    }
+    if (_length <= kMathEpsilon || other_segment.length() <= kMathEpsilon) {
+        return false;
+    }
+    Vec3d con_perpendicular = _unit_direction.cross_prod(other_segment.unit_direction());
+    Vec3d cross_line = other_segment.end() - _start;
+    const double mix_prod = con_perpendicular.inner_prod(cross_line);
+    // not in common plane
+    if (fabs(mix_prod) > kMathEpsilon) {
+        return false;
+    }
+    Vec3d v1 = cross_prod(_start, _end, other_segment.start());
+    v1.normalize();
+    Vec3d v2 = cross_prod(_start, _end, other_segment.end());
+    v1.normalize();
+    const double cc1 = v1.inner_prod(v2);
+    if (cc1 >= -kMathEpsilon) {
+        return false;
+    }
+    Vec3d v3 = cross_prod(other_segment.start(), other_segment.end(), _start);
+    v3.normalize();
+    Vec3d v4 = cross_prod(other_segment.start(), other_segment.end(), _end);
+    v4.normalize();
+    const double cc2 = v3.inner_prod(v4);
+
+    if (cc2 >= -kMathEpsilon) {
+        return false;
+    }
+    const double ratio = v3.length() / (v3.length() + v4.length());
+    *point = _start * (1 - ratio) + _end * ratio;
+    return true;
+}
+
+// return distance with perpendicular foot point.
+double Segment3d::get_perpendicular_foot(const Vec3d& point, Vec3d* const foot_point) const
+{
+    CHECK_NOTNULL(foot_point);
+    if (_length <= kMathEpsilon) {
+        *foot_point = _start;
+        return point.distance_to(_start);
+    }
+    const double x0 = point.x() - _start.x();
+    const double y0 = point.y() - _start.y();
+    const double z0 = point.z() - _start.z();
+    const double proj = x0 * _unit_direction.x() + y0 * _unit_direction.y() + z0 * _unit_direction.z();
+    *foot_point = _start + _unit_direction * proj;
+    return std::abs(_unit_direction.cross_prod(Vec3d(x0, y0, z0)).length());
+}
+
+Segment2d Segment3d::segment2d() const
+{
+    Vec2d start(_start.x(), _start.y());
+    Vec2d end(_end.x(), _end.y());
+    return Segment2d(start, end);
+}
+
+std::string Segment3d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "Segment3d ( start = " << _start.debug_string()
+         << "  end = " << _end.debug_string() << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/segment3d.h

@@ -0,0 +1,51 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: segment for 3d
+
+#pragma once
+
+#include "math/segment2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Segment3d {
+public:
+    Segment3d();
+    Segment3d(const Vec3d& start, const Vec3d& end);
+
+    const Vec3d& start() const { return _start; }
+    const Vec3d& end() const { return _end; }
+    const Vec3d& unit_direction() const { return _unit_direction; }
+    Vec3d center() const { return (_start + _end) / 2.0; }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec3d& point) const;
+    double distance_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+    double distance_sqr_to(const Vec3d& point) const;
+    double distance_sqr_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+
+    bool is_point_in(const Vec3d& point) const;
+
+    bool has_intersect(const Segment3d& other_segment) const;
+    bool get_intersect(const Segment3d& other_segment, Vec3d* const point) const;
+
+    double project_onto_unit(const Vec3d& point) const;
+    double product_onto_unit(const Vec3d& point) const;
+    double get_perpendicular_foot(const Vec3d& point, Vec3d* const foot_point) const;
+
+    Segment2d segment2d() const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _start;
+    Vec3d _end;
+    Vec3d _unit_direction;
+    double _length = 0.0;
+};
+
+}

src/common/math/sin_table.cpp

@@ -0,0 +1,3287 @@
+// Do NOT modify this file. It was generated by gen_sin_table.py.
+
+#include "math/sin_table.h"
+
+namespace decision::math {
+
+const float SIN_TABLE[16385] = {
+    0.000000000f, 0.000095874f, 0.000191748f, 0.000287621f, 0.000383495f,
+    0.000479369f, 0.000575243f, 0.000671117f, 0.000766990f, 0.000862864f,
+    0.000958738f, 0.001054612f, 0.001150485f, 0.001246359f, 0.001342233f,
+    0.001438106f, 0.001533980f, 0.001629854f, 0.001725728f, 0.001821601f,
+    0.001917475f, 0.002013348f, 0.002109222f, 0.002205096f, 0.002300969f,
+    0.002396843f, 0.002492716f, 0.002588590f, 0.002684463f, 0.002780337f,
+    0.002876210f, 0.002972083f, 0.003067957f, 0.003163830f, 0.003259703f,
+    0.003355577f, 0.003451450f, 0.003547323f, 0.003643196f, 0.003739069f,
+    0.003834943f, 0.003930816f, 0.004026689f, 0.004122562f, 0.004218435f,
+    0.004314308f, 0.004410180f, 0.004506053f, 0.004601926f, 0.004697799f,
+    0.004793672f, 0.004889544f, 0.004985417f, 0.005081289f, 0.005177162f,
+    0.005273035f, 0.005368907f, 0.005464779f, 0.005560652f, 0.005656524f,
+    0.005752396f, 0.005848268f, 0.005944141f, 0.006040013f, 0.006135885f,
+    0.006231757f, 0.006327629f, 0.006423500f, 0.006519372f, 0.006615244f,
+    0.006711116f, 0.006806987f, 0.006902859f, 0.006998730f, 0.007094602f,
+    0.007190473f, 0.007286344f, 0.007382215f, 0.007478087f, 0.007573958f,
+    0.007669829f, 0.007765700f, 0.007861571f, 0.007957441f, 0.008053312f,
+    0.008149183f, 0.008245053f, 0.008340924f, 0.008436794f, 0.008532665f,
+    0.008628535f, 0.008724405f, 0.008820275f, 0.008916145f, 0.009012015f,
+    0.009107885f, 0.009203755f, 0.009299624f, 0.009395494f, 0.009491364f,
+    0.009587233f, 0.009683102f, 0.009778972f, 0.009874841f, 0.009970710f,
+    0.010066579f, 0.010162448f, 0.010258317f, 0.010354185f, 0.010450054f,
+    0.010545922f, 0.010641791f, 0.010737659f, 0.010833527f, 0.010929396f,
+    0.011025264f, 0.011121131f, 0.011216999f, 0.011312867f, 0.011408735f,
+    0.011504602f, 0.011600470f, 0.011696337f, 0.011792204f, 0.011888071f,
+    0.011983938f, 0.012079805f, 0.012175672f, 0.012271538f, 0.012367405f,
+    0.012463271f, 0.012559138f, 0.012655004f, 0.012750870f, 0.012846736f,
+    0.012942602f, 0.013038467f, 0.013134333f, 0.013230198f, 0.013326064f,
+    0.013421929f, 0.013517794f, 0.013613659f, 0.013709524f, 0.013805389f,
+    0.013901253f, 0.013997118f, 0.014092982f, 0.014188846f, 0.014284710f,
+    0.014380574f, 0.014476438f, 0.014572302f, 0.014668165f, 0.014764029f,
+    0.014859892f, 0.014955755f, 0.015051618f, 0.015147481f, 0.015243344f,
+    0.015339206f, 0.015435069f, 0.015530931f, 0.015626793f, 0.015722655f,
+    0.015818517f, 0.015914379f, 0.016010240f, 0.016106102f, 0.016201963f,
+    0.016297824f, 0.016393685f, 0.016489546f, 0.016585407f, 0.016681267f,
+    0.016777128f, 0.016872988f, 0.016968848f, 0.017064708f, 0.017160568f,
+    0.017256427f, 0.017352287f, 0.017448146f, 0.017544005f, 0.017639864f,
+    0.017735723f, 0.017831582f, 0.017927440f, 0.018023298f, 0.018119156f,
+    0.018215014f, 0.018310872f, 0.018406730f, 0.018502587f, 0.018598445f,
+    0.018694302f, 0.018790159f, 0.018886016f, 0.018981872f, 0.019077729f,
+    0.019173585f, 0.019269441f, 0.019365297f, 0.019461153f, 0.019557008f,
+    0.019652864f, 0.019748719f, 0.019844574f, 0.019940429f, 0.020036283f,
+    0.020132138f, 0.020227992f, 0.020323846f, 0.020419700f, 0.020515554f,
+    0.020611407f, 0.020707261f, 0.020803114f, 0.020898967f, 0.020994819f,
+    0.021090672f, 0.021186524f, 0.021282376f, 0.021378228f, 0.021474080f,
+    0.021569932f, 0.021665783f, 0.021761634f, 0.021857485f, 0.021953336f,
+    0.022049187f, 0.022145037f, 0.022240887f, 0.022336737f, 0.022432587f,
+    0.022528437f, 0.022624286f, 0.022720135f, 0.022815984f, 0.022911833f,
+    0.023007681f, 0.023103530f, 0.023199378f, 0.023295226f, 0.023391073f,
+    0.023486921f, 0.023582768f, 0.023678615f, 0.023774462f, 0.023870309f,
+    0.023966155f, 0.024062001f, 0.024157847f, 0.024253693f, 0.024349538f,
+    0.024445383f, 0.024541229f, 0.024637073f, 0.024732918f, 0.024828762f,
+    0.024924606f, 0.025020450f, 0.025116294f, 0.025212137f, 0.025307981f,
+    0.025403824f, 0.025499666f, 0.025595509f, 0.025691351f, 0.025787193f,
+    0.025883035f, 0.025978877f, 0.026074718f, 0.026170559f, 0.026266400f,
+    0.026362240f, 0.026458081f, 0.026553921f, 0.026649761f, 0.026745600f,
+    0.026841440f, 0.026937279f, 0.027033118f, 0.027128956f, 0.027224795f,
+    0.027320633f, 0.027416471f, 0.027512308f, 0.027608146f, 0.027703983f,
+    0.027799820f, 0.027895656f, 0.027991493f, 0.028087329f, 0.028183165f,
+    0.028279000f, 0.028374836f, 0.028470671f, 0.028566505f, 0.028662340f,
+    0.028758174f, 0.028854008f, 0.028949842f, 0.029045676f, 0.029141509f,
+    0.029237342f, 0.029333174f, 0.029429007f, 0.029524839f, 0.029620671f,
+    0.029716502f, 0.029812334f, 0.029908165f, 0.030003996f, 0.030099826f,
+    0.030195656f, 0.030291486f, 0.030387316f, 0.030483145f, 0.030578974f,
+    0.030674803f, 0.030770632f, 0.030866460f, 0.030962288f, 0.031058116f,
+    0.031153943f, 0.031249770f, 0.031345597f, 0.031441424f, 0.031537250f,
+    0.031633076f, 0.031728901f, 0.031824727f, 0.031920552f, 0.032016377f,
+    0.032112201f, 0.032208025f, 0.032303849f, 0.032399673f, 0.032495496f,
+    0.032591319f, 0.032687142f, 0.032782964f, 0.032878787f, 0.032974608f,
+    0.033070430f, 0.033166251f, 0.033262072f, 0.033357893f, 0.033453713f,
+    0.033549533f, 0.033645352f, 0.033741172f, 0.033836991f, 0.033932810f,
+    0.034028628f, 0.034124446f, 0.034220264f, 0.034316081f, 0.034411899f,
+    0.034507716f, 0.034603532f, 0.034699348f, 0.034795164f, 0.034890980f,
+    0.034986795f, 0.035082610f, 0.035178425f, 0.035274239f, 0.035370053f,
+    0.035465867f, 0.035561680f, 0.035657493f, 0.035753305f, 0.035849118f,
+    0.035944930f, 0.036040742f, 0.036136553f, 0.036232364f, 0.036328175f,
+    0.036423985f, 0.036519795f, 0.036615605f, 0.036711414f, 0.036807223f,
+    0.036903032f, 0.036998840f, 0.037094648f, 0.037190456f, 0.037286263f,
+    0.037382070f, 0.037477876f, 0.037573683f, 0.037669489f, 0.037765294f,
+    0.037861099f, 0.037956904f, 0.038052709f, 0.038148513f, 0.038244317f,
+    0.038340120f, 0.038435924f, 0.038531726f, 0.038627529f, 0.038723331f,
+    0.038819132f, 0.038914934f, 0.039010735f, 0.039106535f, 0.039202336f,
+    0.039298136f, 0.039393935f, 0.039489734f, 0.039585533f, 0.039681332f,
+    0.039777130f, 0.039872928f, 0.039968725f, 0.040064522f, 0.040160319f,
+    0.040256115f, 0.040351911f, 0.040447706f, 0.040543501f, 0.040639296f,
+    0.040735091f, 0.040830885f, 0.040926678f, 0.041022472f, 0.041118265f,
+    0.041214057f, 0.041309849f, 0.041405641f, 0.041501432f, 0.041597223f,
+    0.041693014f, 0.041788804f, 0.041884594f, 0.041980384f, 0.042076173f,
+    0.042171961f, 0.042267750f, 0.042363538f, 0.042459325f, 0.042555112f,
+    0.042650899f, 0.042746685f, 0.042842471f, 0.042938257f, 0.043034042f,
+    0.043129827f, 0.043225611f, 0.043321395f, 0.043417179f, 0.043512962f,
+    0.043608745f, 0.043704527f, 0.043800309f, 0.043896091f, 0.043991872f,
+    0.044087653f, 0.044183433f, 0.044279213f, 0.044374993f, 0.044470772f,
+    0.044566551f, 0.044662329f, 0.044758107f, 0.044853884f, 0.044949661f,
+    0.045045438f, 0.045141214f, 0.045236990f, 0.045332766f, 0.045428541f,
+    0.045524315f, 0.045620090f, 0.045715863f, 0.045811637f, 0.045907410f,
+    0.046003182f, 0.046098954f, 0.046194726f, 0.046290497f, 0.046386268f,
+    0.046482038f, 0.046577808f, 0.046673578f, 0.046769347f, 0.046865116f,
+    0.046960884f, 0.047056652f, 0.047152419f, 0.047248186f, 0.047343952f,
+    0.047439719f, 0.047535484f, 0.047631249f, 0.047727014f, 0.047822778f,
+    0.047918542f, 0.048014306f, 0.048110069f, 0.048205831f, 0.048301593f,
+    0.048397355f, 0.048493116f, 0.048588877f, 0.048684637f, 0.048780397f,
+    0.048876157f, 0.048971916f, 0.049067674f, 0.049163432f, 0.049259190f,
+    0.049354947f, 0.049450704f, 0.049546460f, 0.049642216f, 0.049737971f,
+    0.049833726f, 0.049929481f, 0.050025235f, 0.050120988f, 0.050216741f,
+    0.050312494f, 0.050408246f, 0.050503998f, 0.050599749f, 0.050695500f,
+    0.050791250f, 0.050887000f, 0.050982749f, 0.051078498f, 0.051174247f,
+    0.051269994f, 0.051365742f, 0.051461489f, 0.051557235f, 0.051652982f,
+    0.051748727f, 0.051844472f, 0.051940217f, 0.052035961f, 0.052131705f,
+    0.052227448f, 0.052323191f, 0.052418933f, 0.052514675f, 0.052610416f,
+    0.052706157f, 0.052801897f, 0.052897637f, 0.052993376f, 0.053089115f,
+    0.053184853f, 0.053280591f, 0.053376328f, 0.053472065f, 0.053567802f,
+    0.053663538f, 0.053759273f, 0.053855008f, 0.053950742f, 0.054046476f,
+    0.054142210f, 0.054237943f, 0.054333675f, 0.054429407f, 0.054525138f,
+    0.054620869f, 0.054716600f, 0.054812330f, 0.054908059f, 0.055003788f,
+    0.055099516f, 0.055195244f, 0.055290972f, 0.055386699f, 0.055482425f,
+    0.055578151f, 0.055673876f, 0.055769601f, 0.055865325f, 0.055961049f,
+    0.056056773f, 0.056152495f, 0.056248218f, 0.056343939f, 0.056439661f,
+    0.056535381f, 0.056631101f, 0.056726821f, 0.056822540f, 0.056918259f,
+    0.057013977f, 0.057109695f, 0.057205412f, 0.057301128f, 0.057396844f,
+    0.057492560f, 0.057588275f, 0.057683989f, 0.057779703f, 0.057875416f,
+    0.057971129f, 0.058066842f, 0.058162553f, 0.058258265f, 0.058353975f,
+    0.058449685f, 0.058545395f, 0.058641104f, 0.058736813f, 0.058832521f,
+    0.058928228f, 0.059023935f, 0.059119641f, 0.059215347f, 0.059311052f,
+    0.059406757f, 0.059502461f, 0.059598165f, 0.059693868f, 0.059789571f,
+    0.059885273f, 0.059980974f, 0.060076675f, 0.060172375f, 0.060268075f,
+    0.060363775f, 0.060459473f, 0.060555171f, 0.060650869f, 0.060746566f,
+    0.060842262f, 0.060937958f, 0.061033654f, 0.061129348f, 0.061225043f,
+    0.061320736f, 0.061416429f, 0.061512122f, 0.061607814f, 0.061703505f,
+    0.061799196f, 0.061894886f, 0.061990576f, 0.062086265f, 0.062181954f,
+    0.062277642f, 0.062373329f, 0.062469016f, 0.062564702f, 0.062660388f,
+    0.062756073f, 0.062851758f, 0.062947442f, 0.063043125f, 0.063138808f,
+    0.063234490f, 0.063330172f, 0.063425853f, 0.063521533f, 0.063617213f,
+    0.063712892f, 0.063808571f, 0.063904249f, 0.063999927f, 0.064095604f,
+    0.064191280f, 0.064286956f, 0.064382631f, 0.064478306f, 0.064573980f,
+    0.064669653f, 0.064765326f, 0.064860998f, 0.064956670f, 0.065052341f,
+    0.065148011f, 0.065243681f, 0.065339350f, 0.065435019f, 0.065530687f,
+    0.065626354f, 0.065722021f, 0.065817687f, 0.065913353f, 0.066009018f,
+    0.066104682f, 0.066200346f, 0.066296009f, 0.066391672f, 0.066487334f,
+    0.066582995f, 0.066678656f, 0.066774316f, 0.066869975f, 0.066965634f,
+    0.067061293f, 0.067156950f, 0.067252607f, 0.067348264f, 0.067443920f,
+    0.067539575f, 0.067635229f, 0.067730883f, 0.067826537f, 0.067922189f,
+    0.068017841f, 0.068113493f, 0.068209144f, 0.068304794f, 0.068400443f,
+    0.068496092f, 0.068591741f, 0.068687388f, 0.068783035f, 0.068878682f,
+    0.068974328f, 0.069069973f, 0.069165617f, 0.069261261f, 0.069356904f,
+    0.069452547f, 0.069548189f, 0.069643830f, 0.069739471f, 0.069835111f,
+    0.069930750f, 0.070026389f, 0.070122027f, 0.070217665f, 0.070313302f,
+    0.070408938f, 0.070504573f, 0.070600208f, 0.070695843f, 0.070791476f,
+    0.070887109f, 0.070982741f, 0.071078373f, 0.071174004f, 0.071269634f,
+    0.071365264f, 0.071460893f, 0.071556521f, 0.071652149f, 0.071747776f,
+    0.071843402f, 0.071939028f, 0.072034653f, 0.072130278f, 0.072225901f,
+    0.072321524f, 0.072417147f, 0.072512769f, 0.072608390f, 0.072704010f,
+    0.072799630f, 0.072895249f, 0.072990867f, 0.073086485f, 0.073182102f,
+    0.073277718f, 0.073373334f, 0.073468949f, 0.073564564f, 0.073660177f,
+    0.073755790f, 0.073851403f, 0.073947014f, 0.074042625f, 0.074138236f,
+    0.074233845f, 0.074329454f, 0.074425062f, 0.074520670f, 0.074616277f,
+    0.074711883f, 0.074807488f, 0.074903093f, 0.074998697f, 0.075094301f,
+    0.075189904f, 0.075285506f, 0.075381107f, 0.075476708f, 0.075572308f,
+    0.075667907f, 0.075763506f, 0.075859103f, 0.075954701f, 0.076050297f,
+    0.076145893f, 0.076241488f, 0.076337082f, 0.076432676f, 0.076528269f,
+    0.076623861f, 0.076719453f, 0.076815044f, 0.076910634f, 0.077006223f,
+    0.077101812f, 0.077197400f, 0.077292988f, 0.077388574f, 0.077484160f,
+    0.077579745f, 0.077675330f, 0.077770914f, 0.077866497f, 0.077962079f,
+    0.078057661f, 0.078153242f, 0.078248822f, 0.078344401f, 0.078439980f,
+    0.078535558f, 0.078631135f, 0.078726712f, 0.078822288f, 0.078917863f,
+    0.079013437f, 0.079109011f, 0.079204584f, 0.079300156f, 0.079395728f,
+    0.079491299f, 0.079586869f, 0.079682438f, 0.079778007f, 0.079873574f,
+    0.079969142f, 0.080064708f, 0.080160274f, 0.080255838f, 0.080351403f,
+    0.080446966f, 0.080542529f, 0.080638091f, 0.080733652f, 0.080829212f,
+    0.080924772f, 0.081020331f, 0.081115889f, 0.081211447f, 0.081307004f,
+    0.081402560f, 0.081498115f, 0.081593669f, 0.081689223f, 0.081784776f,
+    0.081880328f, 0.081975880f, 0.082071431f, 0.082166981f, 0.082262530f,
+    0.082358078f, 0.082453626f, 0.082549173f, 0.082644719f, 0.082740265f,
+    0.082835809f, 0.082931353f, 0.083026896f, 0.083122439f, 0.083217980f,
+    0.083313521f, 0.083409061f, 0.083504601f, 0.083600139f, 0.083695677f,
+    0.083791214f, 0.083886750f, 0.083982286f, 0.084077820f, 0.084173354f,
+    0.084268888f, 0.084364420f, 0.084459952f, 0.084555482f, 0.084651013f,
+    0.084746542f, 0.084842070f, 0.084937598f, 0.085033125f, 0.085128651f,
+    0.085224177f, 0.085319701f, 0.085415225f, 0.085510748f, 0.085606270f,
+    0.085701792f, 0.085797312f, 0.085892832f, 0.085988351f, 0.086083870f,
+    0.086179387f, 0.086274904f, 0.086370420f, 0.086465935f, 0.086561449f,
+    0.086656963f, 0.086752476f, 0.086847987f, 0.086943499f, 0.087039009f,
+    0.087134519f, 0.087230027f, 0.087325535f, 0.087421042f, 0.087516549f,
+    0.087612054f, 0.087707559f, 0.087803063f, 0.087898566f, 0.087994068f,
+    0.088089570f, 0.088185070f, 0.088280570f, 0.088376069f, 0.088471568f,
+    0.088567065f, 0.088662562f, 0.088758058f, 0.088853553f, 0.088949047f,
+    0.089044540f, 0.089140033f, 0.089235524f, 0.089331015f, 0.089426505f,
+    0.089521995f, 0.089617483f, 0.089712971f, 0.089808457f, 0.089903943f,
+    0.089999429f, 0.090094913f, 0.090190396f, 0.090285879f, 0.090381361f,
+    0.090476842f, 0.090572322f, 0.090667801f, 0.090763280f, 0.090858758f,
+    0.090954234f, 0.091049710f, 0.091145185f, 0.091240660f, 0.091336133f,
+    0.091431606f, 0.091527078f, 0.091622549f, 0.091718019f, 0.091813488f,
+    0.091908956f, 0.092004424f, 0.092099891f, 0.092195357f, 0.092290822f,
+    0.092386286f, 0.092481749f, 0.092577212f, 0.092672673f, 0.092768134f,
+    0.092863594f, 0.092959053f, 0.093054511f, 0.093149969f, 0.093245425f,
+    0.093340881f, 0.093436336f, 0.093531790f, 0.093627243f, 0.093722695f,
+    0.093818146f, 0.093913597f, 0.094009047f, 0.094104495f, 0.094199943f,
+    0.094295390f, 0.094390837f, 0.094486282f, 0.094581726f, 0.094677170f,
+    0.094772613f, 0.094868054f, 0.094963495f, 0.095058935f, 0.095154375f,
+    0.095249813f, 0.095345250f, 0.095440687f, 0.095536123f, 0.095631558f,
+    0.095726991f, 0.095822425f, 0.095917857f, 0.096013288f, 0.096108718f,
+    0.096204148f, 0.096299577f, 0.096395004f, 0.096490431f, 0.096585857f,
+    0.096681282f, 0.096776707f, 0.096872130f, 0.096967552f, 0.097062974f,
+    0.097158395f, 0.097253814f, 0.097349233f, 0.097444651f, 0.097540068f,
+    0.097635485f, 0.097730900f, 0.097826314f, 0.097921728f, 0.098017140f,
+    0.098112552f, 0.098207963f, 0.098303373f, 0.098398782f, 0.098494190f,
+    0.098589597f, 0.098685003f, 0.098780409f, 0.098875813f, 0.098971217f,
+    0.099066619f, 0.099162021f, 0.099257422f, 0.099352822f, 0.099448221f,
+    0.099543619f, 0.099639016f, 0.099734412f, 0.099829807f, 0.099925202f,
+    0.100020595f, 0.100115988f, 0.100211379f, 0.100306770f, 0.100402160f,
+    0.100497549f, 0.100592937f, 0.100688324f, 0.100783710f, 0.100879095f,
+    0.100974479f, 0.101069863f, 0.101165245f, 0.101260627f, 0.101356007f,
+    0.101451387f, 0.101546765f, 0.101642143f, 0.101737520f, 0.101832896f,
+    0.101928271f, 0.102023645f, 0.102119018f, 0.102214390f, 0.102309761f,
+    0.102405131f, 0.102500501f, 0.102595869f, 0.102691236f, 0.102786603f,
+    0.102881968f, 0.102977333f, 0.103072697f, 0.103168059f, 0.103263421f,
+    0.103358782f, 0.103454142f, 0.103549501f, 0.103644859f, 0.103740215f,
+    0.103835572f, 0.103930927f, 0.104026281f, 0.104121634f, 0.104216986f,
+    0.104312337f, 0.104407688f, 0.104503037f, 0.104598385f, 0.104693733f,
+    0.104789079f, 0.104884425f, 0.104979769f, 0.105075113f, 0.105170455f,
+    0.105265797f, 0.105361138f, 0.105456477f, 0.105551816f, 0.105647154f,
+    0.105742490f, 0.105837826f, 0.105933161f, 0.106028495f, 0.106123828f,
+    0.106219160f, 0.106314491f, 0.106409821f, 0.106505150f, 0.106600478f,
+    0.106695805f, 0.106791131f, 0.106886456f, 0.106981780f, 0.107077103f,
+    0.107172425f, 0.107267746f, 0.107363066f, 0.107458385f, 0.107553704f,
+    0.107649021f, 0.107744337f, 0.107839652f, 0.107934966f, 0.108030279f,
+    0.108125592f, 0.108220903f, 0.108316213f, 0.108411522f, 0.108506831f,
+    0.108602138f, 0.108697444f, 0.108792749f, 0.108888053f, 0.108983357f,
+    0.109078659f, 0.109173960f, 0.109269260f, 0.109364560f, 0.109459858f,
+    0.109555155f, 0.109650451f, 0.109745746f, 0.109841041f, 0.109936334f,
+    0.110031626f, 0.110126917f, 0.110222207f, 0.110317496f, 0.110412785f,
+    0.110508072f, 0.110603358f, 0.110698643f, 0.110793927f, 0.110889210f,
+    0.110984492f, 0.111079773f, 0.111175053f, 0.111270332f, 0.111365610f,
+    0.111460887f, 0.111556163f, 0.111651437f, 0.111746711f, 0.111841984f,
+    0.111937256f, 0.112032527f, 0.112127796f, 0.112223065f, 0.112318333f,
+    0.112413599f, 0.112508865f, 0.112604129f, 0.112699393f, 0.112794655f,
+    0.112889917f, 0.112985177f, 0.113080437f, 0.113175695f, 0.113270952f,
+    0.113366208f, 0.113461464f, 0.113556718f, 0.113651971f, 0.113747223f,
+    0.113842474f, 0.113937724f, 0.114032973f, 0.114128221f, 0.114223468f,
+    0.114318713f, 0.114413958f, 0.114509202f, 0.114604445f, 0.114699686f,
+    0.114794927f, 0.114890166f, 0.114985404f, 0.115080642f, 0.115175878f,
+    0.115271113f, 0.115366348f, 0.115461581f, 0.115556813f, 0.115652044f,
+    0.115747274f, 0.115842503f, 0.115937730f, 0.116032957f, 0.116128183f,
+    0.116223407f, 0.116318631f, 0.116413853f, 0.116509075f, 0.116604295f,
+    0.116699514f, 0.116794733f, 0.116889950f, 0.116985166f, 0.117080381f,
+    0.117175595f, 0.117270807f, 0.117366019f, 0.117461230f, 0.117556439f,
+    0.117651648f, 0.117746855f, 0.117842062f, 0.117937267f, 0.118032471f,
+    0.118127674f, 0.118222876f, 0.118318077f, 0.118413277f, 0.118508475f,
+    0.118603673f, 0.118698870f, 0.118794065f, 0.118889259f, 0.118984453f,
+    0.119079645f, 0.119174836f, 0.119270026f, 0.119365215f, 0.119460403f,
+    0.119555589f, 0.119650775f, 0.119745959f, 0.119841143f, 0.119936325f,
+    0.120031506f, 0.120126686f, 0.120221865f, 0.120317043f, 0.120412220f,
+    0.120507396f, 0.120602570f, 0.120697744f, 0.120792916f, 0.120888087f,
+    0.120983257f, 0.121078426f, 0.121173594f, 0.121268761f, 0.121363927f,
+    0.121459091f, 0.121554255f, 0.121649417f, 0.121744578f, 0.121839738f,
+    0.121934897f, 0.122030055f, 0.122125212f, 0.122220367f, 0.122315522f,
+    0.122410675f, 0.122505827f, 0.122600979f, 0.122696128f, 0.122791277f,
+    0.122886425f, 0.122981572f, 0.123076717f, 0.123171861f, 0.123267005f,
+    0.123362147f, 0.123457288f, 0.123552427f, 0.123647566f, 0.123742704f,
+    0.123837840f, 0.123932975f, 0.124028109f, 0.124123242f, 0.124218374f,
+    0.124313505f, 0.124408634f, 0.124503763f, 0.124598890f, 0.124694016f,
+    0.124789141f, 0.124884265f, 0.124979387f, 0.125074509f, 0.125169629f,
+    0.125264748f, 0.125359867f, 0.125454983f, 0.125550099f, 0.125645214f,
+    0.125740327f, 0.125835439f, 0.125930551f, 0.126025661f, 0.126120769f,
+    0.126215877f, 0.126310984f, 0.126406089f, 0.126501193f, 0.126596296f,
+    0.126691398f, 0.126786499f, 0.126881598f, 0.126976696f, 0.127071794f,
+    0.127166890f, 0.127261985f, 0.127357078f, 0.127452171f, 0.127547262f,
+    0.127642352f, 0.127737441f, 0.127832529f, 0.127927616f, 0.128022701f,
+    0.128117785f, 0.128212869f, 0.128307950f, 0.128403031f, 0.128498111f,
+    0.128593189f, 0.128688266f, 0.128783342f, 0.128878417f, 0.128973491f,
+    0.129068563f, 0.129163635f, 0.129258705f, 0.129353774f, 0.129448841f,
+    0.129543908f, 0.129638973f, 0.129734037f, 0.129829100f, 0.129924162f,
+    0.130019223f, 0.130114282f, 0.130209340f, 0.130304397f, 0.130399453f,
+    0.130494508f, 0.130589561f, 0.130684613f, 0.130779664f, 0.130874714f,
+    0.130969763f, 0.131064810f, 0.131159856f, 0.131254901f, 0.131349945f,
+    0.131444987f, 0.131540029f, 0.131635069f, 0.131730108f, 0.131825145f,
+    0.131920182f, 0.132015217f, 0.132110251f, 0.132205284f, 0.132300316f,
+    0.132395346f, 0.132490375f, 0.132585403f, 0.132680430f, 0.132775456f,
+    0.132870480f, 0.132965503f, 0.133060525f, 0.133155546f, 0.133250565f,
+    0.133345583f, 0.133440600f, 0.133535616f, 0.133630631f, 0.133725644f,
+    0.133820656f, 0.133915667f, 0.134010677f, 0.134105685f, 0.134200692f,
+    0.134295698f, 0.134390703f, 0.134485706f, 0.134580709f, 0.134675709f,
+    0.134770709f, 0.134865708f, 0.134960705f, 0.135055701f, 0.135150696f,
+    0.135245689f, 0.135340682f, 0.135435673f, 0.135530663f, 0.135625651f,
+    0.135720638f, 0.135815624f, 0.135910609f, 0.136005593f, 0.136100575f,
+    0.136195556f, 0.136290536f, 0.136385515f, 0.136480492f, 0.136575468f,
+    0.136670443f, 0.136765416f, 0.136860389f, 0.136955360f, 0.137050329f,
+    0.137145298f, 0.137240265f, 0.137335231f, 0.137430196f, 0.137525159f,
+    0.137620122f, 0.137715083f, 0.137810042f, 0.137905001f, 0.137999958f,
+    0.138094914f, 0.138189868f, 0.138284822f, 0.138379774f, 0.138474724f,
+    0.138569674f, 0.138664622f, 0.138759569f, 0.138854515f, 0.138949459f,
+    0.139044402f, 0.139139344f, 0.139234285f, 0.139329224f, 0.139424162f,
+    0.139519099f, 0.139614034f, 0.139708968f, 0.139803901f, 0.139898833f,
+    0.139993763f, 0.140088692f, 0.140183620f, 0.140278546f, 0.140373472f,
+    0.140468395f, 0.140563318f, 0.140658239f, 0.140753159f, 0.140848078f,
+    0.140942995f, 0.141037912f, 0.141132826f, 0.141227740f, 0.141322652f,
+    0.141417563f, 0.141512473f, 0.141607381f, 0.141702288f, 0.141797194f,
+    0.141892098f, 0.141987001f, 0.142081903f, 0.142176804f, 0.142271703f,
+    0.142366601f, 0.142461497f, 0.142556392f, 0.142651286f, 0.142746179f,
+    0.142841070f, 0.142935960f, 0.143030849f, 0.143125736f, 0.143220623f,
+    0.143315507f, 0.143410391f, 0.143505273f, 0.143600154f, 0.143695033f,
+    0.143789911f, 0.143884788f, 0.143979664f, 0.144074538f, 0.144169411f,
+    0.144264282f, 0.144359152f, 0.144454021f, 0.144548889f, 0.144643755f,
+    0.144738620f, 0.144833484f, 0.144928346f, 0.145023207f, 0.145118066f,
+    0.145212925f, 0.145307782f, 0.145402637f, 0.145497491f, 0.145592344f,
+    0.145687196f, 0.145782046f, 0.145876895f, 0.145971742f, 0.146066589f,
+    0.146161434f, 0.146256277f, 0.146351119f, 0.146445960f, 0.146540800f,
+    0.146635638f, 0.146730474f, 0.146825310f, 0.146920144f, 0.147014977f,
+    0.147109808f, 0.147204638f, 0.147299467f, 0.147394294f, 0.147489120f,
+    0.147583945f, 0.147678768f, 0.147773590f, 0.147868410f, 0.147963230f,
+    0.148058047f, 0.148152864f, 0.148247679f, 0.148342493f, 0.148437305f,
+    0.148532116f, 0.148626926f, 0.148721734f, 0.148816541f, 0.148911346f,
+    0.149006151f, 0.149100953f, 0.149195755f, 0.149290555f, 0.149385354f,
+    0.149480151f, 0.149574947f, 0.149669741f, 0.149764535f, 0.149859326f,
+    0.149954117f, 0.150048906f, 0.150143694f, 0.150238480f, 0.150333265f,
+    0.150428048f, 0.150522831f, 0.150617611f, 0.150712391f, 0.150807169f,
+    0.150901945f, 0.150996721f, 0.151091494f, 0.151186267f, 0.151281038f,
+    0.151375808f, 0.151470576f, 0.151565343f, 0.151660108f, 0.151754872f,
+    0.151849635f, 0.151944396f, 0.152039156f, 0.152133915f, 0.152228672f,
+    0.152323428f, 0.152418182f, 0.152512935f, 0.152607686f, 0.152702437f,
+    0.152797185f, 0.152891933f, 0.152986678f, 0.153081423f, 0.153176166f,
+    0.153270908f, 0.153365648f, 0.153460387f, 0.153555124f, 0.153649860f,
+    0.153744595f, 0.153839328f, 0.153934060f, 0.154028790f, 0.154123519f,
+    0.154218247f, 0.154312973f, 0.154407698f, 0.154502421f, 0.154597143f,
+    0.154691863f, 0.154786582f, 0.154881300f, 0.154976016f, 0.155070731f,
+    0.155165444f, 0.155260156f, 0.155354867f, 0.155449576f, 0.155544283f,
+    0.155638990f, 0.155733694f, 0.155828398f, 0.155923100f, 0.156017800f,
+    0.156112499f, 0.156207197f, 0.156301893f, 0.156396588f, 0.156491281f,
+    0.156585973f, 0.156680663f, 0.156775352f, 0.156870040f, 0.156964726f,
+    0.157059410f, 0.157154094f, 0.157248775f, 0.157343456f, 0.157438134f,
+    0.157532812f, 0.157627488f, 0.157722162f, 0.157816835f, 0.157911507f,
+    0.158006177f, 0.158100846f, 0.158195513f, 0.158290179f, 0.158384843f,
+    0.158479506f, 0.158574168f, 0.158668828f, 0.158763486f, 0.158858143f,
+    0.158952799f, 0.159047453f, 0.159142106f, 0.159236757f, 0.159331407f,
+    0.159426055f, 0.159520702f, 0.159615347f, 0.159709991f, 0.159804634f,
+    0.159899275f, 0.159993914f, 0.160088552f, 0.160183189f, 0.160277824f,
+    0.160372457f, 0.160467089f, 0.160561720f, 0.160656349f, 0.160750977f,
+    0.160845603f, 0.160940228f, 0.161034851f, 0.161129473f, 0.161224093f,
+    0.161318712f, 0.161413329f, 0.161507945f, 0.161602560f, 0.161697172f,
+    0.161791784f, 0.161886394f, 0.161981002f, 0.162075609f, 0.162170215f,
+    0.162264819f, 0.162359421f, 0.162454022f, 0.162548621f, 0.162643219f,
+    0.162737816f, 0.162832411f, 0.162927004f, 0.163021596f, 0.163116187f,
+    0.163210776f, 0.163305363f, 0.163399949f, 0.163494534f, 0.163589117f,
+    0.163683698f, 0.163778278f, 0.163872857f, 0.163967434f, 0.164062009f,
+    0.164156583f, 0.164251156f, 0.164345727f, 0.164440296f, 0.164534864f,
+    0.164629430f, 0.164723995f, 0.164818559f, 0.164913120f, 0.165007681f,
+    0.165102240f, 0.165196797f, 0.165291353f, 0.165385907f, 0.165480460f,
+    0.165575011f, 0.165669561f, 0.165764109f, 0.165858656f, 0.165953201f,
+    0.166047744f, 0.166142286f, 0.166236827f, 0.166331366f, 0.166425904f,
+    0.166520440f, 0.166614974f, 0.166709507f, 0.166804038f, 0.166898568f,
+    0.166993096f, 0.167087623f, 0.167182148f, 0.167276672f, 0.167371194f,
+    0.167465715f, 0.167560234f, 0.167654752f, 0.167749268f, 0.167843782f,
+    0.167938295f, 0.168032806f, 0.168127316f, 0.168221824f, 0.168316331f,
+    0.168410836f, 0.168505340f, 0.168599842f, 0.168694343f, 0.168788842f,
+    0.168883339f, 0.168977835f, 0.169072329f, 0.169166822f, 0.169261313f,
+    0.169355803f, 0.169450291f, 0.169544778f, 0.169639263f, 0.169733746f,
+    0.169828228f, 0.169922708f, 0.170017187f, 0.170111664f, 0.170206140f,
+    0.170300614f, 0.170395087f, 0.170489558f, 0.170584027f, 0.170678495f,
+    0.170772961f, 0.170867426f, 0.170961889f, 0.171056350f, 0.171150810f,
+    0.171245269f, 0.171339725f, 0.171434181f, 0.171528634f, 0.171623086f,
+    0.171717537f, 0.171811986f, 0.171906433f, 0.172000879f, 0.172095323f,
+    0.172189766f, 0.172284207f, 0.172378646f, 0.172473084f, 0.172567520f,
+    0.172661955f, 0.172756388f, 0.172850820f, 0.172945249f, 0.173039678f,
+    0.173134105f, 0.173228530f, 0.173322953f, 0.173417375f, 0.173511796f,
+    0.173606214f, 0.173700631f, 0.173795047f, 0.173889461f, 0.173983873f,
+    0.174078284f, 0.174172693f, 0.174267101f, 0.174361507f, 0.174455911f,
+    0.174550314f, 0.174644715f, 0.174739115f, 0.174833513f, 0.174927909f,
+    0.175022304f, 0.175116697f, 0.175211088f, 0.175305478f, 0.175399867f,
+    0.175494253f, 0.175588638f, 0.175683022f, 0.175777404f, 0.175871784f,
+    0.175966163f, 0.176060540f, 0.176154915f, 0.176249289f, 0.176343661f,
+    0.176438031f, 0.176532400f, 0.176626768f, 0.176721133f, 0.176815497f,
+    0.176909860f, 0.177004220f, 0.177098580f, 0.177192937f, 0.177287293f,
+    0.177381647f, 0.177476000f, 0.177570351f, 0.177664700f, 0.177759048f,
+    0.177853394f, 0.177947739f, 0.178042081f, 0.178136423f, 0.178230762f,
+    0.178325100f, 0.178419436f, 0.178513771f, 0.178608104f, 0.178702435f,
+    0.178796765f, 0.178891093f, 0.178985420f, 0.179079744f, 0.179174067f,
+    0.179268389f, 0.179362709f, 0.179457027f, 0.179551343f, 0.179645658f,
+    0.179739972f, 0.179834283f, 0.179928593f, 0.180022901f, 0.180117208f,
+    0.180211513f, 0.180305816f, 0.180400118f, 0.180494418f, 0.180588716f,
+    0.180683013f, 0.180777308f, 0.180871601f, 0.180965893f, 0.181060183f,
+    0.181154471f, 0.181248758f, 0.181343043f, 0.181437327f, 0.181531608f,
+    0.181625888f, 0.181720167f, 0.181814443f, 0.181908718f, 0.182002992f,
+    0.182097263f, 0.182191533f, 0.182285802f, 0.182380068f, 0.182474333f,
+    0.182568597f, 0.182662858f, 0.182757118f, 0.182851376f, 0.182945633f,
+    0.183039888f, 0.183134141f, 0.183228393f, 0.183322643f, 0.183416891f,
+    0.183511137f, 0.183605382f, 0.183699625f, 0.183793867f, 0.183888106f,
+    0.183982344f, 0.184076581f, 0.184170815f, 0.184265048f, 0.184359279f,
+    0.184453509f, 0.184547737f, 0.184641963f, 0.184736188f, 0.184830410f,
+    0.184924631f, 0.185018851f, 0.185113069f, 0.185207285f, 0.185301499f,
+    0.185395711f, 0.185489922f, 0.185584131f, 0.185678339f, 0.185772545f,
+    0.185866749f, 0.185960951f, 0.186055152f, 0.186149351f, 0.186243548f,
+    0.186337743f, 0.186431937f, 0.186526129f, 0.186620320f, 0.186714508f,
+    0.186808695f, 0.186902880f, 0.186997064f, 0.187091246f, 0.187185426f,
+    0.187279604f, 0.187373781f, 0.187467955f, 0.187562129f, 0.187656300f,
+    0.187750470f, 0.187844638f, 0.187938804f, 0.188032969f, 0.188127131f,
+    0.188221292f, 0.188315452f, 0.188409609f, 0.188503765f, 0.188597919f,
+    0.188692072f, 0.188786223f, 0.188880371f, 0.188974519f, 0.189068664f,
+    0.189162808f, 0.189256950f, 0.189351090f, 0.189445229f, 0.189539365f,
+    0.189633500f, 0.189727634f, 0.189821765f, 0.189915895f, 0.190010023f,
+    0.190104149f, 0.190198274f, 0.190292397f, 0.190386518f, 0.190480637f,
+    0.190574755f, 0.190668871f, 0.190762985f, 0.190857097f, 0.190951208f,
+    0.191045316f, 0.191139423f, 0.191233529f, 0.191327632f, 0.191421734f,
+    0.191515834f, 0.191609932f, 0.191704029f, 0.191798123f, 0.191892216f,
+    0.191986308f, 0.192080397f, 0.192174485f, 0.192268571f, 0.192362655f,
+    0.192456737f, 0.192550818f, 0.192644897f, 0.192738974f, 0.192833049f,
+    0.192927122f, 0.193021194f, 0.193115264f, 0.193209332f, 0.193303399f,
+    0.193397463f, 0.193491526f, 0.193585587f, 0.193679647f, 0.193773704f,
+    0.193867760f, 0.193961814f, 0.194055866f, 0.194149916f, 0.194243965f,
+    0.194338012f, 0.194432057f, 0.194526100f, 0.194620142f, 0.194714181f,
+    0.194808219f, 0.194902255f, 0.194996290f, 0.195090322f, 0.195184353f,
+    0.195278382f, 0.195372409f, 0.195466434f, 0.195560458f, 0.195654479f,
+    0.195748499f, 0.195842517f, 0.195936534f, 0.196030548f, 0.196124561f,
+    0.196218572f, 0.196312581f, 0.196406588f, 0.196500594f, 0.196594598f,
+    0.196688600f, 0.196782600f, 0.196876598f, 0.196970594f, 0.197064589f,
+    0.197158582f, 0.197252573f, 0.197346562f, 0.197440550f, 0.197534535f,
+    0.197628519f, 0.197722501f, 0.197816481f, 0.197910460f, 0.198004436f,
+    0.198098411f, 0.198192384f, 0.198286355f, 0.198380324f, 0.198474291f,
+    0.198568257f, 0.198662221f, 0.198756183f, 0.198850143f, 0.198944101f,
+    0.199038057f, 0.199132012f, 0.199225965f, 0.199319916f, 0.199413865f,
+    0.199507812f, 0.199601758f, 0.199695701f, 0.199789643f, 0.199883583f,
+    0.199977521f, 0.200071457f, 0.200165392f, 0.200259324f, 0.200353255f,
+    0.200447184f, 0.200541111f, 0.200635036f, 0.200728960f, 0.200822881f,
+    0.200916801f, 0.201010719f, 0.201104635f, 0.201198549f, 0.201292461f,
+    0.201386372f, 0.201480280f, 0.201574187f, 0.201668092f, 0.201761995f,
+    0.201855896f, 0.201949796f, 0.202043693f, 0.202137589f, 0.202231482f,
+    0.202325374f, 0.202419264f, 0.202513153f, 0.202607039f, 0.202700923f,
+    0.202794806f, 0.202888687f, 0.202982565f, 0.203076442f, 0.203170318f,
+    0.203264191f, 0.203358062f, 0.203451932f, 0.203545799f, 0.203639665f,
+    0.203733529f, 0.203827391f, 0.203921251f, 0.204015110f, 0.204108966f,
+    0.204202821f, 0.204296673f, 0.204390524f, 0.204484373f, 0.204578220f,
+    0.204672065f, 0.204765908f, 0.204859750f, 0.204953589f, 0.205047427f,
+    0.205141263f, 0.205235097f, 0.205328928f, 0.205422759f, 0.205516587f,
+    0.205610413f, 0.205704237f, 0.205798060f, 0.205891881f, 0.205985699f,
+    0.206079516f, 0.206173331f, 0.206267144f, 0.206360955f, 0.206454765f,
+    0.206548572f, 0.206642377f, 0.206736181f, 0.206829983f, 0.206923782f,
+    0.207017580f, 0.207111376f, 0.207205170f, 0.207298962f, 0.207392753f,
+    0.207486541f, 0.207580327f, 0.207674112f, 0.207767895f, 0.207861675f,
+    0.207955454f, 0.208049231f, 0.208143006f, 0.208236779f, 0.208330550f,
+    0.208424319f, 0.208518087f, 0.208611852f, 0.208705615f, 0.208799377f,
+    0.208893137f, 0.208986894f, 0.209080650f, 0.209174404f, 0.209268156f,
+    0.209361906f, 0.209455654f, 0.209549400f, 0.209643145f, 0.209736887f,
+    0.209830627f, 0.209924366f, 0.210018102f, 0.210111837f, 0.210205570f,
+    0.210299300f, 0.210393029f, 0.210486756f, 0.210580481f, 0.210674204f,
+    0.210767925f, 0.210861644f, 0.210955361f, 0.211049077f, 0.211142790f,
+    0.211236501f, 0.211330211f, 0.211423918f, 0.211517624f, 0.211611327f,
+    0.211705029f, 0.211798729f, 0.211892427f, 0.211986122f, 0.212079816f,
+    0.212173508f, 0.212267198f, 0.212360886f, 0.212454572f, 0.212548256f,
+    0.212641938f, 0.212735619f, 0.212829297f, 0.212922973f, 0.213016648f,
+    0.213110320f, 0.213203990f, 0.213297659f, 0.213391325f, 0.213484990f,
+    0.213578652f, 0.213672313f, 0.213765972f, 0.213859628f, 0.213953283f,
+    0.214046936f, 0.214140587f, 0.214234235f, 0.214327882f, 0.214421527f,
+    0.214515170f, 0.214608811f, 0.214702450f, 0.214796087f, 0.214889722f,
+    0.214983355f, 0.215076986f, 0.215170615f, 0.215264242f, 0.215357867f,
+    0.215451491f, 0.215545112f, 0.215638731f, 0.215732348f, 0.215825963f,
+    0.215919577f, 0.216013188f, 0.216106797f, 0.216200404f, 0.216294010f,
+    0.216387613f, 0.216481214f, 0.216574814f, 0.216668411f, 0.216762006f,
+    0.216855600f, 0.216949191f, 0.217042780f, 0.217136368f, 0.217229953f,
+    0.217323536f, 0.217417118f, 0.217510697f, 0.217604275f, 0.217697850f,
+    0.217791423f, 0.217884995f, 0.217978564f, 0.218072132f, 0.218165697f,
+    0.218259260f, 0.218352822f, 0.218446381f, 0.218539938f, 0.218633494f,
+    0.218727047f, 0.218820598f, 0.218914148f, 0.219007695f, 0.219101240f,
+    0.219194783f, 0.219288325f, 0.219381864f, 0.219475401f, 0.219568936f,
+    0.219662469f, 0.219756001f, 0.219849530f, 0.219943057f, 0.220036582f,
+    0.220130105f, 0.220223626f, 0.220317145f, 0.220410662f, 0.220504177f,
+    0.220597690f, 0.220691201f, 0.220784710f, 0.220878217f, 0.220971722f,
+    0.221065224f, 0.221158725f, 0.221252224f, 0.221345721f, 0.221439215f,
+    0.221532708f, 0.221626199f, 0.221719687f, 0.221813174f, 0.221906658f,
+    0.222000141f, 0.222093621f, 0.222187099f, 0.222280576f, 0.222374050f,
+    0.222467522f, 0.222560992f, 0.222654461f, 0.222747927f, 0.222841391f,
+    0.222934853f, 0.223028313f, 0.223121771f, 0.223215226f, 0.223308680f,
+    0.223402132f, 0.223495582f, 0.223589029f, 0.223682475f, 0.223775918f,
+    0.223869360f, 0.223962799f, 0.224056237f, 0.224149672f, 0.224243105f,
+    0.224336536f, 0.224429965f, 0.224523392f, 0.224616817f, 0.224710240f,
+    0.224803661f, 0.224897080f, 0.224990497f, 0.225083911f, 0.225177324f,
+    0.225270734f, 0.225364143f, 0.225457549f, 0.225550954f, 0.225644356f,
+    0.225737756f, 0.225831154f, 0.225924550f, 0.226017944f, 0.226111336f,
+    0.226204726f, 0.226298113f, 0.226391499f, 0.226484882f, 0.226578264f,
+    0.226671643f, 0.226765020f, 0.226858396f, 0.226951769f, 0.227045140f,
+    0.227138509f, 0.227231876f, 0.227325240f, 0.227418603f, 0.227511964f,
+    0.227605322f, 0.227698679f, 0.227792033f, 0.227885385f, 0.227978735f,
+    0.228072083f, 0.228165429f, 0.228258773f, 0.228352115f, 0.228445454f,
+    0.228538792f, 0.228632127f, 0.228725461f, 0.228818792f, 0.228912121f,
+    0.229005448f, 0.229098773f, 0.229192096f, 0.229285416f, 0.229378735f,
+    0.229472051f, 0.229565366f, 0.229658678f, 0.229751988f, 0.229845296f,
+    0.229938602f, 0.230031906f, 0.230125208f, 0.230218507f, 0.230311805f,
+    0.230405100f, 0.230498393f, 0.230591685f, 0.230684974f, 0.230778260f,
+    0.230871545f, 0.230964828f, 0.231058108f, 0.231151387f, 0.231244663f,
+    0.231337937f, 0.231431209f, 0.231524479f, 0.231617747f, 0.231711012f,
+    0.231804276f, 0.231897537f, 0.231990796f, 0.232084054f, 0.232177309f,
+    0.232270561f, 0.232363812f, 0.232457061f, 0.232550307f, 0.232643551f,
+    0.232736793f, 0.232830033f, 0.232923271f, 0.233016507f, 0.233109741f,
+    0.233202972f, 0.233296201f, 0.233389429f, 0.233482654f, 0.233575876f,
+    0.233669097f, 0.233762316f, 0.233855532f, 0.233948746f, 0.234041959f,
+    0.234135169f, 0.234228376f, 0.234321582f, 0.234414786f, 0.234507987f,
+    0.234601186f, 0.234694383f, 0.234787578f, 0.234880771f, 0.234973961f,
+    0.235067150f, 0.235160336f, 0.235253520f, 0.235346702f, 0.235439882f,
+    0.235533059f, 0.235626235f, 0.235719408f, 0.235812579f, 0.235905748f,
+    0.235998915f, 0.236092080f, 0.236185242f, 0.236278402f, 0.236371560f,
+    0.236464716f, 0.236557870f, 0.236651021f, 0.236744171f, 0.236837318f,
+    0.236930463f, 0.237023606f, 0.237116747f, 0.237209885f, 0.237303021f,
+    0.237396156f, 0.237489288f, 0.237582417f, 0.237675545f, 0.237768670f,
+    0.237861794f, 0.237954915f, 0.238048033f, 0.238141150f, 0.238234265f,
+    0.238327377f, 0.238420487f, 0.238513595f, 0.238606701f, 0.238699804f,
+    0.238792905f, 0.238886004f, 0.238979101f, 0.239072196f, 0.239165289f,
+    0.239258379f, 0.239351467f, 0.239444553f, 0.239537637f, 0.239630718f,
+    0.239723798f, 0.239816875f, 0.239909950f, 0.240003022f, 0.240096093f,
+    0.240189161f, 0.240282227f, 0.240375291f, 0.240468353f, 0.240561412f,
+    0.240654470f, 0.240747525f, 0.240840578f, 0.240933628f, 0.241026677f,
+    0.241119723f, 0.241212767f, 0.241305808f, 0.241398848f, 0.241491885f,
+    0.241584920f, 0.241677953f, 0.241770984f, 0.241864012f, 0.241957039f,
+    0.242050063f, 0.242143084f, 0.242236104f, 0.242329121f, 0.242422136f,
+    0.242515149f, 0.242608160f, 0.242701168f, 0.242794174f, 0.242887178f,
+    0.242980180f, 0.243073179f, 0.243166177f, 0.243259172f, 0.243352164f,
+    0.243445155f, 0.243538143f, 0.243631129f, 0.243724113f, 0.243817095f,
+    0.243910074f, 0.244003051f, 0.244096026f, 0.244188998f, 0.244281969f,
+    0.244374937f, 0.244467903f, 0.244560866f, 0.244653828f, 0.244746787f,
+    0.244839744f, 0.244932698f, 0.245025651f, 0.245118601f, 0.245211549f,
+    0.245304494f, 0.245397438f, 0.245490379f, 0.245583318f, 0.245676254f,
+    0.245769188f, 0.245862121f, 0.245955050f, 0.246047978f, 0.246140903f,
+    0.246233826f, 0.246326747f, 0.246419665f, 0.246512582f, 0.246605496f,
+    0.246698407f, 0.246791317f, 0.246884224f, 0.246977129f, 0.247070031f,
+    0.247162932f, 0.247255830f, 0.247348726f, 0.247441619f, 0.247534510f,
+    0.247627399f, 0.247720286f, 0.247813171f, 0.247906053f, 0.247998933f,
+    0.248091810f, 0.248184685f, 0.248277558f, 0.248370429f, 0.248463298f,
+    0.248556164f, 0.248649028f, 0.248741889f, 0.248834749f, 0.248927606f,
+    0.249020460f, 0.249113313f, 0.249206163f, 0.249299011f, 0.249391857f,
+    0.249484700f, 0.249577541f, 0.249670380f, 0.249763216f, 0.249856050f,
+    0.249948882f, 0.250041711f, 0.250134539f, 0.250227364f, 0.250320186f,
+    0.250413007f, 0.250505825f, 0.250598640f, 0.250691454f, 0.250784265f,
+    0.250877074f, 0.250969880f, 0.251062684f, 0.251155486f, 0.251248286f,
+    0.251341083f, 0.251433878f, 0.251526671f, 0.251619461f, 0.251712249f,
+    0.251805035f, 0.251897818f, 0.251990599f, 0.252083378f, 0.252176154f,
+    0.252268929f, 0.252361700f, 0.252454470f, 0.252547237f, 0.252640002f,
+    0.252732764f, 0.252825525f, 0.252918282f, 0.253011038f, 0.253103791f,
+    0.253196542f, 0.253289291f, 0.253382037f, 0.253474781f, 0.253567522f,
+    0.253660262f, 0.253752999f, 0.253845733f, 0.253938466f, 0.254031195f,
+    0.254123923f, 0.254216648f, 0.254309371f, 0.254402092f, 0.254494810f,
+    0.254587526f, 0.254680240f, 0.254772951f, 0.254865660f, 0.254958366f,
+    0.255051070f, 0.255143772f, 0.255236472f, 0.255329169f, 0.255421864f,
+    0.255514556f, 0.255607246f, 0.255699934f, 0.255792619f, 0.255885302f,
+    0.255977983f, 0.256070662f, 0.256163338f, 0.256256011f, 0.256348682f,
+    0.256441351f, 0.256534018f, 0.256626682f, 0.256719344f, 0.256812004f,
+    0.256904661f, 0.256997316f, 0.257089968f, 0.257182618f, 0.257275266f,
+    0.257367911f, 0.257460554f, 0.257553195f, 0.257645833f, 0.257738469f,
+    0.257831102f, 0.257923733f, 0.258016362f, 0.258108988f, 0.258201612f,
+    0.258294234f, 0.258386853f, 0.258479470f, 0.258572085f, 0.258664697f,
+    0.258757307f, 0.258849914f, 0.258942519f, 0.259035122f, 0.259127722f,
+    0.259220320f, 0.259312915f, 0.259405508f, 0.259498099f, 0.259590687f,
+    0.259683273f, 0.259775857f, 0.259868438f, 0.259961017f, 0.260053593f,
+    0.260146167f, 0.260238739f, 0.260331308f, 0.260423875f, 0.260516439f,
+    0.260609001f, 0.260701561f, 0.260794118f, 0.260886673f, 0.260979225f,
+    0.261071775f, 0.261164323f, 0.261256868f, 0.261349411f, 0.261441951f,
+    0.261534489f, 0.261627025f, 0.261719558f, 0.261812089f, 0.261904617f,
+    0.261997143f, 0.262089667f, 0.262182188f, 0.262274707f, 0.262367223f,
+    0.262459737f, 0.262552249f, 0.262644758f, 0.262737265f, 0.262829769f,
+    0.262922271f, 0.263014770f, 0.263107267f, 0.263199762f, 0.263292254f,
+    0.263384744f, 0.263477231f, 0.263569716f, 0.263662199f, 0.263754679f,
+    0.263847157f, 0.263939632f, 0.264032105f, 0.264124575f, 0.264217043f,
+    0.264309509f, 0.264401972f, 0.264494432f, 0.264586891f, 0.264679346f,
+    0.264771800f, 0.264864251f, 0.264956699f, 0.265049145f, 0.265141589f,
+    0.265234030f, 0.265326469f, 0.265418905f, 0.265511339f, 0.265603771f,
+    0.265696200f, 0.265788626f, 0.265881050f, 0.265973472f, 0.266065891f,
+    0.266158308f, 0.266250722f, 0.266343134f, 0.266435544f, 0.266527951f,
+    0.266620355f, 0.266712757f, 0.266805157f, 0.266897554f, 0.266989949f,
+    0.267082341f, 0.267174731f, 0.267267119f, 0.267359503f, 0.267451886f,
+    0.267544266f, 0.267636643f, 0.267729019f, 0.267821391f, 0.267913761f,
+    0.268006129f, 0.268098494f, 0.268190857f, 0.268283217f, 0.268375575f,
+    0.268467931f, 0.268560283f, 0.268652634f, 0.268744982f, 0.268837327f,
+    0.268929670f, 0.269022011f, 0.269114349f, 0.269206685f, 0.269299018f,
+    0.269391348f, 0.269483677f, 0.269576002f, 0.269668326f, 0.269760646f,
+    0.269852965f, 0.269945280f, 0.270037594f, 0.270129905f, 0.270222213f,
+    0.270314519f, 0.270406822f, 0.270499123f, 0.270591421f, 0.270683717f,
+    0.270776011f, 0.270868302f, 0.270960590f, 0.271052876f, 0.271145160f,
+    0.271237441f, 0.271329719f, 0.271421995f, 0.271514268f, 0.271606539f,
+    0.271698808f, 0.271791074f, 0.271883337f, 0.271975598f, 0.272067857f,
+    0.272160113f, 0.272252366f, 0.272344617f, 0.272436866f, 0.272529112f,
+    0.272621355f, 0.272713596f, 0.272805835f, 0.272898071f, 0.272990304f,
+    0.273082535f, 0.273174764f, 0.273266990f, 0.273359213f, 0.273451434f,
+    0.273543652f, 0.273635868f, 0.273728082f, 0.273820292f, 0.273912501f,
+    0.274004707f, 0.274096910f, 0.274189111f, 0.274281309f, 0.274373505f,
+    0.274465698f, 0.274557889f, 0.274650077f, 0.274742262f, 0.274834445f,
+    0.274926626f, 0.275018804f, 0.275110980f, 0.275203153f, 0.275295323f,
+    0.275387491f, 0.275479656f, 0.275571819f, 0.275663980f, 0.275756137f,
+    0.275848293f, 0.275940445f, 0.276032596f, 0.276124743f, 0.276216888f,
+    0.276309031f, 0.276401171f, 0.276493309f, 0.276585444f, 0.276677576f,
+    0.276769706f, 0.276861833f, 0.276953958f, 0.277046080f, 0.277138200f,
+    0.277230317f, 0.277322432f, 0.277414544f, 0.277506653f, 0.277598760f,
+    0.277690865f, 0.277782967f, 0.277875066f, 0.277967163f, 0.278059257f,
+    0.278151348f, 0.278243438f, 0.278335524f, 0.278427608f, 0.278519689f,
+    0.278611768f, 0.278703845f, 0.278795918f, 0.278887989f, 0.278980058f,
+    0.279072124f, 0.279164187f, 0.279256248f, 0.279348307f, 0.279440362f,
+    0.279532416f, 0.279624466f, 0.279716514f, 0.279808560f, 0.279900603f,
+    0.279992643f, 0.280084681f, 0.280176716f, 0.280268749f, 0.280360779f,
+    0.280452806f, 0.280544831f, 0.280636853f, 0.280728873f, 0.280820890f,
+    0.280912905f, 0.281004917f, 0.281096926f, 0.281188933f, 0.281280937f,
+    0.281372939f, 0.281464938f, 0.281556934f, 0.281648928f, 0.281740920f,
+    0.281832908f, 0.281924894f, 0.282016878f, 0.282108859f, 0.282200837f,
+    0.282292813f, 0.282384786f, 0.282476757f, 0.282568725f, 0.282660690f,
+    0.282752653f, 0.282844613f, 0.282936570f, 0.283028525f, 0.283120478f,
+    0.283212428f, 0.283304375f, 0.283396319f, 0.283488261f, 0.283580201f,
+    0.283672137f, 0.283764071f, 0.283856003f, 0.283947932f, 0.284039858f,
+    0.284131782f, 0.284223703f, 0.284315621f, 0.284407537f, 0.284499450f,
+    0.284591361f, 0.284683269f, 0.284775174f, 0.284867077f, 0.284958977f,
+    0.285050875f, 0.285142770f, 0.285234662f, 0.285326552f, 0.285418439f,
+    0.285510323f, 0.285602205f, 0.285694084f, 0.285785961f, 0.285877835f,
+    0.285969706f, 0.286061575f, 0.286153441f, 0.286245304f, 0.286337165f,
+    0.286429023f, 0.286520879f, 0.286612731f, 0.286704582f, 0.286796429f,
+    0.286888274f, 0.286980117f, 0.287071956f, 0.287163793f, 0.287255628f,
+    0.287347460f, 0.287439289f, 0.287531115f, 0.287622939f, 0.287714760f,
+    0.287806579f, 0.287898395f, 0.287990208f, 0.288082019f, 0.288173827f,
+    0.288265632f, 0.288357435f, 0.288449235f, 0.288541032f, 0.288632827f,
+    0.288724619f, 0.288816408f, 0.288908195f, 0.288999979f, 0.289091761f,
+    0.289183539f, 0.289275315f, 0.289367089f, 0.289458860f, 0.289550628f,
+    0.289642393f, 0.289734156f, 0.289825916f, 0.289917674f, 0.290009429f,
+    0.290101181f, 0.290192930f, 0.290284677f, 0.290376421f, 0.290468163f,
+    0.290559902f, 0.290651638f, 0.290743371f, 0.290835102f, 0.290926830f,
+    0.291018556f, 0.291110279f, 0.291201999f, 0.291293716f, 0.291385431f,
+    0.291477143f, 0.291568852f, 0.291660559f, 0.291752263f, 0.291843965f,
+    0.291935663f, 0.292027359f, 0.292119053f, 0.292210743f, 0.292302431f,
+    0.292394116f, 0.292485799f, 0.292577479f, 0.292669156f, 0.292760831f,
+    0.292852502f, 0.292944171f, 0.293035838f, 0.293127502f, 0.293219163f,
+    0.293310821f, 0.293402477f, 0.293494130f, 0.293585780f, 0.293677427f,
+    0.293769072f, 0.293860714f, 0.293952354f, 0.294043991f, 0.294135625f,
+    0.294227256f, 0.294318885f, 0.294410511f, 0.294502134f, 0.294593754f,
+    0.294685372f, 0.294776987f, 0.294868600f, 0.294960209f, 0.295051816f,
+    0.295143421f, 0.295235022f, 0.295326621f, 0.295418217f, 0.295509811f,
+    0.295601401f, 0.295692989f, 0.295784574f, 0.295876157f, 0.295967737f,
+    0.296059314f, 0.296150888f, 0.296242460f, 0.296334029f, 0.296425595f,
+    0.296517159f, 0.296608719f, 0.296700277f, 0.296791833f, 0.296883385f,
+    0.296974935f, 0.297066482f, 0.297158027f, 0.297249568f, 0.297341107f,
+    0.297432643f, 0.297524177f, 0.297615707f, 0.297707235f, 0.297798761f,
+    0.297890283f, 0.297981803f, 0.298073320f, 0.298164834f, 0.298256346f,
+    0.298347855f, 0.298439361f, 0.298530864f, 0.298622365f, 0.298713862f,
+    0.298805358f, 0.298896850f, 0.298988339f, 0.299079826f, 0.299171310f,
+    0.299262792f, 0.299354270f, 0.299445746f, 0.299537219f, 0.299628690f,
+    0.299720157f, 0.299811622f, 0.299903084f, 0.299994543f, 0.300086000f,
+    0.300177454f, 0.300268905f, 0.300360353f, 0.300451799f, 0.300543241f,
+    0.300634681f, 0.300726119f, 0.300817553f, 0.300908985f, 0.301000414f,
+    0.301091840f, 0.301183263f, 0.301274684f, 0.301366102f, 0.301457517f,
+    0.301548929f, 0.301640339f, 0.301731746f, 0.301823150f, 0.301914551f,
+    0.302005949f, 0.302097345f, 0.302188738f, 0.302280128f, 0.302371515f,
+    0.302462900f, 0.302554282f, 0.302645661f, 0.302737037f, 0.302828410f,
+    0.302919781f, 0.303011149f, 0.303102514f, 0.303193876f, 0.303285236f,
+    0.303376593f, 0.303467947f, 0.303559298f, 0.303650646f, 0.303741992f,
+    0.303833334f, 0.303924674f, 0.304016012f, 0.304107346f, 0.304198678f,
+    0.304290006f, 0.304381332f, 0.304472656f, 0.304563976f, 0.304655294f,
+    0.304746609f, 0.304837921f, 0.304929230f, 0.305020536f, 0.305111840f,
+    0.305203141f, 0.305294439f, 0.305385734f, 0.305477026f, 0.305568316f,
+    0.305659602f, 0.305750886f, 0.305842168f, 0.305933446f, 0.306024721f,
+    0.306115994f, 0.306207264f, 0.306298531f, 0.306389795f, 0.306481057f,
+    0.306572315f, 0.306663571f, 0.306754824f, 0.306846074f, 0.306937322f,
+    0.307028566f, 0.307119808f, 0.307211047f, 0.307302283f, 0.307393516f,
+    0.307484747f, 0.307575974f, 0.307667199f, 0.307758421f, 0.307849640f,
+    0.307940856f, 0.308032070f, 0.308123280f, 0.308214488f, 0.308305693f,
+    0.308396895f, 0.308488094f, 0.308579291f, 0.308670485f, 0.308761675f,
+    0.308852863f, 0.308944048f, 0.309035231f, 0.309126410f, 0.309217587f,
+    0.309308760f, 0.309399931f, 0.309491099f, 0.309582264f, 0.309673427f,
+    0.309764586f, 0.309855743f, 0.309946897f, 0.310038048f, 0.310129196f,
+    0.310220341f, 0.310311484f, 0.310402623f, 0.310493760f, 0.310584894f,
+    0.310676025f, 0.310767153f, 0.310858278f, 0.310949400f, 0.311040520f,
+    0.311131637f, 0.311222751f, 0.311313862f, 0.311404970f, 0.311496075f,
+    0.311587177f, 0.311678277f, 0.311769374f, 0.311860467f, 0.311951558f,
+    0.312042646f, 0.312133732f, 0.312224814f, 0.312315893f, 0.312406970f,
+    0.312498044f, 0.312589115f, 0.312680183f, 0.312771248f, 0.312862310f,
+    0.312953369f, 0.313044426f, 0.313135479f, 0.313226530f, 0.313317578f,
+    0.313408623f, 0.313499665f, 0.313590704f, 0.313681740f, 0.313772774f,
+    0.313863804f, 0.313954832f, 0.314045857f, 0.314136879f, 0.314227898f,
+    0.314318914f, 0.314409927f, 0.314500937f, 0.314591945f, 0.314682949f,
+    0.314773951f, 0.314864950f, 0.314955946f, 0.315046939f, 0.315137929f,
+    0.315228916f, 0.315319900f, 0.315410882f, 0.315501860f, 0.315592836f,
+    0.315683808f, 0.315774778f, 0.315865745f, 0.315956709f, 0.316047670f,
+    0.316138628f, 0.316229584f, 0.316320536f, 0.316411485f, 0.316502432f,
+    0.316593376f, 0.316684316f, 0.316775254f, 0.316866189f, 0.316957121f,
+    0.317048050f, 0.317138976f, 0.317229899f, 0.317320820f, 0.317411737f,
+    0.317502652f, 0.317593563f, 0.317684472f, 0.317775378f, 0.317866281f,
+    0.317957180f, 0.318048077f, 0.318138971f, 0.318229863f, 0.318320751f,
+    0.318411636f, 0.318502518f, 0.318593398f, 0.318684274f, 0.318775148f,
+    0.318866018f, 0.318956886f, 0.319047751f, 0.319138613f, 0.319229472f,
+    0.319320328f, 0.319411181f, 0.319502031f, 0.319592878f, 0.319683722f,
+    0.319774563f, 0.319865402f, 0.319956237f, 0.320047070f, 0.320137899f,
+    0.320228726f, 0.320319549f, 0.320410370f, 0.320501188f, 0.320592003f,
+    0.320682815f, 0.320773623f, 0.320864429f, 0.320955232f, 0.321046032f,
+    0.321136830f, 0.321227624f, 0.321318415f, 0.321409203f, 0.321499989f,
+    0.321590771f, 0.321681550f, 0.321772327f, 0.321863100f, 0.321953871f,
+    0.322044638f, 0.322135403f, 0.322226164f, 0.322316923f, 0.322407679f,
+    0.322498432f, 0.322589181f, 0.322679928f, 0.322770672f, 0.322861413f,
+    0.322952151f, 0.323042886f, 0.323133618f, 0.323224347f, 0.323315073f,
+    0.323405796f, 0.323496516f, 0.323587233f, 0.323677947f, 0.323768658f,
+    0.323859367f, 0.323950072f, 0.324040774f, 0.324131473f, 0.324222170f,
+    0.324312863f, 0.324403553f, 0.324494240f, 0.324584925f, 0.324675606f,
+    0.324766285f, 0.324856960f, 0.324947632f, 0.325038302f, 0.325128968f,
+    0.325219632f, 0.325310292f, 0.325400950f, 0.325491604f, 0.325582256f,
+    0.325672904f, 0.325763550f, 0.325854192f, 0.325944832f, 0.326035468f,
+    0.326126102f, 0.326216732f, 0.326307360f, 0.326397984f, 0.326488606f,
+    0.326579224f, 0.326669840f, 0.326760452f, 0.326851062f, 0.326941668f,
+    0.327032272f, 0.327122872f, 0.327213470f, 0.327304064f, 0.327394656f,
+    0.327485244f, 0.327575830f, 0.327666412f, 0.327756992f, 0.327847568f,
+    0.327938141f, 0.328028712f, 0.328119279f, 0.328209844f, 0.328300405f,
+    0.328390963f, 0.328481519f, 0.328572071f, 0.328662620f, 0.328753166f,
+    0.328843710f, 0.328934250f, 0.329024787f, 0.329115321f, 0.329205852f,
+    0.329296380f, 0.329386905f, 0.329477428f, 0.329567947f, 0.329658463f,
+    0.329748975f, 0.329839485f, 0.329929992f, 0.330020496f, 0.330110997f,
+    0.330201495f, 0.330291990f, 0.330382481f, 0.330472970f, 0.330563456f,
+    0.330653938f, 0.330744418f, 0.330834894f, 0.330925368f, 0.331015838f,
+    0.331106306f, 0.331196770f, 0.331287231f, 0.331377690f, 0.331468145f,
+    0.331558597f, 0.331649046f, 0.331739492f, 0.331829935f, 0.331920375f,
+    0.332010812f, 0.332101246f, 0.332191677f, 0.332282105f, 0.332372530f,
+    0.332462951f, 0.332553370f, 0.332643785f, 0.332734198f, 0.332824607f,
+    0.332915014f, 0.333005417f, 0.333095817f, 0.333186215f, 0.333276609f,
+    0.333367000f, 0.333457388f, 0.333547773f, 0.333638155f, 0.333728533f,
+    0.333818909f, 0.333909282f, 0.333999651f, 0.334090018f, 0.334180381f,
+    0.334270742f, 0.334361099f, 0.334451453f, 0.334541805f, 0.334632153f,
+    0.334722498f, 0.334812840f, 0.334903179f, 0.334993514f, 0.335083847f,
+    0.335174177f, 0.335264503f, 0.335354827f, 0.335445147f, 0.335535464f,
+    0.335625779f, 0.335716090f, 0.335806398f, 0.335896703f, 0.335987005f,
+    0.336077303f, 0.336167599f, 0.336257892f, 0.336348181f, 0.336438468f,
+    0.336528751f, 0.336619031f, 0.336709308f, 0.336799582f, 0.336889853f,
+    0.336980121f, 0.337070386f, 0.337160648f, 0.337250906f, 0.337341162f,
+    0.337431414f, 0.337521663f, 0.337611909f, 0.337702153f, 0.337792392f,
+    0.337882629f, 0.337972863f, 0.338063094f, 0.338153321f, 0.338243546f,
+    0.338333767f, 0.338423985f, 0.338514200f, 0.338604412f, 0.338694621f,
+    0.338784827f, 0.338875029f, 0.338965229f, 0.339055425f, 0.339145619f,
+    0.339235809f, 0.339325996f, 0.339416180f, 0.339506361f, 0.339596538f,
+    0.339686713f, 0.339776884f, 0.339867053f, 0.339957218f, 0.340047380f,
+    0.340137539f, 0.340227695f, 0.340317848f, 0.340407997f, 0.340498144f,
+    0.340588287f, 0.340678427f, 0.340768564f, 0.340858698f, 0.340948829f,
+    0.341038956f, 0.341129081f, 0.341219202f, 0.341309321f, 0.341399436f,
+    0.341489548f, 0.341579656f, 0.341669762f, 0.341759865f, 0.341849964f,
+    0.341940060f, 0.342030154f, 0.342120243f, 0.342210330f, 0.342300414f,
+    0.342390495f, 0.342480572f, 0.342570646f, 0.342660717f, 0.342750785f,
+    0.342840850f, 0.342930912f, 0.343020970f, 0.343111026f, 0.343201078f,
+    0.343291127f, 0.343381173f, 0.343471215f, 0.343561255f, 0.343651291f,
+    0.343741325f, 0.343831355f, 0.343921382f, 0.344011405f, 0.344101426f,
+    0.344191443f, 0.344281458f, 0.344371469f, 0.344461477f, 0.344551482f,
+    0.344641483f, 0.344731482f, 0.344821477f, 0.344911469f, 0.345001458f,
+    0.345091444f, 0.345181426f, 0.345271406f, 0.345361382f, 0.345451355f,
+    0.345541325f, 0.345631292f, 0.345721255f, 0.345811216f, 0.345901173f,
+    0.345991127f, 0.346081078f, 0.346171025f, 0.346260970f, 0.346350911f,
+    0.346440849f, 0.346530784f, 0.346620716f, 0.346710644f, 0.346800570f,
+    0.346890492f, 0.346980411f, 0.347070327f, 0.347160239f, 0.347250149f,
+    0.347340055f, 0.347429958f, 0.347519858f, 0.347609754f, 0.347699648f,
+    0.347789538f, 0.347879425f, 0.347969309f, 0.348059190f, 0.348149067f,
+    0.348238941f, 0.348328812f, 0.348418680f, 0.348508545f, 0.348598406f,
+    0.348688265f, 0.348778120f, 0.348867971f, 0.348957820f, 0.349047666f,
+    0.349137508f, 0.349227347f, 0.349317183f, 0.349407015f, 0.349496844f,
+    0.349586671f, 0.349676494f, 0.349766313f, 0.349856130f, 0.349945943f,
+    0.350035753f, 0.350125560f, 0.350215364f, 0.350305164f, 0.350394961f,
+    0.350484755f, 0.350574546f, 0.350664334f, 0.350754118f, 0.350843899f,
+    0.350933677f, 0.351023452f, 0.351113223f, 0.351202991f, 0.351292756f,
+    0.351382518f, 0.351472276f, 0.351562032f, 0.351651784f, 0.351741532f,
+    0.351831278f, 0.351921020f, 0.352010759f, 0.352100495f, 0.352190228f,
+    0.352279957f, 0.352369684f, 0.352459406f, 0.352549126f, 0.352638843f,
+    0.352728556f, 0.352818266f, 0.352907972f, 0.352997676f, 0.353087376f,
+    0.353177073f, 0.353266767f, 0.353356457f, 0.353446145f, 0.353535829f,
+    0.353625509f, 0.353715187f, 0.353804861f, 0.353894532f, 0.353984200f,
+    0.354073864f, 0.354163525f, 0.354253183f, 0.354342838f, 0.354432490f,
+    0.354522138f, 0.354611783f, 0.354701424f, 0.354791063f, 0.354880698f,
+    0.354970330f, 0.355059958f, 0.355149584f, 0.355239206f, 0.355328825f,
+    0.355418440f, 0.355508053f, 0.355597662f, 0.355687267f, 0.355776870f,
+    0.355866469f, 0.355956065f, 0.356045658f, 0.356135247f, 0.356224833f,
+    0.356314416f, 0.356403996f, 0.356493572f, 0.356583145f, 0.356672715f,
+    0.356762281f, 0.356851845f, 0.356941405f, 0.357030961f, 0.357120515f,
+    0.357210065f, 0.357299611f, 0.357389155f, 0.357478695f, 0.357568232f,
+    0.357657766f, 0.357747296f, 0.357836823f, 0.357926347f, 0.358015868f,
+    0.358105385f, 0.358194899f, 0.358284409f, 0.358373917f, 0.358463421f,
+    0.358552921f, 0.358642419f, 0.358731913f, 0.358821404f, 0.358910891f,
+    0.359000376f, 0.359089857f, 0.359179334f, 0.359268809f, 0.359358280f,
+    0.359447747f, 0.359537212f, 0.359626673f, 0.359716131f, 0.359805585f,
+    0.359895037f, 0.359984484f, 0.360073929f, 0.360163370f, 0.360252808f,
+    0.360342243f, 0.360431674f, 0.360521102f, 0.360610527f, 0.360699949f,
+    0.360789367f, 0.360878781f, 0.360968193f, 0.361057601f, 0.361147006f,
+    0.361236407f, 0.361325806f, 0.361415200f, 0.361504592f, 0.361593980f,
+    0.361683365f, 0.361772747f, 0.361862125f, 0.361951500f, 0.362040871f,
+    0.362130240f, 0.362219605f, 0.362308966f, 0.362398325f, 0.362487680f,
+    0.362577031f, 0.362666379f, 0.362755724f, 0.362845066f, 0.362934404f,
+    0.363023739f, 0.363113071f, 0.363202399f, 0.363291724f, 0.363381046f,
+    0.363470364f, 0.363559679f, 0.363648990f, 0.363738299f, 0.363827603f,
+    0.363916905f, 0.364006203f, 0.364095498f, 0.364184790f, 0.364274078f,
+    0.364363363f, 0.364452644f, 0.364541922f, 0.364631197f, 0.364720468f,
+    0.364809736f, 0.364899001f, 0.364988262f, 0.365077520f, 0.365166775f,
+    0.365256026f, 0.365345274f, 0.365434519f, 0.365523760f, 0.365612998f,
+    0.365702232f, 0.365791463f, 0.365880691f, 0.365969916f, 0.366059137f,
+    0.366148354f, 0.366237569f, 0.366326780f, 0.366415987f, 0.366505191f,
+    0.366594392f, 0.366683590f, 0.366772784f, 0.366861974f, 0.366951162f,
+    0.367040346f, 0.367129526f, 0.367218704f, 0.367307877f, 0.367397048f,
+    0.367486215f, 0.367575379f, 0.367664539f, 0.367753696f, 0.367842850f,
+    0.367932000f, 0.368021147f, 0.368110290f, 0.368199430f, 0.368288567f,
+    0.368377700f, 0.368466830f, 0.368555956f, 0.368645080f, 0.368734199f,
+    0.368823316f, 0.368912429f, 0.369001538f, 0.369090644f, 0.369179747f,
+    0.369268847f, 0.369357943f, 0.369447035f, 0.369536124f, 0.369625210f,
+    0.369714293f, 0.369803372f, 0.369892447f, 0.369981519f, 0.370070588f,
+    0.370159654f, 0.370248716f, 0.370337774f, 0.370426829f, 0.370515881f,
+    0.370604930f, 0.370693975f, 0.370783016f, 0.370872054f, 0.370961089f,
+    0.371050120f, 0.371139148f, 0.371228173f, 0.371317194f, 0.371406212f,
+    0.371495226f, 0.371584237f, 0.371673244f, 0.371762248f, 0.371851249f,
+    0.371940246f, 0.372029240f, 0.372118230f, 0.372207217f, 0.372296201f,
+    0.372385181f, 0.372474158f, 0.372563131f, 0.372652101f, 0.372741067f,
+    0.372830030f, 0.372918990f, 0.373007946f, 0.373096898f, 0.373185848f,
+    0.373274793f, 0.373363736f, 0.373452675f, 0.373541610f, 0.373630542f,
+    0.373719471f, 0.373808396f, 0.373897318f, 0.373986237f, 0.374075151f,
+    0.374164063f, 0.374252971f, 0.374341876f, 0.374430777f, 0.374519675f,
+    0.374608569f, 0.374697460f, 0.374786347f, 0.374875231f, 0.374964112f,
+    0.375052989f, 0.375141862f, 0.375230732f, 0.375319599f, 0.375408462f,
+    0.375497322f, 0.375586178f, 0.375675031f, 0.375763881f, 0.375852727f,
+    0.375941569f, 0.376030408f, 0.376119244f, 0.376208076f, 0.376296905f,
+    0.376385730f, 0.376474552f, 0.376563370f, 0.376652185f, 0.376740997f,
+    0.376829805f, 0.376918609f, 0.377007410f, 0.377096208f, 0.377185002f,
+    0.377273793f, 0.377362580f, 0.377451363f, 0.377540144f, 0.377628920f,
+    0.377717694f, 0.377806463f, 0.377895230f, 0.377983993f, 0.378072752f,
+    0.378161508f, 0.378250260f, 0.378339009f, 0.378427755f, 0.378516497f,
+    0.378605235f, 0.378693970f, 0.378782702f, 0.378871430f, 0.378960155f,
+    0.379048876f, 0.379137593f, 0.379226308f, 0.379315018f, 0.379403725f,
+    0.379492429f, 0.379581129f, 0.379669826f, 0.379758519f, 0.379847209f,
+    0.379935895f, 0.380024578f, 0.380113257f, 0.380201933f, 0.380290605f,
+    0.380379274f, 0.380467939f, 0.380556601f, 0.380645259f, 0.380733914f,
+    0.380822565f, 0.380911213f, 0.380999857f, 0.381088498f, 0.381177135f,
+    0.381265769f, 0.381354399f, 0.381443026f, 0.381531650f, 0.381620269f,
+    0.381708885f, 0.381797498f, 0.381886107f, 0.381974713f, 0.382063315f,
+    0.382151914f, 0.382240509f, 0.382329101f, 0.382417689f, 0.382506274f,
+    0.382594855f, 0.382683432f, 0.382772006f, 0.382860577f, 0.382949144f,
+    0.383037708f, 0.383126268f, 0.383214824f, 0.383303377f, 0.383391926f,
+    0.383480472f, 0.383569015f, 0.383657554f, 0.383746089f, 0.383834621f,
+    0.383923149f, 0.384011674f, 0.384100195f, 0.384188713f, 0.384277227f,
+    0.384365737f, 0.384454245f, 0.384542748f, 0.384631248f, 0.384719745f,
+    0.384808238f, 0.384896727f, 0.384985213f, 0.385073695f, 0.385162174f,
+    0.385250649f, 0.385339121f, 0.385427589f, 0.385516054f, 0.385604515f,
+    0.385692972f, 0.385781426f, 0.385869877f, 0.385958324f, 0.386046767f,
+    0.386135207f, 0.386223643f, 0.386312076f, 0.386400505f, 0.386488931f,
+    0.386577353f, 0.386665771f, 0.386754186f, 0.386842598f, 0.386931006f,
+    0.387019410f, 0.387107811f, 0.387196208f, 0.387284601f, 0.387372991f,
+    0.387461378f, 0.387549761f, 0.387638140f, 0.387726516f, 0.387814888f,
+    0.387903257f, 0.387991622f, 0.388079983f, 0.388168341f, 0.388256696f,
+    0.388345047f, 0.388433394f, 0.388521738f, 0.388610078f, 0.388698414f,
+    0.388786747f, 0.388875077f, 0.388963403f, 0.389051725f, 0.389140044f,
+    0.389228359f, 0.389316670f, 0.389404978f, 0.389493282f, 0.389581583f,
+    0.389669880f, 0.389758174f, 0.389846464f, 0.389934751f, 0.390023033f,
+    0.390111313f, 0.390199588f, 0.390287861f, 0.390376129f, 0.390464394f,
+    0.390552655f, 0.390640913f, 0.390729167f, 0.390817418f, 0.390905665f,
+    0.390993908f, 0.391082148f, 0.391170384f, 0.391258617f, 0.391346846f,
+    0.391435071f, 0.391523293f, 0.391611511f, 0.391699726f, 0.391787937f,
+    0.391876144f, 0.391964348f, 0.392052548f, 0.392140745f, 0.392228938f,
+    0.392317128f, 0.392405313f, 0.392493495f, 0.392581674f, 0.392669849f,
+    0.392758020f, 0.392846188f, 0.392934352f, 0.393022513f, 0.393110670f,
+    0.393198823f, 0.393286973f, 0.393375119f, 0.393463261f, 0.393551400f,
+    0.393639535f, 0.393727667f, 0.393815795f, 0.393903919f, 0.393992040f,
+    0.394080157f, 0.394168271f, 0.394256381f, 0.394344487f, 0.394432589f,
+    0.394520688f, 0.394608784f, 0.394696876f, 0.394784964f, 0.394873048f,
+    0.394961129f, 0.395049206f, 0.395137280f, 0.395225350f, 0.395313416f,
+    0.395401479f, 0.395489538f, 0.395577593f, 0.395665645f, 0.395753693f,
+    0.395841738f, 0.395929779f, 0.396017816f, 0.396105850f, 0.396193880f,
+    0.396281906f, 0.396369929f, 0.396457948f, 0.396545963f, 0.396633975f,
+    0.396721983f, 0.396809987f, 0.396897988f, 0.396985985f, 0.397073979f,
+    0.397161969f, 0.397249955f, 0.397337938f, 0.397425916f, 0.397513892f,
+    0.397601863f, 0.397689831f, 0.397777796f, 0.397865756f, 0.397953713f,
+    0.398041666f, 0.398129616f, 0.398217562f, 0.398305504f, 0.398393443f,
+    0.398481378f, 0.398569310f, 0.398657237f, 0.398745161f, 0.398833082f,
+    0.398920998f, 0.399008911f, 0.399096821f, 0.399184726f, 0.399272628f,
+    0.399360527f, 0.399448421f, 0.399536313f, 0.399624200f, 0.399712084f,
+    0.399799964f, 0.399887840f, 0.399975712f, 0.400063581f, 0.400151447f,
+    0.400239308f, 0.400327166f, 0.400415021f, 0.400502871f, 0.400590718f,
+    0.400678561f, 0.400766401f, 0.400854237f, 0.400942069f, 0.401029897f,
+    0.401117722f, 0.401205543f, 0.401293360f, 0.401381174f, 0.401468984f,
+    0.401556791f, 0.401644593f, 0.401732392f, 0.401820187f, 0.401907979f,
+    0.401995767f, 0.402083551f, 0.402171332f, 0.402259108f, 0.402346881f,
+    0.402434651f, 0.402522417f, 0.402610179f, 0.402697937f, 0.402785691f,
+    0.402873442f, 0.402961190f, 0.403048933f, 0.403136673f, 0.403224409f,
+    0.403312141f, 0.403399870f, 0.403487595f, 0.403575316f, 0.403663034f,
+    0.403750747f, 0.403838458f, 0.403926164f, 0.404013867f, 0.404101566f,
+    0.404189261f, 0.404276952f, 0.404364640f, 0.404452324f, 0.404540005f,
+    0.404627681f, 0.404715354f, 0.404803024f, 0.404890689f, 0.404978351f,
+    0.405066009f, 0.405153663f, 0.405241314f, 0.405328961f, 0.405416604f,
+    0.405504244f, 0.405591879f, 0.405679511f, 0.405767140f, 0.405854764f,
+    0.405942385f, 0.406030002f, 0.406117615f, 0.406205225f, 0.406292831f,
+    0.406380433f, 0.406468031f, 0.406555626f, 0.406643217f, 0.406730804f,
+    0.406818388f, 0.406905967f, 0.406993543f, 0.407081115f, 0.407168684f,
+    0.407256249f, 0.407343810f, 0.407431367f, 0.407518920f, 0.407606470f,
+    0.407694016f, 0.407781559f, 0.407869097f, 0.407956632f, 0.408044163f,
+    0.408131690f, 0.408219214f, 0.408306733f, 0.408394249f, 0.408481762f,
+    0.408569270f, 0.408656775f, 0.408744276f, 0.408831773f, 0.408919267f,
+    0.409006756f, 0.409094242f, 0.409181725f, 0.409269203f, 0.409356678f,
+    0.409444149f, 0.409531616f, 0.409619079f, 0.409706539f, 0.409793995f,
+    0.409881447f, 0.409968895f, 0.410056340f, 0.410143781f, 0.410231218f,
+    0.410318651f, 0.410406080f, 0.410493506f, 0.410580928f, 0.410668346f,
+    0.410755760f, 0.410843171f, 0.410930578f, 0.411017981f, 0.411105380f,
+    0.411192776f, 0.411280167f, 0.411367555f, 0.411454940f, 0.411542320f,
+    0.411629697f, 0.411717069f, 0.411804438f, 0.411891804f, 0.411979165f,
+    0.412066523f, 0.412153877f, 0.412241227f, 0.412328573f, 0.412415915f,
+    0.412503254f, 0.412590589f, 0.412677920f, 0.412765248f, 0.412852571f,
+    0.412939891f, 0.413027207f, 0.413114519f, 0.413201827f, 0.413289132f,
+    0.413376433f, 0.413463730f, 0.413551023f, 0.413638312f, 0.413725598f,
+    0.413812880f, 0.413900158f, 0.413987432f, 0.414074702f, 0.414161969f,
+    0.414249231f, 0.414336490f, 0.414423745f, 0.414510997f, 0.414598244f,
+    0.414685488f, 0.414772728f, 0.414859964f, 0.414947196f, 0.415034424f,
+    0.415121649f, 0.415208870f, 0.415296087f, 0.415383300f, 0.415470509f,
+    0.415557715f, 0.415644917f, 0.415732115f, 0.415819309f, 0.415906499f,
+    0.415993685f, 0.416080868f, 0.416168047f, 0.416255222f, 0.416342393f,
+    0.416429560f, 0.416516724f, 0.416603883f, 0.416691039f, 0.416778191f,
+    0.416865339f, 0.416952484f, 0.417039624f, 0.417126761f, 0.417213893f,
+    0.417301022f, 0.417388148f, 0.417475269f, 0.417562386f, 0.417649500f,
+    0.417736610f, 0.417823716f, 0.417910818f, 0.417997916f, 0.418085011f,
+    0.418172101f, 0.418259188f, 0.418346271f, 0.418433350f, 0.418520425f,
+    0.418607497f, 0.418694564f, 0.418781628f, 0.418868688f, 0.418955744f,
+    0.419042796f, 0.419129844f, 0.419216888f, 0.419303929f, 0.419390966f,
+    0.419477998f, 0.419565027f, 0.419652053f, 0.419739074f, 0.419826091f,
+    0.419913105f, 0.420000115f, 0.420087120f, 0.420174122f, 0.420261121f,
+    0.420348115f, 0.420435105f, 0.420522092f, 0.420609074f, 0.420696053f,
+    0.420783028f, 0.420869999f, 0.420956966f, 0.421043930f, 0.421130889f,
+    0.421217845f, 0.421304797f, 0.421391744f, 0.421478688f, 0.421565628f,
+    0.421652565f, 0.421739497f, 0.421826425f, 0.421913350f, 0.422000271f,
+    0.422087188f, 0.422174101f, 0.422261010f, 0.422347915f, 0.422434816f,
+    0.422521714f, 0.422608607f, 0.422695497f, 0.422782383f, 0.422869264f,
+    0.422956142f, 0.423043017f, 0.423129887f, 0.423216753f, 0.423303616f,
+    0.423390474f, 0.423477329f, 0.423564180f, 0.423651026f, 0.423737869f,
+    0.423824709f, 0.423911544f, 0.423998375f, 0.424085202f, 0.424172026f,
+    0.424258846f, 0.424345661f, 0.424432473f, 0.424519281f, 0.424606085f,
+    0.424692885f, 0.424779681f, 0.424866473f, 0.424953262f, 0.425040046f,
+    0.425126827f, 0.425213604f, 0.425300376f, 0.425387145f, 0.425473910f,
+    0.425560671f, 0.425647428f, 0.425734181f, 0.425820931f, 0.425907676f,
+    0.425994418f, 0.426081155f, 0.426167889f, 0.426254618f, 0.426341344f,
+    0.426428066f, 0.426514784f, 0.426601498f, 0.426688208f, 0.426774914f,
+    0.426861617f, 0.426948315f, 0.427035009f, 0.427121700f, 0.427208386f,
+    0.427295069f, 0.427381748f, 0.427468423f, 0.427555093f, 0.427641760f,
+    0.427728423f, 0.427815082f, 0.427901738f, 0.427988389f, 0.428075036f,
+    0.428161679f, 0.428248319f, 0.428334954f, 0.428421586f, 0.428508213f,
+    0.428594837f, 0.428681457f, 0.428768072f, 0.428854684f, 0.428941292f,
+    0.429027896f, 0.429114496f, 0.429201092f, 0.429287684f, 0.429374272f,
+    0.429460856f, 0.429547437f, 0.429634013f, 0.429720585f, 0.429807154f,
+    0.429893718f, 0.429980279f, 0.430066835f, 0.430153388f, 0.430239937f,
+    0.430326481f, 0.430413022f, 0.430499559f, 0.430586092f, 0.430672621f,
+    0.430759145f, 0.430845666f, 0.430932183f, 0.431018696f, 0.431105206f,
+    0.431191711f, 0.431278212f, 0.431364709f, 0.431451202f, 0.431537691f,
+    0.431624177f, 0.431710658f, 0.431797135f, 0.431883609f, 0.431970078f,
+    0.432056544f, 0.432143005f, 0.432229463f, 0.432315916f, 0.432402366f,
+    0.432488811f, 0.432575253f, 0.432661690f, 0.432748124f, 0.432834554f,
+    0.432920979f, 0.433007401f, 0.433093819f, 0.433180233f, 0.433266642f,
+    0.433353048f, 0.433439450f, 0.433525848f, 0.433612242f, 0.433698631f,
+    0.433785017f, 0.433871399f, 0.433957777f, 0.434044151f, 0.434130521f,
+    0.434216887f, 0.434303249f, 0.434389607f, 0.434475961f, 0.434562311f,
+    0.434648656f, 0.434734998f, 0.434821336f, 0.434907670f, 0.434994000f,
+    0.435080326f, 0.435166648f, 0.435252966f, 0.435339280f, 0.435425590f,
+    0.435511896f, 0.435598198f, 0.435684496f, 0.435770790f, 0.435857080f,
+    0.435943366f, 0.436029648f, 0.436115926f, 0.436202200f, 0.436288470f,
+    0.436374735f, 0.436460997f, 0.436547255f, 0.436633509f, 0.436719759f,
+    0.436806005f, 0.436892247f, 0.436978484f, 0.437064718f, 0.437150948f,
+    0.437237174f, 0.437323395f, 0.437409613f, 0.437495827f, 0.437582036f,
+    0.437668242f, 0.437754444f, 0.437840641f, 0.437926835f, 0.438013024f,
+    0.438099210f, 0.438185391f, 0.438271569f, 0.438357742f, 0.438443912f,
+    0.438530077f, 0.438616239f, 0.438702396f, 0.438788549f, 0.438874698f,
+    0.438960844f, 0.439046985f, 0.439133122f, 0.439219255f, 0.439305384f,
+    0.439391509f, 0.439477630f, 0.439563747f, 0.439649860f, 0.439735969f,
+    0.439822074f, 0.439908175f, 0.439994271f, 0.440080364f, 0.440166453f,
+    0.440252537f, 0.440338618f, 0.440424694f, 0.440510767f, 0.440596835f,
+    0.440682900f, 0.440768960f, 0.440855016f, 0.440941068f, 0.441027117f,
+    0.441113161f, 0.441199201f, 0.441285237f, 0.441371269f, 0.441457297f,
+    0.441543320f, 0.441629340f, 0.441715356f, 0.441801368f, 0.441887375f,
+    0.441973379f, 0.442059378f, 0.442145374f, 0.442231365f, 0.442317352f,
+    0.442403335f, 0.442489315f, 0.442575290f, 0.442661261f, 0.442747228f,
+    0.442833190f, 0.442919149f, 0.443005104f, 0.443091055f, 0.443177001f,
+    0.443262944f, 0.443348882f, 0.443434816f, 0.443520747f, 0.443606673f,
+    0.443692595f, 0.443778513f, 0.443864427f, 0.443950337f, 0.444036243f,
+    0.444122145f, 0.444208042f, 0.444293936f, 0.444379825f, 0.444465711f,
+    0.444551592f, 0.444637469f, 0.444723342f, 0.444809211f, 0.444895076f,
+    0.444980937f, 0.445066794f, 0.445152647f, 0.445238495f, 0.445324340f,
+    0.445410180f, 0.445496017f, 0.445581849f, 0.445667677f, 0.445753501f,
+    0.445839321f, 0.445925137f, 0.446010948f, 0.446096756f, 0.446182560f,
+    0.446268359f, 0.446354154f, 0.446439946f, 0.446525733f, 0.446611516f,
+    0.446697295f, 0.446783069f, 0.446868840f, 0.446954607f, 0.447040369f,
+    0.447126128f, 0.447211882f, 0.447297632f, 0.447383378f, 0.447469120f,
+    0.447554858f, 0.447640592f, 0.447726321f, 0.447812047f, 0.447897768f,
+    0.447983485f, 0.448069198f, 0.448154907f, 0.448240612f, 0.448326313f,
+    0.448412010f, 0.448497702f, 0.448583391f, 0.448669075f, 0.448754755f,
+    0.448840431f, 0.448926103f, 0.449011771f, 0.449097434f, 0.449183094f,
+    0.449268749f, 0.449354401f, 0.449440048f, 0.449525691f, 0.449611330f,
+    0.449696964f, 0.449782595f, 0.449868221f, 0.449953844f, 0.450039462f,
+    0.450125076f, 0.450210686f, 0.450296292f, 0.450381893f, 0.450467491f,
+    0.450553084f, 0.450638674f, 0.450724259f, 0.450809840f, 0.450895416f,
+    0.450980989f, 0.451066558f, 0.451152122f, 0.451237682f, 0.451323238f,
+    0.451408790f, 0.451494338f, 0.451579882f, 0.451665421f, 0.451750956f,
+    0.451836487f, 0.451922014f, 0.452007537f, 0.452093056f, 0.452178571f,
+    0.452264081f, 0.452349587f, 0.452435089f, 0.452520587f, 0.452606081f,
+    0.452691571f, 0.452777056f, 0.452862537f, 0.452948014f, 0.453033487f,
+    0.453118956f, 0.453204421f, 0.453289881f, 0.453375338f, 0.453460790f,
+    0.453546238f, 0.453631681f, 0.453717121f, 0.453802556f, 0.453887988f,
+    0.453973415f, 0.454058838f, 0.454144257f, 0.454229671f, 0.454315081f,
+    0.454400488f, 0.454485890f, 0.454571288f, 0.454656681f, 0.454742071f,
+    0.454827456f, 0.454912837f, 0.454998214f, 0.455083587f, 0.455168956f,
+    0.455254320f, 0.455339680f, 0.455425036f, 0.455510388f, 0.455595736f,
+    0.455681080f, 0.455766419f, 0.455851754f, 0.455937085f, 0.456022412f,
+    0.456107734f, 0.456193053f, 0.456278367f, 0.456363677f, 0.456448982f,
+    0.456534284f, 0.456619581f, 0.456704875f, 0.456790164f, 0.456875448f,
+    0.456960729f, 0.457046005f, 0.457131277f, 0.457216545f, 0.457301809f,
+    0.457387069f, 0.457472324f, 0.457557575f, 0.457642822f, 0.457728065f,
+    0.457813304f, 0.457898538f, 0.457983768f, 0.458068994f, 0.458154216f,
+    0.458239433f, 0.458324646f, 0.458409856f, 0.458495060f, 0.458580261f,
+    0.458665457f, 0.458750650f, 0.458835838f, 0.458921021f, 0.459006201f,
+    0.459091376f, 0.459176548f, 0.459261714f, 0.459346877f, 0.459432036f,
+    0.459517190f, 0.459602340f, 0.459687486f, 0.459772627f, 0.459857765f,
+    0.459942898f, 0.460028026f, 0.460113151f, 0.460198272f, 0.460283388f,
+    0.460368500f, 0.460453607f, 0.460538711f, 0.460623810f, 0.460708905f,
+    0.460793996f, 0.460879083f, 0.460964165f, 0.461049243f, 0.461134317f,
+    0.461219386f, 0.461304452f, 0.461389513f, 0.461474570f, 0.461559623f,
+    0.461644671f, 0.461729715f, 0.461814755f, 0.461899791f, 0.461984822f,
+    0.462069849f, 0.462154872f, 0.462239891f, 0.462324905f, 0.462409916f,
+    0.462494922f, 0.462579923f, 0.462664921f, 0.462749914f, 0.462834903f,
+    0.462919887f, 0.463004868f, 0.463089844f, 0.463174816f, 0.463259784f,
+    0.463344747f, 0.463429706f, 0.463514661f, 0.463599612f, 0.463684558f,
+    0.463769500f, 0.463854438f, 0.463939371f, 0.464024301f, 0.464109226f,
+    0.464194146f, 0.464279063f, 0.464363975f, 0.464448883f, 0.464533787f,
+    0.464618686f, 0.464703581f, 0.464788472f, 0.464873359f, 0.464958241f,
+    0.465043119f, 0.465127993f, 0.465212863f, 0.465297728f, 0.465382589f,
+    0.465467446f, 0.465552298f, 0.465637146f, 0.465721990f, 0.465806829f,
+    0.465891665f, 0.465976496f, 0.466061322f, 0.466146145f, 0.466230963f,
+    0.466315777f, 0.466400587f, 0.466485392f, 0.466570193f, 0.466654990f,
+    0.466739782f, 0.466824570f, 0.466909354f, 0.466994133f, 0.467078909f,
+    0.467163680f, 0.467248446f, 0.467333209f, 0.467417967f, 0.467502721f,
+    0.467587470f, 0.467672215f, 0.467756956f, 0.467841693f, 0.467926425f,
+    0.468011153f, 0.468095877f, 0.468180596f, 0.468265311f, 0.468350022f,
+    0.468434728f, 0.468519431f, 0.468604128f, 0.468688822f, 0.468773511f,
+    0.468858196f, 0.468942877f, 0.469027553f, 0.469112225f, 0.469196893f,
+    0.469281556f, 0.469366215f, 0.469450870f, 0.469535520f, 0.469620167f,
+    0.469704808f, 0.469789446f, 0.469874079f, 0.469958708f, 0.470043332f,
+    0.470127953f, 0.470212569f, 0.470297180f, 0.470381787f, 0.470466390f,
+    0.470550989f, 0.470635583f, 0.470720173f, 0.470804759f, 0.470889340f,
+    0.470973917f, 0.471058490f, 0.471143058f, 0.471227622f, 0.471312182f,
+    0.471396737f, 0.471481288f, 0.471565834f, 0.471650377f, 0.471734915f,
+    0.471819448f, 0.471903978f, 0.471988503f, 0.472073023f, 0.472157540f,
+    0.472242051f, 0.472326559f, 0.472411062f, 0.472495561f, 0.472580056f,
+    0.472664546f, 0.472749032f, 0.472833513f, 0.472917991f, 0.473002464f,
+    0.473086932f, 0.473171396f, 0.473255856f, 0.473340311f, 0.473424763f,
+    0.473509209f, 0.473593652f, 0.473678090f, 0.473762523f, 0.473846953f,
+    0.473931378f, 0.474015798f, 0.474100215f, 0.474184627f, 0.474269034f,
+    0.474353437f, 0.474437836f, 0.474522231f, 0.474606621f, 0.474691006f,
+    0.474775388f, 0.474859765f, 0.474944138f, 0.475028506f, 0.475112870f,
+    0.475197229f, 0.475281584f, 0.475365935f, 0.475450282f, 0.475534624f,
+    0.475618962f, 0.475703295f, 0.475787624f, 0.475871948f, 0.475956269f,
+    0.476040584f, 0.476124896f, 0.476209203f, 0.476293506f, 0.476377804f,
+    0.476462098f, 0.476546388f, 0.476630673f, 0.476714954f, 0.476799230f,
+    0.476883502f, 0.476967770f, 0.477052033f, 0.477136292f, 0.477220546f,
+    0.477304797f, 0.477389042f, 0.477473284f, 0.477557521f, 0.477641753f,
+    0.477725981f, 0.477810205f, 0.477894425f, 0.477978640f, 0.478062850f,
+    0.478147056f, 0.478231258f, 0.478315456f, 0.478399649f, 0.478483837f,
+    0.478568022f, 0.478652201f, 0.478736377f, 0.478820548f, 0.478904715f,
+    0.478988877f, 0.479073035f, 0.479157188f, 0.479241337f, 0.479325482f,
+    0.479409622f, 0.479493758f, 0.479577889f, 0.479662016f, 0.479746139f,
+    0.479830257f, 0.479914371f, 0.479998480f, 0.480082585f, 0.480166685f,
+    0.480250781f, 0.480334873f, 0.480418960f, 0.480503043f, 0.480587122f,
+    0.480671196f, 0.480755265f, 0.480839331f, 0.480923391f, 0.481007448f,
+    0.481091500f, 0.481175547f, 0.481259590f, 0.481343629f, 0.481427663f,
+    0.481511693f, 0.481595718f, 0.481679739f, 0.481763756f, 0.481847768f,
+    0.481931776f, 0.482015779f, 0.482099778f, 0.482183772f, 0.482267762f,
+    0.482351748f, 0.482435729f, 0.482519705f, 0.482603678f, 0.482687645f,
+    0.482771609f, 0.482855568f, 0.482939522f, 0.483023472f, 0.483107418f,
+    0.483191359f, 0.483275295f, 0.483359228f, 0.483443156f, 0.483527079f,
+    0.483610998f, 0.483694912f, 0.483778822f, 0.483862728f, 0.483946629f,
+    0.484030526f, 0.484114418f, 0.484198306f, 0.484282189f, 0.484366068f,
+    0.484449943f, 0.484533813f, 0.484617678f, 0.484701539f, 0.484785396f,
+    0.484869248f, 0.484953096f, 0.485036939f, 0.485120778f, 0.485204612f,
+    0.485288442f, 0.485372267f, 0.485456088f, 0.485539905f, 0.485623717f,
+    0.485707524f, 0.485791328f, 0.485875126f, 0.485958920f, 0.486042710f,
+    0.486126495f, 0.486210276f, 0.486294052f, 0.486377824f, 0.486461592f,
+    0.486545355f, 0.486629113f, 0.486712867f, 0.486796616f, 0.486880361f,
+    0.486964102f, 0.487047838f, 0.487131569f, 0.487215297f, 0.487299019f,
+    0.487382737f, 0.487466451f, 0.487550160f, 0.487633865f, 0.487717565f,
+    0.487801261f, 0.487884952f, 0.487968639f, 0.488052321f, 0.488135999f,
+    0.488219672f, 0.488303341f, 0.488387005f, 0.488470665f, 0.488554320f,
+    0.488637971f, 0.488721618f, 0.488805260f, 0.488888897f, 0.488972530f,
+    0.489056158f, 0.489139782f, 0.489223401f, 0.489307016f, 0.489390627f,
+    0.489474233f, 0.489557834f, 0.489641431f, 0.489725023f, 0.489808611f,
+    0.489892195f, 0.489975774f, 0.490059348f, 0.490142918f, 0.490226483f,
+    0.490310044f, 0.490393601f, 0.490477152f, 0.490560700f, 0.490644243f,
+    0.490727781f, 0.490811315f, 0.490894844f, 0.490978369f, 0.491061889f,
+    0.491145405f, 0.491228916f, 0.491312423f, 0.491395925f, 0.491479423f,
+    0.491562916f, 0.491646405f, 0.491729889f, 0.491813369f, 0.491896844f,
+    0.491980314f, 0.492063780f, 0.492147242f, 0.492230699f, 0.492314151f,
+    0.492397599f, 0.492481043f, 0.492564482f, 0.492647916f, 0.492731346f,
+    0.492814771f, 0.492898192f, 0.492981609f, 0.493065020f, 0.493148427f,
+    0.493231830f, 0.493315228f, 0.493398622f, 0.493482011f, 0.493565396f,
+    0.493648776f, 0.493732151f, 0.493815522f, 0.493898888f, 0.493982250f,
+    0.494065608f, 0.494148960f, 0.494232309f, 0.494315652f, 0.494398991f,
+    0.494482326f, 0.494565656f, 0.494648982f, 0.494732302f, 0.494815619f,
+    0.494898931f, 0.494982238f, 0.495065541f, 0.495148839f, 0.495232133f,
+    0.495315422f, 0.495398706f, 0.495481986f, 0.495565262f, 0.495648533f,
+    0.495731799f, 0.495815061f, 0.495898318f, 0.495981571f, 0.496064819f,
+    0.496148062f, 0.496231301f, 0.496314536f, 0.496397766f, 0.496480991f,
+    0.496564212f, 0.496647428f, 0.496730640f, 0.496813847f, 0.496897049f,
+    0.496980247f, 0.497063440f, 0.497146629f, 0.497229813f, 0.497312993f,
+    0.497396168f, 0.497479338f, 0.497562504f, 0.497645666f, 0.497728822f,
+    0.497811975f, 0.497895122f, 0.497978265f, 0.498061404f, 0.498144538f,
+    0.498227667f, 0.498310792f, 0.498393912f, 0.498477027f, 0.498560138f,
+    0.498643245f, 0.498726347f, 0.498809444f, 0.498892537f, 0.498975625f,
+    0.499058708f, 0.499141787f, 0.499224861f, 0.499307931f, 0.499390996f,
+    0.499474057f, 0.499557113f, 0.499640164f, 0.499723211f, 0.499806253f,
+    0.499889290f, 0.499972323f, 0.500055352f, 0.500138376f, 0.500221395f,
+    0.500304409f, 0.500387419f, 0.500470425f, 0.500553425f, 0.500636422f,
+    0.500719413f, 0.500802400f, 0.500885383f, 0.500968360f, 0.501051334f,
+    0.501134302f, 0.501217266f, 0.501300225f, 0.501383180f, 0.501466130f,
+    0.501549076f, 0.501632017f, 0.501714953f, 0.501797885f, 0.501880812f,
+    0.501963734f, 0.502046652f, 0.502129565f, 0.502212474f, 0.502295378f,
+    0.502378277f, 0.502461172f, 0.502544062f, 0.502626948f, 0.502709829f,
+    0.502792705f, 0.502875577f, 0.502958444f, 0.503041306f, 0.503124164f,
+    0.503207017f, 0.503289866f, 0.503372710f, 0.503455549f, 0.503538384f,
+    0.503621214f, 0.503704039f, 0.503786860f, 0.503869676f, 0.503952488f,
+    0.504035295f, 0.504118097f, 0.504200894f, 0.504283687f, 0.504366476f,
+    0.504449259f, 0.504532039f, 0.504614813f, 0.504697583f, 0.504780348f,
+    0.504863109f, 0.504945864f, 0.505028616f, 0.505111362f, 0.505194104f,
+    0.505276842f, 0.505359574f, 0.505442302f, 0.505525026f, 0.505607744f,
+    0.505690458f, 0.505773168f, 0.505855873f, 0.505938573f, 0.506021268f,
+    0.506103959f, 0.506186645f, 0.506269327f, 0.506352004f, 0.506434676f,
+    0.506517344f, 0.506600006f, 0.506682665f, 0.506765318f, 0.506847967f,
+    0.506930612f, 0.507013251f, 0.507095886f, 0.507178516f, 0.507261142f,
+    0.507343763f, 0.507426379f, 0.507508991f, 0.507591598f, 0.507674200f,
+    0.507756798f, 0.507839391f, 0.507921979f, 0.508004563f, 0.508087142f,
+    0.508169716f, 0.508252286f, 0.508334851f, 0.508417411f, 0.508499967f,
+    0.508582518f, 0.508665064f, 0.508747606f, 0.508830143f, 0.508912675f,
+    0.508995202f, 0.509077725f, 0.509160243f, 0.509242757f, 0.509325266f,
+    0.509407770f, 0.509490269f, 0.509572764f, 0.509655254f, 0.509737740f,
+    0.509820221f, 0.509902697f, 0.509985168f, 0.510067635f, 0.510150097f,
+    0.510232554f, 0.510315007f, 0.510397455f, 0.510479898f, 0.510562336f,
+    0.510644770f, 0.510727199f, 0.510809624f, 0.510892044f, 0.510974459f,
+    0.511056869f, 0.511139275f, 0.511221676f, 0.511304072f, 0.511386464f,
+    0.511468850f, 0.511551233f, 0.511633610f, 0.511715983f, 0.511798351f,
+    0.511880714f, 0.511963073f, 0.512045427f, 0.512127776f, 0.512210121f,
+    0.512292461f, 0.512374796f, 0.512457126f, 0.512539452f, 0.512621773f,
+    0.512704089f, 0.512786401f, 0.512868707f, 0.512951010f, 0.513033307f,
+    0.513115600f, 0.513197888f, 0.513280171f, 0.513362450f, 0.513444723f,
+    0.513526993f, 0.513609257f, 0.513691517f, 0.513773772f, 0.513856022f,
+    0.513938267f, 0.514020508f, 0.514102744f, 0.514184976f, 0.514267202f,
+    0.514349424f, 0.514431641f, 0.514513854f, 0.514596061f, 0.514678264f,
+    0.514760463f, 0.514842656f, 0.514924845f, 0.515007029f, 0.515089208f,
+    0.515171383f, 0.515253553f, 0.515335718f, 0.515417878f, 0.515500034f,
+    0.515582185f, 0.515664331f, 0.515746472f, 0.515828609f, 0.515910741f,
+    0.515992868f, 0.516074990f, 0.516157108f, 0.516239221f, 0.516321329f,
+    0.516403433f, 0.516485531f, 0.516567625f, 0.516649715f, 0.516731799f,
+    0.516813879f, 0.516895954f, 0.516978024f, 0.517060089f, 0.517142150f,
+    0.517224206f, 0.517306257f, 0.517388304f, 0.517470345f, 0.517552382f,
+    0.517634415f, 0.517716442f, 0.517798465f, 0.517880483f, 0.517962496f,
+    0.518044504f, 0.518126508f, 0.518208507f, 0.518290501f, 0.518372490f,
+    0.518454475f, 0.518536454f, 0.518618429f, 0.518700400f, 0.518782365f,
+    0.518864326f, 0.518946282f, 0.519028233f, 0.519110180f, 0.519192121f,
+    0.519274058f, 0.519355990f, 0.519437917f, 0.519519840f, 0.519601758f,
+    0.519683671f, 0.519765579f, 0.519847483f, 0.519929381f, 0.520011275f,
+    0.520093164f, 0.520175049f, 0.520256928f, 0.520338803f, 0.520420673f,
+    0.520502538f, 0.520584399f, 0.520666254f, 0.520748105f, 0.520829951f,
+    0.520911792f, 0.520993629f, 0.521075461f, 0.521157287f, 0.521239110f,
+    0.521320927f, 0.521402739f, 0.521484547f, 0.521566350f, 0.521648148f,
+    0.521729942f, 0.521811730f, 0.521893514f, 0.521975293f, 0.522057067f,
+    0.522138836f, 0.522220601f, 0.522302361f, 0.522384116f, 0.522465866f,
+    0.522547611f, 0.522629352f, 0.522711088f, 0.522792819f, 0.522874545f,
+    0.522956266f, 0.523037983f, 0.523119694f, 0.523201401f, 0.523283103f,
+    0.523364801f, 0.523446493f, 0.523528181f, 0.523609864f, 0.523691542f,
+    0.523773215f, 0.523854884f, 0.523936547f, 0.524018206f, 0.524099860f,
+    0.524181509f, 0.524263153f, 0.524344793f, 0.524426428f, 0.524508058f,
+    0.524589683f, 0.524671303f, 0.524752918f, 0.524834529f, 0.524916135f,
+    0.524997736f, 0.525079332f, 0.525160923f, 0.525242510f, 0.525324091f,
+    0.525405668f, 0.525487240f, 0.525568807f, 0.525650369f, 0.525731927f,
+    0.525813480f, 0.525895027f, 0.525976570f, 0.526058109f, 0.526139642f,
+    0.526221170f, 0.526302694f, 0.526384213f, 0.526465727f, 0.526547236f,
+    0.526628740f, 0.526710240f, 0.526791734f, 0.526873224f, 0.526954709f,
+    0.527036189f, 0.527117664f, 0.527199135f, 0.527280600f, 0.527362061f,
+    0.527443517f, 0.527524968f, 0.527606414f, 0.527687855f, 0.527769292f,
+    0.527850723f, 0.527932150f, 0.528013572f, 0.528094989f, 0.528176401f,
+    0.528257809f, 0.528339211f, 0.528420609f, 0.528502002f, 0.528583389f,
+    0.528664773f, 0.528746151f, 0.528827524f, 0.528908893f, 0.528990256f,
+    0.529071615f, 0.529152969f, 0.529234318f, 0.529315662f, 0.529397001f,
+    0.529478336f, 0.529559665f, 0.529640990f, 0.529722310f, 0.529803625f,
+    0.529884935f, 0.529966240f, 0.530047540f, 0.530128836f, 0.530210126f,
+    0.530291412f, 0.530372693f, 0.530453969f, 0.530535240f, 0.530616506f,
+    0.530697768f, 0.530779024f, 0.530860276f, 0.530941522f, 0.531022764f,
+    0.531104001f, 0.531185233f, 0.531266460f, 0.531347683f, 0.531428900f,
+    0.531510113f, 0.531591320f, 0.531672523f, 0.531753721f, 0.531834914f,
+    0.531916102f, 0.531997285f, 0.532078464f, 0.532159637f, 0.532240805f,
+    0.532321969f, 0.532403128f, 0.532484282f, 0.532565431f, 0.532646575f,
+    0.532727714f, 0.532808848f, 0.532889978f, 0.532971102f, 0.533052222f,
+    0.533133336f, 0.533214446f, 0.533295551f, 0.533376651f, 0.533457746f,
+    0.533538836f, 0.533619921f, 0.533701002f, 0.533782077f, 0.533863148f,
+    0.533944213f, 0.534025274f, 0.534106330f, 0.534187381f, 0.534268427f,
+    0.534349468f, 0.534430504f, 0.534511535f, 0.534592562f, 0.534673583f,
+    0.534754600f, 0.534835611f, 0.534916618f, 0.534997620f, 0.535078617f,
+    0.535159609f, 0.535240596f, 0.535321578f, 0.535402555f, 0.535483527f,
+    0.535564495f, 0.535645457f, 0.535726415f, 0.535807367f, 0.535888315f,
+    0.535969257f, 0.536050195f, 0.536131128f, 0.536212056f, 0.536292979f,
+    0.536373897f, 0.536454810f, 0.536535719f, 0.536616622f, 0.536697520f,
+    0.536778414f, 0.536859302f, 0.536940186f, 0.537021064f, 0.537101938f,
+    0.537182807f, 0.537263670f, 0.537344529f, 0.537425383f, 0.537506232f,
+    0.537587076f, 0.537667915f, 0.537748750f, 0.537829579f, 0.537910403f,
+    0.537991222f, 0.538072037f, 0.538152846f, 0.538233651f, 0.538314450f,
+    0.538395245f, 0.538476035f, 0.538556819f, 0.538637599f, 0.538718374f,
+    0.538799144f, 0.538879909f, 0.538960668f, 0.539041423f, 0.539122173f,
+    0.539202919f, 0.539283659f, 0.539364394f, 0.539445124f, 0.539525849f,
+    0.539606570f, 0.539687285f, 0.539767995f, 0.539848701f, 0.539929401f,
+    0.540010097f, 0.540090787f, 0.540171473f, 0.540252153f, 0.540332829f,
+    0.540413500f, 0.540494165f, 0.540574826f, 0.540655482f, 0.540736133f,
+    0.540816778f, 0.540897419f, 0.540978055f, 0.541058686f, 0.541139312f,
+    0.541219933f, 0.541300549f, 0.541381160f, 0.541461766f, 0.541542367f,
+    0.541622963f, 0.541703554f, 0.541784140f, 0.541864721f, 0.541945297f,
+    0.542025869f, 0.542106435f, 0.542186996f, 0.542267552f, 0.542348103f,
+    0.542428650f, 0.542509191f, 0.542589727f, 0.542670259f, 0.542750785f,
+    0.542831306f, 0.542911823f, 0.542992334f, 0.543072840f, 0.543153342f,
+    0.543233838f, 0.543314329f, 0.543394816f, 0.543475297f, 0.543555773f,
+    0.543636245f, 0.543716711f, 0.543797173f, 0.543877629f, 0.543958080f,
+    0.544038527f, 0.544118968f, 0.544199405f, 0.544279836f, 0.544360262f,
+    0.544440684f, 0.544521100f, 0.544601511f, 0.544681918f, 0.544762319f,
+    0.544842716f, 0.544923107f, 0.545003493f, 0.545083875f, 0.545164251f,
+    0.545244622f, 0.545324988f, 0.545405350f, 0.545485706f, 0.545566057f,
+    0.545646403f, 0.545726745f, 0.545807081f, 0.545887412f, 0.545967738f,
+    0.546048059f, 0.546128376f, 0.546208687f, 0.546288993f, 0.546369294f,
+    0.546449590f, 0.546529881f, 0.546610167f, 0.546690448f, 0.546770724f,
+    0.546850995f, 0.546931261f, 0.547011522f, 0.547091777f, 0.547172028f,
+    0.547252274f, 0.547332515f, 0.547412750f, 0.547492981f, 0.547573207f,
+    0.547653427f, 0.547733643f, 0.547813854f, 0.547894059f, 0.547974260f,
+    0.548054455f, 0.548134646f, 0.548214831f, 0.548295011f, 0.548375187f,
+    0.548455357f, 0.548535522f, 0.548615682f, 0.548695837f, 0.548775987f,
+    0.548856132f, 0.548936272f, 0.549016407f, 0.549096537f, 0.549176662f,
+    0.549256782f, 0.549336897f, 0.549417006f, 0.549497111f, 0.549577211f,
+    0.549657305f, 0.549737395f, 0.549817479f, 0.549897559f, 0.549977633f,
+    0.550057702f, 0.550137767f, 0.550217826f, 0.550297880f, 0.550377929f,
+    0.550457973f, 0.550538012f, 0.550618046f, 0.550698075f, 0.550778098f,
+    0.550858117f, 0.550938131f, 0.551018139f, 0.551098143f, 0.551178141f,
+    0.551258135f, 0.551338123f, 0.551418106f, 0.551498084f, 0.551578057f,
+    0.551658025f, 0.551737988f, 0.551817946f, 0.551897899f, 0.551977847f,
+    0.552057790f, 0.552137727f, 0.552217660f, 0.552297587f, 0.552377509f,
+    0.552457427f, 0.552537339f, 0.552617246f, 0.552697148f, 0.552777045f,
+    0.552856937f, 0.552936824f, 0.553016706f, 0.553096582f, 0.553176454f,
+    0.553256320f, 0.553336182f, 0.553416038f, 0.553495889f, 0.553575735f,
+    0.553655576f, 0.553735412f, 0.553815243f, 0.553895069f, 0.553974890f,
+    0.554054705f, 0.554134516f, 0.554214321f, 0.554294121f, 0.554373917f,
+    0.554453707f, 0.554533492f, 0.554613272f, 0.554693047f, 0.554772816f,
+    0.554852581f, 0.554932340f, 0.555012095f, 0.555091844f, 0.555171588f,
+    0.555251328f, 0.555331062f, 0.555410791f, 0.555490514f, 0.555570233f,
+    0.555649947f, 0.555729655f, 0.555809358f, 0.555889057f, 0.555968750f,
+    0.556048438f, 0.556128121f, 0.556207799f, 0.556287471f, 0.556367139f,
+    0.556446802f, 0.556526459f, 0.556606111f, 0.556685758f, 0.556765400f,
+    0.556845037f, 0.556924669f, 0.557004296f, 0.557083917f, 0.557163534f,
+    0.557243145f, 0.557322751f, 0.557402352f, 0.557481948f, 0.557561539f,
+    0.557641125f, 0.557720705f, 0.557800281f, 0.557879851f, 0.557959416f,
+    0.558038976f, 0.558118531f, 0.558198081f, 0.558277626f, 0.558357165f,
+    0.558436700f, 0.558516229f, 0.558595753f, 0.558675272f, 0.558754786f,
+    0.558834295f, 0.558913798f, 0.558993297f, 0.559072790f, 0.559152278f,
+    0.559231761f, 0.559311239f, 0.559390712f, 0.559470179f, 0.559549642f,
+    0.559629099f, 0.559708551f, 0.559787999f, 0.559867440f, 0.559946877f,
+    0.560026309f, 0.560105735f, 0.560185157f, 0.560264573f, 0.560343984f,
+    0.560423390f, 0.560502790f, 0.560582186f, 0.560661576f, 0.560740961f,
+    0.560820342f, 0.560899716f, 0.560979086f, 0.561058451f, 0.561137810f,
+    0.561217165f, 0.561296514f, 0.561375858f, 0.561455197f, 0.561534530f,
+    0.561613859f, 0.561693182f, 0.561772500f, 0.561851813f, 0.561931121f,
+    0.562010424f, 0.562089721f, 0.562169014f, 0.562248301f, 0.562327583f,
+    0.562406860f, 0.562486132f, 0.562565398f, 0.562644659f, 0.562723916f,
+    0.562803167f, 0.562882412f, 0.562961653f, 0.563040889f, 0.563120119f,
+    0.563199344f, 0.563278564f, 0.563357779f, 0.563436988f, 0.563516193f,
+    0.563595392f, 0.563674586f, 0.563753775f, 0.563832959f, 0.563912137f,
+    0.563991310f, 0.564070479f, 0.564149642f, 0.564228799f, 0.564307952f,
+    0.564387099f, 0.564466242f, 0.564545379f, 0.564624510f, 0.564703637f,
+    0.564782758f, 0.564861875f, 0.564940986f, 0.565020092f, 0.565099192f,
+    0.565178288f, 0.565257378f, 0.565336463f, 0.565415543f, 0.565494618f,
+    0.565573687f, 0.565652752f, 0.565731811f, 0.565810865f, 0.565889913f,
+    0.565968957f, 0.566047995f, 0.566127028f, 0.566206056f, 0.566285079f,
+    0.566364096f, 0.566443109f, 0.566522116f, 0.566601118f, 0.566680114f,
+    0.566759106f, 0.566838092f, 0.566917073f, 0.566996049f, 0.567075019f,
+    0.567153985f, 0.567232945f, 0.567311900f, 0.567390850f, 0.567469794f,
+    0.567548734f, 0.567627668f, 0.567706597f, 0.567785520f, 0.567864439f,
+    0.567943352f, 0.568022260f, 0.568101163f, 0.568180060f, 0.568258953f,
+    0.568337840f, 0.568416722f, 0.568495598f, 0.568574470f, 0.568653336f,
+    0.568732197f, 0.568811053f, 0.568889903f, 0.568968749f, 0.569047589f,
+    0.569126424f, 0.569205253f, 0.569284078f, 0.569362897f, 0.569441711f,
+    0.569520519f, 0.569599323f, 0.569678121f, 0.569756914f, 0.569835702f,
+    0.569914484f, 0.569993262f, 0.570072034f, 0.570150800f, 0.570229562f,
+    0.570308318f, 0.570387069f, 0.570465815f, 0.570544556f, 0.570623291f,
+    0.570702021f, 0.570780746f, 0.570859465f, 0.570938180f, 0.571016889f,
+    0.571095593f, 0.571174291f, 0.571252985f, 0.571331673f, 0.571410356f,
+    0.571489033f, 0.571567706f, 0.571646373f, 0.571725035f, 0.571803691f,
+    0.571882342f, 0.571960989f, 0.572039629f, 0.572118265f, 0.572196895f,
+    0.572275520f, 0.572354140f, 0.572432754f, 0.572511364f, 0.572589968f,
+    0.572668566f, 0.572747160f, 0.572825748f, 0.572904331f, 0.572982909f,
+    0.573061481f, 0.573140048f, 0.573218610f, 0.573297167f, 0.573375718f,
+    0.573454264f, 0.573532805f, 0.573611340f, 0.573689871f, 0.573768396f,
+    0.573846915f, 0.573925430f, 0.574003939f, 0.574082443f, 0.574160941f,
+    0.574239435f, 0.574317923f, 0.574396405f, 0.574474883f, 0.574553355f,
+    0.574631822f, 0.574710284f, 0.574788740f, 0.574867191f, 0.574945637f,
+    0.575024077f, 0.575102512f, 0.575180942f, 0.575259367f, 0.575337786f,
+    0.575416200f, 0.575494609f, 0.575573013f, 0.575651411f, 0.575729804f,
+    0.575808191f, 0.575886574f, 0.575964951f, 0.576043322f, 0.576121689f,
+    0.576200050f, 0.576278406f, 0.576356756f, 0.576435102f, 0.576513442f,
+    0.576591776f, 0.576670106f, 0.576748430f, 0.576826748f, 0.576905062f,
+    0.576983370f, 0.577061673f, 0.577139970f, 0.577218263f, 0.577296550f,
+    0.577374831f, 0.577453107f, 0.577531378f, 0.577609644f, 0.577687905f,
+    0.577766160f, 0.577844409f, 0.577922654f, 0.578000893f, 0.578079127f,
+    0.578157355f, 0.578235579f, 0.578313796f, 0.578392009f, 0.578470216f,
+    0.578548418f, 0.578626615f, 0.578704806f, 0.578782992f, 0.578861173f,
+    0.578939348f, 0.579017518f, 0.579095683f, 0.579173842f, 0.579251996f,
+    0.579330145f, 0.579408288f, 0.579486426f, 0.579564559f, 0.579642687f,
+    0.579720809f, 0.579798925f, 0.579877037f, 0.579955143f, 0.580033244f,
+    0.580111339f, 0.580189429f, 0.580267514f, 0.580345593f, 0.580423668f,
+    0.580501736f, 0.580579800f, 0.580657858f, 0.580735911f, 0.580813958f,
+    0.580892000f, 0.580970037f, 0.581048068f, 0.581126094f, 0.581204115f,
+    0.581282131f, 0.581360141f, 0.581438145f, 0.581516145f, 0.581594139f,
+    0.581672127f, 0.581750111f, 0.581828089f, 0.581906061f, 0.581984028f,
+    0.582061990f, 0.582139947f, 0.582217898f, 0.582295844f, 0.582373785f,
+    0.582451720f, 0.582529649f, 0.582607574f, 0.582685493f, 0.582763407f,
+    0.582841315f, 0.582919218f, 0.582997116f, 0.583075008f, 0.583152895f,
+    0.583230777f, 0.583308653f, 0.583386524f, 0.583464389f, 0.583542249f,
+    0.583620104f, 0.583697954f, 0.583775798f, 0.583853636f, 0.583931470f,
+    0.584009298f, 0.584087120f, 0.584164937f, 0.584242749f, 0.584320556f,
+    0.584398357f, 0.584476153f, 0.584553943f, 0.584631728f, 0.584709508f,
+    0.584787282f, 0.584865051f, 0.584942814f, 0.585020572f, 0.585098325f,
+    0.585176072f, 0.585253814f, 0.585331551f, 0.585409282f, 0.585487008f,
+    0.585564728f, 0.585642443f, 0.585720153f, 0.585797857f, 0.585875556f,
+    0.585953250f, 0.586030938f, 0.586108621f, 0.586186298f, 0.586263970f,
+    0.586341637f, 0.586419298f, 0.586496954f, 0.586574604f, 0.586652249f,
+    0.586729889f, 0.586807523f, 0.586885152f, 0.586962775f, 0.587040394f,
+    0.587118006f, 0.587195613f, 0.587273215f, 0.587350812f, 0.587428403f,
+    0.587505989f, 0.587583569f, 0.587661144f, 0.587738713f, 0.587816277f,
+    0.587893836f, 0.587971389f, 0.588048937f, 0.588126480f, 0.588204017f,
+    0.588281548f, 0.588359074f, 0.588436595f, 0.588514111f, 0.588591621f,
+    0.588669125f, 0.588746625f, 0.588824118f, 0.588901607f, 0.588979090f,
+    0.589056567f, 0.589134039f, 0.589211506f, 0.589288967f, 0.589366423f,
+    0.589443874f, 0.589521319f, 0.589598758f, 0.589676192f, 0.589753621f,
+    0.589831045f, 0.589908463f, 0.589985875f, 0.590063282f, 0.590140684f,
+    0.590218080f, 0.590295471f, 0.590372856f, 0.590450236f, 0.590527611f,
+    0.590604980f, 0.590682344f, 0.590759702f, 0.590837055f, 0.590914402f,
+    0.590991744f, 0.591069081f, 0.591146412f, 0.591223737f, 0.591301058f,
+    0.591378372f, 0.591455682f, 0.591532986f, 0.591610284f, 0.591687577f,
+    0.591764865f, 0.591842147f, 0.591919424f, 0.591996695f, 0.592073961f,
+    0.592151221f, 0.592228476f, 0.592305726f, 0.592382970f, 0.592460208f,
+    0.592537442f, 0.592614669f, 0.592691892f, 0.592769108f, 0.592846320f,
+    0.592923526f, 0.593000726f, 0.593077921f, 0.593155111f, 0.593232295f,
+    0.593309474f, 0.593386647f, 0.593463815f, 0.593540977f, 0.593618134f,
+    0.593695285f, 0.593772431f, 0.593849572f, 0.593926707f, 0.594003836f,
+    0.594080961f, 0.594158079f, 0.594235192f, 0.594312300f, 0.594389402f,
+    0.594466499f, 0.594543591f, 0.594620676f, 0.594697757f, 0.594774832f,
+    0.594851901f, 0.594928965f, 0.595006024f, 0.595083077f, 0.595160124f,
+    0.595237167f, 0.595314203f, 0.595391234f, 0.595468260f, 0.595545280f,
+    0.595622295f, 0.595699304f, 0.595776308f, 0.595853307f, 0.595930300f,
+    0.596007287f, 0.596084269f, 0.596161245f, 0.596238216f, 0.596315182f,
+    0.596392142f, 0.596469096f, 0.596546045f, 0.596622989f, 0.596699927f,
+    0.596776859f, 0.596853786f, 0.596930708f, 0.597007624f, 0.597084535f,
+    0.597161440f, 0.597238340f, 0.597315234f, 0.597392122f, 0.597469006f,
+    0.597545883f, 0.597622755f, 0.597699622f, 0.597776483f, 0.597853339f,
+    0.597930189f, 0.598007034f, 0.598083873f, 0.598160707f, 0.598237535f,
+    0.598314358f, 0.598391175f, 0.598467987f, 0.598544793f, 0.598621594f,
+    0.598698389f, 0.598775179f, 0.598851963f, 0.598928742f, 0.599005515f,
+    0.599082283f, 0.599159045f, 0.599235802f, 0.599312553f, 0.599389298f,
+    0.599466039f, 0.599542773f, 0.599619502f, 0.599696226f, 0.599772944f,
+    0.599849657f, 0.599926364f, 0.600003065f, 0.600079761f, 0.600156452f,
+    0.600233137f, 0.600309817f, 0.600386491f, 0.600463159f, 0.600539822f,
+    0.600616479f, 0.600693131f, 0.600769778f, 0.600846419f, 0.600923054f,
+    0.600999684f, 0.601076308f, 0.601152927f, 0.601229540f, 0.601306148f,
+    0.601382750f, 0.601459347f, 0.601535938f, 0.601612523f, 0.601689103f,
+    0.601765678f, 0.601842247f, 0.601918811f, 0.601995369f, 0.602071921f,
+    0.602148468f, 0.602225009f, 0.602301545f, 0.602378075f, 0.602454600f,
+    0.602531119f, 0.602607633f, 0.602684141f, 0.602760644f, 0.602837141f,
+    0.602913632f, 0.602990118f, 0.603066599f, 0.603143073f, 0.603219543f,
+    0.603296007f, 0.603372465f, 0.603448917f, 0.603525365f, 0.603601806f,
+    0.603678242f, 0.603754673f, 0.603831098f, 0.603907517f, 0.603983931f,
+    0.604060339f, 0.604136742f, 0.604213139f, 0.604289531f, 0.604365917f,
+    0.604442298f, 0.604518673f, 0.604595042f, 0.604671406f, 0.604747764f,
+    0.604824117f, 0.604900464f, 0.604976806f, 0.605053142f, 0.605129472f,
+    0.605205797f, 0.605282117f, 0.605358430f, 0.605434739f, 0.605511041f,
+    0.605587339f, 0.605663630f, 0.605739916f, 0.605816197f, 0.605892471f,
+    0.605968741f, 0.606045004f, 0.606121263f, 0.606197515f, 0.606273762f,
+    0.606350004f, 0.606426239f, 0.606502470f, 0.606578694f, 0.606654913f,
+    0.606731127f, 0.606807335f, 0.606883537f, 0.606959734f, 0.607035925f,
+    0.607112111f, 0.607188291f, 0.607264466f, 0.607340635f, 0.607416798f,
+    0.607492956f, 0.607569108f, 0.607645254f, 0.607721395f, 0.607797531f,
+    0.607873661f, 0.607949785f, 0.608025904f, 0.608102017f, 0.608178124f,
+    0.608254226f, 0.608330322f, 0.608406413f, 0.608482498f, 0.608558578f,
+    0.608634652f, 0.608710720f, 0.608786783f, 0.608862840f, 0.608938891f,
+    0.609014937f, 0.609090978f, 0.609167012f, 0.609243041f, 0.609319065f,
+    0.609395083f, 0.609471095f, 0.609547102f, 0.609623103f, 0.609699099f,
+    0.609775089f, 0.609851073f, 0.609927052f, 0.610003025f, 0.610078992f,
+    0.610154954f, 0.610230911f, 0.610306861f, 0.610382806f, 0.610458746f,
+    0.610534680f, 0.610610608f, 0.610686530f, 0.610762447f, 0.610838359f,
+    0.610914265f, 0.610990165f, 0.611066059f, 0.611141948f, 0.611217832f,
+    0.611293709f, 0.611369581f, 0.611445448f, 0.611521309f, 0.611597164f,
+    0.611673014f, 0.611748858f, 0.611824696f, 0.611900529f, 0.611976356f,
+    0.612052177f, 0.612127993f, 0.612203803f, 0.612279608f, 0.612355407f,
+    0.612431200f, 0.612506988f, 0.612582770f, 0.612658546f, 0.612734317f,
+    0.612810082f, 0.612885842f, 0.612961596f, 0.613037344f, 0.613113087f,
+    0.613188824f, 0.613264555f, 0.613340281f, 0.613416001f, 0.613491716f,
+    0.613567424f, 0.613643128f, 0.613718825f, 0.613794517f, 0.613870203f,
+    0.613945884f, 0.614021559f, 0.614097228f, 0.614172892f, 0.614248550f,
+    0.614324202f, 0.614399849f, 0.614475490f, 0.614551126f, 0.614626756f,
+    0.614702380f, 0.614777998f, 0.614853611f, 0.614929218f, 0.615004820f,
+    0.615080416f, 0.615156006f, 0.615231591f, 0.615307170f, 0.615382743f,
+    0.615458310f, 0.615533872f, 0.615609429f, 0.615684979f, 0.615760524f,
+    0.615836064f, 0.615911597f, 0.615987125f, 0.616062648f, 0.616138164f,
+    0.616213675f, 0.616289181f, 0.616364681f, 0.616440175f, 0.616515663f,
+    0.616591146f, 0.616666623f, 0.616742094f, 0.616817560f, 0.616893020f,
+    0.616968474f, 0.617043923f, 0.617119366f, 0.617194803f, 0.617270235f,
+    0.617345661f, 0.617421081f, 0.617496496f, 0.617571905f, 0.617647308f,
+    0.617722706f, 0.617798097f, 0.617873484f, 0.617948864f, 0.618024239f,
+    0.618099608f, 0.618174972f, 0.618250330f, 0.618325682f, 0.618401028f,
+    0.618476369f, 0.618551704f, 0.618627034f, 0.618702358f, 0.618777676f,
+    0.618852988f, 0.618928295f, 0.619003596f, 0.619078891f, 0.619154181f,
+    0.619229464f, 0.619304743f, 0.619380015f, 0.619455282f, 0.619530543f,
+    0.619605799f, 0.619681048f, 0.619756292f, 0.619831531f, 0.619906764f,
+    0.619981990f, 0.620057212f, 0.620132427f, 0.620207637f, 0.620282841f,
+    0.620358040f, 0.620433233f, 0.620508420f, 0.620583601f, 0.620658777f,
+    0.620733947f, 0.620809111f, 0.620884269f, 0.620959422f, 0.621034569f,
+    0.621109711f, 0.621184847f, 0.621259977f, 0.621335101f, 0.621410219f,
+    0.621485332f, 0.621560439f, 0.621635541f, 0.621710637f, 0.621785727f,
+    0.621860811f, 0.621935889f, 0.622010962f, 0.622086029f, 0.622161091f,
+    0.622236147f, 0.622311197f, 0.622386241f, 0.622461279f, 0.622536312f,
+    0.622611339f, 0.622686361f, 0.622761376f, 0.622836386f, 0.622911390f,
+    0.622986389f, 0.623061382f, 0.623136369f, 0.623211350f, 0.623286326f,
+    0.623361295f, 0.623436260f, 0.623511218f, 0.623586171f, 0.623661118f,
+    0.623736059f, 0.623810994f, 0.623885924f, 0.623960848f, 0.624035766f,
+    0.624110679f, 0.624185585f, 0.624260486f, 0.624335382f, 0.624410271f,
+    0.624485155f, 0.624560033f, 0.624634906f, 0.624709772f, 0.624784633f,
+    0.624859488f, 0.624934338f, 0.625009181f, 0.625084019f, 0.625158851f,
+    0.625233678f, 0.625308498f, 0.625383313f, 0.625458122f, 0.625532926f,
+    0.625607723f, 0.625682515f, 0.625757301f, 0.625832082f, 0.625906856f,
+    0.625981625f, 0.626056388f, 0.626131146f, 0.626205897f, 0.626280643f,
+    0.626355383f, 0.626430118f, 0.626504846f, 0.626579569f, 0.626654286f,
+    0.626728998f, 0.626803703f, 0.626878403f, 0.626953097f, 0.627027785f,
+    0.627102468f, 0.627177145f, 0.627251815f, 0.627326481f, 0.627401140f,
+    0.627475794f, 0.627550442f, 0.627625084f, 0.627699720f, 0.627774351f,
+    0.627848976f, 0.627923595f, 0.627998208f, 0.628072816f, 0.628147417f,
+    0.628222013f, 0.628296604f, 0.628371188f, 0.628445767f, 0.628520339f,
+    0.628594907f, 0.628669468f, 0.628744023f, 0.628818573f, 0.628893117f,
+    0.628967655f, 0.629042188f, 0.629116714f, 0.629191235f, 0.629265750f,
+    0.629340260f, 0.629414763f, 0.629489261f, 0.629563753f, 0.629638239f,
+    0.629712719f, 0.629787194f, 0.629861663f, 0.629936126f, 0.630010583f,
+    0.630085034f, 0.630159480f, 0.630233920f, 0.630308354f, 0.630382782f,
+    0.630457204f, 0.630531621f, 0.630606032f, 0.630680437f, 0.630754836f,
+    0.630829230f, 0.630903617f, 0.630977999f, 0.631052375f, 0.631126745f,
+    0.631201110f, 0.631275469f, 0.631349821f, 0.631424169f, 0.631498510f,
+    0.631572845f, 0.631647175f, 0.631721499f, 0.631795817f, 0.631870129f,
+    0.631944435f, 0.632018736f, 0.632093031f, 0.632167320f, 0.632241603f,
+    0.632315880f, 0.632390152f, 0.632464418f, 0.632538677f, 0.632612932f,
+    0.632687180f, 0.632761422f, 0.632835659f, 0.632909890f, 0.632984115f,
+    0.633058334f, 0.633132547f, 0.633206755f, 0.633280957f, 0.633355153f,
+    0.633429343f, 0.633503527f, 0.633577706f, 0.633651878f, 0.633726045f,
+    0.633800206f, 0.633874361f, 0.633948511f, 0.634022654f, 0.634096792f,
+    0.634170924f, 0.634245050f, 0.634319170f, 0.634393284f, 0.634467393f,
+    0.634541495f, 0.634615592f, 0.634689683f, 0.634763769f, 0.634837848f,
+    0.634911921f, 0.634985989f, 0.635060051f, 0.635134107f, 0.635208157f,
+    0.635282202f, 0.635356240f, 0.635430273f, 0.635504300f, 0.635578320f,
+    0.635652336f, 0.635726345f, 0.635800348f, 0.635874346f, 0.635948338f,
+    0.636022324f, 0.636096304f, 0.636170278f, 0.636244246f, 0.636318209f,
+    0.636392166f, 0.636466116f, 0.636540061f, 0.636614001f, 0.636687934f,
+    0.636761861f, 0.636835783f, 0.636909699f, 0.636983608f, 0.637057512f,
+    0.637131411f, 0.637205303f, 0.637279189f, 0.637353070f, 0.637426945f,
+    0.637500813f, 0.637574677f, 0.637648534f, 0.637722385f, 0.637796230f,
+    0.637870070f, 0.637943904f, 0.638017731f, 0.638091553f, 0.638165370f,
+    0.638239180f, 0.638312984f, 0.638386783f, 0.638460575f, 0.638534362f,
+    0.638608143f, 0.638681918f, 0.638755687f, 0.638829450f, 0.638903208f,
+    0.638976959f, 0.639050705f, 0.639124445f, 0.639198179f, 0.639271907f,
+    0.639345629f, 0.639419345f, 0.639493056f, 0.639566760f, 0.639640459f,
+    0.639714152f, 0.639787839f, 0.639861520f, 0.639935195f, 0.640008864f,
+    0.640082527f, 0.640156185f, 0.640229836f, 0.640303482f, 0.640377122f,
+    0.640450756f, 0.640524384f, 0.640598006f, 0.640671622f, 0.640745233f,
+    0.640818837f, 0.640892436f, 0.640966029f, 0.641039616f, 0.641113197f,
+    0.641186772f, 0.641260341f, 0.641333904f, 0.641407461f, 0.641481013f,
+    0.641554558f, 0.641628098f, 0.641701632f, 0.641775160f, 0.641848682f,
+    0.641922198f, 0.641995708f, 0.642069212f, 0.642142711f, 0.642216203f,
+    0.642289690f, 0.642363170f, 0.642436645f, 0.642510114f, 0.642583577f,
+    0.642657034f, 0.642730485f, 0.642803930f, 0.642877370f, 0.642950803f,
+    0.643024231f, 0.643097652f, 0.643171068f, 0.643244478f, 0.643317881f,
+    0.643391279f, 0.643464671f, 0.643538058f, 0.643611438f, 0.643684812f,
+    0.643758180f, 0.643831543f, 0.643904899f, 0.643978250f, 0.644051595f,
+    0.644124934f, 0.644198266f, 0.644271593f, 0.644344914f, 0.644418229f,
+    0.644491539f, 0.644564842f, 0.644638139f, 0.644711431f, 0.644784716f,
+    0.644857996f, 0.644931269f, 0.645004537f, 0.645077799f, 0.645151054f,
+    0.645224304f, 0.645297548f, 0.645370786f, 0.645444018f, 0.645517245f,
+    0.645590465f, 0.645663679f, 0.645736887f, 0.645810090f, 0.645883286f,
+    0.645956477f, 0.646029662f, 0.646102840f, 0.646176013f, 0.646249180f,
+    0.646322341f, 0.646395496f, 0.646468645f, 0.646541788f, 0.646614925f,
+    0.646688056f, 0.646761181f, 0.646834300f, 0.646907414f, 0.646980521f,
+    0.647053622f, 0.647126718f, 0.647199807f, 0.647272891f, 0.647345969f,
+    0.647419040f, 0.647492106f, 0.647565166f, 0.647638220f, 0.647711268f,
+    0.647784309f, 0.647857345f, 0.647930375f, 0.648003399f, 0.648076418f,
+    0.648149430f, 0.648222436f, 0.648295436f, 0.648368430f, 0.648441419f,
+    0.648514401f, 0.648587377f, 0.648660348f, 0.648733312f, 0.648806271f,
+    0.648879223f, 0.648952170f, 0.649025110f, 0.649098045f, 0.649170974f,
+    0.649243897f, 0.649316813f, 0.649389724f, 0.649462629f, 0.649535528f,
+    0.649608421f, 0.649681307f, 0.649754188f, 0.649827063f, 0.649899932f,
+    0.649972795f, 0.650045652f, 0.650118503f, 0.650191348f, 0.650264187f,
+    0.650337021f, 0.650409848f, 0.650482669f, 0.650555484f, 0.650628293f,
+    0.650701096f, 0.650773894f, 0.650846685f, 0.650919470f, 0.650992250f,
+    0.651065023f, 0.651137790f, 0.651210552f, 0.651283307f, 0.651356056f,
+    0.651428800f, 0.651501537f, 0.651574268f, 0.651646994f, 0.651719713f,
+    0.651792427f, 0.651865134f, 0.651937836f, 0.652010531f, 0.652083221f,
+    0.652155904f, 0.652228582f, 0.652301253f, 0.652373918f, 0.652446578f,
+    0.652519231f, 0.652591879f, 0.652664520f, 0.652737156f, 0.652809785f,
+    0.652882409f, 0.652955026f, 0.653027638f, 0.653100243f, 0.653172843f,
+    0.653245436f, 0.653318024f, 0.653390605f, 0.653463181f, 0.653535750f,
+    0.653608314f, 0.653680871f, 0.653753423f, 0.653825968f, 0.653898508f,
+    0.653971041f, 0.654043568f, 0.654116090f, 0.654188605f, 0.654261114f,
+    0.654333618f, 0.654406115f, 0.654478606f, 0.654551092f, 0.654623571f,
+    0.654696044f, 0.654768512f, 0.654840973f, 0.654913428f, 0.654985877f,
+    0.655058320f, 0.655130758f, 0.655203189f, 0.655275614f, 0.655348033f,
+    0.655420446f, 0.655492853f, 0.655565254f, 0.655637649f, 0.655710038f,
+    0.655782421f, 0.655854798f, 0.655927169f, 0.655999534f, 0.656071892f,
+    0.656144245f, 0.656216592f, 0.656288933f, 0.656361267f, 0.656433596f,
+    0.656505919f, 0.656578235f, 0.656650546f, 0.656722850f, 0.656795149f,
+    0.656867441f, 0.656939728f, 0.657012008f, 0.657084282f, 0.657156551f,
+    0.657228813f, 0.657301069f, 0.657373319f, 0.657445563f, 0.657517801f,
+    0.657590033f, 0.657662259f, 0.657734479f, 0.657806693f, 0.657878901f,
+    0.657951103f, 0.658023299f, 0.658095488f, 0.658167672f, 0.658239850f,
+    0.658312021f, 0.658384187f, 0.658456346f, 0.658528500f, 0.658600647f,
+    0.658672788f, 0.658744924f, 0.658817053f, 0.658889176f, 0.658961293f,
+    0.659033404f, 0.659105509f, 0.659177608f, 0.659249701f, 0.659321788f,
+    0.659393868f, 0.659465943f, 0.659538012f, 0.659610074f, 0.659682131f,
+    0.659754181f, 0.659826225f, 0.659898264f, 0.659970296f, 0.660042322f,
+    0.660114342f, 0.660186356f, 0.660258364f, 0.660330366f, 0.660402362f,
+    0.660474351f, 0.660546335f, 0.660618313f, 0.660690284f, 0.660762250f,
+    0.660834209f, 0.660906162f, 0.660978110f, 0.661050051f, 0.661121986f,
+    0.661193915f, 0.661265838f, 0.661337755f, 0.661409665f, 0.661481570f,
+    0.661553469f, 0.661625361f, 0.661697248f, 0.661769128f, 0.661841002f,
+    0.661912871f, 0.661984733f, 0.662056589f, 0.662128439f, 0.662200283f,
+    0.662272120f, 0.662343952f, 0.662415778f, 0.662487597f, 0.662559411f,
+    0.662631218f, 0.662703019f, 0.662774814f, 0.662846603f, 0.662918386f,
+    0.662990163f, 0.663061934f, 0.663133699f, 0.663205457f, 0.663277210f,
+    0.663348956f, 0.663420696f, 0.663492431f, 0.663564159f, 0.663635881f,
+    0.663707597f, 0.663779306f, 0.663851010f, 0.663922708f, 0.663994399f,
+    0.664066084f, 0.664137764f, 0.664209437f, 0.664281104f, 0.664352765f,
+    0.664424420f, 0.664496069f, 0.664567711f, 0.664639348f, 0.664710978f,
+    0.664782603f, 0.664854221f, 0.664925833f, 0.664997439f, 0.665069039f,
+    0.665140632f, 0.665212220f, 0.665283802f, 0.665355377f, 0.665426946f,
+    0.665498510f, 0.665570067f, 0.665641618f, 0.665713162f, 0.665784701f,
+    0.665856234f, 0.665927760f, 0.665999280f, 0.666070795f, 0.666142303f,
+    0.666213805f, 0.666285301f, 0.666356790f, 0.666428274f, 0.666499751f,
+    0.666571223f, 0.666642688f, 0.666714147f, 0.666785600f, 0.666857047f,
+    0.666928488f, 0.666999922f, 0.667071351f, 0.667142773f, 0.667214189f,
+    0.667285599f, 0.667357003f, 0.667428401f, 0.667499793f, 0.667571178f,
+    0.667642558f, 0.667713931f, 0.667785298f, 0.667856659f, 0.667928014f,
+    0.667999362f, 0.668070705f, 0.668142041f, 0.668213372f, 0.668284696f,
+    0.668356014f, 0.668427326f, 0.668498631f, 0.668569931f, 0.668641224f,
+    0.668712512f, 0.668783793f, 0.668855068f, 0.668926336f, 0.668997599f,
+    0.669068856f, 0.669140106f, 0.669211350f, 0.669282588f, 0.669353820f,
+    0.669425046f, 0.669496266f, 0.669567479f, 0.669638686f, 0.669709888f,
+    0.669781083f, 0.669852271f, 0.669923454f, 0.669994631f, 0.670065801f,
+    0.670136965f, 0.670208123f, 0.670279275f, 0.670350421f, 0.670421560f,
+    0.670492694f, 0.670563821f, 0.670634942f, 0.670706057f, 0.670777166f,
+    0.670848268f, 0.670919365f, 0.670990455f, 0.671061539f, 0.671132617f,
+    0.671203689f, 0.671274754f, 0.671345814f, 0.671416867f, 0.671487914f,
+    0.671558955f, 0.671629990f, 0.671701018f, 0.671772040f, 0.671843057f,
+    0.671914067f, 0.671985071f, 0.672056068f, 0.672127060f, 0.672198045f,
+    0.672269024f, 0.672339997f, 0.672410964f, 0.672481924f, 0.672552879f,
+    0.672623827f, 0.672694769f, 0.672765705f, 0.672836635f, 0.672907558f,
+    0.672978475f, 0.673049387f, 0.673120291f, 0.673191190f, 0.673262083f,
+    0.673332969f, 0.673403849f, 0.673474723f, 0.673545591f, 0.673616453f,
+    0.673687308f, 0.673758157f, 0.673829000f, 0.673899837f, 0.673970668f,
+    0.674041492f, 0.674112311f, 0.674183123f, 0.674253928f, 0.674324728f,
+    0.674395522f, 0.674466309f, 0.674537090f, 0.674607865f, 0.674678633f,
+    0.674749396f, 0.674820152f, 0.674890902f, 0.674961646f, 0.675032384f,
+    0.675103115f, 0.675173840f, 0.675244559f, 0.675315272f, 0.675385979f,
+    0.675456679f, 0.675527374f, 0.675598062f, 0.675668743f, 0.675739419f,
+    0.675810088f, 0.675880751f, 0.675951408f, 0.676022059f, 0.676092704f,
+    0.676163342f, 0.676233974f, 0.676304600f, 0.676375219f, 0.676445833f,
+    0.676516440f, 0.676587041f, 0.676657636f, 0.676728224f, 0.676798807f,
+    0.676869383f, 0.676939953f, 0.677010516f, 0.677081074f, 0.677151625f,
+    0.677222170f, 0.677292709f, 0.677363241f, 0.677433768f, 0.677504288f,
+    0.677574802f, 0.677645309f, 0.677715811f, 0.677786306f, 0.677856795f,
+    0.677927278f, 0.677997754f, 0.678068224f, 0.678138688f, 0.678209146f,
+    0.678279598f, 0.678350043f, 0.678420482f, 0.678490915f, 0.678561342f,
+    0.678631762f, 0.678702176f, 0.678772584f, 0.678842986f, 0.678913381f,
+    0.678983770f, 0.679054153f, 0.679124530f, 0.679194900f, 0.679265265f,
+    0.679335623f, 0.679405974f, 0.679476320f, 0.679546659f, 0.679616992f,
+    0.679687319f, 0.679757639f, 0.679827954f, 0.679898262f, 0.679968563f,
+    0.680038859f, 0.680109148f, 0.680179431f, 0.680249708f, 0.680319978f,
+    0.680390243f, 0.680460501f, 0.680530752f, 0.680600998f, 0.680671237f,
+    0.680741470f, 0.680811697f, 0.680881917f, 0.680952131f, 0.681022339f,
+    0.681092541f, 0.681162736f, 0.681232925f, 0.681303108f, 0.681373285f,
+    0.681443455f, 0.681513619f, 0.681583777f, 0.681653929f, 0.681724074f,
+    0.681794213f, 0.681864346f, 0.681934472f, 0.682004593f, 0.682074707f,
+    0.682144814f, 0.682214916f, 0.682285011f, 0.682355100f, 0.682425182f,
+    0.682495259f, 0.682565329f, 0.682635393f, 0.682705450f, 0.682775501f,
+    0.682845546f, 0.682915585f, 0.682985617f, 0.683055644f, 0.683125663f,
+    0.683195677f, 0.683265684f, 0.683335685f, 0.683405680f, 0.683475669f,
+    0.683545651f, 0.683615627f, 0.683685596f, 0.683755560f, 0.683825517f,
+    0.683895467f, 0.683965412f, 0.684035350f, 0.684105282f, 0.684175207f,
+    0.684245127f, 0.684315040f, 0.684384947f, 0.684454847f, 0.684524741f,
+    0.684594629f, 0.684664511f, 0.684734386f, 0.684804255f, 0.684874117f,
+    0.684943974f, 0.685013824f, 0.685083668f, 0.685153505f, 0.685223336f,
+    0.685293161f, 0.685362980f, 0.685432792f, 0.685502598f, 0.685572398f,
+    0.685642191f, 0.685711978f, 0.685781759f, 0.685851534f, 0.685921302f,
+    0.685991064f, 0.686060819f, 0.686130569f, 0.686200312f, 0.686270048f,
+    0.686339779f, 0.686409503f, 0.686479220f, 0.686548932f, 0.686618637f,
+    0.686688336f, 0.686758028f, 0.686827714f, 0.686897394f, 0.686967068f,
+    0.687036735f, 0.687106396f, 0.687176051f, 0.687245699f, 0.687315341f,
+    0.687384977f, 0.687454606f, 0.687524229f, 0.687593846f, 0.687663456f,
+    0.687733060f, 0.687802658f, 0.687872249f, 0.687941834f, 0.688011413f,
+    0.688080985f, 0.688150552f, 0.688220111f, 0.688289665f, 0.688359212f,
+    0.688428753f, 0.688498287f, 0.688567815f, 0.688637337f, 0.688706853f,
+    0.688776362f, 0.688845865f, 0.688915361f, 0.688984851f, 0.689054335f,
+    0.689123813f, 0.689193284f, 0.689262749f, 0.689332207f, 0.689401659f,
+    0.689471105f, 0.689540545f, 0.689609978f, 0.689679405f, 0.689748825f,
+    0.689818239f, 0.689887647f, 0.689957049f, 0.690026444f, 0.690095832f,
+    0.690165215f, 0.690234591f, 0.690303961f, 0.690373324f, 0.690442681f,
+    0.690512032f, 0.690581376f, 0.690650714f, 0.690720046f, 0.690789371f,
+    0.690858690f, 0.690928003f, 0.690997309f, 0.691066609f, 0.691135902f,
+    0.691205190f, 0.691274470f, 0.691343745f, 0.691413013f, 0.691482275f,
+    0.691551530f, 0.691620779f, 0.691690022f, 0.691759258f, 0.691828488f,
+    0.691897712f, 0.691966929f, 0.692036140f, 0.692105345f, 0.692174543f,
+    0.692243735f, 0.692312920f, 0.692382099f, 0.692451272f, 0.692520438f,
+    0.692589598f, 0.692658752f, 0.692727899f, 0.692797040f, 0.692866175f,
+    0.692935303f, 0.693004425f, 0.693073540f, 0.693142649f, 0.693211752f,
+    0.693280848f, 0.693349938f, 0.693419022f, 0.693488099f, 0.693557170f,
+    0.693626234f, 0.693695292f, 0.693764344f, 0.693833389f, 0.693902428f,
+    0.693971461f, 0.694040487f, 0.694109507f, 0.694178520f, 0.694247527f,
+    0.694316528f, 0.694385522f, 0.694454510f, 0.694523492f, 0.694592467f,
+    0.694661436f, 0.694730398f, 0.694799354f, 0.694868304f, 0.694937247f,
+    0.695006184f, 0.695075114f, 0.695144038f, 0.695212956f, 0.695281867f,
+    0.695350772f, 0.695419670f, 0.695488562f, 0.695557448f, 0.695626327f,
+    0.695695200f, 0.695764067f, 0.695832927f, 0.695901781f, 0.695970628f,
+    0.696039469f, 0.696108303f, 0.696177131f, 0.696245953f, 0.696314769f,
+    0.696383577f, 0.696452380f, 0.696521176f, 0.696589966f, 0.696658749f,
+    0.696727526f, 0.696796297f, 0.696865061f, 0.696933818f, 0.697002570f,
+    0.697071315f, 0.697140053f, 0.697208785f, 0.697277511f, 0.697346230f,
+    0.697414943f, 0.697483649f, 0.697552349f, 0.697621043f, 0.697689730f,
+    0.697758411f, 0.697827085f, 0.697895753f, 0.697964415f, 0.698033070f,
+    0.698101719f, 0.698170361f, 0.698238997f, 0.698307626f, 0.698376249f,
+    0.698444866f, 0.698513476f, 0.698582080f, 0.698650677f, 0.698719268f,
+    0.698787853f, 0.698856431f, 0.698925003f, 0.698993568f, 0.699062127f,
+    0.699130679f, 0.699199225f, 0.699267765f, 0.699336298f, 0.699404824f,
+    0.699473345f, 0.699541858f, 0.699610366f, 0.699678867f, 0.699747361f,
+    0.699815849f, 0.699884331f, 0.699952806f, 0.700021275f, 0.700089738f,
+    0.700158194f, 0.700226643f, 0.700295086f, 0.700363523f, 0.700431953f,
+    0.700500377f, 0.700568794f, 0.700637205f, 0.700705609f, 0.700774007f,
+    0.700842399f, 0.700910784f, 0.700979163f, 0.701047535f, 0.701115901f,
+    0.701184260f, 0.701252613f, 0.701320959f, 0.701389299f, 0.701457633f,
+    0.701525960f, 0.701594281f, 0.701662595f, 0.701730902f, 0.701799204f,
+    0.701867499f, 0.701935787f, 0.702004069f, 0.702072345f, 0.702140614f,
+    0.702208876f, 0.702277132f, 0.702345382f, 0.702413625f, 0.702481862f,
+    0.702550092f, 0.702618316f, 0.702686534f, 0.702754744f, 0.702822949f,
+    0.702891147f, 0.702959339f, 0.703027524f, 0.703095702f, 0.703163874f,
+    0.703232040f, 0.703300199f, 0.703368352f, 0.703436498f, 0.703504638f,
+    0.703572772f, 0.703640899f, 0.703709019f, 0.703777133f, 0.703845241f,
+    0.703913342f, 0.703981436f, 0.704049524f, 0.704117606f, 0.704185681f,
+    0.704253750f, 0.704321812f, 0.704389868f, 0.704457917f, 0.704525960f,
+    0.704593996f, 0.704662026f, 0.704730049f, 0.704798066f, 0.704866076f,
+    0.704934080f, 0.705002078f, 0.705070069f, 0.705138053f, 0.705206031f,
+    0.705274003f, 0.705341968f, 0.705409926f, 0.705477878f, 0.705545824f,
+    0.705613763f, 0.705681696f, 0.705749622f, 0.705817541f, 0.705885455f,
+    0.705953361f, 0.706021261f, 0.706089155f, 0.706157042f, 0.706224923f,
+    0.706292797f, 0.706360665f, 0.706428526f, 0.706496381f, 0.706564229f,
+    0.706632071f, 0.706699906f, 0.706767735f, 0.706835557f, 0.706903373f,
+    0.706971182f, 0.707038985f, 0.707106781f, 0.707174571f, 0.707242354f,
+    0.707310131f, 0.707377901f, 0.707445665f, 0.707513422f, 0.707581173f,
+    0.707648917f, 0.707716655f, 0.707784386f, 0.707852111f, 0.707919829f,
+    0.707987541f, 0.708055246f, 0.708122945f, 0.708190637f, 0.708258323f,
+    0.708326002f, 0.708393675f, 0.708461341f, 0.708529000f, 0.708596653f,
+    0.708664300f, 0.708731940f, 0.708799574f, 0.708867201f, 0.708934821f,
+    0.709002435f, 0.709070043f, 0.709137644f, 0.709205238f, 0.709272826f,
+    0.709340408f, 0.709407983f, 0.709475551f, 0.709543113f, 0.709610668f,
+    0.709678217f, 0.709745760f, 0.709813295f, 0.709880825f, 0.709948347f,
+    0.710015864f, 0.710083373f, 0.710150877f, 0.710218373f, 0.710285863f,
+    0.710353347f, 0.710420824f, 0.710488294f, 0.710555758f, 0.710623216f,
+    0.710690667f, 0.710758111f, 0.710825549f, 0.710892980f, 0.710960405f,
+    0.711027823f, 0.711095235f, 0.711162640f, 0.711230039f, 0.711297431f,
+    0.711364817f, 0.711432196f, 0.711499568f, 0.711566934f, 0.711634294f,
+    0.711701646f, 0.711768993f, 0.711836333f, 0.711903666f, 0.711970993f,
+    0.712038313f, 0.712105626f, 0.712172933f, 0.712240234f, 0.712307528f,
+    0.712374815f, 0.712442096f, 0.712509371f, 0.712576638f, 0.712643900f,
+    0.712711154f, 0.712778402f, 0.712845644f, 0.712912879f, 0.712980107f,
+    0.713047329f, 0.713114545f, 0.713181754f, 0.713248956f, 0.713316152f,
+    0.713383341f, 0.713450523f, 0.713517699f, 0.713584869f, 0.713652032f,
+    0.713719188f, 0.713786338f, 0.713853481f, 0.713920618f, 0.713987748f,
+    0.714054871f, 0.714121988f, 0.714189099f, 0.714256203f, 0.714323300f,
+    0.714390391f, 0.714457475f, 0.714524552f, 0.714591623f, 0.714658688f,
+    0.714725746f, 0.714792797f, 0.714859842f, 0.714926880f, 0.714993912f,
+    0.715060937f, 0.715127955f, 0.715194967f, 0.715261972f, 0.715328971f,
+    0.715395963f, 0.715462949f, 0.715529928f, 0.715596900f, 0.715663866f,
+    0.715730825f, 0.715797778f, 0.715864724f, 0.715931664f, 0.715998597f,
+    0.716065523f, 0.716132443f, 0.716199356f, 0.716266263f, 0.716333163f,
+    0.716400056f, 0.716466943f, 0.716533823f, 0.716600697f, 0.716667564f,
+    0.716734425f, 0.716801279f, 0.716868126f, 0.716934967f, 0.717001801f,
+    0.717068628f, 0.717135449f, 0.717202264f, 0.717269072f, 0.717335873f,
+    0.717402667f, 0.717469455f, 0.717536237f, 0.717603012f, 0.717669780f,
+    0.717736542f, 0.717803297f, 0.717870045f, 0.717936787f, 0.718003522f,
+    0.718070251f, 0.718136973f, 0.718203688f, 0.718270397f, 0.718337099f,
+    0.718403795f, 0.718470484f, 0.718537166f, 0.718603842f, 0.718670512f,
+    0.718737174f, 0.718803830f, 0.718870480f, 0.718937122f, 0.719003759f,
+    0.719070388f, 0.719137011f, 0.719203627f, 0.719270237f, 0.719336840f,
+    0.719403437f, 0.719470027f, 0.719536610f, 0.719603187f, 0.719669757f,
+    0.719736320f, 0.719802877f, 0.719869427f, 0.719935971f, 0.720002508f,
+    0.720069038f, 0.720135562f, 0.720202079f, 0.720268590f, 0.720335094f,
+    0.720401591f, 0.720468082f, 0.720534566f, 0.720601043f, 0.720667514f,
+    0.720733978f, 0.720800435f, 0.720866886f, 0.720933331f, 0.720999768f,
+    0.721066199f, 0.721132624f, 0.721199041f, 0.721265453f, 0.721331857f,
+    0.721398255f, 0.721464646f, 0.721531031f, 0.721597409f, 0.721663780f,
+    0.721730145f, 0.721796503f, 0.721862854f, 0.721929199f, 0.721995537f,
+    0.722061869f, 0.722128194f, 0.722194512f, 0.722260824f, 0.722327129f,
+    0.722393427f, 0.722459719f, 0.722526004f, 0.722592282f, 0.722658554f,
+    0.722724819f, 0.722791078f, 0.722857330f, 0.722923575f, 0.722989813f,
+    0.723056045f, 0.723122271f, 0.723188489f, 0.723254701f, 0.723320907f,
+    0.723387105f, 0.723453297f, 0.723519483f, 0.723585661f, 0.723651834f,
+    0.723717999f, 0.723784158f, 0.723850310f, 0.723916455f, 0.723982594f,
+    0.724048726f, 0.724114852f, 0.724180971f, 0.724247083f, 0.724313188f,
+    0.724379287f, 0.724445380f, 0.724511465f, 0.724577544f, 0.724643616f,
+    0.724709682f, 0.724775741f, 0.724841793f, 0.724907839f, 0.724973878f,
+    0.725039910f, 0.725105936f, 0.725171954f, 0.725237967f, 0.725303972f,
+    0.725369971f, 0.725435964f, 0.725501949f, 0.725567928f, 0.725633900f,
+    0.725699866f, 0.725765825f, 0.725831777f, 0.725897723f, 0.725963662f,
+    0.726029594f, 0.726095520f, 0.726161438f, 0.726227351f, 0.726293256f,
+    0.726359155f, 0.726425047f, 0.726490933f, 0.726556812f, 0.726622684f,
+    0.726688549f, 0.726754408f, 0.726820260f, 0.726886106f, 0.726951944f,
+    0.727017776f, 0.727083602f, 0.727149421f, 0.727215233f, 0.727281038f,
+    0.727346837f, 0.727412629f, 0.727478414f, 0.727544192f, 0.727609964f,
+    0.727675730f, 0.727741488f, 0.727807240f, 0.727872985f, 0.727938724f,
+    0.728004455f, 0.728070180f, 0.728135899f, 0.728201611f, 0.728267316f,
+    0.728333014f, 0.728398706f, 0.728464390f, 0.728530069f, 0.728595740f,
+    0.728661405f, 0.728727063f, 0.728792715f, 0.728858359f, 0.728923997f,
+    0.728989629f, 0.729055253f, 0.729120871f, 0.729186483f, 0.729252087f,
+    0.729317685f, 0.729383276f, 0.729448860f, 0.729514438f, 0.729580009f,
+    0.729645573f, 0.729711131f, 0.729776682f, 0.729842226f, 0.729907764f,
+    0.729973294f, 0.730038818f, 0.730104336f, 0.730169846f, 0.730235350f,
+    0.730300848f, 0.730366338f, 0.730431822f, 0.730497299f, 0.730562769f,
+    0.730628233f, 0.730693690f, 0.730759140f, 0.730824583f, 0.730890020f,
+    0.730955450f, 0.731020874f, 0.731086290f, 0.731151700f, 0.731217103f,
+    0.731282500f, 0.731347890f, 0.731413273f, 0.731478649f, 0.731544018f,
+    0.731609381f, 0.731674737f, 0.731740087f, 0.731805429f, 0.731870765f,
+    0.731936095f, 0.732001417f, 0.732066733f, 0.732132042f, 0.732197344f,
+    0.732262640f, 0.732327928f, 0.732393211f, 0.732458486f, 0.732523755f,
+    0.732589017f, 0.732654272f, 0.732719520f, 0.732784762f, 0.732849997f,
+    0.732915225f, 0.732980446f, 0.733045661f, 0.733110869f, 0.733176071f,
+    0.733241265f, 0.733306453f, 0.733371634f, 0.733436808f, 0.733501976f,
+    0.733567137f, 0.733632291f, 0.733697438f, 0.733762579f, 0.733827713f,
+    0.733892840f, 0.733957960f, 0.734023074f, 0.734088181f, 0.734153281f,
+    0.734218374f, 0.734283461f, 0.734348541f, 0.734413614f, 0.734478680f,
+    0.734543740f, 0.734608793f, 0.734673839f, 0.734738878f, 0.734803911f,
+    0.734868937f, 0.734933956f, 0.734998968f, 0.735063974f, 0.735128972f,
+    0.735193965f, 0.735258950f, 0.735323928f, 0.735388900f, 0.735453865f,
+    0.735518824f, 0.735583775f, 0.735648720f, 0.735713658f, 0.735778589f,
+    0.735843514f, 0.735908431f, 0.735973342f, 0.736038247f, 0.736103144f,
+    0.736168035f, 0.736232918f, 0.736297796f, 0.736362666f, 0.736427530f,
+    0.736492386f, 0.736557236f, 0.736622080f, 0.736686916f, 0.736751746f,
+    0.736816569f, 0.736881385f, 0.736946194f, 0.737010997f, 0.737075793f,
+    0.737140582f, 0.737205364f, 0.737270140f, 0.737334909f, 0.737399671f,
+    0.737464426f, 0.737529174f, 0.737593916f, 0.737658651f, 0.737723379f,
+    0.737788100f, 0.737852815f, 0.737917523f, 0.737982223f, 0.738046918f,
+    0.738111605f, 0.738176286f, 0.738240960f, 0.738305627f, 0.738370287f,
+    0.738434940f, 0.738499587f, 0.738564227f, 0.738628860f, 0.738693486f,
+    0.738758106f, 0.738822719f, 0.738887324f, 0.738951924f, 0.739016516f,
+    0.739081102f, 0.739145680f, 0.739210252f, 0.739274817f, 0.739339376f,
+    0.739403927f, 0.739468472f, 0.739533010f, 0.739597541f, 0.739662066f,
+    0.739726583f, 0.739791094f, 0.739855598f, 0.739920095f, 0.739984586f,
+    0.740049069f, 0.740113546f, 0.740178016f, 0.740242479f, 0.740306936f,
+    0.740371385f, 0.740435828f, 0.740500264f, 0.740564693f, 0.740629116f,
+    0.740693531f, 0.740757940f, 0.740822342f, 0.740886737f, 0.740951125f,
+    0.741015507f, 0.741079882f, 0.741144249f, 0.741208610f, 0.741272965f,
+    0.741337312f, 0.741401653f, 0.741465987f, 0.741530314f, 0.741594634f,
+    0.741658947f, 0.741723254f, 0.741787553f, 0.741851846f, 0.741916132f,
+    0.741980412f, 0.742044684f, 0.742108950f, 0.742173209f, 0.742237461f,
+    0.742301706f, 0.742365944f, 0.742430176f, 0.742494400f, 0.742558618f,
+    0.742622829f, 0.742687033f, 0.742751231f, 0.742815421f, 0.742879605f,
+    0.742943782f, 0.743007952f, 0.743072115f, 0.743136272f, 0.743200421f,
+    0.743264564f, 0.743328700f, 0.743392829f, 0.743456951f, 0.743521067f,
+    0.743585175f, 0.743649277f, 0.743713372f, 0.743777460f, 0.743841541f,
+    0.743905616f, 0.743969683f, 0.744033744f, 0.744097798f, 0.744161845f,
+    0.744225885f, 0.744289919f, 0.744353945f, 0.744417965f, 0.744481978f,
+    0.744545984f, 0.744609983f, 0.744673975f, 0.744737961f, 0.744801939f,
+    0.744865911f, 0.744929876f, 0.744993834f, 0.745057785f, 0.745121730f,
+    0.745185667f, 0.745249598f, 0.745313522f, 0.745377439f, 0.745441349f,
+    0.745505252f, 0.745569149f, 0.745633038f, 0.745696921f, 0.745760797f,
+    0.745824666f, 0.745888528f, 0.745952383f, 0.746016232f, 0.746080074f,
+    0.746143908f, 0.746207736f, 0.746271557f, 0.746335371f, 0.746399179f,
+    0.746462979f, 0.746526773f, 0.746590559f, 0.746654339f, 0.746718112f,
+    0.746781878f, 0.746845638f, 0.746909390f, 0.746973136f, 0.747036874f,
+    0.747100606f, 0.747164331f, 0.747228049f, 0.747291760f, 0.747355465f,
+    0.747419162f, 0.747482853f, 0.747546536f, 0.747610213f, 0.747673883f,
+    0.747737546f, 0.747801202f, 0.747864852f, 0.747928494f, 0.747992130f,
+    0.748055759f, 0.748119380f, 0.748182995f, 0.748246604f, 0.748310205f,
+    0.748373799f, 0.748437387f, 0.748500967f, 0.748564541f, 0.748628108f,
+    0.748691668f, 0.748755221f, 0.748818767f, 0.748882306f, 0.748945839f,
+    0.749009364f, 0.749072883f, 0.749136395f, 0.749199899f, 0.749263397f,
+    0.749326888f, 0.749390373f, 0.749453850f, 0.749517320f, 0.749580784f,
+    0.749644241f, 0.749707690f, 0.749771133f, 0.749834569f, 0.749897998f,
+    0.749961421f, 0.750024836f, 0.750088244f, 0.750151646f, 0.750215040f,
+    0.750278428f, 0.750341809f, 0.750405183f, 0.750468550f, 0.750531910f,
+    0.750595263f, 0.750658610f, 0.750721949f, 0.750785282f, 0.750848607f,
+    0.750911926f, 0.750975238f, 0.751038543f, 0.751101841f, 0.751165132f,
+    0.751228416f, 0.751291693f, 0.751354964f, 0.751418227f, 0.751481484f,
+    0.751544734f, 0.751607976f, 0.751671212f, 0.751734441f, 0.751797663f,
+    0.751860878f, 0.751924087f, 0.751987288f, 0.752050482f, 0.752113670f,
+    0.752176850f, 0.752240024f, 0.752303191f, 0.752366351f, 0.752429504f,
+    0.752492650f, 0.752555789f, 0.752618921f, 0.752682046f, 0.752745164f,
+    0.752808276f, 0.752871380f, 0.752934478f, 0.752997569f, 0.753060652f,
+    0.753123729f, 0.753186799f, 0.753249862f, 0.753312918f, 0.753375967f,
+    0.753439009f, 0.753502045f, 0.753565073f, 0.753628094f, 0.753691109f,
+    0.753754116f, 0.753817117f, 0.753880111f, 0.753943098f, 0.754006077f,
+    0.754069050f, 0.754132016f, 0.754194975f, 0.754257927f, 0.754320873f,
+    0.754383811f, 0.754446742f, 0.754509667f, 0.754572584f, 0.754635495f,
+    0.754698398f, 0.754761295f, 0.754824184f, 0.754887067f, 0.754949943f,
+    0.755012812f, 0.755075674f, 0.755138529f, 0.755201377f, 0.755264218f,
+    0.755327052f, 0.755389879f, 0.755452700f, 0.755515513f, 0.755578319f,
+    0.755641119f, 0.755703911f, 0.755766697f, 0.755829476f, 0.755892247f,
+    0.755955012f, 0.756017770f, 0.756080521f, 0.756143264f, 0.756206001f,
+    0.756268731f, 0.756331454f, 0.756394170f, 0.756456880f, 0.756519582f,
+    0.756582277f, 0.756644965f, 0.756707647f, 0.756770321f, 0.756832988f,
+    0.756895649f, 0.756958302f, 0.757020949f, 0.757083588f, 0.757146221f,
+    0.757208847f, 0.757271465f, 0.757334077f, 0.757396682f, 0.757459279f,
+    0.757521870f, 0.757584454f, 0.757647031f, 0.757709601f, 0.757772164f,
+    0.757834720f, 0.757897269f, 0.757959811f, 0.758022346f, 0.758084874f,
+    0.758147396f, 0.758209910f, 0.758272417f, 0.758334917f, 0.758397411f,
+    0.758459897f, 0.758522376f, 0.758584849f, 0.758647314f, 0.758709773f,
+    0.758772224f, 0.758834669f, 0.758897106f, 0.758959537f, 0.759021960f,
+    0.759084377f, 0.759146786f, 0.759209189f, 0.759271585f, 0.759333973f,
+    0.759396355f, 0.759458730f, 0.759521097f, 0.759583458f, 0.759645812f,
+    0.759708159f, 0.759770499f, 0.759832831f, 0.759895157f, 0.759957476f,
+    0.760019788f, 0.760082093f, 0.760144391f, 0.760206682f, 0.760268966f,
+    0.760331243f, 0.760393512f, 0.760455775f, 0.760518031f, 0.760580280f,
+    0.760642522f, 0.760704757f, 0.760766985f, 0.760829206f, 0.760891420f,
+    0.760953627f, 0.761015827f, 0.761078020f, 0.761140206f, 0.761202385f,
+    0.761264558f, 0.761326723f, 0.761388881f, 0.761451032f, 0.761513176f,
+    0.761575313f, 0.761637443f, 0.761699566f, 0.761761682f, 0.761823791f,
+    0.761885893f, 0.761947988f, 0.762010076f, 0.762072157f, 0.762134231f,
+    0.762196298f, 0.762258358f, 0.762320411f, 0.762382457f, 0.762444496f,
+    0.762506528f, 0.762568553f, 0.762630571f, 0.762692582f, 0.762754586f,
+    0.762816583f, 0.762878573f, 0.762940556f, 0.763002532f, 0.763064501f,
+    0.763126462f, 0.763188417f, 0.763250365f, 0.763312306f, 0.763374240f,
+    0.763436167f, 0.763498086f, 0.763559999f, 0.763621905f, 0.763683804f,
+    0.763745695f, 0.763807580f, 0.763869458f, 0.763931328f, 0.763993192f,
+    0.764055048f, 0.764116898f, 0.764178741f, 0.764240576f, 0.764302405f,
+    0.764364226f, 0.764426040f, 0.764487848f, 0.764549648f, 0.764611442f,
+    0.764673228f, 0.764735007f, 0.764796780f, 0.764858545f, 0.764920303f,
+    0.764982054f, 0.765043798f, 0.765105536f, 0.765167266f, 0.765228989f,
+    0.765290705f, 0.765352414f, 0.765414116f, 0.765475811f, 0.765537498f,
+    0.765599179f, 0.765660853f, 0.765722520f, 0.765784180f, 0.765845832f,
+    0.765907478f, 0.765969117f, 0.766030748f, 0.766092373f, 0.766153990f,
+    0.766215601f, 0.766277204f, 0.766338800f, 0.766400390f, 0.766461972f,
+    0.766523547f, 0.766585115f, 0.766646677f, 0.766708231f, 0.766769778f,
+    0.766831318f, 0.766892851f, 0.766954377f, 0.767015895f, 0.767077407f,
+    0.767138912f, 0.767200410f, 0.767261900f, 0.767323384f, 0.767384860f,
+    0.767446330f, 0.767507792f, 0.767569248f, 0.767630696f, 0.767692137f,
+    0.767753571f, 0.767814999f, 0.767876419f, 0.767937832f, 0.767999238f,
+    0.768060637f, 0.768122029f, 0.768183413f, 0.768244791f, 0.768306162f,
+    0.768367525f, 0.768428882f, 0.768490231f, 0.768551574f, 0.768612909f,
+    0.768674237f, 0.768735559f, 0.768796873f, 0.768858180f, 0.768919480f,
+    0.768980773f, 0.769042059f, 0.769103338f, 0.769164609f, 0.769225874f,
+    0.769287132f, 0.769348382f, 0.769409626f, 0.769470862f, 0.769532091f,
+    0.769593314f, 0.769654529f, 0.769715737f, 0.769776938f, 0.769838132f,
+    0.769899319f, 0.769960499f, 0.770021671f, 0.770082837f, 0.770143996f,
+    0.770205147f, 0.770266291f, 0.770327429f, 0.770388559f, 0.770449682f,
+    0.770510798f, 0.770571907f, 0.770633009f, 0.770694104f, 0.770755192f,
+    0.770816272f, 0.770877346f, 0.770938413f, 0.770999472f, 0.771060524f,
+    0.771121569f, 0.771182608f, 0.771243639f, 0.771304663f, 0.771365680f,
+    0.771426689f, 0.771487692f, 0.771548688f, 0.771609676f, 0.771670658f,
+    0.771731632f, 0.771792599f, 0.771853559f, 0.771914512f, 0.771975458f,
+    0.772036397f, 0.772097329f, 0.772158254f, 0.772219171f, 0.772280082f,
+    0.772340985f, 0.772401881f, 0.772462770f, 0.772523652f, 0.772584527f,
+    0.772645395f, 0.772706256f, 0.772767110f, 0.772827956f, 0.772888796f,
+    0.772949628f, 0.773010453f, 0.773071272f, 0.773132083f, 0.773192886f,
+    0.773253683f, 0.773314473f, 0.773375256f, 0.773436031f, 0.773496799f,
+    0.773557561f, 0.773618315f, 0.773679062f, 0.773739802f, 0.773800535f,
+    0.773861261f, 0.773921979f, 0.773982691f, 0.774043395f, 0.774104092f,
+    0.774164782f, 0.774225465f, 0.774286141f, 0.774346810f, 0.774407472f,
+    0.774468126f, 0.774528774f, 0.774589414f, 0.774650047f, 0.774710673f,
+    0.774771292f, 0.774831904f, 0.774892509f, 0.774953107f, 0.775013697f,
+    0.775074280f, 0.775134857f, 0.775195426f, 0.775255988f, 0.775316543f,
+    0.775377090f, 0.775437631f, 0.775498164f, 0.775558691f, 0.775619210f,
+    0.775679722f, 0.775740227f, 0.775800725f, 0.775861215f, 0.775921699f,
+    0.775982175f, 0.776042645f, 0.776103107f, 0.776163562f, 0.776224010f,
+    0.776284451f, 0.776344884f, 0.776405311f, 0.776465730f, 0.776526142f,
+    0.776586547f, 0.776646945f, 0.776707336f, 0.776767720f, 0.776828096f,
+    0.776888466f, 0.776948828f, 0.777009183f, 0.777069531f, 0.777129872f,
+    0.777190205f, 0.777250532f, 0.777310851f, 0.777371164f, 0.777431469f,
+    0.777491767f, 0.777552057f, 0.777612341f, 0.777672618f, 0.777732887f,
+    0.777793149f, 0.777853404f, 0.777913652f, 0.777973893f, 0.778034126f,
+    0.778094353f, 0.778154572f, 0.778214784f, 0.778274989f, 0.778335187f,
+    0.778395378f, 0.778455561f, 0.778515738f, 0.778575907f, 0.778636069f,
+    0.778696224f, 0.778756372f, 0.778816512f, 0.778876646f, 0.778936772f,
+    0.778996891f, 0.779057003f, 0.779117108f, 0.779177206f, 0.779237296f,
+    0.779297379f, 0.779357456f, 0.779417524f, 0.779477586f, 0.779537641f,
+    0.779597688f, 0.779657729f, 0.779717762f, 0.779777788f, 0.779837807f,
+    0.779897818f, 0.779957823f, 0.780017820f, 0.780077810f, 0.780137793f,
+    0.780197769f, 0.780257738f, 0.780317699f, 0.780377653f, 0.780437601f,
+    0.780497541f, 0.780557473f, 0.780617399f, 0.780677317f, 0.780737229f,
+    0.780797133f, 0.780857030f, 0.780916919f, 0.780976802f, 0.781036677f,
+    0.781096545f, 0.781156406f, 0.781216260f, 0.781276107f, 0.781335946f,
+    0.781395778f, 0.781455604f, 0.781515421f, 0.781575232f, 0.781635036f,
+    0.781694832f, 0.781754621f, 0.781814403f, 0.781874178f, 0.781933946f,
+    0.781993706f, 0.782053459f, 0.782113205f, 0.782172944f, 0.782232676f,
+    0.782292400f, 0.782352118f, 0.782411828f, 0.782471531f, 0.782531226f,
+    0.782590915f, 0.782650596f, 0.782710270f, 0.782769937f, 0.782829597f,
+    0.782889250f, 0.782948895f, 0.783008533f, 0.783068164f, 0.783127788f,
+    0.783187404f, 0.783247014f, 0.783306616f, 0.783366211f, 0.783425799f,
+    0.783485379f, 0.783544952f, 0.783604519f, 0.783664078f, 0.783723629f,
+    0.783783174f, 0.783842711f, 0.783902241f, 0.783961764f, 0.784021280f,
+    0.784080789f, 0.784140290f, 0.784199784f, 0.784259271f, 0.784318751f,
+    0.784378223f, 0.784437688f, 0.784497146f, 0.784556597f, 0.784616041f,
+    0.784675477f, 0.784734906f, 0.784794328f, 0.784853743f, 0.784913151f,
+    0.784972551f, 0.785031944f, 0.785091330f, 0.785150709f, 0.785210080f,
+    0.785269445f, 0.785328802f, 0.785388152f, 0.785447494f, 0.785506830f,
+    0.785566158f, 0.785625479f, 0.785684792f, 0.785744099f, 0.785803398f,
+    0.785862690f, 0.785921975f, 0.785981253f, 0.786040523f, 0.786099786f,
+    0.786159042f, 0.786218291f, 0.786277532f, 0.786336767f, 0.786395994f,
+    0.786455214f, 0.786514426f, 0.786573632f, 0.786632830f, 0.786692021f,
+    0.786751204f, 0.786810381f, 0.786869550f, 0.786928712f, 0.786987867f,
+    0.787047014f, 0.787106154f, 0.787165287f, 0.787224413f, 0.787283532f,
+    0.787342643f, 0.787401747f, 0.787460844f, 0.787519933f, 0.787579016f,
+    0.787638091f, 0.787697159f, 0.787756220f, 0.787815273f, 0.787874319f,
+    0.787933358f, 0.787992390f, 0.788051414f, 0.788110431f, 0.788169441f,
+    0.788228444f, 0.788287439f, 0.788346428f, 0.788405409f, 0.788464382f,
+    0.788523349f, 0.788582308f, 0.788641260f, 0.788700205f, 0.788759142f,
+    0.788818072f, 0.788876995f, 0.788935911f, 0.788994820f, 0.789053721f,
+    0.789112615f, 0.789171502f, 0.789230381f, 0.789289253f, 0.789348118f,
+    0.789406976f, 0.789465826f, 0.789524669f, 0.789583505f, 0.789642334f,
+    0.789701155f, 0.789759970f, 0.789818776f, 0.789877576f, 0.789936368f,
+    0.789995154f, 0.790053931f, 0.790112702f, 0.790171465f, 0.790230221f,
+    0.790288970f, 0.790347712f, 0.790406446f, 0.790465173f, 0.790523893f,
+    0.790582605f, 0.790641310f, 0.790700008f, 0.790758699f, 0.790817382f,
+    0.790876059f, 0.790934727f, 0.790993389f, 0.791052043f, 0.791110690f,
+    0.791169330f, 0.791227963f, 0.791286588f, 0.791345206f, 0.791403817f,
+    0.791462420f, 0.791521016f, 0.791579605f, 0.791638187f, 0.791696761f,
+    0.791755328f, 0.791813888f, 0.791872440f, 0.791930985f, 0.791989523f,
+    0.792048054f, 0.792106577f, 0.792165093f, 0.792223602f, 0.792282104f,
+    0.792340598f, 0.792399085f, 0.792457565f, 0.792516037f, 0.792574502f,
+    0.792632960f, 0.792691410f, 0.792749854f, 0.792808290f, 0.792866718f,
+    0.792925140f, 0.792983554f, 0.793041960f, 0.793100360f, 0.793158752f,
+    0.793217137f, 0.793275515f, 0.793333885f, 0.793392248f, 0.793450604f,
+    0.793508952f, 0.793567294f, 0.793625627f, 0.793683954f, 0.793742273f,
+    0.793800585f, 0.793858890f, 0.793917187f, 0.793975478f, 0.794033760f,
+    0.794092036f, 0.794150304f, 0.794208565f, 0.794266819f, 0.794325065f,
+    0.794383304f, 0.794441536f, 0.794499760f, 0.794557977f, 0.794616187f,
+    0.794674389f, 0.794732585f, 0.794790772f, 0.794848953f, 0.794907126f,
+    0.794965292f, 0.795023451f, 0.795081602f, 0.795139746f, 0.795197883f,
+    0.795256012f, 0.795314135f, 0.795372249f, 0.795430357f, 0.795488457f,
+    0.795546550f, 0.795604636f, 0.795662714f, 0.795720785f, 0.795778848f,
+    0.795836905f, 0.795894954f, 0.795952995f, 0.796011030f, 0.796069057f,
+    0.796127076f, 0.796185089f, 0.796243094f, 0.796301092f, 0.796359082f,
+    0.796417065f, 0.796475041f, 0.796533009f, 0.796590971f, 0.796648924f,
+    0.796706871f, 0.796764810f, 0.796822742f, 0.796880667f, 0.796938584f,
+    0.796996494f, 0.797054396f, 0.797112292f, 0.797170179f, 0.797228060f,
+    0.797285933f, 0.797343799f, 0.797401658f, 0.797459509f, 0.797517353f,
+    0.797575190f, 0.797633019f, 0.797690841f, 0.797748656f, 0.797806463f,
+    0.797864263f, 0.797922055f, 0.797979841f, 0.798037619f, 0.798095389f,
+    0.798153153f, 0.798210908f, 0.798268657f, 0.798326398f, 0.798384132f,
+    0.798441859f, 0.798499578f, 0.798557290f, 0.798614995f, 0.798672692f,
+    0.798730382f, 0.798788064f, 0.798845740f, 0.798903407f, 0.798961068f,
+    0.799018721f, 0.799076367f, 0.799134005f, 0.799191637f, 0.799249260f,
+    0.799306877f, 0.799364486f, 0.799422088f, 0.799479682f, 0.799537269f,
+    0.799594849f, 0.799652421f, 0.799709986f, 0.799767544f, 0.799825094f,
+    0.799882637f, 0.799940173f, 0.799997701f, 0.800055222f, 0.800112735f,
+    0.800170242f, 0.800227740f, 0.800285232f, 0.800342716f, 0.800400193f,
+    0.800457662f, 0.800515124f, 0.800572579f, 0.800630026f, 0.800687466f,
+    0.800744899f, 0.800802324f, 0.800859742f, 0.800917153f, 0.800974556f,
+    0.801031952f, 0.801089340f, 0.801146721f, 0.801204095f, 0.801261461f,
+    0.801318820f, 0.801376172f, 0.801433516f, 0.801490853f, 0.801548182f,
+    0.801605505f, 0.801662819f, 0.801720127f, 0.801777427f, 0.801834719f,
+    0.801892005f, 0.801949283f, 0.802006553f, 0.802063816f, 0.802121072f,
+    0.802178321f, 0.802235562f, 0.802292796f, 0.802350022f, 0.802407241f,
+    0.802464452f, 0.802521657f, 0.802578853f, 0.802636043f, 0.802693225f,
+    0.802750400f, 0.802807567f, 0.802864727f, 0.802921879f, 0.802979025f,
+    0.803036162f, 0.803093293f, 0.803150416f, 0.803207531f, 0.803264640f,
+    0.803321741f, 0.803378834f, 0.803435920f, 0.803492999f, 0.803550070f,
+    0.803607134f, 0.803664191f, 0.803721240f, 0.803778282f, 0.803835316f,
+    0.803892343f, 0.803949363f, 0.804006375f, 0.804063380f, 0.804120377f,
+    0.804177367f, 0.804234350f, 0.804291325f, 0.804348293f, 0.804405254f,
+    0.804462207f, 0.804519153f, 0.804576091f, 0.804633022f, 0.804689945f,
+    0.804746861f, 0.804803770f, 0.804860672f, 0.804917565f, 0.804974452f,
+    0.805031331f, 0.805088203f, 0.805145067f, 0.805201924f, 0.805258774f,
+    0.805315616f, 0.805372451f, 0.805429278f, 0.805486098f, 0.805542910f,
+    0.805599715f, 0.805656513f, 0.805713303f, 0.805770086f, 0.805826862f,
+    0.805883630f, 0.805940391f, 0.805997144f, 0.806053890f, 0.806110628f,
+    0.806167359f, 0.806224083f, 0.806280799f, 0.806337508f, 0.806394209f,
+    0.806450903f, 0.806507590f, 0.806564269f, 0.806620941f, 0.806677605f,
+    0.806734262f, 0.806790911f, 0.806847554f, 0.806904188f, 0.806960815f,
+    0.807017435f, 0.807074048f, 0.807130653f, 0.807187250f, 0.807243840f,
+    0.807300423f, 0.807356999f, 0.807413566f, 0.807470127f, 0.807526680f,
+    0.807583226f, 0.807639764f, 0.807696295f, 0.807752818f, 0.807809334f,
+    0.807865842f, 0.807922343f, 0.807978837f, 0.808035323f, 0.808091802f,
+    0.808148274f, 0.808204737f, 0.808261194f, 0.808317643f, 0.808374085f,
+    0.808430519f, 0.808486946f, 0.808543365f, 0.808599777f, 0.808656182f,
+    0.808712579f, 0.808768968f, 0.808825350f, 0.808881725f, 0.808938093f,
+    0.808994452f, 0.809050805f, 0.809107150f, 0.809163488f, 0.809219818f,
+    0.809276140f, 0.809332456f, 0.809388764f, 0.809445064f, 0.809501357f,
+    0.809557642f, 0.809613920f, 0.809670191f, 0.809726454f, 0.809782710f,
+    0.809838958f, 0.809895199f, 0.809951433f, 0.810007659f, 0.810063877f,
+    0.810120088f, 0.810176292f, 0.810232488f, 0.810288677f, 0.810344858f,
+    0.810401032f, 0.810457198f, 0.810513357f, 0.810569509f, 0.810625653f,
+    0.810681789f, 0.810737918f, 0.810794040f, 0.810850154f, 0.810906261f,
+    0.810962360f, 0.811018452f, 0.811074537f, 0.811130614f, 0.811186683f,
+    0.811242745f, 0.811298800f, 0.811354847f, 0.811410887f, 0.811466919f,
+    0.811522944f, 0.811578961f, 0.811634971f, 0.811690973f, 0.811746968f,
+    0.811802956f, 0.811858936f, 0.811914908f, 0.811970873f, 0.812026831f,
+    0.812082781f, 0.812138724f, 0.812194659f, 0.812250587f, 0.812306507f,
+    0.812362420f, 0.812418325f, 0.812474223f, 0.812530113f, 0.812585996f,
+    0.812641872f, 0.812697740f, 0.812753600f, 0.812809453f, 0.812865299f,
+    0.812921137f, 0.812976968f, 0.813032791f, 0.813088607f, 0.813144415f,
+    0.813200216f, 0.813256009f, 0.813311795f, 0.813367573f, 0.813423344f,
+    0.813479107f, 0.813534863f, 0.813590612f, 0.813646353f, 0.813702086f,
+    0.813757812f, 0.813813530f, 0.813869242f, 0.813924945f, 0.813980641f,
+    0.814036330f, 0.814092011f, 0.814147684f, 0.814203351f, 0.814259009f,
+    0.814314660f, 0.814370304f, 0.814425940f, 0.814481569f, 0.814537190f,
+    0.814592804f, 0.814648410f, 0.814704009f, 0.814759600f, 0.814815184f,
+    0.814870760f, 0.814926329f, 0.814981890f, 0.815037444f, 0.815092991f,
+    0.815148529f, 0.815204061f, 0.815259585f, 0.815315101f, 0.815370610f,
+    0.815426111f, 0.815481605f, 0.815537091f, 0.815592570f, 0.815648042f,
+    0.815703506f, 0.815758962f, 0.815814411f, 0.815869852f, 0.815925286f,
+    0.815980712f, 0.816036131f, 0.816091543f, 0.816146947f, 0.816202343f,
+    0.816257732f, 0.816313113f, 0.816368487f, 0.816423854f, 0.816479212f,
+    0.816534564f, 0.816589908f, 0.816645244f, 0.816700573f, 0.816755894f,
+    0.816811208f, 0.816866514f, 0.816921813f, 0.816977104f, 0.817032388f,
+    0.817087665f, 0.817142933f, 0.817198195f, 0.817253448f, 0.817308695f,
+    0.817363933f, 0.817419165f, 0.817474388f, 0.817529604f, 0.817584813f,
+    0.817640014f, 0.817695208f, 0.817750394f, 0.817805573f, 0.817860744f,
+    0.817915907f, 0.817971063f, 0.818026212f, 0.818081353f, 0.818136487f,
+    0.818191612f, 0.818246731f, 0.818301842f, 0.818356945f, 0.818412041f,
+    0.818467130f, 0.818522210f, 0.818577284f, 0.818632350f, 0.818687408f,
+    0.818742459f, 0.818797502f, 0.818852538f, 0.818907566f, 0.818962586f,
+    0.819017599f, 0.819072605f, 0.819127603f, 0.819182594f, 0.819237577f,
+    0.819292552f, 0.819347520f, 0.819402480f, 0.819457433f, 0.819512379f,
+    0.819567317f, 0.819622247f, 0.819677170f, 0.819732085f, 0.819786992f,
+    0.819841893f, 0.819896785f, 0.819951670f, 0.820006548f, 0.820061418f,
+    0.820116280f, 0.820171135f, 0.820225983f, 0.820280822f, 0.820335655f,
+    0.820390479f, 0.820445297f, 0.820500106f, 0.820554909f, 0.820609703f,
+    0.820664490f, 0.820719270f, 0.820774042f, 0.820828806f, 0.820883563f,
+    0.820938312f, 0.820993054f, 0.821047788f, 0.821102515f, 0.821157234f,
+    0.821211946f, 0.821266650f, 0.821321346f, 0.821376035f, 0.821430717f,
+    0.821485390f, 0.821540057f, 0.821594716f, 0.821649367f, 0.821704010f,
+    0.821758646f, 0.821813275f, 0.821867896f, 0.821922509f, 0.821977115f,
+    0.822031714f, 0.822086304f, 0.822140888f, 0.822195463f, 0.822250031f,
+    0.822304592f, 0.822359145f, 0.822413690f, 0.822468228f, 0.822522758f,
+    0.822577281f, 0.822631796f, 0.822686304f, 0.822740804f, 0.822795296f,
+    0.822849781f, 0.822904259f, 0.822958729f, 0.823013191f, 0.823067645f,
+    0.823122093f, 0.823176532f, 0.823230964f, 0.823285388f, 0.823339805f,
+    0.823394215f, 0.823448616f, 0.823503010f, 0.823557397f, 0.823611776f,
+    0.823666147f, 0.823720511f, 0.823774868f, 0.823829216f, 0.823883557f,
+    0.823937891f, 0.823992217f, 0.824046535f, 0.824100846f, 0.824155149f,
+    0.824209445f, 0.824263733f, 0.824318014f, 0.824372287f, 0.824426552f,
+    0.824480810f, 0.824535060f, 0.824589303f, 0.824643538f, 0.824697765f,
+    0.824751985f, 0.824806198f, 0.824860402f, 0.824914599f, 0.824968789f,
+    0.825022971f, 0.825077145f, 0.825131312f, 0.825185472f, 0.825239623f,
+    0.825293767f, 0.825347904f, 0.825402033f, 0.825456154f, 0.825510268f,
+    0.825564374f, 0.825618472f, 0.825672563f, 0.825726647f, 0.825780723f,
+    0.825834791f, 0.825888851f, 0.825942904f, 0.825996950f, 0.826050988f,
+    0.826105018f, 0.826159040f, 0.826213056f, 0.826267063f, 0.826321063f,
+    0.826375055f, 0.826429040f, 0.826483017f, 0.826536986f, 0.826590948f,
+    0.826644902f, 0.826698849f, 0.826752788f, 0.826806720f, 0.826860644f,
+    0.826914560f, 0.826968469f, 0.827022370f, 0.827076263f, 0.827130149f,
+    0.827184027f, 0.827237898f, 0.827291761f, 0.827345616f, 0.827399464f,
+    0.827453305f, 0.827507137f, 0.827560962f, 0.827614780f, 0.827668590f,
+    0.827722392f, 0.827776186f, 0.827829973f, 0.827883753f, 0.827937525f,
+    0.827991289f, 0.828045045f, 0.828098794f, 0.828152536f, 0.828206269f,
+    0.828259995f, 0.828313714f, 0.828367425f, 0.828421128f, 0.828474824f,
+    0.828528512f, 0.828582192f, 0.828635865f, 0.828689530f, 0.828743188f,
+    0.828796838f, 0.828850480f, 0.828904115f, 0.828957742f, 0.829011361f,
+    0.829064973f, 0.829118577f, 0.829172174f, 0.829225763f, 0.829279344f,
+    0.829332918f, 0.829386484f, 0.829440043f, 0.829493594f, 0.829547137f,
+    0.829600673f, 0.829654201f, 0.829707721f, 0.829761234f, 0.829814739f,
+    0.829868236f, 0.829921726f, 0.829975209f, 0.830028683f, 0.830082150f,
+    0.830135610f, 0.830189061f, 0.830242505f, 0.830295942f, 0.830349371f,
+    0.830402792f, 0.830456205f, 0.830509611f, 0.830563010f, 0.830616400f,
+    0.830669783f, 0.830723159f, 0.830776526f, 0.830829887f, 0.830883239f,
+    0.830936584f, 0.830989921f, 0.831043251f, 0.831096573f, 0.831149887f,
+    0.831203194f, 0.831256493f, 0.831309784f, 0.831363068f, 0.831416344f,
+    0.831469612f, 0.831522873f, 0.831576126f, 0.831629372f, 0.831682610f,
+    0.831735840f, 0.831789062f, 0.831842277f, 0.831895485f, 0.831948684f,
+    0.832001876f, 0.832055061f, 0.832108237f, 0.832161406f, 0.832214568f,
+    0.832267722f, 0.832320868f, 0.832374006f, 0.832427137f, 0.832480260f,
+    0.832533376f, 0.832586484f, 0.832639584f, 0.832692676f, 0.832745761f,
+    0.832798838f, 0.832851908f, 0.832904970f, 0.832958024f, 0.833011071f,
+    0.833064110f, 0.833117141f, 0.833170165f, 0.833223181f, 0.833276189f,
+    0.833329190f, 0.833382183f, 0.833435168f, 0.833488146f, 0.833541116f,
+    0.833594078f, 0.833647033f, 0.833699980f, 0.833752919f, 0.833805851f,
+    0.833858775f, 0.833911691f, 0.833964600f, 0.834017501f, 0.834070394f,
+    0.834123280f, 0.834176158f, 0.834229029f, 0.834281891f, 0.834334746f,
+    0.834387594f, 0.834440433f, 0.834493266f, 0.834546090f, 0.834598907f,
+    0.834651716f, 0.834704517f, 0.834757311f, 0.834810097f, 0.834862875f,
+    0.834915646f, 0.834968409f, 0.835021164f, 0.835073912f, 0.835126652f,
+    0.835179384f, 0.835232108f, 0.835284825f, 0.835337535f, 0.835390236f,
+    0.835442930f, 0.835495616f, 0.835548295f, 0.835600966f, 0.835653629f,
+    0.835706284f, 0.835758932f, 0.835811572f, 0.835864205f, 0.835916830f,
+    0.835969447f, 0.836022056f, 0.836074658f, 0.836127252f, 0.836179838f,
+    0.836232417f, 0.836284988f, 0.836337551f, 0.836390107f, 0.836442654f,
+    0.836495195f, 0.836547727f, 0.836600252f, 0.836652769f, 0.836705279f,
+    0.836757780f, 0.836810275f, 0.836862761f, 0.836915240f, 0.836967711f,
+    0.837020174f, 0.837072630f, 0.837125077f, 0.837177518f, 0.837229950f,
+    0.837282375f, 0.837334792f, 0.837387202f, 0.837439603f, 0.837491997f,
+    0.837544384f, 0.837596762f, 0.837649133f, 0.837701497f, 0.837753852f,
+    0.837806200f, 0.837858540f, 0.837910873f, 0.837963197f, 0.838015514f,
+    0.838067824f, 0.838120125f, 0.838172419f, 0.838224706f, 0.838276984f,
+    0.838329255f, 0.838381518f, 0.838433773f, 0.838486021f, 0.838538261f,
+    0.838590493f, 0.838642718f, 0.838694935f, 0.838747144f, 0.838799345f,
+    0.838851539f, 0.838903725f, 0.838955904f, 0.839008074f, 0.839060237f,
+    0.839112392f, 0.839164540f, 0.839216679f, 0.839268812f, 0.839320936f,
+    0.839373052f, 0.839425161f, 0.839477263f, 0.839529356f, 0.839581442f,
+    0.839633520f, 0.839685590f, 0.839737653f, 0.839789708f, 0.839841755f,
+    0.839893794f, 0.839945826f, 0.839997850f, 0.840049866f, 0.840101875f,
+    0.840153876f, 0.840205869f, 0.840257854f, 0.840309832f, 0.840361802f,
+    0.840413764f, 0.840465718f, 0.840517665f, 0.840569604f, 0.840621536f,
+    0.840673459f, 0.840725375f, 0.840777283f, 0.840829184f, 0.840881076f,
+    0.840932961f, 0.840984838f, 0.841036708f, 0.841088570f, 0.841140424f,
+    0.841192270f, 0.841244108f, 0.841295939f, 0.841347762f, 0.841399578f,
+    0.841451385f, 0.841503185f, 0.841554977f, 0.841606762f, 0.841658539f,
+    0.841710307f, 0.841762069f, 0.841813822f, 0.841865568f, 0.841917306f,
+    0.841969036f, 0.842020759f, 0.842072474f, 0.842124181f, 0.842175880f,
+    0.842227571f, 0.842279255f, 0.842330931f, 0.842382600f, 0.842434260f,
+    0.842485913f, 0.842537558f, 0.842589196f, 0.842640825f, 0.842692447f,
+    0.842744061f, 0.842795668f, 0.842847266f, 0.842898857f, 0.842950440f,
+    0.843002016f, 0.843053583f, 0.843105143f, 0.843156695f, 0.843208240f,
+    0.843259776f, 0.843311305f, 0.843362826f, 0.843414340f, 0.843465845f,
+    0.843517343f, 0.843568833f, 0.843620316f, 0.843671790f, 0.843723257f,
+    0.843774716f, 0.843826168f, 0.843877611f, 0.843929047f, 0.843980475f,
+    0.844031895f, 0.844083308f, 0.844134713f, 0.844186110f, 0.844237499f,
+    0.844288881f, 0.844340254f, 0.844391621f, 0.844442979f, 0.844494329f,
+    0.844545672f, 0.844597007f, 0.844648334f, 0.844699654f, 0.844750965f,
+    0.844802269f, 0.844853565f, 0.844904854f, 0.844956134f, 0.845007407f,
+    0.845058672f, 0.845109929f, 0.845161179f, 0.845212421f, 0.845263655f,
+    0.845314881f, 0.845366099f, 0.845417310f, 0.845468513f, 0.845519708f,
+    0.845570896f, 0.845622075f, 0.845673247f, 0.845724411f, 0.845775567f,
+    0.845826716f, 0.845877857f, 0.845928990f, 0.845980115f, 0.846031232f,
+    0.846082342f, 0.846133444f, 0.846184538f, 0.846235624f, 0.846286702f,
+    0.846337773f, 0.846388836f, 0.846439891f, 0.846490939f, 0.846541978f,
+    0.846593010f, 0.846644034f, 0.846695051f, 0.846746059f, 0.846797060f,
+    0.846848053f, 0.846899038f, 0.846950015f, 0.847000985f, 0.847051947f,
+    0.847102901f, 0.847153847f, 0.847204785f, 0.847255716f, 0.847306639f,
+    0.847357554f, 0.847408461f, 0.847459361f, 0.847510252f, 0.847561136f,
+    0.847612012f, 0.847662881f, 0.847713741f, 0.847764594f, 0.847815439f,
+    0.847866276f, 0.847917105f, 0.847967927f, 0.848018741f, 0.848069547f,
+    0.848120345f, 0.848171135f, 0.848221918f, 0.848272693f, 0.848323460f,
+    0.848374219f, 0.848424970f, 0.848475714f, 0.848526450f, 0.848577178f,
+    0.848627898f, 0.848678610f, 0.848729315f, 0.848780012f, 0.848830701f,
+    0.848881382f, 0.848932055f, 0.848982721f, 0.849033379f, 0.849084029f,
+    0.849134671f, 0.849185305f, 0.849235932f, 0.849286550f, 0.849337161f,
+    0.849387765f, 0.849438360f, 0.849488947f, 0.849539527f, 0.849590099f,
+    0.849640663f, 0.849691220f, 0.849741768f, 0.849792309f, 0.849842842f,
+    0.849893367f, 0.849943884f, 0.849994393f, 0.850044895f, 0.850095389f,
+    0.850145875f, 0.850196353f, 0.850246823f, 0.850297286f, 0.850347741f,
+    0.850398187f, 0.850448627f, 0.850499058f, 0.850549481f, 0.850599897f,
+    0.850650305f, 0.850700705f, 0.850751097f, 0.850801481f, 0.850851858f,
+    0.850902227f, 0.850952587f, 0.851002941f, 0.851053286f, 0.851103623f,
+    0.851153953f, 0.851204275f, 0.851254589f, 0.851304895f, 0.851355193f,
+    0.851405484f, 0.851455766f, 0.851506041f, 0.851556308f, 0.851606567f,
+    0.851656819f, 0.851707062f, 0.851757298f, 0.851807526f, 0.851857746f,
+    0.851907958f, 0.851958162f, 0.852008359f, 0.852058548f, 0.852108729f,
+    0.852158902f, 0.852209067f, 0.852259224f, 0.852309374f, 0.852359516f,
+    0.852409649f, 0.852459775f, 0.852509894f, 0.852560004f, 0.852610107f,
+    0.852660201f, 0.852710288f, 0.852760367f, 0.852810438f, 0.852860502f,
+    0.852910557f, 0.852960605f, 0.853010645f, 0.853060677f, 0.853110701f,
+    0.853160717f, 0.853210726f, 0.853260726f, 0.853310719f, 0.853360704f,
+    0.853410681f, 0.853460650f, 0.853510612f, 0.853560565f, 0.853610511f,
+    0.853660449f, 0.853710379f, 0.853760301f, 0.853810215f, 0.853860122f,
+    0.853910021f, 0.853959911f, 0.854009794f, 0.854059669f, 0.854109537f,
+    0.854159396f, 0.854209248f, 0.854259091f, 0.854308927f, 0.854358755f,
+    0.854408575f, 0.854458387f, 0.854508192f, 0.854557988f, 0.854607777f,
+    0.854657558f, 0.854707331f, 0.854757096f, 0.854806853f, 0.854856603f,
+    0.854906344f, 0.854956078f, 0.855005804f, 0.855055522f, 0.855105232f,
+    0.855154934f, 0.855204629f, 0.855254315f, 0.855303994f, 0.855353665f,
+    0.855403328f, 0.855452983f, 0.855502630f, 0.855552269f, 0.855601901f,
+    0.855651525f, 0.855701140f, 0.855750748f, 0.855800348f, 0.855849940f,
+    0.855899525f, 0.855949101f, 0.855998670f, 0.856048231f, 0.856097783f,
+    0.856147328f, 0.856196865f, 0.856246395f, 0.856295916f, 0.856345430f,
+    0.856394935f, 0.856444433f, 0.856493923f, 0.856543405f, 0.856592879f,
+    0.856642345f, 0.856691804f, 0.856741254f, 0.856790697f, 0.856840132f,
+    0.856889558f, 0.856938977f, 0.856988389f, 0.857037792f, 0.857087187f,
+    0.857136575f, 0.857185954f, 0.857235326f, 0.857284690f, 0.857334046f,
+    0.857383394f, 0.857432734f, 0.857482067f, 0.857531391f, 0.857580708f,
+    0.857630016f, 0.857679317f, 0.857728610f, 0.857777895f, 0.857827172f,
+    0.857876441f, 0.857925703f, 0.857974956f, 0.858024202f, 0.858073440f,
+    0.858122670f, 0.858171891f, 0.858221106f, 0.858270312f, 0.858319510f,
+    0.858368700f, 0.858417883f, 0.858467058f, 0.858516224f, 0.858565383f,
+    0.858614534f, 0.858663677f, 0.858712812f, 0.858761940f, 0.858811059f,
+    0.858860170f, 0.858909274f, 0.858958370f, 0.859007457f, 0.859056537f,
+    0.859105609f, 0.859154673f, 0.859203730f, 0.859252778f, 0.859301818f,
+    0.859350851f, 0.859399875f, 0.859448892f, 0.859497901f, 0.859546902f,
+    0.859595895f, 0.859644880f, 0.859693857f, 0.859742827f, 0.859791788f,
+    0.859840741f, 0.859889687f, 0.859938625f, 0.859987555f, 0.860036476f,
+    0.860085390f, 0.860134297f, 0.860183195f, 0.860232085f, 0.860280967f,
+    0.860329842f, 0.860378708f, 0.860427567f, 0.860476418f, 0.860525260f,
+    0.860574095f, 0.860622922f, 0.860671741f, 0.860720553f, 0.860769356f,
+    0.860818151f, 0.860866939f, 0.860915718f, 0.860964490f, 0.861013253f,
+    0.861062009f, 0.861110757f, 0.861159497f, 0.861208229f, 0.861256953f,
+    0.861305669f, 0.861354378f, 0.861403078f, 0.861451771f, 0.861500455f,
+    0.861549132f, 0.861597800f, 0.861646461f, 0.861695114f, 0.861743759f,
+    0.861792396f, 0.861841025f, 0.861889646f, 0.861938259f, 0.861986865f,
+    0.862035462f, 0.862084052f, 0.862132633f, 0.862181207f, 0.862229773f,
+    0.862278330f, 0.862326880f, 0.862375422f, 0.862423956f, 0.862472482f,
+    0.862521000f, 0.862569511f, 0.862618013f, 0.862666507f, 0.862714994f,
+    0.862763472f, 0.862811943f, 0.862860405f, 0.862908860f, 0.862957307f,
+    0.863005746f, 0.863054177f, 0.863102600f, 0.863151015f, 0.863199422f,
+    0.863247821f, 0.863296212f, 0.863344595f, 0.863392971f, 0.863441338f,
+    0.863489698f, 0.863538049f, 0.863586393f, 0.863634729f, 0.863683056f,
+    0.863731376f, 0.863779688f, 0.863827992f, 0.863876288f, 0.863924576f,
+    0.863972856f, 0.864021128f, 0.864069393f, 0.864117649f, 0.864165897f,
+    0.864214138f, 0.864262370f, 0.864310594f, 0.864358811f, 0.864407020f,
+    0.864455220f, 0.864503413f, 0.864551598f, 0.864599775f, 0.864647944f,
+    0.864696105f, 0.864744258f, 0.864792403f, 0.864840540f, 0.864888669f,
+    0.864936790f, 0.864984903f, 0.865033009f, 0.865081106f, 0.865129195f,
+    0.865177277f, 0.865225350f, 0.865273416f, 0.865321473f, 0.865369523f,
+    0.865417565f, 0.865465598f, 0.865513624f, 0.865561642f, 0.865609652f,
+    0.865657654f, 0.865705648f, 0.865753634f, 0.865801612f, 0.865849582f,
+    0.865897544f, 0.865945498f, 0.865993444f, 0.866041382f, 0.866089313f,
+    0.866137235f, 0.866185149f, 0.866233056f, 0.866280954f, 0.866328844f,
+    0.866376727f, 0.866424602f, 0.866472468f, 0.866520327f, 0.866568177f,
+    0.866616020f, 0.866663855f, 0.866711681f, 0.866759500f, 0.866807311f,
+    0.866855114f, 0.866902909f, 0.866950696f, 0.866998475f, 0.867046246f,
+    0.867094009f, 0.867141764f, 0.867189511f, 0.867237250f, 0.867284981f,
+    0.867332704f, 0.867380419f, 0.867428126f, 0.867475826f, 0.867523517f,
+    0.867571200f, 0.867618875f, 0.867666543f, 0.867714202f, 0.867761853f,
+    0.867809497f, 0.867857132f, 0.867904760f, 0.867952379f, 0.867999991f,
+    0.868047594f, 0.868095190f, 0.868142777f, 0.868190357f, 0.868237928f,
+    0.868285492f, 0.868333048f, 0.868380595f, 0.868428135f, 0.868475667f,
+    0.868523190f, 0.868570706f, 0.868618214f, 0.868665713f, 0.868713205f,
+    0.868760689f, 0.868808165f, 0.868855633f, 0.868903092f, 0.868950544f,
+    0.868997988f, 0.869045424f, 0.869092852f, 0.869140272f, 0.869187684f,
+    0.869235088f, 0.869282483f, 0.869329871f, 0.869377251f, 0.869424623f,
+    0.869471987f, 0.869519343f, 0.869566691f, 0.869614031f, 0.869661363f,
+    0.869708687f, 0.869756003f, 0.869803311f, 0.869850611f, 0.869897903f,
+    0.869945187f, 0.869992463f, 0.870039731f, 0.870086991f, 0.870134243f,
+    0.870181487f, 0.870228723f, 0.870275951f, 0.870323171f, 0.870370383f,
+    0.870417587f, 0.870464783f, 0.870511971f, 0.870559151f, 0.870606323f,
+    0.870653487f, 0.870700643f, 0.870747791f, 0.870794931f, 0.870842063f,
+    0.870889187f, 0.870936303f, 0.870983411f, 0.871030511f, 0.871077603f,
+    0.871124687f, 0.871171763f, 0.871218831f, 0.871265891f, 0.871312943f,
+    0.871359987f, 0.871407023f, 0.871454051f, 0.871501071f, 0.871548083f,
+    0.871595087f, 0.871642083f, 0.871689070f, 0.871736050f, 0.871783022f,
+    0.871829986f, 0.871876942f, 0.871923889f, 0.871970829f, 0.872017761f,
+    0.872064685f, 0.872111600f, 0.872158508f, 0.872205408f, 0.872252300f,
+    0.872299183f, 0.872346059f, 0.872392927f, 0.872439786f, 0.872486638f,
+    0.872533481f, 0.872580317f, 0.872627144f, 0.872673964f, 0.872720775f,
+    0.872767579f, 0.872814374f, 0.872861162f, 0.872907941f, 0.872954712f,
+    0.873001476f, 0.873048231f, 0.873094978f, 0.873141718f, 0.873188449f,
+    0.873235172f, 0.873281887f, 0.873328595f, 0.873375294f, 0.873421985f,
+    0.873468668f, 0.873515343f, 0.873562010f, 0.873608669f, 0.873655320f,
+    0.873701963f, 0.873748598f, 0.873795225f, 0.873841843f, 0.873888454f,
+    0.873935057f, 0.873981652f, 0.874028239f, 0.874074817f, 0.874121388f,
+    0.874167950f, 0.874214505f, 0.874261052f, 0.874307590f, 0.874354121f,
+    0.874400643f, 0.874447157f, 0.874493664f, 0.874540162f, 0.874586652f,
+    0.874633135f, 0.874679609f, 0.874726075f, 0.874772533f, 0.874818983f,
+    0.874865425f, 0.874911859f, 0.874958285f, 0.875004703f, 0.875051113f,
+    0.875097515f, 0.875143908f, 0.875190294f, 0.875236672f, 0.875283042f,
+    0.875329403f, 0.875375757f, 0.875422102f, 0.875468440f, 0.875514769f,
+    0.875561090f, 0.875607404f, 0.875653709f, 0.875700006f, 0.875746295f,
+    0.875792577f, 0.875838850f, 0.875885115f, 0.875931372f, 0.875977621f,
+    0.876023861f, 0.876070094f, 0.876116319f, 0.876162536f, 0.876208744f,
+    0.876254945f, 0.876301137f, 0.876347322f, 0.876393498f, 0.876439667f,
+    0.876485827f, 0.876531979f, 0.876578124f, 0.876624260f, 0.876670388f,
+    0.876716508f, 0.876762620f, 0.876808724f, 0.876854820f, 0.876900907f,
+    0.876946987f, 0.876993059f, 0.877039123f, 0.877085178f, 0.877131226f,
+    0.877177265f, 0.877223296f, 0.877269320f, 0.877315335f, 0.877361342f,
+    0.877407341f, 0.877453332f, 0.877499315f, 0.877545290f, 0.877591257f,
+    0.877637216f, 0.877683167f, 0.877729109f, 0.877775044f, 0.877820970f,
+    0.877866889f, 0.877912799f, 0.877958701f, 0.878004596f, 0.878050482f,
+    0.878096360f, 0.878142230f, 0.878188092f, 0.878233946f, 0.878279792f,
+    0.878325629f, 0.878371459f, 0.878417281f, 0.878463094f, 0.878508900f,
+    0.878554697f, 0.878600486f, 0.878646267f, 0.878692041f, 0.878737806f,
+    0.878783563f, 0.878829312f, 0.878875052f, 0.878920785f, 0.878966510f,
+    0.879012226f, 0.879057935f, 0.879103635f, 0.879149328f, 0.879195012f,
+    0.879240688f, 0.879286356f, 0.879332016f, 0.879377668f, 0.879423312f,
+    0.879468948f, 0.879514576f, 0.879560195f, 0.879605807f, 0.879651410f,
+    0.879697006f, 0.879742593f, 0.879788172f, 0.879833743f, 0.879879306f,
+    0.879924861f, 0.879970408f, 0.880015947f, 0.880061477f, 0.880107000f,
+    0.880152514f, 0.880198021f, 0.880243519f, 0.880289009f, 0.880334491f,
+    0.880379965f, 0.880425431f, 0.880470889f, 0.880516339f, 0.880561780f,
+    0.880607214f, 0.880652639f, 0.880698057f, 0.880743466f, 0.880788867f,
+    0.880834260f, 0.880879645f, 0.880925022f, 0.880970391f, 0.881015752f,
+    0.881061104f, 0.881106449f, 0.881151785f, 0.881197113f, 0.881242434f,
+    0.881287746f, 0.881333050f, 0.881378346f, 0.881423633f, 0.881468913f,
+    0.881514185f, 0.881559448f, 0.881604704f, 0.881649951f, 0.881695190f,
+    0.881740421f, 0.881785644f, 0.881830859f, 0.881876066f, 0.881921264f,
+    0.881966455f, 0.882011637f, 0.882056812f, 0.882101978f, 0.882147136f,
+    0.882192286f, 0.882237428f, 0.882282562f, 0.882327687f, 0.882372805f,
+    0.882417914f, 0.882463016f, 0.882508109f, 0.882553194f, 0.882598271f,
+    0.882643340f, 0.882688401f, 0.882733453f, 0.882778498f, 0.882823534f,
+    0.882868563f, 0.882913583f, 0.882958595f, 0.883003599f, 0.883048595f,
+    0.883093583f, 0.883138562f, 0.883183534f, 0.883228497f, 0.883273452f,
+    0.883318400f, 0.883363339f, 0.883408270f, 0.883453192f, 0.883498107f,
+    0.883543014f, 0.883587912f, 0.883632802f, 0.883677685f, 0.883722559f,
+    0.883767425f, 0.883812282f, 0.883857132f, 0.883901974f, 0.883946807f,
+    0.883991632f, 0.884036450f, 0.884081259f, 0.884126060f, 0.884170852f,
+    0.884215637f, 0.884260414f, 0.884305182f, 0.884349942f, 0.884394695f,
+    0.884439439f, 0.884484175f, 0.884528902f, 0.884573622f, 0.884618334f,
+    0.884663037f, 0.884707732f, 0.884752419f, 0.884797098f, 0.884841769f,
+    0.884886432f, 0.884931087f, 0.884975733f, 0.885020371f, 0.885065002f,
+    0.885109624f, 0.885154238f, 0.885198843f, 0.885243441f, 0.885288031f,
+    0.885332612f, 0.885377185f, 0.885421750f, 0.885466307f, 0.885510856f,
+    0.885555397f, 0.885599929f, 0.885644454f, 0.885688970f, 0.885733478f,
+    0.885777978f, 0.885822470f, 0.885866954f, 0.885911429f, 0.885955897f,
+    0.886000356f, 0.886044807f, 0.886089250f, 0.886133685f, 0.886178112f,
+    0.886222530f, 0.886266941f, 0.886311343f, 0.886355737f, 0.886400123f,
+    0.886444501f, 0.886488870f, 0.886533232f, 0.886577585f, 0.886621930f,
+    0.886666268f, 0.886710596f, 0.886754917f, 0.886799230f, 0.886843534f,
+    0.886887831f, 0.886932119f, 0.886976399f, 0.887020671f, 0.887064934f,
+    0.887109190f, 0.887153437f, 0.887197677f, 0.887241908f, 0.887286131f,
+    0.887330345f, 0.887374552f, 0.887418750f, 0.887462941f, 0.887507123f,
+    0.887551297f, 0.887595463f, 0.887639620f, 0.887683770f, 0.887727911f,
+    0.887772044f, 0.887816170f, 0.887860286f, 0.887904395f, 0.887948496f,
+    0.887992588f, 0.888036672f, 0.888080748f, 0.888124816f, 0.888168876f,
+    0.888212928f, 0.888256971f, 0.888301006f, 0.888345033f, 0.888389052f,
+    0.888433063f, 0.888477066f, 0.888521060f, 0.888565046f, 0.888609024f,
+    0.888652994f, 0.888696956f, 0.888740910f, 0.888784855f, 0.888828792f,
+    0.888872721f, 0.888916642f, 0.888960555f, 0.889004459f, 0.889048356f,
+    0.889092244f, 0.889136124f, 0.889179996f, 0.889223860f, 0.889267715f,
+    0.889311563f, 0.889355402f, 0.889399233f, 0.889443056f, 0.889486870f,
+    0.889530677f, 0.889574475f, 0.889618265f, 0.889662047f, 0.889705821f,
+    0.889749586f, 0.889793344f, 0.889837093f, 0.889880834f, 0.889924567f,
+    0.889968292f, 0.890012008f, 0.890055716f, 0.890099417f, 0.890143109f,
+    0.890186792f, 0.890230468f, 0.890274135f, 0.890317795f, 0.890361446f,
+    0.890405089f, 0.890448723f, 0.890492350f, 0.890535968f, 0.890579578f,
+    0.890623180f, 0.890666774f, 0.890710359f, 0.890753937f, 0.890797506f,
+    0.890841067f, 0.890884620f, 0.890928164f, 0.890971701f, 0.891015229f,
+    0.891058749f, 0.891102261f, 0.891145765f, 0.891189260f, 0.891232748f,
+    0.891276227f, 0.891319698f, 0.891363160f, 0.891406615f, 0.891450061f,
+    0.891493499f, 0.891536929f, 0.891580351f, 0.891623765f, 0.891667170f,
+    0.891710567f, 0.891753956f, 0.891797337f, 0.891840709f, 0.891884074f,
+    0.891927430f, 0.891970778f, 0.892014118f, 0.892057449f, 0.892100773f,
+    0.892144088f, 0.892187395f, 0.892230694f, 0.892273984f, 0.892317267f,
+    0.892360541f, 0.892403807f, 0.892447064f, 0.892490314f, 0.892533555f,
+    0.892576789f, 0.892620014f, 0.892663230f, 0.892706439f, 0.892749639f,
+    0.892792831f, 0.892836015f, 0.892879191f, 0.892922358f, 0.892965518f,
+    0.893008669f, 0.893051812f, 0.893094946f, 0.893138073f, 0.893181191f,
+    0.893224301f, 0.893267403f, 0.893310497f, 0.893353582f, 0.893396659f,
+    0.893439728f, 0.893482789f, 0.893525842f, 0.893568886f, 0.893611922f,
+    0.893654950f, 0.893697970f, 0.893740981f, 0.893783985f, 0.893826980f,
+    0.893869967f, 0.893912945f, 0.893955916f, 0.893998878f, 0.894041832f,
+    0.894084778f, 0.894127715f, 0.894170644f, 0.894213566f, 0.894256478f,
+    0.894299383f, 0.894342280f, 0.894385168f, 0.894428048f, 0.894470920f,
+    0.894513783f, 0.894556639f, 0.894599486f, 0.894642325f, 0.894685155f,
+    0.894727978f, 0.894770792f, 0.894813598f, 0.894856396f, 0.894899185f,
+    0.894941967f, 0.894984740f, 0.895027505f, 0.895070261f, 0.895113010f,
+    0.895155750f, 0.895198482f, 0.895241206f, 0.895283921f, 0.895326628f,
+    0.895369327f, 0.895412018f, 0.895454701f, 0.895497375f, 0.895540041f,
+    0.895582699f, 0.895625349f, 0.895667990f, 0.895710623f, 0.895753248f,
+    0.895795865f, 0.895838474f, 0.895881074f, 0.895923666f, 0.895966250f,
+    0.896008825f, 0.896051393f, 0.896093952f, 0.896136503f, 0.896179045f,
+    0.896221580f, 0.896264106f, 0.896306624f, 0.896349133f, 0.896391635f,
+    0.896434128f, 0.896476613f, 0.896519090f, 0.896561558f, 0.896604018f,
+    0.896646470f, 0.896688914f, 0.896731349f, 0.896773777f, 0.896816196f,
+    0.896858606f, 0.896901009f, 0.896943403f, 0.896985789f, 0.897028167f,
+    0.897070537f, 0.897112898f, 0.897155251f, 0.897197596f, 0.897239932f,
+    0.897282261f, 0.897324581f, 0.897366893f, 0.897409196f, 0.897451491f,
+    0.897493778f, 0.897536057f, 0.897578328f, 0.897620590f, 0.897662844f,
+    0.897705090f, 0.897747328f, 0.897789557f, 0.897831778f, 0.897873991f,
+    0.897916195f, 0.897958392f, 0.898000580f, 0.898042760f, 0.898084931f,
+    0.898127094f, 0.898169249f, 0.898211396f, 0.898253535f, 0.898295665f,
+    0.898337787f, 0.898379901f, 0.898422006f, 0.898464103f, 0.898506192f,
+    0.898548273f, 0.898590346f, 0.898632410f, 0.898674466f, 0.898716513f,
+    0.898758553f, 0.898800584f, 0.898842607f, 0.898884621f, 0.898926628f,
+    0.898968626f, 0.899010616f, 0.899052597f, 0.899094571f, 0.899136536f,
+    0.899178492f, 0.899220441f, 0.899262381f, 0.899304313f, 0.899346237f,
+    0.899388152f, 0.899430060f, 0.899471959f, 0.899513849f, 0.899555732f,
+    0.899597606f, 0.899639472f, 0.899681329f, 0.899723178f, 0.899765019f,
+    0.899806852f, 0.899848677f, 0.899890493f, 0.899932301f, 0.899974101f,
+    0.900015892f, 0.900057675f, 0.900099450f, 0.900141217f, 0.900182975f,
+    0.900224725f, 0.900266467f, 0.900308200f, 0.900349925f, 0.900391642f,
+    0.900433351f, 0.900475051f, 0.900516744f, 0.900558427f, 0.900600103f,
+    0.900641770f, 0.900683429f, 0.900725080f, 0.900766722f, 0.900808357f,
+    0.900849982f, 0.900891600f, 0.900933209f, 0.900974810f, 0.901016403f,
+    0.901057988f, 0.901099564f, 0.901141132f, 0.901182691f, 0.901224243f,
+    0.901265786f, 0.901307321f, 0.901348847f, 0.901390365f, 0.901431875f,
+    0.901473377f, 0.901514870f, 0.901556355f, 0.901597832f, 0.901639300f,
+    0.901680761f, 0.901722213f, 0.901763656f, 0.901805092f, 0.901846519f,
+    0.901887937f, 0.901929348f, 0.901970750f, 0.902012144f, 0.902053529f,
+    0.902094907f, 0.902136276f, 0.902177637f, 0.902218989f, 0.902260333f,
+    0.902301669f, 0.902342996f, 0.902384316f, 0.902425627f, 0.902466929f,
+    0.902508224f, 0.902549510f, 0.902590788f, 0.902632057f, 0.902673318f,
+    0.902714571f, 0.902755816f, 0.902797052f, 0.902838280f, 0.902879500f,
+    0.902920711f, 0.902961914f, 0.903003109f, 0.903044295f, 0.903085474f,
+    0.903126644f, 0.903167805f, 0.903208958f, 0.903250103f, 0.903291240f,
+    0.903332368f, 0.903373489f, 0.903414600f, 0.903455704f, 0.903496799f,
+    0.903537886f, 0.903578964f, 0.903620035f, 0.903661097f, 0.903702150f,
+    0.903743196f, 0.903784233f, 0.903825261f, 0.903866282f, 0.903907294f,
+    0.903948298f, 0.903989293f, 0.904030280f, 0.904071259f, 0.904112230f,
+    0.904153192f, 0.904194146f, 0.904235092f, 0.904276029f, 0.904316958f,
+    0.904357879f, 0.904398791f, 0.904439695f, 0.904480591f, 0.904521478f,
+    0.904562357f, 0.904603228f, 0.904644091f, 0.904684945f, 0.904725791f,
+    0.904766628f, 0.904807457f, 0.904848278f, 0.904889091f, 0.904929895f,
+    0.904970691f, 0.905011479f, 0.905052258f, 0.905093029f, 0.905133792f,
+    0.905174546f, 0.905215292f, 0.905256030f, 0.905296759f, 0.905337480f,
+    0.905378193f, 0.905418898f, 0.905459594f, 0.905500282f, 0.905540961f,
+    0.905581632f, 0.905622295f, 0.905662949f, 0.905703596f, 0.905744233f,
+    0.905784863f, 0.905825484f, 0.905866097f, 0.905906702f, 0.905947298f,
+    0.905987886f, 0.906028465f, 0.906069036f, 0.906109599f, 0.906150154f,
+    0.906190700f, 0.906231238f, 0.906271768f, 0.906312289f, 0.906352802f,
+    0.906393307f, 0.906433803f, 0.906474291f, 0.906514770f, 0.906555242f,
+    0.906595705f, 0.906636159f, 0.906676605f, 0.906717043f, 0.906757473f,
+    0.906797894f, 0.906838307f, 0.906878712f, 0.906919108f, 0.906959496f,
+    0.906999875f, 0.907040247f, 0.907080610f, 0.907120964f, 0.907161310f,
+    0.907201648f, 0.907241978f, 0.907282299f, 0.907322612f, 0.907362917f,
+    0.907403213f, 0.907443501f, 0.907483780f, 0.907524051f, 0.907564314f,
+    0.907604569f, 0.907644815f, 0.907685053f, 0.907725282f, 0.907765503f,
+    0.907805716f, 0.907845920f, 0.907886116f, 0.907926304f, 0.907966484f,
+    0.908006655f, 0.908046817f, 0.908086972f, 0.908127118f, 0.908167255f,
+    0.908207385f, 0.908247506f, 0.908287618f, 0.908327723f, 0.908367819f,
+    0.908407906f, 0.908447985f, 0.908488056f, 0.908528119f, 0.908568173f,
+    0.908608219f, 0.908648256f, 0.908688285f, 0.908728306f, 0.908768318f,
+    0.908808323f, 0.908848318f, 0.908888306f, 0.908928285f, 0.908968255f,
+    0.909008218f, 0.909048171f, 0.909088117f, 0.909128054f, 0.909167983f,
+    0.909207904f, 0.909247816f, 0.909287720f, 0.909327615f, 0.909367502f,
+    0.909407381f, 0.909447251f, 0.909487113f, 0.909526967f, 0.909566812f,
+    0.909606649f, 0.909646477f, 0.909686298f, 0.909726110f, 0.909765913f,
+    0.909805708f, 0.909845495f, 0.909885273f, 0.909925043f, 0.909964805f,
+    0.910004558f, 0.910044303f, 0.910084040f, 0.910123768f, 0.910163488f,
+    0.910203199f, 0.910242902f, 0.910282597f, 0.910322283f, 0.910361961f,
+    0.910401631f, 0.910441292f, 0.910480945f, 0.910520590f, 0.910560226f,
+    0.910599854f, 0.910639473f, 0.910679084f, 0.910718687f, 0.910758281f,
+    0.910797867f, 0.910837445f, 0.910877014f, 0.910916575f, 0.910956127f,
+    0.910995671f, 0.911035207f, 0.911074734f, 0.911114253f, 0.911153764f,
+    0.911193266f, 0.911232760f, 0.911272245f, 0.911311722f, 0.911351191f,
+    0.911390651f, 0.911430103f, 0.911469547f, 0.911508982f, 0.911548409f,
+    0.911587827f, 0.911627237f, 0.911666639f, 0.911706032f, 0.911745417f,
+    0.911784793f, 0.911824162f, 0.911863521f, 0.911902873f, 0.911942216f,
+    0.911981550f, 0.912020877f, 0.912060194f, 0.912099504f, 0.912138805f,
+    0.912178098f, 0.912217382f, 0.912256658f, 0.912295925f, 0.912335185f,
+    0.912374435f, 0.912413678f, 0.912452912f, 0.912492137f, 0.912531355f,
+    0.912570563f, 0.912609764f, 0.912648956f, 0.912688140f, 0.912727315f,
+    0.912766482f, 0.912805640f, 0.912844790f, 0.912883932f, 0.912923065f,
+    0.912962190f, 0.913001307f, 0.913040415f, 0.913079515f, 0.913118606f,
+    0.913157689f, 0.913196764f, 0.913235830f, 0.913274888f, 0.913313937f,
+    0.913352978f, 0.913392011f, 0.913431035f, 0.913470051f, 0.913509058f,
+    0.913548057f, 0.913587048f, 0.913626030f, 0.913665004f, 0.913703969f,
+    0.913742926f, 0.913781875f, 0.913820815f, 0.913859747f, 0.913898671f,
+    0.913937586f, 0.913976492f, 0.914015391f, 0.914054280f, 0.914093162f,
+    0.914132035f, 0.914170899f, 0.914209756f, 0.914248604f, 0.914287443f,
+    0.914326274f, 0.914365097f, 0.914403911f, 0.914442717f, 0.914481514f,
+    0.914520303f, 0.914559084f, 0.914597856f, 0.914636619f, 0.914675375f,
+    0.914714122f, 0.914752860f, 0.914791591f, 0.914830312f, 0.914869026f,
+    0.914907730f, 0.914946427f, 0.914985115f, 0.915023795f, 0.915062466f,
+    0.915101129f, 0.915139783f, 0.915178429f, 0.915217067f, 0.915255696f,
+    0.915294317f, 0.915332929f, 0.915371533f, 0.915410129f, 0.915448716f,
+    0.915487295f, 0.915525865f, 0.915564427f, 0.915602981f, 0.915641526f,
+    0.915680062f, 0.915718590f, 0.915757110f, 0.915795622f, 0.915834125f,
+    0.915872619f, 0.915911105f, 0.915949583f, 0.915988052f, 0.916026513f,
+    0.916064966f, 0.916103410f, 0.916141845f, 0.916180273f, 0.916218691f,
+    0.916257102f, 0.916295504f, 0.916333897f, 0.916372282f, 0.916410659f,
+    0.916449027f, 0.916487387f, 0.916525739f, 0.916564082f, 0.916602416f,
+    0.916640742f, 0.916679060f, 0.916717369f, 0.916755670f, 0.916793962f,
+    0.916832246f, 0.916870522f, 0.916908789f, 0.916947048f, 0.916985298f,
+    0.917023540f, 0.917061773f, 0.917099998f, 0.917138215f, 0.917176423f,
+    0.917214623f, 0.917252814f, 0.917290997f, 0.917329171f, 0.917367337f,
+    0.917405495f, 0.917443644f, 0.917481785f, 0.917519917f, 0.917558041f,
+    0.917596156f, 0.917634263f, 0.917672362f, 0.917710452f, 0.917748533f,
+    0.917786607f, 0.917824671f, 0.917862728f, 0.917900776f, 0.917938815f,
+    0.917976846f, 0.918014869f, 0.918052883f, 0.918090889f, 0.918128886f,
+    0.918166875f, 0.918204855f, 0.918242827f, 0.918280791f, 0.918318746f,
+    0.918356692f, 0.918394630f, 0.918432560f, 0.918470481f, 0.918508394f,
+    0.918546299f, 0.918584195f, 0.918622082f, 0.918659961f, 0.918697832f,
+    0.918735694f, 0.918773548f, 0.918811393f, 0.918849230f, 0.918887059f,
+    0.918924879f, 0.918962690f, 0.919000493f, 0.919038288f, 0.919076074f,
+    0.919113852f, 0.919151621f, 0.919189382f, 0.919227134f, 0.919264878f,
+    0.919302614f, 0.919340341f, 0.919378059f, 0.919415769f, 0.919453471f,
+    0.919491164f, 0.919528849f, 0.919566525f, 0.919604193f, 0.919641853f,
+    0.919679504f, 0.919717146f, 0.919754780f, 0.919792406f, 0.919830023f,
+    0.919867632f, 0.919905232f, 0.919942824f, 0.919980407f, 0.920017982f,
+    0.920055549f, 0.920093107f, 0.920130656f, 0.920168197f, 0.920205730f,
+    0.920243254f, 0.920280769f, 0.920318277f, 0.920355775f, 0.920393266f,
+    0.920430748f, 0.920468221f, 0.920505686f, 0.920543142f, 0.920580590f,
+    0.920618030f, 0.920655461f, 0.920692884f, 0.920730298f, 0.920767703f,
+    0.920805101f, 0.920842489f, 0.920879870f, 0.920917242f, 0.920954605f,
+    0.920991960f, 0.921029306f, 0.921066644f, 0.921103974f, 0.921141295f,
+    0.921178607f, 0.921215911f, 0.921253207f, 0.921290494f, 0.921327773f,
+    0.921365043f, 0.921402305f, 0.921439558f, 0.921476803f, 0.921514039f,
+    0.921551267f, 0.921588487f, 0.921625698f, 0.921662900f, 0.921700094f,
+    0.921737280f, 0.921774457f, 0.921811625f, 0.921848785f, 0.921885937f,
+    0.921923080f, 0.921960215f, 0.921997341f, 0.922034459f, 0.922071568f,
+    0.922108669f, 0.922145761f, 0.922182845f, 0.922219920f, 0.922256987f,
+    0.922294046f, 0.922331095f, 0.922368137f, 0.922405170f, 0.922442194f,
+    0.922479210f, 0.922516218f, 0.922553217f, 0.922590208f, 0.922627190f,
+    0.922664163f, 0.922701128f, 0.922738085f, 0.922775033f, 0.922811973f,
+    0.922848904f, 0.922885827f, 0.922922741f, 0.922959647f, 0.922996544f,
+    0.923033433f, 0.923070313f, 0.923107185f, 0.923144048f, 0.923180903f,
+    0.923217749f, 0.923254587f, 0.923291417f, 0.923328238f, 0.923365050f,
+    0.923401854f, 0.923438649f, 0.923475436f, 0.923512215f, 0.923548985f,
+    0.923585746f, 0.923622499f, 0.923659244f, 0.923695980f, 0.923732707f,
+    0.923769426f, 0.923806137f, 0.923842839f, 0.923879533f, 0.923916218f,
+    0.923952894f, 0.923989562f, 0.924026222f, 0.924062873f, 0.924099516f,
+    0.924136150f, 0.924172775f, 0.924209392f, 0.924246001f, 0.924282601f,
+    0.924319193f, 0.924355776f, 0.924392351f, 0.924428917f, 0.924465474f,
+    0.924502023f, 0.924538564f, 0.924575096f, 0.924611620f, 0.924648135f,
+    0.924684642f, 0.924721140f, 0.924757630f, 0.924794111f, 0.924830583f,
+    0.924867048f, 0.924903503f, 0.924939950f, 0.924976389f, 0.925012819f,
+    0.925049241f, 0.925085654f, 0.925122059f, 0.925158455f, 0.925194842f,
+    0.925231221f, 0.925267592f, 0.925303954f, 0.925340308f, 0.925376653f,
+    0.925412990f, 0.925449318f, 0.925485637f, 0.925521948f, 0.925558251f,
+    0.925594545f, 0.925630831f, 0.925667108f, 0.925703376f, 0.925739636f,
+    0.925775888f, 0.925812131f, 0.925848365f, 0.925884591f, 0.925920809f,
+    0.925957018f, 0.925993218f, 0.926029410f, 0.926065594f, 0.926101768f,
+    0.926137935f, 0.926174093f, 0.926210242f, 0.926246383f, 0.926282515f,
+    0.926318639f, 0.926354755f, 0.926390861f, 0.926426960f, 0.926463049f,
+    0.926499131f, 0.926535203f, 0.926571268f, 0.926607323f, 0.926643371f,
+    0.926679409f, 0.926715440f, 0.926751461f, 0.926787474f, 0.926823479f,
+    0.926859475f, 0.926895463f, 0.926931442f, 0.926967412f, 0.927003374f,
+    0.927039328f, 0.927075273f, 0.927111209f, 0.927147137f, 0.927183056f,
+    0.927218967f, 0.927254870f, 0.927290764f, 0.927326649f, 0.927362526f,
+    0.927398394f, 0.927434254f, 0.927470105f, 0.927505948f, 0.927541782f,
+    0.927577607f, 0.927613425f, 0.927649233f, 0.927685033f, 0.927720825f,
+    0.927756608f, 0.927792382f, 0.927828148f, 0.927863906f, 0.927899654f,
+    0.927935395f, 0.927971127f, 0.928006850f, 0.928042565f, 0.928078271f,
+    0.928113969f, 0.928149658f, 0.928185339f, 0.928221011f, 0.928256674f,
+    0.928292329f, 0.928327976f, 0.928363614f, 0.928399243f, 0.928434864f,
+    0.928470477f, 0.928506080f, 0.928541676f, 0.928577263f, 0.928612841f,
+    0.928648411f, 0.928683972f, 0.928719524f, 0.928755068f, 0.928790604f,
+    0.928826131f, 0.928861650f, 0.928897160f, 0.928932661f, 0.928968154f,
+    0.929003638f, 0.929039114f, 0.929074581f, 0.929110040f, 0.929145490f,
+    0.929180932f, 0.929216365f, 0.929251789f, 0.929287205f, 0.929322613f,
+    0.929358012f, 0.929393402f, 0.929428784f, 0.929464157f, 0.929499522f,
+    0.929534878f, 0.929570226f, 0.929605565f, 0.929640896f, 0.929676218f,
+    0.929711531f, 0.929746836f, 0.929782133f, 0.929817421f, 0.929852700f,
+    0.929887971f, 0.929923233f, 0.929958487f, 0.929993732f, 0.930028968f,
+    0.930064196f, 0.930099416f, 0.930134627f, 0.930169829f, 0.930205023f,
+    0.930240208f, 0.930275385f, 0.930310553f, 0.930345713f, 0.930380864f,
+    0.930416006f, 0.930451140f, 0.930486266f, 0.930521383f, 0.930556491f,
+    0.930591591f, 0.930626682f, 0.930661764f, 0.930696839f, 0.930731904f,
+    0.930766961f, 0.930802010f, 0.930837049f, 0.930872081f, 0.930907103f,
+    0.930942118f, 0.930977123f, 0.931012120f, 0.931047109f, 0.931082089f,
+    0.931117060f, 0.931152023f, 0.931186977f, 0.931221923f, 0.931256860f,
+    0.931291789f, 0.931326709f, 0.931361621f, 0.931396524f, 0.931431418f,
+    0.931466304f, 0.931501181f, 0.931536050f, 0.931570910f, 0.931605761f,
+    0.931640604f, 0.931675439f, 0.931710265f, 0.931745082f, 0.931779891f,
+    0.931814691f, 0.931849483f, 0.931884266f, 0.931919040f, 0.931953806f,
+    0.931988563f, 0.932023312f, 0.932058052f, 0.932092784f, 0.932127507f,
+    0.932162222f, 0.932196928f, 0.932231625f, 0.932266314f, 0.932300994f,
+    0.932335666f, 0.932370329f, 0.932404983f, 0.932439629f, 0.932474267f,
+    0.932508895f, 0.932543516f, 0.932578127f, 0.932612731f, 0.932647325f,
+    0.932681911f, 0.932716488f, 0.932751057f, 0.932785617f, 0.932820169f,
+    0.932854712f, 0.932889247f, 0.932923773f, 0.932958290f, 0.932992799f,
+    0.933027299f, 0.933061791f, 0.933096274f, 0.933130748f, 0.933165214f,
+    0.933199671f, 0.933234120f, 0.933268560f, 0.933302992f, 0.933337415f,
+    0.933371829f, 0.933406235f, 0.933440633f, 0.933475021f, 0.933509401f,
+    0.933543773f, 0.933578136f, 0.933612490f, 0.933646836f, 0.933681173f,
+    0.933715502f, 0.933749822f, 0.933784133f, 0.933818436f, 0.933852731f,
+    0.933887016f, 0.933921294f, 0.933955562f, 0.933989822f, 0.934024073f,
+    0.934058316f, 0.934092550f, 0.934126776f, 0.934160993f, 0.934195202f,
+    0.934229401f, 0.934263593f, 0.934297775f, 0.934331949f, 0.934366115f,
+    0.934400272f, 0.934434420f, 0.934468560f, 0.934502691f, 0.934536814f,
+    0.934570928f, 0.934605033f, 0.934639130f, 0.934673218f, 0.934707298f,
+    0.934741369f, 0.934775431f, 0.934809485f, 0.934843530f, 0.934877567f,
+    0.934911595f, 0.934945614f, 0.934979625f, 0.935013627f, 0.935047621f,
+    0.935081606f, 0.935115583f, 0.935149551f, 0.935183510f, 0.935217461f,
+    0.935251403f, 0.935285336f, 0.935319261f, 0.935353177f, 0.935387085f,
+    0.935420984f, 0.935454875f, 0.935488757f, 0.935522630f, 0.935556495f,
+    0.935590351f, 0.935624198f, 0.935658037f, 0.935691868f, 0.935725689f,
+    0.935759503f, 0.935793307f, 0.935827103f, 0.935860890f, 0.935894669f,
+    0.935928439f, 0.935962201f, 0.935995954f, 0.936029698f, 0.936063434f,
+    0.936097161f, 0.936130879f, 0.936164589f, 0.936198290f, 0.936231983f,
+    0.936265667f, 0.936299343f, 0.936333010f, 0.936366668f, 0.936400317f,
+    0.936433958f, 0.936467591f, 0.936501215f, 0.936534830f, 0.936568437f,
+    0.936602035f, 0.936635624f, 0.936669205f, 0.936702777f, 0.936736340f,
+    0.936769895f, 0.936803442f, 0.936836979f, 0.936870509f, 0.936904029f,
+    0.936937541f, 0.936971044f, 0.937004539f, 0.937038025f, 0.937071502f,
+    0.937104971f, 0.937138432f, 0.937171883f, 0.937205326f, 0.937238760f,
+    0.937272186f, 0.937305603f, 0.937339012f, 0.937372412f, 0.937405803f,
+    0.937439186f, 0.937472560f, 0.937505925f, 0.937539282f, 0.937572630f,
+    0.937605970f, 0.937639301f, 0.937672623f, 0.937705937f, 0.937739242f,
+    0.937772539f, 0.937805827f, 0.937839106f, 0.937872376f, 0.937905638f,
+    0.937938892f, 0.937972137f, 0.938005373f, 0.938038600f, 0.938071819f,
+    0.938105030f, 0.938138231f, 0.938171424f, 0.938204609f, 0.938237784f,
+    0.938270952f, 0.938304110f, 0.938337260f, 0.938370401f, 0.938403534f,
+    0.938436658f, 0.938469774f, 0.938502880f, 0.938535978f, 0.938569068f,
+    0.938602149f, 0.938635221f, 0.938668285f, 0.938701340f, 0.938734386f,
+    0.938767424f, 0.938800453f, 0.938833474f, 0.938866486f, 0.938899489f,
+    0.938932484f, 0.938965470f, 0.938998447f, 0.939031416f, 0.939064376f,
+    0.939097327f, 0.939130270f, 0.939163204f, 0.939196130f, 0.939229047f,
+    0.939261955f, 0.939294855f, 0.939327746f, 0.939360628f, 0.939393502f,
+    0.939426367f, 0.939459224f, 0.939492071f, 0.939524911f, 0.939557741f,
+    0.939590563f, 0.939623377f, 0.939656181f, 0.939688977f, 0.939721765f,
+    0.939754543f, 0.939787314f, 0.939820075f, 0.939852828f, 0.939885572f,
+    0.939918308f, 0.939951035f, 0.939983753f, 0.940016463f, 0.940049164f,
+    0.940081856f, 0.940114540f, 0.940147215f, 0.940179881f, 0.940212539f,
+    0.940245188f, 0.940277829f, 0.940310461f, 0.940343084f, 0.940375699f,
+    0.940408305f, 0.940440902f, 0.940473491f, 0.940506071f, 0.940538642f,
+    0.940571205f, 0.940603759f, 0.940636304f, 0.940668841f, 0.940701369f,
+    0.940733889f, 0.940766400f, 0.940798902f, 0.940831395f, 0.940863880f,
+    0.940896356f, 0.940928824f, 0.940961283f, 0.940993733f, 0.941026175f,
+    0.941058608f, 0.941091032f, 0.941123448f, 0.941155855f, 0.941188254f,
+    0.941220643f, 0.941253025f, 0.941285397f, 0.941317761f, 0.941350116f,
+    0.941382462f, 0.941414800f, 0.941447130f, 0.941479450f, 0.941511762f,
+    0.941544065f, 0.941576360f, 0.941608646f, 0.941640923f, 0.941673192f,
+    0.941705452f, 0.941737703f, 0.941769946f, 0.941802179f, 0.941834405f,
+    0.941866622f, 0.941898830f, 0.941931029f, 0.941963220f, 0.941995402f,
+    0.942027575f, 0.942059740f, 0.942091896f, 0.942124043f, 0.942156182f,
+    0.942188312f, 0.942220434f, 0.942252546f, 0.942284650f, 0.942316746f,
+    0.942348833f, 0.942380911f, 0.942412980f, 0.942445041f, 0.942477093f,
+    0.942509137f, 0.942541171f, 0.942573198f, 0.942605215f, 0.942637224f,
+    0.942669224f, 0.942701216f, 0.942733198f, 0.942765173f, 0.942797138f,
+    0.942829095f, 0.942861043f, 0.942892983f, 0.942924913f, 0.942956836f,
+    0.942988749f, 0.943020654f, 0.943052550f, 0.943084437f, 0.943116316f,
+    0.943148186f, 0.943180048f, 0.943211901f, 0.943243745f, 0.943275580f,
+    0.943307407f, 0.943339225f, 0.943371035f, 0.943402836f, 0.943434628f,
+    0.943466411f, 0.943498186f, 0.943529952f, 0.943561709f, 0.943593458f,
+    0.943625198f, 0.943656930f, 0.943688652f, 0.943720366f, 0.943752072f,
+    0.943783769f, 0.943815457f, 0.943847136f, 0.943878807f, 0.943910469f,
+    0.943942122f, 0.943973767f, 0.944005403f, 0.944037030f, 0.944068649f,
+    0.944100258f, 0.944131860f, 0.944163452f, 0.944195036f, 0.944226612f,
+    0.944258178f, 0.944289736f, 0.944321285f, 0.944352826f, 0.944384357f,
+    0.944415881f, 0.944447395f, 0.944478901f, 0.944510398f, 0.944541886f,
+    0.944573366f, 0.944604837f, 0.944636300f, 0.944667753f, 0.944699198f,
+    0.944730635f, 0.944762062f, 0.944793481f, 0.944824892f, 0.944856293f,
+    0.944887686f, 0.944919070f, 0.944950446f, 0.944981813f, 0.945013171f,
+    0.945044520f, 0.945075861f, 0.945107193f, 0.945138517f, 0.945169831f,
+    0.945201137f, 0.945232435f, 0.945263724f, 0.945295004f, 0.945326275f,
+    0.945357537f, 0.945388791f, 0.945420037f, 0.945451273f, 0.945482501f,
+    0.945513720f, 0.945544931f, 0.945576132f, 0.945607325f, 0.945638510f,
+    0.945669685f, 0.945700852f, 0.945732011f, 0.945763160f, 0.945794301f,
+    0.945825434f, 0.945856557f, 0.945887672f, 0.945918778f, 0.945949876f,
+    0.945980964f, 0.946012044f, 0.946043116f, 0.946074178f, 0.946105232f,
+    0.946136278f, 0.946167314f, 0.946198342f, 0.946229361f, 0.946260372f,
+    0.946291374f, 0.946322367f, 0.946353351f, 0.946384327f, 0.946415294f,
+    0.946446252f, 0.946477202f, 0.946508143f, 0.946539075f, 0.946569998f,
+    0.946600913f, 0.946631819f, 0.946662717f, 0.946693605f, 0.946724485f,
+    0.946755357f, 0.946786219f, 0.946817073f, 0.946847918f, 0.946878755f,
+    0.946909582f, 0.946940402f, 0.946971212f, 0.947002014f, 0.947032807f,
+    0.947063591f, 0.947094366f, 0.947125133f, 0.947155891f, 0.947186641f,
+    0.947217381f, 0.947248114f, 0.947278837f, 0.947309551f, 0.947340257f,
+    0.947370955f, 0.947401643f, 0.947432323f, 0.947462994f, 0.947493656f,
+    0.947524310f, 0.947554955f, 0.947585591f, 0.947616219f, 0.947646837f,
+    0.947677447f, 0.947708049f, 0.947738642f, 0.947769225f, 0.947799801f,
+    0.947830367f, 0.947860925f, 0.947891474f, 0.947922015f, 0.947952546f,
+    0.947983069f, 0.948013584f, 0.948044089f, 0.948074586f, 0.948105074f,
+    0.948135553f, 0.948166024f, 0.948196486f, 0.948226939f, 0.948257384f,
+    0.948287820f, 0.948318247f, 0.948348665f, 0.948379075f, 0.948409476f,
+    0.948439868f, 0.948470252f, 0.948500626f, 0.948530993f, 0.948561350f,
+    0.948591699f, 0.948622038f, 0.948652370f, 0.948682692f, 0.948713006f,
+    0.948743311f, 0.948773607f, 0.948803895f, 0.948834174f, 0.948864444f,
+    0.948894705f, 0.948924958f, 0.948955202f, 0.948985437f, 0.949015664f,
+    0.949045882f, 0.949076091f, 0.949106291f, 0.949136483f, 0.949166666f,
+    0.949196840f, 0.949227006f, 0.949257162f, 0.949287310f, 0.949317450f,
+    0.949347580f, 0.949377702f, 0.949407815f, 0.949437920f, 0.949468015f,
+    0.949498102f, 0.949528181f, 0.949558250f, 0.949588311f, 0.949618363f,
+    0.949648406f, 0.949678441f, 0.949708467f, 0.949738484f, 0.949768492f,
+    0.949798492f, 0.949828483f, 0.949858465f, 0.949888438f, 0.949918403f,
+    0.949948359f, 0.949978306f, 0.950008245f, 0.950038175f, 0.950068096f,
+    0.950098008f, 0.950127912f, 0.950157807f, 0.950187693f, 0.950217570f,
+    0.950247439f, 0.950277299f, 0.950307150f, 0.950336993f, 0.950366826f,
+    0.950396651f, 0.950426468f, 0.950456275f, 0.950486074f, 0.950515864f,
+    0.950545645f, 0.950575418f, 0.950605182f, 0.950634937f, 0.950664683f,
+    0.950694421f, 0.950724150f, 0.950753870f, 0.950783581f, 0.950813284f,
+    0.950842978f, 0.950872663f, 0.950902340f, 0.950932007f, 0.950961666f,
+    0.950991317f, 0.951020958f, 0.951050591f, 0.951080215f, 0.951109830f,
+    0.951139437f, 0.951169034f, 0.951198623f, 0.951228204f, 0.951257775f,
+    0.951287338f, 0.951316892f, 0.951346437f, 0.951375974f, 0.951405502f,
+    0.951435021f, 0.951464531f, 0.951494033f, 0.951523526f, 0.951553010f,
+    0.951582485f, 0.951611952f, 0.951641410f, 0.951670859f, 0.951700299f,
+    0.951729731f, 0.951759154f, 0.951788568f, 0.951817973f, 0.951847370f,
+    0.951876758f, 0.951906137f, 0.951935507f, 0.951964869f, 0.951994222f,
+    0.952023566f, 0.952052901f, 0.952082228f, 0.952111546f, 0.952140855f,
+    0.952170155f, 0.952199447f, 0.952228730f, 0.952258004f, 0.952287269f,
+    0.952316526f, 0.952345774f, 0.952375013f, 0.952404243f, 0.952433465f,
+    0.952462677f, 0.952491882f, 0.952521077f, 0.952550263f, 0.952579441f,
+    0.952608610f, 0.952637771f, 0.952666922f, 0.952696065f, 0.952725199f,
+    0.952754324f, 0.952783441f, 0.952812549f, 0.952841648f, 0.952870738f,
+    0.952899819f, 0.952928892f, 0.952957956f, 0.952987011f, 0.953016058f,
+    0.953045095f, 0.953074124f, 0.953103144f, 0.953132156f, 0.953161159f,
+    0.953190152f, 0.953219138f, 0.953248114f, 0.953277082f, 0.953306040f,
+    0.953334990f, 0.953363932f, 0.953392864f, 0.953421788f, 0.953450703f,
+    0.953479609f, 0.953508507f, 0.953537396f, 0.953566276f, 0.953595147f,
+    0.953624009f, 0.953652863f, 0.953681708f, 0.953710544f, 0.953739371f,
+    0.953768190f, 0.953797000f, 0.953825801f, 0.953854593f, 0.953883377f,
+    0.953912151f, 0.953940917f, 0.953969675f, 0.953998423f, 0.954027163f,
+    0.954055894f, 0.954084616f, 0.954113329f, 0.954142034f, 0.954170730f,
+    0.954199417f, 0.954228095f, 0.954256765f, 0.954285425f, 0.954314077f,
+    0.954342721f, 0.954371355f, 0.954399981f, 0.954428598f, 0.954457206f,
+    0.954485805f, 0.954514396f, 0.954542978f, 0.954571551f, 0.954600115f,
+    0.954628670f, 0.954657217f, 0.954685755f, 0.954714284f, 0.954742804f,
+    0.954771316f, 0.954799819f, 0.954828313f, 0.954856798f, 0.954885275f,
+    0.954913742f, 0.954942201f, 0.954970652f, 0.954999093f, 0.955027526f,
+    0.955055949f, 0.955084365f, 0.955112771f, 0.955141168f, 0.955169557f,
+    0.955197937f, 0.955226308f, 0.955254671f, 0.955283024f, 0.955311369f,
+    0.955339705f, 0.955368032f, 0.955396351f, 0.955424660f, 0.955452961f,
+    0.955481253f, 0.955509537f, 0.955537811f, 0.955566077f, 0.955594334f,
+    0.955622582f, 0.955650822f, 0.955679052f, 0.955707274f, 0.955735487f,
+    0.955763692f, 0.955791887f, 0.955820074f, 0.955848252f, 0.955876421f,
+    0.955904581f, 0.955932733f, 0.955960876f, 0.955989010f, 0.956017135f,
+    0.956045251f, 0.956073359f, 0.956101458f, 0.956129548f, 0.956157629f,
+    0.956185702f, 0.956213765f, 0.956241820f, 0.956269866f, 0.956297904f,
+    0.956325932f, 0.956353952f, 0.956381963f, 0.956409965f, 0.956437959f,
+    0.956465943f, 0.956493919f, 0.956521886f, 0.956549844f, 0.956577794f,
+    0.956605734f, 0.956633666f, 0.956661589f, 0.956689503f, 0.956717409f,
+    0.956745305f, 0.956773193f, 0.956801072f, 0.956828943f, 0.956856804f,
+    0.956884657f, 0.956912501f, 0.956940336f, 0.956968162f, 0.956995980f,
+    0.957023788f, 0.957051588f, 0.957079379f, 0.957107162f, 0.957134935f,
+    0.957162700f, 0.957190456f, 0.957218203f, 0.957245941f, 0.957273671f,
+    0.957301391f, 0.957329103f, 0.957356806f, 0.957384501f, 0.957412186f,
+    0.957439863f, 0.957467531f, 0.957495190f, 0.957522840f, 0.957550482f,
+    0.957578115f, 0.957605739f, 0.957633354f, 0.957660960f, 0.957688558f,
+    0.957716146f, 0.957743726f, 0.957771297f, 0.957798860f, 0.957826413f,
+    0.957853958f, 0.957881494f, 0.957909021f, 0.957936539f, 0.957964048f,
+    0.957991549f, 0.958019041f, 0.958046524f, 0.958073998f, 0.958101464f,
+    0.958128920f, 0.958156368f, 0.958183807f, 0.958211237f, 0.958238659f,
+    0.958266071f, 0.958293475f, 0.958320870f, 0.958348256f, 0.958375634f,
+    0.958403002f, 0.958430362f, 0.958457713f, 0.958485055f, 0.958512388f,
+    0.958539713f, 0.958567029f, 0.958594335f, 0.958621634f, 0.958648923f,
+    0.958676203f, 0.958703475f, 0.958730738f, 0.958757992f, 0.958785237f,
+    0.958812473f, 0.958839701f, 0.958866920f, 0.958894130f, 0.958921331f,
+    0.958948523f, 0.958975707f, 0.959002881f, 0.959030047f, 0.959057204f,
+    0.959084352f, 0.959111492f, 0.959138622f, 0.959165744f, 0.959192857f,
+    0.959219961f, 0.959247057f, 0.959274143f, 0.959301221f, 0.959328290f,
+    0.959355350f, 0.959382401f, 0.959409444f, 0.959436477f, 0.959463502f,
+    0.959490518f, 0.959517525f, 0.959544524f, 0.959571513f, 0.959598494f,
+    0.959625466f, 0.959652429f, 0.959679383f, 0.959706328f, 0.959733265f,
+    0.959760193f, 0.959787112f, 0.959814022f, 0.959840923f, 0.959867816f,
+    0.959894699f, 0.959921574f, 0.959948440f, 0.959975297f, 0.960002146f,
+    0.960028985f, 0.960055816f, 0.960082638f, 0.960109451f, 0.960136255f,
+    0.960163051f, 0.960189837f, 0.960216615f, 0.960243384f, 0.960270144f,
+    0.960296895f, 0.960323638f, 0.960350371f, 0.960377096f, 0.960403812f,
+    0.960430519f, 0.960457218f, 0.960483907f, 0.960510588f, 0.960537260f,
+    0.960563923f, 0.960590577f, 0.960617222f, 0.960643859f, 0.960670487f,
+    0.960697105f, 0.960723715f, 0.960750317f, 0.960776909f, 0.960803493f,
+    0.960830067f, 0.960856633f, 0.960883190f, 0.960909738f, 0.960936278f,
+    0.960962808f, 0.960989330f, 0.961015843f, 0.961042347f, 0.961068842f,
+    0.961095329f, 0.961121806f, 0.961148275f, 0.961174735f, 0.961201186f,
+    0.961227628f, 0.961254061f, 0.961280486f, 0.961306902f, 0.961333308f,
+    0.961359706f, 0.961386096f, 0.961412476f, 0.961438847f, 0.961465210f,
+    0.961491564f, 0.961517909f, 0.961544245f, 0.961570572f, 0.961596891f,
+    0.961623201f, 0.961649501f, 0.961675793f, 0.961702077f, 0.961728351f,
+    0.961754616f, 0.961780873f, 0.961807121f, 0.961833360f, 0.961859590f,
+    0.961885811f, 0.961912023f, 0.961938227f, 0.961964422f, 0.961990608f,
+    0.962016785f, 0.962042953f, 0.962069112f, 0.962095263f, 0.962121404f,
+    0.962147537f, 0.962173661f, 0.962199776f, 0.962225882f, 0.962251980f,
+    0.962278069f, 0.962304148f, 0.962330219f, 0.962356281f, 0.962382334f,
+    0.962408379f, 0.962434414f, 0.962460441f, 0.962486459f, 0.962512468f,
+    0.962538468f, 0.962564459f, 0.962590442f, 0.962616415f, 0.962642380f,
+    0.962668336f, 0.962694283f, 0.962720221f, 0.962746151f, 0.962772071f,
+    0.962797983f, 0.962823886f, 0.962849780f, 0.962875665f, 0.962901541f,
+    0.962927408f, 0.962953267f, 0.962979117f, 0.963004957f, 0.963030789f,
+    0.963056613f, 0.963082427f, 0.963108232f, 0.963134029f, 0.963159817f,
+    0.963185596f, 0.963211366f, 0.963237127f, 0.963262879f, 0.963288623f,
+    0.963314357f, 0.963340083f, 0.963365800f, 0.963391508f, 0.963417207f,
+    0.963442897f, 0.963468579f, 0.963494251f, 0.963519915f, 0.963545570f,
+    0.963571216f, 0.963596853f, 0.963622482f, 0.963648101f, 0.963673712f,
+    0.963699314f, 0.963724907f, 0.963750491f, 0.963776066f, 0.963801632f,
+    0.963827190f, 0.963852738f, 0.963878278f, 0.963903809f, 0.963929331f,
+    0.963954844f, 0.963980348f, 0.964005844f, 0.964031330f, 0.964056808f,
+    0.964082277f, 0.964107737f, 0.964133188f, 0.964158631f, 0.964184064f,
+    0.964209489f, 0.964234904f, 0.964260311f, 0.964285709f, 0.964311098f,
+    0.964336478f, 0.964361850f, 0.964387212f, 0.964412566f, 0.964437911f,
+    0.964463247f, 0.964488574f, 0.964513892f, 0.964539201f, 0.964564502f,
+    0.964589793f, 0.964615076f, 0.964640350f, 0.964665615f, 0.964690871f,
+    0.964716118f, 0.964741357f, 0.964766586f, 0.964791807f, 0.964817019f,
+    0.964842222f, 0.964867416f, 0.964892601f, 0.964917777f, 0.964942945f,
+    0.964968103f, 0.964993253f, 0.965018394f, 0.965043526f, 0.965068649f,
+    0.965093763f, 0.965118868f, 0.965143965f, 0.965169052f, 0.965194131f,
+    0.965219201f, 0.965244262f, 0.965269314f, 0.965294357f, 0.965319392f,
+    0.965344417f, 0.965369434f, 0.965394442f, 0.965419441f, 0.965444431f,
+    0.965469412f, 0.965494384f, 0.965519347f, 0.965544302f, 0.965569248f,
+    0.965594184f, 0.965619112f, 0.965644031f, 0.965668941f, 0.965693843f,
+    0.965718735f, 0.965743618f, 0.965768493f, 0.965793359f, 0.965818216f,
+    0.965843064f, 0.965867903f, 0.965892733f, 0.965917554f, 0.965942367f,
+    0.965967171f, 0.965991965f, 0.966016751f, 0.966041528f, 0.966066296f,
+    0.966091055f, 0.966115806f, 0.966140547f, 0.966165280f, 0.966190003f,
+    0.966214718f, 0.966239424f, 0.966264121f, 0.966288809f, 0.966313489f,
+    0.966338159f, 0.966362821f, 0.966387473f, 0.966412117f, 0.966436752f,
+    0.966461378f, 0.966485995f, 0.966510603f, 0.966535202f, 0.966559793f,
+    0.966584374f, 0.966608947f, 0.966633511f, 0.966658066f, 0.966682612f,
+    0.966707149f, 0.966731677f, 0.966756197f, 0.966780707f, 0.966805209f,
+    0.966829701f, 0.966854185f, 0.966878660f, 0.966903126f, 0.966927583f,
+    0.966952032f, 0.966976471f, 0.967000902f, 0.967025323f, 0.967049736f,
+    0.967074140f, 0.967098535f, 0.967122921f, 0.967147298f, 0.967171666f,
+    0.967196025f, 0.967220376f, 0.967244718f, 0.967269050f, 0.967293374f,
+    0.967317689f, 0.967341995f, 0.967366292f, 0.967390580f, 0.967414860f,
+    0.967439130f, 0.967463392f, 0.967487645f, 0.967511888f, 0.967536123f,
+    0.967560349f, 0.967584566f, 0.967608775f, 0.967632974f, 0.967657164f,
+    0.967681346f, 0.967705519f, 0.967729682f, 0.967753837f, 0.967777983f,
+    0.967802120f, 0.967826248f, 0.967850368f, 0.967874478f, 0.967898579f,
+    0.967922672f, 0.967946756f, 0.967970830f, 0.967994896f, 0.968018953f,
+    0.968043001f, 0.968067041f, 0.968091071f, 0.968115092f, 0.968139105f,
+    0.968163108f, 0.968187103f, 0.968211089f, 0.968235066f, 0.968259034f,
+    0.968282993f, 0.968306943f, 0.968330884f, 0.968354817f, 0.968378740f,
+    0.968402655f, 0.968426561f, 0.968450457f, 0.968474345f, 0.968498224f,
+    0.968522094f, 0.968545955f, 0.968569808f, 0.968593651f, 0.968617486f,
+    0.968641311f, 0.968665128f, 0.968688936f, 0.968712734f, 0.968736524f,
+    0.968760305f, 0.968784078f, 0.968807841f, 0.968831595f, 0.968855341f,
+    0.968879077f, 0.968902805f, 0.968926523f, 0.968950233f, 0.968973934f,
+    0.968997626f, 0.969021309f, 0.969044983f, 0.969068649f, 0.969092305f,
+    0.969115953f, 0.969139591f, 0.969163221f, 0.969186842f, 0.969210453f,
+    0.969234056f, 0.969257650f, 0.969281235f, 0.969304812f, 0.969328379f,
+    0.969351937f, 0.969375487f, 0.969399027f, 0.969422559f, 0.969446082f,
+    0.969469595f, 0.969493100f, 0.969516596f, 0.969540083f, 0.969563562f,
+    0.969587031f, 0.969610491f, 0.969633943f, 0.969657385f, 0.969680819f,
+    0.969704243f, 0.969727659f, 0.969751066f, 0.969774464f, 0.969797853f,
+    0.969821233f, 0.969844604f, 0.969867967f, 0.969891320f, 0.969914665f,
+    0.969938000f, 0.969961327f, 0.969984644f, 0.970007953f, 0.970031253f,
+    0.970054544f, 0.970077826f, 0.970101099f, 0.970124364f, 0.970147619f,
+    0.970170865f, 0.970194103f, 0.970217331f, 0.970240551f, 0.970263762f,
+    0.970286963f, 0.970310156f, 0.970333340f, 0.970356515f, 0.970379681f,
+    0.970402839f, 0.970425987f, 0.970449126f, 0.970472257f, 0.970495378f,
+    0.970518491f, 0.970541595f, 0.970564689f, 0.970587775f, 0.970610852f,
+    0.970633920f, 0.970656979f, 0.970680029f, 0.970703071f, 0.970726103f,
+    0.970749126f, 0.970772141f, 0.970795146f, 0.970818143f, 0.970841131f,
+    0.970864109f, 0.970887079f, 0.970910040f, 0.970932992f, 0.970955935f,
+    0.970978869f, 0.971001795f, 0.971024711f, 0.971047618f, 0.971070517f,
+    0.971093406f, 0.971116287f, 0.971139158f, 0.971162021f, 0.971184875f,
+    0.971207720f, 0.971230556f, 0.971253383f, 0.971276201f, 0.971299010f,
+    0.971321810f, 0.971344602f, 0.971367384f, 0.971390158f, 0.971412922f,
+    0.971435678f, 0.971458424f, 0.971481162f, 0.971503891f, 0.971526611f,
+    0.971549322f, 0.971572024f, 0.971594717f, 0.971617401f, 0.971640076f,
+    0.971662743f, 0.971685400f, 0.971708048f, 0.971730688f, 0.971753319f,
+    0.971775940f, 0.971798553f, 0.971821157f, 0.971843752f, 0.971866337f,
+    0.971888914f, 0.971911483f, 0.971934042f, 0.971956592f, 0.971979133f,
+    0.972001665f, 0.972024189f, 0.972046703f, 0.972069209f, 0.972091705f,
+    0.972114193f, 0.972136672f, 0.972159141f, 0.972181602f, 0.972204054f,
+    0.972226497f, 0.972248931f, 0.972271356f, 0.972293772f, 0.972316180f,
+    0.972338578f, 0.972360967f, 0.972383348f, 0.972405719f, 0.972428082f,
+    0.972450435f, 0.972472780f, 0.972495115f, 0.972517442f, 0.972539760f,
+    0.972562069f, 0.972584369f, 0.972606660f, 0.972628942f, 0.972651215f,
+    0.972673479f, 0.972695735f, 0.972717981f, 0.972740218f, 0.972762447f,
+    0.972784666f, 0.972806877f, 0.972829078f, 0.972851271f, 0.972873455f,
+    0.972895629f, 0.972917795f, 0.972939952f, 0.972962100f, 0.972984239f,
+    0.973006369f, 0.973028490f, 0.973050603f, 0.973072706f, 0.973094800f,
+    0.973116885f, 0.973138962f, 0.973161029f, 0.973183088f, 0.973205137f,
+    0.973227178f, 0.973249210f, 0.973271232f, 0.973293246f, 0.973315251f,
+    0.973337247f, 0.973359234f, 0.973381212f, 0.973403181f, 0.973425141f,
+    0.973447092f, 0.973469034f, 0.973490967f, 0.973512892f, 0.973534807f,
+    0.973556714f, 0.973578611f, 0.973600499f, 0.973622379f, 0.973644250f,
+    0.973666111f, 0.973687964f, 0.973709808f, 0.973731643f, 0.973753468f,
+    0.973775285f, 0.973797093f, 0.973818892f, 0.973840682f, 0.973862464f,
+    0.973884236f, 0.973905999f, 0.973927753f, 0.973949498f, 0.973971235f,
+    0.973992962f, 0.974014681f, 0.974036390f, 0.974058091f, 0.974079782f,
+    0.974101465f, 0.974123139f, 0.974144803f, 0.974166459f, 0.974188106f,
+    0.974209744f, 0.974231373f, 0.974252993f, 0.974274604f, 0.974296206f,
+    0.974317799f, 0.974339383f, 0.974360958f, 0.974382524f, 0.974404081f,
+    0.974425630f, 0.974447169f, 0.974468699f, 0.974490221f, 0.974511733f,
+    0.974533237f, 0.974554731f, 0.974576217f, 0.974597694f, 0.974619161f,
+    0.974640620f, 0.974662070f, 0.974683511f, 0.974704943f, 0.974726365f,
+    0.974747779f, 0.974769184f, 0.974790580f, 0.974811967f, 0.974833345f,
+    0.974854715f, 0.974876075f, 0.974897426f, 0.974918768f, 0.974940102f,
+    0.974961426f, 0.974982741f, 0.975004048f, 0.975025345f, 0.975046634f,
+    0.975067913f, 0.975089184f, 0.975110445f, 0.975131698f, 0.975152941f,
+    0.975174176f, 0.975195402f, 0.975216619f, 0.975237827f, 0.975259025f,
+    0.975280215f, 0.975301396f, 0.975322568f, 0.975343731f, 0.975364885f,
+    0.975386030f, 0.975407166f, 0.975428293f, 0.975449412f, 0.975470521f,
+    0.975491621f, 0.975512712f, 0.975533795f, 0.975554868f, 0.975575932f,
+    0.975596988f, 0.975618034f, 0.975639071f, 0.975660100f, 0.975681119f,
+    0.975702130f, 0.975723132f, 0.975744124f, 0.975765108f, 0.975786083f,
+    0.975807048f, 0.975828005f, 0.975848953f, 0.975869892f, 0.975890821f,
+    0.975911742f, 0.975932654f, 0.975953557f, 0.975974451f, 0.975995336f,
+    0.976016212f, 0.976037079f, 0.976057937f, 0.976078786f, 0.976099626f,
+    0.976120457f, 0.976141280f, 0.976162093f, 0.976182897f, 0.976203692f,
+    0.976224479f, 0.976245256f, 0.976266024f, 0.976286784f, 0.976307534f,
+    0.976328275f, 0.976349008f, 0.976369731f, 0.976390446f, 0.976411151f,
+    0.976431848f, 0.976452535f, 0.976473214f, 0.976493884f, 0.976514544f,
+    0.976535196f, 0.976555839f, 0.976576472f, 0.976597097f, 0.976617713f,
+    0.976638320f, 0.976658917f, 0.976679506f, 0.976700086f, 0.976720657f,
+    0.976741219f, 0.976761772f, 0.976782316f, 0.976802851f, 0.976823377f,
+    0.976843894f, 0.976864402f, 0.976884901f, 0.976905391f, 0.976925872f,
+    0.976946344f, 0.976966807f, 0.976987261f, 0.977007706f, 0.977028143f,
+    0.977048570f, 0.977068988f, 0.977089397f, 0.977109798f, 0.977130189f,
+    0.977150571f, 0.977170944f, 0.977191309f, 0.977211664f, 0.977232011f,
+    0.977252348f, 0.977272676f, 0.977292996f, 0.977313306f, 0.977333608f,
+    0.977353900f, 0.977374184f, 0.977394458f, 0.977414724f, 0.977434980f,
+    0.977455228f, 0.977475466f, 0.977495696f, 0.977515917f, 0.977536128f,
+    0.977556331f, 0.977576524f, 0.977596709f, 0.977616885f, 0.977637051f,
+    0.977657209f, 0.977677358f, 0.977697498f, 0.977717628f, 0.977737750f,
+    0.977757863f, 0.977777967f, 0.977798061f, 0.977818147f, 0.977838224f,
+    0.977858292f, 0.977878351f, 0.977898401f, 0.977918441f, 0.977938473f,
+    0.977958496f, 0.977978510f, 0.977998515f, 0.978018511f, 0.978038498f,
+    0.978058476f, 0.978078445f, 0.978098405f, 0.978118356f, 0.978138298f,
+    0.978158231f, 0.978178155f, 0.978198070f, 0.978217976f, 0.978237873f,
+    0.978257761f, 0.978277640f, 0.978297510f, 0.978317371f, 0.978337223f,
+    0.978357066f, 0.978376900f, 0.978396725f, 0.978416541f, 0.978436348f,
+    0.978456146f, 0.978475935f, 0.978495715f, 0.978515487f, 0.978535249f,
+    0.978555002f, 0.978574746f, 0.978594481f, 0.978614207f, 0.978633924f,
+    0.978653633f, 0.978673332f, 0.978693022f, 0.978712703f, 0.978732375f,
+    0.978752038f, 0.978771693f, 0.978791338f, 0.978810974f, 0.978830601f,
+    0.978850219f, 0.978869829f, 0.978889429f, 0.978909020f, 0.978928602f,
+    0.978948175f, 0.978967740f, 0.978987295f, 0.979006841f, 0.979026378f,
+    0.979045906f, 0.979065426f, 0.979084936f, 0.979104437f, 0.979123929f,
+    0.979143412f, 0.979162887f, 0.979182352f, 0.979201808f, 0.979221255f,
+    0.979240693f, 0.979260123f, 0.979279543f, 0.979298954f, 0.979318356f,
+    0.979337749f, 0.979357134f, 0.979376509f, 0.979395875f, 0.979415232f,
+    0.979434580f, 0.979453920f, 0.979473250f, 0.979492571f, 0.979511883f,
+    0.979531186f, 0.979550481f, 0.979569766f, 0.979589042f, 0.979608309f,
+    0.979627567f, 0.979646816f, 0.979666056f, 0.979685288f, 0.979704510f,
+    0.979723723f, 0.979742927f, 0.979762122f, 0.979781308f, 0.979800485f,
+    0.979819653f, 0.979838813f, 0.979857963f, 0.979877104f, 0.979896236f,
+    0.979915359f, 0.979934473f, 0.979953578f, 0.979972674f, 0.979991761f,
+    0.980010839f, 0.980029908f, 0.980048968f, 0.980068019f, 0.980087061f,
+    0.980106094f, 0.980125118f, 0.980144133f, 0.980163139f, 0.980182136f,
+    0.980201124f, 0.980220103f, 0.980239073f, 0.980258034f, 0.980276986f,
+    0.980295928f, 0.980314862f, 0.980333787f, 0.980352703f, 0.980371610f,
+    0.980390508f, 0.980409397f, 0.980428276f, 0.980447147f, 0.980466009f,
+    0.980484862f, 0.980503706f, 0.980522540f, 0.980541366f, 0.980560183f,
+    0.980578990f, 0.980597789f, 0.980616579f, 0.980635360f, 0.980654131f,
+    0.980672894f, 0.980691647f, 0.980710392f, 0.980729128f, 0.980747854f,
+    0.980766572f, 0.980785280f, 0.980803980f, 0.980822670f, 0.980841352f,
+    0.980860024f, 0.980878688f, 0.980897342f, 0.980915988f, 0.980934624f,
+    0.980953252f, 0.980971870f, 0.980990480f, 0.981009080f, 0.981027671f,
+    0.981046254f, 0.981064827f, 0.981083391f, 0.981101946f, 0.981120493f,
+    0.981139030f, 0.981157558f, 0.981176077f, 0.981194588f, 0.981213089f,
+    0.981231581f, 0.981250064f, 0.981268538f, 0.981287003f, 0.981305459f,
+    0.981323906f, 0.981342344f, 0.981360773f, 0.981379193f, 0.981397604f,
+    0.981416006f, 0.981434399f, 0.981452783f, 0.981471158f, 0.981489524f,
+    0.981507881f, 0.981526228f, 0.981544567f, 0.981562897f, 0.981581218f,
+    0.981599530f, 0.981617832f, 0.981636126f, 0.981654411f, 0.981672686f,
+    0.981690953f, 0.981709210f, 0.981727459f, 0.981745699f, 0.981763929f,
+    0.981782151f, 0.981800363f, 0.981818566f, 0.981836761f, 0.981854946f,
+    0.981873123f, 0.981891290f, 0.981909448f, 0.981927598f, 0.981945738f,
+    0.981963869f, 0.981981991f, 0.982000105f, 0.982018209f, 0.982036304f,
+    0.982054390f, 0.982072467f, 0.982090535f, 0.982108594f, 0.982126644f,
+    0.982144685f, 0.982162717f, 0.982180740f, 0.982198754f, 0.982216759f,
+    0.982234755f, 0.982252741f, 0.982270719f, 0.982288688f, 0.982306648f,
+    0.982324598f, 0.982342540f, 0.982360473f, 0.982378396f, 0.982396311f,
+    0.982414216f, 0.982432113f, 0.982450000f, 0.982467879f, 0.982485748f,
+    0.982503609f, 0.982521460f, 0.982539302f, 0.982557136f, 0.982574960f,
+    0.982592775f, 0.982610581f, 0.982628378f, 0.982646167f, 0.982663946f,
+    0.982681716f, 0.982699477f, 0.982717229f, 0.982734972f, 0.982752706f,
+    0.982770431f, 0.982788147f, 0.982805853f, 0.982823551f, 0.982841240f,
+    0.982858920f, 0.982876590f, 0.982894252f, 0.982911905f, 0.982929548f,
+    0.982947183f, 0.982964808f, 0.982982425f, 0.983000032f, 0.983017631f,
+    0.983035220f, 0.983052801f, 0.983070372f, 0.983087934f, 0.983105487f,
+    0.983123032f, 0.983140567f, 0.983158093f, 0.983175610f, 0.983193118f,
+    0.983210617f, 0.983228107f, 0.983245588f, 0.983263060f, 0.983280523f,
+    0.983297977f, 0.983315422f, 0.983332857f, 0.983350284f, 0.983367702f,
+    0.983385110f, 0.983402510f, 0.983419900f, 0.983437282f, 0.983454655f,
+    0.983472018f, 0.983489372f, 0.983506718f, 0.983524054f, 0.983541381f,
+    0.983558700f, 0.983576009f, 0.983593309f, 0.983610600f, 0.983627882f,
+    0.983645155f, 0.983662419f, 0.983679674f, 0.983696920f, 0.983714157f,
+    0.983731385f, 0.983748604f, 0.983765813f, 0.983783014f, 0.983800206f,
+    0.983817388f, 0.983834562f, 0.983851726f, 0.983868882f, 0.983886028f,
+    0.983903166f, 0.983920294f, 0.983937413f, 0.983954524f, 0.983971625f,
+    0.983988717f, 0.984005800f, 0.984022874f, 0.984039939f, 0.984056995f,
+    0.984074042f, 0.984091080f, 0.984108109f, 0.984125129f, 0.984142140f,
+    0.984159141f, 0.984176134f, 0.984193118f, 0.984210092f, 0.984227058f,
+    0.984244014f, 0.984260962f, 0.984277900f, 0.984294830f, 0.984311750f,
+    0.984328661f, 0.984345563f, 0.984362457f, 0.984379341f, 0.984396216f,
+    0.984413082f, 0.984429939f, 0.984446787f, 0.984463626f, 0.984480455f,
+    0.984497276f, 0.984514088f, 0.984530891f, 0.984547684f, 0.984564469f,
+    0.984581244f, 0.984598011f, 0.984614768f, 0.984631517f, 0.984648256f,
+    0.984664986f, 0.984681707f, 0.984698420f, 0.984715123f, 0.984731817f,
+    0.984748502f, 0.984765178f, 0.984781845f, 0.984798503f, 0.984815151f,
+    0.984831791f, 0.984848422f, 0.984865043f, 0.984881656f, 0.984898260f,
+    0.984914854f, 0.984931440f, 0.984948016f, 0.984964583f, 0.984981142f,
+    0.984997691f, 0.985014231f, 0.985030762f, 0.985047284f, 0.985063797f,
+    0.985080301f, 0.985096796f, 0.985113282f, 0.985129759f, 0.985146226f,
+    0.985162685f, 0.985179135f, 0.985195575f, 0.985212007f, 0.985228429f,
+    0.985244843f, 0.985261247f, 0.985277642f, 0.985294029f, 0.985310406f,
+    0.985326774f, 0.985343133f, 0.985359483f, 0.985375824f, 0.985392156f,
+    0.985408479f, 0.985424792f, 0.985441097f, 0.985457393f, 0.985473679f,
+    0.985489957f, 0.985506226f, 0.985522485f, 0.985538735f, 0.985554977f,
+    0.985571209f, 0.985587432f, 0.985603646f, 0.985619851f, 0.985636047f,
+    0.985652234f, 0.985668412f, 0.985684581f, 0.985700741f, 0.985716891f,
+    0.985733033f, 0.985749166f, 0.985765289f, 0.985781404f, 0.985797509f,
+    0.985813606f, 0.985829693f, 0.985845771f, 0.985861840f, 0.985877900f,
+    0.985893951f, 0.985909993f, 0.985926026f, 0.985942050f, 0.985958065f,
+    0.985974071f, 0.985990067f, 0.986006055f, 0.986022033f, 0.986038003f,
+    0.986053963f, 0.986069915f, 0.986085857f, 0.986101790f, 0.986117714f,
+    0.986133629f, 0.986149535f, 0.986165432f, 0.986181320f, 0.986197199f,
+    0.986213069f, 0.986228930f, 0.986244781f, 0.986260624f, 0.986276457f,
+    0.986292282f, 0.986308097f, 0.986323904f, 0.986339701f, 0.986355489f,
+    0.986371268f, 0.986387038f, 0.986402799f, 0.986418551f, 0.986434294f,
+    0.986450028f, 0.986465752f, 0.986481468f, 0.986497175f, 0.986512872f,
+    0.986528561f, 0.986544240f, 0.986559910f, 0.986575572f, 0.986591224f,
+    0.986606867f, 0.986622501f, 0.986638126f, 0.986653742f, 0.986669349f,
+    0.986684946f, 0.986700535f, 0.986716115f, 0.986731685f, 0.986747247f,
+    0.986762799f, 0.986778342f, 0.986793877f, 0.986809402f, 0.986824918f,
+    0.986840425f, 0.986855923f, 0.986871412f, 0.986886892f, 0.986902363f,
+    0.986917824f, 0.986933277f, 0.986948720f, 0.986964155f, 0.986979580f,
+    0.986994997f, 0.987010404f, 0.987025802f, 0.987041191f, 0.987056571f,
+    0.987071942f, 0.987087304f, 0.987102657f, 0.987118001f, 0.987133335f,
+    0.987148661f, 0.987163978f, 0.987179285f, 0.987194583f, 0.987209873f,
+    0.987225153f, 0.987240424f, 0.987255686f, 0.987270939f, 0.987286183f,
+    0.987301418f, 0.987316644f, 0.987331861f, 0.987347068f, 0.987362267f,
+    0.987377456f, 0.987392637f, 0.987407808f, 0.987422970f, 0.987438124f,
+    0.987453268f, 0.987468403f, 0.987483529f, 0.987498646f, 0.987513753f,
+    0.987528852f, 0.987543942f, 0.987559022f, 0.987574094f, 0.987589156f,
+    0.987604210f, 0.987619254f, 0.987634289f, 0.987649315f, 0.987664332f,
+    0.987679340f, 0.987694339f, 0.987709329f, 0.987724310f, 0.987739281f,
+    0.987754244f, 0.987769197f, 0.987784142f, 0.987799077f, 0.987814003f,
+    0.987828920f, 0.987843828f, 0.987858727f, 0.987873617f, 0.987888498f,
+    0.987903370f, 0.987918233f, 0.987933086f, 0.987947931f, 0.987962766f,
+    0.987977593f, 0.987992410f, 0.988007218f, 0.988022017f, 0.988036807f,
+    0.988051588f, 0.988066360f, 0.988081123f, 0.988095876f, 0.988110621f,
+    0.988125357f, 0.988140083f, 0.988154800f, 0.988169509f, 0.988184208f,
+    0.988198898f, 0.988213579f, 0.988228251f, 0.988242914f, 0.988257568f,
+    0.988272212f, 0.988286848f, 0.988301475f, 0.988316092f, 0.988330700f,
+    0.988345300f, 0.988359890f, 0.988374471f, 0.988389043f, 0.988403606f,
+    0.988418160f, 0.988432705f, 0.988447240f, 0.988461767f, 0.988476284f,
+    0.988490793f, 0.988505292f, 0.988519782f, 0.988534264f, 0.988548736f,
+    0.988563199f, 0.988577653f, 0.988592097f, 0.988606533f, 0.988620960f,
+    0.988635377f, 0.988649786f, 0.988664185f, 0.988678576f, 0.988692957f,
+    0.988707329f, 0.988721692f, 0.988736046f, 0.988750391f, 0.988764727f,
+    0.988779053f, 0.988793371f, 0.988807679f, 0.988821979f, 0.988836269f,
+    0.988850550f, 0.988864822f, 0.988879086f, 0.988893340f, 0.988907584f,
+    0.988921820f, 0.988936047f, 0.988950265f, 0.988964473f, 0.988978672f,
+    0.988992863f, 0.989007044f, 0.989021216f, 0.989035379f, 0.989049533f,
+    0.989063678f, 0.989077814f, 0.989091941f, 0.989106058f, 0.989120167f,
+    0.989134266f, 0.989148357f, 0.989162438f, 0.989176510f, 0.989190573f,
+    0.989204627f, 0.989218672f, 0.989232708f, 0.989246734f, 0.989260752f,
+    0.989274760f, 0.989288760f, 0.989302750f, 0.989316731f, 0.989330704f,
+    0.989344667f, 0.989358621f, 0.989372565f, 0.989386501f, 0.989400428f,
+    0.989414345f, 0.989428254f, 0.989442153f, 0.989456043f, 0.989469925f,
+    0.989483797f, 0.989497660f, 0.989511514f, 0.989525358f, 0.989539194f,
+    0.989553021f, 0.989566838f, 0.989580647f, 0.989594446f, 0.989608236f,
+    0.989622017f, 0.989635790f, 0.989649552f, 0.989663306f, 0.989677051f,
+    0.989690787f, 0.989704513f, 0.989718231f, 0.989731939f, 0.989745638f,
+    0.989759329f, 0.989773010f, 0.989786682f, 0.989800344f, 0.989813998f,
+    0.989827643f, 0.989841278f, 0.989854905f, 0.989868522f, 0.989882131f,
+    0.989895730f, 0.989909320f, 0.989922901f, 0.989936473f, 0.989950036f,
+    0.989963589f, 0.989977134f, 0.989990669f, 0.990004196f, 0.990017713f,
+    0.990031221f, 0.990044720f, 0.990058210f, 0.990071691f, 0.990085163f,
+    0.990098626f, 0.990112079f, 0.990125524f, 0.990138959f, 0.990152385f,
+    0.990165803f, 0.990179211f, 0.990192610f, 0.990205999f, 0.990219380f,
+    0.990232752f, 0.990246115f, 0.990259468f, 0.990272812f, 0.990286148f,
+    0.990299474f, 0.990312791f, 0.990326099f, 0.990339398f, 0.990352687f,
+    0.990365968f, 0.990379240f, 0.990392502f, 0.990405755f, 0.990419000f,
+    0.990432235f, 0.990445461f, 0.990458678f, 0.990471886f, 0.990485084f,
+    0.990498274f, 0.990511454f, 0.990524626f, 0.990537788f, 0.990550941f,
+    0.990564085f, 0.990577220f, 0.990590346f, 0.990603463f, 0.990616571f,
+    0.990629669f, 0.990642759f, 0.990655839f, 0.990668910f, 0.990681972f,
+    0.990695025f, 0.990708069f, 0.990721104f, 0.990734130f, 0.990747147f,
+    0.990760154f, 0.990773152f, 0.990786142f, 0.990799122f, 0.990812093f,
+    0.990825055f, 0.990838008f, 0.990850952f, 0.990863886f, 0.990876812f,
+    0.990889728f, 0.990902635f, 0.990915534f, 0.990928423f, 0.990941303f,
+    0.990954174f, 0.990967035f, 0.990979888f, 0.990992732f, 0.991005566f,
+    0.991018391f, 0.991031208f, 0.991044015f, 0.991056813f, 0.991069602f,
+    0.991082381f, 0.991095152f, 0.991107914f, 0.991120666f, 0.991133410f,
+    0.991146144f, 0.991158869f, 0.991171585f, 0.991184292f, 0.991196990f,
+    0.991209678f, 0.991222358f, 0.991235028f, 0.991247690f, 0.991260342f,
+    0.991272985f, 0.991285619f, 0.991298244f, 0.991310860f, 0.991323467f,
+    0.991336064f, 0.991348653f, 0.991361232f, 0.991373802f, 0.991386363f,
+    0.991398915f, 0.991411458f, 0.991423992f, 0.991436517f, 0.991449032f,
+    0.991461539f, 0.991474036f, 0.991486524f, 0.991499003f, 0.991511473f,
+    0.991523934f, 0.991536386f, 0.991548829f, 0.991561262f, 0.991573687f,
+    0.991586102f, 0.991598508f, 0.991610905f, 0.991623293f, 0.991635672f,
+    0.991648042f, 0.991660402f, 0.991672754f, 0.991685096f, 0.991697430f,
+    0.991709754f, 0.991722069f, 0.991734375f, 0.991746671f, 0.991758959f,
+    0.991771238f, 0.991783507f, 0.991795768f, 0.991808019f, 0.991820261f,
+    0.991832494f, 0.991844718f, 0.991856933f, 0.991869138f, 0.991881335f,
+    0.991893522f, 0.991905700f, 0.991917870f, 0.991930030f, 0.991942181f,
+    0.991954322f, 0.991966455f, 0.991978579f, 0.991990693f, 0.992002799f,
+    0.992014895f, 0.992026982f, 0.992039060f, 0.992051129f, 0.992063189f,
+    0.992075239f, 0.992087281f, 0.992099313f, 0.992111336f, 0.992123351f,
+    0.992135356f, 0.992147352f, 0.992159338f, 0.992171316f, 0.992183285f,
+    0.992195244f, 0.992207194f, 0.992219136f, 0.992231068f, 0.992242991f,
+    0.992254905f, 0.992266809f, 0.992278705f, 0.992290591f, 0.992302469f,
+    0.992314337f, 0.992326196f, 0.992338046f, 0.992349887f, 0.992361719f,
+    0.992373541f, 0.992385355f, 0.992397159f, 0.992408955f, 0.992420741f,
+    0.992432518f, 0.992444286f, 0.992456044f, 0.992467794f, 0.992479535f,
+    0.992491266f, 0.992502988f, 0.992514701f, 0.992526406f, 0.992538100f,
+    0.992549786f, 0.992561463f, 0.992573130f, 0.992584789f, 0.992596438f,
+    0.992608078f, 0.992619709f, 0.992631331f, 0.992642944f, 0.992654548f,
+    0.992666142f, 0.992677728f, 0.992689304f, 0.992700871f, 0.992712429f,
+    0.992723978f, 0.992735518f, 0.992747049f, 0.992758570f, 0.992770083f,
+    0.992781586f, 0.992793080f, 0.992804565f, 0.992816041f, 0.992827508f,
+    0.992838966f, 0.992850414f, 0.992861854f, 0.992873284f, 0.992884705f,
+    0.992896117f, 0.992907520f, 0.992918914f, 0.992930299f, 0.992941674f,
+    0.992953041f, 0.992964398f, 0.992975746f, 0.992987085f, 0.992998415f,
+    0.993009736f, 0.993021048f, 0.993032350f, 0.993043644f, 0.993054928f,
+    0.993066203f, 0.993077469f, 0.993088726f, 0.993099974f, 0.993111212f,
+    0.993122442f, 0.993133662f, 0.993144873f, 0.993156076f, 0.993167269f,
+    0.993178452f, 0.993189627f, 0.993200793f, 0.993211949f, 0.993223097f,
+    0.993234235f, 0.993245364f, 0.993256484f, 0.993267595f, 0.993278696f,
+    0.993289789f, 0.993300872f, 0.993311947f, 0.993323012f, 0.993334068f,
+    0.993345115f, 0.993356153f, 0.993367181f, 0.993378201f, 0.993389211f,
+    0.993400212f, 0.993411205f, 0.993422188f, 0.993433161f, 0.993444126f,
+    0.993455082f, 0.993466028f, 0.993476966f, 0.993487894f, 0.993498813f,
+    0.993509723f, 0.993520624f, 0.993531515f, 0.993542398f, 0.993553271f,
+    0.993564136f, 0.993574991f, 0.993585837f, 0.993596674f, 0.993607501f,
+    0.993618320f, 0.993629129f, 0.993639930f, 0.993650721f, 0.993661503f,
+    0.993672276f, 0.993683040f, 0.993693795f, 0.993704540f, 0.993715277f,
+    0.993726004f, 0.993736722f, 0.993747431f, 0.993758131f, 0.993768822f,
+    0.993779503f, 0.993790176f, 0.993800839f, 0.993811493f, 0.993822138f,
+    0.993832774f, 0.993843401f, 0.993854019f, 0.993864627f, 0.993875227f,
+    0.993885817f, 0.993896398f, 0.993906970f, 0.993917533f, 0.993928087f,
+    0.993938631f, 0.993949167f, 0.993959693f, 0.993970210f, 0.993980718f,
+    0.993991217f, 0.994001707f, 0.994012187f, 0.994022659f, 0.994033121f,
+    0.994043574f, 0.994054019f, 0.994064453f, 0.994074879f, 0.994085296f,
+    0.994095704f, 0.994106102f, 0.994116491f, 0.994126871f, 0.994137242f,
+    0.994147604f, 0.994157957f, 0.994168300f, 0.994178635f, 0.994188960f,
+    0.994199276f, 0.994209583f, 0.994219881f, 0.994230170f, 0.994240449f,
+    0.994250720f, 0.994260981f, 0.994271233f, 0.994281476f, 0.994291710f,
+    0.994301935f, 0.994312151f, 0.994322357f, 0.994332555f, 0.994342743f,
+    0.994352922f, 0.994363092f, 0.994373253f, 0.994383404f, 0.994393547f,
+    0.994403680f, 0.994413804f, 0.994423919f, 0.994434025f, 0.994444122f,
+    0.994454210f, 0.994464288f, 0.994474358f, 0.994484418f, 0.994494469f,
+    0.994504511f, 0.994514544f, 0.994524567f, 0.994534582f, 0.994544587f,
+    0.994554584f, 0.994564571f, 0.994574549f, 0.994584518f, 0.994594477f,
+    0.994604428f, 0.994614369f, 0.994624301f, 0.994634225f, 0.994644138f,
+    0.994654043f, 0.994663939f, 0.994673826f, 0.994683703f, 0.994693571f,
+    0.994703430f, 0.994713280f, 0.994723121f, 0.994732953f, 0.994742775f,
+    0.994752589f, 0.994762393f, 0.994772188f, 0.994781974f, 0.994791751f,
+    0.994801519f, 0.994811277f, 0.994821026f, 0.994830767f, 0.994840498f,
+    0.994850220f, 0.994859933f, 0.994869636f, 0.994879331f, 0.994889016f,
+    0.994898692f, 0.994908359f, 0.994918017f, 0.994927666f, 0.994937306f,
+    0.994946936f, 0.994956558f, 0.994966170f, 0.994975773f, 0.994985367f,
+    0.994994952f, 0.995004527f, 0.995014094f, 0.995023651f, 0.995033199f,
+    0.995042738f, 0.995052268f, 0.995061789f, 0.995071301f, 0.995080803f,
+    0.995090297f, 0.995099781f, 0.995109256f, 0.995118722f, 0.995128178f,
+    0.995137626f, 0.995147064f, 0.995156494f, 0.995165914f, 0.995175325f,
+    0.995184727f, 0.995194119f, 0.995203503f, 0.995212877f, 0.995222243f,
+    0.995231599f, 0.995240946f, 0.995250283f, 0.995259612f, 0.995268932f,
+    0.995278242f, 0.995287543f, 0.995296835f, 0.995306118f, 0.995315392f,
+    0.995324657f, 0.995333912f, 0.995343158f, 0.995352396f, 0.995361624f,
+    0.995370843f, 0.995380052f, 0.995389253f, 0.995398444f, 0.995407627f,
+    0.995416800f, 0.995425964f, 0.995435119f, 0.995444264f, 0.995453401f,
+    0.995462528f, 0.995471646f, 0.995480755f, 0.995489855f, 0.995498946f,
+    0.995508028f, 0.995517100f, 0.995526164f, 0.995535218f, 0.995544263f,
+    0.995553299f, 0.995562325f, 0.995571343f, 0.995580352f, 0.995589351f,
+    0.995598341f, 0.995607322f, 0.995616294f, 0.995625256f, 0.995634210f,
+    0.995643154f, 0.995652089f, 0.995661016f, 0.995669932f, 0.995678840f,
+    0.995687739f, 0.995696628f, 0.995705509f, 0.995714380f, 0.995723242f,
+    0.995732095f, 0.995740938f, 0.995749773f, 0.995758598f, 0.995767414f,
+    0.995776222f, 0.995785019f, 0.995793808f, 0.995802588f, 0.995811358f,
+    0.995820120f, 0.995828872f, 0.995837615f, 0.995846349f, 0.995855073f,
+    0.995863789f, 0.995872495f, 0.995881193f, 0.995889881f, 0.995898560f,
+    0.995907229f, 0.995915890f, 0.995924542f, 0.995933184f, 0.995941817f,
+    0.995950441f, 0.995959056f, 0.995967662f, 0.995976258f, 0.995984846f,
+    0.995993424f, 0.996001993f, 0.996010553f, 0.996019104f, 0.996027645f,
+    0.996036178f, 0.996044701f, 0.996053215f, 0.996061720f, 0.996070216f,
+    0.996078703f, 0.996087180f, 0.996095648f, 0.996104108f, 0.996112558f,
+    0.996120999f, 0.996129430f, 0.996137853f, 0.996146266f, 0.996154671f,
+    0.996163066f, 0.996171452f, 0.996179829f, 0.996188196f, 0.996196555f,
+    0.996204904f, 0.996213244f, 0.996221575f, 0.996229897f, 0.996238210f,
+    0.996246513f, 0.996254808f, 0.996263093f, 0.996271369f, 0.996279636f,
+    0.996287894f, 0.996296142f, 0.996304382f, 0.996312612f, 0.996320833f,
+    0.996329045f, 0.996337248f, 0.996345442f, 0.996353626f, 0.996361802f,
+    0.996369968f, 0.996378125f, 0.996386273f, 0.996394411f, 0.996402541f,
+    0.996410661f, 0.996418773f, 0.996426875f, 0.996434968f, 0.996443051f,
+    0.996451126f, 0.996459191f, 0.996467248f, 0.996475295f, 0.996483333f,
+    0.996491362f, 0.996499381f, 0.996507392f, 0.996515393f, 0.996523385f,
+    0.996531368f, 0.996539342f, 0.996547307f, 0.996555262f, 0.996563209f,
+    0.996571146f, 0.996579074f, 0.996586993f, 0.996594902f, 0.996602803f,
+    0.996610694f, 0.996618577f, 0.996626450f, 0.996634314f, 0.996642168f,
+    0.996650014f, 0.996657850f, 0.996665678f, 0.996673496f, 0.996681305f,
+    0.996689105f, 0.996696895f, 0.996704677f, 0.996712449f, 0.996720212f,
+    0.996727966f, 0.996735711f, 0.996743447f, 0.996751173f, 0.996758890f,
+    0.996766599f, 0.996774298f, 0.996781987f, 0.996789668f, 0.996797340f,
+    0.996805002f, 0.996812655f, 0.996820299f, 0.996827934f, 0.996835560f,
+    0.996843176f, 0.996850784f, 0.996858382f, 0.996865971f, 0.996873551f,
+    0.996881122f, 0.996888683f, 0.996896236f, 0.996903779f, 0.996911313f,
+    0.996918838f, 0.996926354f, 0.996933860f, 0.996941358f, 0.996948846f,
+    0.996956325f, 0.996963795f, 0.996971256f, 0.996978707f, 0.996986150f,
+    0.996993583f, 0.997001007f, 0.997008422f, 0.997015828f, 0.997023225f,
+    0.997030612f, 0.997037990f, 0.997045360f, 0.997052720f, 0.997060070f,
+    0.997067412f, 0.997074744f, 0.997082068f, 0.997089382f, 0.997096687f,
+    0.997103983f, 0.997111269f, 0.997118547f, 0.997125815f, 0.997133074f,
+    0.997140324f, 0.997147565f, 0.997154797f, 0.997162019f, 0.997169233f,
+    0.997176437f, 0.997183632f, 0.997190818f, 0.997197994f, 0.997205162f,
+    0.997212320f, 0.997219469f, 0.997226609f, 0.997233740f, 0.997240862f,
+    0.997247974f, 0.997255078f, 0.997262172f, 0.997269257f, 0.997276333f,
+    0.997283399f, 0.997290457f, 0.997297505f, 0.997304544f, 0.997311574f,
+    0.997318595f, 0.997325607f, 0.997332609f, 0.997339602f, 0.997346587f,
+    0.997353562f, 0.997360527f, 0.997367484f, 0.997374432f, 0.997381370f,
+    0.997388299f, 0.997395219f, 0.997402130f, 0.997409032f, 0.997415924f,
+    0.997422807f, 0.997429682f, 0.997436546f, 0.997443402f, 0.997450249f,
+    0.997457086f, 0.997463915f, 0.997470734f, 0.997477544f, 0.997484345f,
+    0.997491136f, 0.997497919f, 0.997504692f, 0.997511456f, 0.997518211f,
+    0.997524957f, 0.997531694f, 0.997538421f, 0.997545139f, 0.997551848f,
+    0.997558548f, 0.997565239f, 0.997571921f, 0.997578593f, 0.997585256f,
+    0.997591910f, 0.997598555f, 0.997605191f, 0.997611818f, 0.997618435f,
+    0.997625043f, 0.997631642f, 0.997638232f, 0.997644813f, 0.997651385f,
+    0.997657947f, 0.997664500f, 0.997671044f, 0.997677579f, 0.997684105f,
+    0.997690621f, 0.997697129f, 0.997703627f, 0.997710116f, 0.997716596f,
+    0.997723067f, 0.997729528f, 0.997735981f, 0.997742424f, 0.997748858f,
+    0.997755283f, 0.997761698f, 0.997768105f, 0.997774502f, 0.997780890f,
+    0.997787269f, 0.997793639f, 0.997800000f, 0.997806351f, 0.997812693f,
+    0.997819026f, 0.997825350f, 0.997831665f, 0.997837971f, 0.997844267f,
+    0.997850554f, 0.997856833f, 0.997863101f, 0.997869361f, 0.997875612f,
+    0.997881853f, 0.997888085f, 0.997894309f, 0.997900522f, 0.997906727f,
+    0.997912923f, 0.997919109f, 0.997925286f, 0.997931454f, 0.997937613f,
+    0.997943763f, 0.997949903f, 0.997956035f, 0.997962157f, 0.997968270f,
+    0.997974374f, 0.997980468f, 0.997986554f, 0.997992630f, 0.997998697f,
+    0.998004755f, 0.998010804f, 0.998016843f, 0.998022874f, 0.998028895f,
+    0.998034907f, 0.998040910f, 0.998046904f, 0.998052888f, 0.998058864f,
+    0.998064830f, 0.998070787f, 0.998076735f, 0.998082673f, 0.998088603f,
+    0.998094523f, 0.998100434f, 0.998106336f, 0.998112229f, 0.998118113f,
+    0.998123987f, 0.998129853f, 0.998135709f, 0.998141556f, 0.998147393f,
+    0.998153222f, 0.998159041f, 0.998164852f, 0.998170653f, 0.998176445f,
+    0.998182227f, 0.998188001f, 0.998193765f, 0.998199521f, 0.998205267f,
+    0.998211003f, 0.998216731f, 0.998222450f, 0.998228159f, 0.998233859f,
+    0.998239550f, 0.998245232f, 0.998250904f, 0.998256568f, 0.998262222f,
+    0.998267867f, 0.998273503f, 0.998279130f, 0.998284747f, 0.998290356f,
+    0.998295955f, 0.998301545f, 0.998307126f, 0.998312697f, 0.998318260f,
+    0.998323813f, 0.998329357f, 0.998334892f, 0.998340418f, 0.998345935f,
+    0.998351442f, 0.998356941f, 0.998362430f, 0.998367910f, 0.998373380f,
+    0.998378842f, 0.998384294f, 0.998389737f, 0.998395171f, 0.998400596f,
+    0.998406012f, 0.998411418f, 0.998416816f, 0.998422204f, 0.998427583f,
+    0.998432953f, 0.998438313f, 0.998443665f, 0.998449007f, 0.998454340f,
+    0.998459664f, 0.998464979f, 0.998470284f, 0.998475581f, 0.998480868f,
+    0.998486146f, 0.998491415f, 0.998496674f, 0.998501925f, 0.998507166f,
+    0.998512398f, 0.998517621f, 0.998522835f, 0.998528039f, 0.998533235f,
+    0.998538421f, 0.998543598f, 0.998548766f, 0.998553925f, 0.998559074f,
+    0.998564215f, 0.998569346f, 0.998574468f, 0.998579581f, 0.998584684f,
+    0.998589779f, 0.998594864f, 0.998599940f, 0.998605007f, 0.998610065f,
+    0.998615113f, 0.998620152f, 0.998625183f, 0.998630204f, 0.998635216f,
+    0.998640218f, 0.998645212f, 0.998650196f, 0.998655171f, 0.998660137f,
+    0.998665094f, 0.998670041f, 0.998674980f, 0.998679909f, 0.998684829f,
+    0.998689740f, 0.998694642f, 0.998699534f, 0.998704417f, 0.998709291f,
+    0.998714156f, 0.998719012f, 0.998723859f, 0.998728696f, 0.998733524f,
+    0.998738344f, 0.998743153f, 0.998747954f, 0.998752746f, 0.998757528f,
+    0.998762301f, 0.998767065f, 0.998771820f, 0.998776566f, 0.998781302f,
+    0.998786029f, 0.998790747f, 0.998795456f, 0.998800156f, 0.998804846f,
+    0.998809528f, 0.998814200f, 0.998818863f, 0.998823517f, 0.998828161f,
+    0.998832797f, 0.998837423f, 0.998842040f, 0.998846648f, 0.998851247f,
+    0.998855836f, 0.998860417f, 0.998864988f, 0.998869550f, 0.998874103f,
+    0.998878646f, 0.998883181f, 0.998887706f, 0.998892222f, 0.998896729f,
+    0.998901227f, 0.998905715f, 0.998910195f, 0.998914665f, 0.998919126f,
+    0.998923578f, 0.998928020f, 0.998932454f, 0.998936878f, 0.998941293f,
+    0.998945699f, 0.998950096f, 0.998954483f, 0.998958862f, 0.998963231f,
+    0.998967591f, 0.998971942f, 0.998976283f, 0.998980616f, 0.998984939f,
+    0.998989253f, 0.998993558f, 0.998997854f, 0.999002140f, 0.999006418f,
+    0.999010686f, 0.999014945f, 0.999019195f, 0.999023435f, 0.999027667f,
+    0.999031889f, 0.999036102f, 0.999040306f, 0.999044501f, 0.999048686f,
+    0.999052863f, 0.999057030f, 0.999061188f, 0.999065336f, 0.999069476f,
+    0.999073607f, 0.999077728f, 0.999081840f, 0.999085943f, 0.999090036f,
+    0.999094121f, 0.999098196f, 0.999102262f, 0.999106319f, 0.999110367f,
+    0.999114406f, 0.999118435f, 0.999122455f, 0.999126466f, 0.999130468f,
+    0.999134461f, 0.999138444f, 0.999142419f, 0.999146384f, 0.999150340f,
+    0.999154287f, 0.999158224f, 0.999162153f, 0.999166072f, 0.999169982f,
+    0.999173883f, 0.999177774f, 0.999181657f, 0.999185530f, 0.999189394f,
+    0.999193249f, 0.999197095f, 0.999200931f, 0.999204759f, 0.999208577f,
+    0.999212386f, 0.999216186f, 0.999219976f, 0.999223758f, 0.999227530f,
+    0.999231293f, 0.999235047f, 0.999238792f, 0.999242527f, 0.999246253f,
+    0.999249971f, 0.999253679f, 0.999257377f, 0.999261067f, 0.999264747f,
+    0.999268419f, 0.999272081f, 0.999275733f, 0.999279377f, 0.999283012f,
+    0.999286637f, 0.999290253f, 0.999293860f, 0.999297458f, 0.999301046f,
+    0.999304626f, 0.999308196f, 0.999311757f, 0.999315309f, 0.999318851f,
+    0.999322385f, 0.999325909f, 0.999329424f, 0.999332930f, 0.999336426f,
+    0.999339914f, 0.999343392f, 0.999346861f, 0.999350321f, 0.999353772f,
+    0.999357214f, 0.999360646f, 0.999364069f, 0.999367483f, 0.999370888f,
+    0.999374284f, 0.999377670f, 0.999381048f, 0.999384416f, 0.999387775f,
+    0.999391124f, 0.999394465f, 0.999397796f, 0.999401118f, 0.999404431f,
+    0.999407735f, 0.999411030f, 0.999414315f, 0.999417591f, 0.999420859f,
+    0.999424116f, 0.999427365f, 0.999430605f, 0.999433835f, 0.999437056f,
+    0.999440268f, 0.999443471f, 0.999446664f, 0.999449849f, 0.999453024f,
+    0.999456190f, 0.999459347f, 0.999462494f, 0.999465633f, 0.999468762f,
+    0.999471882f, 0.999474993f, 0.999478094f, 0.999481187f, 0.999484270f,
+    0.999487344f, 0.999490409f, 0.999493465f, 0.999496512f, 0.999499549f,
+    0.999502577f, 0.999505596f, 0.999508606f, 0.999511607f, 0.999514598f,
+    0.999517580f, 0.999520553f, 0.999523517f, 0.999526472f, 0.999529418f,
+    0.999532354f, 0.999535281f, 0.999538199f, 0.999541108f, 0.999544007f,
+    0.999546898f, 0.999549779f, 0.999552651f, 0.999555514f, 0.999558367f,
+    0.999561212f, 0.999564047f, 0.999566873f, 0.999569690f, 0.999572498f,
+    0.999575296f, 0.999578085f, 0.999580865f, 0.999583636f, 0.999586398f,
+    0.999589151f, 0.999591894f, 0.999594628f, 0.999597353f, 0.999600069f,
+    0.999602776f, 0.999605473f, 0.999608161f, 0.999610840f, 0.999613510f,
+    0.999616171f, 0.999618822f, 0.999621465f, 0.999624098f, 0.999626722f,
+    0.999629337f, 0.999631942f, 0.999634538f, 0.999637126f, 0.999639704f,
+    0.999642272f, 0.999644832f, 0.999647382f, 0.999649924f, 0.999652456f,
+    0.999654979f, 0.999657492f, 0.999659997f, 0.999662492f, 0.999664978f,
+    0.999667455f, 0.999669923f, 0.999672381f, 0.999674831f, 0.999677271f,
+    0.999679702f, 0.999682124f, 0.999684536f, 0.999686940f, 0.999689334f,
+    0.999691719f, 0.999694095f, 0.999696461f, 0.999698819f, 0.999701167f,
+    0.999703506f, 0.999705836f, 0.999708157f, 0.999710468f, 0.999712770f,
+    0.999715064f, 0.999717348f, 0.999719622f, 0.999721888f, 0.999724144f,
+    0.999726391f, 0.999728629f, 0.999730858f, 0.999733078f, 0.999735288f,
+    0.999737489f, 0.999739682f, 0.999741864f, 0.999744038f, 0.999746203f,
+    0.999748358f, 0.999750504f, 0.999752641f, 0.999754769f, 0.999756887f,
+    0.999758996f, 0.999761097f, 0.999763188f, 0.999765269f, 0.999767342f,
+    0.999769405f, 0.999771460f, 0.999773505f, 0.999775540f, 0.999777567f,
+    0.999779584f, 0.999781593f, 0.999783592f, 0.999785582f, 0.999787562f,
+    0.999789534f, 0.999791496f, 0.999793449f, 0.999795393f, 0.999797328f,
+    0.999799254f, 0.999801170f, 0.999803077f, 0.999804975f, 0.999806864f,
+    0.999808743f, 0.999810614f, 0.999812475f, 0.999814327f, 0.999816170f,
+    0.999818004f, 0.999819828f, 0.999821643f, 0.999823449f, 0.999825246f,
+    0.999827034f, 0.999828812f, 0.999830582f, 0.999832342f, 0.999834093f,
+    0.999835835f, 0.999837567f, 0.999839291f, 0.999841005f, 0.999842710f,
+    0.999844405f, 0.999846092f, 0.999847770f, 0.999849438f, 0.999851097f,
+    0.999852747f, 0.999854387f, 0.999856019f, 0.999857641f, 0.999859254f,
+    0.999860858f, 0.999862453f, 0.999864038f, 0.999865615f, 0.999867182f,
+    0.999868740f, 0.999870288f, 0.999871828f, 0.999873358f, 0.999874879f,
+    0.999876391f, 0.999877894f, 0.999879388f, 0.999880872f, 0.999882347f,
+    0.999883813f, 0.999885270f, 0.999886718f, 0.999888156f, 0.999889586f,
+    0.999891006f, 0.999892417f, 0.999893818f, 0.999895211f, 0.999896594f,
+    0.999897968f, 0.999899333f, 0.999900689f, 0.999902036f, 0.999903373f,
+    0.999904701f, 0.999906020f, 0.999907330f, 0.999908630f, 0.999909922f,
+    0.999911204f, 0.999912477f, 0.999913741f, 0.999914996f, 0.999916241f,
+    0.999917477f, 0.999918704f, 0.999919922f, 0.999921131f, 0.999922330f,
+    0.999923521f, 0.999924702f, 0.999925874f, 0.999927036f, 0.999928190f,
+    0.999929334f, 0.999930470f, 0.999931596f, 0.999932712f, 0.999933820f,
+    0.999934918f, 0.999936007f, 0.999937087f, 0.999938158f, 0.999939220f,
+    0.999940272f, 0.999941316f, 0.999942350f, 0.999943375f, 0.999944390f,
+    0.999945397f, 0.999946394f, 0.999947382f, 0.999948361f, 0.999949331f,
+    0.999950291f, 0.999951243f, 0.999952185f, 0.999953118f, 0.999954041f,
+    0.999954956f, 0.999955861f, 0.999956758f, 0.999957645f, 0.999958522f,
+    0.999959391f, 0.999960250f, 0.999961101f, 0.999961942f, 0.999962774f,
+    0.999963596f, 0.999964410f, 0.999965214f, 0.999966009f, 0.999966795f,
+    0.999967572f, 0.999968339f, 0.999969097f, 0.999969846f, 0.999970586f,
+    0.999971317f, 0.999972039f, 0.999972751f, 0.999973454f, 0.999974148f,
+    0.999974833f, 0.999975509f, 0.999976175f, 0.999976832f, 0.999977480f,
+    0.999978119f, 0.999978749f, 0.999979369f, 0.999979980f, 0.999980582f,
+    0.999981175f, 0.999981759f, 0.999982333f, 0.999982899f, 0.999983455f,
+    0.999984002f, 0.999984539f, 0.999985068f, 0.999985587f, 0.999986097f,
+    0.999986598f, 0.999987090f, 0.999987573f, 0.999988046f, 0.999988510f,
+    0.999988965f, 0.999989411f, 0.999989848f, 0.999990275f, 0.999990693f,
+    0.999991102f, 0.999991502f, 0.999991893f, 0.999992274f, 0.999992647f,
+    0.999993010f, 0.999993364f, 0.999993708f, 0.999994044f, 0.999994370f,
+    0.999994687f, 0.999994995f, 0.999995294f, 0.999995583f, 0.999995864f,
+    0.999996135f, 0.999996397f, 0.999996650f, 0.999996893f, 0.999997128f,
+    0.999997353f, 0.999997569f, 0.999997776f, 0.999997973f, 0.999998162f,
+    0.999998341f, 0.999998511f, 0.999998672f, 0.999998823f, 0.999998966f,
+    0.999999099f, 0.999999223f, 0.999999338f, 0.999999444f, 0.999999540f,
+    0.999999628f, 0.999999706f, 0.999999775f, 0.999999835f, 0.999999885f,
+    0.999999926f, 0.999999959f, 0.999999982f, 0.999999995f, 1.000000000f
+};
+
+}

src/common/math/sin_table.h

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace decision::math {
+
+extern const float SIN_TABLE[16385];
+
+}

src/common/math/vec2d.cpp

@@ -0,0 +1,131 @@
+#include "math/vec2d.h"
+#include "math/math_utils.h"
+
+#include <cmath>
+#include <sstream>
+
+using namespace decision::math;
+
+Vec2d Vec2d::create_unit_vec(const double angle)
+{
+    return Vec2d(cos(angle), sin(angle));
+}
+
+double Vec2d::length() const
+{
+    return hypot(_x, _y);
+}
+
+double Vec2d::length_sqr() const
+{
+    return _x * _x + _y * _y;
+}
+
+double Vec2d::distance_to(const Vec2d& other) const
+{
+    return hypot(_x - other._x, _y - other._y);
+}
+
+double Vec2d::distance_sqr_to(const Vec2d& other) const
+{
+    const double dx = _x - other._x;
+    const double dy = _y - other._y;
+    return dx * dx + dy * dy;
+}
+
+void Vec2d::normalize()
+{
+    const double l = length();
+    if (l > kMathEpsilon) {
+        _x /= l;
+        _y /= l;
+    }
+}
+
+Vec2d Vec2d::operator+(const Vec2d& other) const
+{
+    return Vec2d(_x + other.x(), _y + other.y());
+}
+
+Vec2d Vec2d::operator-(const Vec2d& other) const
+{
+    return Vec2d(_x - other.x(), _y - other.y());
+}
+
+Vec2d Vec2d::operator*(const double ratio) const
+{
+    return Vec2d(_x * ratio, _y * ratio);
+}
+
+Vec2d Vec2d::operator/(const double ratio) const
+{
+    CHECK_GT(std::abs(ratio), kMathEpsilon);
+    return Vec2d(_x / ratio, _y / ratio);
+}
+
+Vec2d& Vec2d::operator+=(const Vec2d& other)
+{
+    _x += other.x();
+    _y += other.y();
+    return *this;
+}
+
+Vec2d& Vec2d::operator-=(const Vec2d& other)
+{
+    _x -= other.x();
+    _y -= other.y();
+    return *this;
+}
+
+Vec2d& Vec2d::operator*=(const double ratio)
+{
+    _x *= ratio;
+    _y *= ratio;
+    return *this;
+}
+
+Vec2d& Vec2d::operator/=(const double ratio)
+{
+    CHECK_GT(std::abs(ratio), kMathEpsilon);
+    _x /= ratio;
+    _y /= ratio;
+    return *this;
+}
+
+bool Vec2d::operator==(const Vec2d& other) const
+{
+    return (std::abs(_x - other.x()) < kMathEpsilon && std::abs(_y - other.y()) < kMathEpsilon);
+}
+
+Vec2d Vec2d::rotate(const double angle) const
+{
+    return Vec2d(_x * cos(angle) - _y * sin(angle),
+        _x * sin(angle) + _y * cos(angle));
+}
+
+double Vec2d::cross_prod(const Vec2d& other) const
+{
+    return _x * other.y() - _y * other.x();
+}
+
+double Vec2d::inner_prod(const Vec2d& other) const
+{
+    return _x * other.x() + _y * other.y();
+}
+
+namespace decision::math {
+
+Vec2d operator*(const double ratio, const Vec2d& vec)
+{
+    return vec * ratio;
+}
+
+}
+
+std::string Vec2d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "vec2d ( x = " << _x << "  y = " << _y << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/vec2d.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <cmath>
+#include <string>
+
+namespace decision::math {
+
+class Vec2d {
+public:
+    constexpr Vec2d() noexcept
+        : Vec2d(0, 0)
+    {
+    }
+    constexpr Vec2d(const double x, const double y) noexcept
+        : _x(x)
+        , _y(y)
+    {
+    }
+    static Vec2d create_unit_vec(const double angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec2d& other) const;
+    double distance_sqr_to(const Vec2d& other) const;
+
+    Vec2d operator+(const Vec2d& other) const;
+    Vec2d operator-(const Vec2d& other) const;
+    Vec2d operator*(const double ratio) const;
+    Vec2d operator/(const double ratio) const;
+    Vec2d& operator+=(const Vec2d& other);
+    Vec2d& operator-=(const Vec2d& other);
+    Vec2d& operator*=(const double ratio);
+    Vec2d& operator/=(const double ratio);
+    bool operator==(const Vec2d& other) const;
+
+    //! rotate the vector by angle.
+    Vec2d rotate(const double angle) const;
+
+    double cross_prod(const Vec2d& other) const;
+    double inner_prod(const Vec2d& other) const;
+
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+};
+
+Vec2d operator*(const double ratio, const Vec2d& vec);
+
+}

src/common/math/vec3d.cpp

@@ -0,0 +1,132 @@
+#include "math/vec3d.h"
+#include "math/math_utils.h"
+
+#include <sstream>
+
+using namespace decision::math;
+
+Vec3d Vec3d::create_unit_vec(const double yaw_angle, const double pitch_angle)
+{
+    double x = cos(pitch_angle) * cos(yaw_angle);
+    double y = cos(pitch_angle) * sin(yaw_angle);
+    double z = sin(pitch_angle);
+    return Vec3d(x, y, z);
+}
+
+double Vec3d::length() const
+{
+    return hypot(hypot(_x, _y), _z);
+}
+
+double Vec3d::length_sqr() const
+{
+    return _x * _x + _y * _y + _z * _z;
+}
+
+double Vec3d::distance_to(const Vec3d& other) const
+{
+    return hypot(hypot(_x - other.x(), _y - other.y()), _z - other.z());
+}
+
+double Vec3d::distance_sqr_to(const Vec3d& other) const
+{
+    const double dx = _x - other._x;
+    const double dy = _y - other._y;
+    const double dz = _z - other._z;
+    return dx * dx + dy * dy + dz * dz;
+}
+
+void Vec3d::normalize()
+{
+    const double l = length();
+    if (l > kMathEpsilon) {
+        _x /= l;
+        _y /= l;
+        _z /= l;
+    }
+}
+
+Vec3d Vec3d::operator+(const Vec3d& other) const
+{
+    return Vec3d(_x + other.x(), _y + other.y(), _z + other.z());
+}
+
+Vec3d Vec3d::operator-(const Vec3d& other) const
+{
+    return Vec3d(_x - other.x(), _y - other.y(), _z - other.z());
+}
+
+Vec3d Vec3d::operator*(const double ratio) const
+{
+    return Vec3d(_x * ratio, _y * ratio, _z * ratio);
+}
+
+Vec3d Vec3d::operator/(const double ratio) const
+{
+    CHECK_GT(std::abs(ratio), kMathEpsilon);
+    return Vec3d(_x / ratio, _y / ratio, _z / ratio);
+}
+
+Vec3d& Vec3d::operator+=(const Vec3d& other)
+{
+    _x += other.x();
+    _y += other.y();
+    _z += other.z();
+    return *this;
+}
+
+Vec3d& Vec3d::operator-=(const Vec3d& other)
+{
+    _x -= other.x();
+    _y -= other.y();
+    _z -= other.z();
+    return *this;
+}
+
+Vec3d& Vec3d::operator*=(const double ratio)
+{
+    _x *= ratio;
+    _y *= ratio;
+    _z *= ratio;
+    return *this;
+}
+
+Vec3d& Vec3d::operator/=(const double ratio)
+{
+    CHECK_GT(std::abs(ratio), kMathEpsilon);
+    _x /= ratio;
+    _y /= ratio;
+    _z /= ratio;
+    return *this;
+}
+
+bool Vec3d::operator==(const Vec3d& other) const
+{
+    return (std::abs(_x - other.x()) < kMathEpsilon && std::abs(_y - other.y()) < kMathEpsilon && std::abs(_z - other.z()) < kMathEpsilon);
+}
+
+Vec3d Vec3d::cross_prod(const Vec3d& other) const
+{
+    double x = _y * other.z() - _z * other.y();
+    double y = _z * other.x() - _x * other.z();
+    double z = _x * other.y() - _y * other.x();
+    return Vec3d(x, y, z);
+}
+
+double Vec3d::inner_prod(const Vec3d& other) const
+{
+    return _x * other.x() + _y * other.y() + _z * other.z();
+}
+
+Vec3d operator*(const double ratio, const Vec3d& vec)
+{
+    return vec * ratio;
+}
+
+std::string Vec3d::debug_string() const
+{
+    std::ostringstream sout;
+    sout << "vec3d ( x = " << _x << "  y = " << _y << "  z = " << _z << " )";
+    sout.flush();
+    return sout.str();
+}

src/common/math/vec3d.h

@@ -0,0 +1,66 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: supporting for 3D element
+
+#pragma once
+
+// #include <cmath>
+// #include <string>
+#include "math/vec2d.h"
+
+namespace decision::math {
+
+class Vec3d {
+public:
+    constexpr Vec3d() noexcept
+        : Vec3d(0, 0, 0)
+    {
+    }
+    constexpr Vec3d(const double x, const double y, const double z) noexcept
+        : _x(x)
+        , _y(y)
+        , _z(z)
+    {
+    }
+    static Vec3d create_unit_vec(const double yaw_angle, const double pitch_angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double z() const { return _z; }
+    double yaw_angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void set_z(const double z) { _z = z; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec3d& other) const;
+    double distance_sqr_to(const Vec3d& other) const;
+
+    Vec3d operator+(const Vec3d& other) const;
+    Vec3d operator-(const Vec3d& other) const;
+    Vec3d operator*(const double ratio) const;
+    Vec3d operator/(const double ratio) const;
+    Vec3d& operator+=(const Vec3d& other);
+    Vec3d& operator-=(const Vec3d& other);
+    Vec3d& operator*=(const double ratio);
+    Vec3d& operator/=(const double ratio);
+    bool operator==(const Vec3d& other) const;
+
+    Vec3d cross_prod(const Vec3d& other) const;
+    double inner_prod(const Vec3d& other) const;
+
+    Vec2d vec2d() const { return Vec2d(_x, _y); }
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+    double _z = 0.0;
+};
+
+Vec3d operator*(const double ratio, const Vec3d& vec);
+
+}

src/common/mmw/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+
+file(GLOB SRCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.cpp *.c)
+
+include(../cmake/add3rd.cmake)
+add3rds(muduo)
+
+add_library(mmw STATIC ${SRCS})
+
+target_include_directories(mmw PUBLIC .)
+
+file(GLOB INCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.h *.hpp)
+install(FILES ${INCS} DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/mmw/include/mmw)
+install(TARGETS mmw DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/mmw/lib)
+
+target_link_libraries(mmw PRIVATE muduo_net muduo_base)

src/common/mmw/mmwabstract.cpp

@@ -0,0 +1,48 @@
+#include "mmwabstract.h"
+#include "muduo/net/EventLoopThread.h"
+using namespace muduo::net;
+namespace mmw {
+/**
+ * @brief 根据topic发布一条消息
+ * @param topic[in] 要发布消息的topic
+ * @param content[in] 消息的具体内容
+ * @return true 成功
+ * @return false 失败
+ */
+bool PublisherAbstract::publish(const std::string& topic, const std::string& content)
+{
+    return publish(topic.data(), topic.size(), content.data(), content.size());
+}
+/**
+ * @brief SubscriberAbstract类的析构函数
+ */
+SubscriberAbstract::~SubscriberAbstract(void)
+{
+    workFlags_ = false;
+}
+/**
+ * @brief 此函数内部开启一个线程来运行readyMessage函数,来接收订阅到的数据所以,
+ * 此类的派生类需要重写readyMessage函数
+ * @param func[in] 读取回调函数
+ * @return true 启动成功
+ * @return false 启动失败
+ */
+bool SubscriberAbstract::readStart(ReadCallBack func)
+{
+    std::call_once(onceFlag_, [=] {
+        eventLoopthreadPtr_ = std::make_shared<muduo::net::EventLoopThread>();
+        workFlags_ = true;
+        eventLoopthreadPtr_->startLoop()->runInLoop([=] {
+            auto func_ = std::move(func);
+            while (workFlags_) {
+                if (func_) {
+                    readyMessage(func_);
+                }
+            }
+        });
+    });
+
+    return true;
+}
+
+}

src/common/mmw/mmwabstract.h

@@ -0,0 +1,47 @@
+#pragma once
+#include "muduo/base/noncopyable.h"
+#include "muduo/net/EventLoop.h"
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+
+namespace muduo::net {
+class EventLoopThread;
+};
+
+namespace mmw {
+
+class MMWAbstract : public muduo::noncopyable {
+public:
+    MMWAbstract(void) = default;
+    virtual ~MMWAbstract(void) = default;
+    virtual bool init(void) = 0;
+};
+
+class PublisherAbstract : public MMWAbstract {
+public:
+    virtual bool init(void) = 0;
+    virtual bool publish(const char* topic, size_t topicSize, const char* content, size_t contentSize) = 0;
+    bool publish(const std::string& topic, const std::string& content);
+};
+
+class SubscriberAbstract : public MMWAbstract {
+public:
+    using ReadCallBack = std::function<void(const char*, size_t, const char*, size_t)>;
+    virtual ~SubscriberAbstract(void);
+    bool readStart(ReadCallBack func);
+    virtual bool init(void) = 0;
+    virtual bool subscribe(const std::string& topic) = 0;
+    virtual bool unSubscribe(const std::string& topic) = 0;
+
+protected:
+    virtual void readyMessage(ReadCallBack& func) = 0;
+
+private:
+    std::once_flag onceFlag_;
+    std::atomic<bool> workFlags_;
+    std::shared_ptr<muduo::net::EventLoopThread> eventLoopthreadPtr_ { nullptr };
+};
+
+}

src/common/mmw/mmwfactory.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "zmqmmw.h"
+namespace mmw {
+
+enum class MMWSelector : uint32_t {
+    ZMQ = 0
+};
+
+class MMWFactory {
+public:
+    template <typename... Args>
+    std::shared_ptr<PublisherAbstract> getPublisher(MMWSelector mmwSelector, Args&&... args)
+    {
+        switch (mmwSelector) {
+        case MMWSelector::ZMQ:
+            return std::make_shared<ZmqPublisher>(std::forward<Args>(args)...);
+        default:
+            return nullptr;
+        }
+    }
+
+    template <typename... Args>
+    std::shared_ptr<SubscriberAbstract> getSubscriber(MMWSelector mmwSelector, Args&&... args)
+    {
+        switch (mmwSelector) {
+        case MMWSelector::ZMQ:
+            return std::make_shared<ZmqSubscriber>(std::forward<Args>(args)...);
+        default:
+            return nullptr;
+        }
+    }
+};
+
+}

src/common/mmw/zmqmmw.cpp

@@ -0,0 +1,160 @@
+#include "zmqmmw.h"
+#include "../3rd/x86/muduo/include/muduo/base/FileUtil.h"
+#include "../utils/defer.h"
+#include <assert.h>
+#include <string>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <zmq.hpp>
+
+using namespace muduo::FileUtil;
+namespace mmw {
+/**
+ * @brief ZmqPublisher类的构造函数
+ * @param localAddr[in] 发布者本地绑定的地址
+ */
+ZmqPublisher::ZmqPublisher(std::string localAddr)
+    : localAddr_(std::move(localAddr))
+{
+    return;
+}
+/**
+ * @brief ZmqPublisher对象的初始化动作,完成一些初始化功能
+ * @return true 成功
+ * @return false 失败
+ */
+bool ZmqPublisher::init(void)
+{
+    if (localAddr_.empty()) {
+        return false;
+    }
+    // 如果通信方式是ipc的话且文件加没有被创建好,那先创建文件夹
+
+    auto prefixPos = localAddr_.find("ipc://");
+    if (prefixPos == 0) {
+        auto pos = localAddr_.rfind("/");
+        assert(!(pos == localAddr_.npos || pos <= prefixPos));
+        auto prrfixLen = strlen("ipc://");
+        auto dirPath = std::string(localAddr_.data() + prrfixLen, pos - prrfixLen);
+        //assert(!(!isFileExist(dirPath) && !afl::file_util::createRecursionDir(dirPath.c_str())));
+    }
+
+    publisher_.bind(localAddr_);
+    return true;
+}
+/**
+ * @brief 根据topic发布一条消息
+ * @param topic[in] 要发布消息的topic缓冲区地址
+ * @param topicSize[in] 要发布消息的topic字节个数
+ * @param content[in] 要发送消息的content缓冲区地址
+ * @param contentSize[in] 要发送消息的content字节个数
+ * @return true
+ * @return false
+ */
+bool ZmqPublisher::publish(const char* topic, size_t topicSize, const char* content, size_t contentSize)
+{
+    if (topic == nullptr && topicSize != 0) {
+        return false;
+    }
+    if (content == nullptr && contentSize != 0) {
+        return false;
+    }
+    auto result = publisher_.send(zmq::const_buffer(topic, topicSize), zmq::send_flags::sndmore);
+    if (!result.has_value() || result.value() != topicSize) {
+        return false;
+    }
+    result = publisher_.send(zmq::const_buffer(content, contentSize), zmq::send_flags::none);
+    if (!result.has_value() || result.value() != contentSize) {
+        return false;
+    }
+    return true;
+}
+/**
+ * @brief ZmqSubscriber类的构造函数
+ * @param connectAddrList[in] 连接地址表
+ */
+ZmqSubscriber::ZmqSubscriber(std::set<std::string> connectAddrSet)
+    : connectAddrSet_(std::move(connectAddrSet))
+{
+    return;
+}
+/**
+ * @brief ZmqSubscriber对象的初始化动作,完成一些初始化功能
+ * @return true 成功
+ * @return false 失败
+ */
+bool ZmqSubscriber::init(void)
+{
+    for (const auto& addr : connectAddrSet_) {
+        if (addr.empty()) {
+            continue;
+        }
+        subscriber_.connect(addr);
+    }
+    return true;
+}
+/**
+ * @brief 根据topic订阅一条消息
+ * @param topic[in] 订阅消息的topic
+ * @return true 成功
+ * @return false 失败
+ */
+bool ZmqSubscriber::subscribe(const std::string& topic)
+{
+    if (topic.find(" ") != topic.npos) {
+        return false;
+    }
+    // 开启 tcp 保活机制
+    int tcpKeepAlive = 1; // context
+    subscriber_.setsockopt(ZMQ_TCP_KEEPALIVE, &tcpKeepAlive, sizeof(tcpKeepAlive));
+
+    // 网络连接空闲60s 即发送保活包
+    int tcpKeepIdle = 60; // context
+    subscriber_.setsockopt(ZMQ_TCP_KEEPALIVE_IDLE, &tcpKeepIdle, sizeof(tcpKeepIdle));
+    subscriber_.setsockopt(ZMQ_SUBSCRIBE, topic.c_str(), topic.size());
+    return true;
+}
+/**
+ * @brief 根据topic取消订阅一条消息
+ * @param topic[in] 取消订阅消息的topic
+ * @return true 成功
+ * @return false 失败
+ */
+bool ZmqSubscriber::unSubscribe(const std::string& topic)
+{
+    if (topic.find(" ") != topic.npos) {
+        return false;
+    }
+    subscriber_.setsockopt(ZMQ_UNSUBSCRIBE, topic.c_str(), topic.size());
+    return true;
+}
+/**
+ * @brief 此函数需要需要被重写,以多态的方式调用此函数,此函数需要被重写为非阻塞模式
+ * 否则可能会在程序关闭时进行阻塞,且此函数重写时要调用回调函数
+ * @param func[in] 回调函数,此函数格式为:void(const void*, size_t, const void*, size_t),参数依次为
+ * 存储topic的首地址,topic的字节长度,存储content的首地址,存储content的字节长度
+ */
+void ZmqSubscriber::readyMessage(ReadCallBack& func)
+{
+    if (connectAddrSet_.empty()) {
+        return;
+    }
+    uint64_t time = 0;
+    zmq::message_t topic;
+    zmq::message_t content;
+    zmq::message_t* data = &topic;
+    do {
+        if (time == 1) {
+            data = &content;
+        }
+        auto result = subscriber_.recv(*data);
+        if (!result.has_value() || result.value() != data->size()) {
+            continue;
+        }
+        ++time;
+    } while (data->more());
+    if (time == 2 && func) {
+        func((char*)topic.data(), topic.size(), (char*)content.data(), content.size());
+    }
+}
+}

src/common/mmw/zmqmmw.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "../utils/vl_common.h"
+#include "mmwabstract.h"
+#include <atomic>
+#include <set>
+#include <zmq.hpp>
+using namespace utils;
+namespace mmw {
+
+class ZmqPublisher : public PublisherAbstract {
+public:
+    ZmqPublisher(std::string localIp = "");
+    virtual bool init(void) override final;
+    virtual bool publish(const char* topic, size_t topicSize, const char* content, size_t contentSize) override final;
+
+private:
+    std::string localAddr_;
+    zmq::context_t context_ { 1 };
+    zmq::socket_t publisher_ { context_, ZMQ_PUB };
+};
+
+class ZmqSubscriber : public SubscriberAbstract {
+public:
+    ZmqSubscriber(std::set<std::string> connectAddrList);
+    virtual bool init(void) override final;
+    virtual bool subscribe(const std::string& topic) override final;
+    virtual bool unSubscribe(const std::string& topic) override final;
+
+protected:
+    virtual void readyMessage(ReadCallBack& func) override final;
+
+private:
+    std::set<std::string> connectAddrSet_;
+    zmq::context_t context_ { 1 };
+    zmq::socket_t subscriber_ { context_, ZMQ_SUB };
+};
+
+}

src/common/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>common</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="todo@todo.todo">todo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rclcpp</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+

src/common/serial_ros2/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.5)
+project(serial)
+
+# Find catkin
+#find_package(catkin REQUIRED)
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+endif()
+
+
+find_package(rosidl_default_generators REQUIRED)
+find_package(ament_cmake REQUIRED)
+## Sources
+set(serial_SRCS
+    src/serial.cc
+    include/serial/serial.h
+    include/serial/v8stdint.h
+)
+
+# If unix
+list(APPEND serial_SRCS src/impl/unix.cc)
+list(APPEND serial_SRCS src/impl/list_ports/list_ports_linux.cc)
+
+
+## Add serial library
+add_library(${PROJECT_NAME} ${serial_SRCS})
+
+
+target_link_libraries(${PROJECT_NAME} rt pthread)
+
+
+## Include headers
+include_directories(include)
+
+## Install executable
+install(TARGETS
+        ${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+)
+
+ament_export_include_directories(include)
+
+ament_export_libraries(${PROJECT_NAME})
+#add_executable(${PROJECT_NAME} src/serial.cc)
+## Install headers
+install(
+        DIRECTORY include/serial
+        DESTINATION include)
+
+
+## Tests
+#if(BUILD_TESTING)
+#    add_subdirectory(tests)
+#endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
+

src/common/serial_ros2/README.md

@@ -0,0 +1,19 @@
+# Serial Communication Library
+just for linux ros2
+
+
+clone from http://wjwwood.github.com/serial/
+
+API Documentation: http://wjwwood.github.com/serial/doc/1.1.0/index.html
+
+
+
+### Install
+
+Get the code to your ros2_workspace/src:
+
+    git clone https://github.com/jinmenglei/serial_ros2
+Build:
+
+    ament build
+

src/common/serial_ros2/include/serial/impl/unix.h

@@ -0,0 +1,221 @@
+/*!
+ * \file serial/impl/unix.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a unix based pimpl for the Serial class. This implementation is
+ * based off termios.h and uses select for multiplexing the IO ports.
+ *
+ */
+
+#if !defined(_WIN32)
+
+#ifndef SERIAL_IMPL_UNIX_H
+#define SERIAL_IMPL_UNIX_H
+
+#include "serial/serial.h"
+
+#include <pthread.h>
+
+namespace serial {
+
+using std::size_t;
+using std::string;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class MillisecondTimer {
+public:
+  MillisecondTimer(const uint32_t millis);         
+  int64_t remaining();
+
+private:
+  static timespec timespec_now();
+  timespec expiry;
+};
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  string port_;               // Path to the file descriptor
+  int fd_;                    // The current file descriptor
+
+  bool is_open_;
+  bool xonxoff_;
+  bool rtscts_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+  uint32_t byte_time_ns_;     // Nanoseconds to transmit/receive a single byte
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  pthread_mutex_t read_mutex;
+  // Mutex used to lock the write functions
+  pthread_mutex_t write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_UNIX_H
+
+#endif // !defined(_WIN32)

src/common/serial_ros2/include/serial/impl/win.h

@@ -0,0 +1,207 @@
+/*!
+ * \file serial/impl/windows.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a windows implementation of the Serial class interface.
+ *
+ */
+
+#if defined(_WIN32)
+
+#ifndef SERIAL_IMPL_WINDOWS_H
+#define SERIAL_IMPL_WINDOWS_H
+
+#include "serial/serial.h"
+
+#include "windows.h"
+
+namespace serial {
+
+using std::string;
+using std::wstring;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+  
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  wstring port_;               // Path to the file descriptor
+  HANDLE fd_;
+
+  bool is_open_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  HANDLE read_mutex;
+  // Mutex used to lock the write functions
+  HANDLE write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_WINDOWS_H
+
+#endif // if defined(_WIN32)

src/common/serial_ros2/include/serial/serial.h

@@ -0,0 +1,775 @@
+/*!
+ * \file serial/serial.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison   <ash.gti@gmail.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a cross platform interface for interacting with Serial Ports.
+ */
+
+#ifndef SERIAL_H
+#define SERIAL_H
+
+#include <limits>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <sstream>
+#include <exception>
+#include <stdexcept>
+#include <serial/v8stdint.h>
+
+#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
+__LINE__, (message) )
+
+namespace serial {
+
+/*!
+ * Enumeration defines the possible bytesizes for the serial port.
+ */
+typedef enum {
+  fivebits = 5,
+  sixbits = 6,
+  sevenbits = 7,
+  eightbits = 8
+} bytesize_t;
+
+/*!
+ * Enumeration defines the possible parity types for the serial port.
+ */
+typedef enum {
+  parity_none = 0,
+  parity_odd = 1,
+  parity_even = 2,
+  parity_mark = 3,
+  parity_space = 4
+} parity_t;
+
+/*!
+ * Enumeration defines the possible stopbit types for the serial port.
+ */
+typedef enum {
+  stopbits_one = 1,
+  stopbits_two = 2,
+  stopbits_one_point_five
+} stopbits_t;
+
+/*!
+ * Enumeration defines the possible flowcontrol types for the serial port.
+ */
+typedef enum {
+  flowcontrol_none = 0,
+  flowcontrol_software,
+  flowcontrol_hardware
+} flowcontrol_t;
+
+/*!
+ * Structure for setting the timeout of the serial port, times are
+ * in milliseconds.
+ *
+ * In order to disable the interbyte timeout, set it to Timeout::max().
+ */
+struct Timeout {
+#ifdef max
+# undef max
+#endif
+  static uint32_t max() {return std::numeric_limits<uint32_t>::max();}
+  /*!
+   * Convenience function to generate Timeout structs using a
+   * single absolute timeout.
+   *
+   * \param timeout A long that defines the time in milliseconds until a
+   * timeout occurs after a call to read or write is made.
+   *
+   * \return Timeout struct that represents this simple timeout provided.
+   */
+  static Timeout simpleTimeout(uint32_t timeout) {
+    return Timeout(max(), timeout, 0, timeout, 0);
+  }
+
+  /*! Number of milliseconds between bytes received to timeout on. */
+  uint32_t inter_byte_timeout;
+  /*! A constant number of milliseconds to wait after calling read. */
+  uint32_t read_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling read.
+   */
+  uint32_t read_timeout_multiplier;
+  /*! A constant number of milliseconds to wait after calling write. */
+  uint32_t write_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling write.
+   */
+  uint32_t write_timeout_multiplier;
+
+  explicit Timeout (uint32_t inter_byte_timeout_=0,
+                    uint32_t read_timeout_constant_=0,
+                    uint32_t read_timeout_multiplier_=0,
+                    uint32_t write_timeout_constant_=0,
+                    uint32_t write_timeout_multiplier_=0)
+  : inter_byte_timeout(inter_byte_timeout_),
+    read_timeout_constant(read_timeout_constant_),
+    read_timeout_multiplier(read_timeout_multiplier_),
+    write_timeout_constant(write_timeout_constant_),
+    write_timeout_multiplier(write_timeout_multiplier_)
+  {}
+};
+
+/*!
+ * Class that provides a portable serial port interface.
+ */
+class Serial {
+public:
+  /*!
+   * Creates a Serial object and opens the port if a port is specified,
+   * otherwise it remains closed until serial::Serial::open is called.
+   *
+   * \param port A std::string containing the address of the serial port,
+   *        which would be something like 'COM1' on Windows and '/dev/ttyS0'
+   *        on Linux.
+   *
+   * \param baudrate An unsigned 32-bit integer that represents the baudrate
+   *
+   * \param timeout A serial::Timeout struct that defines the timeout
+   * conditions for the serial port. \see serial::Timeout
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \param flowcontrol Type of flowcontrol used, default is
+   * flowcontrol_none, possible values are: flowcontrol_none,
+   * flowcontrol_software, flowcontrol_hardware
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::IOException
+   * \throw std::invalid_argument
+   */
+  Serial (const std::string &port = "",
+          uint32_t baudrate = 9600,
+          Timeout timeout = Timeout(),
+          bytesize_t bytesize = eightbits,
+          parity_t parity = parity_none,
+          stopbits_t stopbits = stopbits_one,
+          flowcontrol_t flowcontrol = flowcontrol_none);
+
+  /*! Destructor */
+  virtual ~Serial ();
+
+  /*!
+   * Opens the serial port as long as the port is set and the port isn't
+   * already open.
+   *
+   * If the port is provided to the constructor then an explicit call to open
+   * is not needed.
+   *
+   * \see Serial::Serial
+   *
+   * \throw std::invalid_argument
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  void
+  open ();
+
+  /*! Gets the open status of the serial port.
+   *
+   * \return Returns true if the port is open, false otherwise.
+   */
+  bool
+  isOpen () const;
+
+  /*! Closes the serial port. */
+  void
+  close ();
+
+  /*! Return the number of characters in the buffer. */
+  size_t
+  available ();
+
+  /*! Block until there is serial data to read or read_timeout_constant
+   * number of milliseconds have elapsed. The return value is true when
+   * the function exits with the port in a readable state, false otherwise
+   * (due to timeout or select interruption). */
+  bool
+  waitReadable ();
+
+  /*! Block for a period of time corresponding to the transmission time of
+   * count characters at present serial settings. This may be used in con-
+   * junction with waitReadable to read larger blocks of data from the
+   * port. */
+  void
+  waitByteTimes (size_t count);
+
+  /*! Read a given amount of bytes from the serial port into a given buffer.
+   *
+   * The read function will return in one of three cases:
+   *  * The number of requested bytes was read.
+   *    * In this case the number of bytes requested will match the size_t
+   *      returned by read.
+   *  * A timeout occurred, in this case the number of bytes read will not
+   *    match the amount requested, but no exception will be thrown.  One of
+   *    two possible timeouts occurred:
+   *    * The inter byte timeout expired, this means that number of
+   *      milliseconds elapsed between receiving bytes from the serial port
+   *      exceeded the inter byte timeout.
+   *    * The total timeout expired, which is calculated by multiplying the
+   *      read timeout multiplier by the number of requested bytes and then
+   *      added to the read timeout constant.  If that total number of
+   *      milliseconds elapses after the initial call to read a timeout will
+   *      occur.
+   *  * An exception occurred, in this case an actual exception will be thrown.
+   *
+   * \param buffer An uint8_t array of at least the requested size.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (uint8_t *buffer, size_t size);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::vector of uint8_t.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::vector<uint8_t> &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::string.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::string &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port and return a string
+   *  containing the data.
+   *
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A std::string containing the data read from the port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  read (size_t size = 1);
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param buffer A std::string reference used to store the data.
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A size_t representing the number of bytes read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  readline (std::string &buffer, size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A std::string containing the line.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  readline (size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in multiple lines until the serial port times out.
+   *
+   * This requires a timeout > 0 before it can be run. It will read until a
+   * timeout occurs and return a list of strings.
+   *
+   * \param size A maximum length of combined lines, defaults to 65536 (2^16)
+   *
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A vector<string> containing the lines.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::vector<std::string>
+  readlines (size_t size = 65536, std::string eol = "\n");
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \param size A size_t that indicates how many bytes should be written from
+   * the given data buffer.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const uint8_t *data, size_t size);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::vector<uint8_t> &data);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::string &data);
+
+  /*! Sets the serial port identifier.
+   *
+   * \param port A const std::string reference containing the address of the
+   * serial port, which would be something like 'COM1' on Windows and
+   * '/dev/ttyS0' on Linux.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setPort (const std::string &port);
+
+  /*! Gets the serial port identifier.
+   *
+   * \see Serial::setPort
+   *
+   * \throw std::invalid_argument
+   */
+  std::string
+  getPort () const;
+
+  /*! Sets the timeout for reads and writes using the Timeout struct.
+   *
+   * There are two timeout conditions described here:
+   *  * The inter byte timeout:
+   *    * The inter_byte_timeout component of serial::Timeout defines the
+   *      maximum amount of time, in milliseconds, between receiving bytes on
+   *      the serial port that can pass before a timeout occurs.  Setting this
+   *      to zero will prevent inter byte timeouts from occurring.
+   *  * Total time timeout:
+   *    * The constant and multiplier component of this timeout condition,
+   *      for both read and write, are defined in serial::Timeout.  This
+   *      timeout occurs if the total time since the read or write call was
+   *      made exceeds the specified time in milliseconds.
+   *    * The limit is defined by multiplying the multiplier component by the
+   *      number of requested bytes and adding that product to the constant
+   *      component.  In this way if you want a read call, for example, to
+   *      timeout after exactly one second regardless of the number of bytes
+   *      you asked for then set the read_timeout_constant component of
+   *      serial::Timeout to 1000 and the read_timeout_multiplier to zero.
+   *      This timeout condition can be used in conjunction with the inter
+   *      byte timeout condition with out any problems, timeout will simply
+   *      occur when one of the two timeout conditions is met.  This allows
+   *      users to have maximum control over the trade-off between
+   *      responsiveness and efficiency.
+   *
+   * Read and write functions will return in one of three cases.  When the
+   * reading or writing is complete, when a timeout occurs, or when an
+   * exception occurs.
+   *
+   * A timeout of 0 enables non-blocking mode.
+   *
+   * \param timeout A serial::Timeout struct containing the inter byte
+   * timeout, and the read and write timeout constants and multipliers.
+   *
+   * \see serial::Timeout
+   */
+  void
+  setTimeout (Timeout &timeout);
+
+  /*! Sets the timeout for reads and writes. */
+  void
+  setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
+              uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
+              uint32_t write_timeout_multiplier)
+  {
+    Timeout timeout(inter_byte_timeout, read_timeout_constant,
+                    read_timeout_multiplier, write_timeout_constant,
+                    write_timeout_multiplier);
+    return setTimeout(timeout);
+  }
+
+  /*! Gets the timeout for reads in seconds.
+   *
+   * \return A Timeout struct containing the inter_byte_timeout, and read
+   * and write timeout constants and multipliers.
+   *
+   * \see Serial::setTimeout
+   */
+  Timeout
+  getTimeout () const;
+
+  /*! Sets the baudrate for the serial port.
+   *
+   * Possible baudrates depends on the system but some safe baudrates include:
+   * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
+   * 57600, 115200
+   * Some other baudrates that are supported by some comports:
+   * 128000, 153600, 230400, 256000, 460800, 500000, 921600
+   *
+   * \param baudrate An integer that sets the baud rate for the serial port.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBaudrate (uint32_t baudrate);
+
+  /*! Gets the baudrate for the serial port.
+   *
+   * \return An integer that sets the baud rate for the serial port.
+   *
+   * \see Serial::setBaudrate
+   *
+   * \throw std::invalid_argument
+   */
+  uint32_t
+  getBaudrate () const;
+
+  /*! Sets the bytesize for the serial port.
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBytesize (bytesize_t bytesize);
+
+  /*! Gets the bytesize for the serial port.
+   *
+   * \see Serial::setBytesize
+   *
+   * \throw std::invalid_argument
+   */
+  bytesize_t
+  getBytesize () const;
+
+  /*! Sets the parity for the serial port.
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setParity (parity_t parity);
+
+  /*! Gets the parity for the serial port.
+   *
+   * \see Serial::setParity
+   *
+   * \throw std::invalid_argument
+   */
+  parity_t
+  getParity () const;
+
+  /*! Sets the stopbits for the serial port.
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setStopbits (stopbits_t stopbits);
+
+  /*! Gets the stopbits for the serial port.
+   *
+   * \see Serial::setStopbits
+   *
+   * \throw std::invalid_argument
+   */
+  stopbits_t
+  getStopbits () const;
+
+  /*! Sets the flow control for the serial port.
+   *
+   * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
+   * possible values are: flowcontrol_none, flowcontrol_software,
+   * flowcontrol_hardware
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  /*! Gets the flow control for the serial port.
+   *
+   * \see Serial::setFlowcontrol
+   *
+   * \throw std::invalid_argument
+   */
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  /*! Flush the input and output buffers */
+  void
+  flush ();
+
+  /*! Flush only the input buffer */
+  void
+  flushInput ();
+
+  /*! Flush only the output buffer */
+  void
+  flushOutput ();
+
+  /*! Sends the RS-232 break signal.  See tcsendbreak(3). */
+  void
+  sendBreak (int duration);
+
+  /*! Set the break condition to a given level.  Defaults to true. */
+  void
+  setBreak (bool level = true);
+
+  /*! Set the RTS handshaking line to the given level.  Defaults to true. */
+  void
+  setRTS (bool level = true);
+
+  /*! Set the DTR handshaking line to the given level.  Defaults to true. */
+  void
+  setDTR (bool level = true);
+
+  /*!
+   * Blocks until CTS, DSR, RI, CD changes or something interrupts it.
+   *
+   * Can throw an exception if an error occurs while waiting.
+   * You can check the status of CTS, DSR, RI, and CD once this returns.
+   * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
+   * resolution of less than +-1ms and as good as +-0.2ms.  Otherwise a
+   * polling method is used which can give +-2ms.
+   *
+   * \return Returns true if one of the lines changed, false if something else
+   * occurred.
+   *
+   * \throw SerialException
+   */
+  bool
+  waitForChange ();
+
+  /*! Returns the current status of the CTS line. */
+  bool
+  getCTS ();
+
+  /*! Returns the current status of the DSR line. */
+  bool
+  getDSR ();
+
+  /*! Returns the current status of the RI line. */
+  bool
+  getRI ();
+
+  /*! Returns the current status of the CD line. */
+  bool
+  getCD ();
+
+private:
+  // Disable copy constructors
+  Serial(const Serial&);
+  Serial& operator=(const Serial&);
+
+  // Pimpl idiom, d_pointer
+  class SerialImpl;
+  SerialImpl *pimpl_;
+
+  // Scoped Lock Classes
+  class ScopedReadLock;
+  class ScopedWriteLock;
+
+  // Read common function
+  size_t
+  read_ (uint8_t *buffer, size_t size);
+  // Write common function
+  size_t
+  write_ (const uint8_t *data, size_t length);
+
+};
+
+class SerialException : public std::exception
+{
+  // Disable copy constructors
+  SerialException& operator=(const SerialException&);
+  std::string e_what_;
+public:
+  SerialException (const char *description) {
+      std::stringstream ss;
+      ss << "SerialException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  SerialException (const SerialException& other) : e_what_(other.e_what_) {}
+  virtual ~SerialException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class IOException : public std::exception
+{
+  // Disable copy constructors
+  IOException& operator=(const IOException&);
+  std::string file_;
+  int line_;
+  std::string e_what_;
+  int errno_;
+public:
+  explicit IOException (std::string file, int line, int errnum)
+    : file_(file), line_(line), errno_(errnum) {
+      std::stringstream ss;
+#if defined(_WIN32) && !defined(__MINGW32__)
+      char error_str [1024];
+      strerror_s(error_str, 1024, errnum);
+#else
+      char * error_str = strerror(errnum);
+#endif
+      ss << "IO Exception (" << errno_ << "): " << error_str;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  explicit IOException (std::string file, int line, const char * description)
+    : file_(file), line_(line), errno_(0) {
+      std::stringstream ss;
+      ss << "IO Exception: " << description;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  virtual ~IOException() throw() {}
+  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
+
+  int getErrorNumber () const { return errno_; }
+
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class PortNotOpenedException : public std::exception
+{
+  // Disable copy constructors
+  const PortNotOpenedException& operator=(PortNotOpenedException);
+  std::string e_what_;
+public:
+  PortNotOpenedException (const char * description)  {
+      std::stringstream ss;
+      ss << "PortNotOpenedException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {}
+  virtual ~PortNotOpenedException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+/*!
+ * Structure that describes a serial device.
+ */
+struct PortInfo {
+
+  /*! Address of the serial port (this can be passed to the constructor of Serial). */
+  std::string port;
+
+  /*! Human readable description of serial device if available. */
+  std::string description;
+
+  /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
+  std::string hardware_id;
+
+};
+
+/* Lists the serial ports available on the system
+ *
+ * Returns a vector of available serial ports, each represented
+ * by a serial::PortInfo data structure:
+ *
+ * \return vector of serial::PortInfo.
+ */
+std::vector<PortInfo>
+list_ports();
+
+} // namespace serial
+
+#endif

src/common/serial_ros2/include/serial/v8stdint.h

@@ -0,0 +1,57 @@
+// This header is from the v8 google project:
+// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h
+
+// Copyright 2012 the V8 project authors. All rights reserved.
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//     * Neither the name of Google Inc. nor the names of its
+//       contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Load definitions of standard types.
+
+#ifndef V8STDINT_H_
+#define V8STDINT_H_
+
+#include <stddef.h>
+#include <stdio.h>
+
+#if defined(_WIN32) && !defined(__MINGW32__)
+
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;  // NOLINT
+typedef unsigned short uint16_t;  // NOLINT
+typedef int int32_t;
+typedef unsigned int uint32_t;
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+// intptr_t and friends are defined in crtdefs.h through stdio.h.
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+#endif  // V8STDINT_H_

src/common/serial_ros2/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>serial</name>
+  <version>1.2.1</version>
+  <description>
+    Serial is a cross-platform, simple to use library for using serial ports on computers.
+    This library provides a C++, object oriented interface for interacting with RS-232
+    like devices on Linux and Windows.
+  </description>
+
+  <maintainer email="841549111@qq.com">jinmenglei</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://github.com/jinmenglei/serial/</url>
+  <url type="repository">https://github.com/jinmenglei/serial</url>
+  <url type="bugtracker">https://github.com/jinmenglei/serial/issues</url>
+
+  <author email="wjwwood@gmail.com">William Woodall</author>
+  <author email="ash.gti@gmail.com">John Harrison</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>boost</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/common/serial_ros2/src/impl/list_ports/list_ports_linux.cc

@@ -0,0 +1,335 @@
+#if defined(__linux__)
+
+/*
+ * Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
+ * This software is made available under the terms of the MIT licence.
+ * A copy of the licence can be obtained from:
+ * http://opensource.org/licenses/MIT
+ */
+
+#include <vector>
+#include <string>
+#include <sstream>
+#include <stdexcept>
+#include <iostream>
+#include <fstream>
+#include <cstdio>
+#include <cstdarg>
+#include <cstdlib>
+
+#include <glob.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include "serial/serial.h"
+
+using serial::PortInfo;
+using std::istringstream;
+using std::ifstream;
+using std::getline;
+using std::vector;
+using std::string;
+using std::cout;
+using std::endl;
+
+static vector<string> glob(const vector<string>& patterns);
+static string basename(const string& path);
+static string dirname(const string& path);
+static bool path_exists(const string& path);
+static string realpath(const string& path);
+static string usb_sysfs_friendly_name(const string& sys_usb_path);
+static vector<string> get_sysfs_info(const string& device_path);
+static string read_line(const string& file);
+static string usb_sysfs_hw_string(const string& sysfs_path);
+static string format(const char* format, ...);
+
+vector<string>
+glob(const vector<string>& patterns)
+{
+    vector<string> paths_found;
+
+	if(patterns.size() == 0)
+	    return paths_found;
+
+    glob_t glob_results;
+
+    int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results);
+
+    vector<string>::const_iterator iter = patterns.begin();
+
+    while(++iter != patterns.end())
+    {
+        glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results);
+    }
+
+    for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++)
+    {
+        paths_found.push_back(glob_results.gl_pathv[path_index]);
+    }
+
+    globfree(&glob_results);
+
+    return paths_found;
+}
+
+string
+basename(const string& path)
+{
+    size_t pos = path.rfind("/");
+
+    if(pos == std::string::npos)
+        return path;
+
+    return string(path, pos+1, string::npos);
+}
+
+string
+dirname(const string& path)
+{
+    size_t pos = path.rfind("/");
+
+    if(pos == std::string::npos)
+        return path;
+    else if(pos == 0)
+        return "/";
+
+    return string(path, 0, pos);
+}
+
+bool
+path_exists(const string& path)
+{
+    struct stat sb;
+
+    if( stat(path.c_str(), &sb ) == 0 )
+        return true;
+
+    return false;
+}
+
+string
+realpath(const string& path)
+{
+    char* real_path = realpath(path.c_str(), NULL);
+
+    string result;
+
+    if(real_path != NULL)
+    {
+        result = real_path;
+
+        free(real_path);
+    }
+
+    return result;
+}
+
+string
+usb_sysfs_friendly_name(const string& sys_usb_path)
+{
+    unsigned int device_number = 0;
+
+    istringstream( read_line(sys_usb_path + "/devnum") ) >> device_number;
+
+    string manufacturer = read_line( sys_usb_path + "/manufacturer" );
+
+    string product = read_line( sys_usb_path + "/product" );
+
+    string serial = read_line( sys_usb_path + "/serial" );
+
+    if( manufacturer.empty() && product.empty() && serial.empty() )
+        return "";
+
+    return format("%s %s %s", manufacturer.c_str(), product.c_str(), serial.c_str() );
+}
+
+vector<string>
+get_sysfs_info(const string& device_path)
+{
+    string device_name = basename( device_path );
+
+    string friendly_name;
+
+    string hardware_id;
+
+    string sys_device_path = format( "/sys/class/tty/%s/device", device_name.c_str() );
+
+    if( device_name.compare(0,6,"ttyUSB") == 0 )
+    {
+        sys_device_path = dirname( dirname( realpath( sys_device_path ) ) );
+
+        if( path_exists( sys_device_path ) )
+        {
+            friendly_name = usb_sysfs_friendly_name( sys_device_path );
+
+            hardware_id = usb_sysfs_hw_string( sys_device_path );
+        }
+    }
+    else if( device_name.compare(0,6,"ttyACM") == 0 )
+    {
+        sys_device_path = dirname( realpath( sys_device_path ) );
+
+        if( path_exists( sys_device_path ) )
+        {
+            friendly_name = usb_sysfs_friendly_name( sys_device_path );
+
+            hardware_id = usb_sysfs_hw_string( sys_device_path );
+        }
+    }
+    else
+    {
+        // Try to read ID string of PCI device
+
+        string sys_id_path = sys_device_path + "/id";
+
+        if( path_exists( sys_id_path ) )
+            hardware_id = read_line( sys_id_path );
+    }
+
+    if( friendly_name.empty() )
+        friendly_name = device_name;
+
+    if( hardware_id.empty() )
+        hardware_id = "n/a";
+
+    vector<string> result;
+    result.push_back(friendly_name);
+    result.push_back(hardware_id);
+
+    return result;
+}
+
+string
+read_line(const string& file)
+{
+    ifstream ifs(file.c_str(), ifstream::in);
+
+    string line;
+
+    if(ifs)
+    {
+        getline(ifs, line);
+    }
+
+    return line;
+}
+
+string
+format(const char* format, ...)
+{
+    va_list ap;
+
+    size_t buffer_size_bytes = 256;
+
+    string result;
+
+    char* buffer = (char*)malloc(buffer_size_bytes);
+
+    if( buffer == NULL )
+        return result;
+
+    bool done = false;
+
+    unsigned int loop_count = 0;
+
+    while(!done)
+    {
+        va_start(ap, format);
+
+        int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap);
+
+        if( return_value < 0 )
+        {
+            done = true;
+        }
+        else if( return_value >= buffer_size_bytes )
+        {
+            // Realloc and try again.
+
+            buffer_size_bytes = return_value + 1;
+
+            char* new_buffer_ptr = (char*)realloc(buffer, buffer_size_bytes);
+
+            if( new_buffer_ptr == NULL )
+            {
+                done = true;
+            }
+            else
+            {
+                buffer = new_buffer_ptr;
+            }
+        }
+        else
+        {
+            result = buffer;
+            done = true;
+        }
+
+        va_end(ap);
+
+        if( ++loop_count > 5 )
+            done = true;
+    }
+
+    free(buffer);
+
+    return result;
+}
+
+string
+usb_sysfs_hw_string(const string& sysfs_path)
+{
+    string serial_number = read_line( sysfs_path + "/serial" );
+
+    if( serial_number.length() > 0 )
+    {
+        serial_number = format( "SNR=%s", serial_number.c_str() );
+    }
+
+    string vid = read_line( sysfs_path + "/idVendor" );
+
+    string pid = read_line( sysfs_path + "/idProduct" );
+
+    return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(), serial_number.c_str() );
+}
+
+vector<PortInfo>
+serial::list_ports()
+{
+    vector<PortInfo> results;
+
+    vector<string> search_globs;
+    search_globs.push_back("/dev/ttyACM*");
+    search_globs.push_back("/dev/ttyS*");
+    search_globs.push_back("/dev/ttyUSB*");
+    search_globs.push_back("/dev/tty.*");
+    search_globs.push_back("/dev/cu.*");
+
+    vector<string> devices_found = glob( search_globs );
+
+    vector<string>::iterator iter = devices_found.begin();
+
+    while( iter != devices_found.end() )
+    {
+        string device = *iter++;
+
+        vector<string> sysfs_info = get_sysfs_info( device );
+
+        string friendly_name = sysfs_info[0];
+
+        string hardware_id = sysfs_info[1];
+
+        PortInfo device_entry;
+        device_entry.port = device;
+        device_entry.description = friendly_name;
+        device_entry.hardware_id = hardware_id;
+
+        results.push_back( device_entry );
+
+    }
+
+    return results;
+}
+
+#endif // defined(__linux__)

src/common/serial_ros2/src/impl/list_ports/list_ports_osx.cc

@@ -0,0 +1,286 @@
+#if defined(__APPLE__)
+
+#include <sys/param.h>
+#include <stdint.h>
+
+#include <CoreFoundation/CoreFoundation.h>
+#include <IOKit/IOKitLib.h>
+#include <IOKit/serial/IOSerialKeys.h>
+#include <IOKit/IOBSD.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "serial/serial.h"
+
+using serial::PortInfo;
+using std::string;
+using std::vector;
+
+#define HARDWARE_ID_STRING_LENGTH 128
+
+string cfstring_to_string( CFStringRef cfstring );
+string get_device_path( io_object_t& serial_port );
+string get_class_name( io_object_t& obj );
+io_registry_entry_t get_parent_iousb_device( io_object_t& serial_port );
+string get_string_property( io_object_t& device, const char* property );
+uint16_t get_int_property( io_object_t& device, const char* property );
+string rtrim(const string& str);
+
+string
+cfstring_to_string( CFStringRef cfstring )
+{
+    char cstring[MAXPATHLEN];
+    string result;
+
+    if( cfstring )
+    {
+        Boolean success = CFStringGetCString( cfstring,
+            cstring,
+            sizeof(cstring),
+            kCFStringEncodingASCII );
+
+        if( success )
+            result = cstring;
+    }
+
+    return result;
+}
+
+string
+get_device_path( io_object_t& serial_port )
+{
+    CFTypeRef callout_path;
+    string device_path;
+
+    callout_path = IORegistryEntryCreateCFProperty( serial_port,
+        CFSTR(kIOCalloutDeviceKey),
+        kCFAllocatorDefault,
+        0 );
+
+    if (callout_path)
+    {
+        if( CFGetTypeID(callout_path) == CFStringGetTypeID() )
+            device_path = cfstring_to_string( static_cast<CFStringRef>(callout_path) );
+
+        CFRelease(callout_path);
+    }
+
+    return device_path;
+}
+
+string
+get_class_name( io_object_t& obj )
+{
+    string result;
+    io_name_t class_name;
+    kern_return_t kern_result;
+
+    kern_result = IOObjectGetClass( obj, class_name );
+
+    if( kern_result == KERN_SUCCESS )
+        result = class_name;
+
+    return result;
+}
+
+io_registry_entry_t
+get_parent_iousb_device( io_object_t& serial_port )
+{
+    io_object_t device = serial_port;
+    io_registry_entry_t parent = 0;
+    io_registry_entry_t result = 0;
+    kern_return_t kern_result = KERN_FAILURE;
+    string name = get_class_name(device);
+
+    // Walk the IO Registry tree looking for this devices parent IOUSBDevice.
+    while( name != "IOUSBDevice" )
+    {
+        kern_result = IORegistryEntryGetParentEntry( device,
+        kIOServicePlane,
+        &parent );
+
+        if(kern_result != KERN_SUCCESS)
+        {
+            result = 0;
+            break;
+        }
+
+        device = parent;
+
+        name = get_class_name(device);
+    }
+
+    if(kern_result == KERN_SUCCESS)
+        result = device;
+
+    return result;
+}
+
+string
+get_string_property( io_object_t& device, const char* property )
+{
+    string property_name;
+
+    if( device )
+    {
+        CFStringRef property_as_cfstring = CFStringCreateWithCString (
+            kCFAllocatorDefault,
+            property,
+            kCFStringEncodingASCII );
+
+        CFTypeRef name_as_cfstring = IORegistryEntryCreateCFProperty(
+            device,
+            property_as_cfstring,
+            kCFAllocatorDefault,
+            0 );
+
+        if( name_as_cfstring )
+        {
+            if( CFGetTypeID(name_as_cfstring) == CFStringGetTypeID() )
+                property_name = cfstring_to_string( static_cast<CFStringRef>(name_as_cfstring) );
+
+            CFRelease(name_as_cfstring);
+        }
+
+        if(property_as_cfstring)
+            CFRelease(property_as_cfstring);
+    }
+
+    return property_name;
+}
+
+uint16_t
+get_int_property( io_object_t& device, const char* property )
+{
+    uint16_t result = 0;
+
+    if( device )
+    {
+        CFStringRef property_as_cfstring = CFStringCreateWithCString (
+            kCFAllocatorDefault,
+            property,
+            kCFStringEncodingASCII );
+
+        CFTypeRef number = IORegistryEntryCreateCFProperty( device,
+            property_as_cfstring,
+            kCFAllocatorDefault,
+            0 );
+
+        if(property_as_cfstring)
+            CFRelease(property_as_cfstring);
+
+        if( number )
+        {
+            if( CFGetTypeID(number) == CFNumberGetTypeID() )
+            {
+                bool success = CFNumberGetValue( static_cast<CFNumberRef>(number),
+                    kCFNumberSInt16Type,
+                    &result );
+
+                if( !success )
+                    result = 0;
+            }
+
+            CFRelease(number);
+        }
+
+    }
+
+    return result;
+}
+
+string rtrim(const string& str)
+{
+    string result = str;
+
+    string whitespace = " \t\f\v\n\r";
+
+    std::size_t found = result.find_last_not_of(whitespace);
+
+    if (found != std::string::npos)
+        result.erase(found+1);
+    else
+        result.clear();
+
+    return result;
+}
+
+vector<PortInfo>
+serial::list_ports(void)
+{
+    vector<PortInfo> devices_found;
+    CFMutableDictionaryRef classes_to_match;
+    io_iterator_t serial_port_iterator;
+    io_object_t serial_port;
+    mach_port_t master_port;
+    kern_return_t kern_result;
+
+    kern_result = IOMasterPort(MACH_PORT_NULL, &master_port);
+
+    if(kern_result != KERN_SUCCESS)
+        return devices_found;
+
+    classes_to_match = IOServiceMatching(kIOSerialBSDServiceValue);
+
+    if (classes_to_match == NULL)
+        return devices_found;
+
+    CFDictionarySetValue( classes_to_match,
+        CFSTR(kIOSerialBSDTypeKey),
+        CFSTR(kIOSerialBSDAllTypes) );
+
+    kern_result = IOServiceGetMatchingServices(master_port, classes_to_match, &serial_port_iterator);
+
+    if (KERN_SUCCESS != kern_result)
+        return devices_found;
+
+    while ( (serial_port = IOIteratorNext(serial_port_iterator)) )
+    {
+        string device_path = get_device_path( serial_port );
+        io_registry_entry_t parent = get_parent_iousb_device( serial_port );
+        IOObjectRelease(serial_port);
+
+        if( device_path.empty() )
+            continue;
+
+        PortInfo port_info;
+        port_info.port = device_path;
+        port_info.description = "n/a";
+        port_info.hardware_id = "n/a";
+
+        string device_name = rtrim( get_string_property( parent, "USB Product Name" ) );
+        string vendor_name = rtrim( get_string_property( parent, "USB Vendor Name") );
+        string description = rtrim( vendor_name + " " + device_name );
+        if( !description.empty() )
+            port_info.description = description;
+
+        string serial_number = rtrim(get_string_property( parent, "USB Serial Number" ) );
+        uint16_t vendor_id = get_int_property( parent, "idVendor" );
+        uint16_t product_id = get_int_property( parent, "idProduct" );
+
+        if( vendor_id && product_id )
+        {
+            char cstring[HARDWARE_ID_STRING_LENGTH];
+
+            if(serial_number.empty())
+                serial_number = "None";
+
+            int ret = snprintf( cstring, HARDWARE_ID_STRING_LENGTH, "USB VID:PID=%04x:%04x SNR=%s",
+                vendor_id,
+                product_id,
+                serial_number.c_str() );
+
+            if( (ret >= 0) && (ret < HARDWARE_ID_STRING_LENGTH) )
+                port_info.hardware_id = cstring;
+        }
+
+        devices_found.push_back(port_info);
+    }
+
+    IOObjectRelease(serial_port_iterator);
+    return devices_found;
+}
+
+#endif // defined(__APPLE__)

src/common/serial_ros2/src/impl/list_ports/list_ports_win.cc

@@ -0,0 +1,152 @@
+#if defined(_WIN32)
+
+/*
+ * Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
+ * This software is made available under the terms of the MIT licence.
+ * A copy of the licence can be obtained from:
+ * http://opensource.org/licenses/MIT
+ */
+
+#include "serial/serial.h"
+#include <tchar.h>
+#include <windows.h>
+#include <setupapi.h>
+#include <initguid.h>
+#include <devguid.h>
+#include <cstring>
+
+using serial::PortInfo;
+using std::vector;
+using std::string;
+
+static const DWORD port_name_max_length = 256;
+static const DWORD friendly_name_max_length = 256;
+static const DWORD hardware_id_max_length = 256;
+
+// Convert a wide Unicode string to an UTF8 string
+std::string utf8_encode(const std::wstring &wstr)
+{
+	int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL);
+	std::string strTo( size_needed, 0 );
+	WideCharToMultiByte                  (CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL);
+	return strTo;
+}
+
+vector<PortInfo>
+serial::list_ports()
+{
+	vector<PortInfo> devices_found;
+
+	HDEVINFO device_info_set = SetupDiGetClassDevs(
+		(const GUID *) &GUID_DEVCLASS_PORTS,
+		NULL,
+		NULL,
+		DIGCF_PRESENT);
+
+	unsigned int device_info_set_index = 0;
+	SP_DEVINFO_DATA device_info_data;
+
+	device_info_data.cbSize = sizeof(SP_DEVINFO_DATA);
+
+	while(SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data))
+	{
+		device_info_set_index++;
+
+		// Get port name
+
+		HKEY hkey = SetupDiOpenDevRegKey(
+			device_info_set,
+			&device_info_data,
+			DICS_FLAG_GLOBAL,
+			0,
+			DIREG_DEV,
+			KEY_READ);
+
+		TCHAR port_name[port_name_max_length];
+		DWORD port_name_length = port_name_max_length;
+
+		LONG return_code = RegQueryValueEx(
+					hkey,
+					_T("PortName"),
+					NULL,
+					NULL,
+					(LPBYTE)port_name,
+					&port_name_length);
+
+		RegCloseKey(hkey);
+
+		if(return_code != EXIT_SUCCESS)
+			continue;
+
+		if(port_name_length > 0 && port_name_length <= port_name_max_length)
+			port_name[port_name_length-1] = '\0';
+		else
+			port_name[0] = '\0';
+
+		// Ignore parallel ports
+
+		if(_tcsstr(port_name, _T("LPT")) != NULL)
+			continue;
+
+		// Get port friendly name
+
+		TCHAR friendly_name[friendly_name_max_length];
+		DWORD friendly_name_actual_length = 0;
+
+		BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty(
+					device_info_set,
+					&device_info_data,
+					SPDRP_FRIENDLYNAME,
+					NULL,
+					(PBYTE)friendly_name,
+					friendly_name_max_length,
+					&friendly_name_actual_length);
+
+		if(got_friendly_name == TRUE && friendly_name_actual_length > 0)
+			friendly_name[friendly_name_actual_length-1] = '\0';
+		else
+			friendly_name[0] = '\0';
+
+		// Get hardware ID
+
+		TCHAR hardware_id[hardware_id_max_length];
+		DWORD hardware_id_actual_length = 0;
+
+		BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty(
+					device_info_set,
+					&device_info_data,
+					SPDRP_HARDWAREID,
+					NULL,
+					(PBYTE)hardware_id,
+					hardware_id_max_length,
+					&hardware_id_actual_length);
+
+		if(got_hardware_id == TRUE && hardware_id_actual_length > 0)
+			hardware_id[hardware_id_actual_length-1] = '\0';
+		else
+			hardware_id[0] = '\0';
+
+		#ifdef UNICODE
+			std::string portName = utf8_encode(port_name);
+			std::string friendlyName = utf8_encode(friendly_name);
+			std::string hardwareId = utf8_encode(hardware_id);
+		#else
+			std::string portName = port_name;
+			std::string friendlyName = friendly_name;
+			std::string hardwareId = hardware_id;
+		#endif
+
+		PortInfo port_entry;
+		port_entry.port = portName;
+		port_entry.description = friendlyName;
+		port_entry.hardware_id = hardwareId;
+
+		devices_found.push_back(port_entry);
+	}
+
+	SetupDiDestroyDeviceInfoList(device_info_set);
+
+	return devices_found;
+}
+
+#endif // #if defined(_WIN32)

src/common/serial_ros2/src/impl/unix.cc

@@ -0,0 +1,1066 @@
+/* Copyright 2012 William Woodall and John Harrison
+ *
+ * Additional Contributors: Christopher Baker @bakercp
+ */
+
+#if !defined(_WIN32)
+
+#include <stdio.h>
+#include <string.h>
+#include <sstream>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/signal.h>
+#include <errno.h>
+#include <paths.h>
+#include <sysexits.h>
+#include <termios.h>
+#include <sys/param.h>
+#include <pthread.h>
+
+#if defined(__linux__)
+# include <linux/serial.h>
+#endif
+
+#include <sys/select.h>
+#include <sys/time.h>
+#include <time.h>
+#ifdef __MACH__
+#include <AvailabilityMacros.h>
+#include <mach/clock.h>
+#include <mach/mach.h>
+#endif
+
+#include "serial/impl/unix.h"
+
+#ifndef TIOCINQ
+#ifdef FIONREAD
+#define TIOCINQ FIONREAD
+#else
+#define TIOCINQ 0x541B
+#endif
+#endif
+
+#if defined(MAC_OS_X_VERSION_10_3) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_3)
+#include <IOKit/serial/ioss.h>
+#endif
+
+using std::string;
+using std::stringstream;
+using std::invalid_argument;
+using serial::MillisecondTimer;
+using serial::Serial;
+using serial::SerialException;
+using serial::PortNotOpenedException;
+using serial::IOException;
+
+
+MillisecondTimer::MillisecondTimer (const uint32_t millis)
+  : expiry(timespec_now())
+{
+  int64_t tv_nsec = expiry.tv_nsec + (millis * 1e6);
+  if (tv_nsec >= 1e9) {
+    int64_t sec_diff = tv_nsec / static_cast<int> (1e9);
+    expiry.tv_nsec = tv_nsec % static_cast<int>(1e9);
+    expiry.tv_sec += sec_diff;
+  } else {
+    expiry.tv_nsec = tv_nsec;
+  }
+}
+
+int64_t
+MillisecondTimer::remaining ()
+{
+  timespec now(timespec_now());
+  int64_t millis = (expiry.tv_sec - now.tv_sec) * 1e3;
+  millis += (expiry.tv_nsec - now.tv_nsec) / 1e6;
+  return millis;
+}
+
+timespec
+MillisecondTimer::timespec_now ()
+{
+  timespec time;
+# ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
+  clock_serv_t cclock;
+  mach_timespec_t mts;
+  host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
+  clock_get_time(cclock, &mts);
+  mach_port_deallocate(mach_task_self(), cclock);
+  time.tv_sec = mts.tv_sec;
+  time.tv_nsec = mts.tv_nsec;
+# else
+  clock_gettime(CLOCK_MONOTONIC, &time);
+# endif
+  return time;
+}
+
+timespec
+timespec_from_ms (const uint32_t millis)
+{
+  timespec time;
+  time.tv_sec = millis / 1e3;
+  time.tv_nsec = (millis - (time.tv_sec * 1e3)) * 1e6;
+  return time;
+}
+
+Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
+                                bytesize_t bytesize,
+                                parity_t parity, stopbits_t stopbits,
+                                flowcontrol_t flowcontrol)
+  : port_ (port), fd_ (-1), is_open_ (false), xonxoff_ (false), rtscts_ (false),
+    baudrate_ (baudrate), parity_ (parity),
+    bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
+{
+  pthread_mutex_init(&this->read_mutex, NULL);
+  pthread_mutex_init(&this->write_mutex, NULL);
+  if (port_.empty () == false)
+    open ();
+}
+
+Serial::SerialImpl::~SerialImpl ()
+{
+  close();
+  pthread_mutex_destroy(&this->read_mutex);
+  pthread_mutex_destroy(&this->write_mutex);
+}
+
+void
+Serial::SerialImpl::open ()
+{
+  if (port_.empty ()) {
+    throw invalid_argument ("Empty port is invalid.");
+  }
+  if (is_open_ == true) {
+    throw SerialException ("Serial port already open.");
+  }
+
+  fd_ = ::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+  if (fd_ == -1) {
+    switch (errno) {
+    case EINTR:
+      // Recurse because this is a recoverable error.
+      open ();
+      return;
+    case ENFILE:
+    case EMFILE:
+      THROW (IOException, "Too many file handles open.");
+    default:
+      THROW (IOException, errno);
+    }
+  }
+
+  reconfigurePort();
+  is_open_ = true;
+}
+
+void
+Serial::SerialImpl::reconfigurePort ()
+{
+  if (fd_ == -1) {
+    // Can only operate on a valid file descriptor
+    THROW (IOException, "Invalid file descriptor, is the serial port open?");
+  }
+
+  struct termios options; // The options for the file descriptor
+
+  if (tcgetattr(fd_, &options) == -1) {
+    THROW (IOException, "::tcgetattr");
+  }
+
+  // set up raw mode / no echo / binary
+  options.c_cflag |= (tcflag_t)  (CLOCAL | CREAD);
+  options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
+                                       ISIG | IEXTEN); //|ECHOPRT
+
+  options.c_oflag &= (tcflag_t) ~(OPOST);
+  options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK);
+#ifdef IUCLC
+  options.c_iflag &= (tcflag_t) ~IUCLC;
+#endif
+#ifdef PARMRK
+  options.c_iflag &= (tcflag_t) ~PARMRK;
+#endif
+
+  // setup baud rate
+  bool custom_baud = false;
+  speed_t baud;
+  switch (baudrate_) {
+#ifdef B0
+  case 0: baud = B0; break;
+#endif
+#ifdef B50
+  case 50: baud = B50; break;
+#endif
+#ifdef B75
+  case 75: baud = B75; break;
+#endif
+#ifdef B110
+  case 110: baud = B110; break;
+#endif
+#ifdef B134
+  case 134: baud = B134; break;
+#endif
+#ifdef B150
+  case 150: baud = B150; break;
+#endif
+#ifdef B200
+  case 200: baud = B200; break;
+#endif
+#ifdef B300
+  case 300: baud = B300; break;
+#endif
+#ifdef B600
+  case 600: baud = B600; break;
+#endif
+#ifdef B1200
+  case 1200: baud = B1200; break;
+#endif
+#ifdef B1800
+  case 1800: baud = B1800; break;
+#endif
+#ifdef B2400
+  case 2400: baud = B2400; break;
+#endif
+#ifdef B4800
+  case 4800: baud = B4800; break;
+#endif
+#ifdef B7200
+  case 7200: baud = B7200; break;
+#endif
+#ifdef B9600
+  case 9600: baud = B9600; break;
+#endif
+#ifdef B14400
+  case 14400: baud = B14400; break;
+#endif
+#ifdef B19200
+  case 19200: baud = B19200; break;
+#endif
+#ifdef B28800
+  case 28800: baud = B28800; break;
+#endif
+#ifdef B57600
+  case 57600: baud = B57600; break;
+#endif
+#ifdef B76800
+  case 76800: baud = B76800; break;
+#endif
+#ifdef B38400
+  case 38400: baud = B38400; break;
+#endif
+#ifdef B115200
+  case 115200: baud = B115200; break;
+#endif
+#ifdef B128000
+  case 128000: baud = B128000; break;
+#endif
+#ifdef B153600
+  case 153600: baud = B153600; break;
+#endif
+#ifdef B230400
+  case 230400: baud = B230400; break;
+#endif
+#ifdef B256000
+  case 256000: baud = B256000; break;
+#endif
+#ifdef B460800
+  case 460800: baud = B460800; break;
+#endif
+#ifdef B500000
+  case 500000: baud = B500000; break;
+#endif
+#ifdef B576000
+  case 576000: baud = B576000; break;
+#endif
+#ifdef B921600
+  case 921600: baud = B921600; break;
+#endif
+#ifdef B1000000
+  case 1000000: baud = B1000000; break;
+#endif
+#ifdef B1152000
+  case 1152000: baud = B1152000; break;
+#endif
+#ifdef B1500000
+  case 1500000: baud = B1500000; break;
+#endif
+#ifdef B2000000
+  case 2000000: baud = B2000000; break;
+#endif
+#ifdef B2500000
+  case 2500000: baud = B2500000; break;
+#endif
+#ifdef B3000000
+  case 3000000: baud = B3000000; break;
+#endif
+#ifdef B3500000
+  case 3500000: baud = B3500000; break;
+#endif
+#ifdef B4000000
+  case 4000000: baud = B4000000; break;
+#endif
+  default:
+    custom_baud = true;
+    // OS X support
+#if defined(MAC_OS_X_VERSION_10_4) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_4)
+    // Starting with Tiger, the IOSSIOSPEED ioctl can be used to set arbitrary baud rates
+    // other than those specified by POSIX. The driver for the underlying serial hardware
+    // ultimately determines which baud rates can be used. This ioctl sets both the input
+    // and output speed.
+    speed_t new_baud = static_cast<speed_t> (baudrate_);
+    if (-1 == ioctl (fd_, IOSSIOSPEED, &new_baud, 1)) {
+      THROW (IOException, errno);
+    }
+    // Linux Support
+#elif defined(__linux__) && defined (TIOCSSERIAL)
+    struct serial_struct ser;
+
+    if (-1 == ioctl (fd_, TIOCGSERIAL, &ser)) {
+      THROW (IOException, errno);
+    }
+
+    // set custom divisor
+    ser.custom_divisor = ser.baud_base / static_cast<int> (baudrate_);
+    // update flags
+    ser.flags &= ~ASYNC_SPD_MASK;
+    ser.flags |= ASYNC_SPD_CUST;
+
+    if (-1 == ioctl (fd_, TIOCSSERIAL, &ser)) {
+      THROW (IOException, errno);
+    }
+#else
+    throw invalid_argument ("OS does not currently support custom bauds");
+#endif
+  }
+  if (custom_baud == false) {
+#ifdef _BSD_SOURCE
+    ::cfsetspeed(&options, baud);
+#else
+    ::cfsetispeed(&options, baud);
+    ::cfsetospeed(&options, baud);
+#endif
+  }
+
+  // setup char len
+  options.c_cflag &= (tcflag_t) ~CSIZE;
+  if (bytesize_ == eightbits)
+    options.c_cflag |= CS8;
+  else if (bytesize_ == sevenbits)
+    options.c_cflag |= CS7;
+  else if (bytesize_ == sixbits)
+    options.c_cflag |= CS6;
+  else if (bytesize_ == fivebits)
+    options.c_cflag |= CS5;
+  else
+    throw invalid_argument ("invalid char len");
+  // setup stopbits
+  if (stopbits_ == stopbits_one)
+    options.c_cflag &= (tcflag_t) ~(CSTOPB);
+  else if (stopbits_ == stopbits_one_point_five)
+    // ONE POINT FIVE same as TWO.. there is no POSIX support for 1.5
+    options.c_cflag |=  (CSTOPB);
+  else if (stopbits_ == stopbits_two)
+    options.c_cflag |=  (CSTOPB);
+  else
+    throw invalid_argument ("invalid stop bit");
+  // setup parity
+  options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP);
+  if (parity_ == parity_none) {
+    options.c_cflag &= (tcflag_t) ~(PARENB | PARODD);
+  } else if (parity_ == parity_even) {
+    options.c_cflag &= (tcflag_t) ~(PARODD);
+    options.c_cflag |=  (PARENB);
+  } else if (parity_ == parity_odd) {
+    options.c_cflag |=  (PARENB | PARODD);
+  }
+#ifdef CMSPAR
+  else if (parity_ == parity_mark) {
+    options.c_cflag |=  (PARENB | CMSPAR | PARODD);
+  }
+  else if (parity_ == parity_space) {
+    options.c_cflag |=  (PARENB | CMSPAR);
+    options.c_cflag &= (tcflag_t) ~(PARODD);
+  }
+#else
+  // CMSPAR is not defined on OSX. So do not support mark or space parity.
+  else if (parity_ == parity_mark || parity_ == parity_space) {
+    throw invalid_argument ("OS does not support mark or space parity");
+  }
+#endif  // ifdef CMSPAR
+  else {
+    throw invalid_argument ("invalid parity");
+  }
+  // setup flow control
+  if (flowcontrol_ == flowcontrol_none) {
+    xonxoff_ = false;
+    rtscts_ = false;
+  }
+  if (flowcontrol_ == flowcontrol_software) {
+    xonxoff_ = true;
+    rtscts_ = false;
+  }
+  if (flowcontrol_ == flowcontrol_hardware) {
+    xonxoff_ = false;
+    rtscts_ = true;
+  }
+  // xonxoff
+#ifdef IXANY
+  if (xonxoff_)
+    options.c_iflag |=  (IXON | IXOFF); //|IXANY)
+  else
+    options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY);
+#else
+  if (xonxoff_)
+    options.c_iflag |=  (IXON | IXOFF);
+  else
+    options.c_iflag &= (tcflag_t) ~(IXON | IXOFF);
+#endif
+  // rtscts
+#ifdef CRTSCTS
+  if (rtscts_)
+    options.c_cflag |=  (CRTSCTS);
+  else
+    options.c_cflag &= (unsigned long) ~(CRTSCTS);
+#elif defined CNEW_RTSCTS
+  if (rtscts_)
+    options.c_cflag |=  (CNEW_RTSCTS);
+  else
+    options.c_cflag &= (unsigned long) ~(CNEW_RTSCTS);
+#else
+#error "OS Support seems wrong."
+#endif
+
+  // http://www.unixwiz.net/techtips/termios-vmin-vtime.html
+  // this basically sets the read call up to be a polling read,
+  // but we are using select to ensure there is data available
+  // to read before each call, so we should never needlessly poll
+  options.c_cc[VMIN] = 0;
+  options.c_cc[VTIME] = 0;
+
+  // activate settings
+  ::tcsetattr (fd_, TCSANOW, &options);
+
+  // Update byte_time_ based on the new settings.
+  uint32_t bit_time_ns = 1e9 / baudrate_;
+  byte_time_ns_ = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_);
+
+  // Compensate for the stopbits_one_point_five enum being equal to int 3,
+  // and not 1.5.
+  if (stopbits_ == stopbits_one_point_five) {
+    byte_time_ns_ += ((1.5 - stopbits_one_point_five) * bit_time_ns);
+  }
+}
+
+void
+Serial::SerialImpl::close ()
+{
+  if (is_open_ == true) {
+    if (fd_ != -1) {
+      int ret;
+      ret = ::close (fd_);
+      if (ret == 0) {
+        fd_ = -1;
+      } else {
+        THROW (IOException, errno);
+      }
+    }
+    is_open_ = false;
+  }
+}
+
+bool
+Serial::SerialImpl::isOpen () const
+{
+  return is_open_;
+}
+
+size_t
+Serial::SerialImpl::available ()
+{
+  if (!is_open_) {
+    return 0;
+  }
+  int count = 0;
+  if (-1 == ioctl (fd_, TIOCINQ, &count)) {
+      THROW (IOException, errno);
+  } else {
+      return static_cast<size_t> (count);
+  }
+}
+
+bool
+Serial::SerialImpl::waitReadable (uint32_t timeout)
+{
+  // Setup a select call to block for serial data or a timeout
+  fd_set readfds;
+  FD_ZERO (&readfds);
+  FD_SET (fd_, &readfds);
+  timespec timeout_ts (timespec_from_ms (timeout));
+  int r = pselect (fd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL);
+
+  if (r < 0) {
+    // Select was interrupted
+    if (errno == EINTR) {
+      return false;
+    }
+    // Otherwise there was some error
+    THROW (IOException, errno);
+  }
+  // Timeout occurred
+  if (r == 0) {
+    return false;
+  }
+  // This shouldn't happen, if r > 0 our fd has to be in the list!
+  if (!FD_ISSET (fd_, &readfds)) {
+    THROW (IOException, "select reports ready to read, but our fd isn't"
+           " in the list, this shouldn't happen!");
+  }
+  // Data available to read.
+  return true;
+}
+
+void
+Serial::SerialImpl::waitByteTimes (size_t count)
+{
+  timespec wait_time = { 0, static_cast<long>(byte_time_ns_ * count)};
+  pselect (0, NULL, NULL, NULL, &wait_time, NULL);
+}
+
+size_t
+Serial::SerialImpl::read (uint8_t *buf, size_t size)
+{
+  // If the port is not open, throw
+  if (!is_open_) {
+    throw PortNotOpenedException ("Serial::read");
+  }
+  size_t bytes_read = 0;
+
+  // Calculate total timeout in milliseconds t_c + (t_m * N)
+  long total_timeout_ms = timeout_.read_timeout_constant;
+  total_timeout_ms += timeout_.read_timeout_multiplier * static_cast<long> (size);
+  MillisecondTimer total_timeout(total_timeout_ms);
+
+  // Pre-fill buffer with available bytes
+  {
+    ssize_t bytes_read_now = ::read (fd_, buf, size);
+    if (bytes_read_now > 0) {
+      bytes_read = bytes_read_now;
+    }
+  }
+
+  while (bytes_read < size) {
+    int64_t timeout_remaining_ms = total_timeout.remaining();
+    if (timeout_remaining_ms <= 0) {
+      // Timed out
+      break;
+    }
+    // Timeout for the next select is whichever is less of the remaining
+    // total read timeout and the inter-byte timeout.
+    uint32_t timeout = std::min(static_cast<uint32_t> (timeout_remaining_ms),
+                                timeout_.inter_byte_timeout);
+    // Wait for the device to be readable, and then attempt to read.
+    if (waitReadable(timeout)) {
+      // If it's a fixed-length multi-byte read, insert a wait here so that
+      // we can attempt to grab the whole thing in a single IO call. Skip
+      // this wait if a non-max inter_byte_timeout is specified.
+      if (size > 1 && timeout_.inter_byte_timeout == Timeout::max()) {
+        size_t bytes_available = available();
+        if (bytes_available + bytes_read < size) {
+          waitByteTimes(size - (bytes_available + bytes_read));
+        }
+      }
+      // This should be non-blocking returning only what is available now
+      //  Then returning so that select can block again.
+      ssize_t bytes_read_now =
+        ::read (fd_, buf + bytes_read, size - bytes_read);
+      // read should always return some data as select reported it was
+      // ready to read when we get to this point.
+      if (bytes_read_now < 1) {
+        // Disconnected devices, at least on Linux, show the
+        // behavior that they are always ready to read immediately
+        // but reading returns nothing.
+        throw SerialException ("device reports readiness to read but "
+                               "returned no data (device disconnected?)");
+      }
+      // Update bytes_read
+      bytes_read += static_cast<size_t> (bytes_read_now);
+      // If bytes_read == size then we have read everything we need
+      if (bytes_read == size) {
+        break;
+      }
+      // If bytes_read < size then we have more to read
+      if (bytes_read < size) {
+        continue;
+      }
+      // If bytes_read > size then we have over read, which shouldn't happen
+      if (bytes_read > size) {
+        throw SerialException ("read over read, too many bytes where "
+                               "read, this shouldn't happen, might be "
+                               "a logical error!");
+      }
+    }
+  }
+  return bytes_read;
+}
+
+size_t
+Serial::SerialImpl::write (const uint8_t *data, size_t length)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::write");
+  }
+  fd_set writefds;
+  size_t bytes_written = 0;
+
+  // Calculate total timeout in milliseconds t_c + (t_m * N)
+  long total_timeout_ms = timeout_.write_timeout_constant;
+  total_timeout_ms += timeout_.write_timeout_multiplier * static_cast<long> (length);
+  MillisecondTimer total_timeout(total_timeout_ms);
+
+  bool first_iteration = true;
+  while (bytes_written < length) {
+    int64_t timeout_remaining_ms = total_timeout.remaining();
+    // Only consider the timeout if it's not the first iteration of the loop
+    // otherwise a timeout of 0 won't be allowed through
+    if (!first_iteration && (timeout_remaining_ms <= 0)) {
+      // Timed out
+      break;
+    }
+    first_iteration = false;
+
+    timespec timeout(timespec_from_ms(timeout_remaining_ms));
+
+    FD_ZERO (&writefds);
+    FD_SET (fd_, &writefds);
+
+    // Do the select
+    int r = pselect (fd_ + 1, NULL, &writefds, NULL, &timeout, NULL);
+
+    // Figure out what happened by looking at select's response 'r'
+    /** Error **/
+    if (r < 0) {
+      // Select was interrupted, try again
+      if (errno == EINTR) {
+        continue;
+      }
+      // Otherwise there was some error
+      THROW (IOException, errno);
+    }
+    /** Timeout **/
+    if (r == 0) {
+      break;
+    }
+    /** Port ready to write **/
+    if (r > 0) {
+      // Make sure our file descriptor is in the ready to write list
+      if (FD_ISSET (fd_, &writefds)) {
+        // This will write some
+        ssize_t bytes_written_now =
+          ::write (fd_, data + bytes_written, length - bytes_written);
+        // write should always return some data as select reported it was
+        // ready to write when we get to this point.
+        if (bytes_written_now < 1) {
+          // Disconnected devices, at least on Linux, show the
+          // behavior that they are always ready to write immediately
+          // but writing returns nothing.
+          throw SerialException ("device reports readiness to write but "
+                                 "returned no data (device disconnected?)");
+        }
+        // Update bytes_written
+        bytes_written += static_cast<size_t> (bytes_written_now);
+        // If bytes_written == size then we have written everything we need to
+        if (bytes_written == length) {
+          break;
+        }
+        // If bytes_written < size then we have more to write
+        if (bytes_written < length) {
+          continue;
+        }
+        // If bytes_written > size then we have over written, which shouldn't happen
+        if (bytes_written > length) {
+          throw SerialException ("write over wrote, too many bytes where "
+                                 "written, this shouldn't happen, might be "
+                                 "a logical error!");
+        }
+      }
+      // This shouldn't happen, if r > 0 our fd has to be in the list!
+      THROW (IOException, "select reports ready to write, but our fd isn't"
+                          " in the list, this shouldn't happen!");
+    }
+  }
+  return bytes_written;
+}
+
+void
+Serial::SerialImpl::setPort (const string &port)
+{
+  port_ = port;
+}
+
+string
+Serial::SerialImpl::getPort () const
+{
+  return port_;
+}
+
+void
+Serial::SerialImpl::setTimeout (serial::Timeout &timeout)
+{
+  timeout_ = timeout;
+}
+
+serial::Timeout
+Serial::SerialImpl::getTimeout () const
+{
+  return timeout_;
+}
+
+void
+Serial::SerialImpl::setBaudrate (unsigned long baudrate)
+{
+  baudrate_ = baudrate;
+  if (is_open_)
+    reconfigurePort ();
+}
+
+unsigned long
+Serial::SerialImpl::getBaudrate () const
+{
+  return baudrate_;
+}
+
+void
+Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
+{
+  bytesize_ = bytesize;
+  if (is_open_)
+    reconfigurePort ();
+}
+
+serial::bytesize_t
+Serial::SerialImpl::getBytesize () const
+{
+  return bytesize_;
+}
+
+void
+Serial::SerialImpl::setParity (serial::parity_t parity)
+{
+  parity_ = parity;
+  if (is_open_)
+    reconfigurePort ();
+}
+
+serial::parity_t
+Serial::SerialImpl::getParity () const
+{
+  return parity_;
+}
+
+void
+Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
+{
+  stopbits_ = stopbits;
+  if (is_open_)
+    reconfigurePort ();
+}
+
+serial::stopbits_t
+Serial::SerialImpl::getStopbits () const
+{
+  return stopbits_;
+}
+
+void
+Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
+{
+  flowcontrol_ = flowcontrol;
+  if (is_open_)
+    reconfigurePort ();
+}
+
+serial::flowcontrol_t
+Serial::SerialImpl::getFlowcontrol () const
+{
+  return flowcontrol_;
+}
+
+void
+Serial::SerialImpl::flush ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::flush");
+  }
+  tcdrain (fd_);
+}
+
+void
+Serial::SerialImpl::flushInput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::flushInput");
+  }
+  tcflush (fd_, TCIFLUSH);
+}
+
+void
+Serial::SerialImpl::flushOutput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::flushOutput");
+  }
+  tcflush (fd_, TCOFLUSH);
+}
+
+void
+Serial::SerialImpl::sendBreak (int duration)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::sendBreak");
+  }
+  tcsendbreak (fd_, static_cast<int> (duration / 4));
+}
+
+void
+Serial::SerialImpl::setBreak (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setBreak");
+  }
+
+  if (level) {
+    if (-1 == ioctl (fd_, TIOCSBRK))
+    {
+        stringstream ss;
+        ss << "setBreak failed on a call to ioctl(TIOCSBRK): " << errno << " " << strerror(errno);
+        throw(SerialException(ss.str().c_str()));
+    }
+  } else {
+    if (-1 == ioctl (fd_, TIOCCBRK))
+    {
+        stringstream ss;
+        ss << "setBreak failed on a call to ioctl(TIOCCBRK): " << errno << " " << strerror(errno);
+        throw(SerialException(ss.str().c_str()));
+    }
+  }
+}
+
+void
+Serial::SerialImpl::setRTS (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setRTS");
+  }
+
+  int command = TIOCM_RTS;
+
+  if (level) {
+    if (-1 == ioctl (fd_, TIOCMBIS, &command))
+    {
+      stringstream ss;
+      ss << "setRTS failed on a call to ioctl(TIOCMBIS): " << errno << " " << strerror(errno);
+      throw(SerialException(ss.str().c_str()));
+    }
+  } else {
+    if (-1 == ioctl (fd_, TIOCMBIC, &command))
+    {
+      stringstream ss;
+      ss << "setRTS failed on a call to ioctl(TIOCMBIC): " << errno << " " << strerror(errno);
+      throw(SerialException(ss.str().c_str()));
+    }
+  }
+}
+
+void
+Serial::SerialImpl::setDTR (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setDTR");
+  }
+
+  int command = TIOCM_DTR;
+
+  if (level) {
+    if (-1 == ioctl (fd_, TIOCMBIS, &command))
+    {
+      stringstream ss;
+      ss << "setDTR failed on a call to ioctl(TIOCMBIS): " << errno << " " << strerror(errno);
+      throw(SerialException(ss.str().c_str()));
+    }
+  } else {
+    if (-1 == ioctl (fd_, TIOCMBIC, &command))
+    {
+      stringstream ss;
+      ss << "setDTR failed on a call to ioctl(TIOCMBIC): " << errno << " " << strerror(errno);
+      throw(SerialException(ss.str().c_str()));
+    }
+  }
+}
+
+bool
+Serial::SerialImpl::waitForChange ()
+{
+#ifndef TIOCMIWAIT
+
+while (is_open_ == true) {
+
+    int status;
+
+    if (-1 == ioctl (fd_, TIOCMGET, &status))
+    {
+        stringstream ss;
+        ss << "waitForChange failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno);
+        throw(SerialException(ss.str().c_str()));
+    }
+    else
+    {
+        if (0 != (status & TIOCM_CTS)
+         || 0 != (status & TIOCM_DSR)
+         || 0 != (status & TIOCM_RI)
+         || 0 != (status & TIOCM_CD))
+        {
+          return true;
+        }
+    }
+
+    usleep(1000);
+  }
+
+  return false;
+#else
+  int command = (TIOCM_CD|TIOCM_DSR|TIOCM_RI|TIOCM_CTS);
+
+  if (-1 == ioctl (fd_, TIOCMIWAIT, &command)) {
+    stringstream ss;
+    ss << "waitForDSR failed on a call to ioctl(TIOCMIWAIT): "
+       << errno << " " << strerror(errno);
+    throw(SerialException(ss.str().c_str()));
+  }
+  return true;
+#endif
+}
+
+bool
+Serial::SerialImpl::getCTS ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCTS");
+  }
+
+  int status;
+
+  if (-1 == ioctl (fd_, TIOCMGET, &status))
+  {
+    stringstream ss;
+    ss << "getCTS failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno);
+    throw(SerialException(ss.str().c_str()));
+  }
+  else
+  {
+    return 0 != (status & TIOCM_CTS);
+  }
+}
+
+bool
+Serial::SerialImpl::getDSR ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getDSR");
+  }
+
+  int status;
+
+  if (-1 == ioctl (fd_, TIOCMGET, &status))
+  {
+      stringstream ss;
+      ss << "getDSR failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno);
+      throw(SerialException(ss.str().c_str()));
+  }
+  else
+  {
+      return 0 != (status & TIOCM_DSR);
+  }
+}
+
+bool
+Serial::SerialImpl::getRI ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getRI");
+  }
+
+  int status;
+
+  if (-1 == ioctl (fd_, TIOCMGET, &status))
+  {
+    stringstream ss;
+    ss << "getRI failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno);
+    throw(SerialException(ss.str().c_str()));
+  }
+  else
+  {
+    return 0 != (status & TIOCM_RI);
+  }
+}
+
+bool
+Serial::SerialImpl::getCD ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCD");
+  }
+
+  int status;
+
+  if (-1 == ioctl (fd_, TIOCMGET, &status))
+  {
+    stringstream ss;
+    ss << "getCD failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno);
+    throw(SerialException(ss.str().c_str()));
+  }
+  else
+  {
+    return 0 != (status & TIOCM_CD);
+  }
+}
+
+void
+Serial::SerialImpl::readLock ()
+{
+  int result = pthread_mutex_lock(&this->read_mutex);
+  if (result) {
+    THROW (IOException, result);
+  }
+}
+
+void
+Serial::SerialImpl::readUnlock ()
+{
+  int result = pthread_mutex_unlock(&this->read_mutex);
+  if (result) {
+    THROW (IOException, result);
+  }
+}
+
+void
+Serial::SerialImpl::writeLock ()
+{
+  int result = pthread_mutex_lock(&this->write_mutex);
+  if (result) {
+    THROW (IOException, result);
+  }
+}
+
+void
+Serial::SerialImpl::writeUnlock ()
+{
+  int result = pthread_mutex_unlock(&this->write_mutex);
+  if (result) {
+    THROW (IOException, result);
+  }
+}
+
+#endif // !defined(_WIN32)

src/common/serial_ros2/src/impl/win.cc

@@ -0,0 +1,646 @@
+#if defined(_WIN32)
+
+/* Copyright 2012 William Woodall and John Harrison */
+
+#include <sstream>
+
+#include "serial/impl/win.h"
+
+using std::string;
+using std::wstring;
+using std::stringstream;
+using std::invalid_argument;
+using serial::Serial;
+using serial::Timeout;
+using serial::bytesize_t;
+using serial::parity_t;
+using serial::stopbits_t;
+using serial::flowcontrol_t;
+using serial::SerialException;
+using serial::PortNotOpenedException;
+using serial::IOException;
+
+inline wstring
+_prefix_port_if_needed(const wstring &input)
+{
+  static wstring windows_com_port_prefix = L"\\\\.\\";
+  if (input.compare(windows_com_port_prefix) != 0)
+  {
+    return windows_com_port_prefix + input;
+  }
+  return input;
+}
+
+Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
+                                bytesize_t bytesize,
+                                parity_t parity, stopbits_t stopbits,
+                                flowcontrol_t flowcontrol)
+  : port_ (port.begin(), port.end()), fd_ (INVALID_HANDLE_VALUE), is_open_ (false),
+    baudrate_ (baudrate), parity_ (parity),
+    bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
+{
+  if (port_.empty () == false)
+    open ();
+  read_mutex = CreateMutex(NULL, false, NULL);
+  write_mutex = CreateMutex(NULL, false, NULL);
+}
+
+Serial::SerialImpl::~SerialImpl ()
+{
+  this->close();
+  CloseHandle(read_mutex);
+  CloseHandle(write_mutex);
+}
+
+void
+Serial::SerialImpl::open ()
+{
+  if (port_.empty ()) {
+    throw invalid_argument ("Empty port is invalid.");
+  }
+  if (is_open_ == true) {
+    throw SerialException ("Serial port already open.");
+  }
+
+  // See: https://github.com/wjwwood/serial/issues/84
+  wstring port_with_prefix = _prefix_port_if_needed(port_);
+  LPCWSTR lp_port = port_with_prefix.c_str();
+  fd_ = CreateFileW(lp_port,
+                    GENERIC_READ | GENERIC_WRITE,
+                    0,
+                    0,
+                    OPEN_EXISTING,
+                    FILE_ATTRIBUTE_NORMAL,
+                    0);
+
+  if (fd_ == INVALID_HANDLE_VALUE) {
+    DWORD create_file_err = GetLastError();
+	stringstream ss;
+    switch (create_file_err) {
+    case ERROR_FILE_NOT_FOUND:
+      // Use this->getPort to convert to a std::string
+      ss << "Specified port, " << this->getPort() << ", does not exist.";
+      THROW (IOException, ss.str().c_str());
+    default:
+      ss << "Unknown error opening the serial port: " << create_file_err;
+      THROW (IOException, ss.str().c_str());
+    }
+  }
+
+  reconfigurePort();
+  is_open_ = true;
+}
+
+void
+Serial::SerialImpl::reconfigurePort ()
+{
+  if (fd_ == INVALID_HANDLE_VALUE) {
+    // Can only operate on a valid file descriptor
+    THROW (IOException, "Invalid file descriptor, is the serial port open?");
+  }
+
+  DCB dcbSerialParams = {0};
+
+  dcbSerialParams.DCBlength=sizeof(dcbSerialParams);
+
+  if (!GetCommState(fd_, &dcbSerialParams)) {
+    //error getting state
+    THROW (IOException, "Error getting the serial port state.");
+  }
+
+  // setup baud rate
+  switch (baudrate_) {
+#ifdef CBR_0
+  case 0: dcbSerialParams.BaudRate = CBR_0; break;
+#endif
+#ifdef CBR_50
+  case 50: dcbSerialParams.BaudRate = CBR_50; break;
+#endif
+#ifdef CBR_75
+  case 75: dcbSerialParams.BaudRate = CBR_75; break;
+#endif
+#ifdef CBR_110
+  case 110: dcbSerialParams.BaudRate = CBR_110; break;
+#endif
+#ifdef CBR_134
+  case 134: dcbSerialParams.BaudRate = CBR_134; break;
+#endif
+#ifdef CBR_150
+  case 150: dcbSerialParams.BaudRate = CBR_150; break;
+#endif
+#ifdef CBR_200
+  case 200: dcbSerialParams.BaudRate = CBR_200; break;
+#endif
+#ifdef CBR_300
+  case 300: dcbSerialParams.BaudRate = CBR_300; break;
+#endif
+#ifdef CBR_600
+  case 600: dcbSerialParams.BaudRate = CBR_600; break;
+#endif
+#ifdef CBR_1200
+  case 1200: dcbSerialParams.BaudRate = CBR_1200; break;
+#endif
+#ifdef CBR_1800
+  case 1800: dcbSerialParams.BaudRate = CBR_1800; break;
+#endif
+#ifdef CBR_2400
+  case 2400: dcbSerialParams.BaudRate = CBR_2400; break;
+#endif
+#ifdef CBR_4800
+  case 4800: dcbSerialParams.BaudRate = CBR_4800; break;
+#endif
+#ifdef CBR_7200
+  case 7200: dcbSerialParams.BaudRate = CBR_7200; break;
+#endif
+#ifdef CBR_9600
+  case 9600: dcbSerialParams.BaudRate = CBR_9600; break;
+#endif
+#ifdef CBR_14400
+  case 14400: dcbSerialParams.BaudRate = CBR_14400; break;
+#endif
+#ifdef CBR_19200
+  case 19200: dcbSerialParams.BaudRate = CBR_19200; break;
+#endif
+#ifdef CBR_28800
+  case 28800: dcbSerialParams.BaudRate = CBR_28800; break;
+#endif
+#ifdef CBR_57600
+  case 57600: dcbSerialParams.BaudRate = CBR_57600; break;
+#endif
+#ifdef CBR_76800
+  case 76800: dcbSerialParams.BaudRate = CBR_76800; break;
+#endif
+#ifdef CBR_38400
+  case 38400: dcbSerialParams.BaudRate = CBR_38400; break;
+#endif
+#ifdef CBR_115200
+  case 115200: dcbSerialParams.BaudRate = CBR_115200; break;
+#endif
+#ifdef CBR_128000
+  case 128000: dcbSerialParams.BaudRate = CBR_128000; break;
+#endif
+#ifdef CBR_153600
+  case 153600: dcbSerialParams.BaudRate = CBR_153600; break;
+#endif
+#ifdef CBR_230400
+  case 230400: dcbSerialParams.BaudRate = CBR_230400; break;
+#endif
+#ifdef CBR_256000
+  case 256000: dcbSerialParams.BaudRate = CBR_256000; break;
+#endif
+#ifdef CBR_460800
+  case 460800: dcbSerialParams.BaudRate = CBR_460800; break;
+#endif
+#ifdef CBR_921600
+  case 921600: dcbSerialParams.BaudRate = CBR_921600; break;
+#endif
+  default:
+    // Try to blindly assign it
+    dcbSerialParams.BaudRate = baudrate_;
+  }
+
+  // setup char len
+  if (bytesize_ == eightbits)
+    dcbSerialParams.ByteSize = 8;
+  else if (bytesize_ == sevenbits)
+    dcbSerialParams.ByteSize = 7;
+  else if (bytesize_ == sixbits)
+    dcbSerialParams.ByteSize = 6;
+  else if (bytesize_ == fivebits)
+    dcbSerialParams.ByteSize = 5;
+  else
+    throw invalid_argument ("invalid char len");
+
+  // setup stopbits
+  if (stopbits_ == stopbits_one)
+    dcbSerialParams.StopBits = ONESTOPBIT;
+  else if (stopbits_ == stopbits_one_point_five)
+    dcbSerialParams.StopBits = ONE5STOPBITS;
+  else if (stopbits_ == stopbits_two)
+    dcbSerialParams.StopBits = TWOSTOPBITS;
+  else
+    throw invalid_argument ("invalid stop bit");
+
+  // setup parity
+  if (parity_ == parity_none) {
+    dcbSerialParams.Parity = NOPARITY;
+  } else if (parity_ == parity_even) {
+    dcbSerialParams.Parity = EVENPARITY;
+  } else if (parity_ == parity_odd) {
+    dcbSerialParams.Parity = ODDPARITY;
+  } else if (parity_ == parity_mark) {
+    dcbSerialParams.Parity = MARKPARITY;
+  } else if (parity_ == parity_space) {
+    dcbSerialParams.Parity = SPACEPARITY;
+  } else {
+    throw invalid_argument ("invalid parity");
+  }
+
+  // setup flowcontrol
+  if (flowcontrol_ == flowcontrol_none) {
+    dcbSerialParams.fOutxCtsFlow = false;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
+    dcbSerialParams.fOutX = false;
+    dcbSerialParams.fInX = false;
+  }
+  if (flowcontrol_ == flowcontrol_software) {
+    dcbSerialParams.fOutxCtsFlow = false;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
+    dcbSerialParams.fOutX = true;
+    dcbSerialParams.fInX = true;
+  }
+  if (flowcontrol_ == flowcontrol_hardware) {
+    dcbSerialParams.fOutxCtsFlow = true;
+    dcbSerialParams.fRtsControl = RTS_CONTROL_HANDSHAKE;
+    dcbSerialParams.fOutX = false;
+    dcbSerialParams.fInX = false;
+  }
+
+  // activate settings
+  if (!SetCommState(fd_, &dcbSerialParams)){
+    CloseHandle(fd_);
+    THROW (IOException, "Error setting serial port settings.");
+  }
+
+  // Setup timeouts
+  COMMTIMEOUTS timeouts = {0};
+  timeouts.ReadIntervalTimeout = timeout_.inter_byte_timeout;
+  timeouts.ReadTotalTimeoutConstant = timeout_.read_timeout_constant;
+  timeouts.ReadTotalTimeoutMultiplier = timeout_.read_timeout_multiplier;
+  timeouts.WriteTotalTimeoutConstant = timeout_.write_timeout_constant;
+  timeouts.WriteTotalTimeoutMultiplier = timeout_.write_timeout_multiplier;
+  if (!SetCommTimeouts(fd_, &timeouts)) {
+    THROW (IOException, "Error setting timeouts.");
+  }
+}
+
+void
+Serial::SerialImpl::close ()
+{
+  if (is_open_ == true) {
+    if (fd_ != INVALID_HANDLE_VALUE) {
+      int ret;
+      ret = CloseHandle(fd_);
+      if (ret == 0) {
+        stringstream ss;
+        ss << "Error while closing serial port: " << GetLastError();
+        THROW (IOException, ss.str().c_str());
+      } else {
+        fd_ = INVALID_HANDLE_VALUE;
+      }
+    }
+    is_open_ = false;
+  }
+}
+
+bool
+Serial::SerialImpl::isOpen () const
+{
+  return is_open_;
+}
+
+size_t
+Serial::SerialImpl::available ()
+{
+  if (!is_open_) {
+    return 0;
+  }
+  COMSTAT cs;
+  if (!ClearCommError(fd_, NULL, &cs)) {
+    stringstream ss;
+    ss << "Error while checking status of the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return static_cast<size_t>(cs.cbInQue);
+}
+
+bool
+Serial::SerialImpl::waitReadable (uint32_t /*timeout*/)
+{
+  THROW (IOException, "waitReadable is not implemented on Windows.");
+  return false;
+}
+
+void
+Serial::SerialImpl::waitByteTimes (size_t /*count*/)
+{
+  THROW (IOException, "waitByteTimes is not implemented on Windows.");
+}
+
+size_t
+Serial::SerialImpl::read (uint8_t *buf, size_t size)
+{
+  if (!is_open_) {
+    throw PortNotOpenedException ("Serial::read");
+  }
+  DWORD bytes_read;
+  if (!ReadFile(fd_, buf, static_cast<DWORD>(size), &bytes_read, NULL)) {
+    stringstream ss;
+    ss << "Error while reading from the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return (size_t) (bytes_read);
+}
+
+size_t
+Serial::SerialImpl::write (const uint8_t *data, size_t length)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::write");
+  }
+  DWORD bytes_written;
+  if (!WriteFile(fd_, data, static_cast<DWORD>(length), &bytes_written, NULL)) {
+    stringstream ss;
+    ss << "Error while writing to the serial port: " << GetLastError();
+    THROW (IOException, ss.str().c_str());
+  }
+  return (size_t) (bytes_written);
+}
+
+void
+Serial::SerialImpl::setPort (const string &port)
+{
+  port_ = wstring(port.begin(), port.end());
+}
+
+string
+Serial::SerialImpl::getPort () const
+{
+  return string(port_.begin(), port_.end());
+}
+
+void
+Serial::SerialImpl::setTimeout (serial::Timeout &timeout)
+{
+  timeout_ = timeout;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::Timeout
+Serial::SerialImpl::getTimeout () const
+{
+  return timeout_;
+}
+
+void
+Serial::SerialImpl::setBaudrate (unsigned long baudrate)
+{
+  baudrate_ = baudrate;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+unsigned long
+Serial::SerialImpl::getBaudrate () const
+{
+  return baudrate_;
+}
+
+void
+Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
+{
+  bytesize_ = bytesize;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::bytesize_t
+Serial::SerialImpl::getBytesize () const
+{
+  return bytesize_;
+}
+
+void
+Serial::SerialImpl::setParity (serial::parity_t parity)
+{
+  parity_ = parity;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::parity_t
+Serial::SerialImpl::getParity () const
+{
+  return parity_;
+}
+
+void
+Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
+{
+  stopbits_ = stopbits;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::stopbits_t
+Serial::SerialImpl::getStopbits () const
+{
+  return stopbits_;
+}
+
+void
+Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
+{
+  flowcontrol_ = flowcontrol;
+  if (is_open_) {
+    reconfigurePort ();
+  }
+}
+
+serial::flowcontrol_t
+Serial::SerialImpl::getFlowcontrol () const
+{
+  return flowcontrol_;
+}
+
+void
+Serial::SerialImpl::flush ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::flush");
+  }
+  FlushFileBuffers (fd_);
+}
+
+void
+Serial::SerialImpl::flushInput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException("Serial::flushInput");
+  }
+  PurgeComm(fd_, PURGE_RXCLEAR);
+}
+
+void
+Serial::SerialImpl::flushOutput ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException("Serial::flushOutput");
+  }
+  PurgeComm(fd_, PURGE_TXCLEAR);
+}
+
+void
+Serial::SerialImpl::sendBreak (int /*duration*/)
+{
+  THROW (IOException, "sendBreak is not supported on Windows.");
+}
+
+void
+Serial::SerialImpl::setBreak (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setBreak");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETBREAK);
+  } else {
+    EscapeCommFunction (fd_, CLRBREAK);
+  }
+}
+
+void
+Serial::SerialImpl::setRTS (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setRTS");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETRTS);
+  } else {
+    EscapeCommFunction (fd_, CLRRTS);
+  }
+}
+
+void
+Serial::SerialImpl::setDTR (bool level)
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::setDTR");
+  }
+  if (level) {
+    EscapeCommFunction (fd_, SETDTR);
+  } else {
+    EscapeCommFunction (fd_, CLRDTR);
+  }
+}
+
+bool
+Serial::SerialImpl::waitForChange ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::waitForChange");
+  }
+  DWORD dwCommEvent;
+
+  if (!SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) {
+    // Error setting communications mask
+    return false;
+  }
+
+  if (!WaitCommEvent(fd_, &dwCommEvent, NULL)) {
+    // An error occurred waiting for the event.
+    return false;
+  } else {
+    // Event has occurred.
+    return true;
+  }
+}
+
+bool
+Serial::SerialImpl::getCTS ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCTS");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the CTS line.");
+  }
+
+  return (MS_CTS_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getDSR ()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getDSR");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the DSR line.");
+  }
+
+  return (MS_DSR_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getRI()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getRI");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    THROW (IOException, "Error getting the status of the RI line.");
+  }
+
+  return (MS_RING_ON & dwModemStatus) != 0;
+}
+
+bool
+Serial::SerialImpl::getCD()
+{
+  if (is_open_ == false) {
+    throw PortNotOpenedException ("Serial::getCD");
+  }
+  DWORD dwModemStatus;
+  if (!GetCommModemStatus(fd_, &dwModemStatus)) {
+    // Error in GetCommModemStatus;
+    THROW (IOException, "Error getting the status of the CD line.");
+  }
+
+  return (MS_RLSD_ON & dwModemStatus) != 0;
+}
+
+void
+Serial::SerialImpl::readLock()
+{
+  if (WaitForSingleObject(read_mutex, INFINITE) != WAIT_OBJECT_0) {
+    THROW (IOException, "Error claiming read mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::readUnlock()
+{
+  if (!ReleaseMutex(read_mutex)) {
+    THROW (IOException, "Error releasing read mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::writeLock()
+{
+  if (WaitForSingleObject(write_mutex, INFINITE) != WAIT_OBJECT_0) {
+    THROW (IOException, "Error claiming write mutex.");
+  }
+}
+
+void
+Serial::SerialImpl::writeUnlock()
+{
+  if (!ReleaseMutex(write_mutex)) {
+    THROW (IOException, "Error releasing write mutex.");
+  }
+}
+
+#endif // #if defined(_WIN32)
+

src/common/serial_ros2/src/serial.cc

@@ -0,0 +1,430 @@
+/* Copyright 2012 William Woodall and John Harrison */
+#include <algorithm>
+
+#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
+# include <alloca.h>
+#endif
+
+#if defined (__MINGW32__)
+# define alloca __builtin_alloca
+#endif
+
+#include "serial/serial.h"
+
+#ifdef _WIN32
+#include "serial/impl/win.h"
+#else
+#include "serial/impl/unix.h"
+#endif
+
+using std::invalid_argument;
+using std::min;
+using std::numeric_limits;
+using std::vector;
+using std::size_t;
+using std::string;
+
+using serial::Serial;
+using serial::SerialException;
+using serial::IOException;
+using serial::bytesize_t;
+using serial::parity_t;
+using serial::stopbits_t;
+using serial::flowcontrol_t;
+
+class Serial::ScopedReadLock {
+public:
+  ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->readLock();
+  }
+  ~ScopedReadLock() {
+    this->pimpl_->readUnlock();
+  }
+private:
+  // Disable copy constructors
+  ScopedReadLock(const ScopedReadLock&);
+  const ScopedReadLock& operator=(ScopedReadLock);
+
+  SerialImpl *pimpl_;
+};
+
+class Serial::ScopedWriteLock {
+public:
+  ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->writeLock();
+  }
+  ~ScopedWriteLock() {
+    this->pimpl_->writeUnlock();
+  }
+private:
+  // Disable copy constructors
+  ScopedWriteLock(const ScopedWriteLock&);
+  const ScopedWriteLock& operator=(ScopedWriteLock);
+  SerialImpl *pimpl_;
+};
+
+Serial::Serial (const string &port, uint32_t baudrate, serial::Timeout timeout,
+                bytesize_t bytesize, parity_t parity, stopbits_t stopbits,
+                flowcontrol_t flowcontrol)
+ : pimpl_(new SerialImpl (port, baudrate, bytesize, parity,
+                                           stopbits, flowcontrol))
+{
+  pimpl_->setTimeout(timeout);
+}
+
+Serial::~Serial ()
+{
+  delete pimpl_;
+}
+
+void
+Serial::open ()
+{
+  pimpl_->open ();
+}
+
+void
+Serial::close ()
+{
+  pimpl_->close ();
+}
+
+bool
+Serial::isOpen () const
+{
+  return pimpl_->isOpen ();
+}
+
+size_t
+Serial::available ()
+{
+  return pimpl_->available ();
+}
+
+bool
+Serial::waitReadable ()
+{
+  serial::Timeout timeout(pimpl_->getTimeout ());
+  return pimpl_->waitReadable(timeout.read_timeout_constant);
+}
+
+void
+Serial::waitByteTimes (size_t count)
+{
+  pimpl_->waitByteTimes(count);
+}
+
+size_t
+Serial::read_ (uint8_t *buffer, size_t size)
+{
+  return this->pimpl_->read (buffer, size);
+}
+
+size_t
+Serial::read (uint8_t *buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  return this->pimpl_->read (buffer, size);
+}
+
+size_t
+Serial::read (std::vector<uint8_t> &buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+
+  try {
+    bytes_read = this->pimpl_->read (buffer_, size);
+  }
+  catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+
+  buffer.insert (buffer.end (), buffer_, buffer_+bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+size_t
+Serial::read (std::string &buffer, size_t size)
+{
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+  try {
+    bytes_read = this->pimpl_->read (buffer_, size);
+  }
+  catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+  buffer.append (reinterpret_cast<const char*>(buffer_), bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+string
+Serial::read (size_t size)
+{
+  std::string buffer;
+  this->read (buffer, size);
+  return buffer;
+}
+
+size_t
+Serial::readline (string &buffer, size_t size, string eol)
+{
+  ScopedReadLock lock(this->pimpl_);
+  size_t eol_len = eol.length ();
+  uint8_t *buffer_ = static_cast<uint8_t*>
+                              (alloca (size * sizeof (uint8_t)));
+  size_t read_so_far = 0;
+  while (true)
+  {
+    size_t bytes_read = this->read_ (buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      break; // Timeout occured on reading 1 byte
+    }
+    if (string (reinterpret_cast<const char*>
+         (buffer_ + read_so_far - eol_len), eol_len) == eol) {
+      break; // EOL found
+    }
+    if (read_so_far == size) {
+      break; // Reached the maximum read length
+    }
+  }
+  buffer.append(reinterpret_cast<const char*> (buffer_), read_so_far);
+  return read_so_far;
+}
+
+string
+Serial::readline (size_t size, string eol)
+{
+  std::string buffer;
+  this->readline (buffer, size, eol);
+  return buffer;
+}
+
+vector<string>
+Serial::readlines (size_t size, string eol)
+{
+  ScopedReadLock lock(this->pimpl_);
+  std::vector<std::string> lines;
+  size_t eol_len = eol.length ();
+  uint8_t *buffer_ = static_cast<uint8_t*>
+    (alloca (size * sizeof (uint8_t)));
+  size_t read_so_far = 0;
+  size_t start_of_line = 0;
+  while (read_so_far < size) {
+    size_t bytes_read = this->read_ (buffer_+read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      if (start_of_line != read_so_far) {
+        lines.push_back (
+          string (reinterpret_cast<const char*> (buffer_ + start_of_line),
+            read_so_far - start_of_line));
+      }
+      break; // Timeout occured on reading 1 byte
+    }
+    if (string (reinterpret_cast<const char*>
+         (buffer_ + read_so_far - eol_len), eol_len) == eol) {
+      // EOL found
+      lines.push_back(
+        string(reinterpret_cast<const char*> (buffer_ + start_of_line),
+          read_so_far - start_of_line));
+      start_of_line = read_so_far;
+    }
+    if (read_so_far == size) {
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+          string(reinterpret_cast<const char*> (buffer_ + start_of_line),
+            read_so_far - start_of_line));
+      }
+      break; // Reached the maximum read length
+    }
+  }
+  return lines;
+}
+
+size_t
+Serial::write (const string &data)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_ (reinterpret_cast<const uint8_t*>(data.c_str()),
+                       data.length());
+}
+
+size_t
+Serial::write (const std::vector<uint8_t> &data)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_ (&data[0], data.size());
+}
+
+size_t
+Serial::write (const uint8_t *data, size_t size)
+{
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(data, size);
+}
+
+size_t
+Serial::write_ (const uint8_t *data, size_t length)
+{
+  return pimpl_->write (data, length);
+}
+
+void
+Serial::setPort (const string &port)
+{
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  bool was_open = pimpl_->isOpen ();
+  if (was_open) close();
+  pimpl_->setPort (port);
+  if (was_open) open ();
+}
+
+string
+Serial::getPort () const
+{
+  return pimpl_->getPort ();
+}
+
+void
+Serial::setTimeout (serial::Timeout &timeout)
+{
+  pimpl_->setTimeout (timeout);
+}
+
+serial::Timeout
+Serial::getTimeout () const {
+  return pimpl_->getTimeout ();
+}
+
+void
+Serial::setBaudrate (uint32_t baudrate)
+{
+  pimpl_->setBaudrate (baudrate);
+}
+
+uint32_t
+Serial::getBaudrate () const
+{
+  return uint32_t(pimpl_->getBaudrate ());
+}
+
+void
+Serial::setBytesize (bytesize_t bytesize)
+{
+  pimpl_->setBytesize (bytesize);
+}
+
+bytesize_t
+Serial::getBytesize () const
+{
+  return pimpl_->getBytesize ();
+}
+
+void
+Serial::setParity (parity_t parity)
+{
+  pimpl_->setParity (parity);
+}
+
+parity_t
+Serial::getParity () const
+{
+  return pimpl_->getParity ();
+}
+
+void
+Serial::setStopbits (stopbits_t stopbits)
+{
+  pimpl_->setStopbits (stopbits);
+}
+
+stopbits_t
+Serial::getStopbits () const
+{
+  return pimpl_->getStopbits ();
+}
+
+void
+Serial::setFlowcontrol (flowcontrol_t flowcontrol)
+{
+  pimpl_->setFlowcontrol (flowcontrol);
+}
+
+flowcontrol_t
+Serial::getFlowcontrol () const
+{
+  return pimpl_->getFlowcontrol ();
+}
+
+void Serial::flush ()
+{
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  pimpl_->flush ();
+}
+
+void Serial::flushInput ()
+{
+  ScopedReadLock lock(this->pimpl_);
+  pimpl_->flushInput ();
+}
+
+void Serial::flushOutput ()
+{
+  ScopedWriteLock lock(this->pimpl_);
+  pimpl_->flushOutput ();
+}
+
+void Serial::sendBreak (int duration)
+{
+  pimpl_->sendBreak (duration);
+}
+
+void Serial::setBreak (bool level)
+{
+  pimpl_->setBreak (level);
+}
+
+void Serial::setRTS (bool level)
+{
+  pimpl_->setRTS (level);
+}
+
+void Serial::setDTR (bool level)
+{
+  pimpl_->setDTR (level);
+}
+
+bool Serial::waitForChange()
+{
+  return pimpl_->waitForChange();
+}
+
+bool Serial::getCTS ()
+{
+  return pimpl_->getCTS ();
+}
+
+bool Serial::getDSR ()
+{
+  return pimpl_->getDSR ();
+}
+
+bool Serial::getRI ()
+{
+  return pimpl_->getRI ();
+}
+
+bool Serial::getCD ()
+{
+  return pimpl_->getCD ();
+}

src/common/utils/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+
+file(GLOB SRCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.cpp *.c)
+
+add_library(utils STATIC ${SRCS})
+
+target_include_directories(utils PUBLIC .)
+
+file(GLOB INCS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" *.h *.hpp)
+install(FILES ${INCS} DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/utils/include/utils)
+install(TARGETS utils DESTINATION  ${CMAKE_CURRENT_SOURCE_DIR}/../3rd/x86/utils/lib)

src/common/utils/bevdetConfig.h

@@ -0,0 +1,6 @@
+#pragma once
+
+constexpr const char* bevDetConfigurePath = "/cfgs/configure.yaml";
+constexpr const char* bevDetImgStageEngine = "model/img_stage_lt_d_fp16.engine";
+constexpr const char* bevDetBEVStageEngine = "model/bev_stage_lt_d_fp16.engine";
+constexpr const char* bevDetDepth = "cfgs/bevdet_lt_depth.yaml";

src/common/utils/defer.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <functional>
+
+class defer_t final {
+public:
+    template <typename F>
+    explicit defer_t(F&& f) noexcept
+        : f_ {
+            std::forward<F>(f)
+        }
+    {
+    }
+
+    ~defer_t()
+    {
+
+        if (f_) {
+
+            f_();
+        }
+    }
+
+    defer_t(const defer_t&) = delete;
+    defer_t& operator=(const defer_t&) = delete;
+
+    defer_t(defer_t&& rhs) noexcept
+        : f_ {
+            std::move(rhs.f_)
+        }
+    {
+    }
+
+    defer_t& operator=(defer_t&& rhs) noexcept
+    {
+
+        f_ = std::move(rhs.f_);
+        return *this;
+    }
+
+private:
+    std::function<void()> f_;
+};
+
+class defer_maker final {
+
+public:
+    template <typename F>
+    defer_t operator+(F&& f)
+    {
+
+        return defer_t {
+            std::forward<F>(f)
+        };
+    }
+};
+
+#define DEFER_CONCAT_NAME(l, r) l##r
+#define DEFER_CREATE_NAME(l, r) DEFER_CONCAT_NAME(l, r)
+#define defer auto DEFER_CREATE_NAME(defer_, __COUNTER__) = defer_maker {} +
+#define defer_scope defer[&]

src/common/utils/error_event.h

@@ -0,0 +1,135 @@
+#pragma once
+
+/**
+0x01	系统基础	SYS
+0x02	can0	CAN0
+0x03	can1	CAN1
+0x04	can2	CAN2
+0x10	IMU	IMU
+0x11	GPS	GPS
+0x12	单线雷达 #1 左	LIDAR1
+0x13	单线雷达 #2 右	LIDAR2
+0x14	单线雷达 #3 左斜	LIDAR3
+0x15	单线雷达 #4 右斜	LIDAR4
+0x16	单线雷达 #5 后	LIDAR5
+0x17	相机 #1（RGB）	CAM1
+0x18	相机 #2（RGB-D/后）	CAM2
+0x19	屏幕	TFT
+
+0x20	电机驱动器	MOTOR
+0x21	上控	TOP
+ */
+enum class ErrorEventCategory {
+    SYS = 0x01,
+    CAN0 = 0x02,
+    CAN1 = 0x03,
+    CAN2 = 0x04,
+    IMU = 0x10,
+    GPS = 0x11,
+    LIDAR1 = 0x12,
+    LIDAR2 = 0x13,
+    LIDAR3 = 0x14,
+    LIDAR4 = 0x15,
+    LIDAR5 = 0x16,
+    CAM1 = 0x17,
+    CAM2 = 0x18,
+    TFT = 0x19,
+    MOTOR = 0x20,
+    TOP = 0x21,
+};
+//  01 发生  02 恢复
+enum class ErrorEventLevel {
+    OCCUR = 0x01,
+    RECOVER = 0x02,
+};
+
+#define SYS_CONFIG_ERROR 0x010101 // 配置读取失败
+#define SYS_INIT_FAIL 0x010102 // 初始化失败
+#define SYS_FILE_ERROR 0x010103 // 文件 IO 异常
+#define SYS_NETWORK_FAIL 0x010201 // 4G / 网络错误
+#define SYS_TIME_SYNC_ERROR 0x010501 // 系统时间异常
+
+#define CAN0_NOT_FOUND 0x020201 // 未检测到 CAN0
+#define CAN0_DISCONNECTED 0x020202 // CAN0 断连
+
+#define CAN1_NOT_FOUND 0x030201 // 未检测到 CAN1
+#define CAN1_DISCONNECTED 0x030202 // CAN1 断连
+
+#define CAN2_NOT_FOUND 0x040201 // 未检测到 CAN2
+#define CAN2_DISCONNECTED 0x040202 // CAN2 断连
+
+#define IMU_NOT_FOUND 0x100201 // 未检测到 IMU
+#define IMU_DISCONNECTED 0x100202 // IMU 断连
+#define IMU_TIMEOUT 0x100303 // 超时无数据
+#define IMU_DATA_INVALID 0x100304 // 数据异常（NaN/饱和）
+#define IMU_CALIB_FAIL 0x100407 // 校准失败
+#define IMU_TIME_SYNC_ERROR 0x100508 // 时间戳跳变
+#define IMU_DRIVER_ERROR 0x100609 // SDK/协议解析失败
+
+#define GPS_NOT_FOUND 0x110201 // 未检测到 GPS
+#define GPS_DISCONNECTED 0x110202 // 断连
+#define GPS_NO_SIGNAL 0x110304 // 无卫星 / SNR 太低
+#define GPS_DATA_LOSS 0x110305 // 数据丢包
+#define GPS_TIME_SYNC_ERROR 0x110508 // PPS/时间异常
+#define GPS_DRIVER_ERROR 0x110609 // 协议解析失败（NMEA/UBlox）
+
+// 左雷达
+#define LIDAR1_NOT_FOUND 0x120201 // 未检测到雷达
+#define LIDAR1_DISCONNECTED 0x120202 // 断连
+#define LIDAR1_TIMEOUT 0x120303 // 无数据
+#define LIDAR1_DATA_INVALID 0x120304 // 数据无效
+#define LIDAR1_TIME_SYNC_ERROR 0x120508 // 时间戳异常
+#define LIDAR1_DRIVER_ERROR 0x120609 // 协议解析失败
+// 右雷达
+#define LIDAR2_NOT_FOUND 0x130201 // 未检测到雷达
+#define LIDAR2_DISCONNECTED 0x130202 // 断连
+#define LIDAR2_TIMEOUT 0x130303 // 无数据
+#define LIDAR2_DATA_INVALID 0x130304 // 数据无效
+#define LIDAR2_TIME_SYNC_ERROR 0x130508 // 时间戳异常
+#define LIDAR2_DRIVER_ERROR 0x130609 // 协议解析失败
+// 左斜雷达
+#define LIDAR3_NOT_FOUND 0x140201 // 未检测到雷达
+#define LIDAR3_DISCONNECTED 0x140202 // 断连
+#define LIDAR3_TIMEOUT 0x140303 // 无数据
+#define LIDAR3_DATA_INVALID 0x140304 // 数据无效
+#define LIDAR3_TIME_SYNC_ERROR 0x140508 // 时间戳异常
+#define LIDAR3_DRIVER_ERROR 0x140609 // 协议解析失败
+// 右斜雷达
+#define LIDAR4_NOT_FOUND 0x150201 // 未检测到雷达
+#define LIDAR4_DISCONNECTED 0x150202 // 断连
+#define LIDAR4_TIMEOUT 0x150303 // 无数据
+#define LIDAR4_DATA_INVALID 0x150304 // 数据无效
+#define LIDAR4_TIME_SYNC_ERROR 0x150508 // 时间戳异常
+#define LIDAR4_DRIVER_ERROR 0x150609 // 协议解析失败
+// 后雷达
+#define LIDAR5_NOT_FOUND 0x160201 // 未检测到雷达
+#define LIDAR5_DISCONNECTED 0x160202 // 断连
+#define LIDAR5_TIMEOUT 0x160303 // 无数据
+#define LIDAR5_DATA_INVALID 0x160304 // 数据无效
+#define LIDAR5_TIME_SYNC_ERROR 0x160508 // 时间戳异常
+#define LIDAR5_DRIVER_ERROR 0x160609 // 协议解析失败
+
+#define CAM1_NOT_FOUND 0x170201 // 未检测到相机
+#define CAM1_DISCONNECTED 0x170202 // 断连
+#define CAM1_FRAME_DROP 0x170305 // 丢帧过多
+#define CAM1_DATA_INVALID 0x170304 // 图像损坏/格式错误
+#define CAM1_TIME_SYNC_ERROR 0x170508 // 时间异常
+#define CAM1_DRIVER_ERROR 0x170609 // SDK/驱动错误
+
+#define CAM2_NOT_FOUND 0x180201 // 未检测到相机
+#define CAM2_DISCONNECTED 0x180202 // 断连
+#define CAM2_DEPTH_INVALID 0x180304 // 深度数据异常
+#define CAM2_FRAME_DROP 0x180305 // 丢帧过多
+#define CAM2_TIME_SYNC_ERROR 0x180508 // 时间异常
+#define CAM2_DRIVER_ERROR 0x180609 // SDK/驱动错误
+
+#define TFT_NOT_FOUND 0x190201 // 未检测到TFT
+#define TFT_DISCONNECTED 0x190202 // 断连
+#define TFT_DEPTH_INVALID 0x190304 //
+#define TFT_FRAME_DROP 0x190305 //
+#define TFT_TIME_SYNC_ERROR 0x190508 // 时间异常
+#define TFT_DRIVER_ERROR 0x190609 // SDK/驱动错误
+
+inline uint32_t encodeErrorLevel(uint32_t error_code, ErrorEventLevel level) {
+    return (error_code << 8) | (static_cast<uint32_t>(level) & 0xFF);
+}

src/common/utils/non_timer.h

@@ -0,0 +1,62 @@
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <thread>
+namespace utils {
+// 定义定时器类
+class NonBlockingTimer {
+public:
+    // 构造函数，接受定时器时间（毫秒）和回调函数
+    NonBlockingTimer(int milliseconds, std::function<void()> callback)
+        : duration(std::chrono::milliseconds(milliseconds))
+        , callback(callback)
+        , is_running(false)
+    {
+    }
+
+    // 启动定时器
+    void start()
+    {
+        if (is_running)
+            return; // 如果定时器已经在运行，返回
+        is_running = true;
+        timer_thread = std::thread([this]() {
+            while (is_running) {
+                std::this_thread::sleep_for(duration); // 睡眠指定的时间
+                if (is_running) {
+                    callback(); // 时间到后执行回调函数
+                }
+            }
+        });
+        timer_thread.detach(); // 分离线程，非阻塞
+    }
+
+    // 停止定时器
+    void stop()
+    {
+        is_running = false;
+    }
+
+    // 检查定时器是否正在运行
+    bool running() const
+    {
+        return is_running;
+    }
+
+    ~NonBlockingTimer()
+    {
+        stop(); // 确保在销毁时停止定时器
+        if (timer_thread.joinable()) {
+            timer_thread.join(); // 如果线程可加入，加入线程
+        }
+    }
+
+private:
+    std::chrono::milliseconds duration; // 定时器的时间（毫秒）
+    std::function<void()> callback; // 回调函数
+    std::atomic<bool> is_running; // 定时器是否正在运行
+    std::thread timer_thread; // 定时器线程
+};
+}

src/common/utils/noncopyable.h

@@ -0,0 +1,15 @@
+#pragma once
+
+namespace utils {
+
+class Noncopyable {
+public:
+    Noncopyable(const Noncopyable&) = delete;
+    void operator=(const Noncopyable&) = delete;
+
+protected:
+    Noncopyable() = default;
+    ~Noncopyable() = default;
+};
+
+}

src/common/utils/serial_port.cpp

@@ -0,0 +1,149 @@
+/*
+ * 文件功能：串口参数设置、数据读写等功能的实现
+ * 创建人：yangshuai15
+ * 日  期：2021/6/16
+ */
+#include "serial_port.h"
+#include <dirent.h>
+#include <fcntl.h>
+#include <iostream>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+namespace utils {
+
+SerialPort::SerialPort(const std::string& devpath, const OpenOptions& options, bool is_block)
+    : _path(devpath)
+    , _open_options(options)
+    , _is_block(is_block)
+{
+}
+
+SerialPort::~SerialPort()
+{
+    close(_tty_fd);
+}
+
+bool SerialPort::start()
+{
+    return serial_port_init();
+}
+
+//::read(_tty_fd, data, length)
+int SerialPort::read(char* buffer, int buf_len)
+{
+    return ::read(_tty_fd, buffer, buf_len);
+}
+
+// int SerialPort::write(char* buffer, int buf_len)
+// {
+//     return ::write(_tty_fd, buffer, buf_len);
+// }
+
+int SerialPort::write(char* buffer, int buf_len)
+{
+    int offset = 0, total = buf_len;
+    ssize_t wcnt = 0;
+
+    do {
+        wcnt = ::write(_tty_fd, buffer + offset, total);
+        if (wcnt > 0) {
+            offset += wcnt;
+            total -= wcnt;
+        } else if (wcnt < 0 && errno == EINTR) {
+            usleep(1000);
+            continue;
+        } else {
+            printf("write data to serial failed. errno:%d \n", errno);
+            break;
+        }
+    } while (total > 0);
+    // std::cout << " wcnt " << wcnt << std::endl;
+    return 0;
+    // int wcnt = ::write(_tty_fd, buffer, buf_len);
+    // std::cout << " 1wcnt2 " << wcnt << std::endl;
+    // return wcnt;
+    // return ::write(_tty_fd, buffer, buf_len);
+}
+
+int SerialPort::get_tty_fd()
+{
+    if (_tty_fd > 0) {
+        return _tty_fd;
+    }
+    return -1;
+}
+
+bool SerialPort::serial_port_init()
+{
+    if (!_path.size()) {
+        return false;
+    }
+    if (_is_block) {
+        _tty_fd = open(_path.c_str(), O_RDWR | O_NOCTTY);
+    } else {
+        _tty_fd = open(_path.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+    }
+    if (_tty_fd < 0) {
+        std::cout << "open serial device " << _path << " failed!" << std::endl;
+        return false;
+    }
+
+    struct termios tios;
+    termios_options(tios, _open_options);
+    tcsetattr(_tty_fd, TCSANOW, &tios);
+    tcflush(_tty_fd, TCIOFLUSH);
+    return true;
+}
+
+void SerialPort::termios_options(termios& tios, const OpenOptions& options)
+{
+    tcgetattr(_tty_fd, &tios);
+
+    cfmakeraw(&tios);
+    tios.c_cflag &= ~(CSIZE | CRTSCTS);
+    tios.c_iflag &= ~(IXON | IXOFF | IXANY | IGNPAR);
+    tios.c_lflag &= ~(ECHOK | ECHOCTL | ECHOKE);
+    tios.c_oflag &= ~(OPOST | ONLCR);
+
+    cfsetispeed(&tios, options.baud_rate);
+    cfsetospeed(&tios, options.baud_rate);
+
+    tios.c_iflag |= (options.xon ? IXON : 0) | (options.xoff ? IXOFF : 0) | (options.xany ? IXANY : 0);
+
+    // data bits
+    int databits[] = { CS5, CS6, CS7, CS8 };
+    tios.c_cflag &= ~0x30;
+    tios.c_cflag |= databits[options.data_bits];
+
+    // stop bits
+    if (options.stop_bits == STOP_BITS_2) {
+        tios.c_cflag |= CSTOPB;
+    } else {
+        tios.c_cflag &= ~CSTOPB;
+    }
+
+    // parity  奇偶校验
+    if (options.parity == PARITY_NONE) {
+        tios.c_cflag &= ~PARENB;
+    } else {
+        tios.c_cflag |= PARENB;
+
+        if (options.parity == PARITY_MARK) {
+            tios.c_cflag |= PARMRK;
+        } else {
+            tios.c_cflag &= ~PARMRK;
+        }
+
+        if (options.parity == PARITY_ODD) {
+            tios.c_cflag |= PARODD;
+        } else {
+            tios.c_cflag &= ~PARODD;
+        }
+    }
+
+    tios.c_cc[VMIN] = options.vmin;
+    tios.c_cc[VTIME] = options.vtime;
+}
+
+} // namespace sensor

src/common/utils/serial_port.h

@@ -0,0 +1,102 @@
+/*
+ * @copyright: Copyright (c) 2024 zhengqi.com, Inc. All Rights Reserved
+ * @Author: likang(likang@baidu.com)
+ * @Date: 2024-01-05 16:32:38
+ * @FilePath: mem/src/sensor/serial_port/serial_port.h
+ * @brief:
+ */
+#ifndef SERIALPORT_H
+#define SERIALPORT_H
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+struct termios;
+namespace utils {
+
+enum BaudRate {
+    BR0 = 0000000,
+    BR50 = 0000001,
+    BR75 = 0000002,
+    BR110 = 0000003,
+    BR134 = 0000004,
+    BR150 = 0000005,
+    BR200 = 0000006,
+    BR300 = 0000007,
+    BR600 = 0000010,
+    BR1200 = 0000011,
+    BR1800 = 0000012,
+    BR2400 = 0000013,
+    BR4800 = 0000014,
+    BR9600 = 0000015,
+    BR19200 = 0000016,
+    BR38400 = 0000017,
+    BR57600 = 0010001,
+    BR115200 = 0010002,
+    BR230400 = 0010003,
+    BR460800 = 0010004,
+    BR500000 = 0010005,
+    BR576000 = 0010006,
+    BR921600 = 0010007,
+    BR1000000 = 0010010,
+    BR1152000 = 0010011,
+    BR1500000 = 0010012,
+    BR2000000 = 0010013,
+    BR2500000 = 0010014,
+    BR3000000 = 0010015,
+    BR3500000 = 0010016,
+    BR4000000 = 0010017
+};
+
+enum DataBits {
+    DATA_BITS_5,
+    DATA_BITS_6,
+    DATA_BITS_7,
+    DATA_BITS_8,
+};
+
+enum StopBits { STOP_BITS_1,
+    STOP_BITS_2 };
+
+enum Parity { PARITY_NONE,
+    PARITY_EVEN,
+    PARITY_MARK,
+    PARITY_ODD,
+    PARITY_SPACE };
+
+struct OpenOptions {
+    bool auto_open;
+    BaudRate baud_rate;
+    DataBits data_bits;
+    StopBits stop_bits;
+    Parity parity;
+    bool xon; // 当一端的数据连接不再能接受更多数据（或者接近这个状态），它发送XOFF字节给另一端。另一端收到XOFF字节，挂起数据发送。
+    bool xoff; // 一端如果准备好继续接收数据，它发送XON字节给另一端，另一端恢复数据发送。
+    bool xany;
+    int vmin;
+    int vtime;
+};
+
+class SerialPort {
+public:
+    static BaudRate baud_rate_make(unsigned long baud_rate);
+    static const OpenOptions default_options;
+    SerialPort(const std::string& devpath, const OpenOptions& options, bool isblock = false);
+    ~SerialPort();
+    bool start();
+    int read(char* buffer, int buf_len);
+    int write(char* buffer, int buf_len);
+    int get_tty_fd();
+
+private:
+    bool _is_block;
+    bool serial_port_init();
+    void termios_options(termios& tios, const OpenOptions& options);
+    std::string _path;
+    OpenOptions _open_options;
+    int _tty_fd = 0;
+};
+
+} // namespace sensor
+#endif // SERIALPORT_H

src/common/utils/singleton.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "noncopyable.h"
+#include <memory>
+
+namespace utils {
+
+template <typename T>
+class Singleton : public Noncopyable {
+public:
+    template <typename... Args>
+    static T& getInstance(Args&&... args)
+    {
+        static T t(std::forward<Args>(args)...);
+        return t;
+    }
+};
+
+}

src/common/utils/topic.h

@@ -0,0 +1,147 @@
+#pragma once
+
+constexpr const char *imagesTopic = "/sensor/camera/images";
+constexpr const char *cameraFusedImagesTopic = "/perception/cameraFused/images";
+
+constexpr const char *cameraFusedObjectsTopic = "/perception/cameraFused/objects";
+constexpr const char *multiFusedObjectsTopic = "/perception/multiFused/objects";
+constexpr const char *multiFusedObjectsTopicDecision = "/perception/decision/multiFused/objects";
+constexpr const char *obstacleMapTopicDecision = "/perception/obstacle_map";
+constexpr const char *eventTopic = "/decision/event";
+
+constexpr const char *danceStatusTopic = "/xhi/status";
+
+constexpr const char *backupCarTopic = "/sensor/backup/car";
+constexpr const char *handleControlTopic = "/handle/control";              // 手柄控制
+constexpr const char *nav2ControlTopic = "/nav/control";                   // nva2控制
+constexpr const char *appControlTopic = "/app/control";                    // app控制
+constexpr const char *carWheelSpeedTopic = "/car/speed";                   // 车辆底盘信息
+constexpr const char *carWheelSpeedStaTopic = "/wheel/twist";              // 车辆底盘信息
+constexpr const char *carEncoderTopic = "/car/encoder";                    // 车辆电机圈数
+constexpr const char *healthDataTopic = "/sensor/health/data";             // 健康计信息
+constexpr const char *healthTftDataTopic = "/sensor/tft/health/data";      // tft健康计信息
+constexpr const char *healthStatusTopic = "/sensor/health/start";          // 健康计启动
+constexpr const char *sensorhandleControlTopic = "/sensor/handle/control"; // 上控健康控制
+constexpr const char *gpsTopic = "/sensor/gps/data";
+constexpr const char *imuTopic = "/sensor/imu/data";
+constexpr const char *hrvStartTopic = "/sensor/hrv/start";
+constexpr const char *ultrasoundLeftTopic = "/sensor/left/ultrasound";
+constexpr const char *ultrasoundRightTopic = "/sensor/right/ultrasound";
+constexpr const char *ultrasoundBackTopic = "/sensor/back/ultrasound";
+constexpr const char *joystickTopic = "/sensor/joystick";
+constexpr const char *carInfoTopic = "/vlago/car/info";
+constexpr const char *operationSceTopic = "/service/operation/scenario";
+constexpr const char *serviceColourScreenTopic = "/service/colour_screen";
+constexpr const char *serviceGearTopic = "/service/gear";
+constexpr const char *modeChangeTopic = "/mode/change";
+constexpr const char *cameraH264Topic = "/sensor/backup/camera/h264";
+constexpr const char *leftLaserTopic = "/scan/left";
+constexpr const char *rightLaserTopic = "/scan/right";
+constexpr const char *leftTiltLaserTopic = "/scan/left_tilt";
+constexpr const char *rightTiltLaserTopic = "/scan/right_tilt";
+constexpr const char *backLaserTopic = "/scan/back";
+constexpr const char *leftTiltPointTopice = "/perception/left_tilt/pointcloud";
+constexpr const char *rightTiltPointTopice = "/perception/right_tilt/pointcloud";
+
+// 场景辅助测试相关topic
+constexpr const char *leftLaserROITopic = "/scene_auxiliary/left_laser/roi_filtered";
+constexpr const char *rightLaserROITopic = "/scene_auxiliary/right_laser/roi_filtered";
+constexpr const char *leftLaserClusterTopic = "/scene_auxiliary/left_laser/clusters";
+constexpr const char *rightLaserClusterTopic = "/scene_auxiliary/right_laser/clusters";
+constexpr const char *leftTiltClusterTopic = "/scene_auxiliary/left_tilt/clusters";
+constexpr const char *rightTiltClusterTopic = "/scene_auxiliary/right_tilt/clusters";
+constexpr const char *backClusterTopic = "/scene_auxiliary/back_laser/clusters";
+
+// base_link坐标系转换后的点云话题
+constexpr const char *leftLaserBaseLinkTopic = "/scene_auxiliary/left_laser/base_link";
+constexpr const char *rightLaserBaseLinkTopic = "/scene_auxiliary/right_laser/base_link";
+constexpr const char *leftTiltBaseLinkTopic = "/scene_auxiliary/left_tilt/base_link";
+constexpr const char *rightTiltBaseLinkTopic = "/scene_auxiliary/right_tilt/base_link";
+constexpr const char *backLaserBaseLinkTopic = "/scene_auxiliary/back_laser/base_link";
+
+constexpr const char *modeStatusTopic = "/vlago/mode/status";
+constexpr const char *rechargeStatusTopic = "/vlago/recharge/status";  //废弃
+constexpr const char *rechargeStateTopic = "/vlago/recharge/state";
+constexpr const char *sceneServiceModeTopic = "/scene_services/mode";
+constexpr const char *rgbPointCloudTopic = "/rgb/pointcloud";
+
+// 自车
+// constexpr const char* carInfo = "/sensor/car/information";
+
+// service
+constexpr const char *hrvStartService = "/vlago/hrv/start";
+constexpr const char *hrvInfoService = "/vlago/hrv/info";
+constexpr const char *operationSceService = "/vlago/operation/scenario";
+constexpr const char *operationStatusService = "/vlago/operation/status";
+constexpr const char *carControlService = "/vlago/car/control";
+// constexpr const char* carInfoService = "/vlago/car/info";
+constexpr const char *followControlService = "/vlago/follow_me";
+constexpr const char *followControlStatusService = "/vlago/follow_me/status";
+constexpr const char *summonService = "/valgo/car/summon";
+constexpr const char *summonStatusService = "/valgo/car/summon/status";
+constexpr const char *highService = "/vaglo/car/high";
+constexpr const char *gearsControlService = "/vlago/gear";
+constexpr const char *sensorGearControlService = "/sensor/vlago/gear";
+constexpr const char *idControlService = "/vlago/car_id";
+constexpr const char *narrowControlService = "/vlago/narrow_assistant_mode";
+constexpr const char *standardControlService = "/vlago/standard_mode";
+constexpr const char *cruiseControlService = "/vlago/cruise_mode";
+constexpr const char *auxiliaryControlService = "/vlago/safe_mode";
+constexpr const char *tftControlService = "/vlago/lamp_language_screen";
+constexpr const char *sensorTftControlService = "/sensor/vlago/lamp_language_screen";
+constexpr const char *danceControlService = "/vlago/dance_show";
+constexpr const char *modeSwitchService = "/mode/switch";
+constexpr const char *vlagoSettingService = "/vlago/setting";
+constexpr const char *vlagoModeEnableService = "/vlago/mode/enable";
+constexpr const char *rechargeService = "/vlago/auto_charge";
+constexpr const char *manageFollowmeService = "/manage/followme";
+constexpr const char *manageCruiseService = "/manage/cruise";
+constexpr const char *autoindoorService = "/vlago/autoDriveMode";
+constexpr const char *autoindoorNavGotoService = "/vlago/autoindoor/goto";
+constexpr const char *autoindoorNavCannelService = "/vlago/autoindoor/cannel";
+
+constexpr const char *feedNarrowService = "/feed/narrow/ctrl";
+constexpr const char *feedRechargeService = "/feed/recharge/ctrl";
+constexpr const char *feedFollwmeService = "/feed/follwme/ctrl";
+constexpr const char *feedDanceService = "/feed/dance/ctrl";
+//
+constexpr const char *ServiceFollowControlTopic = "/service/follow/control";
+// 模式
+constexpr const char *sceneAuxiliaryService = "/scene/auxiliary";
+constexpr const char *sceneNarrowService = "/scene/narrow";
+constexpr const char *sceneStandardService = "/scene/standard";
+constexpr const char *sceneCruiseService = "/scene/cruise";
+constexpr const char *sceneHelpcarService = "/scene/helpcar";
+constexpr const char *sceneDanceService = "/scene/dance";
+constexpr const char *sceneRechargeService = "/scene/recharge";
+constexpr const char *sceneFollowmeService = "/scene/followme";
+// 德国版本健康健 启动舞蹈秀
+constexpr const char *sceneKeyDanceTopic = "/scene/dance/key";
+
+constexpr const char *manageFollowMe = "/manage/followme";
+constexpr const char *manageDance = "/manage/dance";
+constexpr const char *manageStandard = "/manage/standard";
+
+// 模型
+constexpr const char *perTracekerTopic = "/perception/tracker/data";
+constexpr const char *perFeatuerTopic = "/perception/localization/data";
+constexpr const char *perDetctorTopic = "/perception/detector/data";
+constexpr const char *perMonnaiTopic = "/perception/end_points";
+constexpr const char *perSpeedbumpTopic = "/speedbump_marker_array";
+constexpr const char *perPoDaoTopic = "/perception/podao_detect_status";
+// 预警
+constexpr const char *auxiliaryAlarmTopic = "/vlago/auxiliary/alarm";
+constexpr const char *auxiliaryAlarmBackTopic = "/vlago/backcar/alarm";
+constexpr const char *cursorAlarmTopic = "/vlago/cruise/status";
+constexpr const char *speedbumpAlarmTopic = "/speed_bump/status";
+constexpr const char *downhillAlarmTopic = "/downhill/status";
+constexpr const char *uphillAlarmTopic = "/uphill/status";
+constexpr const char *footLimiterTopic = "/foot/limiter/status";
+
+// tft模式
+constexpr const char *tftModeTopic = "/vlago/tft/mode";
+
+constexpr const char *audioTopic = "/vlago/audio/play";
+
+// 错误码topic
+constexpr const char* errorEventTopic = "/error_events";

src/common/utils/trigger_logger.cpp

@@ -0,0 +1,358 @@
+#include "trigger_logger.h"
+
+#include <chrono>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+#include <cstdio>
+
+// WebSocket客户端实现 (复用cloud_service中的实现)
+#include <boost/asio.hpp>
+#include <boost/beast/core.hpp>
+#include <boost/beast/websocket.hpp>
+
+namespace beast = boost::beast;
+namespace http = beast::http;
+namespace websocket = beast::websocket;
+namespace net = boost::asio;
+using tcp = boost::asio::ip::tcp;
+
+namespace utils {
+static std::string escapeJsonString(const std::string& str);
+// WebSocket客户端实现
+class TriggerLogger::WebSocketClient {
+public:
+    explicit WebSocketClient(const std::string& url)
+        : url_(url), ioc_(), resolver_(ioc_), ws_(ioc_), 
+          work_guard_(net::make_work_guard(ioc_)), 
+          reconnect_timer_(ioc_), run_thread_(nullptr),
+          should_stop_(false) {
+        parseUrl();
+        run_thread_ = std::make_unique<std::thread>([this] { this->run(); });
+    }
+
+    ~WebSocketClient() {
+        should_stop_.store(true);
+        net::post(ioc_, [this] {
+            beast::error_code ec;
+            ws_.close(websocket::close_code::normal, ec);
+        });
+        if (run_thread_ && run_thread_->joinable()) {
+            run_thread_->join();
+        }
+    }
+
+    void send(const std::string& text) {
+        net::post(ioc_, [this, text] {
+            outbound_.push(text);
+            if (ws_.is_open()) {
+                this->doWrite();
+            }
+        });
+    }
+
+private:
+    void parseUrl() {
+        const std::string prefix = "ws://";
+        if (url_.rfind(prefix, 0) == 0) {
+            host_port_path_ = url_.substr(prefix.size());
+        } else {
+            host_port_path_ = url_;
+        }
+        auto slash_pos = host_port_path_.find('/');
+        if (slash_pos == std::string::npos) {
+            host_port_ = host_port_path_;
+            target_ = "/";
+        } else {
+            host_port_ = host_port_path_.substr(0, slash_pos);
+            target_ = host_port_path_.substr(slash_pos);
+        }
+        auto colon_pos = host_port_.find(':');
+        host_ = colon_pos == std::string::npos ? host_port_ : host_port_.substr(0, colon_pos);
+        port_ = colon_pos == std::string::npos ? "80" : host_port_.substr(colon_pos + 1);
+    }
+
+    void doWrite() {
+        if (outbound_.empty())
+            return;
+        auto msg = outbound_.front();
+        outbound_.pop();
+        ws_.async_write(net::buffer(msg), [this](beast::error_code ec, std::size_t) {
+            if (ec) {
+                this->reconnectSoon();
+                return;
+            }
+            if (!outbound_.empty())
+                doWrite();
+        });
+    }
+
+    void reconnectSoon() {
+        reconnect_timer_.expires_after(std::chrono::seconds(2));
+        reconnect_timer_.async_wait([this](beast::error_code) { this->connect(); });
+    }
+
+    void connect() {
+        resolver_.async_resolve(host_, port_, [this](beast::error_code ec, tcp::resolver::results_type results) {
+            if (ec) {
+                reconnectSoon();
+                return;
+            }
+            net::async_connect(ws_.next_layer(), results, [this](beast::error_code ec, const tcp::endpoint&) {
+                if (ec) {
+                    reconnectSoon();
+                    return;
+                }
+                ws_.async_handshake(host_, target_, [this](beast::error_code ec) {
+                    if (ec) {
+                        reconnectSoon();
+                        return;
+                    }
+                    if (!outbound_.empty())
+                        doWrite();
+                });
+            });
+        });
+    }
+
+    void run() {
+        connect();
+        while (!should_stop_.load()) {
+            ioc_.run_one();
+        }
+    }
+
+    std::string url_;
+    std::string host_port_path_;
+    std::string host_port_;
+    std::string host_;
+    std::string port_;
+    std::string target_;
+    net::io_context ioc_;
+    tcp::resolver resolver_;
+    websocket::stream<tcp::socket> ws_;
+    net::executor_work_guard<net::io_context::executor_type> work_guard_;
+    net::steady_timer reconnect_timer_;
+    std::unique_ptr<std::thread> run_thread_;
+    std::atomic<bool> should_stop_;
+    std::queue<std::string> outbound_;
+};
+
+// TriggerLogger 实现
+
+TriggerLogger& TriggerLogger::getInstance() {
+    static TriggerLogger instance;
+    return instance;
+}
+
+TriggerLogger::~TriggerLogger() {
+    shutdown();
+}
+
+void TriggerLogger::initialize(const std::string& websocket_url,
+                              const std::string& vehicle_id,
+                              const std::string& version) {
+    if (initialized_.load()) {
+        return;
+    }
+    
+    websocket_url_ = websocket_url;
+    vehicle_id_ = vehicle_id;
+    version_ = version;
+    
+    // 创建WebSocket客户端
+    ws_client_ = std::make_unique<WebSocketClient>(websocket_url_);
+    
+    // 启动上传线程
+    should_stop_.store(false);
+    upload_thread_ = std::thread(&TriggerLogger::uploadThread, this);
+    
+    initialized_.store(true);
+}
+
+void TriggerLogger::log(const std::string& event_name,
+                       const std::map<std::string, std::string>& data,
+                       const std::string& file_path,
+                       int line_number,
+                       const std::string& function_name,
+                       const std::string& node_name) {
+    if (!initialized_.load()) {
+        // 如果未初始化，静默失败或记录到本地日志
+        return;
+    }
+    
+    TriggerData trigger_data;
+    trigger_data.from = "vlago";
+    trigger_data.type = "trigger";
+    trigger_data.event_name = event_name;
+    trigger_data.timestamp = getCurrentTimestamp();
+    trigger_data.timestamp_str = formatTimestamp(trigger_data.timestamp);
+    trigger_data.node_name = node_name;
+    trigger_data.file_path = file_path;
+    trigger_data.line_number = line_number;
+    trigger_data.function_name = function_name;
+    trigger_data.data = data;
+    trigger_data.vehicle_id = vehicle_id_;
+    trigger_data.version = version_;
+    
+    // 添加到队列
+    {
+        std::lock_guard<std::mutex> lock(queue_mutex_);
+        if (data_queue_.size() >= MAX_QUEUE_SIZE) {
+            // 队列满时，丢弃最旧的数据
+            data_queue_.pop();
+        }
+        data_queue_.push(trigger_data);
+    }
+}
+
+void TriggerLogger::setVehicleId(const std::string& vehicle_id) {
+    vehicle_id_ = vehicle_id;
+}
+
+void TriggerLogger::setVersion(const std::string& version) {
+    version_ = version;
+}
+
+void TriggerLogger::shutdown() {
+    if (!initialized_.load()) {
+        return;
+    }
+    
+    should_stop_.store(true);
+    
+    if (upload_thread_.joinable()) {
+        upload_thread_.join();
+    }
+    
+    ws_client_.reset();
+    initialized_.store(false);
+}
+
+// JSON字符串转义辅助函数
+static std::string escapeJsonString(const std::string& str) {
+    std::string escaped;
+    escaped.reserve(str.length());
+    for (char c : str) {
+        switch (c) {
+            case '"':  escaped += "\\\""; break;
+            case '\\': escaped += "\\\\"; break;
+            case '\b': escaped += "\\b"; break;
+            case '\f': escaped += "\\f"; break;
+            case '\n': escaped += "\\n"; break;
+            case '\r': escaped += "\\r"; break;
+            case '\t': escaped += "\\t"; break;
+            default:
+                if (c >= 0 && c < 0x20) {
+                    // 控制字符转义为Unicode
+                    char buf[7];
+                    snprintf(buf, sizeof(buf), "\\u%04x", static_cast<unsigned char>(c));
+                    escaped += buf;
+                } else {
+                    escaped += c;
+                }
+                break;
+        }
+    }
+    return escaped;
+}
+
+std::string TriggerLogger::toJson(const TriggerData& trigger_data) {
+    std::ostringstream oss;
+    oss << "{";
+    
+    // 基础字段
+    oss << "\"from\":\"" << escapeJsonString(trigger_data.from) << "\"";
+    oss << ",\"type\":\"" << escapeJsonString(trigger_data.type) << "\"";
+    oss << ",\"event\":\"" << escapeJsonString(trigger_data.event_name) << "\"";
+    oss << ",\"timestamp\":" << trigger_data.timestamp;
+    oss << ",\"timestamp_str\":\"" << escapeJsonString(trigger_data.timestamp_str) << "\"";
+    
+    // 位置信息
+    if (!trigger_data.node_name.empty()) {
+        oss << ",\"node_name\":\"" << escapeJsonString(trigger_data.node_name) << "\"";
+    }
+    if (!trigger_data.file_path.empty()) {
+        oss << ",\"file\":\"" << escapeJsonString(trigger_data.file_path) << "\"";
+    }
+    if (trigger_data.line_number > 0) {
+        oss << ",\"line\":" << trigger_data.line_number;
+    }
+    if (!trigger_data.function_name.empty()) {
+        oss << ",\"function\":\"" << escapeJsonString(trigger_data.function_name) << "\"";
+    }
+    
+    // 业务数据
+    if (!trigger_data.data.empty()) {
+        oss << ",\"data\":{";
+        bool first = true;
+        for (const auto& [key, value] : trigger_data.data) {
+            if (!first) oss << ",";
+            first = false;
+            // 转义JSON特殊字符
+            std::string escaped_key = escapeJsonString(key);
+            std::string escaped_value = escapeJsonString(value);
+            oss << "\"" << escaped_key << "\":\"" << escaped_value << "\"";
+        }
+        oss << "}";
+    }
+    
+    // 系统信息
+    if (!trigger_data.vehicle_id.empty()) {
+        oss << ",\"vehicle_id\":\"" << escapeJsonString(trigger_data.vehicle_id) << "\"";
+    }
+    if (!trigger_data.version.empty()) {
+        oss << ",\"version\":\"" << escapeJsonString(trigger_data.version) << "\"";
+    }
+    
+    oss << "}";
+    return oss.str();
+}
+
+void TriggerLogger::uploadThread() {
+    while (!should_stop_.load()) {
+        TriggerData trigger_data;
+        bool has_data = false;
+        
+        // 从队列中取出数据
+        {
+            std::lock_guard<std::mutex> lock(queue_mutex_);
+            if (!data_queue_.empty()) {
+                trigger_data = data_queue_.front();
+                data_queue_.pop();
+                has_data = true;
+            }
+        }
+        
+        if (has_data && ws_client_) {
+            // 转换为JSON并上传
+            std::string json = toJson(trigger_data);
+            ws_client_->send(json);
+        } else {
+            // 没有数据时，短暂休眠
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    }
+}
+
+int64_t TriggerLogger::getCurrentTimestamp() {
+    auto now = std::chrono::system_clock::now();
+    auto duration = now.time_since_epoch();
+    return std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
+}
+
+std::string TriggerLogger::formatTimestamp(int64_t timestamp_ms) {
+    std::time_t tt = timestamp_ms / 1000;
+    std::tm tm_buf;
+#if defined(_WIN32)
+    localtime_s(&tm_buf, &tt);
+#else
+    localtime_r(&tt, &tm_buf);
+#endif
+    std::ostringstream ss;
+    ss << std::put_time(&tm_buf, "%Y-%m-%d %H:%M:%S");
+    return ss.str();
+}
+
+} // namespace utils
+