public static Vector3D VectorMultiMatrix(Vector3D B, CalMatrix IA)
{
Vector3D result = new Vector3D();
result.x = IA.CreatedM[0, 0] * B.x + IA.CreatedM[0, 1] * B.y + IA.CreatedM[0, 2] * B.z;
result.y = IA.CreatedM[1, 0] * B.x + IA.CreatedM[1, 1] * B.y + IA.CreatedM[1, 2] * B.z;
result.z = IA.CreatedM[2, 0] * B.x + IA.CreatedM[2, 1] * B.y + IA.CreatedM[2, 2] * B.z;
return(result);
}
static void Main(string[] args)
{
Model.NormalDistribution();
CalMatrix test = new CalMatrix();
test.Values = new Double[, ] {
{ 1, 0, 1 }, { 1, 1, 2 }, { 3, 4, 2 }
};
test.CalDet();
test.InverseMatrix();
Double result = test.MatrixDet;
Console.WriteLine("Please input the Direction and Velocity of the boundary: 4 5 6 0.01");
String input = "4 5 6 0.01";//Console.ReadLine();
String[] inputs = input.Split(' ');
BoundaryInSpace test2 = new BoundaryInSpace(Double.Parse(inputs[0]), Double.Parse(inputs[1]), Double.Parse(inputs[2]), Double.Parse(inputs[3]), -30, -30, -30);
test2.ResultCalwithTime();
}
public void ResultCalwithTime()
{
//StreamWriter tw = File.CreateText(@"E:\MonteCarloForErrorAnalysis\Result0110\Result.txt");
Double[] flags = new Double[] { 0, 0, 0, 0 };
Double[] time = new Double[] { 0, 0, 0, 0 };
//1,0,-1/sqrt(2)) , (-1,0,-1/sqrt(2)) , (0,1,1/sqrt(2)) , (0,-1,1/sqrt(2)
Double[,] position = new Double[, ] {
{ 1, 0, -1 / Math.Sqrt(2) }, { -1, 0, -1 / Math.Sqrt(2) }, { 0, 1, 1 / Math.Sqrt(2) }, { 0, -1, 1 / Math.Sqrt(2) }
};
double count = 4;
for (t = 0; t < 10000; t += 0.001)
{
//tw.Write(t);
for (int i = 0; i < 4; i++)
{
Vector3D point = new Vector3D();
point = Tetrahedron.VertexPosition(t, i);
double tmpresult = this.CalAValueWithTimeAndPosition(point.x, point.y, point.z, t);
if (flags[i] == 0 && tmpresult == 1)
{
flags[i] = 1;
time[i] = t;
// position[i,0] = point.x;
//position[i, 1] = point.y;
//position[i, 2] = point.z;
count--;
}
//tw.Write(" ");
//tw.Write(tmpresult);
}
if (count <= 0)
{
break;
}
//tw.WriteLine();
}
double t0 = (time[0] + time[1] + time[2] + time[3]) / 4;
time[0] -= t0;
time[1] -= t0;
time[2] -= t0;
time[3] -= t0;
CalMatrix CalM = new CalMatrix();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
CalM.Values[i, j] = 0;
for (int k = 0; k < 4; k++)
{
CalM.Values[i, j] += position[k, i] * position[k, j];
}
}
}
Vector3D RightB = new Vector3D();
RightB.x = 0;
RightB.y = 0;
RightB.z = 0;
for (int i = 0; i < 4; i++)
{
RightB.x += time[i] * position[i, 0];
RightB.y += time[i] * position[i, 1];
RightB.z += time[i] * position[i, 2];
}
CalM.CalDet();
CalM.InverseMatrix();
Vector3D mresult = Vector3D.VectorMultiMatrix(RightB, CalM);
v = 1.0 / Math.Sqrt(mresult.x * mresult.x + mresult.y * mresult.y + mresult.z * mresult.z);
mresult.x *= v;
mresult.y *= v;
mresult.z *= v;
Console.WriteLine("The detected Vector of the Boundary is :");
Console.Write(mresult.x);
Console.Write(" ");
Console.Write(mresult.y);
Console.Write(" ");
Console.Write(mresult.z);
Console.WriteLine();
Console.WriteLine("Detected velocity is : ");
Console.Write(v);
Vector3D yAxis = Vector3D.CrossProduct(mresult, new Vector3D());
Vector3D zAxix = Vector3D.CrossProduct(yAxis, mresult);
#region Single Parameter Chaning
//for (int cout = 0; cout < 4; cout++)
//{
// StreamWriter etw = File.CreateText(@"E:\MonteCarloForErrorAnalysis\Result0110\PositioningParameterChanging" + cout.ToString() + ".txt");
// for (int erroradd = 0; erroradd < 100; erroradd++)
// {
// Double[] TmpTime = new Double[4];
// for (int i = 0; i < 4; i++)
// TmpTime[i] = time[i];
// //TmpTime[cout] += 0.001 * erroradd;
// double[,] TmpPositions = new Double[,] { { 1, 0, -1 / Math.Sqrt(2) }, { -1, 0, -1 / Math.Sqrt(2) }, { 0, 1, 1 / Math.Sqrt(2) }, { 0, -1, 1 / Math.Sqrt(2) } };
// for (int j = 0; j < 3; j++)
// TmpPositions[cout, j] += 0.0001 * erroradd;
// CalMatrix TmpCalM = new CalMatrix();
// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 3; j++)
// {
// TmpCalM.Values[i, j] = 0;
// for (int k = 0; k < 4; k++)
// {
// TmpCalM.Values[i, j] += TmpPositions[k, i] * TmpPositions[k, j];
// }
// }
// }
// Vector3D TmpRightB = new Vector3D();
// TmpRightB.x = 0;
// TmpRightB.y = 0;
// TmpRightB.z = 0;
// for (int i = 0; i < 4; i++)
// {
// TmpRightB.x += TmpTime[i] * TmpPositions[i, 0];
// TmpRightB.y += TmpTime[i] * TmpPositions[i, 1];
// TmpRightB.z += TmpTime[i] * TmpPositions[i, 2];
// }
// TmpCalM.CalDet();
// TmpCalM.InverseMatrix();
// Vector3D TmpResult = Vector3D.VectorMultiMatrix(TmpRightB, TmpCalM);
// double tmpv = 1.0 / Math.Sqrt(TmpResult.x * TmpResult.x + TmpResult.y * TmpResult.y + TmpResult.z * TmpResult.z);
// //TmpResult.x *= tmpv;
// //TmpResult.y *= tmpv;
// //TmpResult.z *= tmpv;
// double tmpAngle = Vector3D.AngleBetweenVector(TmpResult, mresult);
// double errorInVelocity = (tmpv - v) / v;
// etw.WriteLine((0.0001 * erroradd).ToString() + " " + TmpResult.x.ToString() + " " + TmpResult.y.ToString() + " " + TmpResult.z.ToString() + " " + errorInVelocity.ToString() + " " + tmpAngle.ToString());
// }
// etw.Flush();
// etw.Close();
//}
#endregion
#region 2D map
//for (int cout = 3; cout < 4; cout++)
//{
// StreamWriter etw = File.CreateText(@"E:\MonteCarloForErrorAnalysis\Result0110\2DCover_V" /*+ cout.ToString() */+ ".txt");
// for (int xerroradd = 0; xerroradd < 200; xerroradd++)
// {
// for (int yerroradd = 0; yerroradd < 200; yerroradd++)
// {
// Double[] TmpTime = new Double[4];
// for (int i = 0; i < 4; i++)
// TmpTime[i] = time[i];
// TmpTime[0] = time[0] + 0.01 * xerroradd;
// double[,] TmpPositions = new Double[,] { { 1, 0, -1 / Math.Sqrt(2) }, { -1, 0, -1 / Math.Sqrt(2) }, { 0, 1, 1 / Math.Sqrt(2) }, { 0, -1, 1 / Math.Sqrt(2) } };
// for (int j = 0; j < 3; j++)
// TmpPositions[0, j] += 0.0001 * yerroradd;
// CalMatrix TmpCalM = new CalMatrix();
// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 3; j++)
// {
// TmpCalM.Values[i, j] = 0;
// for (int k = 0; k < 4; k++)
// {
// TmpCalM.Values[i, j] += TmpPositions[k, i] * TmpPositions[k, j];
// }
// }
// }
// Vector3D TmpRightB = new Vector3D();
// TmpRightB.x = 0;
// TmpRightB.y = 0;
// TmpRightB.z = 0;
// for (int i = 0; i < 4; i++)
// {
// TmpRightB.x += TmpTime[i] * TmpPositions[i, 0];
// TmpRightB.y += TmpTime[i] * TmpPositions[i, 1];
// TmpRightB.z += TmpTime[i] * TmpPositions[i, 2];
// }
// TmpCalM.CalDet();
// TmpCalM.InverseMatrix();
// Vector3D TmpResult = Vector3D.VectorMultiMatrix(TmpRightB, TmpCalM);
// double tmpv = 1.0 / Math.Sqrt(TmpResult.x * TmpResult.x + TmpResult.y * TmpResult.y + TmpResult.z * TmpResult.z);
// //TmpResult.x *= tmpv;
// //TmpResult.y *= tmpv;
// //TmpResult.z *= tmpv;
// double tmpAngle = Vector3D.AngleBetweenVector(TmpResult, mresult);
// //etw.WriteLine((0.001 * xerroradd).ToString() + " " + (0.0001 * yerroradd).ToString() + " " /*+ TmpResult.x.ToString() + " " + TmpResult.y.ToString() + " " + TmpResult.z.ToString() + " "*/ + tmpAngle.ToString());
// etw.Write(/*(0.001 * xerroradd).ToString() + " " + (0.0001 * yerroradd).ToString() + " " *//*+ TmpResult.x.ToString() + " " + TmpResult.y.ToString() + " " + TmpResult.z.ToString() + " "*/ /*+*/ Math.Abs(v-tmpv).ToString() + " ");
// }
// etw.WriteLine();
// }
// etw.Flush();
// etw.Close();
//}
#endregion
// For Vector detection, the Limit-State-Function is assumed as G(x)=Angle|Detected - True| - 0.05
#region MonteCarloCalculate
//for(int MonteI = 0; MonteI<100; MonteI+=10)
{
StreamWriter tw = File.CreateText(@"E:\MonteCarloForErrorAnalysis\Result0110\MonteCarloAngleResult_All_Error_WRAMethodValidation" + /*MonteI.ToString()+*/ ".txt");
//Random Error Generate
GaussianRNG tmpRandom = new GaussianRNG();
//.WriteLine("0 0 0 0 " + mresult.x.ToString() + " " + mresult.y.ToString() + " " + mresult.z.ToString() + " " + v.ToString());
tw.WriteLine(mresult.x.ToString() + " " + mresult.y.ToString() + " " + mresult.z.ToString());
Random rforV = new Random(unchecked ((int)DateTime.Now.Ticks));
for (int RepeatTimes = 0; RepeatTimes < 100000; RepeatTimes++)
{
Double[] RandomErrorForTime1 = new Double[2]; //Model.NormalDistribution();
Double[] RandomErrorForTime2 = new Double[2]; //Model.NormalDistribution();
RandomErrorForTime1[0] = 0.1 * 0.1 * tmpRandom.Next();
RandomErrorForTime2[0] = 0.1 * 0.3 * tmpRandom.Next();
RandomErrorForTime1[1] = 0.1 * 0.2 * tmpRandom.Next();
RandomErrorForTime2[1] = 0.1 * 0.4 * tmpRandom.Next();
Double[] TmpTime = new Double[4];
for (int i = 0; i < 4; i++)
{
TmpTime[i] = time[i];
}
//Add Timing error for Sat0 -Sat3
TmpTime[0] += /*MonteI **/ RandomErrorForTime1[0];
TmpTime[1] += /*MonteI **/ RandomErrorForTime1[1];
TmpTime[2] += /*MonteI **/ RandomErrorForTime2[0];
TmpTime[3] += /*MonteI **/ RandomErrorForTime2[1];
Vector3D TmpRightB = new Vector3D();
TmpRightB.x = 0;
TmpRightB.y = 0;
TmpRightB.z = 0;
#region position error
double[,] TmpPositions = new Double[, ] {
{ 1, 0, -1 / Math.Sqrt(2) }, { -1, 0, -1 / Math.Sqrt(2) }, { 0, 1, 1 / Math.Sqrt(2) }, { 0, -1, 1 / Math.Sqrt(2) }
};
//Add Positioning error for Sat0 - Sat3
Double[] RandomErrorForP = new Double[4];
RandomErrorForP[0] = 0.004 * tmpRandom.Next();
RandomErrorForP[1] = 0.003 * tmpRandom.Next();
RandomErrorForP[2] = 0.002 * tmpRandom.Next();
RandomErrorForP[3] = 0.001 * tmpRandom.Next();
for (int i = 0; i < 4; i++)
{
#region Generate Random Vector
double u = rforV.Next(0, 1);
double v = rforV.Next(0, 1);
double theta = 2 * Math.PI * u;
double phi = Math.Acos(2 * v - 1);
Vector3D randomv = new Vector3D();
randomv.x = Math.Sin(theta) * Math.Sin(phi);
randomv.y = Math.Cos(theta) * Math.Sin(phi);
randomv.z = Math.Cos(phi);
#endregion
TmpPositions[i, 0] += RandomErrorForP[i] * randomv.x;
TmpPositions[i, 1] += RandomErrorForP[i] * randomv.y;
TmpPositions[i, 2] += RandomErrorForP[i] * randomv.z;
//for (int j = 0; j < 3; j++)
// TmpPositions[i, j] += 0.001/**MonteI*/ * tmpRandom.Next();
}
CalMatrix TmpCalM = new CalMatrix();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
TmpCalM.Values[i, j] = 0;
for (int k = 0; k < 4; k++)
{
TmpCalM.Values[i, j] += TmpPositions[k, i] * TmpPositions[k, j];
}
}
}
TmpCalM.CalDet();
TmpCalM.InverseMatrix();
#endregion
for (int i = 0; i < 4; i++)
{
TmpRightB.x += TmpTime[i] * TmpPositions[i, 0];
TmpRightB.y += TmpTime[i] * TmpPositions[i, 1];
TmpRightB.z += TmpTime[i] * TmpPositions[i, 2];
}
//CalM.CalDet();
//CalM.InverseMatrix();
Vector3D TmpResult = Vector3D.VectorMultiMatrix(TmpRightB, TmpCalM);
double tmpv = 1.0 / Math.Sqrt(TmpResult.x * TmpResult.x + TmpResult.y * TmpResult.y + TmpResult.z * TmpResult.z);
//TmpResult.x *= tmpv;
//TmpResult.y *= tmpv;
//TmpResult.z *= tmpv;
double tmpAngle = Vector3D.AngleBetweenVector(TmpResult, mresult);
double Aresult = tmpAngle - 0.05;
//tw.WriteLine(RandomErrorForTime1[0].ToString() + " " + RandomErrorForTime1[1].ToString() + " " + RandomErrorForTime2[0].ToString() + " " + RandomErrorForTime2[1].ToString() + " " + TmpResult.x.ToString() + " " + TmpResult.y.ToString() + " " + TmpResult.z.ToString() + " " + (tmpv - v).ToString() + " " + tmpAngle.ToString());
//tw.WriteLine(TmpResult.x.ToString() + " " + TmpResult.y.ToString() + " " + TmpResult.z.ToString() + " " + (tmpv - v).ToString() + " " + tmpAngle.ToString());
tw.WriteLine(RandomErrorForTime1[0].ToString() + " " + RandomErrorForTime1[1].ToString() + " " + RandomErrorForTime2[0].ToString() + " " + RandomErrorForTime2[1].ToString() + " "
+ RandomErrorForP[0].ToString() + " " + RandomErrorForP[1].ToString() + " " + RandomErrorForP[2].ToString() + " " + RandomErrorForP[3].ToString() + " "
+ Aresult.ToString());
//+ (tmpv - v).ToString());
//Double[] RandomErrorForPos1 = Model.NormalDistribution();
//Double[] RandomErrorForPos2 = Model.NormalDistribution();
}
tw.Close();
}
#endregion
double canubelieveit = 0;
//tw.Close();
}
