xmathlib.cpp

矩阵和初等几何常用算法

  b[0][0] = b[1][1] = e;
  b[0][1] = d;
  b[0][2] = b[1][2] = 0.0;
  b[1][0] = -d;
  b[2][0] = - x2 * (e - 1) + y2 * d;
  b[2][1] = - x2 * d - y2 * (e - 1);
  b[2][2] = 1.0;
  for(i = 0; i < 1; i++)
    for(j = 0; j < 3; j++)
      {
      c[i][j] = 0.0;
      for(k = 0; k < 3; k++)
      c[i][j] = c[i][j] + a[i][k] * b[k][j];
      }
  x[0] = c[0][0];
  x[1] = c[0][1];
  }

/* get the tangent between a line through a point(x1,y1)//得到过某一点的直线与圆的切线
   and a circle(x2,y2,r2) */
short __stdcall tangent_line_circle(double x1, double y1, double x2, double y2,
           double r2, double x_tan[], double y_tan[])
  {
  double        r1,     dist;

  dist = dpp(x1,y1,x2,y2);
  r1 = sqrt(fabs(dist*dist-r2*r2));
  return pcc(x1,y1,r1,x2,y2,r2,x_tan,y_tan);
  }

/* get the perpendicular point from a point to a line *///得到垂足
void __stdcall get_ver_point(double x, double y, double x1, double y1, double x2,
         double y2, double *xv, double *yv)
  {
  double        a, b, c;

  lpp(x1,y1,x2,y2,&a,&b,&c);
  ppln(x,y,a,b,c,xv,yv);
  }

/* get the intersective points(x_int[0],y_int[0])(x_int[1],y_int[1])//得到直线和圆的交点
   of a line(a,b,c) and a circle(xc,yc,r)
   return 2: intersect
   return 1: tangent
   return 0: not_intersect */
short __stdcall plc(double a,double b,double c,double xc,double yc,double r,
     double x_int[],double y_int[])
  {
  double d, e;

  d = a*xc + b*yc + c;
  if( (fabs(d)-r) < 1.e-3)//GBL_DistTol )
    {
	  double dd = (r+d)*(r-d);
	  if( dd < 0.005)// 98.3.3     0.05)//1e-7 )
		e = 0;
	  else
		e = sqrt(dd);
    x_int[0] = xc - (a*d - b*e);
    y_int[0] = yc - (b*d + a*e);
    x_int[1] = xc - (a*d + b*e);
    y_int[1] = yc - (b*d - a*e);
    if( same_point(x_int[0],y_int[0],x_int[1],y_int[1]) )
      return 1;
    else
      return 2;
    }
  else
    {
    x_int[0] = y_int[0] = x_int[1] = y_int[1] = BIG;
    return 0;
    }
  }

/*short plc(double a,double b,double c,double xc,double yc,double r,
     double x_int[],double y_int[])
{
	double d,dd,e;
	
	d = a*xc + b*yc + c;
	if( (fabs(d)-r)<1.e-7)
	{
		e=sqrt((r+d)*(r-d));
		x_int[0] = xc - (a*d - b*e);
		y_int[0] = yc - (b*d + a*e);
		x_int[1] = xc - (a*d + b*e);
		y_int[1] = yc - (b*d - a*e);
		if(e/r<0.01)
		{
			x_int[0]=(x_int[0]+x_int[1])/2;
			y_int[0]=(y_int[0]+y_int[1])/2;
			x_int[1]=x_int[0];
			y_int[1]=y_int[0];
			return 1;
		}
		else
			return 2;
	}
  else
    {
    x_int[0] = y_int[0] = x_int[1] = y_int[1] = BIG;
    return 0;
    }
}*/
/* get the intersective points(x_int[0],y_int[0])(x_int[1],y_int[1])得到两个圆的交点
   of two circles(xc1,yc1,r1)(xc2,yc2,r2)
   return 2: intersect
   return 1: tangent
   return 0: not_intersect */
short __stdcall pcc(double xc1,double yc1,double r1,double xc2,double yc2,double r2,
     double x_int[],double y_int[])
  {
  double        d;

  d = dpp(xc1,yc1,xc2,yc2);
  
  if( d < GBL_DistTol ||  r1 < GBL_DistTol || r2 < GBL_DistTol || (d - r1 - r2) > GBL_DistTol ||
      (d - fabs(r1 - r2)) < -GBL_DistTol )
    {
    x_int[0] = y_int[0] = x_int[1] = y_int[1] = BIG;
    return 0;
    }
  else
    {
    double sina, cosa, sinb, cosb, xx, yy;

    cosb = (r1*r1 + d*d - r2*r2) / (2.0*d*r1);
    sinb = - sqrt(fabs(1.0 - cosb*cosb));
    cosa = (xc2 - xc1) / d;
    sina = (yc2 - yc1) / d;
    xx = r1 * cosb;
    yy = r1 * sinb;
    x_int[0] = xx * cosa - yy * sina + xc1;
    y_int[0] = xx * sina + yy * cosa + yc1;
    x_int[1] = xx * cosa + yy * sina + xc1;
    y_int[1] = xx * sina - yy * cosa + yc1;
    if( same_point(x_int[0],y_int[0],x_int[1],y_int[1]) )
      return 1;
    else
      return 2;
    }
  }

/* get a line(a,b,c) tangent to a circle(xc,yc,r) and得到过圆上一点的切线
   through a point(xt,yt) on the circle */
void __stdcall lcpt(double xc, double yc, double r, double xt, double yt,
     double *a, double *b, double *c)
  {
  double tmp;

  lpp(xc,yc,xt,yt,a,b,c);
  tmp = *a;
  *a = *b;
  *b = -tmp;
  if( r < 1.0e-5 )
    {
    *a = -*a;
    *b = -*b;
    }
  *c = -(*a)*xt - (*b)*yt;
  return;
  }

/* get the common tangent lines of two circles *///得到两个圆的公切线
short __stdcall lcct(double xc1, double yc1, double r1, double xc2, double yc2,
      double r2, double *xt1, double *yt1, double *xt2, double *yt2)
  {
  double rr1, rr2, d, ca, sa, cb, sb, ct, st;

  rr1 = fabs(r1);
  rr2 = r1 * r2 > 0 ? fabs(r2) : -fabs(r2);
  d = sqrt((xc2-xc1)*(xc2-xc1) + (yc2-yc1)*(yc2-yc1));
  if( same_coor(d,0.0) )
    return 0;
  ca = (rr1 - rr2) / d;
  sa = sqrt(fabs(1.0-ca*ca));
  sa = r1 > 0 ? fabs(sa) : -fabs(sa);
  sb = (yc2 - yc1) / d;
  cb = (xc2 - xc1) / d;
  ct = ca * cb + sa * sb;
  st = sb * ca - cb * sa;
  *xt1 = xc1 + rr1 * ct;
  *yt1 = yc1 + rr1 * st;
  *xt2 = xc2 + rr2 * ct;
  *yt2 = yc2 + rr2 * st;
  return 1;
  }

// 	Circles' Relative Position///圆的位置关系
#define		CC_ABSENT			0      //不存在
#define		CC_SAME				1      //相同
#define		CC_CONCENTRIC		2      //同心圆
#define		CC_INTERN_TANGENT	3      //内切 
#define		CC_INCLUSIVE		4      //包含 
#define		CC_EXTERN_TANGENT	5      //外切 
#define		CC_INTERSECT		6      //相交 

//------------------------------------------------------------------------
//	Determine two circles( (xc1, yc1, r1), (xc2, yc2, r2) ) relative position
//	whthin Dist_Tol.
//
//		same circle:        return 	CC_SAME;
//		concentric:         return 	CC_CONCENTRIC;
//		internal tangent:   return 	CC_INTERN_TANGENT;
//		one in another:		return  CC_INCLUSIVE;
//		intersect:			return 	CC_INTERSECT;
//		external tangent:   return 	CC_EXTERN_TANGENT;
//		absent:				return 	CC_ABSENT;
//------------------------------------------------------------------------

int __stdcall CCRelatPos( double xc1, double yc1, double r1,
	double xc2, double yc2, double r2 )
{
	double rr1 = r1 + r2;
	double rr2 = fabs(r1 - r2);
	double dist= dpp(xc1, yc1, xc2, yc2);
    
    if( dist > rr1 )
    	return CC_ABSENT;
    
	else if( same_coor(rr1, dist) )
		return CC_EXTERN_TANGENT;

	else if( same_coor(rr2, dist) )
		return CC_INTERN_TANGENT;

	else if( rr1 > dist && dist > rr2 )
		return CC_INTERSECT;

	else
		return CC_INCLUSIVE;
}       

//得到两个圆的交点
//------------------------------------------------------------------------
// 	Get intersect points(x, y) of two circles
//	( (xc1, yc1, r1), (xc2, yc2, r2) ).
//
//		same circle:  	return 0;
//   	intersect; 		return 2;
//   	tangent;		return 1;
//   	not intersect; 	return 0.
//------------------------------------------------------------------------

int __stdcall CCGetInterPoint( double xc1, double yc1, double r1,
	double xc2, double yc2, double r2, double *x, double *y )
{
	switch(CCRelatPos(xc1, yc1, r1, xc2, yc2, r2))
	{
		case CC_SAME:				//	infinite intersect points无穷多交点
			return 0;
 
		case CC_INTERN_TANGENT:		//	one intersect point仅有一个交点
		case CC_EXTERN_TANGENT:
		{
			double dist = dpp(xc1, yc1, xc2, yc2);
			double cosa = (xc2 - xc1) / dist;
			double sina = (yc2 - yc1) / dist;
			
			x[0] = x[1] = xc1 + r1 * cosa;
			y[0] = y[1] = yc1 + r1 * sina;

			return 1;
		}

		case CC_INTERSECT:			//	two intersect points两个交点
		{
			double dist = dpp(xc1, yc1, xc2, yc2);
			double cosb = (r1*r1 + dist*dist - r2*r2) / (2.0*dist*r1);
			double sinb = - sqrt(fabs(1.0 - cosb*cosb));
			double cosa = (xc2 - xc1) / dist;
			double sina = (yc2 - yc1) / dist;
			double xx = r1 * cosb;
			double yy = r1 * sinb;
			x[0] = xx * cosa - yy * sina + xc1;
			y[0] = xx * sina + yy * cosa + yc1;
			x[1] = xx * cosa + yy * sina + xc1;
			y[1] = xx * sina - yy * cosa + yc1;

			return 2;
		}

		default:			//	case CC_ABSENT, No intersect point
			return 0;
	}
}




//得到公切线交点
//------------------------------------------------------------------------
// 	Get common tangent lines( (xs, ys), (xe, ye) )   
//	of two circles( (xc1, yc1, r1), (xc2, yc2, r2) ).
//
//		No tangent line, 	return 0;
//		1 tangent lines,	return 1;
//		2 tangent lines,	return 2;
//		3 tangent lines,	return 3;
//		4 tangent lines,	return 4;
//------------------------------------------------------------------------

int __stdcall CCGetTanLine( double xc1, double yc1, double r1, double xc2, double yc2, double r2,
	double *xs, double *ys, double *xe, double *ye )
{
	switch( CCRelatPos(xc1, yc1, r1, xc2, yc2, r2) )
	{
		case CC_INTERN_TANGENT:
		{
			// 	one tangent line
			CCGetInterPoint(xc1, yc1, r1, xc2, yc2, r2, xs, ys);
			xe[0] = xs[0];
			ye[0] = ys[0];
			return 1;
		}

		case CC_INTERSECT:
		{
			//	two tangent lines
			//	external common tangent lines
			double 	dist= dpp(xc1, yc1, xc2, yc2);

			double sina = (r2 - r1) / dist;
			double cosa = sqrt(fabs(1.0 - sina*sina));
			double sinb = (yc2 - yc1) / dist;
			double cosb = (xc2 - xc1) / dist;

			//	first tangent line
			double cost = cosb * cosa + sinb * sina;	//	cos(t) = cos(b-a)
			double sint = sinb * cosa - cosb * sina;	//	sin(t) = sin(b-a)
			xs[0] = xc1 + r1 * sint;
			ys[0] = yc1 - r1 * cost;
			xe[0] = xc2 + r2 * sint;
			ye[0] = yc2 - r2 * cost;

			//	second tangent line
			cost = cosb * cosa - sinb * sina;	//	cos(t) = cos(b+a)
			sint = sinb * cosa + cosb * sina; 	//	sin(t) = sin(b+a)
			xs[1] = xc1 - r1 * sint;
			ys[1] = yc1 + r1 * cost;
			xe[1] = xc2 - r2 * sint;
			ye[1] = yc2 + r2 * cost;

			return 2;
		}

		case CC_EXTERN_TANGENT:
		{
			// 	three tangent lines
			//	external common tangent lines
			double sina = (r2-r1) / (r1+r2);
			double cosa = sqrt(fabs(1.0 - sina*sina));
			double sinb = (yc2 - yc1) / (r1+r2);
			double cosb = (xc2 - xc1) / (r1+r2);

			//	the first tangent line
			double cost = cosb * cosa + sinb * sina;	//	cos(t) = cos(b-a)
			double sint = sinb * cosa - cosb * sina;	//	sin(t) = sin(b-a)
			xs[0] = xc1 + r1 * sint;
			ys[0] = yc1 - r1 * cost;
			xe[0] = xc2 + r2 * sint;
			ye[0] = yc2 - r2 * cost;

			//	the second tangent line
			cost = cosb * cosa - sinb * sina;	//	cos(t) = cos(b+a)
			sint = sinb * cosa + cosb * sina; 	//	sin(t) = sin(b+a)
			xs[1] = xc1 - r1 * sint;
			ys[1] = yc1 + r1 * cost;
			xe[1] = xc2 - r2 * sint;
			ye[1] = yc2 + r2 * cost;

			// 	internal tangent line
			//	the third tangent line
			CCGetInterPoint(xc1, yc1, r1, xc2, yc2, r2, &xs[2], &ys[2]);
			xe[2] = xs[2];
			ye[2] = ys[2];

			return 3;
		}

		case CC_ABSENT:
		{
			//	four tangent lines
			double 	dist= dpp(xc1, yc1, xc2, yc2);
			double 	rr1 = r2 - r1;
			double  rr2 = r2 + r1;

			//	external common tangent lines
			double sina = rr1 / dist;
			double cosa = sqrt(fabs(1.0 - sina*sina));
			double sinb = (yc2 - yc1) / dist;
			double cosb = (xc2 - xc1) / dist;

			//	the first tangent line
			double cost = cosb * cosa + sinb * sina;	//	cos(t) = cos(b-a)
			double sint = sinb * cosa - cosb * sina;	//	sin(t) = sin(b-a)
			xs[0] = xc1 + r1 * sint;
			ys[0] = yc1 - r1 * cost;
			xe[0] = xc2 + r2 * sint;
			ye[0] = yc2 - r2 * cost;

			//	the second tangent line
			cost = cosb * cosa - sinb * sina;	//	cos(t) = cos(b+a)
			sint = sinb * cosa + cosb * sina; 	//	sin(t) = sin(b+a)
			xs[1] = xc1 - r1 * sint;
			ys[1] = yc1 + r1 * cost;
			xe[1] = xc2 - r2 * sint;
			ye[1] = yc2 + r2 * cost;

			// 	the internal common tangent lines
			sina = rr2 / dist;
			cosa = sqrt(fabs(1.0 - sina*sina));

			//	the third tangent line
			cost = cosa * cosb + sina * sinb;	//	cos(t) = cos(a-b)
			sint = sina * cosb - cosa * sinb;	//	sin(t) = sin(a-b)
			xs[2] = xc1 + r1 * sint;
			ys[2] = yc1 + r1 * cost;
			xe[2] = xc2 - r2 * sint;
			ye[2] = yc2 - r2 * cost;

			//	the forth tangent line
			cost = cosa * cosb - sina * sinb;	//	cos(t) = cos(a+b)
			sint = sina * cosb + cosa * sinb; 	//	sin(t) = sin(a+b)
			xs[3] = xc1 + r1 * sint;
			ys[3] = yc1 - r1 * cost;
			xe[3] = xc2 - r2 * sint;
			ye[3] = yc2 + r2 * cost;

			return 4;
		}

		default:
			return 0;			// 	No tangent line

	}
}



/* restrict angle to be >= 0.0 and < 360.0 *///限制角度在0到360度之间
double __stdcall standard_angle(double angle)
  {
  if( angle >= 360.0 ) return angle-360.0;
  if( angle < 0.0 )    return angle+360.0;
  return angle;
  }

/* Get intersection poin (x0,y0) of two line segments (x1,y1)-(x2,y2)//
   and (x3,y3)-(x4,y4) 
   return 1: intersect
   return 0: not_intersect */

short __stdcall ppppp(double x1, double y1, double x2, double y2, double x3, double y3,
      double x4, double y4, double *x0, double *y0)
  {
  double        a1,b1,c1,
      a2,b2,c2;

  lpp(x1,y1,x2,y2,&a1,&b1,&c1);
  lpp(x3,y3,x4,y4,&a2,&b2,&c2);
  return pll(a1,b1,c1,a2,b2,c2,x0,y0);
  }

/* Get an angle between a known line and x_axis *///得到已知直线与X轴的夹角
double __stdcall alx(double a,double b)
  {
  double angle;

  if( fabs(a) > 1.0 )