public static matrix Line(complex z1, complex z2)//m(0,0) = A, m(0,1)=B,m(0,2)=C
{
matrix m = new matrix(1, 3);
m.SetAt(0, 0, z2.imag() - z1.imag());
m.SetAt(0, 1, z1.real() - z2.real());
m.SetAt(0, 2, -z1.real() * m.GetAt(0, 0) - z1.imag() * m.GetAt(0, 1));
return(m);
}
public UCS(quaternion O, quaternion Xort, quaternion Yort)
{
o = O;
quaternion I = new quaternion(0.0, (Xort - O).imag());
quaternion K = new quaternion(0.0, (I * (Yort - O)).imag());
quaternion J = new quaternion(0.0, (K * I).imag());
ang.SetAt(1, 1, I.cosToX());
ang.SetAt(1, 2, J.cosToX());
ang.SetAt(1, 3, K.cosToX());
ang.SetAt(2, 1, I.cosToY());
ang.SetAt(2, 2, J.cosToY());
ang.SetAt(2, 3, K.cosToY());
ang.SetAt(3, 1, I.cosToZ());
ang.SetAt(3, 2, J.cosToZ());
ang.SetAt(3, 3, K.cosToZ());
}
public static matrix IntersectCircleAndLine(double r, complex z1, complex z2)
//m(0,0) = X1, m(0,1)=Y1, m(0,2)=X2, m(0,3) = Y2, m(0,4) < 0 ako nema presi4ane
{
matrix m = new matrix(1, 5);
matrix L = Line(z1, z2);
double a, b, c, A, B, C;
A = L.GetAt(0, 0); B = L.GetAt(0, 1); C = L.GetAt(0, 2);
if (A != 0.0)
{
#region
a = B * B / (A * A) + 1;
b = 2 * B * C / (A * A);
c = C * C / (A * A) - r * r;
double D = b * b - 4 * a * c;
if (D >= 0)
{
D = Math.Sqrt(D);
m.SetAt(0, 1, (-b + D) / (2 * a));
m.SetAt(0, 3, (-b - D) / (2 * a));
m.SetAt(0, 0, (-C - B * m.GetAt(0, 1)) / A);
m.SetAt(0, 2, (-C - B * m.GetAt(0, 3)) / A);
}
else
{
m.SetAt(0, 0, 0); m.SetAt(0, 1, 0); m.SetAt(0, 2, 0); m.SetAt(0, 3, 0);
m.SetAt(0, 4, -1);
}
#endregion
}
else
{
#region
a = A * A / (B * B) + 1;
b = 2 * A * C / (B * B);
c = C * C / (B * B) - r * r;
double D = b * b - 4 * a * c;
if (D >= 0)
{
D = Math.Sqrt(D);
m.SetAt(0, 0, (-b + D) / (2 * a));
m.SetAt(0, 2, (-b - D) / (2 * a));
m.SetAt(0, 1, (-C - A * m.GetAt(0, 0)) / B);
m.SetAt(0, 3, (-C - A * m.GetAt(0, 2)) / B);
}
else
{
m.SetAt(0, 0, 0); m.SetAt(0, 1, 0); m.SetAt(0, 2, 0); m.SetAt(0, 3, 0);
m.SetAt(0, 4, -1);
}
#endregion
}
return(m);
}
