1 | #include "triple.hpp" |
---|
2 | |
---|
3 | namespace sphereRemap { |
---|
4 | |
---|
5 | extern const Coord ORIGIN(0.0, 0.0, 0.0); |
---|
6 | |
---|
7 | std::ostream& operator<<(std::ostream& os, const Coord& c) { |
---|
8 | return os << "(" << c.x << ", " << c.y << ", " << c.z << ")"; |
---|
9 | } |
---|
10 | |
---|
11 | std::ostream& operator<<(std::ostream& os, const Vector& vec) |
---|
12 | { |
---|
13 | return os << "(" << vec.u << ", " << vec.v << ", " << vec.w << ")"; |
---|
14 | } |
---|
15 | |
---|
16 | double norm(const Coord &x) |
---|
17 | { |
---|
18 | return sqrt(x.x*x.x + x.y*x.y + x.z*x.z); |
---|
19 | } |
---|
20 | |
---|
21 | Coord proj(const Coord &x) |
---|
22 | { |
---|
23 | double n=norm(x); |
---|
24 | return Coord(x.x/n, x.y/n, x.z/n); |
---|
25 | } |
---|
26 | |
---|
27 | double scalarprod(const Coord &a, const Coord &b) |
---|
28 | { |
---|
29 | return a.x*b.x + a.y*b.y + a.z*b.z; |
---|
30 | } |
---|
31 | |
---|
32 | Coord crossprod(const Coord &a, const Coord &b) |
---|
33 | { |
---|
34 | return Coord(a.y*b.z - a.z*b.y, |
---|
35 | a.z*b.x - a.x*b.z, |
---|
36 | a.x*b.y - a.y*b.x); |
---|
37 | } |
---|
38 | |
---|
39 | /* arc distance (along great circle) */ |
---|
40 | double arcdist(const Coord &x, const Coord &y) |
---|
41 | { // et angles aigus non orientes |
---|
42 | double n = norm(crossprod(x, y)); |
---|
43 | if (n>1.) n=1. ; |
---|
44 | double a = asin(n); |
---|
45 | if (squaredist(x,y) > 2.0) a = M_PI - a; |
---|
46 | return a; |
---|
47 | } |
---|
48 | |
---|
49 | /* alternate way to compute distance along great circle */ |
---|
50 | double ds(const Coord &x, const Coord &y) |
---|
51 | { |
---|
52 | return 2*asin(0.5*eucldist(x,y)); |
---|
53 | } |
---|
54 | |
---|
55 | /* dp is both small circle distances */ |
---|
56 | double dp(const Coord &x, const Coord &y, const Coord &pole) |
---|
57 | { |
---|
58 | return 2*asin(0.5 * eucldist(x,y) / sin(ds(pole,x))); |
---|
59 | } |
---|
60 | |
---|
61 | //* angle between `p` and `q` */ |
---|
62 | /*double angle(const Vector &p, const Vector &q) |
---|
63 | { |
---|
64 | return acos(scalarprod(p, q)); |
---|
65 | }*/ |
---|
66 | |
---|
67 | void lonlat(const Coord &a, double &lon, double &lat) |
---|
68 | { |
---|
69 | lon = atan2(a.y, a.x) * 180.0/M_PI; |
---|
70 | lat = (M_PI_2 - acos(a.z)) * 180.0/M_PI; |
---|
71 | } |
---|
72 | |
---|
73 | Coord xyz(double lon, double lat) |
---|
74 | { |
---|
75 | double phi = M_PI/180.0*lon; |
---|
76 | double theta = M_PI_2 - M_PI/180.0*lat; |
---|
77 | return Coord(sin(theta)*cos(phi), |
---|
78 | sin(theta)*sin(phi), |
---|
79 | cos(theta)); |
---|
80 | } |
---|
81 | |
---|
82 | /** computes the midpoint on spherical arc between a and b */ |
---|
83 | Coord midpoint(const Coord &a, const Coord &b) |
---|
84 | { |
---|
85 | return proj(a + b); |
---|
86 | } |
---|
87 | |
---|
88 | /** computes the midpoint on *small circle* between a and b */ |
---|
89 | Coord midpointSC(const Coord& p, const Coord& q) |
---|
90 | { |
---|
91 | double phi1 = atan2(p.y, p.x); |
---|
92 | double phi2 = atan2(q.y, q.x); |
---|
93 | if (phi1*phi2 < 0) |
---|
94 | phi1 += (phi1 < phi2) ? 2*M_PI : -2*M_PI; |
---|
95 | double theta = acos(p.z); |
---|
96 | double phi = 0.5*(phi1 + phi2); |
---|
97 | return Coord(sin(theta)*cos(phi), |
---|
98 | sin(theta)*sin(phi), |
---|
99 | cos(theta)); |
---|
100 | } |
---|
101 | |
---|
102 | /* rotates us by angle theta around u (r is rotatiion matrix) */ |
---|
103 | void Coord::rot(const Coord &u, double theta) |
---|
104 | { |
---|
105 | double x = this->x; |
---|
106 | double y = this->y; |
---|
107 | double z = this->z; |
---|
108 | |
---|
109 | double ux2 = u.x*u.x, uy2 = u.y*u.y, uz2 = u.z*u.z; |
---|
110 | double k = 1 - cos(theta); |
---|
111 | |
---|
112 | double r00 = ux2 + (1-ux2)*cos(theta), r01 = u.x*u.y*k - u.z*sin(theta), r02 = u.x*u.z*k + u.y*sin(theta); |
---|
113 | double r10 = u.x*u.y*k + u.z*sin(theta), r11 = uy2 + (1-uy2)*cos(theta), r12 = u.y*u.z*k - u.x*sin(theta); |
---|
114 | double r20 = u.x*u.z*k - u.y*sin(theta), r21 = u.y*u.z*k + u.x*sin(theta), r22 = uz2 + (1-uz2)*cos(theta); |
---|
115 | |
---|
116 | this->x = r00*x + r01*y + r02*z; |
---|
117 | this->y = r10*x + r11*y + r12*z; |
---|
118 | this->z = r20*x + r21*y + r22*z; |
---|
119 | } |
---|
120 | |
---|
121 | double angle(const Coord &a, const Coord &b, const Coord &pole) |
---|
122 | { |
---|
123 | return scalarprod(crossprod(a, b), pole); |
---|
124 | } |
---|
125 | |
---|
126 | // return oriented vector angle in range [-pi..pi], pole must be orthogonal to a and b |
---|
127 | double vectAngle(const Coord &a, const Coord &b, const Coord &pole) |
---|
128 | { |
---|
129 | double nab = 1./(norm(a)*norm(b)) ; |
---|
130 | |
---|
131 | Coord a_cross_b=crossprod(a, b)*nab ; |
---|
132 | double sinVect ; |
---|
133 | if (scalarprod(a_cross_b, pole) >= 0) sinVect=norm(a_cross_b) ; |
---|
134 | else sinVect=-norm(a_cross_b) ; |
---|
135 | double cosVect=scalarprod(a,b)*nab ; |
---|
136 | |
---|
137 | return atan2(sinVect,cosVect) ; |
---|
138 | } |
---|
139 | |
---|
140 | } |
---|