!best002.c

supplementary source file 1 for The BESTLibrary

/*----------------------------------------------------------------------------
 * v is scaled to length newlen
 * RETURNS: v
 */
vector2 *v2_scale(vector2 *v, double newlen)
{
  double len = V2Length(v);

  if (len != 0.0)
    v->x *= newlen/len, v->y *= newlen/len;
  return( v );
}

/*----------------------------------------------------------------------------
 * RETURNS: length of line segment pq
 */
double v2_seg_len(point2 *p, point2 *q)
{
  double dx = p->x - q->x;
  double dy = p->y - q->y;

  return( sqrt(dx*dx + dy*dy) );
}

/*----------------------------------------------------------------------------
 * w = u - v
 * RETURNS: w
 */
vector2 *v2_sub(vector2 *u, vector2 *v, vector2 *w)
{
  w->x = u->x - v->x, w->y = u->y - v->y;
  return( w );
}


/*==========================================================================
 *                                3D VECTORS
 *--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
 * w = u + v
 * RETURNS: w
 */
vector3 *v3_add(vector3 *u, vector3 *v, vector3 *w)
{
  w->x = u->x + v->x, w->y = u->y + v->y, w->z = u->z + v->z;
  return( w );
}

/*----------------------------------------------------------------------------
 * w = c1(u) + c2(v)
 * RETURNS: w
 */
vector3 *v3_combine(vector3 *u, double c1,
                    vector3 *v, double c2, vector3 *w)
{
  w->x = (c1 * u->x) + (c2 * v->x);
  w->y = (c1 * u->y) + (c2 * v->y);
  w->z = (c1 * u->z) + (c2 * v->z);
  return( w );
}

/*----------------------------------------------------------------------------
 * RETURNS: copy of u
 */
vector3 *v3_copy(vector3 *v)
{
  vector3 *w = NEWTYPE(vector3);

  w->x = v->x, w->y = v->y, w->z = v->z;
  return( v );
}

/*----------------------------------------------------------------------------
 * w = u cross v
 * RETURNS: w
 */
vector3 *v3_cross(vector3 *u, vector3 *v, vector3 *w)
{
  w->x = (u->y * v->z) - (u->z * v->y);
  w->y = (u->z * v->x) - (u->x * v->z);
  w->z = (u->x * v->y) - (u->y * v->x);
  return( w );
}

/*----------------------------------------------------------------------------
 * w = u dot v
 * RETURNS: w
 */
double v3_dot(vector3 *u, vector3 *v)
{
  return( (u->x * v->x) + (u->y * v->y) + (u->z * v->z) );
}

/*----------------------------------------------------------------------------
 * RETURNS: magnitude of v
 */
double v3_len(vector3 *v)
{
  return( sqrt(v3_len_sqr(v)) );
}

/*----------------------------------------------------------------------------
 * RETURNS: magnitude of v squared
 */
double v3_len_sqr(vector3 *v)
{
  return( (v->x * v->x) + (v->y * v->y) + (v->z * v->z) );
}

/*----------------------------------------------------------------------------
 * w = linear interpolation between lo and hi by amount alpha
 * RETURNS: w
 */
vector3 *v3_lerp(vector3 *lo, vector3 *hi, double alpha, vector3 *w)
{
  w->x = LERP(alpha, lo->x, hi->x);
  w->y = LERP(alpha, lo->y, hi->y);
  w->z = LERP(alpha, lo->z, hi->z);
  return( w );
}

/*----------------------------------------------------------------------------
 * w = u * v (multiplication component-wise)
 * RETURNS: w
 */
vector3 *v3_mul(vector3 *u, vector3 *v, vector3 *w)
{
  w->x = u->x * v->x;
  w->y = u->y * v->y;
  w->z = u->z * v->z;
  return( w );
}

/*----------------------------------------------------------------------------
 * q = p * A
 * RETURNS: transformed point q
 */
point3 *v3_mul_by_matrix(point3 *p, matrix3 *A, point3 *q)
{
  q->x = (p->x * A->element[0][0])
       + (p->y * A->element[1][0]) + (p->z * A->element[2][0]);
  q->y = (p->x * A->element[0][1])
       + (p->y * A->element[1][1]) + (p->z * A->element[2][1]);
  q->z = (p->x * A->element[0][2])
       + (p->y * A->element[1][2]) + (p->z * A->element[2][2]);
  return( q );
}

/*----------------------------------------------------------------------------
 * RETURNS: transformed point p * projection matrix A
 */
point3 *v3_mul_by_proj(point3 *p, matrix4 *A)
{
  double w;
  point3 q;

  q.x = (p->x * A->element[0][0]) + (p->y * A->element[1][0])
      + (p->z * A->element[2][0]) + A->element[3][0];
  q.y = (p->x * A->element[0][1]) + (p->y * A->element[1][1])
      + (p->z * A->element[2][1]) + A->element[3][1];
  q.z = (p->x * A->element[0][2]) + (p->y * A->element[1][2])
      + (p->z * A->element[2][2]) + A->element[3][2];
  w = (p->x * A->element[0][3]) + (p->y * A->element[1][3])
    + (p->z * A->element[2][3]) + A->element[3][3];
  if (w != 0.0)
    q.x /= w, q.y /= w, q.z /= w;
  *p = q;
  return( q );
}

/*----------------------------------------------------------------------------
 * v is negated
 * RETURNS: v
 */
vector3 *v3_neg(vector3 *v)
{
  v->x = -v->x, v->y = -v->y, v->z = -v->z;
  return( v );
}

/*----------------------------------------------------------------------------
 * RETURNS: new 3D vector initialized to (x,y,z)
 */
vector3 *v3_new(double x, double y, double z)
{
  vector3 *v = NEWTYPE(vector3);

  v->x = x, v->y = y, v->z = z;
  return( v );
}

/*----------------------------------------------------------------------------
 * v is normalized (becomes unit length)
 * RETURNS: v
 */
vector3 *v3_norm(vector3 *v)
{
  double len = v3_len(v);

  if (len != 0.0)
    v->x /= len, v->y /= len, v->z /= len;
  return( v );
}

/*----------------------------------------------------------------------------
 * v is scaled to length newlen
 * RETURNS: v
 */
vector3 *v3_scale(vector3 *v, double newlen)
{
  double len = v3Length(v);

  if (len != 0.0)
    v->x *= newlen/len, v->y *= newlen/len, v->z *= newlen/len;
  return( v );
}
        
/*----------------------------------------------------------------------------
 * RETURNS: length of line segment pq
 */
double v3_seg_len(point3 *p, point3 *q)
{
  double dx = p->x - q->x;
  double dy = p->y - q->y;
  double dz = p->z - q->z;

  return( sqrt(dx*dx + dy*dy + dz*dz) );
}

/*----------------------------------------------------------------------------
 * w = u - v
 * RETURNS: w
 */
vector3 *v3_sub(vector3 *u, vector3 *v, vector3 *w)
{
  w->x = u->x - v->x;  w->y = u->y - v->y;  w->z = u->z - v->z;
  return( w );
}

/*==========================================================================
 *                              USEFUL ROUTINES
 *--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
 * binary greatest common divisor by Silver and Terzian.  See Knuth
 * both inputs must be >= 0
 */
int gcd(int u, int v)
{
  int k, t, f;

  if (u < 0 || v < 0)
    return( 1 );                       /* error if u < 0 or v < 0 */
  k = 0, f = 1;
  while ((u%2 == 0) && (v%2 == 0)) {
    k++;
    u >>= 1;
    v >>= 1;
    f *= 2;
  }
  if (u & 1) {
    t = -v;
    goto B4;
  }
  else
    t = u;
B3:
  if (t > 0)
    t >>= 1;
  else
    t = -((-t) >> 1);
B4:
  if (t%2 == 0)
    goto B3;

  if (t > 0)
    u = t;
  else
    v = -t;
  if ((t = u-v) != 0)
    goto B3;
  return( u*f );
}

/*----------------------------------------------------------------------------
 * return roots of a*x^2 + b*x + c
 * stable algebra derived from Numerical Recipes by Press et al
 */
int quadraticRoots(double a, double b, double c, double *roots)
{
  double d, q;
  int count = 0;

  d = b*b - 4*a*c;
  if (d < 0.0) {
    *roots = *(roots+1) = 0.0;
    return( 0 );
  }
  q = -0.5 * (b + SGN(b)*sqrt(d));
  if (a != 0.0) {
    *roots++ = q/a;
    count++;
  }
  if (q != 0.0) {
    *roots++ = c/q;
    count++;
  }
  return( count );
}


/*----------------------------------------------------------------------------
 * generic 1d regula-falsi step.  f is function to evaluate
 * interval known to contain root is given in left, right
 * returns new estimate
 */
double RegulaFalsi(double (*f)(), double left, double right)
{
  double d = (*f)(right) - (*f)(left);

  if (d != 0.0)
    return( right - (*f)(right) * (right - left) / d);
  return( (left+right) / 2.0 );
}

/*----------------------------------------------------------------------------
 * generic 1d Newton-Raphson step. f is function, df is derivative
 * x is current best guess for root location. Returns new estimate
 */
double NewtonRaphson(double (*f)(), double (*df)(), double x)
{
  double d = (*df)(x);

  if (d != 0.0)
    return( x - ((*f)(x) / d) );
  return( x - 1.0 );
}


/*----------------------------------------------------------------------------
 * hybrid 1d Newton-Raphson/Regula Falsi root finder
 * input function f and its derivative df, an interval
 * left, right known to contain the root, and an error tolerance
 * Based on Blinn
 */
double findroot(double left, double right,
		 double tolerance, double (*f)(), double (*df)())
{
  double newx = left;

  while (ABS((*f)(newx)) > tolerance) {
    newx = NewtonRaphson(f, df, newx);
    if (newx < left || newx > right)
      newx = RegulaFalsi(f, left, right);
    if ((*f)(newx) * (*f)(left) <= 0.0)
      right = newx;
    else
      left = newx;
  }
  return( newx );
}

/*==============================  END-OF-FILE  =============================*/