private Geodetic_Point Direct(_3D_Point XYZ)
{
double e_2 = ep.data.e_2,
e2_2 = ep.data.e2_2,
a = ep.data.a,
b = ep.data.b;
double r = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y)),
u0 = Math.Atan(XYZ.Z / r * Math.Sqrt(1 + e2_2)),
tgB = (XYZ.Z + b * e2_2 * Math.Sin(u0) * Math.Sin(u0)) /
(r - a * e_2 * Math.Cos(u0) * Math.Cos(u0)),
B = Math.Atan(XYZ.Z / r + a * e_2 * tgB / r / Math.Sqrt(1 + (1 - e_2) * tgB * tgB));
_3D_Point blh = new _3D_Point();
//blh.X = Angle.ChangeToAngle(B).ChangeToDouble();
blh.X = B;
//var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
//blh.Y = l.ChangeToDouble();
blh.Y = Auxiliary.Get_L(XYZ.X, XYZ.Y);
double W = Auxiliary.GetW(e_2, B),
N = a / W;
double h1 = XYZ.Z - N * (1 - e_2) * Math.Sin(B),
h2 = r - N * Math.Cos(B);
double h = Math.Sqrt(h1 * h1 + h2 * h2);
if (h1 / Math.Sin(B) < 0 || h2 / Math.Cos(B) < 0)
{
h = -h;
}
blh.Z = h;
return(new Geodetic_Point(false, blh.X, blh.Y, blh.Z));
}
/// <summary>
/// 获得点位精度
/// </summary>
/// <param name="distance">摄影距离，单位与物方坐标相同</param>
/// <returns></returns>
public string GetAccuracy(double distance)
{
if (hasKnown)
{
List <double> rs = new List <double>();
List <_3D_Point> ddxyz = new List <_3D_Point>();
for (int i = 0; i < PassOList.Count; i++)
{
_3D_Point dxyz = PassOList[i].pos - KnownOList[i].pos;
ddxyz.Add(dxyz);
double r = _3D_Point.Get_Norm(dxyz, new _3D_Point());
rs.Add(r);
}
double sum = 0;
rs.ForEach(r => sum += r * r);
double a = Math.Sqrt(sum / rs.Count);
return("dr=" + a + ",点位精度:1/" + (distance / a).ToFormatString(0));
}
else
{
return(string.Empty);
}
}
private Geodetic_Point Iteration(_3D_Point XYZ)
{
_3D_Point blh = new _3D_Point();
//var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
blh.Y = Auxiliary.Get_L(XYZ.X, XYZ.Y);
double dis = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
double t0 = XYZ.Z / dis,
p = ep.data.c * ep.data.e_2 / dis,
k = 1 + ep.data.e2_2,
t1 = t0,
t2 = t0;
do
{
t1 = t2;
t2 = t0 + p * t1 / Math.Sqrt(k + t1 * t1);
}while (Math.Abs(Angle.ChangeToAngle(t2 - t1).Miao) > 0.0001);
blh.X = Angle.ChangeToAngle(t2 = Math.Atan(t2)).ChangeToRad();
double W = Auxiliary.GetW(ep.data.e_2, t2),
N = ep.data.a / W;
double h1 = XYZ.Z - N * (1 - ep.data.e_2) * Math.Sin(t2),
h2 = dis - N * Math.Cos(t2);
double h = Math.Sqrt(h1 * h1 + h2 * h2);
if (h1 / Math.Sin(t2) < 0 || h2 / Math.Cos(t2) < 0)
{
h = -h;
}
blh.Z = h;
return(new Geodetic_Point(false, blh.X, blh.Y, blh.Z));
}
/// <summary>
/// 空间直角坐标到大地坐标
/// </summary>
/// <param name="XYZ">直接坐标</param>
/// <param name="mode">计算模式，默认为0（迭代法），其他为直接法</param>
/// <returns>大地坐标</returns>
public Geodetic_Point Xyz2Blh(_3D_Point XYZ, int mode = 0)
{
if (mode == 0)
{
return(Iteration(XYZ));
}
else
{
return(Direct(XYZ));
}
}
/// <summary>
/// 大地坐标到空间直角坐标
/// </summary>
/// <param name="BLH">大地坐标</param>
/// <returns>直角坐标</returns>
public _3D_Point Blh2Xyz(Geodetic_Point BLH)
{
double W = Auxiliary.GetW(ep.data.e_2, BLH.Br);
double V = Auxiliary.GetV(ep.data.e2_2, BLH.Br);
double N = ep.data.a / W;
_3D_Point p = new _3D_Point();
p.X = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Cos(BLH.Lr);
p.Y = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Sin(BLH.Lr);
p.Z = (N * (1 - ep.data.e_2) + BLH.H) * Math.Sin(BLH.Br);
return(p);
}
public void ReadData(string path1, string path2)//读取像方数据文件,控制点，加密点
{
//StreamReader sr = new StreamReader(path1);
var s = File.ReadAllLines(path1, Encoding.Default);
oCount = s.Length;
var ss = Regex.Split(s[0], " +");
Count = (ss.Length - 4) / 2;
for (int i = 0; i < Count; i++)
{
oIPoints.Add(new List <_2D_Point>());
iPoints.Add(new List <_2D_Point>());
}
for (int i = 0; i < oCount; i++)
{
ss = Regex.Split(s[i], " +");
pNames.Add(ss[0]);
for (int j = 0; j < Count; j++)
{
_2D_Point p = new _2D_Point();
p.X = double.Parse(ss[1 + 2 * j]);
p.Y = double.Parse(ss[2 + 2 * j]);
oIPoints[j].Add(p);
}
_3D_Point p3 = new _3D_Point();
p3.X = double.Parse(ss[1 + 2 * Count]);
p3.Y = double.Parse(ss[2 + 2 * Count]);
p3.Z = double.Parse(ss[3 + 2 * Count]);
oPoints.Add(p3);
}
s = File.ReadAllLines(path2, Encoding.Default);
pCount = s.Length;
for (int i = 0; i < pCount; i++)
{
ss = Regex.Split(s[i], " +");
pINames.Add(ss[0]);
for (int j = 0; j < Count; j++)
{
_2D_Point p = new _2D_Point();
p.X = double.Parse(ss[1 + 2 * j]);
p.Y = double.Parse(ss[2 + 2 * j]);
iPoints[j].Add(p);
}
}
}
/// <summary>
/// 默认构造函数(点名为空字符，坐标为原点)
/// </summary>
public ObjectData()
{
pos = new _3D_Point();
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="name">点名</param>
/// <param name="point">坐标</param>
public ObjectData(string name, _3D_Point point)
{
Name = name;
pos = point;
}
/// <summary>
/// 默认构造函数(点名为空字符，坐标为原点)
/// </summary>
public ObjectData()
{
pos = new _3D_Point();
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="name">点名</param>
/// <param name="point">坐标</param>
public ObjectData(string name, _3D_Point point)
{
Name = name;
pos = point;
}
/// <summary>
/// 前方交会
/// </summary>
/// <returns></returns>
public List <OData> ForwardForcus()
{
List <List <_3D_Point> > points = new List <List <_3D_Point> >();
for (int i = 0; i < outE.Count - 1; i++)
{
points.Add(new List <_3D_Point>());
var R1 = outE[i].GetR();
var R2 = outE[i + 1].GetR();
List <_3D_Point> uvw1 = new List <_3D_Point>(),
uvw2 = new List <_3D_Point>();
foreach (var ip in PassIList[i])
{
var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
var uvw = R1 * xyf;
uvw1.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
}
foreach (var ip in PassIList[i + 1])
{
var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
var uvw = R2 * xyf;
uvw2.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
}
double Xs2 = outE[i + 1].Spos.X, Xs1 = outE[i].Spos.X,
Ys2 = outE[i + 1].Spos.Y, Ys1 = outE[i].Spos.Y,
Zs2 = outE[i + 1].Spos.Z, Zs1 = outE[i].Spos.Z,
Bu = Xs2 - Xs1, Bv = Ys2 - Ys1, Bw = Zs2 - Zs1;
for (int j = 0; j < uvw1.Count; j++)
{
double w2 = uvw2[j].Z,
v2 = uvw2[j].Y,
u2 = uvw2[j].X,
u1 = uvw1[j].X,
v1 = uvw1[j].Y,
w1 = uvw1[j].Z,
N1 = (Bu * w2 - Bw * u2) / (u1 * w2 - u2 * w1),
N2 = (Bu * w1 - Bw * u1) / (u1 * w2 - u2 * w1);
var UVW1 = N1 * uvw1[j];
var UVW2 = N2 * uvw2[j];
double X = (Xs1 + Xs2 + UVW1.X + UVW2.X) / 2,
Y = (Ys1 + Ys2 + UVW1.Y + UVW2.Y) / 2,
Z = (Zs1 + Zs2 + UVW1.Z + UVW2.Z) / 2;
points[i].Add(new _3D_Point(X, Y, Z));
}
}
List <_3D_Point> pointList = new List <_3D_Point>();
for (int i = 0; i < points[0].Count; i++)
{
_3D_Point sum = new _3D_Point();
for (int j = 0; j < points.Count; j++)
{
sum += points[j][i];
}
pointList.Add(sum / points.Count);
}
//points[0].ForEach(p => p.Set_Accurate(6));
//points[1].ForEach(p => p.Set_Accurate(6));
//pointList.ForEach(p => p.Set_Accurate(6));
//List<_3D_Point> tp = new List<_3D_Point>();
//List<double> l = new List<double>();
//StreamWriter sw = new StreamWriter("midvalue.txt"),
// sw1 = new StreamWriter("result.txt");
//for (int i = 0; i < PassOList.Count; i++)
//{
// l.Add(_3D_Point.Get_Norm(PassOList[i].pos, pointList[i]));
// l[i] = Methods.Set_Accurate(l[i], 6);
// sw.WriteLine(PassIList[0][i].Name + "," + points[0][i] + "," + points[1][i]);
// sw1.WriteLine(PassIList[0][i].Name + "," + pointList[i] + "," + l[i]);
//}
//sw1.Close();
//sw.Close();
for (int i = 0; i < pointList.Count; i++)
{
PassOList.Add(new ObjectData {
Name = PassIList[0][i].Name, pos = pointList[i]
});
}
return(PassOList);
}
/// <summary>
/// 前方交会
/// </summary>
/// <returns></returns>
public List<OData> ForwardForcus()
{
List<List<_3D_Point>> points = new List<List<_3D_Point>>();
for (int i = 0; i < outE.Count - 1; i++)
{
points.Add(new List<_3D_Point>());
var R1 = GetR(outE[i]);
var R2 = GetR(outE[i + 1]);
List<_3D_Point> uvw1 = new List<_3D_Point>(),
uvw2 = new List<_3D_Point>();
foreach (var ip in PassIList[i])
{
var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
var uvw = R1 * xyf;
uvw1.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
}
foreach (var ip in PassIList[i + 1])
{
var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
var uvw = R2 * xyf;
uvw2.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
}
double Xs2 = outE[i + 1].Spos.X, Xs1 = outE[i].Spos.X,
Ys2 = outE[i + 1].Spos.Y, Ys1 = outE[i].Spos.Y,
Zs2 = outE[i + 1].Spos.Z, Zs1 = outE[i].Spos.Z,
Bu = Xs2 - Xs1, Bv = Ys2 - Ys1, Bw = Zs2 - Zs1;
for (int j = 0; j < uvw1.Count; j++)
{
double w2 = uvw2[j].Z,
v2 = uvw2[j].Y,
u2 = uvw2[j].X,
u1 = uvw1[j].X,
v1 = uvw1[j].Y,
w1 = uvw1[j].Z,
N1 = (Bu * w2 - Bw * u2) / (u1 * w2 - u2 * w1),
N2 = (Bu * w1 - Bw * u1) / (u1 * w2 - u2 * w1);
var UVW1 = N1 * uvw1[j];
var UVW2 = N2 * uvw2[j];
double X = (Xs1 + Xs2 + UVW1.X + UVW2.X) / 2,
Y = (Ys1 + Ys2 + UVW1.Y + UVW2.Y) / 2,
Z = (Zs1 + Zs2 + UVW1.Z + UVW2.Z) / 2;
points[i].Add(new _3D_Point(X, Y, Z));
}
}
List<_3D_Point> pointList = new List<_3D_Point>();
for (int i = 0; i < points[0].Count; i++)
{
_3D_Point sum = new _3D_Point();
for (int j = 0; j < points.Count; j++)
{
sum += points[j][i];
}
pointList.Add(sum / points.Count);
}
//points[0].ForEach(p => p.Set_Accurate(6));
//points[1].ForEach(p => p.Set_Accurate(6));
//pointList.ForEach(p => p.Set_Accurate(6));
//List<_3D_Point> tp = new List<_3D_Point>();
//List<double> l = new List<double>();
//StreamWriter sw = new StreamWriter("midvalue.txt"),
// sw1 = new StreamWriter("result.txt");
//for (int i = 0; i < PassOList.Count; i++)
//{
// l.Add(_3D_Point.Get_Norm(PassOList[i].pos, pointList[i]));
// l[i] = Methods.Set_Accurate(l[i], 6);
// sw.WriteLine(PassIList[0][i].Name + "," + points[0][i] + "," + points[1][i]);
// sw1.WriteLine(PassIList[0][i].Name + "," + pointList[i] + "," + l[i]);
//}
//sw1.Close();
//sw.Close();
for (int i = 0; i < pointList.Count; i++)
{
PassOList.Add(new ObjectData { Name = PassIList[0][i].Name, pos = pointList[i] });
}
return PassOList;
}
private Geodetic_Point Direct(_3D_Point XYZ)
{
double e_2 = ep.data.e_2,
e2_2 = ep.data.e2_2,
a = ep.data.a,
b = ep.data.b;
double r = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y)),
u0 = Math.Atan(XYZ.Z / r * Math.Sqrt(1 + e2_2)),
tgB = (XYZ.Z + b * e2_2 * Math.Sin(u0) * Math.Sin(u0)) /
(r - a * e_2 * Math.Cos(u0) * Math.Cos(u0)),
B = Math.Atan(XYZ.Z / r + a * e_2 * tgB / r / Math.Sqrt(1 + (1 - e_2) * tgB * tgB));
_3D_Point blh = new _3D_Point();
//blh.X = Angle.ChangeToAngle(B).ChangeToDouble();
blh.X = B;
//var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
//blh.Y = l.ChangeToDouble();
blh.Y = Auxiliary.Get_L(XYZ.X,XYZ.Y);
double W = Auxiliary.GetW(e_2, B),
N = a / W;
double h1 = XYZ.Z - N * (1 - e_2) * Math.Sin(B),
h2 = r - N * Math.Cos(B);
double h = Math.Sqrt(h1 * h1 + h2 * h2);
if (h1 / Math.Sin(B) < 0 || h2 / Math.Cos(B) < 0)
h = -h;
blh.Z = h;
return new Geodetic_Point(false, blh.X, blh.Y, blh.Z);
}
private Geodetic_Point Iteration(_3D_Point XYZ)
{
_3D_Point blh = new _3D_Point();
//var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
blh.Y = Auxiliary.Get_L(XYZ.X,XYZ.Y);
double dis = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
double t0 = XYZ.Z / dis,
p = ep.data.c * ep.data.e_2 / dis,
k = 1 + ep.data.e2_2,
t1 = t0,
t2 = t0;
do
{
t1 = t2;
t2 = t0 + p * t1 / Math.Sqrt(k + t1 * t1);
}
while (Math.Abs(Angle.ChangeToAngle(t2 - t1).Miao) > 0.0001);
blh.X = Angle.ChangeToAngle(t2 = Math.Atan(t2)).ChangeToRad();
double W = Auxiliary.GetW(ep.data.e_2, t2),
N = ep.data.a / W;
double h1 = XYZ.Z - N * (1 - ep.data.e_2) * Math.Sin(t2),
h2 = dis - N * Math.Cos(t2);
double h = Math.Sqrt(h1 * h1 + h2 * h2);
if (h1 / Math.Sin(t2) < 0 || h2 / Math.Cos(t2) < 0)
h = -h;
blh.Z = h;
return new Geodetic_Point(false, blh.X, blh.Y, blh.Z);
}
/// <summary>
/// 空间直角坐标到大地坐标
/// </summary>
/// <param name="XYZ">直接坐标</param>
/// <param name="mode">计算模式，默认为0（迭代法），其他为直接法</param>
/// <returns>大地坐标</returns>
public Geodetic_Point Xyz2Blh(_3D_Point XYZ, int mode = 0)
{
if (mode == 0)
return Iteration(XYZ);
else
return Direct(XYZ);
}
/// <summary>
/// 大地坐标到空间直角坐标
/// </summary>
/// <param name="BLH">大地坐标</param>
/// <returns>直角坐标</returns>
public _3D_Point Blh2Xyz(Geodetic_Point BLH)
{
double W = Auxiliary.GetW(ep.data.e_2, BLH.Br);
double V = Auxiliary.GetV(ep.data.e2_2, BLH.Br);
double N = ep.data.a / W;
_3D_Point p = new _3D_Point();
p.X = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Cos(BLH.Lr);
p.Y = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Sin(BLH.Lr);
p.Z = (N * (1 - ep.data.e_2) + BLH.H) * Math.Sin(BLH.Br);
return p;
}