libplateau/src/geometry/geo_reference.cpp at 721f421a032eb68593e167d2d142e2716324d554 · Synesthesias/libplateau · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include <plateau/geometry/geo_reference.h>
#include "polar_to_plane_cartesian.h"
#include "plateau/mesh_writer/obj_writer.h"

namespace plateau::geometry {
    GeoReference::GeoReference(int coordinate_zone_id, const TVec3d& reference_point, float unit_scale,
                               CoordinateSystem coordinate_system) :
        reference_point_(reference_point),
        coordinate_system_(coordinate_system),
        unit_scale_(unit_scale),
        zone_id_(coordinate_zone_id) {
    }

    TVec3d GeoReference::project(const GeoCoordinate& point) const {
        double lat_lon[3] = { point.latitude, point.longitude, point.height };
        return project(lat_lon);
    }

    TVec3d GeoReference::project(const TVec3d& lat_lon) const {
        TVec3d point = lat_lon;
        PolarToPlaneCartesian().project(point, zone_id_);
        TVec3 converted_point = convertAxisFromENUTo(coordinate_system_, point);
        converted_point = converted_point / unit_scale_ - reference_point_;
        return converted_point;
    }

    TVec3d GeoReference::convert(const TVec3d& lat_lon, const bool convert_axis, const bool project) const {
        //平面直角座標変換、座標軸変換をフラグに応じてスキップします。
        TVec3d point = lat_lon;
		// 平面直角座標系に変換
        if (project)
            PolarToPlaneCartesian().project(point, zone_id_);
        if (!convert_axis) {
			// 座標軸変換をしない場合
            TVec3 converted_point = point / unit_scale_ - convertAxisToENU(coordinate_system_, reference_point_);
            return converted_point;
        }
		// 座標軸変換をする場合
        TVec3 converted_point = convertAxisFromENUTo(coordinate_system_, point);
        converted_point = converted_point / unit_scale_ - reference_point_;
        return converted_point;
    }

    TVec3d GeoReference::convertAxisToENU(const TVec3d& vertex) const {
        return convertAxisToENU(getCoordinateSystem(), vertex);
    }

    TVec3d GeoReference::projectWithoutAxisConvert(const TVec3d& lat_lon) const {
        // 前提として、座標軸は 変換前 ENU → 変換後 ENU であるとします。
        // そのため reference_point_ の代わりに reference_point_ を ENU に変換した値が利用されます。
        TVec3d point = lat_lon;
        PolarToPlaneCartesian().project(point, zone_id_);
        TVec3 converted_point = point / unit_scale_ - convertAxisToENU(coordinate_system_, reference_point_);
        return converted_point;
    }

    TVec3d GeoReference::convertAxisFromENUTo(CoordinateSystem axis, const TVec3d& vertex) {
        switch (axis) {
        case CoordinateSystem::ENU:
            return vertex; // 変換なし
        case CoordinateSystem::WUN:
            return { -vertex.x, vertex.z, vertex.y };
        case CoordinateSystem::ESU:
            return { vertex.x, -vertex.y, vertex.z };
        case CoordinateSystem::EUN:
            return { vertex.x, vertex.z, vertex.y };
        default:
            throw std::out_of_range("Invalid argument");
        }
    }

    TVec3d GeoReference::convertAxisToENU(CoordinateSystem axis, const TVec3d& vertex) {
        switch (axis) {
        case CoordinateSystem::ENU:
            // 変換なし
            return vertex;
        case CoordinateSystem::WUN:
            // WUN → ENU の式は 逆変換 ENU → WUN と同じです。
            return { -vertex.x, vertex.z, vertex.y };
        case CoordinateSystem::ESU:
            // EUN → ESU の式は 逆変換 ESU → EUN と同じです。
            return { vertex.x, -vertex.y, vertex.z };
        case CoordinateSystem::EUN:
            // EUN → ENU の式は 逆変換 ENU → EUN と同じです。
            return { vertex.x, vertex.z, vertex.y };
        default:
            throw std::out_of_range("Invalid argument");
        }
    }

    void GeoReference::setReferencePoint(TVec3d point) {
        reference_point_ = point;
    }

    TVec3d GeoReference::getReferencePoint() const {
        return reference_point_;
    }

    int GeoReference::getZoneID() const {
        return zone_id_;
    }

    void GeoReference::setZoneID(int value) {
        zone_id_ = value;
    }

    float GeoReference::getUnitScale() const {
        return unit_scale_;
    }

    CoordinateSystem GeoReference::getCoordinateSystem() const {
        return coordinate_system_;
    }

    GeoCoordinate GeoReference::unproject(const TVec3d& point) const {
        TVec3d before_convert_lat_lon = (point + reference_point_) * unit_scale_;
        TVec3d lat_lon = convertAxisToENU(coordinate_system_, before_convert_lat_lon);
        PolarToPlaneCartesian().unproject(lat_lon, zone_id_);
        return GeoCoordinate(lat_lon.x, lat_lon.y, lat_lon.z);
    }
}