球状罗盘，第一种特征线，非均匀态用来做识别

本文链接：https://blog.youkuaiyun.com/qq_23158477/article/details/122763406

这段代码实现了一个三维空间中的球状罗盘，根据给定的参数调整方向。通过计算一系列射线与空间物体的交点，确定罗盘的最优位置。算法涉及射线投射、几何计算以及空间点的迭代更新，最终找到适应的罗盘中心点。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

/// <summary>
/// 球状罗盘，自适应方向
/// </summary>
/// <param name="p0">交接点</param>
/// <param name="v0">顶点</param>
/// <param name="angle">圆内射线夹角</param>
/// <param name="luopanangle">圆之间的夹角</param>
public void Draw3DZhiLines(double[] p0, double[] v0, double angle)
{

vtkDoubleArray v00array = vtkDoubleArray.New();
v00array.SetNumberOfComponents(3);
vtkDoubleArray v0array = vtkDoubleArray.New();
v0array.SetNumberOfComponents(3);
vtkDoubleArray aveDisarray = vtkDoubleArray.New();
aveDisarray.SetNumberOfComponents(1);
vtkDoubleArray intersect3Points = vtkDoubleArray.New();
intersect3Points.SetNumberOfComponents(9);

for (int kk = 0; kk < LuopanZhiNumber*2; kk++)
{
double[] v0_cur = new double[3];
if (kk < LuopanZhiNumber)
{
v0_cur[0] = p0[0] + (v0[0] - p0[0]) * Math.Cos(kk * Math.PI / 18) - (v0[1] - p0[1]) * Math.Sin(kk * Math.PI / 18);
v0_cur[1] = p0[1] + (v0[0] - p0[0]) * Math.Sin(kk * Math.PI / 18) + (v0[1] - p0[1]) * Math.Cos(kk * Math.PI / 18);

}
else
{
v0_cur[0] = p0[0] + (v0[0] - p0[0]) * Math.Cos(-(kk- LuopanZhiNumber) * Math.PI / 18) - (v0[1] - p0[1]) * Math.Sin(-(kk - LuopanZhiNumber) * Math.PI / 18);
v0_cur[1] = p0[1] + (v0[0] - p0[0]) * Math.Sin(-(kk - LuopanZhiNumber) * Math.PI / 18) + (v0[1] - p0[1]) * Math.Cos(-(kk - LuopanZhiNumber) * Math.PI / 18);

}
v0_cur[2] = v0[2];
double[] p2 ;
vtkDoubleArray parray = vtkDoubleArray.New();
parray.SetNumberOfComponents(3);
// int[,] multiDimensionalArray1 = new int[2, 3];
for (int i = 0; i < raynumbers; i++)//发射raynumbers条射线,第一条与Z面平行
{
if (i == 0)//第一次循环
{
p2 = GetP2(p0, v0_cur, rayLengthRadio * rayLength* tuoluobanjingRadio);
parray.InsertNextTuple3(p2[0], p2[1], p2[2]);
}
else
{
p2 = GetP3(v0_cur, p0, parray.GetTuple3(0), 2 * i * Math.PI / raynumbers);
parray.InsertNextTuple3(p2[0], p2[1], p2[2]);
}

}

//用线来连接显示测试

//计算下一个法线反向点位置，递归到下一个p0

IntPtr p0str = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(double)) * 3);
Marshal.Copy(p0, 0, p0str, 3);
vtkDoubleArray intersectPointsposition = vtkDoubleArray.New();
intersectPointsposition.SetNumberOfComponents(3);
for (int i = 0; i < parray.GetNumberOfTuples(); i++)
{
vtkPoints intersectpoints = vtkPoints.New();
double[] p_cur = new double[3];
p_cur = parray.GetTuple3(i);
IntPtr p_curstr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(double)) * 3);
Marshal.Copy(p_cur, 0, p_curstr, 3);
tree.IntersectWithLine(p0str, p_curstr, intersectpoints, null);
if (intersectpoints.GetNumberOfPoints() > 0)
{

intersectPointsposition.InsertNextTuple3(intersectpoints.GetPoint(0)[0], intersectpoints.GetPoint(0)[1], intersectpoints.GetPoint(0)[2]);
}
}
double[] v00 = new double[3];//下一个p0点
if (intersectPointsposition.GetNumberOfTuples() > 0)
{
for (int j = 0; j < intersectPointsposition.GetNumberOfTuples(); j++)
{
v00[0] += intersectPointsposition.GetTuple3(j)[0];
v00[1] += intersectPointsposition.GetTuple3(j)[1];
v00[2] += intersectPointsposition.GetTuple3(j)[2];
}
v00[0] = v00[0] / intersectPointsposition.GetNumberOfTuples();
v00[1] = v00[1] / intersectPointsposition.GetNumberOfTuples();
v00[2] = v00[2] / intersectPointsposition.GetNumberOfTuples();
double avedistance = 0;
for (int mm = 0; mm < intersectPointsposition.GetNumberOfTuples(); mm++)
{
avedistance += Math.Sqrt((v00[0] - intersectPointsposition.GetTuple3(mm)[0]) * (v00[0] - intersectPointsposition.GetTuple3(mm)[0]) + (v00[1] - intersectPointsposition.GetTuple3(mm)[1]) * (v00[1] - intersectPointsposition.GetTuple3(mm)[1]) + (v00[2] - intersectPointsposition.GetTuple3(mm)[2]) * (v00[2] - intersectPointsposition.GetTuple3(mm)[2]));
}
avedistance = avedistance / intersectPointsposition.GetNumberOfTuples();
v00array.InsertNextTuple3(v00[0], v00[1], v00[2]);
v0array.InsertNextTuple3(v0_cur[0], v0_cur[1], v0_cur[2]);
aveDisarray.InsertNextTuple1(avedistance);
intersect3Points.InsertNextTuple9(intersectPointsposition.GetTuple3(0)[0], intersectPointsposition.GetTuple3(0)[1], intersectPointsposition.GetTuple3(0)[2], intersectPointsposition.GetTuple3(1)[0], intersectPointsposition.GetTuple3(1)[1], intersectPointsposition.GetTuple3(1)[2], intersectPointsposition.GetTuple3(2)[0], intersectPointsposition.GetTuple3(2)[1], intersectPointsposition.GetTuple3(2)[2]);

}
else
{
return;//表示这个点肯定在外侧，或者画圆的半径小于空间物体。
}
}

double[] aves = new double[aveDisarray.GetNumberOfTuples()];
for (int i = 0; i < aves.Length; i++)
{
aves[i] = aveDisarray.GetTuple1(i);
}
var minindex = aves.Min();
int minindexid =-1;
for (int i = 0; i < aves.Length; i++)
{
if (minindex == aves[i])
{
minindexid = i;
break;
}

}
if(minindexid != -1)
{
double[] p00 = new double[3];
p00[0] = v00array.GetTuple3(minindexid)[0] - (v0array.GetTuple3(minindexid)[0] - v00array.GetTuple3(minindexid)[0]);
p00[1] = v00array.GetTuple3(minindexid)[1] - (v0array.GetTuple3(minindexid)[1] - v00array.GetTuple3(minindexid)[1]);
p00[2] = v00array.GetTuple3(minindexid)[2] - (v0array.GetTuple3(minindexid)[2] - v00array.GetTuple3(minindexid)[2]);
var A = intersect3Points.GetTuple9(minindexid)[1] * (intersect3Points.GetTuple9(minindexid)[5] - intersect3Points.GetTuple9(minindexid)[8])+ intersect3Points.GetTuple9(minindexid)[4]*(intersect3Points.GetTuple9(minindexid)[8]- intersect3Points.GetTuple9(minindexid)[2])+ intersect3Points.GetTuple9(minindexid)[7]*(intersect3Points.GetTuple9(minindexid)[2]- intersect3Points.GetTuple9(minindexid)[5]);
var B = intersect3Points.GetTuple9(minindexid)[2] * (intersect3Points.GetTuple9(minindexid)[2] - intersect3Points.GetTuple9(minindexid)[6]) + intersect3Points.GetTuple9(minindexid)[5] * (intersect3Points.GetTuple9(minindexid)[6] - intersect3Points.GetTuple9(minindexid)[0]) + intersect3Points.GetTuple9(minindexid)[8] * (intersect3Points.GetTuple9(minindexid)[0] - intersect3Points.GetTuple9(minindexid)[3]);
var C = intersect3Points.GetTuple9(minindexid)[0] * (intersect3Points.GetTuple9(minindexid)[4] - intersect3Points.GetTuple9(minindexid)[7]) + intersect3Points.GetTuple9(minindexid)[3] * (intersect3Points.GetTuple9(minindexid)[7] - intersect3Points.GetTuple9(minindexid)[1]) + intersect3Points.GetTuple9(minindexid)[6] * (intersect3Points.GetTuple9(minindexid)[1] - intersect3Points.GetTuple9(minindexid)[4]);
var D = -intersect3Points.GetTuple9(minindexid)[0] * (intersect3Points.GetTuple9(minindexid)[4] * intersect3Points.GetTuple9(minindexid)[8] - intersect3Points.GetTuple9(minindexid)[7] * intersect3Points.GetTuple9(minindexid)[5]) - intersect3Points.GetTuple9(minindexid)[3] * (intersect3Points.GetTuple9(minindexid)[7] * intersect3Points.GetTuple9(minindexid)[2] - intersect3Points.GetTuple9(minindexid)[1] * intersect3Points.GetTuple9(minindexid)[8]) - intersect3Points.GetTuple9(minindexid)[6] * (intersect3Points.GetTuple9(minindexid)[1]* intersect3Points.GetTuple9(minindexid)[5]- intersect3Points.GetTuple9(minindexid)[4]* intersect3Points.GetTuple9(minindexid)[2]);
// pointsfangchengparams.InsertNextTuple9(centerposition[0], centerposition[1], centerposition[2], A, B, C, D, 0, 0);
pointsfangchengparams.InsertNextTuple9(v00array.GetTuple3(minindexid)[0], v00array.GetTuple3(minindexid)[1], v00array.GetTuple3(minindexid)[2],A,B,C,D,0,0);

vtkSphereSource sperep1 = vtkSphereSource.New();
sperep1.SetRadius(0.005 * pointSphereRadiusRadio);
sperep1.SetPhiResolution(20);
sperep1.SetThetaResolution(20);
vtkPolyDataMapper spheremapper = vtkPolyDataMapper.New();
spheremapper.SetInput(sperep1.GetOutput());
vtkActor sperep1actor = vtkActor.New();
sperep1actor.SetMapper(spheremapper);
sperep1actor.SetPosition(v00array.GetTuple3(minindexid)[0], v00array.GetTuple3(minindexid)[1], v00array.GetTuple3(minindexid)[2]);
sperep1actor.GetProperty().SetColor(0.2, 0.6, 0.1);
//3D 质点id显示
vtkVectorText idtext = vtkVectorText.New();
idtext.SetText(((pointsfangchengparams.GetMaxId()-8)/9).ToString());
vtkPolyDataMapper textmapper = vtkPolyDataMapper.New();
textmapper.SetInput(idtext.GetOutput());
vtkFollower textActor = vtkFollower.New();
textActor.SetMapper(textmapper);
textActor.SetScale(0.005,0.005,0.005);
textActor.AddPosition(v00array.GetTuple3(minindexid)[0], v00array.GetTuple3(minindexid)[1], v00array.GetTuple3(minindexid)[2]);
textActor.GetProperty().SetColor(0.6,0.6,0.2);
textActor.RotateWXYZ(Math.PI/4, Math.PI / 4, Math.PI / 4, Math.PI / 4);
render.AddActor(sperep1actor);
render.AddActor(textActor);
Draw3DZhiLines(p00, v00array.GetTuple3(minindexid), angle);
}