// Here define A,B, and H Matrice private Joint kalmanFilterFull(int pos) { // Prepare //GeneralMatrix r = GeneralMatrix.Identity(3, 3); //R = r.MultiplyEquals(0.01); // Set r from Deviation Standard GeneralMatrix r = GeneralMatrix.Identity(3, 3); r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y R = r; // Set estimation from coordinate average in frame buffer GeneralMatrix Z = GeneralMatrix.Random(3, 1); Z.SetElement(0, 0, jointAveragePart[pos, 0]); Z.SetElement(1, 0, jointAveragePart[pos, 1]); Z.SetElement(2, 0, jointAveragePart[pos, 2]); // Predict GeneralMatrix Xp = F * Xk[pos]; GeneralMatrix Pp = F * P[pos] * F.Transpose() + Q; // Measurement update ( Correction ) K = Pp * H.Transpose() * (H * Pp * H.Transpose() + R).Inverse(); Xk[pos] = Xp + (K * (Z - (H * Xp))); GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension); P[pos] = (I - (K * H)) * Pp; return(convertMatrixTojoint(Xk[pos])); }
/// <summary> /// Distance in metres between two points expressed as longitude/latitude /// </summary> /// <param name="longLat0">longitude and latitude for first point</param> /// <param name="longLat1">longitude and latitude for second point</param> /// <returns></returns> public static double DistancePointToPointLongLat(LongLat longLat0, LongLat longLat1) { // use spherical coordinates: rho, phi, theta const double rho = 6378200; // earth radius in metres double sinPhi0 = Math.Sin(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI); double cosPhi0 = Math.Cos(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI); double sinTheta0 = Math.Sin(longLat0.Longitude / 180.0 * Math.PI); double cosTheta0 = Math.Cos(longLat0.Longitude / 180.0 * Math.PI); double sinPhi1 = Math.Sin(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI); double cosPhi1 = Math.Cos(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI); double sinTheta1 = Math.Sin(longLat1.Longitude / 180.0 * Math.PI); double cosTheta1 = Math.Cos(longLat1.Longitude / 180.0 * Math.PI); var p0 = new GeneralMatrix(3, 1); p0.SetElement(0, 0, rho * sinPhi0 * cosTheta0); p0.SetElement(1, 0, rho * sinPhi0 * sinTheta0); p0.SetElement(2, 0, rho * cosPhi0); var p1 = new GeneralMatrix(3, 1); p1.SetElement(0, 0, rho * sinPhi1 * cosTheta1); p1.SetElement(1, 0, rho * sinPhi1 * sinTheta1); p1.SetElement(2, 0, rho * cosPhi1); double distance = DistancePointToPoint(p0, p1); return(distance); }
/******************* Method Kalman Filter ******************/ // Variable A,B, and H we will asumtion this is a constant value // Most probably is 1. private Joint kalmanFilter(GeneralMatrix z, int pos) { // Prepare Joint join = new Joint(); GeneralMatrix r = GeneralMatrix.Identity(3, 3); r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y R = r; // Predict GeneralMatrix Xp = Xk[pos]; GeneralMatrix Pp = P[pos]; // Measurement Update (correction GeneralMatrix s = Pp + R; K = Pp * s.Inverse(); // Calculate Kalman Gain Xk[pos] = Xp + (K * (z - Xp)); GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension); P[pos] = (I - K) * Pp; estimationX = Xk[pos].GetElement(0, 0); estimationY = Xk[pos].GetElement(1, 0); estimationZ = Xk[pos].GetElement(2, 0); join.Position.X = (float)estimationX; join.Position.Y = (float)estimationY; join.Position.Z = (float)estimationZ; return(join); }
/// <summary> /// ( /// </summary> /// <param name="matrix"></param> /// <returns></returns> public static GeneralMatrix ExpandUtility(GeneralMatrix matrix) { double val = 0.0; int n = matrix.RowDimension; int m = matrix.ColumnDimension; if (n != m) { throw new ArgumentException("Criteria matrix must be symmetrical"); } GeneralMatrix newMatrix = matrix.Transpose(); //for all transposed elements calculate their inverse values //set diagonal elements to 0 for (int i = 0; i < n; i++) { for (int j = 0; j <= i; j++) { val = newMatrix.GetElement(i, j); if (val == 0.0) { throw new ArgumentException("Criteria comparison values van't be 0"); } newMatrix.SetElement(i, j, 1 / val); if (i == j) { newMatrix.SetElement(i, j, 0); } } } //add transposed, inverse matrix to the original one //create fully expanded matrix return(newMatrix.Add(matrix)); }
public void Visit(Compiled.Gp elem) { GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length); for (int i = 0; i < elem.Terms.Length; ++i) { cur.SetElement(0, i, ValueOf(elem.Terms[i])); } elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0); for (int i = 0; i < elem.Inputs.Length; ++i) { if (i < elem.dc) { //elem.Inputs[i].Weight = elem.Gpr.PartialDerivative(cur, i); cur.SetElement(0, i, cur.GetElement(0, i) + 0.1); elem.Inputs[i].Weight = (elem.Gpr.Evaluate(cur).GetElement(0, 0) - elem.Value) / 0.1; cur.SetElement(0, i, cur.GetElement(0, i) - 0.1); } else { elem.Inputs[i].Weight = 0; } } //Here Starts a dirty Hack /*double sum=0; * for(int n=0; n<elem.Inputs.Length; n++) { * sum += elem.Inputs[n].Weight; * } * if(sum < 0.00001) { * double val = elem.Value; * * double minDist = Double.MaxValue; * int iMin=-1; * double dist = 0, tmp; * for(int n=0; n<elem.X.RowDimension; n++) { * if(elem.Gpr.Y.GetElement(0, n) > val) { * dist = 0; * for(int m=0; m<elem.X.ColumnDimension; m++) { * tmp = elem.X.GetElement(n, m) - cur.GetElement(0, m); * dist += tmp*tmp; * } * if(dist < minDist) { * minDist = dist; * iMin = n; * } * } * } * if(iMin>=0) { * for(int m=0; m<elem.X.ColumnDimension; m++) { * elem.Inputs[m].Weight = elem.X.GetElement(m, iMin) - cur.GetElement(0, m); * } * } * }*/ }
/******************* End Function for calculate Angle ******************/ #endregion #region Support Function private GeneralMatrix convertJoinToMatrix(Joint j) { GeneralMatrix matrix = GeneralMatrix.Identity(3, 1); matrix.SetElement(0, 0, j.Position.X); matrix.SetElement(1, 0, j.Position.Y); matrix.SetElement(2, 0, j.Position.Z); return(matrix); }
public static GeneralMatrix To3x1Matrix(PointD pointD) { GeneralMatrix m = new GeneralMatrix(3, 1); m.SetElement(0, 0, pointD.X); m.SetElement(1, 0, pointD.Y); m.SetElement(2, 0, 1); return(m); }
public static GeneralMatrix To3x1Matrix(LongLat longLat) { GeneralMatrix m = new GeneralMatrix(3, 1); m.SetElement(0, 0, longLat.Longitude); m.SetElement(1, 0, longLat.Latitude); m.SetElement(2, 0, 1); return(m); }
private QFactorResult calculateLeastSquareLorenz() { int i, j = 0; int f0_index; PointPairInt range; int n; // Preprocessing (removing data points which are far away from transmittance peak) f0_index = findCenterFrequencyIndex(); if (f0_index < 0 || f0_index > pointList.Count) { return(new QFactorResult()); // return "no result" if center frequency not found. } range = findBandwidthIndexes(f0_index, 3.0F); if (range.I0 == int.MaxValue) { return(new QFactorResult()); // return "no result" if center frequency not found. } n = range.I1 - range.I0; if (n < 5) { // When we have very small amount of points, turn off Q-calculations completely - we don't want to have useless results! // We need to return the center frequency which is already found. return(new QFactorResult(pointList[f0_index].frequency, n)); } GeneralMatrix X = new GeneralMatrix(n, 3); // vandermonde matrix with measured frequency points GeneralMatrix Y = new GeneralMatrix(n, 3); // vector with measured power levels for (i = 0; i < pointList.Count; i++) { if (i > range.I0 && i <= range.I1) { X.SetElement(j, 0, Math.Pow(pointList[i].frequency, 2)); X.SetElement(j, 1, pointList[i].frequency); X.SetElement(j, 2, 1); Y.SetElement(j, 0, Math.Pow(10, -pointList[i].gain / 10)); // measured power is converted to linear form and inverted (1/x) j++; } } QRDecomposition QR = new QRDecomposition(X); GeneralMatrix beta = QR.Solve(Y); // Linear regression used to find beta coefficients double A = beta.GetElement(0, 0); double B = beta.GetElement(1, 0); double C = beta.GetElement(2, 0); double F0 = -B / (2 * A); double Q = -B / (2 * Math.Sqrt(4 * A * C - B * B)); double P0 = -1 / ((B * B) / (4 * A) - C); P0 = 10 * Math.Log10(P0); //return new QFactorResult(Q, F0, F0 - 0.5 * F0 / Q, F0 + 0.5 * F0 / Q, P0-3, P0-3); return(new QFactorResult(Q, F0, P0, F0 / Q, n)); }
public Point GetActualPosition(Point p) { GeneralMatrix mat = new GeneralMatrix(3, 1); mat.SetElement(0, 0, (double)p.X); mat.SetElement(1, 0, (double)p.Y); mat.SetElement(2, 0, 1.0); GeneralMatrix ret = m_transform.Inverse().Multiply(mat); return(new Point((int)ret.GetElement(0, 0), (int)ret.GetElement(1, 0))); }
public PointF GetActualDisplayPosition(PointF p) { GeneralMatrix mat = new GeneralMatrix(3, 1); mat.SetElement(0, 0, p.X); mat.SetElement(1, 0, p.Y); mat.SetElement(2, 0, 1.0); GeneralMatrix ret = m_transform.Multiply(mat); return(new PointF((float)ret.GetElement(0, 0), (float)ret.GetElement(1, 0))); }
public void TestNorm1() { GeneralMatrix _gm = new GeneralMatrix(2, 2); _gm.SetElement(0, 0, 1); _gm.SetElement(0, 1, 2); _gm.SetElement(1, 0, 3); _gm.SetElement(1, 1, 4); Assert.AreEqual(6, _gm.Norm1()); }
public void TestTranspose() { GeneralMatrix _gm = new GeneralMatrix(2, 2); _gm.SetElement(0, 0, 1); _gm.SetElement(0, 1, 2); _gm.SetElement(1, 0, 3); _gm.SetElement(1, 1, 4); GeneralMatrix _ngm = _gm.Transpose(); Assert.AreEqual(1, 0, 2); }
/// <summary> /// /// </summary> private void CalculatePriorities() { PrioritiesSelector selector = new PrioritiesSelector(); selector.ComputePriorities(_criteria); //first (zero-th) element of the lambda matrix is the //consistency ratio factor for the selection matrix _lambdas.SetElement(0, 0, selector.ConsistencyRatio); _orderedCriteria = selector.CalculatedMatrix; }
/// <summary> /// /// </summary> /// <param name="p0">First point on route</param> /// <param name="q0">First point on map</param> /// <param name="p1">Second point on route</param> /// <param name="q1">Second point on map</param> /// <param name="p2">Third point on route</param> /// <param name="q2">Third point on map</param> /// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param> /// <returns></returns> public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, PointD p2, PointD q2, GeneralMatrix fallbackMatrix) { try { var m = new GeneralMatrix(3, 3); m.SetElement(0, 0, p0.X); m.SetElement(0, 1, p0.Y); m.SetElement(0, 2, 1.0); m.SetElement(1, 0, p1.X); m.SetElement(1, 1, p1.Y); m.SetElement(1, 2, 1.0); m.SetElement(2, 0, p2.X); m.SetElement(2, 1, p2.Y); m.SetElement(2, 2, 1.0); var v1 = new GeneralMatrix(3, 1); v1.SetElement(0, 0, q0.X); v1.SetElement(1, 0, q1.X); v1.SetElement(2, 0, q2.X); var t1 = m.Inverse() * v1; var v2 = new GeneralMatrix(3, 1); v2.SetElement(0, 0, q0.Y); v2.SetElement(1, 0, q1.Y); v2.SetElement(2, 0, q2.Y); var t2 = m.Inverse() * v2; var v3 = new GeneralMatrix(3, 1); v3.SetElement(0, 0, 1.0); v3.SetElement(1, 0, 1.0); v3.SetElement(2, 0, 1.0); var t3 = m.Inverse() * v3; var t = new GeneralMatrix(3, 3); t.SetElement(0, 0, t1.GetElement(0, 0)); t.SetElement(0, 1, t1.GetElement(1, 0)); t.SetElement(0, 2, t1.GetElement(2, 0)); t.SetElement(1, 0, t2.GetElement(0, 0)); t.SetElement(1, 1, t2.GetElement(1, 0)); t.SetElement(1, 2, t2.GetElement(2, 0)); t.SetElement(2, 0, t3.GetElement(0, 0)); t.SetElement(2, 1, t3.GetElement(1, 0)); t.SetElement(2, 2, t3.GetElement(2, 0)); return(t); } catch (Exception) { return((GeneralMatrix)fallbackMatrix.Clone()); } }
public GeneralMatrix QuantizePolar(int grid_x, int grid_y) { // create a general matrix full of 0s GeneralMatrix mesh = new GeneralMatrix(grid_y, grid_x, 0.0); if (_points.Count == 0) { return(mesh); } double stepX = 2.0 * Math.PI / grid_x; // X is the theta coordinate double stepY = 1.0 / grid_y; // Y is the R coordinate foreach (GenericPoint pt in _points) { int x_index = (int)Math.Floor((pt.Theta(_polarCenter) + Math.PI) / stepX); double r = pt.R(_polarCenter, _averageDist); double t = pt.Theta(_polarCenter); int y_index = (int)Math.Floor(pt.R(_polarCenter, _averageDist) / stepY); if (y_index < grid_y && x_index < grid_x) { mesh.SetElement(y_index, x_index, 1.0); } } #if FALSE Console.WriteLine("Quantized polar"); printGeneralMatrix(ref mesh); #endif return(mesh); }
/// <summary> /// Calculates and saves the distance transform map for the screen coordinates. /// Uses _sMesh and saves to _sDTM. /// A distance transform map is a matrix in which every entry represents the /// distance from that point to the nearest point with ink. /// </summary> private void ScreenDistanceTransform() { List <Coord> indices = IndexList(_sMesh); _sDTM = new GeneralMatrix(GRID_Y_SIZE, GRID_X_SIZE, 0.0); for (int i = 0; i < GRID_Y_SIZE; i++) { for (int j = 0; j < GRID_X_SIZE; j++) { Coord cp = new Coord(j, i); double mindist = double.PositiveInfinity; foreach (Coord pt in indices) { double distan = cp.distanceTo(pt); if (distan < mindist) { mindist = distan; } } _sDTM.SetElement(i, j, mindist); } } }
/// <summary> /// Gets the 3D coordinates relative to the center of the earth. /// </summary> /// <returns></returns> public GeneralMatrix To3DPoint() { // use spherical coordinates: rho, phi, theta const double rho = 6378200; // earth radius in metres double sinPhi = Math.Sin(0.5 * Math.PI + latitude / 180.0 * Math.PI); double cosPhi = Math.Cos(0.5 * Math.PI + latitude / 180.0 * Math.PI); double sinTheta = Math.Sin(longitude / 180.0 * Math.PI); double cosTheta = Math.Cos(longitude / 180.0 * Math.PI); GeneralMatrix p = new GeneralMatrix(3, 1); p.SetElement(0, 0, rho * sinPhi * cosTheta); p.SetElement(1, 0, rho * sinPhi * sinTheta); p.SetElement(2, 0, rho * cosPhi); return(p); }
public void TestSolve() { GeneralMatrix _ls = new GeneralMatrix(2, 2); _ls.SetElement(0, 0, 1); _ls.SetElement(0, 1, 2); _ls.SetElement(1, 0, 3); _ls.SetElement(1, 1, 4); GeneralMatrix _rs = new GeneralMatrix(2, 1); _rs.SetElement(0, 0, -3); _rs.SetElement(1, 0, -5); GeneralMatrix _solution = _ls.Solve(_rs); Assert.AreEqual(_solution.GetElement(0, 0), 1); Assert.AreEqual(_solution.GetElement(1, 0), -2); }
public void Visit(Compiled.Gp elem) { GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length); for (int i = 0; i < elem.Terms.Length; ++i) { cur.SetElement(0, i, ValueOf(elem.Terms[i])); } elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0); }
// /// <summary> /// Calculates all elements for <code>this.alpha</code>. /// </summary> protected void UpdateAlpha() { for (int i = 0; i < parameters.Length; i++) { for (int j = 0; j < parameters.Length; j++) { alpha.SetElement(i, j, CalculateAlphaElement(i, j)); } } }
public void Visit(Compiled.Gp elem) { GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length); for (int i = 0; i < elem.Terms.Length; ++i) { cur.SetElement(0, i, ValueOf(elem.Terms[i])); } LocalDerivative = elem.Gpr.PartialDerivative(cur, ArgumentIndex); }
public void LUDecomposition() { GeneralMatrix A = new GeneralMatrix(columnwise, 4); int n = A.ColumnDimension; A = A.GetMatrix(0, n - 1, 0, n - 1); A.SetElement(0, 0, 0.0); LUDecomposition LU = A.LUD(); Assert.IsTrue(GeneralTests.Check(A.GetMatrix(LU.Pivot, 0, n - 1), LU.L.Multiply(LU.U))); }
public GeneralMatrix CalculateHessian(double[] x) { GeneralMatrix hessian = new GeneralMatrix(Dimension, Dimension); for (int i = 0; i < Dimension; i++) { for (int j = 0; j < Dimension; j++) { hessian.SetElement(i, j, GetPartialDerivativeVal(i, j, x)); } } return(hessian); }
/// <summary> /// Calculates final model results as a sum of product of choices rated /// for each criteria times weighing (preference) of each criteria /// </summary> private void CalculateFinalResult() { double sum; for (int i = 0; i < _mChoices; i++) { sum = 0; for (int j = 0; j < _nCriteria; j++) { sum += _modelResult.GetElement(i, j) * this._orderedCriteria.GetElement(j, 0); } _calculatedChoices.SetElement(i, 0, sum); } }
/// <summary> /// Average values of the priority matrix over sum of columns. /// set values of sum of averaged rows into a new matrix /// </summary> /// <param name="argMatrix"></param> /// <param name="selection"></param> public void PCalc(GeneralMatrix argMatrix, GeneralMatrix selection) { int n = argMatrix.ColumnDimension; GeneralMatrix sMatrix = new GeneralMatrix(argMatrix.ArrayCopy); double c = 0.0; int i, j; for (i = 0; i < sMatrix.ColumnDimension; i++) { c = 0.0; for (j = 0; j < sMatrix.RowDimension; j++) { c += sMatrix.GetElement(j, i); } selection.SetElement(i, 0, c); } for (i = 0; i < sMatrix.ColumnDimension; i++) { for (j = 0; j < sMatrix.RowDimension; j++) { sMatrix.SetElement(j, i, sMatrix.GetElement(j, i) / selection.GetElement(i, 0)); } } for (i = 0; i < sMatrix.RowDimension; i++) { c = 0.0; for (j = 0; j < sMatrix.ColumnDimension; j++) { c += sMatrix.GetElement(i, j); } selection.SetElement(i, 0, c / n); } }
public GeneralMatrix QuantizeScreen(int grid_x, int grid_y) { // create a general matrix full of 0s GeneralMatrix mesh = new GeneralMatrix(grid_x, grid_y, 0.0); // Find the bounding box of the points. System.Drawing.Rectangle bBox = BoundingBox(); if (_points.Count == 0) { return(mesh); } // find the step we want // Note: this is an intentional deviation from the original paper. // We do not maintain the original aspect ratio of the shape. double stepX = bBox.Width / (double)(grid_x - 1); double stepY = bBox.Height / (double)(grid_y - 1); // find the center of the image double cx = (double)(bBox.Left + bBox.Right) / 2.0; double cy = (double)(bBox.Top + bBox.Bottom) / 2.0; foreach (GenericPoint pt in _points) { // normalize the point's coordinates int x_index = (int)Math.Floor(((pt.X - cx) / stepX) + grid_x / 2); int y_index = (int)Math.Floor(((pt.Y - cy) / stepY) + grid_y / 2); if (x_index < grid_x && y_index < grid_y) { mesh.SetElement(y_index, x_index, 1.0); } } #if JESSI Console.WriteLine("Quantized screen"); Console.Write(mesh.ToString()); #endif return(mesh); }
/// <summary> /// Calculates and saves the distance transform map for the polar coordinates. /// Uses _pMesh and saves to _pDTM. /// A distance transform map is a matrix in which every entry represents the /// distance from that point to the nearest point with ink. /// </summary> private void PolarDistanceTransform() { List <Coord> indices = IndexList(_pMesh); for (int i = 0; i < GRID_Y_SIZE; i++) { for (int j = 0; j < GRID_X_SIZE; j++) { Coord cp = new Coord(j, i); double mindist = double.PositiveInfinity; foreach (Coord pt in indices) { // the straight distance between current point and current index double dx = Math.Abs(cp.X - pt.X); double dy = Math.Abs(cp.Y - pt.Y); double straightDist = Math.Sqrt(dx * dx + dy * dy); dx = (double)GRID_X_SIZE - dx; double wrapDist = Math.Sqrt(dx * dx + dy * dy); double distan = Math.Min(straightDist, wrapDist); if (distan < mindist) { mindist = distan; } } _pDTM.SetElement(i, j, mindist); } } #if FALSE Console.WriteLine("Your polar distance transform map:"); Console.Write(_pDTM.ToString()) #endif }
/// <summary> /// /// </summary> /// <param name="p0">First point on route, in projected (metric) coordinates relative to projection origin</param> /// <param name="q0">First point on map, in pixels</param> /// <param name="p1">Second point on route, in projected (metric) coordinates relative to projection origin</param> /// <param name="q1">Second point on map, in pixels</param> /// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param> /// <param name="useRotation">If true, assumes orthogonal map and calculates scale and rotation. If false, calculates different scale in x and y directions and no rotation.</param> /// <returns></returns> public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, GeneralMatrix fallbackMatrix, bool useRotation) { try { if (useRotation) { // note that we need to mirror y pixel value in x axis double angleDifferece = GetAngleR(p1 - p0, new PointD(q1.X, -q1.Y) - new PointD(q0.X, -q0.Y)); double lengthQ = DistancePointToPoint(q0, q1); double lengthP = DistancePointToPoint(p0, p1); double scaleFactor = lengthP == 0 ? 0 : lengthQ / lengthP; double cos = Math.Cos(angleDifferece); double sin = Math.Sin(angleDifferece); // translation to origo in metric space var a = new GeneralMatrix(3, 3); a.SetElement(0, 0, 1); a.SetElement(0, 1, 0); a.SetElement(0, 2, -p0.X); a.SetElement(1, 0, 0); a.SetElement(1, 1, 1); a.SetElement(1, 2, -p0.Y); a.SetElement(2, 0, 0); a.SetElement(2, 1, 0); a.SetElement(2, 2, 1); // rotation var b = new GeneralMatrix(3, 3); b.SetElement(0, 0, cos); b.SetElement(0, 1, -sin); b.SetElement(0, 2, 0); b.SetElement(1, 0, sin); b.SetElement(1, 1, cos); b.SetElement(1, 2, 0); b.SetElement(2, 0, 0); b.SetElement(2, 1, 0); b.SetElement(2, 2, 1); // scaling, note that we need to mirror y scale around x axis var c = new GeneralMatrix(3, 3); c.SetElement(0, 0, scaleFactor); c.SetElement(0, 1, 0); c.SetElement(0, 2, 0); c.SetElement(1, 0, 0); c.SetElement(1, 1, -scaleFactor); c.SetElement(1, 2, 0); c.SetElement(2, 0, 0); c.SetElement(2, 1, 0); c.SetElement(2, 2, 1); // translation from origo to pixel space var d = new GeneralMatrix(3, 3); d.SetElement(0, 0, 1); d.SetElement(0, 1, 0); d.SetElement(0, 2, q0.X); d.SetElement(1, 0, 0); d.SetElement(1, 1, 1); d.SetElement(1, 2, q0.Y); d.SetElement(2, 0, 0); d.SetElement(2, 1, 0); d.SetElement(2, 2, 1); return(d * c * b * a); } else // useRotation == false { var m1 = new GeneralMatrix(2, 2); m1.SetElement(0, 0, p0.X); m1.SetElement(0, 1, 1); m1.SetElement(1, 0, p1.X); m1.SetElement(1, 1, 1); var v1 = new GeneralMatrix(2, 1); v1.SetElement(0, 0, q0.X); v1.SetElement(1, 0, q1.X); var t1 = m1.Inverse() * v1; var m2 = new GeneralMatrix(2, 2); m2.SetElement(0, 0, p0.Y); m2.SetElement(0, 1, 1); m2.SetElement(1, 0, p1.Y); m2.SetElement(1, 1, 1); var v2 = new GeneralMatrix(2, 1); v2.SetElement(0, 0, q0.Y); v2.SetElement(1, 0, q1.Y); var t2 = m2.Inverse() * v2; var t = new GeneralMatrix(3, 3); t.SetElement(0, 0, t1.GetElement(0, 0)); t.SetElement(0, 1, 0); t.SetElement(0, 2, t1.GetElement(1, 0)); t.SetElement(1, 0, 0); t.SetElement(1, 1, t2.GetElement(0, 0)); t.SetElement(1, 2, t2.GetElement(1, 0)); t.SetElement(2, 0, 0); t.SetElement(2, 1, 0); t.SetElement(2, 2, 1); return(t); } } catch (Exception) { return((GeneralMatrix)fallbackMatrix.Clone()); } }
public void BadSetValue1() { B.SetElement(B.RowDimension, B.ColumnDimension - 1, 0.0); }