geometry.h

Go to the documentation of this file.

// Copyright (c) 2024 The YAC Authors
//
// SPDX-License-Identifier: BSD-3-Clause
 
#ifndef GEOMETRY_H
#define GEOMETRY_H
 
#ifdef HAVE_CONFIG_H
// We include 'config.h' in this header file (which is a bad practice)
// because we need the definition of the 'restrict' keyword for the
// inlined functions.
#include "config.h"
#endif
 
#include <math.h>
#include <float.h>
 
#include "yac_types.h"
#include "grids/grid_cell.h"
#include "utils_core.h"
 
#define YAC_RAD (0.01745329251994329576923690768489) // M_PI / 180
 
// angle tolerance
 
#define yac_angle_tol         (1e-9)
#define yac_sq_angle_tol      (1e-9 * 1e-9)
#define yac_cos_angle_tol     (0.9999999999999999995)
#define yac_angle_low_tol     (1e-11)
#define yac_cos_angle_low_tol (1.0)
 
struct sin_cos_angle {
  double sin, cos;
};
 
static const struct sin_cos_angle SIN_COS_ZERO = {0.0, 1.0};
static const struct sin_cos_angle SIN_COS_TOL = {yac_angle_tol, yac_cos_angle_tol};
static const struct sin_cos_angle SIN_COS_LOW_TOL = {yac_angle_low_tol, yac_cos_angle_low_tol};
 
static const struct sin_cos_angle SIN_COS_M_PI_2 = {1.0, 0.0}; /* PI/2 */
static const struct sin_cos_angle SIN_COS_M_PI = {0.0, -1.0}; /* PI */
static const struct sin_cos_angle SIN_COS_7_M_PI_4 = {-0.707106781186547524401, +0.707106781186547524401}; /* (7 * PI)/4 */
 
struct bounding_circle {
 
   double base_vector[3];
   struct sin_cos_angle inc_angle;
   double sq_crd; // sq_crd = (chord)^2
};
 
enum yac_circle_type {
   GREAT_CIRCLE = YAC_GREAT_CIRCLE_EDGE,
   LAT_CIRCLE   = YAC_LAT_CIRCLE_EDGE,
   LON_CIRCLE   = YAC_LON_CIRCLE_EDGE,
   POINT,
};
 
// each circle partitions the sphere into an inside and an outside part
// (the point is the exception; everything except the point is outside)
struct yac_circle {
  enum yac_circle_type type;
  union {
    struct {
      // the norm vector points into the inside direction
      double norm_vector[3];
    } gc, lon;
    struct {
      int north_is_out;
      double z;
      double r;
    } lat;
    struct {
      double vec[3];
    } p;
  } data;
};
 
int yac_point_in_cell (double point_coords[3], struct yac_grid_cell cell);
 
int yac_point_in_cell2 (double point_coords[3], struct yac_grid_cell cell,
                        struct bounding_circle bnd_circle);
 
static inline double get_angle (double a_lon, double b_lon) {
   double diff = a_lon - b_lon;
#if defined(CDO)
   return diff - lround(diff / (2.0 * M_PI)) * (2.0 * M_PI);
#else
   return diff - round(diff / (2.0 * M_PI)) * (2.0 * M_PI);
#endif
}
 
static inline double get_angle_deg (double a_lon, double b_lon) {
   double diff = a_lon - b_lon;
#if defined(CDO)
   return diff - lround(diff / 360.0) * (360.0);
#else
   return diff - round(diff / 360.0) * (360.0);
#endif
}
 
int yac_intersect_vec (
  enum yac_edge_type edge_type_a, double const a[3], double const b[3],
  enum yac_edge_type edge_type_b, double const c[3], double const d[3],
  double p[3], double q[3]);
 
void yac_get_cell_bounding_circle(struct yac_grid_cell cell,
                                  struct bounding_circle * bnd_circle);
 
int yac_extents_overlap(struct bounding_circle * extent_a,
                        struct bounding_circle * extent_b);
 
static inline double sq_len_diff_vec(double const a[3], double const b[3]) {
  double ab[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
  return ab[0] * ab[0] + ab[1] * ab[1] + ab[2] * ab[2];
}
 
static inline double compute_sq_crd(struct sin_cos_angle angle) {
 
  double temp = 1.0 - angle.cos;
  return temp * temp + angle.sin * angle.sin;
}
 
static inline int yac_point_in_bounding_circle_vec(
  double point_vector[3], struct bounding_circle * bnd_circle) {
 
  if (bnd_circle->sq_crd == DBL_MAX)
    bnd_circle->sq_crd = compute_sq_crd(bnd_circle->inc_angle);
 
  return
    bnd_circle->sq_crd >=
      sq_len_diff_vec(bnd_circle->base_vector, point_vector);
}
 
static inline double clamp_abs_one(double val) {
  return val > -1.0 ? (val < 1.0 ? val : 1.0) : -1.0;
}
 
static inline void compute_sin_cos(
  double angle, double * sin_value, double * cos_value) {
 
  double abs_angle = fabs(angle);
  if (abs_angle < yac_angle_low_tol) {
    *sin_value = 0.0;
    *cos_value = 1.0;
  } else  if (fabs(abs_angle - M_PI) < yac_angle_low_tol) {
    *sin_value = 0.0;
    *cos_value = -1.0;
  } else if (fabs(abs_angle - M_PI_2) < yac_angle_low_tol) {
    *sin_value = copysign(1.0, angle);
    *cos_value = 0.0;
  } else {
    *sin_value = clamp_abs_one(sin(angle));
    *cos_value = clamp_abs_one(cos(angle));
  }
}
 
static inline void LLtoXYZ(double lon, double lat, double p_out[]) {
 
  while (lon < -M_PI) lon += 2.0 * M_PI;
  while (lon >= M_PI) lon -= 2.0 * M_PI;
 
  double sin_lon, cos_lon, sin_lat, cos_lat;
  compute_sin_cos(lon, &sin_lon, &cos_lon);
  compute_sin_cos(lat, &sin_lat, &cos_lat);
 
  p_out[0] = cos_lat * cos_lon;
  p_out[1] = cos_lat * sin_lon;
  p_out[2] = sin_lat;
}
 
static inline void LLtoXYZ_deg(double lon, double lat, double p_out[]) {
   LLtoXYZ(lon*YAC_RAD, lat*YAC_RAD, p_out);
}
 
static inline void XYZtoLL (double const p_in[], double * lon, double * lat) {
 
   *lon = atan2(p_in[1] , p_in[0]);
   *lat = M_PI_2 - acos(p_in[2]);
}
 
static inline void product_eft(
  double a, double b, double * restrict product, double * restrict error) {
  *product = a * b;
  *error = fma(a, b, -*product);
}
 
static inline void sum_eft(
  double a, double b, double * restrict sum, double * restrict error) {
  *sum = a + b;
  double temp = *sum - a;
  *error = (a - (*sum - temp)) + (b - temp);
}
 
static inline void accu_eft(
  double a, double * restrict accu, double * restrict error) {
  double a_ = a - *error;
  double temp = *accu + a_;
  *error = (temp - *accu) - a_;
  *accu = temp;
}
 
static inline void accu_product_eft(
  double a, double b, double * restrict accu, double * restrict accu_error) {
 
  // compute product with error
  double product, product_error;
  product_eft(a, b, &product, &product_error);
 
  // compensated accumulation
  accu_eft(product, accu, accu_error);
  *accu_error += product_error;
}
 
static inline void det2_error(
  double a, double b, double c, double d,
  double * restrict det, double * restrict det_error) {
 
  double ad, ad_err;
  product_eft(a, d, &ad, &ad_err);
 
  double bc, bc_err;
  product_eft(b, -c, &bc, &bc_err);
 
  double temp_err;
  sum_eft(ad, bc, det, &temp_err);
  *det_error = ad_err + (temp_err + bc_err);
}
 
// computation of the determinant of a 2x2 matrix using Kahan summation
static inline double det2_kahan(double a, double b, double c, double d) {
  double bc = b*c;
  double e_bc = fma(b,c,-bc); // the rounding error of the multiplication
  double det = fma(a,d,-bc);
  return det + e_bc;
}
 
static inline void crossproduct_kahan (
  double const a[], double const b[], double cross[]) {
 
  /* crossproduct in Cartesian coordinates */
 
  cross[0] = det2_kahan(a[1], a[2], b[1], b[2]);
  cross[1] = det2_kahan(a[2], a[0], b[2], b[0]);
  cross[2] = det2_kahan(a[0], a[1], b[0], b[1]);
 
}
 
static inline void crossproduct_d (
  const double a[], const double b[], double cross[]) {
 
/* crossproduct in Cartesian coordinates */
 
   cross[0] = a[1] * b[2] - a[2] * b[1];
   cross[1] = a[2] * b[0] - a[0] * b[2];
   cross[2] = a[0] * b[1] - a[1] * b[0];
}
 
static inline double dotproduct_eft(double const a[], double const b[]) {
  double accu = 0.0, err = 0.0;
  accu_product_eft(a[0], b[0], &accu, &err);
  accu_product_eft(a[1], b[1], &accu, &err);
  accu_product_eft(a[2], b[2], &accu, &err);
 
  double result, result_err;
  sum_eft(accu, -err, &result, &result_err);
  return result + result_err;
}
 
static inline double get_vector_angle(double const a[3], double const b[3]) {
 
  double sub[3] = {a[0] - b[0], a[1] - b[1], a[2] - b[2]};
  double add[3] = {a[0] + b[0], a[1] + b[1], a[2] + b[2]};
  double sq_sub = sub[0] * sub[0] + sub[1] * sub[1] + sub[2] * sub[2];
  double sq_add = add[0] * add[0] + add[1] * add[1] + add[2] * add[2];
 
  // angle = 2 * atan(|a-b|/|a+b|)
  return (fabs(sq_add) > 1e-12)?(2.0 * atan(sqrt(sq_sub/sq_add))):M_PI;
}
 
static inline struct sin_cos_angle sin_cos_angle_new(double sin, double cos) {
 
  struct sin_cos_angle angle;
 
  angle.sin = clamp_abs_one(sin);
  angle.cos = clamp_abs_one(cos);
 
  return angle;
}
 
static inline struct sin_cos_angle get_vector_angle_2(
  double const a[3], double const b[3]) {
 
  double cross_ab[3];
  crossproduct_kahan(a, b, cross_ab);
 
  return sin_cos_angle_new(sqrt(cross_ab[0]*cross_ab[0] +
                                cross_ab[1]*cross_ab[1] +
                                cross_ab[2]*cross_ab[2]),
                           a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
}
 
// works for angles in the range [0;2*PI[
static inline int compare_angles(
  struct sin_cos_angle a, struct sin_cos_angle b) {
 
  // there are 5 sections:
  // 0: 0      <= angle < PI/4
  // 1: PI/4   <= angle < 3*PI/4
  // 2: 3*PI/4 <= angle < 5*PI/4
  // 3: 5*PI/4 <= angle < 7*PI/4
  // 4: 7*PI/4 <= angle < 2*PI
  int t_a = fabs(a.cos) <= M_SQRT1_2;
  int t_b = fabs(b.cos) <= M_SQRT1_2;
  int a_section = t_a | ((((a.sin < 0.0) & t_a) |
                          ((a.cos < 0.0) & (fabs(a.sin) < M_SQRT1_2))) << 1);
  int b_section = t_b | ((((b.sin < 0.0) & t_b) |
                          ((b.cos < 0.0) & (fabs(b.sin) < M_SQRT1_2))) << 1);
  // if current section is 0, then it could be actually section 4
  if (!a_section) a_section = (a.sin < 0.0) << 2;
  if (!b_section) b_section = (b.sin < 0.0) << 2;
 
  if (a_section != b_section)
    return (a_section > b_section) - (a_section < b_section);
 
  int ret;
 
  switch (a_section) {
    case(0):
    case(4):
    default:
      ret = (b.sin < a.sin + yac_angle_low_tol) -
            (a.sin < b.sin + yac_angle_low_tol);
      if (ret) return ret;
      else {
        ret = (a.cos < b.cos + yac_angle_low_tol) -
              (b.cos < a.cos + yac_angle_low_tol);
        if (a.sin >= 0.0) return ret;
        else return -ret;
      }
    case(1):
      ret = (a.cos < b.cos + yac_angle_low_tol) -
            (b.cos < a.cos + yac_angle_low_tol);
      if (ret) return ret;
      else {
        ret = (b.sin < a.sin + yac_angle_low_tol) -
              (a.sin < b.sin + yac_angle_low_tol);
        if (a.cos >= 0.0) return ret;
        else return -ret;
      }
    case(2):
      ret = (a.sin < b.sin + yac_angle_low_tol) -
            (b.sin < a.sin + yac_angle_low_tol);
      if (ret) return ret;
      else {
        ret = (a.cos < b.cos + yac_angle_low_tol) -
              (b.cos < a.cos + yac_angle_low_tol);
        if (a.sin >= 0.0) return ret;
        else return -ret;
      }
    case(3):
      ret = (b.cos < a.cos + yac_angle_low_tol) -
            (a.cos < b.cos + yac_angle_low_tol);
      if (ret) return ret;
      else {
        ret = (a.sin < b.sin + yac_angle_low_tol) -
              (b.sin < a.sin + yac_angle_low_tol);
        if (a.cos <= 0.0) return ret;
        else return -ret;
      }
  }
}
 
static inline double sin_cos_angle_to_dble(struct sin_cos_angle angle) {
 
  int t = fabs(angle.cos) <= M_SQRT1_2;
  int section = t | ((((angle.sin < 0.0) & t) |
                      ((angle.cos < 0.0) & (fabs(angle.sin) < M_SQRT1_2))) << 1);
  if (!section) section = (angle.sin < 0.0) << 2;
 
  switch (section) {
    default:
    case(0): return 0.0 * M_SQRT1_2 + angle.sin;
    case(1): return 2.0 * M_SQRT1_2 - angle.cos;
    case(2): return 4.0 * M_SQRT1_2 - angle.sin;
    case(3): return 6.0 * M_SQRT1_2 + angle.cos;
    case(4): return 8.0 * M_SQRT1_2 + angle.sin;
  }
}
 
static inline struct sin_cos_angle sum_angles_no_check(
  struct sin_cos_angle a, struct sin_cos_angle b) {
 
  return sin_cos_angle_new(a.sin * b.cos + a.cos * b.sin,
                           a.cos * b.cos - a.sin * b.sin);
}
 
static inline int sum_angles(
  struct sin_cos_angle a, struct sin_cos_angle b,
  struct sin_cos_angle * restrict sum) {
 
  struct sin_cos_angle sum_ = sum_angles_no_check(a, b);
 
  *sum = sum_;
 
  // if a or b is smaller than the result
  return (compare_angles(sum_, a) < 0) || (compare_angles(sum_, b) < 0);
}
 
static inline struct sin_cos_angle sub_angles_no_check(
  struct sin_cos_angle a, struct sin_cos_angle b) {
 
  return sin_cos_angle_new(a.sin * b.cos - a.cos * b.sin,
                           a.cos * b.cos + a.sin * b.sin);
}
 
static inline int sub_angles(
  struct sin_cos_angle a, struct sin_cos_angle b,
  struct sin_cos_angle * restrict sub) {
 
  int compare_result = compare_angles(a, b);
 
  // return sin=0.0 and cos=1.0 if the angles are equal to each other,
  // i.e. compare_result == 0
  *sub = compare_result ? sub_angles_no_check(a, b) : SIN_COS_ZERO;
 
  return compare_result < 0;
}
 
static inline double compute_angle(struct sin_cos_angle angle) {
 
  // the acos and asin are most accurate in the range [-M_SQRT1_2;M_SQRT1_2]
  if (angle.cos > M_SQRT1_2) {
 
    double angle_ = asin(angle.sin);
 
    if (angle_ < 0.0) angle_ += 2.0 * M_PI;
    return angle_;
 
  } else if (angle.cos > - M_SQRT1_2) {
 
    double angle_ = acos(angle.cos);
 
    if (angle.sin > 0.0) return angle_;
    else return 2.0 * M_PI - angle_;
 
  } else {
    return M_PI - asin(angle.sin);
  }
}
 
static inline struct sin_cos_angle half_angle(struct sin_cos_angle angle) {
 
  double x = (1.0 + fabs(angle.cos));
 
  double scale = 1.0 / sqrt(x * x + angle.sin * angle.sin);
 
  // first or fourth quadrant
  if (angle.cos >= 0) {
    scale = copysign(scale, angle.sin);
    return sin_cos_angle_new(angle.sin * scale, x * scale);
 
  // second and third quadrant
  } else return sin_cos_angle_new(x * scale, angle.sin * scale);
}
 
static inline struct sin_cos_angle quarter_angle(struct sin_cos_angle angle) {
 
  double tan = fabs(angle.sin / (1.0 + fabs(angle.cos)));
  double one_plus_sq_tan = 1.0 + tan * tan;
  double sqrt_one_plus_sq_tan = sqrt(one_plus_sq_tan);
 
  double a = 1.0;
  double b = tan;
 
  // second and third quadrant
  if (angle.cos < 0.0) {
    a = tan;
    b = 1.0;
  }
 
  double scale = M_SQRT1_2 / sqrt(one_plus_sq_tan + a * sqrt_one_plus_sq_tan);
  double x = b * scale;
  double y = (a + sqrt_one_plus_sq_tan) * scale;
 
  // first and second quadrant
  if (angle.sin >= 0.0) return sin_cos_angle_new(x, y);
  // third and fourth quadrant
  else return sin_cos_angle_new(y, x);
}
 
static inline int points_are_identically(double const * a, double const * b) {
 
  // for small angles the Euclidean distance is nearly identical to
  // great circle distance between vectors a and b
  return sq_len_diff_vec(a, b) <= yac_sq_angle_tol;
}
 
static inline int compare_coords(double const * a, double const * b) {
 
   // Check if points are identical using squared chord distance
   if (points_are_identically(a, b)) return 0;
 
   int ret;
   if ((ret = (a[0] > b[0]) - (a[0] < b[0]))) return ret;
   if ((ret = (a[1] > b[1]) - (a[1] < b[1]))) return ret;
   return (a[2] > b[2]) - (a[2] < b[2]);
}
 
static inline void normalise_vector(double v[]) {
 
   double norm = 1.0 / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
 
   v[0] *= norm;
   v[1] *= norm;
   v[2] *= norm;
}
 
static inline void rotate_vector2(
  double axis[], struct sin_cos_angle angle, double v_in[], double v_out[]) {
 
  // using Rodrigues' rotation formula
  // v_out = v_in * cos(angle) +
  //         (axis x v_in) * sin(angle) +
  //         axis * (axis * v_in) * (1 - cos(angle))
 
  double cross_axis_v_in[3];
  crossproduct_d(axis, v_in, cross_axis_v_in);
 
  double dot_axis_v_in = axis[0]*v_in[0] + axis[1]*v_in[1] + axis[2]*v_in[2];
  double temp = dot_axis_v_in * (1.0 - angle.cos);
 
  v_out[0] =
    v_in[0] * angle.cos + cross_axis_v_in[0] * angle.sin + axis[0] * temp;
  v_out[1] =
    v_in[1] * angle.cos + cross_axis_v_in[1] * angle.sin + axis[1] * temp;
  v_out[2] =
    v_in[2] * angle.cos + cross_axis_v_in[2] * angle.sin + axis[2] * temp;
}
 
static inline void rotate_vector(
  double axis[], double angle, double v_in[], double v_out[]) {
 
  double sin_angle = sin(angle);
  double cos_angle = cos(angle);
  rotate_vector2(
    axis, sin_cos_angle_new(sin_angle, cos_angle), v_in, v_out);
}
 
void yac_triangulate_cell(
  struct yac_grid_cell cell, size_t start_corner, struct yac_grid_cell * triangles);
 
void yac_triangulate_cell_indices(
  size_t const * corner_indices, size_t num_corners, size_t start_corner,
  size_t (*triangle_indices)[3]);
 
void yac_compute_bnd_triangle(
  double * vertices, size_t num_vertices, double triangle[][3],
  size_t num_tests);
 
int yac_circle_intersect(
  struct yac_circle a, struct yac_circle b, double p[3], double q[3]);
 
int yac_point_on_edge(
  double p[3], double const a[3], double const b[3],
  enum yac_circle_type circle_type);
 
#endif // GEOMETRY_H