// Hyperbolic Rogue -- basic computations in non-Euclidean geometry
// Copyright (C) 2011-2019 Zeno Rogue, see 'hyper.cpp' for details

/** \file hyperpoint.cpp
 *  \brief basic computations in non-Euclidean geometry
 *
 *  This implements hyperpoint (a point in non-Euclidean space), transmatrix (a transformation matrix),
 *  and various basic routines related to them: rotations, translations, inverses and determinants, etc.
 *  For nonisotropic geometries, it rather refers to nonisotropic.cpp.
 */

#include "hyper.h"
namespace hr {

#if HDR

#ifndef M_PI
#define M_PI 3.14159265358979
#endif

static constexpr ld A_PI = M_PI;
static const ld TAU = 2 * A_PI;
static const ld degree = A_PI / 180;
static const ld golden_phi = (sqrt(5)+1)/2;
static const ld log_golden_phi = log(golden_phi);

constexpr ld operator"" _deg(long double deg) { return deg * A_PI / 180; }
#endif

eGeometry geometry;
eVariation variation;


#if HDR
/** \brief A point in our continuous space
 *
 *  Originally used for representing points in the hyperbolic plane.
 *  Currently used for all kinds of supported spaces, as well as
 *  for all vector spaces (up to 4 dimensions). We are using
 *  the normalized homogeneous coordinates, which allows us to work with most
 *  geometries in HyperRogue in a uniform way.

 *  In the hyperbolic plane, this is the Minkowski hyperboloid model:
 *  (x,y,z) such that x*x+y*y-z*z == -1 and z > 0.
 *
 *  In spherical geometry, we have x*x+y*y+z*z == 1.
 *
 *  In Euclidean geometry, we have z = 1.
 *
 *  In isotropic 3D geometries an extra coordinate is added.
 *
 *  In nonisotropic coordinates h[3] == 1.
 *
 *  In product geometries the 'z' coordinate is modelled by multiplying all 
 *  three coordinates with exp(z).
 *
 */

struct hyperpoint : array<ld, MAXMDIM> {
  hyperpoint() {}
  
  #if MAXMDIM == 4
  constexpr hyperpoint(ld x, ld y, ld z, ld w) : array<ld, MAXMDIM> {{x,y,z,w}} {}
  #else
  constexpr hyperpoint(ld x, ld y, ld z, ld w) : array<ld, MAXMDIM> {{x,y,z}} {}
  #endif

  inline hyperpoint& operator *= (ld d) {
    for(int i=0; i<MXDIM; i++) self[i] *= d;
    return self;
    }
  
  inline hyperpoint& operator /= (ld d) { 
    for(int i=0; i<MXDIM; i++) self[i] /= d;
    return self;
    }
  
  inline hyperpoint& operator += (const hyperpoint h2) { 
    for(int i=0; i<MXDIM; i++) self[i] += h2[i];
    return self;
    }

  inline hyperpoint& operator -= (const hyperpoint h2) { 
    for(int i=0; i<MXDIM; i++) self[i] -= h2[i];
    return self;
    }

  inline friend hyperpoint operator * (ld d, hyperpoint h) { return h *= d; }  
  inline friend hyperpoint operator * (hyperpoint h, ld d) { return h *= d; }  
  inline friend hyperpoint operator / (hyperpoint h, ld d) { return h /= d; }  
  inline friend hyperpoint operator + (hyperpoint h, hyperpoint h2) { return h += h2; }
  inline friend hyperpoint operator - (hyperpoint h, hyperpoint h2) { return h -= h2; }

  inline friend hyperpoint operator - (hyperpoint h) { return h * -1; }

  // cross product  
  inline friend hyperpoint operator ^ (hyperpoint h1, hyperpoint h2) {
    return hyperpoint(
      h1[1] * h2[2] - h1[2] * h2[1],
      h1[2] * h2[0] - h1[0] * h2[2],
      h1[0] * h2[1] - h1[1] * h2[0],
      0
      );
    }

  friend ld dot_d(int c, hyperpoint h1, hyperpoint h2) {
    ld sum = 0;
    for(int i=0; i<c; i++) sum += h1[i] * h2[i];
    return sum;
    }

  // Euclidean inner product
  inline friend ld operator | (hyperpoint h1, hyperpoint h2) {
    return dot_d(MXDIM, h1, h2);
    }    
  };

/** \brief A matrix acting on hr::hyperpoint 
 *
 *  Since we are using homogeneous coordinates for hr::hyperpoint,
 *  rotations and translations can be represented
 *  as matrix multiplications. Other applications of matrices in HyperRogue 
 *  (in dimension up to 4) are also implemented using transmatrix.
 */
struct transmatrix {
  ld tab[MAXMDIM][MAXMDIM];
  hyperpoint& operator [] (int i) { return (hyperpoint&)tab[i][0]; }
  const hyperpoint& operator [] (int i) const { return (const hyperpoint&)tab[i]; }
  
  inline friend hyperpoint operator * (const transmatrix& T, const hyperpoint& H) {
    hyperpoint z;
    for(int i=0; i<MXDIM; i++) {
      z[i] = 0;
      for(int j=0; j<MXDIM; j++) z[i] += T[i][j] * H[j];
      }
    return z;
    }

  inline friend transmatrix operator * (const transmatrix& T, const transmatrix& U) {
    transmatrix R;
    for(int i=0; i<MXDIM; i++) for(int j=0; j<MXDIM; j++) {
      R[i][j] = 0;
      for(int k=0; k<MXDIM; k++)
        R[i][j] += T[i][k] * U[k][j];
      }
    return R;
    }  
  };

/** @brief hyperpoint with shift 
 *  shift has two uses:
 *  (1) in the 'universal cover of SL' geometry, shift is used for the extra angular coordinate
 *  (2) in band models, shift is used to draw faraway points correctly
 */
struct shiftpoint {
  hyperpoint h;
  ld shift;
  ld& operator [] (int i) { return h[i]; }
  const ld& operator [] (int i) const { return h[i]; }
  inline friend shiftpoint operator + (const shiftpoint& h, const hyperpoint& h2) {
    return shiftpoint{h.h+h2, h.shift};
    }
  inline friend shiftpoint operator - (const shiftpoint& h, const hyperpoint& h2) {
    return shiftpoint{h.h-h2, h.shift};
    }
  };

inline shiftpoint shiftless(const hyperpoint& h, ld shift = 0) {
  shiftpoint res; res.h = h; res.shift = shift; return res;
  }

struct shiftmatrix {
  transmatrix T;
  ld shift;
  hyperpoint& operator [] (int i) { return T[i]; }
  const hyperpoint& operator [] (int i) const { return T[i]; }
  inline friend shiftpoint operator * (const shiftmatrix& T, const hyperpoint& h) {
    return shiftpoint{T.T*h, T.shift};
    }
  inline friend shiftmatrix operator * (const shiftmatrix& T, const transmatrix& U) {
    return shiftmatrix{T.T*U, T.shift};
    }
  };

inline shiftmatrix shiftless(const transmatrix& T, ld shift = 0) {
  shiftmatrix res; res.T = T; res.shift = shift; return res;
  }

/** returns a diagonal matrix */
constexpr transmatrix diag(ld a, ld b, ld c, ld d) {
  #if MAXMDIM==3
  return transmatrix{{{a,0,0}, {0,b,0}, {0,0,c}}};
  #else
  return transmatrix{{{a,0,0,0}, {0,b,0,0}, {0,0,c,0}, {0,0,0,d}}};
  #endif
  }

constexpr hyperpoint Hypc = hyperpoint(0, 0, 0, 0);

/** identity matrix */
constexpr transmatrix Id = diag(1,1,1,1);

/** zero matrix */
constexpr transmatrix Zero = diag(0,0,0,0);

/** mirror image */
constexpr transmatrix Mirror = diag(1,-1,1,1);

/** mirror image: flip in the Y coordinate */
constexpr transmatrix MirrorY = diag(1,-1,1,1);

/** mirror image: flip in the X coordinate */
constexpr transmatrix MirrorX = diag(-1,1,1,1);

/** mirror image: flip in the Z coordinate */
constexpr transmatrix MirrorZ = diag(1,1,-1,1);

/** rotate by PI in the XY plane */
constexpr transmatrix pispin = diag(-1,-1,1,1);

/** central symmetry matrix */
constexpr transmatrix centralsym = diag(-1,-1,-1,-1);

inline hyperpoint hpxyz(ld x, ld y, ld z) { return MDIM == 3 ? hyperpoint(x,y,z,0) : hyperpoint(x,y,0,z); }
inline hyperpoint hpxyz3(ld x, ld y, ld z, ld w) { return MDIM == 3 ? hyperpoint(x,y,w,0) : hyperpoint(x,y,z,w); }
constexpr hyperpoint point3(ld x, ld y, ld z) { return hyperpoint(x,y,z,0); }
constexpr hyperpoint point30(ld x, ld y, ld z) { return hyperpoint(x,y,z,0); }
constexpr hyperpoint point31(ld x, ld y, ld z) { return hyperpoint(x,y,z,1); }
constexpr hyperpoint point2(ld x, ld y) { return hyperpoint(x,y,0,0); }

constexpr hyperpoint C02 = hyperpoint(0,0,1,0);
constexpr hyperpoint C03 = hyperpoint(0,0,0,1);

/** C0 is the origin in our space */
#define C0 (MDIM == 3 ? C02 : C03)
#endif

// basic functions and types
//===========================

EX ld squar(ld x) { return x*x; }

EX int sig(int z) { return ginf[geometry].g.sig[z]; }

EX int curvature() {
  switch(cgclass) {
    case gcEuclid: return 0;
    case gcHyperbolic: return -1;
    case gcSphere: return 1;
    case gcProduct: return PIU(curvature());
    default: return 0;
    }
  }

EX ld sin_auto(ld x) {
  switch(cgclass) {
    case gcEuclid: return x;
    case gcHyperbolic: return sinh(x);
    case gcSphere: return sin(x);
    case gcProduct: return PIU(sin_auto(x));
    case gcSL2: return sinh(x);
    default: return x;
    }
  }

EX ld asin_auto(ld x) {
  switch(cgclass) {
    case gcEuclid: return x;
    case gcHyperbolic: return asinh(x);
    case gcSphere: return asin(x);
    case gcProduct: return PIU(asin_auto(x));
    case gcSL2: return asinh(x);
    default: return x;
    }
  }

EX ld acos_auto(ld x) {
  switch(cgclass) {
    case gcHyperbolic: return acosh(x);
    case gcSphere: return acos(x);
    case gcProduct: return PIU(acos_auto(x));
    case gcSL2: return acosh(x);
    default: return x;
    }
  }

/** \brief volume of a three-dimensional ball of radius r in the current isotropic geometry */
EX ld volume_auto(ld r) {
  switch(cgclass) {
    case gcEuclid: return r * r * r * 240._deg;
    case gcHyperbolic: return M_PI * (sinh(2*r) - 2 * r);
    case gcSphere: return M_PI * (2 * r - sin(2*r));
    default: return 0;
    }
  }

/** \brief area of a circle of radius r in the current isotropic geometry */
EX ld area_auto(ld r) {
  switch(cgclass) {
    case gcEuclid: return r * r * M_PI;
    case gcHyperbolic: return TAU * (cosh(r) - 1);
    case gcSphere: return TAU * (1 - cos(r));
    default: return 0;
    }
  }

/** \brief volume in 3D, area in 2D */
EX ld wvolarea_auto(ld r) {
  if(WDIM == 3) return volume_auto(r);
  else return area_auto(r);
  }

EX ld asin_clamp(ld x) { return x>1 ? 90._deg : x<-1 ? -90._deg : std::isnan(x) ? 0 : asin(x); }

EX ld acos_clamp(ld x) { return x>1 ? 0 : x<-1 ? M_PI : std::isnan(x) ? 0 : acos(x); }

EX ld asin_auto_clamp(ld x) {
  switch(cgclass) {
    case gcEuclid: return x;
    case gcHyperbolic: return asinh(x);
    case gcSL2: return asinh(x);
    case gcSphere: return asin_clamp(x);
    case gcProduct: return PIU(asin_auto_clamp(x));
    default: return x;
    }
  }

EX ld acos_auto_clamp(ld x) {
  switch(cgclass) {
    case gcHyperbolic: return x < 1 ? 0 : acosh(x);
    case gcSL2: return x < 1 ? 0 : acosh(x);
    case gcSphere: return acos_clamp(x);
    case gcProduct: return PIU(acos_auto_clamp(x));
    default: return x;
    }
  }

EX ld cos_auto(ld x) {
  switch(cgclass) {
    case gcEuclid: return 1;
    case gcHyperbolic: return cosh(x);
    case gcSL2: return cosh(x);
    case gcSphere: return cos(x);
    case gcProduct: return PIU(cos_auto(x));
    default: return 1;
    }
  }

EX ld tan_auto(ld x) {
  switch(cgclass) {
    case gcEuclid: return x;
    case gcHyperbolic: return tanh(x);
    case gcSphere: return tan(x);
    case gcProduct: return PIU(tan_auto(x));
    case gcSL2: return tanh(x);
    default: return 1;
    }
  }

EX ld atan_auto(ld x) {
  switch(cgclass) {
    case gcEuclid: return x;
    case gcHyperbolic: return atanh(x);
    case gcSphere: return atan(x);
    case gcProduct: return PIU(atan_auto(x));
    case gcSL2: return atanh(x);
    default: return x;
    }
  }

EX ld atan2_auto(ld y, ld x) {
  switch(cgclass) {
    case gcEuclid: return y/x;
    case gcHyperbolic: return atanh(y/x);
    case gcSL2: return atanh(y/x);
    case gcSphere: return atan2(y, x);
    case gcProduct: return PIU(atan2_auto(y, x));    
    default: return y/x;
    }
  }

/** This function returns the length of the edge opposite the angle alpha in 
 *  a triangle with angles alpha, beta, gamma. This is called the cosine rule,
 *  and of course works only in non-Euclidean geometry. */ 
EX ld edge_of_triangle_with_angles(ld alpha, ld beta, ld gamma) {
  return acos_auto((cos(alpha) + cos(beta) * cos(gamma)) / (sin(beta) * sin(gamma)));
  }

EX hyperpoint hpxy(ld x, ld y) { 
  if(sl2) return hyperpoint(x, y, 0, sqrt(1+x*x+y*y));
  if(rotspace) return hyperpoint(x, y, 0, sqrt(1-x*x-y*y));
  return PIU(hpxyz(x,y, translatable ? 1 : sphere ? sqrt(1-x*x-y*y) : sqrt(1+x*x+y*y)));
  }

EX hyperpoint hpxy3(ld x, ld y, ld z) { 
  return hpxyz3(x,y,z, sl2 ? sqrt(1+x*x+y*y-z*z) :translatable ? 1 : sphere ? sqrt(1-x*x-y*y-z*z) : sqrt(1+x*x+y*y+z*z));
  }

#if HDR
// a point (I hope this number needs no comments ;) )
constexpr hyperpoint Cx12 = hyperpoint(1,0,1.41421356237,0);
constexpr hyperpoint Cx13 = hyperpoint(1,0,0,1.41421356237);

#define Cx1 (GDIM==2?Cx12:Cx13)
#endif

EX bool zero_d(int d, hyperpoint h) { 
  for(int i=0; i<d; i++) if(h[i]) return false;
  return true;
  }

/** this function returns approximate square of distance between two points
 *  (in the spherical analogy, this would be the distance in the 3D space,
 *  through the interior, not on the surface)
 *  also used to verify whether a point h1 is on the hyperbolic plane by using Hypc for h2
 */

EX ld intval(const hyperpoint &h1, const hyperpoint &h2) {
  ld res = 0;
  for(int i=0; i<MDIM; i++) res += squar(h1[i] - h2[i]) * sig(i);
  if(elliptic) {
    ld res2 = 0;
    for(int i=0; i<MDIM; i++) res2 += squar(h1[i] + h2[i]) * sig(i);
    return min(res, res2);
    }
  return res;
  }

EX ld quickdist(const hyperpoint &h1, const hyperpoint &h2) {
  if(prod) return hdist(h1, h2);
  return intval(h1, h2);
  }

/** square Euclidean hypotenuse in the first d dimensions */
EX ld sqhypot_d(int d, const hyperpoint& h) {
  ld sum = 0;
  for(int i=0; i<d; i++) sum += h[i]*h[i];
  return sum;
  }
  
/** Euclidean hypotenuse in the first d dimensions */
EX ld hypot_d(int d, const hyperpoint& h) {
  return sqrt(sqhypot_d(d, h));
  }

/** @brief h1 and h2 define a line; to_other_side(h1,h2)*x is x moved orthogonally to this line, by double the distance from C0 
 *  (I suppose it could be done better)
 */
EX transmatrix to_other_side(hyperpoint h1, hyperpoint h2) {

  ld d = hdist(h1, h2);
  
  hyperpoint v;  
  if(euclid)
    v = (h2 - h1) / d;    
  else
    v = (h1 * cos_auto(d) - h2) / sin_auto(d);

  ld d1;
  if(euclid)
    d1 = -(v|h1) / (v|v);
  else
    d1 = atan_auto(-v[LDIM] / h1[LDIM]);
  
  hyperpoint hm = h1 * cos_auto(d1) + (sphere ? -1 : 1) * v * sin_auto(d1);
  
  return rspintox(hm) * xpush(-hdist0(hm) * 2) * spintox(hm);
  }

/** @brief positive for a material vertex, 0 for ideal vertex, negative for ultra-ideal vertex */
EX ld material(const hyperpoint& h) {
  if(sphere || in_s2xe()) return intval(h, Hypc);
  else if(hyperbolic || in_h2xe()) return -intval(h, Hypc);
  else if(sl2) return h[2]*h[2] + h[3]*h[3] - h[0]*h[0] - h[1]*h[1];
  else return h[LDIM];
  }

EX int safe_classify_ideals(hyperpoint h) {
  if(hyperbolic || in_h2xe()) {
    h /= h[LDIM];
    ld x = MDIM == 3 ? 1 - (h[0] * h[0] + h[1] * h[1]) : 1 - (h[0] * h[0] + h[1] * h[1] + h[2] * h[2]);
    if(x > 1e-6) return 1;
    if(x < -1e-6) return -1;
    return 0;
    }
  return 1;
  }

EX ld ideal_limit = 10;
EX ld ideal_each = degree;

EX hyperpoint safe_approximation_of_ideal(hyperpoint h) {
  return towards_inf(C0, h, ideal_limit);
  }

/** the point on the line ab which is closest to zero. Might not be normalized. Works even if a and b are (ultra)ideal */
EX hyperpoint closest_to_zero(hyperpoint a, hyperpoint b) {
  if(sqhypot_d(MDIM, a-b) < 1e-9) return a;
  if(isnan(a[0])) return a;
  a /= a[LDIM];
  b /= b[LDIM];
  ld mul_a = 0, mul_b = 0;
  for(int i=0; i<LDIM; i++) {
    ld z = a[i] - b[i];
    mul_a += a[i] * z;
    mul_b -= b[i] * z;
    }

  return (mul_b * a + mul_a * b) / (mul_a + mul_b);
  }

EX ld zlevel(const hyperpoint &h) {
  if(sl2) return sqrt(-intval(h, Hypc));
  else if(translatable) return h[LDIM];
  else if(sphere) return sqrt(intval(h, Hypc));
  else if(in_e2xe()) return log(h[2]);
  else if(prod) return log(sqrt(abs(intval(h, Hypc)))); /* abs works with both underlying spherical and hyperbolic */
  else return (h[LDIM] < 0 ? -1 : 1) * sqrt(-intval(h, Hypc));
  }

EX ld hypot_auto(ld x, ld y) {
  switch(cgclass) {
    case gcEuclid:
      return hypot(x, y);
    case gcHyperbolic:
      return acosh(cosh(x) * cosh(y));
    case gcSphere:
      return acos(cos(x) * cos(y));
    default:
      return hypot(x, y);
    }
  }

/** normalize the homogeneous coordinates */
EX hyperpoint normalize(hyperpoint H) {
  if(prod) return H;
  ld Z = zlevel(H);
  for(int c=0; c<MXDIM; c++) H[c] /= Z;
  return H;
  }

/** like normalize but makes (ultra)ideal points material */
EX hyperpoint ultra_normalize(hyperpoint H) {
  if(material(H) <= 0) {
    H[LDIM] = hypot_d(LDIM, H) + 1e-10;
    }
  return normalize(H);
  }

/** normalize, and in product geometry, also flatten */
EX hyperpoint normalize_flat(hyperpoint h) {
  if(prod) return product_decompose(h).second;
  if(sl2) h = slr::translate(h) * zpush0(-atan2(h[2], h[3]));
  return normalize(h);
  }

/** get the center of the line segment from H1 to H2 */
EX hyperpoint mid(const hyperpoint& H1, const hyperpoint& H2) {
  if(prod) {
    auto d1 = product_decompose(H1);
    auto d2 = product_decompose(H2);
    return zshift(PIU( mid(d1.second, d2.second) ), (d1.first + d2.first) / 2);
    }
  return normalize(H1 + H2);
  }

EX shiftpoint mid(const shiftpoint& H1, const shiftpoint& H2) {
  return shiftless(mid(H1.h, H2.h), (H1.shift + H2.shift)/2);
  }

/** like mid, but take 3D into account */
EX hyperpoint midz(const hyperpoint& H1, const hyperpoint& H2) {
  if(prod) return mid(H1, H2);
  hyperpoint H3 = H1 + H2;
  
  ld Z = 2;
  
  if(!euclid) Z = zlevel(H3) * 2 / (zlevel(H1) + zlevel(H2));
  for(int c=0; c<MXDIM; c++) H3[c] /= Z;
  
  return H3;
  }

// matrices
//==========

/** rotate by alpha degrees in the coordinates a, b */
EX transmatrix cspin(int a, int b, ld alpha) {
  transmatrix T = Id;
  T[a][a] = +cos(alpha); T[a][b] = +sin(alpha);
  T[b][a] = -sin(alpha); T[b][b] = +cos(alpha);
  return T;
  }

/** rotate by 90 degrees in the coordinates a, b */
EX transmatrix cspin90(int a, int b) {
  transmatrix T = Id;
  T[a][a] = 0; T[a][b] = 1;
  T[b][a] = -1; T[b][b] = 0;
  return T;
  }

/** rotate by 180 degrees in the coordinates a, b */
EX transmatrix cspin180(int a, int b) {
  transmatrix T = Id;
  T[a][a] = T[b][b] = -1;
  return T;
  }

/** rotate by alpha degrees in the XY plane */
EX transmatrix spin(ld alpha) { return cspin(0, 1, alpha); }

/** rotate by 90 degrees in the XY plane */
EX transmatrix spin90() { return cspin90(0, 1); }

/** rotate by 180 degrees in the XY plane */
EX transmatrix spin180() { return cspin180(0, 1); }

/** rotate by 270 degrees in the XY plane */
EX transmatrix spin270() { return cspin90(1, 0); }

EX transmatrix random_spin3() {
  ld alpha2 = asin(randd() * 2 - 1);
  ld alpha = randd() * TAU;
  ld alpha3 = randd() * TAU;
  return cspin(0, 1, alpha) * cspin(0, 2, alpha2) * cspin(1, 2, alpha3);
  }

EX transmatrix random_spin() {
  if(WDIM == 2) return spin(randd() * TAU);
  else return random_spin3();
  }

EX transmatrix eupush(ld x, ld y) {
  transmatrix T = Id;
  T[0][LDIM] = x;
  T[1][LDIM] = y;
  return T;
  }

EX transmatrix euclidean_translate(ld x, ld y, ld z) {
  transmatrix T = Id;
  T[0][LDIM] = x;
  T[1][LDIM] = y;
  T[2][LDIM] = z;
  return T;
  }

EX transmatrix euscale(ld x, ld y) {
  transmatrix T = Id;
  T[0][0] = x;
  T[1][1] = y;
  return T;
  }

EX transmatrix euscale3(ld x, ld y, ld z) {
  transmatrix T = Id;
  T[0][0] = x;
  T[1][1] = y;
  T[2][2] = z;
  return T;
  }

EX transmatrix eupush(hyperpoint h, ld co IS(1)) {
  if(nonisotropic) return nisot::translate(h, co);
  transmatrix T = Id;
  for(int i=0; i<GDIM; i++) T[i][LDIM] = h[i] * co;
  return T;
  }

EX transmatrix eupush3(ld x, ld y, ld z) {
  if(sl2) return slr::translate(slr::xyz_point(x, y, z));
  return eupush(point3(x, y, z));
  }

EX transmatrix euscalezoom(hyperpoint h) {
  transmatrix T = Id;
  T[0][0] = h[0];
  T[0][1] = -h[1];
  T[1][0] = h[1];
  T[1][1] = h[0];
  return T;
  }

EX transmatrix euaffine(hyperpoint h) {
  transmatrix T = Id;
  T[0][1] = h[0];
  T[1][1] = exp(h[1]);
  return T;
  }

EX transmatrix cpush(int cid, ld alpha) {
  transmatrix T = Id;
  if(prod && cid == 2)
    return mscale(Id, alpha);
  if(nonisotropic) 
    return eupush3(cid == 0 ? alpha : 0, cid == 1 ? alpha : 0, cid == 2 ? alpha : 0);
  T[LDIM][LDIM] = T[cid][cid] = cos_auto(alpha);
  T[cid][LDIM] = sin_auto(alpha);
  T[LDIM][cid] = -curvature() * sin_auto(alpha);
  return T;
  }

EX transmatrix cmirror(int cid) {
  transmatrix T = Id;
  T[cid][cid] = -1;
  return T;
  }

// push alpha units to the right
EX transmatrix xpush(ld alpha) { return cpush(0, alpha); }

EX bool eqmatrix(transmatrix A, transmatrix B, ld eps IS(.01)) {
  for(int i=0; i<MXDIM; i++)
  for(int j=0; j<MXDIM; j++)
    if(std::abs(A[i][j] - B[i][j]) > eps)
      return false;
  return true;
  }

#if MAXMDIM >= 4
// in the 3D space, move the point h orthogonally to the (x,y) plane by z units
EX hyperpoint orthogonal_move(const hyperpoint& h, ld z) {
  if(prod) return zshift(h, z);
  if(sl2) return slr::translate(h) * cpush0(2, z);
  if(!hyperbolic) return rgpushxto0(h) * cpush(2, z) * C0;
  if(nil) return nisot::translate(h) * cpush0(2, z);
  if(translatable) return hpxy3(h[0], h[1], h[2] + z);
  ld u = 1;
  if(h[2]) z += asin_auto(h[2]), u /= cos_auto(asin_auto(h[2]));
  u *= cos_auto(z);
  return hpxy3(h[0] * u, h[1] * u, sinh(z));
  }
#endif

// push alpha units vertically
EX transmatrix ypush(ld alpha) { return cpush(1, alpha); }

EX transmatrix zpush(ld z) { return cpush(2, z); }

EX transmatrix matrix3(ld a, ld b, ld c, ld d, ld e, ld f, ld g, ld h, ld i) {
  #if MAXMDIM==3
  return transmatrix {{{a,b,c},{d,e,f},{g,h,i}}};
  #else
  if(GDIM == 2)
    return transmatrix {{{a,b,c,0},{d,e,f,0},{g,h,i,0},{0,0,0,1}}};
  else
    return transmatrix {{{a,b,0,c},{d,e,0,f},{0,0,1,0},{g,h,0,i}}};
  #endif
  }

EX transmatrix matrix4(ld a, ld b, ld c, ld d, ld e, ld f, ld g, ld h, ld i, ld j, ld k, ld l, ld m, ld n, ld o, ld p) {
  #if MAXMDIM==3
  return transmatrix {{{a,b,d},{e,f,h},{m,n,p}}};
  #else
  return transmatrix {{{a,b,c,d},{e,f,g,h},{i,j,k,l},{m,n,o,p}}};
  #endif
  }

#if MAXMDIM >= 4
EX void swapmatrix(transmatrix& T) {
  for(int i=0; i<4; i++) swap(T[i][2], T[i][3]);
  for(int i=0; i<4; i++) swap(T[2][i], T[3][i]);
  if(GDIM == 3) {
    for(int i=0; i<4; i++) T[i][2] = T[2][i] = 0;
    T[2][2] = 1;
    }
  fixmatrix(T);
  for(int i=0; i<4; i++) for(int j=0; j<4; j++) if(isnan(T[i][j])) T = Id;
  }

EX void swapmatrix(hyperpoint& h) {
  swap(h[2], h[3]);
  }
#endif

EX transmatrix parabolic1(ld u) {
  if(euclid)
    return ypush(u);
  else {
    ld diag = u*u/2;
    return matrix3(
      -diag+1, u, diag,
      -u, 1, u,
      -diag, u, diag+1
      );
    }
  }

EX transmatrix parabolic13(ld u, ld v) {
  if(euclid)
    return eupush3(0, u, v);
  else {
    ld diag = (u*u+v*v)/2;
    return matrix4(
      -diag+1, u, v, diag,
      -u, 1, 0, u,
      -v, 0, 1, v,
      -diag, u, v, diag+1
      );
    }
  }

EX hyperpoint deparabolic13(hyperpoint h) {
  if(euclid) return h;
  h /= (1 + h[LDIM]);
  h[0] -= 1;
  h /= sqhypot_d(LDIM, h);
  h[0] += .5;
  return point3(log(2) + log(-h[0]), h[1] * 2, LDIM==3 ? h[2] * 2 : 0);
  }

EX hyperpoint parabolic13(hyperpoint h) {
  if(euclid) return h;
  else if(LDIM == 3)
    return parabolic13(h[1], h[2]) * xpush0(h[0]);
  else
    return parabolic1(h[1]) * xpush0(h[0]);
  }

EX transmatrix spintoc(const hyperpoint& H, int t, int f) {
  transmatrix T = Id;
  ld R = hypot(H[f], H[t]);
  if(R >= 1e-15) {
    T[t][t] = +H[t]/R; T[t][f] = +H[f]/R;
    T[f][t] = -H[f]/R; T[f][f] = +H[t]/R;
    }
  return T;
  }

/** an Euclidean rotation in the axes (t,f) which rotates
 *  the point H to the positive 't' axis 
 */

EX transmatrix rspintoc(const hyperpoint& H, int t, int f) {
  transmatrix T = Id;
  ld R = hypot(H[f], H[t]);
  if(R >= 1e-15) {
    T[t][t] = +H[t]/R; T[t][f] = -H[f]/R;
    T[f][t] = +H[f]/R; T[f][f] = +H[t]/R;
    }
  return T;
  }

/** an isometry which takes the point H to the positive X axis 
 *  \see rspintox 
 */
EX transmatrix spintox(const hyperpoint& H) {
  if(GDIM == 2 || prod) return spintoc(H, 0, 1); 
  transmatrix T1 = spintoc(H, 0, 1);
  return spintoc(T1*H, 0, 2) * T1;
  }

/** inverse of hr::spintox 
 */
EX transmatrix rspintox(const hyperpoint& H) {
  if(GDIM == 2 || prod) return rspintoc(H, 0, 1); 
  transmatrix T1 = spintoc(H, 0, 1);
  return rspintoc(H, 0, 1) * rspintoc(T1*H, 0, 2);
  }

/** for H on the X axis, this matrix pushes H to C0 
 *  \see gpushxto0
 */

EX transmatrix pushxto0(const hyperpoint& H) {
  transmatrix T = Id;
  T[0][0] = +H[LDIM]; T[0][LDIM] = -H[0];
  T[LDIM][0] = curvature() * H[0]; T[LDIM][LDIM] = +H[LDIM];
  return T;
  }

/** set the i-th column of T to H */
EX void set_column(transmatrix& T, int i, const hyperpoint& H) {
  for(int j=0; j<MXDIM; j++)
    T[j][i] = H[j];
  }

EX hyperpoint get_column(transmatrix& T, int i) {
  hyperpoint h;
  for(int j=0; j<MXDIM; j++)
    h[j] = T[j][i];
  return h;
  }

/** build a matrix using the given vectors as columns */
EX transmatrix build_matrix(hyperpoint h1, hyperpoint h2, hyperpoint h3, hyperpoint h4) {
  transmatrix T;
  for(int i=0; i<MXDIM; i++) {
    T[i][0] = h1[i],
    T[i][1] = h2[i],
    T[i][2] = h3[i];
    if(MAXMDIM == 4) T[i][3] = h4[i];
    }
  return T;
  }

/** for H on the X axis, this matrix pushes C0 to H
 *  \see rgpushxto0
 */

EX transmatrix rpushxto0(const hyperpoint& H) {
  transmatrix T = Id;
  T[0][0] = +H[LDIM]; T[0][LDIM] = H[0];
  T[LDIM][0] = -curvature() * H[0]; T[LDIM][LDIM] = +H[LDIM];
  return T;
  }

EX transmatrix ggpushxto0(const hyperpoint& H, ld co) {
  if(translatable) 
    return eupush(H, co);
  if(prod) {
    auto d = product_decompose(H);
    return mscale(PIU(ggpushxto0(d.second, co)), d.first * co);
    }
  transmatrix res = Id;
  if(sqhypot_d(GDIM, H) < 1e-16) return res;
  ld fac = -curvature()/(H[LDIM]+1);
  for(int i=0; i<GDIM; i++)
  for(int j=0; j<GDIM; j++)
    res[i][j] += H[i] * H[j] * fac;
    
  for(int d=0; d<GDIM; d++)
    res[d][LDIM] = co * H[d],
    res[LDIM][d] = -curvature() * co * H[d];
  res[LDIM][LDIM] = H[LDIM];
  
  return res;
  }

/** a translation matrix which takes H to 0 */
EX transmatrix gpushxto0(const hyperpoint& H) {
  return ggpushxto0(H, -1);
  }

/** a translation matrix which takes 0 to H */
EX transmatrix rgpushxto0(const hyperpoint& H) {
  return ggpushxto0(H, 1);
  }

EX shiftmatrix rgpushxto0(const shiftpoint& H) {
  return shiftless(rgpushxto0(H.h), H.shift);
  }

/** \brief Fix the numerical inaccuracies in the isometry T
 *
 *  The nature of hyperbolic geometry makes the computations numerically unstable.
 *  The numerical errors tend to accumulate, eventually destroying the projection.
 *  This function fixes this problem by replacing T with a 'correct' isometry.
 */

EX void fixmatrix(transmatrix& T) {
  if(nonisotropic) ; // T may be inverse... do not do that
  else if(cgflags & qAFFINE) ; // affine
  else if(prod) {
    auto z = zlevel(tC0(T));
    T = mscale(T, -z);
    PIU(fixmatrix(T));
    T = mscale(T, +z);
    }
  else if(euclid)
    fixmatrix_euclid(T);
  else
    orthonormalize(T);
  }

EX void fixmatrix_euclid(transmatrix& T) {
  for(int x=0; x<GDIM; x++) for(int y=0; y<=x; y++) {
    ld dp = 0;
    for(int z=0; z<GDIM; z++) dp += T[z][x] * T[z][y];
    
    if(y == x) dp = 1 - sqrt(1/dp);
    
    for(int z=0; z<GDIM; z++) T[z][x] -= dp * T[z][y];
    }
  for(int x=0; x<GDIM; x++) T[LDIM][x] = 0;
  T[LDIM][LDIM] = 1;
  }

EX void orthonormalize(transmatrix& T) {
  for(int x=0; x<MDIM; x++) for(int y=0; y<=x; y++) {
    ld dp = 0;
    for(int z=0; z<MXDIM; z++) dp += T[z][x] * T[z][y] * sig(z);
    
    if(y == x) dp = 1 - sqrt(sig(x)/dp);
    
    for(int z=0; z<MXDIM; z++) T[z][x] -= dp * T[z][y];
    }
  }

/** fix a 3D rotation matrix */
EX void fix_rotation(transmatrix& rot) {
  dynamicval<eGeometry> g(geometry, gSphere); 
  fixmatrix(rot); 
  for(int i=0; i<3; i++) rot[i][3] = rot[3][i] = 0;
  rot[3][3] = 1;
  }

/** determinant 2x2 */
EX ld det2(const transmatrix& T) {
  return T[0][0] * T[1][1] - T[0][1] * T[1][0];
  }

/** determinant 3x3 */
EX ld det3(const transmatrix& T) {
  ld det = 0;
  for(int i=0; i<3; i++) 
    det += T[0][i] * T[1][(i+1)%3] * T[2][(i+2)%3];
  for(int i=0; i<3; i++) 
    det -= T[0][i] * T[1][(i+2)%3] * T[2][(i+1)%3];
  return det;
  }

/** determinant */
EX ld det(const transmatrix& T) {
  if(MDIM == 3) 
    return det3(T);
  else {
    ld det = 1;
    transmatrix M = T;
    for(int a=0; a<MDIM; a++) {
      int max_at = a;
      for(int b=a; b<MDIM; b++)
        if(abs(M[b][a]) > abs(M[max_at][a]))
          max_at = b;
      
      if(max_at != a)
        for(int c=a; c<MDIM; c++) tie(M[max_at][c], M[a][c]) = make_pair(-M[a][c], M[max_at][c]);

      if(!M[a][a]) return 0;
      for(int b=a+1; b<MDIM; b++) {
        ld co = -M[b][a] / M[a][a];
        for(int c=a; c<MDIM; c++) M[b][c] += M[a][c] * co;
        }
      det *= M[a][a];
      }
    return det;
    }
  }

/** warning about incorrect inverse */
void inverse_error(const transmatrix& T) {
  println(hlog, "Warning: inverting a singular matrix: ", T);
  }

/** inverse of a 3x3 matrix */
EX transmatrix inverse3(const transmatrix& T) {
  ld d = det(T);
  transmatrix T2;
  if(d == 0) {
    inverse_error(T); 
    return Id;
    }
  
  for(int i=0; i<3; i++) 
  for(int j=0; j<3; j++) 
    T2[j][i] = (T[(i+1)%3][(j+1)%3] * T[(i+2)%3][(j+2)%3] - T[(i+1)%3][(j+2)%3] * T[(i+2)%3][(j+1)%3]) / d;
  return T2;
  }

/** \brief inverse of a general matrix */
EX transmatrix inverse(const transmatrix& T) {
  if(MDIM == 3) 
    return inverse3(T);
  else {
    transmatrix T1 = T;
    transmatrix T2 = Id;
  
    for(int a=0; a<MDIM; a++) {
      int best = a;
      
      for(int b=a+1; b<MDIM; b++)
        if(abs(T1[b][a]) > abs(T1[best][a]))
          best = b;

      int b = best;

      if(b != a)
        for(int c=0; c<MDIM; c++) 
          swap(T1[b][c], T1[a][c]), swap(T2[b][c], T2[a][c]);

      if(!T1[a][a]) { inverse_error(T); return Id; }
      for(int b=a+1; b<=GDIM; b++) {
        ld co = -T1[b][a] / T1[a][a];
        for(int c=0; c<MDIM; c++) T1[b][c] += T1[a][c] * co, T2[b][c] += T2[a][c] * co;
        }
      }
    
    for(int a=MDIM-1; a>=0; a--) {
      for(int b=0; b<a; b++) {
        ld co = -T1[b][a] / T1[a][a];
        for(int c=0; c<MDIM; c++) T1[b][c] += T1[a][c] * co, T2[b][c] += T2[a][c] * co;
        }
      ld co = 1 / T1[a][a];
      for(int c=0; c<MDIM; c++) T1[a][c] *= co, T2[a][c] *= co;
      }
    return T2;
    }
  }

/** \brief inverse of an orthogonal matrix, i.e., transposition */
EX transmatrix ortho_inverse(transmatrix T) {
  for(int i=1; i<MDIM; i++)
    for(int j=0; j<i; j++)
      swap(T[i][j], T[j][i]);
  return T;
  }

/** \brief inverse of an orthogonal matrix in Minkowski space */
EX transmatrix pseudo_ortho_inverse(transmatrix T) {
  for(int i=1; i<MXDIM; i++)
    for(int j=0; j<i; j++)
      swap(T[i][j], T[j][i]);
  for(int i=0; i<MDIM-1; i++)
    T[i][MDIM-1] = -T[i][MDIM-1],
    T[MDIM-1][i] = -T[MDIM-1][i];
  return T;
  }

/** \brief inverse of an isometry -- in most geometries this can be done more efficiently than using inverse */
EX transmatrix iso_inverse(const transmatrix& T) {
  if(hyperbolic)
    return pseudo_ortho_inverse(T);
  if(sphere) 
    return ortho_inverse(T);
  if(nil) {
    transmatrix U = Id;    
    U[2][LDIM] = T[0][LDIM] * T[1][LDIM] - T[2][LDIM];
    U[1][LDIM] = -T[1][LDIM];
    U[2][1] = U[0][LDIM] = -T[0][LDIM];
    return U;
    }
  if(euclid && !(cgflags & qAFFINE)) {
    transmatrix U = Id;
    for(int i=0; i<MDIM-1; i++)
      for(int j=0; j<MDIM-1; j++)
        U[i][j] = T[j][i];
    hyperpoint h = U * tC0(T);
    for(int i=0; i<MDIM-1; i++)
      U[i][MDIM-1] = -h[i];
    return U;
    }
  return inverse(T);
  }

/** \brief T inverse a matrix T = O*S, where O is isometry and S is a scaling matrix (todo optimize) */
EX transmatrix z_inverse(const transmatrix& T) {
  return inverse(T);
  }

/** \brief T inverse a matrix T = O*P, where O is orthogonal and P is an isometry (todo optimize) */
EX transmatrix view_inverse(transmatrix T) {
  if(nonisotropic) return inverse(T);
  if(prod) return z_inverse(T);
  return iso_inverse(T);
  }

/** \brief T inverse a matrix T = P*O, where O is orthogonal and P is an isometry (todo optimize) */
EX transmatrix iview_inverse(transmatrix T) {
  if(nonisotropic) return inverse(T);
  if(prod) return z_inverse(T);
  return iso_inverse(T);
  }

EX pair<ld, hyperpoint> product_decompose(hyperpoint h) {
  ld z = zlevel(h);
  return make_pair(z, mscale(h, -z));
  }

/** distance from mh and 0 */
EX ld hdist0(const hyperpoint& mh) {
  switch(cgclass) {
    case gcHyperbolic:
      if(mh[LDIM] < 1) return 0;
      return acosh(mh[LDIM]);
    case gcEuclid: {
      return hypot_d(GDIM, mh);
      }
    case gcSphere: {
      ld res = mh[LDIM] >= 1 ? 0 : mh[LDIM] <= -1 ? M_PI : acos(mh[LDIM]);
      return res;
      }
    case gcProduct: {
      auto d1 = product_decompose(mh);
      return hypot(PIU(hdist0(d1.second)), d1.first);
      }
    #if MAXMDIM >= 4
    case gcSL2: {
      auto cosh_r = hypot(mh[2], mh[3]);
      auto phi = atan2(mh[2], mh[3]);
      return hypot(cosh_r < 1 ? 0 : acosh(cosh_r), phi);
      }
    case gcNil: {
      ld bz = mh[0] * mh[1] / 2;
      return hypot(mh[0], mh[1]) + abs(mh[2] - bz);
      }
    #endif
    default:
      return hypot_d(GDIM, mh);
    }
  }

EX ld hdist0(const shiftpoint& mh) {
  return hdist0(unshift(mh));
  }

/** length of a circle of radius r */
EX ld circlelength(ld r) {
  switch(cgclass) {
    case gcEuclid:
      return TAU * r;
    case gcHyperbolic:
      return TAU * sinh(r);
    case gcSphere:
      return TAU * sin(r);
    default:
      return TAU * r;
    }
  }

/* distance between h1 and h2 */
EX ld hdist(const hyperpoint& h1, const hyperpoint& h2) {
  ld iv = intval(h1, h2);
  switch(cgclass) {
    case gcEuclid:
      if(iv < 0) return 0;
      return sqrt(iv);
    case gcHyperbolic:
      if(iv < 0) return 0;
      return 2 * asinh(sqrt(iv) / 2);
    case gcSphere:
      return 2 * asin_auto_clamp(sqrt(iv) / 2);
    case gcProduct: {
      auto d1 = product_decompose(h1);
      auto d2 = product_decompose(h2);
      return hypot(PIU(hdist(d1.second, d2.second)), d1.first - d2.first);
      }
    case gcSL2: 
      return hdist0(stretch::itranslate(h1) * h2);
    default:
      if(iv < 0) return 0;
      return sqrt(iv);
    }
  }

EX ld hdist(const shiftpoint& h1, const shiftpoint& h2) {
  return hdist(h1.h, unshift(h2, h1.shift));
  }

EX hyperpoint mscale(const hyperpoint& t, double fac) {
  if(GDIM == 3 && !prod) return cpush(2, fac) * t;
  if(prod) fac = exp(fac);
  hyperpoint res;
  for(int i=0; i<MXDIM; i++) 
    res[i] = t[i] * fac;
  return res;
  }

EX shiftpoint mscale(const shiftpoint& t, double fac) {
  return shiftless(mscale(t.h, fac), t.shift);
  }

EX transmatrix mscale(const transmatrix& t, double fac) {
  if(GDIM == 3 && !prod) {
    // if(pmodel == mdFlatten) { transmatrix u = t; u[2][LDIM] -= fac; return u; }
    return t * cpush(2, fac);
    }
  if(prod) fac = exp(fac);
  transmatrix res;
  for(int i=0; i<MXDIM; i++) {
    for(int j=0; j<MDIM; j++)
      res[i][j] = t[i][j] * fac;
    for(int j=MDIM; j<MXDIM; j++)
      res[i][j] = t[i][j];
    }
  return res;
  }

EX shiftmatrix mscale(const shiftmatrix& t, double fac) {
  return shiftless(mscale(t.T, fac), t.shift);
  }

EX transmatrix xyscale(const transmatrix& t, double fac) {
  transmatrix res;
  for(int i=0; i<MXDIM; i++) {
    for(int j=0; j<GDIM; j++)
      res[i][j] = t[i][j] * fac;
    for(int j=GDIM; j<MXDIM; j++)
      res[i][j] = t[i][j];
    }
  return res;
  }

EX transmatrix xyzscale(const transmatrix& t, double fac, double facz) {
  transmatrix res;
  for(int i=0; i<MXDIM; i++) {
    for(int j=0; j<GDIM; j++)
      res[i][j] = t[i][j] * fac;
    res[i][LDIM] = 
      t[i][LDIM] * facz;
    for(int j=LDIM+1; j<MXDIM; j++)
      res[i][j] = t[i][j];
    }
  return res;
  }

EX shiftmatrix xyzscale(const shiftmatrix& t, double fac, double facz) {
  return shiftless(xyzscale(t.T, fac, facz), t.shift);
  }

EX transmatrix mzscale(const transmatrix& t, double fac) {
  if(GDIM == 3) return t * cpush(2, fac);
  // take only the spin
  transmatrix tcentered = gpushxto0(tC0(t)) * t;
  // tcentered = tcentered * spin(downspin_zivory);
  fac -= 1;
  transmatrix res = t * inverse(tcentered) * ypush(-fac) * tcentered;
  fac *= .2;
  fac += 1;
  for(int i=0; i<MXDIM; i++) for(int j=0; j<MXDIM; j++)
    res[i][j] = res[i][j] * fac;
  return res;
  }

EX shiftmatrix mzscale(const shiftmatrix& t, double fac) {
  return shiftless(mzscale(t.T, fac), t.shift);
  }

EX hyperpoint mid3(hyperpoint h1, hyperpoint h2, hyperpoint h3) {
  return mid(h1+h2+h3, h1+h2+h3);
  }

EX hyperpoint mid_at(hyperpoint h1, hyperpoint h2, ld v) {
  hyperpoint h = h1 * (1-v) + h2 * v;
  return mid(h, h);
  }  

EX hyperpoint mid_at_actual(hyperpoint h, ld v) {
  return rspintox(h) * xpush0(hdist0(h) * v);
  }

/** in 3D, an orthogonal projection of C0 on the given triangle */
EX hyperpoint orthogonal_of_C0(hyperpoint h0, hyperpoint h1, hyperpoint h2) {
  h0 /= h0[3];
  h1 /= h1[3];
  h2 /= h2[3];
  hyperpoint w = h0;
  hyperpoint d1 = h1 - h0;
  hyperpoint d2 = h2 - h0;
  ld denom = (d1|d1) * (d2|d2) - (d1|d2) * (d1|d2);
  ld a1 = (d2|w) * (d1|d2) - (d1|w) * (d2|d2);
  ld a2 = (d1|w) * (d1|d2) - (d2|w) * (d1|d1);
  hyperpoint h = w * denom + d1 * a1 + d2 * a2;
  return normalize(h);
  }

EX hyperpoint zshift(hyperpoint x, ld z) {
  if(GDIM == 3 && WDIM == 2) return rgpushxto0(x) * cpush0(2, z);
  else if(sl2) return mscale(x, z);
  else if(prod) return mscale(x, z);
  else return mscale(x, z);
  }

EX hyperpoint hpxd(ld d, ld x, ld y, ld z) {
  hyperpoint H = hpxyz(d*x, d*y, z);
  H = mid(H, H);
  return H;
  }

EX ld signum(ld x) { return x<0?-1:x>0?1:0; }

EX bool asign(ld y1, ld y2) { return signum(y1) != signum(y2); }

EX ld xcross(ld x1, ld y1, ld x2, ld y2) { return x1 + (x2 - x1) * y1 / (y1 - y2); }

EX transmatrix parallel_transport(const transmatrix Position, const transmatrix& ori, const hyperpoint direction) {
  if(nonisotropic) return nisot::parallel_transport(Position, direction);
  else if(prod) {
    hyperpoint h = product::direct_exp(ori * direction);
    return Position * rgpushxto0(h);
    }
  else return Position * rgpushxto0(direct_exp(direction));
  }

EX void apply_parallel_transport(transmatrix& Position, const transmatrix orientation, const hyperpoint direction) {
  Position = parallel_transport(Position, orientation, direction);
  }

EX void rotate_object(transmatrix& Position, transmatrix& orientation, transmatrix R) {
  if(prod) orientation = orientation * R;
  else Position = Position * R;
  }

EX transmatrix spin_towards(const transmatrix Position, transmatrix& ori, const hyperpoint goal, int dir, int back) {
  transmatrix T;
  ld alpha = 0;
  if(nonisotropic && nisot::geodesic_movement)
    T = nisot::spin_towards(Position, goal);
  else { 
    hyperpoint U = inverse(Position) * goal;
    if(prod) {
      hyperpoint h = product::inverse_exp(U);
      alpha = asin_clamp(h[2] / hypot_d(3, h));
      U = product_decompose(U).second;
      }
    T = rspintox(U);
    }
  if(back < 0) T = T * spin180(), alpha = -alpha;
  if(prod) {
    if(dir == 0) ori = cspin(2, 0, alpha);
    if(dir == 2) ori = cspin(2, 0, alpha - 90._deg), dir = 0;
    }
  if(dir) T = T * cspin(dir, 0, -90._deg);
  T = Position * T;
  return T;
  }

EX shiftmatrix spin_towards(const shiftmatrix Position, transmatrix& ori, const shiftpoint goal, int dir, int back) {
  return shiftless(spin_towards(Position.T, ori, unshift(goal, Position.shift), dir, back), Position.shift);
  }

EX ld ortho_error(transmatrix T) {

  ld err = 0;
  
  for(int x=0; x<3; x++) for(int y=0; y<3; y++) {
      ld s = 0;
      for(int z=0; z<3; z++) s += T[z][x] * T[z][y];
      
      s -= (x==y);
      err += s*s;
      }

  return err;      
  }

EX transmatrix transpose(transmatrix T) {
  transmatrix result;
  for(int i=0; i<MXDIM; i++)
    for(int j=0; j<MXDIM; j++)
      result[j][i] = T[i][j];
  return result;
  }

#if HDR
namespace slr { 
  hyperpoint xyz_point(ld x, ld y, ld z); 
  hyperpoint polar(ld r, ld theta, ld phi); 
  }

inline hyperpoint cpush0(int c, ld x) { 
  hyperpoint h = Hypc;
  if(sl2) return slr::xyz_point(c==0?x:0, c==1?x:0, c==2?x:0);
  if(c == 2 && prod) {
    h[2] = exp(x);
    return h;
    }
  h[LDIM] = cos_auto(x);
  h[c] = sin_auto(x);
  return h;
  }

inline hyperpoint xspinpush0(ld alpha, ld x) { 
  if(sl2) return slr::polar(x, -alpha, 0);
  hyperpoint h = Hypc;
  h[LDIM] = cos_auto(x);
  h[0] = sin_auto(x) * cos(alpha);
  h[1] = sin_auto(x) * -sin(alpha);
  return h;
  }

inline hyperpoint xpush0(ld x) { return cpush0(0, x); }
inline hyperpoint ypush0(ld x) { return cpush0(1, x); }
inline hyperpoint zpush0(ld x) { return cpush0(2, x); }

/** T * C0, optimized */
inline hyperpoint tC0(const transmatrix &T) {
  hyperpoint z;
  for(int i=0; i<MXDIM; i++) z[i] = T[i][LDIM];
  return z;
  }

inline hyperpoint tC0_t(const transmatrix &T) { return tC0(T); }

inline shiftpoint tC0(const shiftmatrix &T) {
  return shiftpoint{tC0(T.T), T.shift};
  }
#endif

/** tangent vector in the given direction */
EX hyperpoint ctangent(int c, ld x) { return point3(c==0?x:0, c==1?x:0, c==2?x:0); }

/** tangent vector in direction X */
EX hyperpoint xtangent(ld x) { return ctangent(0, x); }

/** tangent vector in direction Y */
EX hyperpoint ztangent(ld z) { return ctangent(2, z); }

/** change the length of the targent vector */
EX hyperpoint tangent_length(hyperpoint dir, ld length) {
  ld r = hypot_d(GDIM, dir);
  if(!r) return dir;
  return dir * (length / r);
  }

/** exponential function: follow the geodesic given by v */
EX hyperpoint direct_exp(hyperpoint v) {
  #if CAP_SOLV
  if(sn::in()) return nisot::numerical_exp(v);
  #endif
  #if MAXMDIM >= 4
  if(nil) return nilv::formula_exp(v);
  if(sl2 || stretch::in()) return stretch::mstretch ? nisot::numerical_exp(v) : rots::formula_exp(v);
  #endif
  if(prod) return product::direct_exp(v);
  ld d = hypot_d(GDIM, v);
  if(d > 0) for(int i=0; i<GDIM; i++) v[i] = v[i] * sin_auto(d) / d;
  v[LDIM] = cos_auto(d);
  return v;
  }

#if HDR
constexpr flagtype pfNO_INTERPOLATION = 1; /**< in tables (sol/nih geometries), do not use interpolations */
constexpr flagtype pfNO_DISTANCE      = 2; /**< we just need the directions -- this makes it a bit faster in sol/nih geometries */
constexpr flagtype pfLOW_BS_ITER      = 4; /**< low iterations in binary search (nil geometry, sl2 not affected currently) */

constexpr flagtype pQUICK     = pfNO_INTERPOLATION | pfLOW_BS_ITER;

constexpr flagtype pNORMAL    = 0;
#endif
  
/** inverse exponential function \see hr::direct_exp */
EX hyperpoint inverse_exp(const shiftpoint h, flagtype prec IS(pNORMAL)) {
  #if CAP_SOLV
  if(sn::in()) {
    /* this will be more precise for use in set_view in intra */
    if(sqhypot_d(3, h.h) < 2e-9) return h.h - C0;
    if(nih) 
      return sn::get_inverse_exp_nsym(h.h, prec);
    else
      return sn::get_inverse_exp_symsol(h.h, prec);
    }
  #endif
  if(nil) return nilv::get_inverse_exp(h.h, prec);
  if(sl2) return slr::get_inverse_exp(h);
  if(prod) return product::inverse_exp(h.h);
  ld d = acos_auto_clamp(h[GDIM]);
  hyperpoint v = Hypc;
  if(d && sin_auto(d)) for(int i=0; i<GDIM; i++) v[i] = h[i] * d / sin_auto(d);
  v[3] = 0;
  return v;
  }

EX ld geo_dist(const hyperpoint h1, const hyperpoint h2, flagtype prec IS(pNORMAL)) {
  if(!nonisotropic) return hdist(h1, h2);
  return hypot_d(3, inverse_exp(shiftless(nisot::translate(h1, -1) * h2, prec)));
  }

EX ld geo_dist(const shiftpoint h1, const shiftpoint h2, flagtype prec IS(pNORMAL)) {
  if(!nonisotropic) return hdist(h1, h2);
  return hypot_d(3, inverse_exp(shiftless(nisot::translate(h1.h, -1) * h2.h, h2.shift - h1.shift), prec));
  }

EX ld geo_dist_q(const hyperpoint h1, const hyperpoint h2, flagtype prec IS(pNORMAL)) {
  auto d = geo_dist(h1, h2, prec);
  if(elliptic && d > 90._deg) return M_PI - d;
  return d;
  }

EX hyperpoint lp_iapply(const hyperpoint h) {
  return nisot::local_perspective_used() ? inverse(NLP) * h : h;
  }

EX hyperpoint lp_apply(const hyperpoint h) {
  return nisot::local_perspective_used() ? NLP * h : h;
  }

EX hyperpoint smalltangent() { return xtangent(.1); }

EX void cyclefix(ld& a, ld b) {
  while(a > b + M_PI) a -= TAU;
  while(a < b - M_PI) a += TAU;
  }

EX ld raddif(ld a, ld b) {
  ld d = a-b;
  if(d < 0) d = -d;
  if(d > TAU) d -= TAU;
  if(d > M_PI) d = TAU-d;
  return d;
  }

EX unsigned bucketer(ld x) {
  return unsigned((long long)(x * 10000 + 100000.5) - 100000);
  }

EX unsigned bucketer(hyperpoint h) {
  unsigned dx = 0;
  if(prod) {
    auto d = product_decompose(h);
    h = d.second;
    dx += bucketer(d.first) * 50;
    }
  dx += bucketer(h[0]) + 1000 * bucketer(h[1]) + 1000000 * bucketer(h[2]);
  if(MDIM == 4) dx += bucketer(h[3]) * 1000000001;
  if(elliptic) dx = min(dx, -dx);
  return dx;
  }  

#if MAXMDIM >= 4
/** @brief project the origin to the triangle [h1,h2,h3] */
EX hyperpoint project_on_triangle(hyperpoint h1, hyperpoint h2, hyperpoint h3) {
  h1 /= h1[3];
  h2 /= h2[3];
  h3 /= h3[3];
  transmatrix T;
  T[0] = h1; T[1] = h2; T[2] = h3;
  T[3] = C0;
  ld det_orig = det3(T);
  hyperpoint orthogonal = (h2 - h1) ^ (h3 - h1);
  T[0] = orthogonal; T[1] = h2-h1; T[2] = h3-h1;
  ld det_orth = det3(T);
  hyperpoint result = orthogonal * (det_orig / det_orth);
  result[3] = 1;
  return normalize(result);
  }
#endif

EX hyperpoint lerp(hyperpoint a0, hyperpoint a1, ld x) {
  return a0 + (a1-a0) * x;
  }

EX hyperpoint linecross(hyperpoint a, hyperpoint b, hyperpoint c, hyperpoint d) {  
  a /= a[LDIM];
  b /= b[LDIM];
  c /= c[LDIM];
  d /= d[LDIM];
  
  ld bax = b[0] - a[0];
  ld dcx = d[0] - c[0];
  ld cax = c[0] - a[0];
  ld bay = b[1] - a[1];
  ld dcy = d[1] - c[1];
  ld cay = c[1] - a[1];
  
  hyperpoint res;
  res[0] = (cay * dcx * bax + a[0] * bay * dcx - c[0] * dcy * bax) / (bay * dcx - dcy * bax);
  res[1] = (cax * dcy * bay + a[1] * bax * dcy - c[1] * dcx * bay) / (bax * dcy - dcx * bay);
  res[2] = 0;
  res[3] = 0;
  res[GDIM] = 1;
  return normalize(res);  
  }

EX ld inner2(hyperpoint h1, hyperpoint h2) {
  return 
    hyperbolic ? h1[LDIM] * h2[LDIM] - h1[0] * h2[0] - h1[1] * h2[1] :
    sphere ? h1[LDIM] * h2[LDIM] + h1[0] * h2[0] + h1[1] * h2[1] :
    h1[0] * h2[0] + h1[1] * h2[1];
  }

EX hyperpoint circumscribe(hyperpoint a, hyperpoint b, hyperpoint c) {
  hyperpoint h = C0;

  b = b - a;
  c = c - a;
  
  if(euclid) {
    ld b2 = inner2(b, b)/2;
    ld c2 = inner2(c, c)/2;
    
    ld det = c[1]*b[0] - b[1]*c[0];
    
    h = a;
    
    h[1] += (c2*b[0] - b2 * c[0]) / det;
    h[0] += (c2*b[1] - b2 * c[1]) / -det;
    
    return h;
    }

  if(inner2(b,b) < 0) {
    b = b / sqrt(-inner2(b, b));
    c = c + b * inner2(c, b);
    h = h + b * inner2(h, b);
    }
  else {
    b = b / sqrt(inner2(b, b));
    c = c - b * inner2(c, b);
    h = h - b * inner2(h, b);
    }
  
  if(inner2(c,c) < 0) {
    c = c / sqrt(-inner2(c, c));
    h = h + c * inner2(h, c);
    }
  else {
    c = c / sqrt(inner2(c, c));
    h = h - c * inner2(h, c);
    }
  
  if(h[LDIM] < 0) h[0] = -h[0], h[1] = -h[1], h[LDIM] = -h[LDIM];

  ld i = inner2(h, h);
  if(i > 0) h /= sqrt(i);
  else h /= -sqrt(-i);

  return h;
  }

EX ld inner3(hyperpoint h1, hyperpoint h2) {
  return 
    hyperbolic ? h1[LDIM] * h2[LDIM] - h1[0] * h2[0] - h1[1] * h2[1]  - h1[2]*h2[2]:
    sphere ? h1[LDIM] * h2[LDIM] + h1[0] * h2[0] + h1[1] * h2[1] + h1[2]*h2[2]:
    h1[0] * h2[0] + h1[1] * h2[1];
  }

/** circumscribe for H3 and S3 (not for E3 yet!) */
EX hyperpoint circumscribe(hyperpoint a, hyperpoint b, hyperpoint c, hyperpoint d) {
  
  array<hyperpoint, 4> ds = { b-a, c-a, d-a, C0 };
  
  for(int i=0; i<3; i++) {
  
    if(inner3(ds[i],ds[i]) < 0) {
      ds[i] = ds[i] / sqrt(-inner3(ds[i], ds[i]));
      for(int j=i+1; j<4; j++)
        ds[j] = ds[j] + ds[i] * inner3(ds[i], ds[j]);
      }
    else {
      ds[i] = ds[i] / sqrt(inner3(ds[i], ds[i]));
      for(int j=i+1; j<4; j++)
        ds[j] = ds[j] - ds[i] * inner3(ds[i], ds[j]);          
      }
    }
  
  hyperpoint& h = ds[3];
  
  if(h[3] < 0) h = -h;

  ld i = inner3(h, h);
  if(i > 0) h /= sqrt(i);
  else h /= -sqrt(-i);

  return h;
  }

/** the point in distance dist from 'material' to 'dir' (usually an (ultra)ideal point) */
EX hyperpoint towards_inf(hyperpoint material, hyperpoint dir, ld dist IS(1)) {
  transmatrix T = gpushxto0(material);
  hyperpoint id = T * dir;
  return rgpushxto0(material) * rspintox(id) * xpush0(dist);
  }

EX bool clockwise(hyperpoint h1, hyperpoint h2) {
  return h1[0] * h2[1] > h1[1] * h2[0];
  }

EX ld worst_precision_error;

#if HDR
struct hr_precision_error : hr_exception { hr_precision_error() : hr_exception("precision error") {} };
#endif

/** check if a and b are the same, testing for equality. Throw an exception or warning if not sure */
EX bool same_point_may_warn(hyperpoint a, hyperpoint b) {
  ld d = hdist(a, b);
  if(d > 1e-2) return false;
  if(d > 1e-3) throw hr_precision_error();
  if(d > 1e-6 && worst_precision_error <= 1e-6)
    addMessage("warning: precision errors are building up!");
  if(d > worst_precision_error) worst_precision_error = d;
  return true;
  }

}