public bool IntersectionWith(Line line2, out Vector Intersection)
{
Intersection = new Vector(Dimensions);
// Lösen der Gleichung n*Direction + P1 = m*Direction2 + P2
// => P2 - P1 = n*Direction - m*Direction2
// => P2 - P1 = [Direction | Direction2] * (n, m)t
// => (n,m)t = [Direction | Direction2]^-1 * P2 - P1
Vector Pdiff = line2.P1 - P1;
Transformations.Matrix M = new Transformations.Matrix(Dimensions, 2);
M.SetColumn(0, Direction);
M.SetColumn(1, line2.Direction);
Transformations.Matrix Minv;
if (!M.Invert(out Minv))
{
return(false);
}
Vector nm = Minv.RightMul(Pdiff);
Intersection.Set((nm[0] * Direction + P1).coordinates);
return(true);
}
public Point2d IntersectionPoint__x(FuncApproximation F1, FuncApproximation F2, Point2d PTX, Point2d PTXdummy, bool DispB)
{
this.F1 = F1; this.F2 = F2;
Point2d PT = PTX, PTnxt = new Point2d(-9999, -9999);
bool B = F1.XYmode;
DenseMatrix M = new DenseMatrix(2, 2), Minv;
DenseVector V = new DenseVector(2);
double a = 0.0, b = 0.0, c = 0.0, a1 = 0.0, b1 = 0.0, c1 = 0.0, d = 0;
int loop;
Mat resImg = null;
if (DispB)
{
resImg = ImgCheck.CvtColor(ColorConversionCodes.GRAY2BGR); //Gray->Color変換
WriteLine();
}
for (loop = 0; loop < 20; loop++) //Up to 20 times
{
int r = 2 + loop * 2;
if (loop == 0)
{
F1.GetTangent(ref a, ref b, ref c);
F2.GetTangent(ref a1, ref b1, ref c1);
}
else
{
double e = Min(d * 0.1, 1.0);
F1.GetTangent1(PT, e, ref a, ref b, ref c);
F2.GetTangent1(PT, e, ref a1, ref b1, ref c1);
}
if (DispB)
{
for (int x = 0; x < ImgCheck.Width; x += 20)
{
Point P1 = new Point(x, -a * x + c);
Point P2 = P1 + (new Point(r, r));
resImg.Rectangle(P1, P2, Scalar.Blue, 3);
}
for (int y = 0; y < ImgCheck.Height; y += 20)
{
Point P1 = new Point(-b1 * y + c1, y);
Point P2 = P1 + (new Point(r, r));
resImg.Rectangle(P1, P2, Scalar.Red, 3);
}
using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); }
}
bool matrixB = true;
if (Abs(a) < 0.1)
{
PTnxt.Y = c / b; PTnxt.X = F2.RegXY.Estimate(PTnxt.Y); matrixB = false;
}
if (Abs(a1) < 0.1)
{
PTnxt.Y = c1 / b1; PTnxt.X = F1.RegXY.Estimate(PTnxt.Y); matrixB = false;
}
if (Abs(b) < 0.1)
{
PTnxt.X = c / a; PTnxt.Y = F2.RegXY.Estimate(PTnxt.X); matrixB = false;
}
if (Abs(b1) < 0.1)
{
PTnxt.X = c1 / a1; PTnxt.Y = F1.RegXY.Estimate(PTnxt.X); matrixB = false;
}
if (DispB)
{
string st = "IntersectionPoint loop:" + loop;
st += string.Format(" a:{0:0.0000} b:{1:0.0000} c:{2:0.0000}", a, b, c);
st += string.Format(" a1:{0:0.0000} b1:{1:0.0000} c1:{2:0.0000} ", a1, b1, c1);
st += matrixB? "..": "◆";
st += string.Format("PT:(X:{0:0.0},Y:{1:0.0}) PTnxt:(X:{2:0.0},Y:{3:0.0})", PT.X, PT.Y, PTnxt.X, PTnxt.Y);
WriteLine(st);
}
if (matrixB)
{
M[0, 0] = a; M[0, 1] = b; V[0] = c;
M[1, 0] = a1; M[1, 1] = b1; V[1] = c1;
Minv = (DenseMatrix)M.Inverse();
V = (DenseVector)Minv.Multiply(V);
PTnxt.X = V[0]; PTnxt.Y = V[1];
}
if (DispB)
{
Point P1 = (Point)PTnxt;
Point P2 = P1 + (new Point(r * 5, r * 5));
resImg.Rectangle(P1, P2, Scalar.Green, 3);
using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); }
}
d = PT.DistanceTo(PTnxt);
if (d < 1.0e-1)
{
break;
}
PT = PTnxt;
}
return(PT);
}
