util.cc 2.83 KB
Newer Older
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
/*
 * File:   util.cc
 * Author: a16
 *
 * Created on January 17, 2013, 9:27 AM
 */

#include "radixmath/util.hh"
#include "radixmath/point3d.hh"
#include "radixmath/vector3d.hh"
#include <cstddef>

namespace radix
{

std::vector<Real> getEqualAreaRadii( const std::vector<Real>& radii, const std::vector<unsigned short>& subrings )
{
    std::vector<Real> newRadii;
    Real              prevRadius;

    prevRadius = 0;

    for( size_t index = 0; index < radii.size(); index++ )
    {
        Real _prevRadius;
        int          _subrings;
        Real area;
        Real radius;
        Real subarea;

        radius      = radii[index];
        _prevRadius = prevRadius;
        _subrings   = subrings[index];

        area    = PI * radius * radius - PI * prevRadius * prevRadius;
        subarea = area / _subrings;

        for( int count = 1; count < _subrings; count++ )
        {
            Real _radius;

            _radius     = std::sqrt( (subarea + PI * _prevRadius * _prevRadius) / PI );
            _prevRadius = _radius;

            newRadii.push_back( _radius );
        }

        newRadii.push_back( radius );

        prevRadius = radius;
    }

    return newRadii;
}

Real lineIntersect(const Point3D &p, const Vector3D &v
                   , const Point3D &sp, const Point3D &ep)
{
    Real t = maxReal;
    Point3D p2(p+v); // second point for line intersect
    Real x1=p.x;
    Real y1=p.y;
    Real x2=p2.x;
    Real y2=p2.y;

    Real x3=sp.x;
    Real y3=sp.y;
    Real x4=ep.x;
    Real y4=ep.y;

    Real denominator = (y1-y2)*(x3-x4) - (x1-x2)*(y3-y4) ;

    if( isWithin(0.0,denominator) ) return t;

    Real n1 = (x1 - x2)*(x3*y4 - y3*x4) - (x1*y2 - y1*x2)*(x3 - x4) ; //px numerator
    Real n2 = (y1 - y2)*(x3*y4 - y3*x4) - (x1*y2 - y1*x2)*(y3 - y4) ; //py numerator

    Real px = n1/denominator;
    Real py = n2/denominator;

    Real tx = px - p.x;
    Real ty = py - p.y;
    Real xymag = sqrt(v.x*v.x+v.y*v.y);
    Real zratio = xymag == 0 ? 0 : v.z/xymag;

    t = std::sqrt(tx*tx + ty*ty);
    Real tz = t*zratio;
    t = std::sqrt(tx*tx + ty*ty + tz*tz);
    t *= denominator < 0 ? -1 : 1;
    return t;
}
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

Real haversine(Real lat1, Real lon1, Real lat2, Real lon2)
{
    // Haversine formula
    Real dlat = toRadians(lat2 - lat1);
    Real dlon = toRadians(lon2 - lon1);
    Real haversine_dlat = std::sin(dlat / 2.0);
    haversine_dlat *= haversine_dlat;
    Real haversine_dlon = std::sin(dlon / 2.0);
    haversine_dlon *= haversine_dlon;
    Real delta_sigma = haversine_dlat
            + std::cos(toRadians(lat1))
            * std::cos(toRadians(lat2))
            * haversine_dlon;
    delta_sigma = 2.0 * std::asin(std::sqrt(delta_sigma));
    return delta_sigma;
}

Real greatCircleDistance(Real lat1, Real lon1, Real lat2, Real lon2, Real radius)
{
112
    return radius * haversine(lat1, lon1, lat2, lon2);
113
114
}

115
} // namespace radix