arcdotl.c

linux下用PCMCIA无线网卡虚拟无线AP的程序源码

/*							arcdot.c
 *
 *	Angle between two vectors
 *
 *
 *
 *
 * SYNOPSIS:
 *
 * long double p[3], q[3], arcdotl();
 *
 * y = arcdotl( p, q );
 *
 *
 *
 * DESCRIPTION:
 *
 * For two vectors p, q, the angle A between them is given by
 *
 *      p.q / (|p| |q|)  = cos A  .
 *
 * where "." represents inner product, "|x|" the length of vector x.
 * If the angle is small, an expression in sin A is preferred.
 * Set r = q - p.  Then
 *
 *     p.q = p.p + p.r ,
 *
 *     |p|^2 = p.p ,
 *
 *     |q|^2 = p.p + 2 p.r + r.r ,
 *
 *                  p.p^2 + 2 p.p p.r + p.r^2
 *     cos^2 A  =  ----------------------------
 *                    p.p (p.p + 2 p.r + r.r)
 *
 *                  p.p + 2 p.r + p.r^2 / p.p
 *              =  --------------------------- ,
 *                     p.p + 2 p.r + r.r
 *
 *     sin^2 A  =  1 - cos^2 A
 *
 *                   r.r - p.r^2 / p.p
 *              =  --------------------
 *                  p.p + 2 p.r + r.r
 *
 *              =   (r.r - p.r^2 / p.p) / q.q  .
 *
 * ACCURACY:
 *
 * About 1 ULP.  See arcdot.c.
 *
 */

/*
Cephes Math Library Release 2.3:  November, 1995
Copyright 1995 by Stephen L. Moshier
*/

#include <math.h>
#ifdef ANSIPROT
extern long double sqrtl ( long double );
extern long double acosl ( long double );
extern long double asinl ( long double );
extern long double atanl ( long double );
#else
long double sqrtl(), acosl(), asinl(), atanl();
#endif
extern long double PIL;

long double arcdotl(p,q)
long double p[], q[];
{
long double pp, pr, qq, rr, rt, pt, qt, pq;
int i;

pq = 0.0L;
qq = 0.0L;
pp = 0.0L;
pr = 0.0L;
rr = 0.0L;
for (i=0; i<3; i++)
  {
    pt = p[i];
    qt = q[i];
    pq += pt * qt;
    qq += qt * qt;
    pp += pt * pt;
    rt = qt - pt;
    pr += pt * rt;
    rr += rt * rt;
  }
if (rr == 0.0L || pp == 0.0L || qq == 0.0L)
  return 0.0L;
rt = (rr - (pr * pr) / pp) / qq;
if (rt <= 0.75L)
  {
    rt = sqrtl(rt);
    qt = asinl(rt);
    if (pq < 0.0L)
      qt = PIL - qt;
  }
else
  {
    pt = pq / sqrtl(pp*qq);
    qt = acosl(pt);
  }
return qt;
}