3D空间位置计算 — 求空间线段上的某一目标点坐标

  • Post author:
  • Post category:其他



空间内,已知一条直线和该直线上的两个点,根据距离求该直线上另一目标点的坐标。

【1】构造用到的三维空间点结构体 Point3d_s

typedef struct Point3d_s
{
    int x;
    int y;
    int z;
}Point3d_s;

【2】用到的求一元二次方程解

typedef struct Ret2ds
{
    float x1;
    float x2;
}Ret2ds;

/***********************************************************************************************************\
	一元二次方程的根
	a*x*x + b*x + c = 0
************************************************************************************************************/
Ret2ds solutionOfSquareEquation(double a, double b, double c)
{
	Ret2ds result;
	double delta;

	delta = b * b - 4 * a * c;
	if (delta > 0)
	{
		result.x1 = (-b + sqrt(delta)) / (2 * a);
		result.x2 = (-b - sqrt(delta)) / (2 * a);
		printf("该一元二次方程有两个解,x1=%lf,	x2=%lf\n", x1, x2);
	}
	else if (delta == 0)
	{
		result.x1 = -b / (2 * a);
		result.x2 = result.x1;
		printf("该一元二次方程有唯一解,x1=x2=%lf\n", x1);
	}
	else
	{
		//printf("NALL.\n");
		printf("该一元二次方程无解\n");
	}


	return result;
}

【3】根据已知空间直线,求该直线上的目标点


/***********************************************************************************************************\
	一条直线先后经过A、B两点,求线上第三点C,使BC距离为L
	@A:射线起点
	@B:射线上一点
	@Len:BC距离
************************************************************************************************************/
Point3d_s findDistancedPointOnLine(Point3d_s A, Point3d_s B, double Len)
{
	Point3d_s resultPoint;
	Ret2ds ret;
	double NORM;
	double eX, eY, eZ;
	double K;//要算出这个K值。决定C的向量的长度的系数,C的坐标为(K*eX,K*eY,K*eZ)//
	double a, b, c;


	//直线基向量,A->B,A到B。
	NORM = sqrt((A.x - B.x) * (A.x - B.x) + (A.y - B.y) * (A.y - B.y) + (A.z - B.z) * (A.z - B.z));
	eX = (B.x - A.x) / NORM;
	eY = (B.y - A.y) / NORM;
	eZ = (B.z - A.z) / NORM;

	//printf("eX= %f\neY= %f\neZ= %f\n", eX, eY, eZ);


	//BC用上述参数表示,得到欧式距离等式,akk + bk + c = 0,(x==k),求K。//
	a = (eX * eX + eY * eY + eZ * eZ);
	b = -2 * (eX * B.x + eY * B.y + eZ * B.z);
	c = B.x * B.x + B.y * B.y + B.z * B.z - Len * Len;

	ret = solutionOfSquareEquation(a, b, c);	//方程两根,需要筛选。计算长度的系数,只能是大于零的标量。至于点的向量的方向由基向量的正负决定。
	K = ret.x1 > 0 ? ret.x1 : ret.x2;

	//printf("k1= %f,	k2= %f\n", ret.x1, ret.x2);
	//printf("K= %f\n", K);

	resultPoint.x = K * eX;
	resultPoint.y = K * eY;
	resultPoint.z = K * eZ;
	printf("findDistancedPointonLine= (%f, %f, %f)\n", K * eX, K * eY, K * eZ);

	return resultPoint;
}



版权声明:本文为zaibeijixing原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。