public void Add()
{
VectorD v1 = new VectorD(new double[] { 1, 2, 3, 4, 5 });
VectorD v2 = new VectorD(new double[] { 6, 7, 8, 9, 10 });
VectorD v3 = VectorD.Add(v1, v2);
Assert.AreEqual(new VectorD(new double[] { 7, 9, 11, 13, 15 }), v3);
}
/// <summary>
/// Load程序后，第一次加载显示Pattern内容后，拍摄Mark点校正Pattern原点及轨迹命令坐标
/// </summary>
/// <param name="patternOldOrigin">Pattern原点被校正前的位置</param>
/// <param name="coordinateTransformer">根据Mark点拍摄结果生成的坐标校正器</param>
/// <param name="patternNewOrigin">Pattern原点被校正后的位置</param>
public override void Correct(PointD patternOldOrigin, CoordinateTransformer coordinateTransformer, PointD patternNewOrigin)
{
//根据拍照目标位置更新系统坐标
//if (!this.modelFindPrm.IsUnStandard)
//{
// VectorD v = modelFindPrm.TargetInMachine.ToSystem() - patternNewOrigin.ToSystem();
// PosInPattern.X = v.X;
// PosInPattern.Y = v.Y;
//}
//else
//{
// PointD oldPosMachine = (PosInPattern + patternOldOrigin.ToSystem()).ToMachine();
// PointD newPosMachine = coordinateTransformer.Transform(oldPosMachine);
// VectorD v = newPosMachine.ToSystem() - patternNewOrigin.ToSystem();
// PosInPattern.X = v.X;
// PosInPattern.Y = v.Y;
//}
// 此时已经传入了坐标校正器和新的pattern原点，可以直接用旧的理论坐标转换为新的理论坐标
// 语义层使用 TargetInMachine 有风险，可能为null，或是之前的目标值。
PointD oldPosMachine = (PosInPattern + patternOldOrigin.ToSystem()).ToMachine();
PointD newPosMachine = coordinateTransformer.Transform(oldPosMachine);
VectorD v = newPosMachine.ToSystem() - patternNewOrigin.ToSystem();
PosInPattern.X = v.X;
PosInPattern.Y = v.Y;
}
private void btnRegister_Click(object sender, EventArgs e)
{
if (markCmdLine.ModelFindPrm.TargetInMachine == null)
{
return;
}
VectorD v = markCmdLine.ModelFindPrm.TargetInMachine.ToSystem() - pattern.GetOriginSys();
this.txtStandardX1.Text = (markCmdLine.ModelFindPrm.TargetInMachine.X - origin.X).ToString();
this.txtStandardY1.Text = (markCmdLine.ModelFindPrm.TargetInMachine.Y - origin.Y).ToString();
markCmdLine.ModelFindPrm.ReferenceX = v.X;
markCmdLine.ModelFindPrm.ReferenceY = v.Y;
if (markCmdLine.ModelFindPrm.UnStandardType == 0)
{
this.txtStandardA.Text = markCmdLine.ModelFindPrm.Angle.ToString();
markCmdLine.ModelFindPrm.ReferenceA = markCmdLine.ModelFindPrm.Angle;
this.txtStandardX2.Text = 0.00.ToString("0.000");
this.txtStandardY2.Text = 0.00.ToString("0.000");
markCmdLine.ModelFindPrm.ReferenceX2 = 0;
markCmdLine.ModelFindPrm.ReferenceY2 = 0;
}
else
{
VectorD v2 = markCmdLine.ModelFindPrm.TargetInMachine2.ToSystem() - pattern.GetOriginSys();
this.txtStandardA.Text = 0.00.ToString("0.000");
markCmdLine.ModelFindPrm.ReferenceA = 0;
this.txtStandardX2.Text = (markCmdLine.ModelFindPrm.TargetInMachine2.X - origin.X).ToString();
this.txtStandardY2.Text = (markCmdLine.ModelFindPrm.TargetInMachine2.Y - origin.Y).ToString();
markCmdLine.ModelFindPrm.ReferenceX2 = v2.X;
markCmdLine.ModelFindPrm.ReferenceY2 = v2.Y;
}
}
void DrawOrbit(ref VectorD rv)
{
#if false
Debug.Log("pos: " + Global.pos[0] + " " + Global.pos[2]);
Debug.Log("vel: " + Global.vel[0] + " " + Global.vel[2]);
#endif
//VectorD rv = Util.convertToRv(ref Global.pos, ref Global.vel);
//VectorD rv = calcNextStep(rv, params_);
var oe = Util.rv2oe(OrbitData.parentGM, rv);
#if true
System.Text.StringBuilder sb = new System.Text.StringBuilder();
sb.Append("Orbital Vector\n");
sb.Append("aop: " + (oe.aop * Mathf.Rad2Deg).ToString("#.0") + "\n");
sb.Append("inc: " + (oe.inc * Mathf.Rad2Deg).ToString("#.0") + "\n");
sb.Append("lan: " + (oe.lan * Mathf.Rad2Deg).ToString("#.0") + "\n");
textRef.text = sb.ToString();
#endif
//build rotation
var aop = Quaternion.AngleAxis((float)oe.aop * Mathf.Rad2Deg, Vector3.up);
var inc = Quaternion.AngleAxis((float)oe.inc * Mathf.Rad2Deg, Vector3.right);
var lan = Quaternion.AngleAxis((float)oe.lan * Mathf.Rad2Deg, Vector3.up);
var rotq = lan * inc * aop;
drawOrbitalPath((float)oe.tra, (float)oe.sma, (float)oe.ecc, rotq);
}
public bool FindIntersection(Line2D other, out VectorD intersection)
{
/* http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
* Now there are five cases:
* If r × s = 0 and (q − p) × r = 0, then the two lines are collinear. If in addition, either 0 ≤ (q − p) · r ≤ r · r or 0 ≤ (p − q) · s ≤ s · s, then the two lines are overlapping.
* If r × s = 0 and (q − p) × r = 0, but neither 0 ≤ (q − p) · r ≤ r · r nor 0 ≤ (p − q) · s ≤ s · s, then the two lines are collinear but disjoint.
* If r × s = 0 and (q − p) × r ≠ 0, then the two lines are parallel and non-intersecting.
* If r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t r = q + u s.
* Otherwise, the two line segments are not parallel but do not intersect.
*/
// line1.Start = p
// line2.Start = q
var r = Stop - Start;
var s = other.Stop - other.Start;
var startPointVector = (other.Start - Start);
var denom = r.Cross(s); // denom = r × s
var firstScalar = startPointVector.Cross(s) / denom; // firstScalar = t
var secondScalar = startPointVector.Cross(r) / denom; // secondScalar = u
intersection = Start + (firstScalar * r);
// ReSharper disable CompareOfFloatsByEqualityOperator
return denom != 0d && firstScalar >= 0d && firstScalar <= 1d && secondScalar >= 0d && secondScalar <= 1d;
// ReSharper restore CompareOfFloatsByEqualityOperator
}
/// <summary>
/// 取两点所在直线上的指定长度的向量
/// </summary>
/// <param name="lineStart">线起点</param>
/// <param name="lineEnd">线终点</param>
/// <param name="length">向量长度</param>
/// <returns></returns>
private static VectorD GetVectorOnLineByLength(PointD lineStart, PointD lineEnd, double length)
{
double scale = length / lineStart.DistanceTo(lineEnd);
VectorD vectorOld = lineEnd - lineStart;
return(vectorOld.ScaleBy(scale));
}
public static PointD GetScalePointOnLine(PointD lineStart, PointD lineEnd, double scale)
{
VectorD vLine = lineEnd - lineStart;
PointD result = lineStart + vLine.ScaleBy(scale);
return(result);
}
//--------------------------------------------------------------
/// <summary>
/// Creates the principal component analysis for the given list of points.
/// </summary>
/// <param name="points">
/// The list of data points. All points must have the same
/// <see cref="VectorD.NumberOfElements"/>.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="points"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="points"/> is empty.
/// </exception>
public PrincipalComponentAnalysisD(IList<VectorD> points)
{
if (points == null)
throw new ArgumentNullException("points");
if (points.Count == 0)
throw new ArgumentException("The list of points is empty.");
// Compute covariance matrix.
MatrixD covarianceMatrix = StatisticsHelper.ComputeCovarianceMatrix(points);
// Perform Eigenvalue decomposition.
EigenvalueDecompositionD evd = new EigenvalueDecompositionD(covarianceMatrix);
int numberOfElements = evd.RealEigenvalues.NumberOfElements;
Variances = new VectorD(numberOfElements);
V = new MatrixD(numberOfElements, numberOfElements);
// Sort eigenvalues by decreasing value.
// Since covarianceMatrix is symmetric, we have no imaginary eigenvalues.
for (int i = 0; i < Variances.NumberOfElements; i++)
{
int index = evd.RealEigenvalues.IndexOfLargestElement;
Variances[i] = evd.RealEigenvalues[index];
V.SetColumn(i, evd.V.GetColumn(index));
evd.RealEigenvalues[index] = double.NegativeInfinity;
}
}
public bool FindIntersection(Line2D other, out VectorD intersection)
{
/* http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
* Now there are five cases:
* If r × s = 0 and (q − p) × r = 0, then the two lines are collinear. If in addition, either 0 ≤ (q − p) · r ≤ r · r or 0 ≤ (p − q) · s ≤ s · s, then the two lines are overlapping.
* If r × s = 0 and (q − p) × r = 0, but neither 0 ≤ (q − p) · r ≤ r · r nor 0 ≤ (p − q) · s ≤ s · s, then the two lines are collinear but disjoint.
* If r × s = 0 and (q − p) × r ≠ 0, then the two lines are parallel and non-intersecting.
* If r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t r = q + u s.
* Otherwise, the two line segments are not parallel but do not intersect.
*/
// line1.Start = p
// line2.Start = q
var r = Stop - Start;
var s = other.Stop - other.Start;
var startPointVector = (other.Start - Start);
var denom = r.Cross(s); // denom = r × s
var firstScalar = startPointVector.Cross(s) / denom; // firstScalar = t
var secondScalar = startPointVector.Cross(r) / denom; // secondScalar = u
intersection = Start + (firstScalar * r);
// ReSharper disable CompareOfFloatsByEqualityOperator
return(denom != 0d && firstScalar >= 0d && firstScalar <= 1d && secondScalar >= 0d && secondScalar <= 1d);
// ReSharper restore CompareOfFloatsByEqualityOperator
}
/// <summary>
/// 获取直线到圆弧的过渡圆弧
/// </summary>
/// <param name="lineStart">线起点</param>
/// <param name="intersection">线与圆弧交点</param>
/// <param name="oriArcCenter">圆弧圆心</param>
/// <param name="transitionAngle">过渡舍去的圆弧角度，单位度</param>
/// <returns>过渡圆弧的起点、圆心、终点</returns>
public static List <PointD> GetLineToArcTransitionArc(PointD lineStart, PointD intersection, PointD oriArcCenter, double transitionAngle, double oriArcClockwise)
{
List <PointD> points = new List <PointD>();
// 获取圆弧上过渡的切点(即过渡圆弧的终点)
VectorD vOriArcStart = intersection - oriArcCenter;
if (oriArcClockwise == 0)
{
transitionAngle = -transitionAngle;
}
VectorD temp = vOriArcStart.Rotate(transitionAngle);
PointD newArcStart = oriArcCenter + temp;
// 做圆弧切点切线，与原直线相交得到新的拐角交点
VectorD tempV1 = temp.Rotate(90);
PointD inflexionLinePoint1 = newArcStart + tempV1 * 10;
PointD inflexionLinePoint2 = newArcStart - tempV1 * 10;
PointD newIntersection = GetIntersection(lineStart, intersection, inflexionLinePoint1, inflexionLinePoint2);
// 得到新的拐角角度
double newAngle = GetAngleOfTwoLine(lineStart, newIntersection, newArcStart);
// 求得新的拐角圆弧半径
VectorD tempV2 = newArcStart - newIntersection;
double r = Math.Tan(newAngle / 2) * tempV2.Length;
points = GetLineTransitionArc(lineStart, newIntersection, newArcStart, r);
return(points);
}
/// <summary>
/// 获取直线与直线拐角过渡圆弧的圆心、起点、终点
/// 返回结果[起点、圆心、终点]
/// </summary>
/// <param name="start">直线1起点</param>
/// <param name="intersection">两线交点</param>
/// <param name="end">直线2终点</param>
/// <param name="r">过渡半径</param>
/// <returns>过渡圆弧的起点、圆心、终点</returns>
public static List <PointD> GetLineTransitionArc(PointD start, PointD intersection, PointD end, double r)
{
List <PointD> arcPoints = new List <PointD>();
// 角平分线与对边交点
PointD point1 = GetAPointOnAngleBiscetor(start, intersection, end);
// 两线夹角
double angle = GetAngleOfTwoLine(start, intersection, end);
// 圆心到拐点距离
double len1 = r / Math.Sin(angle / 2);
// 求得过渡圆弧半径为r时，圆心坐标
VectorD vector1 = GetVectorOnLineByLength(intersection, point1, len1);
PointD arcCenter = intersection + vector1;
// 圆弧起点(终点)到拐点距离
double len2 = Math.Sqrt(len1 * len1 - r * r);
// 求圆弧起点
VectorD vector2 = GetVectorOnLineByLength(intersection, start, len2);
PointD arcStart = intersection + vector2;
// 求圆弧终点
VectorD vector3 = GetVectorOnLineByLength(intersection, end, len2);
PointD arcEnd = intersection + vector3;
arcPoints.Add(arcStart);
arcPoints.Add(arcCenter);
arcPoints.Add(arcEnd);
return(arcPoints);
}
public static double CalculateDistance(VectorD vectorA, VectorD vectorB, DistanceMode mode)
{
if (vectorA.Count != vectorB.Count)
{
throw new Exception("Both vectors should have the same number of elemetns in order to calculate a distance between them!");
}
switch (mode)
{
case DistanceMode.Euclidean:
var sum = vectorA.Select((t, i) => Math.Pow(t - vectorB[i], 2.0)).Sum();
return(Math.Sqrt(sum));
case DistanceMode.Manhattan:
var sum2 = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Sum();
return(sum2);
case DistanceMode.Max:
var max = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Max();
return(max);
case DistanceMode.Min:
var min = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Min();
return(min);
case DistanceMode.Levenshtein:
var l = LevenshteinDistance(vectorA, vectorB);
return(l);
default:
throw new Exception("Unknown distance mode!");
}
}
public void TestWarmStarting()
{
MatrixD A = new MatrixD(new double[, ] {
{ -21, 2, -4, 0 },
{ 2, 3, 0.1, -1 },
{ 2, 10, 111.1, -11 },
{ 23, 112, 111.1, -143 }
});
VectorD b = new VectorD(new double[] { 20, 28, -12, 0.1 });
SorMethodD solver = new SorMethodD();
VectorD x = solver.Solve(A, null, b);
int fullIterationCount = solver.NumberOfIterations;
VectorD solution = MatrixD.SolveLinearEquations(A, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(solution, x));
// Now test make separate solve calls with warm-starting
solver.MaxNumberOfIterations = 3;
x = solver.Solve(A, null, b);
Assert.AreEqual(3, solver.NumberOfIterations);
solver.MaxNumberOfIterations = 100;
x = solver.Solve(A, x, b);
Assert.AreEqual(fullIterationCount, solver.NumberOfIterations + 3);
}
public void Test6()
{
MatrixD A = new MatrixD(new double[, ] {
{ -21, 2, -4, 0 },
{ 2, 3, 0.1, -1 },
{ 2, 10, 111.1, -11 },
{ 23, 112, 111.1, -143 }
});
VectorD b = new VectorD(new double[] { 20, 28, -12, 0.1 });
SorMethodD solver = new SorMethodD();
solver.MaxNumberOfIterations = 4;
VectorD x = solver.Solve(A, null, b);
VectorD solution = MatrixD.SolveLinearEquations(A, b);
Assert.IsFalse(VectorD.AreNumericallyEqual(solution, x));
Assert.AreEqual(4, solver.NumberOfIterations);
// Compare with Gauss-Seidel. Must be equal.
GaussSeidelMethodD gsSolver = new GaussSeidelMethodD();
gsSolver.MaxNumberOfIterations = 4;
VectorD gsSolution = gsSolver.Solve(A, null, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(gsSolution, x));
}
public void Init()
{
eventManager = GameObject.Find("EventManager").GetComponent<EventManager>();
rv = new VectorD();
rv.Resize(6);
Debug.Log("position " + transform.localPosition);
rv[0] = transform.localPosition.x * HoloManager.SolScale;
rv[1] = transform.localPosition.y * HoloManager.SolScale;
rv[2] = transform.localPosition.z * HoloManager.SolScale;
Debug.Log(name + " real dist: " + OVTools.FormatDistance(getRFloat().magnitude));
//comput velocity assuming pos is in the origin
//TODO use double operations?
var vel = Vector3.Cross(transform.localPosition, Vector3.up).normalized;
vel *= Mathf.Sqrt((float)parentGM / (HoloManager.SolScale * transform.localPosition.magnitude));
rv[3] = vel.x;
rv[4] = vel.y;
rv[5] = vel.z;
//set oe
var tempoe = Util.rv2oe(parentGM, rv);
SetOE(tempoe);
params_ = new VectorD();
params_.Resize(7);
params_[0] = 0;
params_[1] = 0;
params_[2] = 0;
params_[3] = parentGM;
params_[4] = 0;
params_[5] = 0;
params_[6] = 0;
}
public void TestCorrelation2()
{
var data = new VectorD[] {
new VectorD(6, 10),
new VectorD(10, 13),
new VectorD(6, 8),
new VectorD(10, 15),
new VectorD(5, 8),
new VectorD(3, 6),
new VectorD(5, 9),
new VectorD(9, 10),
new VectorD(3, 7),
new VectorD(3, 3),
new VectorD(11, 18),
new VectorD(6, 14),
new VectorD(11, 18),
new VectorD(9, 11),
new VectorD(7, 12),
new VectorD(5, 5),
new VectorD(8, 7),
new VectorD(7, 12),
new VectorD(7, 7),
new VectorD(9, 7)
};
var cor = BaseStatistics.Correlation(data, 0, 1);
Assert.AreEqual<double>(0.749659, Math.Round(cor, 6));
}
/// <summary>
/// Computes the new state x1 at time t1.
/// </summary>
/// <param name="x0">The state x0 at time t0.</param>
/// <param name="t0">The time t0.</param>
/// <param name="t1">The target time t1 for which the new state x1 is computed.</param>
/// <returns>The new state x1 at time t1.</returns>
public override VectorD Integrate(VectorD x0, double t0, double t1)
{
double dt = (t1 - t0);
VectorD d = FirstOrderDerivative(x0, t0);
VectorD result = x0 + dt * d;
return result;
}
private void btnGoToOffset_Click(object sender, EventArgs e)
{
Machine.Instance.Robot.MoveSafeZ();
VectorD vecSta2End;
PointD start;
PointD end;
if (this.isRunning)
{
start = this.line.LineCoordinateList[this.indexOfLineCoordinate].Start;
vecSta2End = this.line.LineCoordinateList[this.indexOfLineCoordinate].End - this.line.LineCoordinateList[this.indexOfLineCoordinate].Start;
}
else
{
start = origin + this.lineCoordinateCache[this.indexOfLineCoordinate].Start;
end = origin + this.lineCoordinateCache[this.indexOfLineCoordinate].End;
vecSta2End = end - start;
}
if (Math.Abs(this.offset) > 5)
{
//MessageBox.Show("The setting offset is out of range[-5.0 5.0] ", null, MessageBoxButtons.OKCancel);
MessageBox.Show("设置的值超出范围：[-5.0 5.0] ", null, MessageBoxButtons.OKCancel);
return;
}
VectorD offsetVec = vecSta2End.Normalize() * this.offset;
Machine.Instance.Robot.ManualMovePosXY(offsetVec.X + start.X, offsetVec.Y + start.Y);
}
public void wrong_size_throws()
{
var a = new VectorD(5);
var b = new VectorD(3);
Assert.Throws <ArgumentOutOfRangeException>(() => a.GetDistanceSquared(b));
}
public void TestVectorDProd()
{
var vector = new VectorD(2.5, 3.0, 4.1);
var prod = Math.Round(vector.Prod(), 2);
Assert.AreEqual<double>(30.75, prod);
}
public void TestRandomRegularA()
{
RandomHelper.Random = new Random(1);
for (int i = 0; i < 100; i++)
{
VectorD column1 = new VectorD(3);
RandomHelper.Random.NextVectorD(column1, 1, 2);
VectorD column2 = new VectorD(3);
RandomHelper.Random.NextVectorD(column2, 1, 2);
// Make linearly independent.
if (column1 / column1[0] == column2 / column2[0])
{
column2[0]++;
}
// Create linearly independent third column.
VectorD column3 = column1 + column2;
column3[1]++;
// Create A.
MatrixD a = new MatrixD(3, 3);
a.SetColumn(0, column1);
a.SetColumn(1, column2);
a.SetColumn(2, column3);
SingularValueDecompositionD svd = new SingularValueDecompositionD(a);
Assert.AreEqual(3, svd.NumericalRank);
Assert.IsTrue(MatrixD.AreNumericallyEqual(a, svd.U * svd.S * svd.V.Transposed));
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
double condNumber = svd.ConditionNumber;
}
}
public void projecting_different_dimensions_throws()
{
var u = new VectorD(5);
var v = new VectorD(3);
Assert.Throws <ArgumentOutOfRangeException>(() => u.GetProjected(v));
}
public void adding_null_vector_throws()
{
var left = new VectorD(3);
VectorD right = null;
Assert.Throws <ArgumentNullException>(() => left.GetSum(right));
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
/// <summary>
/// Creates the principal component analysis for the given list of points.
/// </summary>
/// <param name="points">
/// The list of data points. All points must have the same
/// <see cref="VectorD.NumberOfElements"/>.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="points"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="points"/> is empty.
/// </exception>
public PrincipalComponentAnalysisD(IList <VectorD> points)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
if (points.Count == 0)
{
throw new ArgumentException("The list of points is empty.");
}
// Compute covariance matrix.
MatrixD covarianceMatrix = StatisticsHelper.ComputeCovarianceMatrix(points);
// Perform Eigenvalue decomposition.
EigenvalueDecompositionD evd = new EigenvalueDecompositionD(covarianceMatrix);
int numberOfElements = evd.RealEigenvalues.NumberOfElements;
Variances = new VectorD(numberOfElements);
V = new MatrixD(numberOfElements, numberOfElements);
// Sort eigenvalues by decreasing value.
// Since covarianceMatrix is symmetric, we have no imaginary eigenvalues.
for (int i = 0; i < Variances.NumberOfElements; i++)
{
int index = evd.RealEigenvalues.IndexOfLargestElement;
Variances[i] = evd.RealEigenvalues[index];
V.SetColumn(i, evd.V.GetColumn(index));
evd.RealEigenvalues[index] = double.NegativeInfinity;
}
}
public void CompileAndEvaluate_ModifyInputScript_ElementsOfInputListShouldBeSetToZero()
{
var processor = new MonoCSharpProcessor();
processor.AddReferenceAssembly(typeof(MyMath.MyMath).Assembly);
var parameters = new Dictionary <string, object> {
{ "v1", new VectorD() }
};
var script = processor.Compile(ModifyInputScript, parameters);
Assert.IsTrue(script.IsCompiled);
Assert.IsTrue(String.IsNullOrEmpty(processor.MessageOutput.ToString()));
var v = new VectorD()
{
1, 2, 3, 4, 5
};;
parameters["v1"] = v;
processor.Evaluate(script, parameters);
Assert.IsTrue(v.All(e => e == 0));
v = new VectorD()
{
1, 2, 3, 4, 5
};
parameters["v1"] = v;
processor.Evaluate(parameters);
Assert.IsTrue(v.All(e => e == 0));
}
public void DoNotCompile_CallEvaluateByPassingOnlyParameters_ModifyInputScript_ElementsOfInputListShouldBeSetToZero()
{
var processor = new MonoCSharpProcessor();
processor.AddReferenceAssembly(typeof(MyMath.MyMath).Assembly);
var parameters = new Dictionary <string, object> {
{ "v1", new VectorD() }
};
var v = new VectorD()
{
1, 2, 3, 4, 5
};;
parameters["v1"] = v;
processor.Evaluate(ModifyInputScript, parameters);
Assert.IsTrue(v.All(e => e == 0));
v = new VectorD()
{
1, 2, 3, 4, 5
};
parameters["v1"] = v;
processor.Evaluate(parameters);
Assert.IsTrue(v.All(e => e == 0));
}
public static Vector3 rk4(float dt, Vector3 pos, Vector3 vel, Vector3 parentPos, float gm, Vector3 accel)
{
var t0 = (float)dt;
VectorD x0 = new VectorD();
x0.Resize(6);
//pos,vel
x0[0] = pos.x;
x0[1] = pos.y;
x0[2] = pos.z;
x0[3] = vel.x;
x0[4] = vel.y;
x0[5] = vel.z;
VectorD params_ = new VectorD();
params_.Resize(7);
//parent pos, gm, accel
params_[0] = parentPos.x;
params_[1] = parentPos.y;
params_[2] = parentPos.z;
params_[3] = gm;
params_[4] = accel.x;
params_[5] = accel.y;
params_[6] = accel.z;
var result = rungeKutta4(0, t0, x0, params_);
var ret = new Vector3((float)result[3], (float)result[4], (float)result[5]);
return(ret);
}
public void DoNotCompile_CallEvaluateByPassingOnlyParameters_CalculateDistanceScript_ExceptionShouldBeThrown()
{
var processor = new MonoCSharpProcessor();
processor.AddReferenceAssembly(typeof(MyMath.MyMath).Assembly);
var parameters = new Dictionary <string, object> {
{ "v1", new VectorD() }, { "v2", new VectorD() }
};
parameters["v1"] = new VectorD()
{
1, 2, 3, 4, 5
};
parameters["v2"] = new VectorD()
{
-1, -2, -3, -4, -5
};
var distance = processor.Evaluate <double>(CalculateDistanceScript, parameters);
Assert.AreEqual(5, distance);
distance = processor.Evaluate <double>(parameters);
}
public static VectorD dynamics(float t, VectorD x, VectorD params_)
{
Debug.Assert(x.Count == 6);
Debug.Assert(params_.Count == 7);
VectorD dx = new VectorD();
dx.Resize(6);
//the derivative of position is velocity
dx[0] = x[3];
dx[1] = x[4];
dx[2] = x[5];
//distance between the two bodies
float distance = 0;
distance += (x[0] - params_[0]) * (x[0] - params_[0]);
distance += (x[1] - params_[1]) * (x[1] - params_[1]);
distance += (x[2] - params_[2]) * (x[2] - params_[2]);
distance = Mathf.Sqrt(distance);
//acceleration due to gravity
dx[3] = -params_[3] * (x[0] - params_[0]) / Mathf.Pow(distance, 3);
dx[4] = -params_[3] * (x[1] - params_[1]) / Mathf.Pow(distance, 3);
dx[5] = -params_[3] * (x[2] - params_[2]) / Mathf.Pow(distance, 3);
//perturbing acceleration
dx[3] += params_[4];
dx[4] += params_[5];
dx[5] += params_[6];
return(dx);
}
public void subtracting_null_vector_throws()
{
var left = new VectorD(3);
VectorD right = null;
Assert.Throws <ArgumentNullException>(() => left.Subtract(right));
}
public void different_sizes_throws()
{
var left = new VectorD(5);
var right = new VectorD(3);
Assert.Throws <ArgumentOutOfRangeException>(() => left.GetDot(right));
}
void Start()
{
rv = new VectorD();
rv.Resize(6);
Debug.Log("position " + transform.position);
rv[0] = transform.position.x;
rv[1] = transform.position.y;
rv[2] = transform.position.z;
//comput velocity assuming pos is in the origin
var vel = Vector3.Cross(transform.position, Vector3.up).normalized;
vel *= Mathf.Sqrt((float)parentGM / transform.position.magnitude);
rv[3] = vel.x;
rv[4] = vel.y;
rv[5] = vel.z;
params_ = new VectorD();
params_.Resize(7);
params_[0] = 0;
params_[1] = 0;
params_[2] = 0;
params_[3] = parentGM;
params_[4] = 0;
params_[5] = 0;
params_[6] = 0;
}
/// <summary>
/// Creates the eigenvalue decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">The square matrix A.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is non-square (rectangular).
/// </exception>
public EigenvalueDecompositionD(MatrixD matrixA)
{
if (matrixA == null)
{
throw new ArgumentNullException("matrixA");
}
if (matrixA.IsSquare == false)
{
throw new ArgumentException("The matrix A must be square.", "matrixA");
}
_n = matrixA.NumberOfColumns;
_d = new VectorD(_n);
_e = new VectorD(_n);
_isSymmetric = matrixA.IsSymmetric;
if (_isSymmetric)
{
_v = matrixA.Clone();
// Tridiagonalize.
ReduceToTridiagonal();
// Diagonalize.
TridiagonalToQL();
}
else
{
_v = new MatrixD(_n, _n);
// Abort if A contains NaN values.
// If we continue with NaN values, we run into an infinite loop.
for (int i = 0; i < _n; i++)
{
for (int j = 0; j < _n; j++)
{
if (Numeric.IsNaN(matrixA[i, j]))
{
_e.Set(double.NaN);
_v.Set(double.NaN);
_d.Set(double.NaN);
return;
}
}
}
// Storage of nonsymmetric Hessenberg form.
MatrixD matrixH = matrixA.Clone();
// Working storage for nonsymmetric algorithm.
double[] ort = new double[_n];
// Reduce to Hessenberg form.
ReduceToHessenberg(matrixH, ort);
// Reduce Hessenberg to real Schur form.
HessenbergToRealSchur(matrixH);
}
}
public void TestRandomSpdA()
{
RandomHelper.Random = new Random(1);
// Every transpose(A) * A is SPD if A has full column rank and m>n.
for (int i = 0; i < 100; i++)
{
VectorD column1 = new VectorD(4);
RandomHelper.Random.NextVectorD(column1, 1, 2);
VectorD column2 = new VectorD(4);
RandomHelper.Random.NextVectorD(column2, 1, 2);
// Make linearly independent.
if (column1 / column1[0] == column2 / column2[0])
{
column2[0]++;
}
// Create linearly independent third column.
VectorD column3 = column1 + column2;
column3[1]++;
// Create A.
MatrixD a = new MatrixD(4, 3);
a.SetColumn(0, column1);
a.SetColumn(1, column2);
a.SetColumn(2, column3);
MatrixD spdMatrix = a.Transposed * a;
CholeskyDecompositionD d = new CholeskyDecompositionD(spdMatrix);
Assert.AreEqual(true, d.IsSymmetricPositiveDefinite);
MatrixD l = d.L;
// Test if L is a lower triangular matrix.
for (int j = 0; j < l.NumberOfRows; j++)
{
for (int k = 0; k < l.NumberOfColumns; k++)
{
if (j < k)
{
Assert.AreEqual(0, l[j, k]);
}
}
}
Assert.IsTrue(MatrixD.AreNumericallyEqual(spdMatrix, l * l.Transposed));
// Check solving of linear equations.
MatrixD b = new MatrixD(3, 2);
RandomHelper.Random.NextMatrixD(b, 0, 1);
MatrixD x = d.SolveLinearEquations(b);
MatrixD b2 = spdMatrix * x;
Assert.IsTrue(MatrixD.AreNumericallyEqual(b, b2, 0.01f));
}
}
public void Test()
{
// Make a random list.
RandomHelper.Random = new Random(77);
List <VectorD> points = new List <VectorD>();
for (int i = 0; i < 10; i++)
{
var vector = new VectorD(4);
RandomHelper.Random.NextVectorD(vector, -1, 10);
points.Add(vector);
}
PrincipalComponentAnalysisD pca = new PrincipalComponentAnalysisD(points);
Assert.Greater(pca.Variances[0], pca.Variances[1]);
Assert.Greater(pca.Variances[1], pca.Variances[2]);
Assert.Greater(pca.Variances[2], pca.Variances[3]);
Assert.Greater(pca.Variances[3], 0);
Assert.IsTrue(pca.V.GetColumn(0).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(1).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(2).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(3).IsNumericallyNormalized);
// Compute covariance matrix and check if it is diagonal in the transformed space.
MatrixD cov = StatisticsHelper.ComputeCovarianceMatrix(points);
MatrixD transformedCov = pca.V.Transposed * cov * pca.V;
for (int row = 0; row < transformedCov.NumberOfRows; row++)
{
for (int column = 0; column < transformedCov.NumberOfColumns; column++)
{
if (row != column)
{
Assert.IsTrue(Numeric.IsZero(transformedCov[row, column]));
}
}
}
// The principal components must be Eigenvectors which means that multiplying with the covariance
// matrix does only change the length!
VectorD v0 = pca.V.GetColumn(0);
VectorD v0Result = cov * v0;
Assert.IsTrue(VectorD.AreNumericallyEqual(v0.Normalized, v0Result.Normalized));
VectorD v1 = pca.V.GetColumn(1);
VectorD v1Result = cov * v1;
Assert.IsTrue(VectorD.AreNumericallyEqual(v1.Normalized, v1Result.Normalized));
VectorD v2 = pca.V.GetColumn(2);
VectorD v2Result = cov * v2;
Assert.IsTrue(VectorD.AreNumericallyEqual(v2.Normalized, v2Result.Normalized));
VectorD v3 = pca.V.GetColumn(3);
VectorD v3Result = cov * v3;
Assert.IsTrue(VectorD.AreNumericallyEqual(v3.Normalized, v3Result.Normalized));
}
public VectorD GetFirstOrderDerivatives(VectorD x, double t)
{
// A dummy function: f(x[index], t) = index * t;
VectorD result = new VectorD(x.NumberOfElements);
for (int i = 0; i < result.NumberOfElements; i++)
result[i] = i*t;
return result;
}
public void Test1()
{
VectorD state = new VectorD(new double[]{ 0, 1 });
VectorD result = new RungeKutta4IntegratorD(GetFirstOrderDerivatives).Integrate(state, 2, 2.5);
Assert.AreEqual(0, result[0]);
Assert.AreEqual(3.06103515625, result[1]);
}
public void Test1()
{
VectorD state = new VectorD(new double[] { 0, 1 });
VectorD result = new RungeKutta4IntegratorD(GetFirstOrderDerivatives).Integrate(state, 2, 2.5);
Assert.AreEqual(0, result[0]);
Assert.AreEqual(3.06103515625, result[1]);
}
public void TestUnion()
{
var x = new VectorD(2, 4, 6, 8);
var y = new VectorD(4, 8, 12, 14, 16);
var union = x.Union(y);
Assert.AreEqual<VectorD>(
new VectorD(2, 4, 6, 8, 12, 14, 16),
union);
}
public void TestIntersect()
{
var x = new VectorD(2, 4, 6, 8);
var y = new VectorD(4, 8, 12, 14, 16);
var intersect = x.Intersect(y);
Assert.AreEqual<VectorD>(
new VectorD(4, 8),
intersect);
}
//--------------------------------------------------------------
/// <summary>
/// Creates the eigenvalue decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">The square matrix A.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is non-square (rectangular).
/// </exception>
public EigenvalueDecompositionD(MatrixD matrixA)
{
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (matrixA.IsSquare == false)
throw new ArgumentException("The matrix A must be square.", "matrixA");
_n = matrixA.NumberOfColumns;
_d = new VectorD(_n);
_e = new VectorD(_n);
_isSymmetric = matrixA.IsSymmetric;
if (_isSymmetric)
{
_v = matrixA.Clone();
// Tridiagonalize.
ReduceToTridiagonal();
// Diagonalize.
TridiagonalToQL();
}
else
{
_v = new MatrixD(_n, _n);
// Abort if A contains NaN values.
// If we continue with NaN values, we run into an infinite loop.
for (int i = 0; i < _n; i++)
{
for (int j = 0; j < _n; j++)
{
if (Numeric.IsNaN(matrixA[i, j]))
{
_e.Set(double.NaN);
_v.Set(double.NaN);
_d.Set(double.NaN);
return;
}
}
}
// Storage of nonsymmetric Hessenberg form.
MatrixD matrixH = matrixA.Clone();
// Working storage for nonsymmetric algorithm.
double[] ort = new double[_n];
// Reduce to Hessenberg form.
ReduceToHessenberg(matrixH, ort);
// Reduce Hessenberg to real Schur form.
HessenbergToRealSchur(matrixH);
}
}
public void Test1()
{
MatrixD A = new MatrixD(new double[,] { { 4 } });
VectorD b = new VectorD(new double[] { 20 });
SorMethodD solver = new SorMethodD();
VectorD x = solver.Solve(A, null, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(new VectorD(1, 5), x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void SolveWithDefaultInitialGuess()
{
MatrixD A = new MatrixD(new double[,] { { 4 } });
VectorD b = new VectorD(new double[] { 20 });
JacobiMethodD solver = new JacobiMethodD();
VectorD x = solver.Solve(A, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(new VectorD(1, 5), x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test4()
{
MatrixD A = new MatrixD(new double[,] { { -12, 2 },
{ 2, 3 }});
VectorD b = new VectorD(new double[] { 20, 28 });
SorMethodD solver = new SorMethodD();
VectorD x = solver.Solve(A, null, b);
VectorD solution = MatrixD.SolveLinearEquations(A, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(solution, x));
}
public void Test1()
{
VectorD state = new VectorD (new double[] { 0, 1, 2, 3, 4, 5 });
VectorD result = new MidpointIntegratorD(GetFirstOrderDerivatives).Integrate(state, 2, 2.5);
Assert.AreEqual(0, result[0]);
Assert.AreEqual(2.125, result[1]);
Assert.AreEqual(4.25, result[2]);
Assert.AreEqual(6.375, result[3]);
Assert.AreEqual(8.5, result[4]);
Assert.AreEqual(10.625, result[5]);
}
public void Test2()
{
MatrixD A = new MatrixD(new double[,] { { 1, 0 },
{ 0, 1 }});
VectorD b = new VectorD(new double[] { 20, 28 });
JacobiMethodD solver = new JacobiMethodD();
VectorD x = solver.Solve(A, null, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(b, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test1()
{
VectorD state = new VectorD (new double[]{ 1, 2, 3, 4, 5, 6 });
VectorD result = new ExplicitEulerIntegratorD(GetFirstOrderDerivatives).Integrate(state, 2, 2.5);
Assert.AreEqual(1, result[0]);
Assert.AreEqual(3, result[1]);
Assert.AreEqual(5, result[2]);
Assert.AreEqual(7, result[3]);
Assert.AreEqual(9, result[4]);
Assert.AreEqual(11, result[5]);
}
public void Test3()
{
MatrixD A = new MatrixD(new double[,] { { 2, 0 },
{ 0, 2 }});
VectorD b = new VectorD(new double[] { 20, 28 });
GaussSeidelMethodD solver = new GaussSeidelMethodD();
VectorD x = solver.Solve(A, null, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(b / 2, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void TestSolveGE()
{
var a = new MatrixD(new double[] { 1, 4, 4, 2, 2, 5, 3, 1, 1, -2, -3, 1, 1, 4, 1, 3 }, 4, 4);
var b = new VectorD(new double[] { -1, -7, -12, 2 });
var c = SLESolver.Solve(a, b, SLEAlgorithm.GE);
Assert.AreEqual<int>(4, c.Length);
Assert.AreEqual<double>(-2.0000, Math.Round(c[0], 4));
Assert.AreEqual<double>(-1.0000, Math.Round(c[1], 4));
Assert.AreEqual<double>(1.0000, Math.Round(c[2], 4));
Assert.AreEqual<double>(2.0000, Math.Round(c[3], 4));
}
public static VectorD Solve(MatrixD a, VectorD b, SLEAlgorithm algorithm)
{
switch (algorithm)
{
case SLEAlgorithm.GE:
return SolveByGE(a, b);
case SLEAlgorithm.GEPP:
return SolveByGEPP(a, b);
case SLEAlgorithm.LU:
return SolveByLU(a, b);
}
return null;
}
public void Test5()
{
MatrixD A = new MatrixD(new double[,] { { -21, 2, -4, 0 },
{ 2, 3, 0.1, -1 },
{ 2, 10, 111.1, -11 },
{ 23, 112, 111.1, -143 }});
VectorD b = new VectorD(new double[] { 20, 28, -12, 0.1 });
GaussSeidelMethodD solver = new GaussSeidelMethodD();
VectorD x = solver.Solve(A, null, b);
VectorD solution = MatrixD.SolveLinearEquations(A, b);
Assert.IsTrue(VectorD.AreNumericallyEqual(solution, x));
}
public void Test()
{
// Make a random list.
RandomHelper.Random = new Random(77);
List<VectorD> points = new List<VectorD>();
for (int i = 0; i < 10; i++)
{
var vector = new VectorD(4);
RandomHelper.Random.NextVectorD(vector, -1, 10);
points.Add(vector);
}
PrincipalComponentAnalysisD pca = new PrincipalComponentAnalysisD(points);
Assert.Greater(pca.Variances[0], pca.Variances[1]);
Assert.Greater(pca.Variances[1], pca.Variances[2]);
Assert.Greater(pca.Variances[2], pca.Variances[3]);
Assert.Greater(pca.Variances[3], 0);
Assert.IsTrue(pca.V.GetColumn(0).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(1).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(2).IsNumericallyNormalized);
Assert.IsTrue(pca.V.GetColumn(3).IsNumericallyNormalized);
// Compute covariance matrix and check if it is diagonal in the transformed space.
MatrixD cov = StatisticsHelper.ComputeCovarianceMatrix(points);
MatrixD transformedCov = pca.V.Transposed * cov * pca.V;
for (int row = 0; row < transformedCov.NumberOfRows; row++)
for (int column = 0; column < transformedCov.NumberOfColumns; column++)
if (row != column)
Assert.IsTrue(Numeric.IsZero(transformedCov[row, column]));
// The principal components must be Eigenvectors which means that multiplying with the covariance
// matrix does only change the length!
VectorD v0 = pca.V.GetColumn(0);
VectorD v0Result = cov * v0;
Assert.IsTrue(VectorD.AreNumericallyEqual(v0.Normalized, v0Result.Normalized));
VectorD v1 = pca.V.GetColumn(1);
VectorD v1Result = cov * v1;
Assert.IsTrue(VectorD.AreNumericallyEqual(v1.Normalized, v1Result.Normalized));
VectorD v2 = pca.V.GetColumn(2);
VectorD v2Result = cov * v2;
Assert.IsTrue(VectorD.AreNumericallyEqual(v2.Normalized, v2Result.Normalized));
VectorD v3 = pca.V.GetColumn(3);
VectorD v3Result = cov * v3;
Assert.IsTrue(VectorD.AreNumericallyEqual(v3.Normalized, v3Result.Normalized));
}
public void TestNorm()
{
var m1 = new VectorD(1.0, 2.0, 3.0, 4.0).ToMatrix(2, 2);
var normO = m1.Norm(MatrixNormType.O);
Assert.AreEqual<double>(7.0, normO);
var normI = m1.Norm(MatrixNormType.I);
Assert.AreEqual<double>(6.0, normI);
var normF = Math.Round(m1.Norm(MatrixNormType.F), 6);
Assert.AreEqual<double>(5.477226, normF);
var normM = m1.Norm(MatrixNormType.M);
Assert.AreEqual<double>(4.0, normM);
}
public void Absolute()
{
VectorD v = new VectorD(new double[] { -1, -2, -3, -4 });
v.Absolute();
Assert.AreEqual(1, v[0]);
Assert.AreEqual(2, v[1]);
Assert.AreEqual(3, v[2]);
Assert.AreEqual(4, v[3]);
v = new VectorD(new double[] { 1, 2, 3, 4 });
v.Absolute();
Assert.AreEqual(1, v[0]);
Assert.AreEqual(2, v[1]);
Assert.AreEqual(3, v[2]);
Assert.AreEqual(4, v[3]);
}
/// <summary>
/// Solves the specified linear system of equations <i>Ax=b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="initialX">
/// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used
/// as initial guess.
/// </param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="vectorB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is not a square matrix.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of elements of <paramref name="initialX"/> does not match.
/// </exception>
public override VectorD Solve(MatrixD matrixA, VectorD initialX, VectorD vectorB)
{
// TODO: We can possible improve the method by reordering after each step.
// This can be done randomly or we sort by the "convergence" of the elements.
// See book Physics-Based Animation.
NumberOfIterations = 0;
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (vectorB == null)
throw new ArgumentNullException("vectorB");
if (matrixA.IsSquare == false)
throw new ArgumentException("Matrix A must be a square matrix.", "matrixA");
if (matrixA.NumberOfRows != vectorB.NumberOfElements)
throw new ArgumentException("The number of rows of A and b do not match.");
if (initialX != null && initialX.NumberOfElements != vectorB.NumberOfElements)
throw new ArgumentException("The number of elements of the initial guess for x and b do not match.");
VectorD xOld = initialX ?? new VectorD(vectorB.NumberOfElements);
VectorD xNew = new VectorD(vectorB.NumberOfElements);
bool isConverged = false;
// Make iterations until max iteration count or the result has converged.
for (int i = 0; i < MaxNumberOfIterations && !isConverged; i++)
{
for (int j = 0; j < vectorB.NumberOfElements; j++)
{
double delta = 0;
for (int k = 0; k < j; k++)
delta += matrixA[j, k] * xOld[k];
for (int k = j + 1; k < vectorB.NumberOfElements; k++)
delta += matrixA[j, k] * xOld[k];
xNew[j] = (vectorB[j] - delta) / matrixA[j, j];
}
// Test convergence
isConverged = VectorD.AreNumericallyEqual(xOld, xNew, Epsilon);
xOld = xNew.Clone();
NumberOfIterations = i + 1;
}
return xNew;
}
/// <summary>
/// 部分ピボット選択付きガウス消去法により連立一次方程式の解を求める
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static VectorD SolveByGEPP(MatrixD a, VectorD b)
{
int r = 0;
int c = 0;
var a2 = a.Clone();
var b2 = b.Clone();
while (r < a2.Rows - 1)
{
var maxA = a2.GetColumn(c).Where((x, i) => i >= r).Max();
var idxMaxA = a2.GetRowIndex((x, i) => (i >= r) && (x[c] == maxA));
if (idxMaxA != r)
{
a2.SwapRow(r, idxMaxA);
b2.Swap(r, idxMaxA);
}
for (int i = r + 1; i < a2.Rows; i++)
{
var x = a2[i, c] * -1.0;
a2[i, c] = 0;
for (int j = c + 1; j < a2.Columns; j++)
{
a2[i, j] += a2[r, j] * (x / a2[r, c]);
}
b2[i] += b2[r] * (x / a2[r, c]);
}
r++;
c++;
}
var n = b2.Length;
var result = new VectorD(new double[n]);
for (int k = n - 1; k >= 0; k--)
{
var sum = 0.0;
for (int j = k + 1; j < n; j++)
{
sum += a2[k, j] * result[j];
}
result[k] = (b2[k] - sum) / a2[k, k];
}
return result;
}
/// <summary>
/// Computes the new state x1 at time t1.
/// </summary>
/// <param name="x0">The state x0 at time t0.</param>
/// <param name="t0">The time t0.</param>
/// <param name="t1">The target time t1 for which the new state x1 is computed.</param>
/// <returns>The new state x1 at time t1.</returns>
public override VectorD Integrate(VectorD x0, double t0, double t1)
{
double dt = (t1 - t0);
VectorD d1 = FirstOrderDerivative(x0, t0);
VectorD tmp = x0 + dt / 2 * d1;
VectorD d2 = FirstOrderDerivative(tmp, t0 + dt / 2);
tmp = x0 + dt / 2 * d2;
VectorD d3 = FirstOrderDerivative(tmp, t0 + dt / 2);
tmp = x0 + dt * d3;
VectorD d4 = FirstOrderDerivative(tmp, t1);
VectorD result = x0 + dt / 6 * d1 + dt / 3 * d2 + dt / 3 * d3 + dt / 6 * d4;
return result;
}
public void AbsoluteStatic()
{
VectorD v = new VectorD(new double[] { -1, -2, -3, -4 });
VectorD absoluteV = VectorD.Absolute(v);
Assert.AreEqual(1, absoluteV[0]);
Assert.AreEqual(2, absoluteV[1]);
Assert.AreEqual(3, absoluteV[2]);
Assert.AreEqual(4, absoluteV[3]);
v = new VectorD(new double[] { 1, 2, 3, 4 });
absoluteV = VectorD.Absolute(v);
Assert.AreEqual(1, absoluteV[0]);
Assert.AreEqual(2, absoluteV[1]);
Assert.AreEqual(3, absoluteV[2]);
Assert.AreEqual(4, absoluteV[3]);
Assert.IsNull(VectorD.Absolute(null));
}
/// <summary>
/// Solves the specified linear system of equations <i>Ax=b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="initialX">
/// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used
/// as initial guess.
/// </param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="vectorB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is not a square matrix.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of elements of <paramref name="initialX"/> does not match.
/// </exception>
public override VectorD Solve(MatrixD matrixA, VectorD initialX, VectorD vectorB)
{
NumberOfIterations = 0;
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (vectorB == null)
throw new ArgumentNullException("vectorB");
if (matrixA.IsSquare == false)
throw new ArgumentException("Matrix A must be a square matrix.", "matrixA");
if (matrixA.NumberOfRows != vectorB.NumberOfElements)
throw new ArgumentException("The number of rows of A and b do not match.");
if (initialX != null && initialX.NumberOfElements != vectorB.NumberOfElements)
throw new ArgumentException("The number of elements of the initial guess for x and b do not match.");
VectorD xOld = initialX ?? new VectorD(vectorB.NumberOfElements);
VectorD xNew = new VectorD(vectorB.NumberOfElements);
bool isConverged = false;
// Make iterations until max iteration count or the result has converged.
for (int i = 0; i < MaxNumberOfIterations && !isConverged; i++)
{
for (int j=0; j<vectorB.NumberOfElements; j++)
{
double delta = 0;
for (int k=0; k < j; k++)
delta += matrixA[j, k] * xNew[k];
for (int k=j+1; k < vectorB.NumberOfElements; k++)
delta += matrixA[j, k] * xOld[k];
delta = (vectorB[j] - delta) / matrixA[j, j];
xNew[j] = xOld[j] + RelaxationFactor * (delta - xOld[j]);
}
// Test convergence
isConverged = VectorD.AreNumericallyEqual(xOld, xNew, Epsilon);
xOld = xNew.Clone();
NumberOfIterations = i + 1;
}
return xNew;
}
public void TestSolveGEPP()
{
//var a = new MatrixD(new double[] { 0, 1, 3, 1, 0, 1, 2, 3, 0 }, 3, 3);
//var b = new VectorD(new double[] { 2, 2, -3 });
//var c = SLESolver.Solve(a, b, SLEAlgorithm.GEPP);
//Assert.AreEqual<int>(3, c.Length);
//Assert.AreEqual<double>(-1.0000, Math.Round(c[0], 4));
//Assert.AreEqual<double>(0, Math.Round(c[1], 4));
//Assert.AreEqual<double>(1.0000, Math.Round(c[2], 4));
var a = new MatrixD(new double[] { 2, -1, 4, 5, 4, -2, 2, -4, 1, 2, -3, -3, -3, 4, 5, 1 }, 4, 4);
var b = new VectorD(new double[] { 0, 10, 2, 6 });
var c = SLESolver.Solve(a, b, SLEAlgorithm.GEPP);
Assert.AreEqual<int>(4, c.Length);
Assert.AreEqual<double>(2.0000, Math.Round(c[0], 4));
Assert.AreEqual<double>(-1.0000, Math.Round(c[1], 4));
Assert.AreEqual<double>(3.0000, Math.Round(c[2], 4));
Assert.AreEqual<double>(1.0000, Math.Round(c[3], 4));
}