source: XIOS/trunk/extern/remap/src/triple.cpp @ 849

Last change on this file since 849 was 849, checked in by ymipsl, 8 years ago

Remapper : manage some floating rounding error leading to some NAN

YM

File size: 3.1 KB
Line 
1#include "triple.hpp"
2
3namespace sphereRemap {
4
5extern const Coord ORIGIN(0.0, 0.0, 0.0);
6
7std::ostream& operator<<(std::ostream& os, const Coord& c) {
8    return os << "(" << c.x << ", " << c.y << ", " << c.z << ")";
9}
10
11std::ostream& operator<<(std::ostream& os, const Vector& vec)
12{
13    return os << "(" << vec.u << ", " << vec.v << ", " << vec.w << ")";
14}
15
16double norm(const Coord &x)
17{
18        return sqrt(x.x*x.x + x.y*x.y + x.z*x.z);
19}
20
21Coord proj(const Coord &x)
22{
23        double n=norm(x);
24        return Coord(x.x/n, x.y/n, x.z/n);
25}
26
27double scalarprod(const Coord &a, const Coord &b)
28{
29        return a.x*b.x + a.y*b.y + a.z*b.z;
30}
31
32Coord 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) */
40double 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 */
50double 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 */
56double 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
67void 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
73Coord 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 */
83Coord 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 */
89Coord 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) */
103void 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
121double angle(const Coord &a, const Coord &b, const Coord &pole)
122{ 
123        return scalarprod(crossprod(a, b), pole);
124}
125
126}
Note: See TracBrowser for help on using the repository browser.