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

#include <r3.h>

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

#define N 3

void r3_zero (r3_t *r)
  { r->c[0] = 0.0;
    r->c[1] = 0.0;
    r->c[2] = 0.0;
  }

void r3_all (double x, r3_t *r)
  { r->c[0] = x;
    r->c[1] = x;
    r->c[2] = x;
  }

void r3_axis (int i, r3_t *r)
  { affirm((i >= 0) && (i < N), "r3_axis: bad index");
    r->c[0] = 0.0;
    r->c[1] = 0.0;
    r->c[2] = 0.0;

    r->c[i] = 1.0;
  }

void r3_add (r3_t *a, r3_t *b, r3_t *r)
  { r->c[0] = a->c[0] + b->c[0];
    r->c[1] = a->c[1] + b->c[1];
    r->c[2] = a->c[2] + b->c[2];
  }

void r3_sub (r3_t *a, r3_t *b, r3_t *r)
  { r->c[0] = a->c[0] - b->c[0];
    r->c[1] = a->c[1] - b->c[1];
    r->c[2] = a->c[2] - b->c[2];
  }

void r3_neg (r3_t *a, r3_t *r)
  { r->c[0] = - a->c[0];
    r->c[1] = - a->c[1];
    r->c[2] = - a->c[2];
  }

void r3_scale (double s, r3_t *a, r3_t *r)
  { r->c[0] = s * a->c[0];
    r->c[1] = s * a->c[1];
    r->c[2] = s * a->c[2];
  }

void r3_mix (double s, r3_t *a, double t, r3_t *b, r3_t *r)
  { r->c[0] = s * a->c[0] + t * b->c[0];
    r->c[1] = s * a->c[1] + t * b->c[1];
    r->c[2] = s * a->c[2] + t * b->c[2];
  }

void r3_mix_in (double s, r3_t *a, r3_t *r)
  { r->c[0] += s * a->c[0];
    r->c[1] += s * a->c[1];
    r->c[2] += s * a->c[2];
  }

void r3_weigh (r3_t *a, r3_t *w, r3_t *r)
  { r->c[0] = a->c[0] * w->c[0];
    r->c[1] = a->c[1] * w->c[1];
    r->c[2] = a->c[2] * w->c[2];
  }

double r3_norm (r3_t *a)
  { double a0 = a->c[0];
    double a1 = a->c[1];
    double a2 = a->c[2];
    return sqrt(a0*a0 + a1*a1 + a2*a2);
  }

double r3_norm_sqr (r3_t *a)
  { double a0 = a->c[0];
    double a1 = a->c[1];
    double a2 = a->c[2];
    return a0*a0 + a1*a1 + a2*a2;
  }

double r3_L_inf_norm (r3_t *a)
  { double d = 0.0;
    double a0 = fabs(a->c[0]);
    double a1 = fabs(a->c[1]);
    double a2 = fabs(a->c[2]);
    if (a0 > d) d = a0;
    if (a1 > d) d = a1;
    if (a2 > d) d = a2;
    return (d);
  }

double r3_dist (r3_t *a, r3_t *b)
  { double d0 = (a->c[0] - b->c[0]);
    double d1 = (a->c[1] - b->c[1]);
    double d2 = (a->c[2] - b->c[2]);
    double d = sqrt(d0*d0 + d1*d1 + d2*d2);
    return (d);
  }

double r3_dist_sqr (r3_t *a, r3_t *b)
  { double d0 = (a->c[0] - b->c[0]);
    double d1 = (a->c[1] - b->c[1]);
    double d2 = (a->c[2] - b->c[2]);
    return d0*d0 + d1*d1 + d2*d2;
  }

double r3_L_inf_dist (r3_t *a, r3_t *b)
  { double d = 0.0;
    double d0 = fabs(a->c[0] - b->c[0]);
    double d1 = fabs(a->c[1] - b->c[1]);
    double d2 = fabs(a->c[2] - b->c[2]);
    if (d0 > d) d = d0;
    if (d1 > d) d = d1;
    if (d2 > d) d = d2;
    return (d);
  }

double r3_dir (r3_t *a, r3_t *r)
  { double d = sqrt(a->c[0]*a->c[0] + a->c[1]*a->c[1] + a->c[2]*a->c[2]);
    r->c[0] = a->c[0]/d;
    r->c[1] = a->c[1]/d;
    r->c[2] = a->c[2]/d;
    return (d);
  }

double r3_L_inf_dir (r3_t *a, r3_t *r)
  { double d = 0.0;
    double a0 = fabs(a->c[0]);
    double a1 = fabs(a->c[1]);
    double a2 = fabs(a->c[2]);
    if (a0 > d) d = a0;
    if (a1 > d) d = a1;
    if (a2 > d) d = a2;
    r->c[0] = a->c[0]/d;
    r->c[1] = a->c[1]/d;
    r->c[2] = a->c[2]/d;
    return (d);
  }

double r3_dot (r3_t *a, r3_t *b)
  { return a->c[0]*b->c[0] + a->c[1]*b->c[1] + a->c[2]*b->c[2]; }

double r3_cos (r3_t *a, r3_t *b)
  { double a0 = a->c[0];
    double a1 = a->c[1];
    double a2 = a->c[2];

    double b0 = b->c[0];
    double b1 = b->c[1];
    double b2 = b->c[2];

    double ab = a0*b0 + a1*b1 + a2*b2;
    double aa = a0*a0 + a1*a1 + a2*a2;
    double bb = b0*b0 + b1*b1 + b2*b2;
    return ab/(sqrt(aa)*sqrt(bb));
  }

double r3_sin (r3_t *a, r3_t *b)
  { return rn_sin(N, &(a->c[0]), &(b->c[0])); }

void r3_cross (r3_t *a, r3_t *b, r3_t *r)
  { double a0 = a->c[0];
    double a1 = a->c[1];
    double a2 = a->c[2];

    double b0 = b->c[0];
    double b1 = b->c[1];
    double b2 = b->c[2];

    r->c[0] = (a1 * b2) - (a2 * b1);
    r->c[1] = (a2 * b0) - (a0 * b2);
    r->c[2] = (a0 * b1) - (a1 * b0);
  }

double r3_det (r3_t *a, r3_t *b, r3_t *c)
  { r3_t p;
    r3_cross(a, b, &p);
    return p.c[0]*c->c[0] + p.c[1]*c->c[1] + p.c[2]*c->c[2];
  }

double r3_decomp (r3_t *a, r3_t *u, r3_t *para, r3_t *perp)
  { double u0 = u->c[0];
    double u1 = u->c[1];
    double u2 = u->c[2];

    double sau = a->c[0]*u0 + a->c[1]*u1 + a->c[2]*u2;
    if (sau == 0.0) 
      { if (para != NULL)
          { para->c[0] = 0.0; 
            para->c[1] = 0.0;
            para->c[2] = 0.0; 
          }

        if (perp != NULL)
          { perp->c[0] = a->c[0]; 
            perp->c[1] = a->c[1];
            perp->c[2] = a->c[2]; 
          } 
        return 0.0;
      }
    else
      { double suu = u0*u0 + u1*u1 + u2*u2;
        double c = sau / suu;
        double p0 = c * u0; 
        double p1 = c * u1; 
        double p2 = c * u2; 

        if (para != NULL)
          { para->c[0] = p0;
            para->c[1] = p1;
            para->c[2] = p2;
          }

        if (perp != NULL)
          { perp->c[0] = a->c[0] - p0; 
            perp->c[1] = a->c[1] - p1;
            perp->c[2] = a->c[2] - p2; 
          }
        return c;
      }
  }

void r3_throw_cube (r3_t *r)
  { r->c[0] = 2.0 * drandom() - 1.0;
    r->c[1] = 2.0 * drandom() - 1.0;
    r->c[2] = 2.0 * drandom() - 1.0;
  }

void r3_throw_ball (r3_t *r)
  { double x, y, z;
    do
      { x = 2.0 * drandom() - 1.0; r->c[0] = x;
        y = 2.0 * drandom() - 1.0; r->c[1] = y;
        z = 2.0 * drandom() - 1.0; r->c[2] = z;
      }
    while (x*x + y*y + z*z >= 1.0);
  }

void r3_throw_normal (r3_t *r)
  { r->c[0] = dgaussrand();
    r->c[1] = dgaussrand();
    r->c[2] = dgaussrand();
  }

void r3_print (FILE *f, r3_t *a)
  { r3_gen_print(f, a, NULL, NULL, NULL, NULL); }

void r3_gen_print (FILE *f, r3_t *a, char *fmt, char *lp, char *sep, char *rp)
  { rn_gen_print(f, N, &(a->c[0]), fmt, lp, sep, rp); }

/* 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 */