/* See rn.h */
/* Last edited on 2005-01-16 15:06:36 by stolfi */
/* Copyright © 2005 Jorge Stolfi, UNICAMP. See note at end of file. */

/* Based on VectorN.mg, created  95-02-27 by J. Stolfi. */

#include <rn.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <js.h>
#include <gauss_elim.h>

void rn_zero (int n, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = 0.0; }
  }

void rn_all (int n, double x, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = x; }
  }

void rn_axis (int n, int i, double *r)
  { int j;
    affirm((i >= 0) && (i < n), "rn_axis: bad index");
    for (j = 0; j < n; j++) { r[j] = 0.0; }
    r[i] = 1.0;
  }

void rn_add (int n, double *a, double *b, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = a[i] + b[i]; }
  }

void rn_sub (int n, double *a, double *b, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = a[i] - b[i]; }
  }

void rn_neg (int n, double *a, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = - a[i]; }
  }

void rn_scale (int n, double s, double *a, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = s * a[i]; }
  }

void rn_mix (int n, double s, double *a, double t, double *b, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = s * a[i] + t * b[i]; }
  }

void rn_mix_in (int n, double s, double *a, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] += s * a[i]; }
  }

void rn_weigh (int n, double *a, double *w, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = a[i] * w[i]; }
  }

double rn_norm (int n, double *a)
  { /* Don't worry about overflow. */
    /* Client should use {rn_L_inf_dir} first if that is a problem. */
    double sum = 0.0;
    int i;
    for (i = 0; i < n; i++) { double ai = a[i]; sum += ai*ai; }
    return sqrt(sum);
  }

double rn_norm_sqr (int n, double *a)
  { double sum = 0.0;
    int i;
    for (i = 0; i < n; i++) { double ai = a[i]; sum += ai*ai; }
    return sum;
  }

double rn_L_inf_norm (int n, double *a)
  { double mag = 0.0;
    int i;
    for (i = 0; i < n; i++) 
      { double mi = fabs(a[i]); if (mi > mag) { mag = mi; } }
    return mag;
  }

double rn_dist (int n, double *a, double *b)
  { /* Don't worry about overflow. */
    /* Client should use {rn_L_inf_dir} first if that is a problem. */
    double sum = 0.0;
    int i;
    for (i = 0; i < n; i++) { double di = a[i] - b[i]; sum += di*di; }
    return sqrt(sum);
  }

double rn_dist_sqr (int n, double *a, double *b)
  { double sum = 0.0;
    int i;
    for (i = 0; i < n; i++) { double di = (a[i] - b[i]); sum += di*di; }
    return sum;
  }

double rn_L_inf_dist (int n, double *a, double *b)
  { double mag = 0.0;
    int i;
    for (i = 0; i < n; i++) 
      { double mi = fabs(a[i] - b[i]); if (mi > mag) { mag = mi; } }
    return mag;
  }

double rn_dir (int n, double *a, double *r)
  { /* Don't worry about overflow. */
    /* Client should use {rn_L_inf_dir} first if that is a problem. */
    double d = rn_norm(n, a);
    int i;
    for (i = 0; i < n; i++) { r[i] = a[i]/d; }
    return d;
  }

double rn_L_inf_dir (int n, double *a, double *r)
  { double mag = rn_L_inf_norm(n, a);
    int i;
    for (i = 0; i < n; i++) { r[i] = a[i]/mag; }
    return mag;
  }

double rn_dot (int n, double *a, double *b)
  { double sum = 0.0;
    int i;
    for (i = 0; i < n; i++) { sum += a[i]*b[i]; }
    return sum;
  }

double rn_cos (int n, double *a, double *b)
  { double aa = 0.0;
    double bb = 0.0;
    double ab = 0.0;
    int i;
    for (i = 0; i < n; i++)
      { double ai = a[i]; 
        double bi = b[i]; 
        aa += ai*ai; bb += bi*bi; ab += ai*bi;
      }
    return ab/(sqrt(aa)*sqrt(bb));
  }

double rn_sin (int n, double *a, double *b)
  { double aa = 0.0;
    double bb = 0.0;
    int i;
    for (i = 0; i < n; i++)
      { double ai = a[i]; 
        double bi = b[i]; 
        aa += ai*ai; bb += bi*bi;
      }
    { double na = 1.0/sqrt(aa);
      double nb = 1.0/sqrt(bb);
      double dd = 0.0;
      double ss = 0.0;

      for (i = 0; i < n; i++)
        { double ai = na * a[i]; 
          double bi = nb * b[i]; 
          double di = ai - bi;
          double si = ai + bi;
          dd += di*di; ss += si*si;
        }
      return 2.0 * sqrt(dd*ss)/(dd + ss);
    }
  }

void rn_cross (int n, double **a, double *r)
  { int nn1 = (n-1)*n, t = 0, s, i, j, izer;
    double *C = (double *)notnull(malloc(nn1*sizeof(double)), "no mem for C");
    double d;
    for (i = 0; i < n-1; i++) 
      { double *ai = a[i]; 
        for (j = 0; j < n; j++) { C[t] = ai[j]; t++; }
      }
    gsel_triangularize(n-1, n, C, 1);
    gsel_diagonalize(n-1, n, C, 1);
    /* If {det(C)} is not zero, set {d = det(C)}, {izer = -1}.
      Else set {izer} to the first zero column in {C}, and 
      set {d} to the determinant of {C} excluding that column. */
    d = 1.0; izer = -1; t = 0;
    for (i = 0; i < n-1; i++)
      { if (C[t] == 0.0)
          { if (izer < 0) { izer = i; t++; } else { d = 0.0; break; } }
        d *= C[t]; t += n+1;
      }
    if (izer < 0)
      { r[n-1] = d;
        /* For {i=0..n-2}, set {r[i] = -d*(C[i,n-1]/C[i,i])}: */  
        s = nn1-1; t = nn1-2;
        for (i = n-2; i >= 0; i--) 
          { r[i] = -d*C[s]/C[t]; s -= n; t -= n+1; }
      }
    else
      { /* Set {r[izer] = (-1)^(n-1-izer)*d}, all other elems to 0; */
        for (i = 0; i < n; i++) { r[i] = 0.0; }
        r[izer] = ((n - 1 - izer) % 2 == 0 ? d : -d);
      }
    free(C);
  }

double rn_det (int n, double **a)
  { int n2 = n*n, t = 0, i, j;
    double *C = (double *)notnull(malloc(n2*sizeof(double)), "no mem for C");
    double d;
    for (i = 0; i < n; i++) 
      { double *ai = a[i]; 
        for (j = 0; j < n; j++) { C[t] = ai[j]; t++; }
      }
    gsel_triangularize(n, n, C, 0);
    d = 1.0;
    for (t = 0; t < n2; t += n+1) { d *= C[t]; }
    free(C);
    return d;
  }

double rn_decomp (int n, double *a, double *u, double *para, double *perp)
  { double sau = 0.0;
    double suu = 0.0;
    int i;
    for (i = 0; i < n; i++) 
      { double ai = a[i]; double ui = u[i];
        suu += ui*ui;
        sau += ai*ui;
      }
    if (sau == 0.0) 
      { for (i = 0; i < n; i++) 
          { if (para != NULL) { para[i] = 0.0; }
            if (perp != NULL) { perp[i] = a[i]; } 
          } 
        return 0.0;
      }
    else
      { double c = sau / suu;
        for (i = 0; i < n; i++) 
          { double pi = c * u[i]; 
            if (para != NULL) { para[i] = pi; }
            if (perp != NULL) { perp[i] = a[i] - pi; }
          } 
        return c;
      }
  }

void rn_throw_cube (int n, double *r)
  { int i;
    for (i = 0; i < n; i++)
      { r[i] = 2.0 * drandom() - 1.0; }
  }

void rn_throw_ball (int n, double *r)
  { int i;
    while (1)
      { double r2 = 0.0;
        for (i = 0; i < n; i++)
          { double ci = 2.0 * drandom() - 1.0; 
            r[i] = ci;
            r2 += ci*ci;
          }
        if (r2 < 1.0) { return; }
      }
  }

void rn_throw_normal (int n, double *r)
  { int i;
    for (i = 0; i < n; i++) { r[i] = dgaussrand(); }
  }

void rn_print (FILE *f, int n, double *a)
  { rn_gen_print(f, n, a, NULL, NULL, NULL, NULL); }

void rn_gen_print 
  ( FILE *f, int n, double *a, 
    char *fmt, 
    char *lp, char *sep, char *rp
  )
  { int i;
    if (fmt == NULL) { fmt = "%16.8e"; }
    if (lp == NULL) { lp = "("; }
    if (sep == NULL) { sep = " "; }
    if (rp == NULL) { rp = ")"; }
    fputs(lp, f);
    for (i = 0; i < n; i++)
      { if (i > 0) { fputs(sep, f); }
        fprintf(f, fmt, a[i]);
      }
    fputs(rp, f);
  }

double *rn_alloc(int n)
  { void *p = malloc(n*sizeof(double));
    affirm(p != NULL, "no memory for rn_t");
    return (double *)p;
  }
/* COPYRIGHT AND AUTHORSHIP NOTICE

  Copyright © 2005 Jorge Stolfi, Universidade Estadual de Campinas (UNICAMP).
  Created by Jorge Stolfi in 1992--2005.
  
  This source file can be freely distributed, used, and modified,
  provided that this copyright and authorship notice is preserved in
  all copies, and that any modified versions of this file are clearly
  marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the author nor his employers shall be held
  responsible for any losses or damages that may result from its use.

  END OF NOTICE */