public static double OnSeg2(Pt p1, Pt p2, Pt q)
{
if ((p1 - q).Cross(p2 - q) == 0 && (p1 - q).Dot(p2 - q) <= 0)
{
return(0);
}
else
{
return((p1 - q).Cross(p2 - q));
}
}
public static bool HasIntersect(Pt p1, Pt p2, Pt q1, Pt q2)
{
if ((p1 - q1).Cross(p2 - q2) == 0)
{
return(OnSeg(p1, q1, p2) || OnSeg(p1, q1, q2) || OnSeg(p2, q2, p1) || OnSeg(p2, q2, q1));
}
else
{
var r = Intersect(p1, q1, p2, q2);
return(OnSeg(p1, q1, r) && OnSeg(p2, q2, r));
}
}
/// <summary>
/// crosssing point of line p1-p2 and q1-q2
/// </summary>
public static Pt Intersect(Pt p1, Pt p2, Pt q1, Pt q2)
{
return(p1 + (p2 - p1) * ((q2 - q1).Cross(q1 - p1) / (q2 - q1).Cross(p2 - p1)));
}
/// <summary>
/// point q exists on line p1-p2?
/// </summary>
public static bool OnSeg(Pt p1, Pt p2, Pt q)
{
return((p1 - q).Cross(p2 - q) == 0 && (p1 - q).Dot(p2 - q) <= 0);
}
/// <summary>
/// Cross(det) product of two vectors: O -> this and O -> other
/// a x b = |a| |b| sin(theta) = ax by - ay bx
/// zero if two vectors run parallelly
/// </summary>
public double Cross(Pt other)
{
return(DoubleUtil.Add(this.X * other.Y, -this.Y * other.X));
}
/// <summary>
/// Dot product of two vectors: O -> this and O -> other
/// a dot b = |a| |b| cos(theta) = ax bx + ax by
/// zero if two vectors run orthogonally
/// </summary>
public double Dot(Pt other)
{
return(DoubleUtil.Add(this.X * other.X, this.Y * other.Y));
}
public static double Dist(Pt v1, Pt v2)
{
return(v1.Dist(v2));
}
public double Dist(Pt other)
{
return((this - other).Norm());
}
public void Call()
{
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
double d = 0.0;
double t = Math.Atan2(0 - origin.Y, 1 - origin.X); // 公共座標で扱っているので縦軸X、横軸Yで考える（数学軸なら縦軸Y、横軸X）
Console.WriteLine(string.Format("atan2 = {0}", t));
Console.WriteLine(string.Format("ラジアンから度へ:{0}度", ToAngle(t)));
// 指定したモードの範囲内にラジアンを収める
d = Clamp(2, t);
Console.WriteLine(string.Format("正規化した値:{0}", d));
Console.WriteLine(string.Format("ラジアンから度へ:{0}度", ToAngle(d)));
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
Console.WriteLine("原点を点群パラメータに寄せたときの原点座標を求めます");
if (pointsL == null)
{
Console.WriteLine("Lリストがnull");
}
if (pointsC == null)
{
Console.WriteLine("Cリストがnull");
}
if (pointsR == null)
{
Console.WriteLine("Rリストがnull");
}
if (pointsL.Count == 0)
{
Console.WriteLine("Lリストの長さが0");
}
if (pointsC.Count == 0)
{
Console.WriteLine("Cリストの長さが0");
}
if (pointsR.Count == 0)
{
Console.WriteLine("Rリストの長さが0");
}
double minX = double.NaN;
double minY = double.NaN;
double localX = 0.0D;
double localY = 0.0D;
bool b = false;
for (int i = 0; i < 3; i++)
{
if (i == 0)
{
b = ToLocal(pointsL[i].X, pointsL[i].Y, origin.X, origin.Y, -d, ref localX, ref localY);
}
else if (i == 1)
{
b = ToLocal(pointsC[i].X, pointsC[i].Y, origin.X, origin.Y, -d, ref localX, ref localY);
}
else if (i == 2)
{
b = ToLocal(pointsR[i].X, pointsR[i].Y, origin.X, origin.Y, -d, ref localX, ref localY);
}
if (!b)
{
return;
}
if ((localX % meshPitch) != 0)
{
localX -= (localX % meshPitch);
localX -= meshPitch;
}
if ((localY % meshPitch) != 0)
{
localY -= (localY % meshPitch);
localY -= meshPitch;
}
if (localX < minX || double.IsNaN(minX))
{
minX = localX;
}
if (localY < minY || double.IsNaN(minY))
{
minY = localY;
}
}
double originX = 0.0;
double originY = 0.0;
b = ToPublic(minX, minY, origin.X, origin.Y, -d, ref originX, ref originY);
if (!b)
{
return;
}
XY shiftOrigin = new XY(originX, originY);
Console.WriteLine(string.Format("原点座標 X:0 → {0}, Y:0 → {1}", shiftOrigin.X, shiftOrigin.Y));
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
Console.WriteLine("点群パラメータが5000×5000のメッシュに収まるかチェックします");
double maxX = pointsL.Max(a => a.X);
if (maxX < pointsC.Max(a => a.X))
{
maxX = pointsC.Max(a => a.X);
}
if (maxX < pointsL.Max(a => a.X))
{
maxX = pointsL.Max(a => a.X);
}
double maxY = pointsL.Max(a => a.Y);
if (maxX < pointsC.Max(a => a.Y))
{
maxX = pointsC.Max(a => a.Y);
}
if (maxX < pointsL.Max(a => a.Y))
{
maxX = pointsL.Max(a => a.Y);
}
Console.WriteLine(string.Format("点群パラメータのうち最も遠い点 X:{0} Y:{1}", maxX, maxY));
// 5000 * 5000 に収まっているか
if ((meshPitch + shiftOrigin.X) * meshLength < meshPitch * maxX)
{
Console.WriteLine(string.Format("X範囲外:{0}", maxX));
}
else
{
Console.WriteLine("X 範囲内");
}
if ((meshPitch + shiftOrigin.Y) * meshLength < meshPitch * maxY)
{
Console.WriteLine(string.Format("Y範囲外:{0}", maxY));
}
else
{
Console.WriteLine("Y 範囲内");
}
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
Console.WriteLine("点群パラメータから小さい四角形を作ります");
List <Rectangle> rectangles = new List <Rectangle>();
// L列の点とC列の点で構成する四角形を作る
for (int i = 0; i < pointsL.Count; i++)
{
if (i == pointsL.Count - 1)
{
break;
}
Rectangle rectangle = new Rectangle(pointsL[i], pointsL[i + 1], pointsC[i + 1], pointsC[i]);
rectangles.Add(rectangle);
}
Console.WriteLine(string.Format("四角形の数:{0}", rectangles.Count));
// C列の点とR列の点で構成する四角形を作る
for (int i = 0; i < pointsC.Count; i++)
{
if (i == pointsC.Count - 1)
{
break;
}
Rectangle rectangle = new Rectangle(pointsC[i], pointsC[i + 1], pointsR[i + 1], pointsR[i]);
rectangles.Add(rectangle);
}
Console.WriteLine(string.Format("四角形の数:{0}", rectangles.Count));
// 要素の0-2または1-3を結べば対角線を引くことができる
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
Console.WriteLine("点群パラメータの外側の点で構成するメッシュの中心座標が含まれているか判定します");
Console.WriteLine("含まれていたら、深さの按分計算を行います");
// 深さの配列を初期化
int totalLength = meshLength * meshLength;
short[] depths = new short[totalLength];
for (int i = 0; i < depths.Length; i++)
{
// defaultは0なので、-1で初期化
depths[i] = -1;
}
//Console.ReadLine();
// メッシュの中心座標を求め、その中心座標と点群パラメータの外側の点でが構成する矩形との内外判定します
/*
* 点群パラメータの外側の点でが構成する矩形
* ←
* ↓ +---+---+---+---+---+
* | | | | | |
* +---+---+---+---+---+
* | | | | | |
* +---+---+---+---+---+
* | | | | | |
* +---+---+---+---+---+ ↑
* →
* 外側の点を取得します
*
*
*/
// X座標
List <double> outlineX = new List <double>();
// X左
for (int i = 0; i < pointsL.Count; i++)
{
// 先頭から順にX座標をリストに追加
outlineX.Add(pointsL[i].X);
}
// X中央末尾
outlineX.Add(pointsC[pointsC.Count - 1].X);
// X右
for (int i = pointsR.Count; i < 0; i--)
{
// 末尾から順にX座標をリストに追加
outlineX.Add(pointsR[i - 1].X);
}
// X中央先頭
outlineX.Add(pointsC[0].X);
// Y座標
List <double> outlineY = new List <double>();
// Y左
for (int i = 0; i < pointsL.Count; i++)
{
// 先頭から順にY座標をリストに追加
outlineY.Add(pointsL[i].Y);
}
// Y中央末尾
outlineY.Add(pointsC[pointsC.Count - 1].Y);
// Y右
for (int i = pointsR.Count; i < 0; i--)
{
// 末尾から順にY座標をリストに追加
outlineY.Add(pointsR[i - 1].Y);
}
// Y中央先頭
outlineY.Add(pointsC[0].Y);
double seekStartX = 0.25 + shiftOrigin.X;
double seekStartY = 0.25 + shiftOrigin.Y;
for (int x = 0; x < meshLength; x++)
{
for (int y = 0; y < meshLength; y++)
{
double seekX = seekStartX + x * meshPitch;
double seekY = seekStartY + y * meshPitch;
int result = PointInArea(seekX, seekY, outlineX, outlineY);
if (result == 0)
{
// 外
//Console.WriteLine(string.Format("X:{0} Y:{1} {2}", seekX, seekY, "outside"));
}
else
{
// 内
//Console.WriteLine(string.Format("X:{0} Y:{1} {2}", seekX, seekY, "inside"));
for (int i = 0; i < rectangles.Count; i++)
{
Rectangle rectangle = rectangles[i];
List <double> rectangleX = new List <double>();
rectangleX.Add(rectangle.P0.X);
rectangleX.Add(rectangle.P1.X);
rectangleX.Add(rectangle.P2.X);
rectangleX.Add(rectangle.P3.X);
List <double> rectangleY = new List <double>();
rectangleY.Add(rectangle.P0.Y);
rectangleY.Add(rectangle.P1.Y);
rectangleY.Add(rectangle.P2.Y);
rectangleY.Add(rectangle.P3.Y);
if (PointInArea(seekX, seekY, rectangleX, rectangleY) != 0)
{
// 四角の内
Pt p0 = new Pt(rectangle.P0.X, rectangle.P0.Y);
Pt p2 = new Pt(rectangle.P2.X, rectangle.P2.Y);
Pt p = new Pt(seekX, seekY);
double j = Pt.OnSeg2(p0, p2, p);
Console.WriteLine(string.Format("線分AB A(X:{0} Y:{1}) B(X:{2} Y:{3})", p0.X, p0.Y, p2.X, p2.Y));
Console.WriteLine(string.Format("点P X:{0} Y:{1} {2}", seekX, seekY, j));
if (j == 0)
{
Console.WriteLine("オンザライン");
}
else if (j > 0)
{
Console.WriteLine("プラス");
XY A = rectangle.P0;
XY B = rectangle.P2;
XY O = rectangle.P1;
XY xy = new XY(seekX, seekY);
XY pt;
if (Intersection(A, B, O, xy, out pt))
{
}
}
else if (j < 0)
{
Console.WriteLine("マイナス");
XY A = rectangle.P0;
XY B = rectangle.P2;
XY O = rectangle.P3;
XY xy = new XY(seekX, seekY);
XY pt;
if (Intersection(A, B, O, xy, out pt))
{
}
}
break;
}
}
}
}
}
Console.WriteLine(string.Empty);
Console.WriteLine("---------------------------------------------------------------------------");
Console.WriteLine("点群パラメータに含まれていたメッシュの中心座標と三角形の底辺から深さを案分計算します");
//Console.WriteLine(string.Empty);
//Console.WriteLine("---------------------------------------------------------------------------");
//Console.WriteLine("点群パラメータに含まれていたメッシュの中心座標と三角形の底辺から深さを案分計算します");
}