xmathlib.cpp

矩阵和初等几何常用算法

#include "stdafx.h"
#include <math.h>
#include "xmathlib.h"
#define X 0
#define Y 1 
//// 直线方程为：ax+by+c=0
const double GBL_DistTol=0.005;
#define PI 3.1415926535897925
extern "C" void MDS_addDim(int ii,void *d,void *f,long * b, long * c);
void MDS_addDim(int ii,void *d,void *f,long * b, long * c)
{
	return ; 
}
/* check if the two angles are the same within GBL_AngTol
   return 1 same
   return 0 not_same */
short __stdcall same_angle(double value1, double value2)
  {
  if( fabs(value1 - value2) < 0.2/*GBL_AngTol*/ )
    return 1;
  else
    return 0;
  }
short __stdcall same_angle_with_eps(double value1, double value2,double eps)
  {
  if( fabs(value1 - value2) < eps)/*GBL_AngTol*/ 
    return 1;
  else
    return 0;
  }
/* find if angle is between angle_1 and angle_2 */
short __stdcall in_angle(double angle, double angle_1, double angle_2)
  {
  if( same_angle(angle_1,angle) || same_angle(angle_2,angle) )
    return 1;
  if( (angle_1 - angle) * (angle_2 - angle) * (angle_2 - angle_1) < 0.0 )
    return 2;
  else
    return 0;
  }
short __stdcall in_angle_with_eps(double angle, double angle_1, double angle_2,double eps)
  {
  if( same_angle_with_eps(angle_1,angle,eps) || same_angle_with_eps(angle_2,angle,eps) )
    return 1;
  if( (angle_1 - angle) * (angle_2 - angle) * (angle_2 - angle_1) < 0.0 )
    return 2;
  else
    return 0;
  }
/* get the distance of two points (x1,y1) and (x2,y2) *///得到两点距离
double __stdcall dpp(double x1, double y1, double x2, double y2)
  {
  return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
  }

/* get a line(a,b,c) from two points (x1,y1) (x2,y2) */
//根据两点得到直线(a,b,c)，其中a总为正，除非a等于零
void  __stdcall lpp(double x1,double y1,double x2,double y2,
    double *a,double *b,double *c)
  {
  *a = y2 - y1;
  *b = x1 - x2;
  *c = sqrt((*a)*(*a) + (*b)*(*b));

  if(*c == 0)
  {
   *a = 0;
   *b = -1;
   return;
  }

  *a = (*a) / (*c);
  *b = (*b) / (*c);
  *c = - (*a) * x1 - (*b) * y1;
  }
/*
void  __stdcall lpp(double x1,double y1,double x2,double y2,
    double *a,double *b,double *c)
{
     *a = y2 - y1;
     *b = x1 - x2;
     *c = sqrt((*a)*(*a) + (*b)*(*b));
	 
     //或
	 //*c = -((*a)*x1+(*b)*y1);
     if(*c == 0)
	 {
      *a = 0;
      *b = 1;
      return;
	 }
      *a = (*a) / (*c);
	  if(*a<0)
	  {
		 *a=-*a;
         *b = -(*b) / (*c);
         *c = -((y2-y1) * x1 - (x2-x1) * y1);
	  }
	  else if(*a==0)
	  {
		 *b = (*b) / (*c);
		 if(*b<0)
		 {
			 *b=-*b;
			 *c = -((*a) * x1 - (*b) * y1);
		 }
		 else if(*b==0)
		 {
		    AfxMessageBox("输入坐标有误!");
		 }
		 else
		 {
            *c = (*a) * x1 - (*b) * y1;
		 }
	  }
	  else
	  {
		  *b = (*b) / (*c);
          *c = (y2-y1) * x1 - (x2-x1) * y1;
	  }
  }
*/
/* get perpendicular point(xp,yp) from a known point(x,y) to a known//得到已知点到已知直线的垂足
   line(a,b,c) */
void __stdcall ppln(double x, double y, double a, double b, double c,
     double *xp,double *yp)
  {
  double        dist;

  dist = a * x + b * y + c;
  *xp = x - dist * a;
  *yp = y - dist * b;
  }

/* get the distance from a point to a line *///得到点到直线的距离
double __stdcall dpl(double a, double b, double c, double x, double y)
  {
  return a*x + b*y + c;
  }

/* get the intersective point(x,y) of two lines(a1,b1,c1)(a2,b2,c2)//得到两直线交点
   return 1: intersect
   return 0: not_intersect */
short __stdcall pll(double a1,double b1,double c1,double a2,double b2,double c2,
     double *x,double *y)
  {
  double d;

  d = a1*b2 - b1*a2;
  if( fabs(d) > 1e-5 )
    {
    *x = (-c1*b2 + c2*b1)/d;
    *y = (-a1*c2 + a2*c1)/d;
    return 1;
    }
  else
    {
    *x = BIG;
    *y = BIG;
    return 0;
    }
  }
/* Get right a point whose distance to a known point is d, and these two points are
on a same line */
//得到一点，这点在已知点的右边且离已知点距离为d，它们在同一条直线上
void __stdcall plpd(double a, double b, double xp, double yp, double d,
     double *x, double *y)
  {
   *x = xp - d*b;
   *y = yp + d*a;
   }

/* Get left a point whose distance to a known point is d, and these two points are
on a same line */
//得到一点，这点在已知点的左边且离已知点距离为d，它们在同一条直线上
void __stdcall plpd1(double a, double b, double xp, double yp, double d,
     double *x, double *y)
  {
   *x = xp +d*b;
   *y = yp -d*a;
   }
/* Get a parallel line from a known line with a known distance:d */
//得到距离已知直线距离为d的平行线
void __stdcall lld(double c, double d, double *c1)
  {
  *c1=c-d;
  }

/* Get a line through a knonw point, and has a known angle with a known line */
//得到与已知直线呈一定夹角且通过某一点的直线
void  __stdcall lpla(double a1, double b1, double xp, double yp, double alpha,
     double *a, double *b, double *c)
  {
   double ca, sa;

   ca = cos(alpha);
   sa = sin(alpha);
   *a = a1*ca - b1*sa;
   *b = a1*sa + b1*ca;
   *c = -((*a) * xp + (*b) * yp);
   }


/* Through a known point, make a line to be vertical to a known line */
//求垂线
void __stdcall lpln(double a1, double b1, double xp, double yp, double *a, double *b,
     double *c)
  {
  *a = b1;
  *b = -a1;
  *c=-((*a) * xp + (*b) * yp);
  }

/* check if the point is on the line segment//检查点是否在直线上
   return 1:  on
   return 0:  not_on */
short __stdcall point_in_line(double x, double y, double xs, double ys, double xe,
          double ye)
  {
  double        dist, a, b, c;

  if( same_point(x,y,xs,ys) || same_point(x,y,xe,ye) )
    return 1;
  lpp(xs,ys,xe,ye,&a,&b,&c);
  dist = dpl(a,b,c,x,y);
  if( (x - xs) * (x - xe) <= (GBL_DistTol * GBL_DistTol) &&
      (y - ys) * (y - ye) <= (GBL_DistTol * GBL_DistTol) &&
      fabs(dist) < GBL_DistTol )
    return 2;
  else
    return 0;
  }

/* check if the two values are the same within the GBL_DistTol//检查两个值是否足够接近
   return 1: same
   return 0: not_same */
short __stdcall same_coor(double value1, double value2)
  {
  if( fabs(value1 - value2) < GBL_DistTol )
    return 1;
  else
    return 0;
  }

short __stdcall same_coor_with_eps(double value1, double value2,double eps)//检查两个值是否在误差之内
  {
	if( eps == 0)
		eps = GBL_DistTol ;
  if( fabs(value1 - value2) < eps )
    return 1;
  else
    return 0;
  }
//检查两点坐标值是否在误差之内
short __stdcall same_point_with_eps( double x1, double y1,double x2, double y2,double eps )
{
  if( same_coor_with_eps(x1,x2,eps) && same_coor_with_eps(y1,y2,eps) )
    return 1;
  else
    return 0;
}

/* check if the two values are the same within 1.0e-4//检查两个值是否小于万分之一
   return 1: same
   return 0: not_same */
short __stdcall same(double value1, double value2)
  {
  if( fabs(value1 - value2) <= 1.0e-3 )
    return 1;
  else
    return 0;
  }
//检查两个点是否足够接近
/* check if the two points are the same  within GBL_DistTol
   return 1 same
   return 0 not_same */
short __stdcall same_point(double x1, double y1, double x2, double y2)
  {
  if( same_coor(x1,x2) && same_coor(y1,y2) )
    return 1;
  else
    return 0;
  }

//得到两点之间连线与X轴的夹角
/* get the angle between a line(through two points) and X-axis */
double __stdcall points_to_angle(double x1, double y1, double x2, double y2)
  {
  double        angle;

  if ( fabs(x1 - x2) < GBL_DistTol )
    {
    if ( fabs(y1 - y2) < GBL_DistTol )
      angle = 0.0;
    else
      if (y1 < y2)
         angle = 90.0;
      else
         angle = 270.0;
    }
  else
    {
       angle = atand( (y2 - y1) / (x2 - x1));
    if (x1 > x2)
       angle = angle + 180.0;
    else
      if (y2 < y1)
       angle = 360.0 + angle;
    }
  if ( same_angle(angle,360.0) )
        angle = 0.0;
  return angle;
  }
/* Get an angle between a line of two points to X-axis */
//得到两点连线与X轴的夹角
double __stdcall appx(double x1, double y1, double x2, double y2)
  {
   double a, b, c;

   lpp( x1, y1, x2, y2, &a, &b, &c);
   return alx(a,b);
   }

/* Get midddle vertical line from two known points */
//得到垂直平分线
void __stdcall lppn(double x1, double y1, double x2, double y2, double *a, double *b,
     double *c)
  {
   lpp(y2, x1, y1, x2, a, b, c);
   *c = -((*a)*(x1+x2) + (*b)*(y1+y2)) / 2;
   }


/* Get a line through a known point parallel to a known line */
//得到平行于已知直线过某点的直线
void __stdcall lplp(double xp, double yp, double a, double b, double *c)
  {
  *c = -(a*xp + b*yp);
  }
//得到平行于已知直线(a1,b1,c1)，偏距为d的另外一条直线(a,b,c)//左偏
//斜率大于零，向左上偏移，或者斜率小于零，向左下偏移
void __stdcall ldl(double a1,double b1,double c1,double *a, double *b, double *c,double d)
{
  *a=a1;
  *b=b1;
  *c=c1+d*sqrt(a1*a1+b1*b1);
}
//得到平行于已知直线(a1,b1,c1)，偏距为d的另外一条直线(a,b,c)//右偏
//斜率大于零，向右下偏移，或者斜率小于零，向右上偏移
void __stdcall ldl1(double a1,double b1,double c1,double *a, double *b, double *c,double d)
{
  *a=a1;
  *b=b1;
  *c=c1-d*sqrt(a1*a1+b1*b1);
}
//得到平行于已知直线(a1,b1,c1)，偏距为d的另外一条直线(a,b,c)//斜率大于零，向右下偏移

//已知两点，得到两点连线偏距为d的另外一条直线，向左偏移
void __stdcall ppdl(double x1, double y1,double x2,double y2,double d,double *a,double *b,double *c)
{
    lpp(x1,y1,x2,y2,a,b,c);
    double a1=*a;
	double b1=*b;
	double c1=*c;
    ldl(a1,b1, c1, a,  b,  c, d);
}
//已知两点，得到两点连线偏距为d的另外一条直线(a,b,c)//向右偏移
void __stdcall ppdl1(double x1, double y1,double x2,double y2,double d,double *a,double *b,double *c)
{
    lpp(x1,y1,x2,y2,a,b,c);
    double a1=*a;
	double b1=*b;
	double c1=*c;
    ldl1(a1,b1, c1, a,  b,  c, d);
}

/* Determine a point is on which side of a line */
//确定一个点在直线的哪一边
short __stdcall side_of(double a, double b, double c, double x, double y)
  {
  double dist;

  dist = dpl(a,b,c,x,y);
  if( same_coor_with_eps(dist,0.0,0.0001))
    return 0;
  else if( dist > 0.0 )
    return 1;
  else
    return -1;
  }

/* get the Sign of a real value: + or - */
//得到一个实数的正负
short __stdcall Sign(double x)
  {
  short   result = 0;

  if( x != 0.0 )
    result = (short)(fabs(x) / x);
  return result;
  }


short __stdcall point_in_arc(double x, double y, double xc, double yc,
         double xs, double ys, double xe, double ye)
  {
  double        ang  = points_to_angle(xc,yc,x,y),
                ang1 = points_to_angle(xc,yc,xs,ys),
                ang2 = points_to_angle(xc,yc,xe,ye),
                dist = dpp(xc,yc,x,y),
                rad =  dpp(xc,yc,xs,ys);
  short         in_ang, same_dist;

  in_ang = in_angle(ang,ang1,ang2);
  same_dist = same_coor(dist, rad);
  if( in_ang == 1 && same_dist)
    return 1;
  if( in_ang == 2 && same_dist)
    return 2;
  else
    return 0;
  }
short __stdcall point_in_arc_with_eps(double x, double y, double xc, double yc,
         double xs, double ys, double xe, double ye,double eps)
  {
  double        ang  = points_to_angle(xc,yc,x,y),
                ang1 = points_to_angle(xc,yc,xs,ys),
                ang2 = points_to_angle(xc,yc,xe,ye),
                dist = dpp(xc,yc,x,y),
                rad =  dpp(xc,yc,xs,ys);
  short         in_ang, same_dist;

  in_ang = in_angle_with_eps(ang,ang1,ang2,eps);
  same_dist = same_coor(dist, rad);
  if( in_ang == 1 && same_dist)
    return 1;
  if( in_ang == 2 && same_dist)
    return 2;
  else
    return 0;
  }

short __stdcall point_on_circle(double x, double y, double cenx, double ceny, double radius)
  {
  double dist;
  short  same_dist;

  dist = dpp(x, y, cenx, ceny);
  same_dist = same_coor(dist, radius);
  if (same_dist)
    return 1;
  else
    return 0;
  }
//使一个点绕另个点旋转一个角度
/* rotate point(x2,y2) an angle against point(x1,y1)*/
void __stdcall rotate(double x1, double y1, double x2, double y2, double angle,
       double x[])
  {
  double        a[1][3];
  double        b[3][3];
  double        c[1][3];
  double        d,      e;
  register int  i,      j,      k;

  d = sind(angle);
  e = cosd(angle);
  a[0][0] = x1;
  a[0][1] = y1;
  a[0][2] = 1.0;