xmathlib.cpp

矩阵和初等几何常用算法

    angle = PI / 2.0;
  else
    angle = asin(fabs(a));
  if(a >= 0)
    {
    if(b < 0)
      return angle;
    else
      return (PI-angle);
    }
  if(b >= 0)
    return (PI+angle);
  return (2*PI-angle);
  }


/* Queue two points on a line.    判断两个点是否在同一条已知直线上，共线问题
  return 1: orient of two points align with line;
  return 0: orient of two points against the orient of the line, exchange */
short __stdcall tlpp(double x1, double y1, double x2, double y2, double a, double b)
   {
   double t, aa;

   if(fabs(a) <= 1e-5)
     {
     t = x1 - x2;
     aa = b;
     }
   else
     {
     t = y2 - y1;
     aa = a;
     }
   if( t * aa <= 0)
     return 0;
   return 1;
   }
//从两个已知的点得到内部或外部的分割点
/* Get the internal or external division point from two known points
   if success
      return 1;
   else ( when d = -1 )
      return -1.     */
int __stdcall ppp(double x1, double y1, double x2, double y2, double d,
   double *x, double *y)
  {
  if(fabs(d+1) < 1e-5)
    {
    *x = 1;
    *y = 1;
    return -1;
    }
  *x = (x1 + d*x2) / (1+d);
  *y = (y1 + d*y2) / (1+d);
  return 1;
  }

/* Get a symmetrical point from a known point against a known line *///得到一个点关于一条已知直线的对称点
void __stdcall plp(double a, double b, double c, double xp, double yp,
     double *x, double *y)
  {
   double d;

   d = 2 * (a*xp + b*yp + c);
   *x = xp - a*d;
   *y = yp - b*d;
   }

/* Get angle between two lines */
//得到两条直线的夹角
double __stdcall all(double a1, double b1, double a2, double b2)
{
  double        sina, cosa, angle;
  short         flag;

  sina = a1*b2-a2*b1;
  cosa = a1*a2+b1*b2;
  if( sina >= 0 && cosa >= 0 )
    flag = 1;
  else if( sina >= 0 && cosa <= 0 )
    flag = 2;
  else if( sina <= 0 && cosa <= 0 )
    flag = 3;
  else if( sina <= 0 && cosa >= 0 )
    flag = 4;
  if( fabs(sina) > 1.0 )
    sina = Sign(sina);
  angle = asin(sina);
  switch( flag )
    {
    case 1:
      break;
    case 2:
      angle = PI - angle;
      break;
    case 3:
      angle = PI - angle;
      break;
    case 4:
      angle = 2*PI + angle;
    }
  return angle;
}

/* check if two lines are colinear */
//检查两条线是否同线
short __stdcall colinear(double a1, double b1, double c1, double a2, double b2, double c2)
  {
  double angle;

  angle = rtod(all(a1,b1,a2,b2));
  if( (same_angle(angle,0.0) || same_angle(angle,360.0)) &&
      same_coor(c1,c2) )
    return 1;
  if( same_angle(angle,180.0) && same_coor(-c1,c2) )
    return -1;
  return 0;
  }

/* check if two lines are parallel */
//检查两线是否平行
short __stdcall parallel(double a1, double b1, double a2, double b2)
  {
  double angle;

  angle = rtod(all(a1,b1,a2,b2));
  if( same_angle(angle,0.0) || same_angle(angle,360.0) )
    return 1;
  if( same_angle(angle,180.0) )
    return -1;
  return 0;
  }

/* check if two lines are perpendicular */
//检查两条线是否垂直
short __stdcall perpend(double a1, double b1, double a2, double b2)
  {
  double angle;

  angle = rtod(all(a1,b1,a2,b2));
  if( same_angle(angle,90.0) )
    return 1;
  if( same_angle(angle,270.0) )
    return -1;
  return 0;
  }

void __stdcall pcpa(double xc, double yc, double xp, double yp, double alpha,
     double *x, double *y)
  {
  double ca, sa;

  ca = cos(alpha);
  sa = sin(alpha);
  *x = xc + (xp-xc)*ca - (yp-yc)*sa;
  *y = yc + (xp-xc)*sa + (yp-yc)*ca;
  }

/* get a circle tangent to three lines */
//得到切于三条线的圆
short __stdcall clll(double a1, double b1, double c1, short q1,
      double a2, double b2, double c2, short q2,
      double a3, double b3, double c3, short q3,
      double *xc, double *yc, double *r)
  {
  double A, B, C, D;
  short  Sign;

  if( (Sign = parallel(a2,b2,a3,b3)) != 0 )
    {
    *r = fabs(c2-Sign*c3)/2.0;
    if( !parallel(a1,b1,a2,b2) )
      {
      *xc = ((q1*(*r)-c1)*b2-(q2*(*r)-c2)*b1)/(a1*b2-a2*b1);
      *yc = ((q1*(*r)-c1)*a2-(q2*(*r)-c2)*a1)/(b1*a2-b2*a1);
      return 1;
      }
    else
      return 0;
    }
  else
    {
    A = (b3*q2-b2*q3)/(a2*b3-a3*b2);
    B = -(b3*c2-b2*c3)/(a2*b3-a3*b2);
    C = (a3*q2-a2*q3)/(a3*b2-a2*b3);
    D = -(a3*c2-a2*c3)/(a3*b2-a2*b3);
    *r = -(a1*B+b1*D+c1)/(a1*A+b1*C-q1);
    *xc = A*(*r)+B;
    *yc = C*(*r)+D;
    return 1;
    }
  }

/* get a circle tangent to a circle and two lines */
//得到切于一个圆和两条直线的圆
short __stdcall ccll(double x1, double y1, double r1, short q1,
      double a2, double b2, double c2, short q2,
      double a3, double b3, double c3, short q3,
      double rr[], double xxc[], double yyc[])
  {
  double A, B, C, D, G, H, I, delt;
  short  Sign;

  if( (Sign = parallel(a2,b2,a3,b3)) != 0 )
    {
    rr[0] = rr[1] = fabs(c2-Sign*c3)/2.0;
    return plc(a2,b2,(c2+Sign*c3)/2.0,x1,y1,r1+q1*rr[0],xxc,yyc);
    }
  else
    {
    A = (b3*q2-b2*q3)/(a2*b3-a3*b2);
    B = -(b3*c2-b2*c3)/(a2*b3-a3*b2);
    C = (a3*q2-a2*q3)/(a3*b2-a2*b3);
    D = -(a3*c2-a2*c3)/(a3*b2-a2*b3);
    G = A*A+C*C-1;
    H = 2*(A*(B-x1)+C*(D-y1)-q1*r1);
    I = (B-x1)*(B-x1)+(D-y1)*(D-y1)-r1*r1;
    delt = H*H-4*G*I;
    if( delt < 0 )
      return 0;
    else if( same(G,0.0) )
      {
      rr[0] = rr[1] = -I/H;
      xxc[0] = xxc[1] = A*rr[0]+B;
      yyc[0] = yyc[1] = C*rr[0]+D;
      return 1;
      }
    else
      {
      rr[0] = (-H+sqrt(delt))/(2*G);
      xxc[0] = A*rr[0]+B;
      yyc[0] = C*rr[0]+D;
      rr[1] = (-H-sqrt(delt))/(2*G);
      xxc[1] = A*rr[1]+B;
      yyc[1] = C*rr[1]+D;
      return 1;
      }
    }
  }

/* get a circle tangent to two circles and a line */
//得到切于两个圆的一条直线的圆
short __stdcall cccl(double x1, double y1, double r1, short q1,
      double x2, double y2, double r2, short q2,
      double a3, double b3, double c3, short q3,
      double rr[], double xxc[], double yyc[])
  {
  double A, B, C, D, G, H, I, delt;

  if( same_point(x1,y1,x2,y2) )
    {
    rr[0] = rr[1] = fabs(r1-r2)/2.0;
    return plc(a3,b3,c3-q3*rr[0],x1,y1,r1+q1*rr[0],xxc,yyc);
    }
  else if( same_coor(x1,x2) )
    {
    rr[0] = rr[1] = fabs(fabs(y1-y2)-r1*q1-r2*q2)/2.0;
    return plc(a3,b3,c3-q3*rr[0],x1,y1,r1+q1*rr[0],xxc,yyc);
    }
  else if( same_coor(y1,y2) )
    {
    rr[0] = rr[1] = fabs(fabs(x1-x2)-r1*q1-r2*q2)/2.0;
    return plc(a3,b3,c3-q3*rr[0],x1,y1,r1+q1*rr[0],xxc,yyc);
    }
  else
    {
    A = ((y2-y1)*q3-b3*(r1*q1-r2*q2))/(a3*(y2-y1)-b3*(x2-x1));
    B = (b3*(x1*x1-x2*x2+y1*y1-y2*y2+r2*r2-r1*r1)-2*(y2-y1)*c3)
   /(2*a3*(y2-y1)-2*b3*(x2-x1));
    C = (a3*(r1*q1-r2*q2)-(x2-x1)*q3)/(a3*(y2-y1)-b3*(x2-x1));
    D = (2*(x2-x1)*c3-a3*(x1*x1-x2*x2+y1*y1-y2*y2+r2*r2-r1*r1))
    /(2*a3*(y2-y1)-2*b3*(x2-x1));
    G = A*A+C*C-1;
    H = 2*(A*(B-x1)+C*(D-y1)-q1*r1);
    I = (B-x1)*(B-x1)+(D-y1)*(D-y1)-r1*r1;
    delt = H*H-4*G*I;
    if( delt < 0 )
      return 0;
    else if( same(G,0.0) )
      {
      rr[0] = rr[1] = -I/H;
      xxc[0] = xxc[1] = A*rr[0]+B;
      yyc[0] = yyc[1] = C*rr[0]+D;
      return 1;
      }
    else
      {
      rr[0] = (-H+sqrt(delt))/(2*G);
      xxc[0] = A*rr[0]+B;
      yyc[0] = C*rr[0]+D;
      rr[1] = (-H-sqrt(delt))/(2*G);
      xxc[1] = A*rr[1]+B;
      yyc[1] = C*rr[1]+D;
      return 2;
      }
    }
  }

/* get a circle tangent to three circles */
//得到切于三个圆的圆
short __stdcall cccc(double x1, double y1, double r1, short q1,
      double x2, double y2, double r2, short q2,
      double x3, double y3, double r3, short q3,
      double rr[], double xxc[], double yyc[])
  {
  double A, B, C, D, G, H, I, delt;

  if( same_point(x1,y1,x2,y2) )
    {
    rr[0] = rr[1] = fabs(r1-r2)/2.0;
    return pcc(x1,y1,r1+q1*rr[0],x3,y3,r3+q3*rr[0],xxc,yyc);
    }
  else if( same_point(x1,y1,x3,y3) )
    {
    rr[0] = rr[1] = fabs(r1-r3)/2.0;
    return pcc(x1,y1,r1+q1*rr[0],x2,y2,r2+q2*rr[0],xxc,yyc);
    }
  else if( same_point(x2,y2,x3,y3) )
    {
    rr[0] = rr[1] = fabs(r2-r3)/2.0;
    return pcc(x2,y2,r2+q2*rr[0],x1,y1,r1+q1*rr[0],xxc,yyc);
    }
  else
    {
    A = ((q1*r1-q2*r2)*(y3-y1)-(q1*r1-q3*r3)*(y2-y1))/
   ((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1));
    B = ((r1*r1-r2*r2-x1*x1+x2*x2-y1*y1+y2*y2)*(y3-y1)-
    (r1*r1-r3*r3-x1*x1+x3*x3-y1*y1+y3*y3)*(y2-y1))
   /(2*(x2-x1)*(y3-y1)-2*(x3-x1)*(y2-y1));
    C = ((q1*r1-q2*r2)*(x3-x1)-(q1*r1-q3*r3)*(x2-x1))/
   ((y2-y1)*(x3-x1)-(y3-y1)*(x2-x1));
    D = ((r1*r1-r2*r2-x1*x1+x2*x2-y1*y1+y2*y2)*(x3-x1)-
    (r1*r1-r3*r3-x1*x1+x3*x3-y1*y1+y3*y3)*(x2-x1))
   /(2*(y2-y1)*(x3-x1)-2*(y3-y1)*(x2-x1));
    G = A*A+C*C-1;
    H = 2*(A*(B-x1)+C*(D-y1)-q1*r1);
    I = (B-x1)*(B-x1)+(D-y1)*(D-y1)-r1*r1;
    delt = H*H-4*G*I;
    if( delt < 0 )
      return 0;
    else if( same(G,0.0) )
      {
      rr[0] = rr[1] = -I/H;
      xxc[0] = xxc[1] = A*rr[0]+B;
      yyc[0] = yyc[1] = C*rr[0]+D;
      return 1;
      }
    else
      {
      rr[0] = (-H+sqrt(delt))/(2*G);
      xxc[0] = A*rr[0]+B;
      yyc[0] = C*rr[0]+D;
      rr[1] = (-H-sqrt(delt))/(2*G);
      xxc[1] = A*rr[1]+B;
      yyc[1] = C*rr[1]+D;
      return 1;
      }
    }
  }
  
/**********************************************/
//交换
void __stdcall dswap(double *p1,double *p2)
{
  double a;
  a=*p1;
  *p1=*p2;
  *p2=a;
  return;
}

/*************************************************************************/
//判断一个点是否在矩形框中
int __stdcall PointInBox(double x, double y, double x1, double y1, double x2, double y2)
  {
  if( (x>=x1) && (x<=x2) && (y>=y1) && (y<=y2))
    return 1;
  return 0;
  }

/**********************************************************************/
//标准角转换
void __stdcall StandardAngle(double *angle)
  {
  int k;
  double ang = rtod(*angle);

  if( same_angle_with_eps(ang,0.0,0.001) || same_angle_with_eps(ang,360.0,0.001) )
    {
    *angle = 0.0;
    return;
    }
  k = (int)((*angle)/(2.0*PI));
  *angle = (*angle) - 2.0*PI*k;
  if( *angle < 0 )
    *angle = *angle + 2.0*PI;
  return;
  }

// added by Bossin 1996--6--29
//平面向柱面坐标转换
void __stdcall Polar(double *pt1, double angle, double dist, double *pt2)
{
   pt2[0] = pt1[0] + dist * cos(angle);
   pt2[1] = pt1[1] + dist * sin(angle);
   pt2[2] = pt1[2];
}
///////////added bu JLZHOU///////////
//得到两点连线与X轴的标准夹角(弧度制)
double __stdcall AngleLX(double *p1, double *p2)
{
	double a,b,c,ang;
	lpp(p1[X],p1[Y],p2[X],p2[Y],&a,&b,&c);
	ang = alx(a,b);
	StandardAngle(&ang);
	return ang;
}
//得到过两点直线的直接方程参数
void __stdcall GetLineABC(double x1,double y1,double x2,double y2,double *a,double *b,double *c)
{

  if( same_point(x1,y1,x2,y2)){
	 *a=1;
	 *b=0;
	 *c=-x1;
  }
  else
	 lpp(x1,y1,x2,y2,a,b,c);
}
//得到任意两点连线的斜率
double __stdcall get_lineangle(double x1,double y1,double x2,double y2)
{
  double a,b,c;

  GetLineABC(x1,y1,x2,y2,&a,&b,&c);
  return alx(a,b);
}
//得到两线段的位置状态
/* get the real intersect point of line segment (x1,y1),(x2,y2) with line segment (x3,y3) (x4,y4)
   return value: ( line segment 1 represent (x1,y1) (x2,y2) )
   0:  no intersect point, and the two line are parallel   //无交点平行
   1:  have one real  intersect point                      //有一个交点    
   2:  the intersect point in line segment 1,but not in line segment 2.  //交点在险段1上,但是不在线段2上
   3:  the intersect point in line segment 2,but not in line segment 1.  //和上面相反
   4:  the intersect point not in line segment 1,and not in line segment 2. //交点既不在线段1上,也不在线段2上
*/
short  __stdcall GetIntersectPt(double x1, double y1, double x2, double y2, double x3, double y3,
      double x4, double y4, double *x0, double *y0)
{
	if( ppppp(x1,y1,x2,y2,x3,y3,x4,y4,x0,y0) )
	{
		if(point_in_line(*x0, *y0,x1,y1,x2,y2) )
		{
			if(point_in_line(*x0, *y0,x3,y3,x4,y4) )
				return 1;
			else
				return 2;
		}
		else
		{
			if(point_in_line(*x0, *y0,x3,y3,x4,y4) )
				return 3;
			else
				return 4;

		}
	}
	return 0;
}