//Restituisce l'equazione cartesiana del piano passante per V1, V2, V3
public static MyPlane PlanePassingThrough(MyVertex V1, MyVertex V2, MyVertex V3 /*, ref StringBuilder fileOutput*/)
{
//manca il controllo se i punti se i 3 punti sono coincidenti...
MyPlane OutputPlane = new MyPlane();
if (V1.Lieonline(LinePassingThrough(V2, V3)))
{
return(OutputPlane); // Plane does not exist
}
else
{
var aa = (V2.y - V1.y) * (V3.z - V1.z) - (V3.y - V1.y) * (V2.z - V1.z);
var bb = -((V2.x - V1.x) * (V3.z - V1.z) - (V3.x - V1.x) * (V2.z - V1.z));
var cc = (V2.x - V1.x) * (V3.y - V1.y) - (V3.x - V1.x) * (V2.y - V1.y);
var norm = Math.Sqrt(Math.Pow(aa, 2) + Math.Pow(bb, 2) + Math.Pow(cc, 2));
OutputPlane.a = aa / norm;
OutputPlane.b = bb / norm;
OutputPlane.c = cc / norm;
OutputPlane.d = -OutputPlane.a * V1.x - OutputPlane.b * V1.y - OutputPlane.c * V1.z;
//VERSIONE VECCHIA CON CUI FUNZIONAVA: SE IN FUTURO SORGONO PROBLEMI TORNARE A QUESTA
//OutputPlane.a = (V2.y - V1.y) * (V3.z - V1.z) - (V3.y - V1.y) * (V2.z - V1.z);
//OutputPlane.b = -((V2.x - V1.x) * (V3.z - V1.z) - (V3.x - V1.x) * (V2.z - V1.z));
//OutputPlane.c = (V2.x - V1.x) * (V3.y - V1.y) - (V3.x - V1.x) * (V2.y - V1.y);
//OutputPlane.d = -OutputPlane.a * V1.x - OutputPlane.b * V1.y - OutputPlane.c * V1.z;
}
return(OutputPlane);
}
//Restituisce l'equazione cartesiana della sfera passante per V1, V2, V3, V4
//[DAI TEST: i 4 vertici stanno sulla sfera con precisione dell'ordine in media di 10^(-6) se
// le coordinate dei vertici sono numeri con al più 3 cifre dopo la virgola e 3 cifre prima della virgola.
// Inoltre ho osservato che più aumenta la differenza tra l'ordine di grandezza tra tutte le coordinate dei vari
// vertici e più diminuisce la precisione...o no???]
public static MySphere SpherePassingThrough(MyVertex V1, MyVertex V2, MyVertex V3, MyVertex V4, ref StringBuilder fileOutput, ModelDoc2 SwModel)
{
//MySphere OutputMySphere = new MySphere();
//First, the sphere does not exist if 3 of the 4 points lie on a line. Four possible line: V1-V2-V3, V1-V3-V4, V1-V2-V4, V2-V3-V4
//Second, the sphere does not exist if the 4 points lie on the same plane.
//(MANCA IL CONTROLLO CHE NON SIANO PUNTI COINCIDENTI...)
if (V1.Lieonline(LinePassingThrough(V2, V3)) || V1.Lieonline(LinePassingThrough(V3, V4)) || V1.Lieonline(LinePassingThrough(V2, V4)) || V2.Lieonline(LinePassingThrough(V3, V4)))
{
fileOutput.AppendLine("Sistema non risolvibile");
MySphere OutputMySphere = new MySphere();
return(OutputMySphere); // MySphere does not exist
}
else
{
double[,] CoefficientsMatrix =
{
{ V1.x, V1.y, V1.z, 1 },
{ V2.x, V2.y, V2.z, 1 },
{ V3.x, V3.y, V3.z, 1 },
{ V4.x, V4.y, V4.z, 1 },
};
double alfa = Math.Pow(V1.x, 2) + Math.Pow(V1.y, 2) + Math.Pow(V1.z, 2);
double beta = Math.Pow(V2.x, 2) + Math.Pow(V2.y, 2) + Math.Pow(V2.z, 2);
double gamma = Math.Pow(V3.x, 2) + Math.Pow(V3.y, 2) + Math.Pow(V3.z, 2);
double delta = Math.Pow(V4.x, 2) + Math.Pow(V4.y, 2) + Math.Pow(V4.z, 2);
double[,] KnownTerms = { { -alfa }, { -beta }, { -gamma }, { -delta } };
double[,] SphereCoef = Matrix.Solve(CoefficientsMatrix, KnownTerms, leastSquares: true);
//OutputMySphere.a = SphereCoef[0, 0]; OutputMySphere.b = SphereCoef[1, 0]; OutputMySphere.c = SphereCoef[2, 0]; OutputMySphere.d = SphereCoef[3, 0];
MySphere OutputMySphere = new MySphere(SphereCoef[0, 0], SphereCoef[1, 0], SphereCoef[2, 0], SphereCoef[3, 0]);
return(OutputMySphere);
}
}
//This function returns true if the 2 MyLine correspond to the same line in 3D space, false otherwise.
//First the function verifies if the two lines have the same direction versor, then it verifies if
//the point lying on the first line also lies on the second line, and viceversa.
//The direction versor of each line is computed by computing the vector product of the two normal
//vectors of the two planes defining the line.
public static bool MyEqualsMyPlane(MyLine firstLine, MyLine secondLine, MyVertex firstPoint, MyVertex secondPoint)
{
var firstDirection = firstLine.direction;
var secondDirection = secondLine.direction;
var secondDirectionInverted = new double[3];
secondDirectionInverted.SetValue(-secondDirection[0], 0);
secondDirectionInverted.SetValue(-secondDirection[1], 1);
secondDirectionInverted.SetValue(-secondDirection[2], 2);
if (FunctionsLC.MyEqualsArray(firstDirection, secondDirection) || FunctionsLC.MyEqualsArray(firstDirection, secondDirectionInverted))
{
var firstVerify = firstPoint.Lieonline(secondLine);
var secondVerify = secondPoint.Lieonline(firstLine);
if (firstPoint.Lieonline(secondLine) && secondPoint.Lieonline(firstLine))
{
return(true);
}
return(false);
}
return(false);
}
//Restituisce le equazioni cartesiane della circonferenza passante per V1, V2, V3
public static MyCircumForPath CircumPassingThrough(MyVertex V1, MyVertex V2, MyVertex V3, ref StringBuilder fileOutput, ModelDoc2 SwModel, SldWorks swApplication)
{
//The circumference does not exist if the 3 points lie on the same line or if 2 of them are coincident.
if (V1.Lieonline(LinePassingThrough(V2, V3)) || V1.Equals(V2) || V1.Equals(V3) || V2.Equals(V3))
{
MyCircumForPath OutputCircum = new MyCircumForPath();
return(OutputCircum);
}
else
{
MyPlane CircumPlane = PlanePassingThrough(V1, V2, V3);
//There is a unique sphere passing through 4 points: we must arbitrarily choose a fourth point not lying on the V1-V2-V3 plane
//So we choose the point resulting from adding the normal direction to a point lying on the V1-V2-V3 plane, for example V1
MyVertex V4 = new MyVertex(V1.x + CircumPlane.a * 10, V1.y + CircumPlane.b * 10, V1.z + CircumPlane.c * 10);
//Vertex V4 = new Vertex(0, 0, 0);
//if (SwModel != null)
//{
// SwModel = swApplication.ActiveDoc;
// SwModel.ClearSelection2(true);
// SwModel.Insert3DSketch();
// SwModel.CreatePoint2(V1.x, V1.y, V1.z);
// SwModel.InsertSketch();
// SwModel.CreatePoint2(V2.x, V2.y, V2.z);
// SwModel.InsertSketch();
// SwModel.CreatePoint2(V3.x, V3.y, V3.z);
// SwModel.InsertSketch();
// SwModel.CreatePoint2(V4.x, V4.y, V4.z);
// SwModel.InsertSketch();
//}
MySphere CircumSphere = SpherePassingThrough(V1, V2, V3, V4, ref fileOutput, SwModel);
if (CircumSphere.centerSphere == null)
{
}
else if (CircumSphere.centerSphere != null)
{
MyCircumForPath OutputCircum = new MyCircumForPath(CircumPlane, CircumSphere);
return(OutputCircum);
}
}
MyCircumForPath OutputCircumNull = new MyCircumForPath();
return(OutputCircumNull);
}
//INPUT: MyVertex V1, MyVertex V2, MyVertex C
//It returns the angle teta between the two vectors C V1 and C V2
public static double FindAngle(MyVertex V1, MyVertex V2, MyVertex C)
{
//var whatToWrite = string.Format("PROVA: ");
//KLdebug.Print(whatToWrite, "AngleComputation.txt");
double outputAngle = 0.0;
if (V1.Equals(V2) || V1.Equals(C) || V1.Equals(V2))
{
return(outputAngle);
}
if (C.Lieonline(LinePassingThrough(V1, V2)))
{
outputAngle = Math.PI;
return(outputAngle);
}
//KLdebug.Print("calcolo:", "prova.txt");
if (Math.Abs(C.x) < Math.Pow(10, -10))
{
C.x = 0;
}
if (Math.Abs(C.y) < Math.Pow(10, -10))
{
C.y = 0;
}
if (Math.Abs(C.z) < Math.Pow(10, -10))
{
C.z = 0;
}
//whatToWrite = string.Format("centro ({0},{1},{2}) ", C.x, C.y, C.z);
//KLdebug.Print(whatToWrite, "prova.txt");
double[] vectorV1 = { V1.x - C.x, V1.y - C.y, V1.z - C.z };
if (Math.Abs(vectorV1[0]) < Math.Pow(10, -10))
{
vectorV1[0] = 0;
}
if (Math.Abs(vectorV1[1]) < Math.Pow(10, -10))
{
vectorV1[1] = 0;
}
if (Math.Abs(vectorV1[2]) < Math.Pow(10, -10))
{
vectorV1[2] = 0;
}
//whatToWrite = string.Format("vec1 ({0},{1},{2}) ", vectorV1[0], vectorV1[1], vectorV1[2]);
//KLdebug.Print(whatToWrite, "prova.txt");
double vectorV1Norm =
Math.Sqrt(Math.Pow(vectorV1[0], 2) + Math.Pow(vectorV1[1], 2) + Math.Pow(vectorV1[2], 2));
//KLdebug.Print("norma vec1 " + vectorV1Norm, "prova.txt");
double[] vectorV2 = { V2.x - C.x, V2.y - C.y, V2.z - C.z };
if (Math.Abs(vectorV2[0]) < Math.Pow(10, -10))
{
vectorV2[0] = 0;
}
if (Math.Abs(vectorV2[1]) < Math.Pow(10, -10))
{
vectorV2[1] = 0;
}
if (Math.Abs(vectorV2[2]) < Math.Pow(10, -10))
{
vectorV2[2] = 0;
}
//whatToWrite = string.Format("vec2 ({0},{1},{2}) ", vectorV2[0], vectorV2[1], vectorV2[2]);
//KLdebug.Print(whatToWrite, "prova.txt");
double vectorV2Norm =
Math.Sqrt(Math.Pow(vectorV2[0], 2) + Math.Pow(vectorV2[1], 2) + Math.Pow(vectorV2[2], 2));
//KLdebug.Print("norma vec2 " + vectorV2Norm, "prova.txt");
double scalarProduct = vectorV1[0] * vectorV2[0] + vectorV1[1] * vectorV2[1] + vectorV1[2] * vectorV2[2];
//KLdebug.Print("scalarProduct " + scalarProduct, "prova.txt");
//KLdebug.Print("faccio l'arcos di: " + scalarProduct / (vectorV1Norm * vectorV2Norm), "prova.txt");
if (vectorV1Norm * vectorV2Norm != 0)
{
outputAngle = Math.Acos(scalarProduct / (vectorV1Norm * vectorV2Norm));
}
//KLdebug.Print("outputAngle " + outputAngle, "prova.txt");
if (!Double.IsNaN(outputAngle))
{
return(outputAngle);
}
return(-1);
}