/// <summary> /// Initializes a new instance of the CSVD class, /// computing the Singular Value Decomposition of a general complex matrix, /// with the possibility of computing only the singular values. /// </summary> /// <param name="matrix">A general complex matrix.</param> /// <param name="singularValuesOnly">A value that indicating whether only the singular values will be computed.</param> public CSVD(CMatrix matrix, bool singularValuesOnly) { _m = matrix.RowCount; _n = matrix.ColumnCount; var nm = Math.Min(matrix.RowCount, matrix.ColumnCount); var s = new Complex[nm]; var u = new Complex[matrix.RowCount * matrix.RowCount]; var vt = new Complex[matrix.ColumnCount * matrix.ColumnCount]; new ManagedLinearAlgebraProvider().SingularValueDecomposition(!singularValuesOnly, matrix.To1DimArray(), matrix.RowCount, matrix.ColumnCount, s, u, vt); _s = s.ToArray().Select(x => x.Re).ToArray(); _u = new CMatrix(matrix.RowCount, matrix.RowCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.RowCount; j++) { _u[i, j] = u[j * matrix.RowCount + i]; } } _vt = new CMatrix(matrix.ColumnCount, matrix.ColumnCount); for (int i = 0; i < matrix.ColumnCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) { _vt[i, j] = vt[j * matrix.ColumnCount + i]; } } }
/// <summary> /// Returns a vector of approximate values of roots of a complex polynomial by companion matrix method. /// </summary> /// <param name="poly">A complex polynomial.</param> /// <returns>The approximate values of roots of poly.</returns> /// <exception cref="System.ArgumentException"> /// Number of elements in coeffs is less than 2 or more than 99. /// </exception> public static Complex[] CompanionMatrixRootsFinding(this CPolynomial poly) { // Remove zero elements standing at the end. int lidx = 0; int ridx = poly.Length - 1; while (ridx >= 0 && poly[ridx] == Complex.Zero) ridx--; while (lidx < poly.Length && poly[lidx] == Complex.Zero) lidx++; int length = ridx + 1; if (length < 2 || length > 99) throw new ArgumentException("Number of coefficients must be between 1 and 100."); int rootsCount = length - 1; Complex[] roots = new Complex[rootsCount]; int n = ridx - lidx; CMatrix companionMatrix = new CMatrix(n, n); for (int i = 1; i < n; i++) companionMatrix[i, i - 1] = Complex.One; for (int i = 0; i < n; i++) companionMatrix[i, n - 1] = -poly[lidx + i] / poly[ridx]; CMatrix eigenvals = CMatrix.Eigenvalues(companionMatrix); for (int i = 0; i < n; i++) roots[i] = eigenvals[i]; Array.Sort<Complex>(roots, new ComplexComparer()); return roots; }
/// <summary> /// Initializes a new instance of the CEigenproblem class, computing /// the eigenvalues and optionally the right eigenvectors of a square complex matrix. /// </summary> /// <param name="matrix">A complex square matrix.</param> /// <param name="rightEigenvectors">A value that indicating whether the right eigenvectors will be computed.</param> public CEigenproblem(CMatrix matrix, bool rightEigenvectors) { if (!matrix.IsSquare) { throw new MatrixSizeMismatchException("The matrix must be square."); } var order = matrix.RowCount; // Initialize matrices for eigenvalues and eigenvectors var eigenVectors = new Complex[order * order]; var blockDiagonal = new Complex[order * order]; var eigenValues = new Complex[order]; new ManagedLinearAlgebraProvider().EigenDecomp(matrix.IsHermitian(), order, matrix.To1DimArray(), eigenVectors, eigenValues, blockDiagonal); _vals = eigenValues; _rightvecs = new CMatrix(matrix.RowCount, matrix.ColumnCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) { _rightvecs[i, j] = eigenVectors[j * matrix.RowCount + i]; } } }
/// <summary> /// Initializes a new instance of the CSVD class, /// computing the Singular Value Decomposition of a general complex matrix, /// with the possibility of computing only the singular values. /// </summary> /// <param name="matrix">A general complex matrix.</param> /// <param name="singularValuesOnly">A value that indicating whether only the singular values will be computed.</param> public CSVD(CMatrix matrix, bool singularValuesOnly) { _m = matrix.RowCount; _n = matrix.ColumnCount; var nm = Math.Min(matrix.RowCount, matrix.ColumnCount); var s = new Complex[nm]; var u = new Complex[matrix.RowCount * matrix.RowCount]; var vt = new Complex[matrix.ColumnCount * matrix.ColumnCount]; new ManagedLinearAlgebraProvider().SingularValueDecomposition(!singularValuesOnly, matrix.To1DimArray(), matrix.RowCount, matrix.ColumnCount, s, u, vt); _s = s.ToArray().Select(x => x.Re).ToArray(); _u = new CMatrix(matrix.RowCount, matrix.RowCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.RowCount; j++) { _u[i, j] = u[j * matrix.RowCount + i]; } } _vt = new CMatrix(matrix.ColumnCount, matrix.ColumnCount); for (int i = 0; i < matrix.ColumnCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) { _vt[i, j] = vt[j * matrix.ColumnCount + i]; } } }
public static CMatrix Mul(CMatrix p_ma, CMatrix p_mb) { if (p_ma.m_cols != p_mb.m_rows) { return(null); } CMatrix res = new CMatrix(p_ma.m_rows, p_mb.m_cols); int y = p_ma.m_cols; for (int i = 0; i < res.m_rows; i++) { for (int j = 0; j < res.m_cols; j++) { float s = 0.0f; for (int x = 0; x < y; x++) { s += p_ma[i, x] * p_mb[x, j]; // if (i == 1 && j == 0) // { // Debug.Log(" a"+i + "," + x + " * b" + x + "," + j + " +"); // } } // Debug.Log(i+","+j+"="+s); res[i, j] = s; } } return(res); }
public static void assertEquals(CMatrix A, CMatrix B, float tol) { assertShape(A, B); Complex_F32 a = new Complex_F32(); Complex_F32 b = new Complex_F32(); for (int i = 0; i < A.getNumRows(); i++) { for (int j = 0; j < A.getNumCols(); j++) { A.get(i, j, a); B.get(i, j, b); assertTrue(!float.IsNaN(a.real) && !float.IsNaN(b.real), "Real At (" + i + "," + j + ") A = " + a.real + " B = " + b.real); assertTrue(!float.IsInfinity(a.real) && !float.IsInfinity(b.real), "Real At (" + i + "," + j + ") A = " + a.real + " B = " + b.real); assertTrue(Math.Abs(a.real - b.real) <= tol, "Real At (" + i + "," + j + ") A = " + a.real + " B = " + b.real); assertTrue(!float.IsNaN(a.imaginary) && !float.IsNaN(b.imaginary), "Img At (" + i + "," + j + ") A = " + a.imaginary + " B = " + b.imaginary); assertTrue(!float.IsInfinity(a.imaginary) && !float.IsInfinity(b.imaginary), "Img At (" + i + "," + j + ") A = " + a.imaginary + " B = " + b.imaginary); assertTrue(Math.Abs(a.imaginary - b.imaginary) <= tol, "Img At (" + i + "," + j + ") A = " + a.imaginary + " B = " + b.imaginary); } } }
public static CMatrix calculateJacobian(List <Joint> p_joints, List <GameObject> p_jointObjs, List <Vector3> p_dofs, List <int> p_dofJointIds, Vector3 p_targetPos) { // If GPGPU here // First, read dofjoint id, then position from joint array to registry // This is now done in the loop below of course // Then also read targetpos, dof into memory // The J matrix is in global memory and is written in the end (also do transpose, so really Jt) if (p_dofs.Count == 0) { return(null); } // This means all Jt's are computed in parallel // One Jt per dof // Construct Jacobian matrix CMatrix J = new CMatrix(3, p_dofs.Count); // 3 is position in xyz for (int i = 0; i < p_dofs.Count; i++) // this is then the "thread pool" { int id = p_dofJointIds[i]; Joint joint = p_joints[id]; Vector3 linkPos = joint.m_position; // Currently only solve for given axis Vector3 rotAxis = p_jointObjs[id].transform.TransformDirection(p_dofs[i]); Vector3 dirTarget = Vector3.Cross(rotAxis, p_targetPos - linkPos); J[0, i] = dirTarget.x; J[1, i] = dirTarget.y; J[2, i] = dirTarget.z; } return(J); }
public override object Evaluate() { CMatrix m = LeftExpression.EvaluateAsCMatrix(); int order = RightExpression.EvaluateAsInt32(); return(MathStats.PopulationCentralMoment(m, order)); }
public static CMatrix calculateJacobian(List<Joint> p_joints, List<GameObject> p_jointObjs, List<Vector3> p_dofs, List<int> p_dofJointIds, Vector3 p_targetPos) { // If GPGPU here // First, read dofjoint id, then position from joint array to registry // This is now done in the loop below of course // Then also read targetpos, dof into memory // The J matrix is in global memory and is written in the end (also do transpose, so really Jt) if (p_dofs.Count == 0) return null; // This means all Jt's are computed in parallel // One Jt per dof // Construct Jacobian matrix CMatrix J = new CMatrix(3, p_dofs.Count); // 3 is position in xyz for (int i = 0; i < p_dofs.Count; i++) // this is then the "thread pool" { int id=p_dofJointIds[i]; Joint joint = p_joints[id]; Vector3 linkPos = joint.m_position; // Currently only solve for given axis Vector3 rotAxis = p_jointObjs[id].transform.TransformDirection(p_dofs[i]); Vector3 dirTarget = Vector3.Cross(rotAxis, p_targetPos - linkPos); J[0, i] = dirTarget.x; J[1, i] = dirTarget.y; J[2, i] = dirTarget.z; } return J; }
/// <summary> /// Initializes a new instance of the CEigenproblem class, computing /// the eigenvalues and optionally the right eigenvectors of a square complex matrix. /// </summary> /// <param name="matrix">A complex square matrix.</param> /// <param name="rightEigenvectors">A value that indicating whether the right eigenvectors will be computed.</param> public CEigenproblem(CMatrix matrix, bool rightEigenvectors) { if (!matrix.IsSquare) throw new MatrixSizeMismatchException("The matrix must be square."); var order = matrix.RowCount; // Initialize matrices for eigenvalues and eigenvectors var eigenVectors = new Complex[order * order]; var blockDiagonal = new Complex[order * order]; var eigenValues = new Complex[order]; new ManagedLinearAlgebraProvider().EigenDecomp(matrix.IsHermitian(), order, matrix.To1DimArray(), eigenVectors, eigenValues, blockDiagonal); _vals = eigenValues; _rightvecs = new CMatrix(matrix.RowCount, matrix.ColumnCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) { _rightvecs[i, j] = eigenVectors[j * matrix.RowCount + i]; } } }
public override object Evaluate() { CMatrix matrix = new CMatrix(Expressions.Count / Stride, Stride); int curr_row = 0; int curr_col = 0; foreach (Expression <Object> expr in Expressions) { Complex item = (Complex)expr.Evaluate(); matrix[curr_row, curr_col] = item; if (curr_col >= Stride - 1) { curr_col = 0; curr_row++; } else { curr_col++; } } return(matrix); }
public static Matrix ToSHVDNMatrix(this CMatrix matrix) { Matrix retVal; retVal.M11 = matrix.M11; retVal.M12 = matrix.M12; retVal.M13 = matrix.M13; retVal.M14 = matrix.M14; retVal.M21 = matrix.M21; retVal.M22 = matrix.M22; retVal.M23 = matrix.M23; retVal.M24 = matrix.M24; retVal.M31 = matrix.M31; retVal.M32 = matrix.M32; retVal.M33 = matrix.M33; retVal.M34 = matrix.M34; retVal.M41 = matrix.M41; retVal.M42 = matrix.M42; retVal.M43 = matrix.M43; retVal.M44 = matrix.M44; return(retVal); }
public static void print(Stream output, CMatrix mat, string format) { string type = "dense32"; Console.WriteLine("Type = " + type + " complex , numRows = " + mat.getNumRows() + " , numCols = " + mat.getNumCols()); format += " "; Complex_F32 c = new Complex_F32(); for (int y = 0; y < mat.getNumRows(); y++) { for (int x = 0; x < mat.getNumCols(); x++) { mat.get(y, x, c); Console.Write(format, c.real, c.imaginary); if (x < mat.getNumCols() - 1) { Console.Write(" , "); } } Console.WriteLine(); } }
public override object Evaluate() { int n = SubExpression.EvaluateAsInt32(); PerformanceManager.Current.EnsureAcceptableArraySize(n * n); return(CMatrix.Identity(n)); }
public static void updateJacobianTranspose(List <Joint> p_joints, Vector3 p_targetPos, Vector3 p_axis) { int linkCount = p_joints.Count; if (linkCount == 0) { return; } // Calculate Jacobian matrix CMatrix J = calculateJacobian(p_joints, linkCount, p_targetPos, -p_axis); // Calculate Jacobian transpose CMatrix Jt = CMatrix.Transpose(J); // Calculate error matrix CMatrix e = new CMatrix(3, 1); e[0, 0] = p_joints[linkCount - 1].m_endPoint.x - p_targetPos.x; e[1, 0] = p_joints[linkCount - 1].m_endPoint.y - p_targetPos.y; e[2, 0] = p_joints[linkCount - 1].m_endPoint.z - p_targetPos.z; float error = CMatrix.Dot(e, e); if (error < 0.0001f) { return; } // Calculate mu for inverse estimation // ie. a small scalar constant used as step size float mudiv = CMatrix.Dot(J * Jt * e, J * Jt * e); if (mudiv == 0.0f) { return; } float mu = CMatrix.Dot(e, J * Jt * e) / mudiv; // Step matrix CMatrix deltaAngle = Jt * (mu * e); // Make sure the matrix is correct if (deltaAngle.m_rows != linkCount) { Debug.Log("Not correct amount of rows! (" + deltaAngle.m_rows + ") correct is " + linkCount); } if (deltaAngle.m_cols != 1) { Debug.Log("Not correct amount of cols! (" + deltaAngle.m_cols + ") correct is 1"); } for (int i = 0; i < linkCount; i++) { p_joints[i].m_angle += p_axis * deltaAngle[i, 0]; } }
public override object Evaluate() { CMatrix m = Expressions[0].EvaluateAsCMatrix(); int r1 = Expressions[1].EvaluateAsInt32(); int r2 = Expressions[2].EvaluateAsInt32(); int c1 = Expressions[3].EvaluateAsInt32(); int c2 = Expressions[4].EvaluateAsInt32(); return(m.Submatrix(r1, r2, c1, c2)); }
public override ILinAlgMat _Diag(ILinAlgMat mat) { using (new Matlab.NamedLock("LA")) { Matlab.PutMatrix("LA.A", mat.ToArray()); CMatrix diag = Matlab.GetMatrix("diag(LA.A)"); Matlab.Clear(); return(diag); } }
public override ILinAlgMat _Inv(ILinAlgMat A) { using (new Matlab.NamedLock("LA")) { Matlab.PutMatrix("LA.A", A.ToArray()); CMatrix inv = Matlab.GetMatrix("inv(LA.A)"); Matlab.Clear(); return(inv); } }
public static CMatrix operator *(float p_s, CMatrix p_m) { CMatrix res = new CMatrix(p_m.m_rows, p_m.m_cols); for (int i=0;i<p_m.m_rows;i++) for (int j=0;j<p_m.m_cols;j++) { res[i, j] = p_s*p_m[i, j]; } return res; }
public void SetUp() { _m = new CMatrix(new Complex[, ] { { new Complex(12, -3), new Complex(-51, 0), -Complex.I, new Complex(2, -8) }, { new Complex(6, 0), new Complex(-2, 167), new Complex(-68, 0), new Complex(0, 5.3) }, { new Complex(-4, 0), new Complex(24, 0), new Complex(0, -41), new Complex(2, 8.8) }, { new Complex(15, 0), new Complex(24, 20), new Complex(-2.5, -41), new Complex(0, 0) } }); }
public void SetUp() { _m = new CMatrix(new Complex[,] { {new Complex(12, -3), new Complex(-51, 0), -Complex.I, new Complex(2, -8)}, {new Complex(6, 0), new Complex(-2, 167), new Complex(-68, 0), new Complex(0, 5.3)}, {new Complex(-4, 0), new Complex(24, 0), new Complex(0, -41), new Complex(2, 8.8)}, {new Complex(15, 0), new Complex(24, 20), new Complex(-2.5, -41), new Complex(0, 0)} }); }
/// <summary> /// Метод Гаусса, годный и при нулевых коэффициентах в системе /// </summary> public void GaussSelection() { CMatrix S = new CMatrix(this.Dim, this.Dim + 1); for (int j = 0; j < this.Dim; j++) { for (int i = 0; i < this.Dim; i++) { S[i, j] = this.A[i, j]; } S[j, this.Dim] = this.b[j]; } for (int j = 0; j < this.Dim; j++) { int k = j; if (S[k, j] == 0)//если ведущий элемент равен нулю, поменять эту строку местами с ненулевой { int h = k; while (S[h, j] == 0) { h++; } S.LinesSwap(k, h); } while (S[k, j] == 0 && k < this.Dim - 1) { k++; //найти ненулевой элемент } int l = k + 1; if (k != this.Dim - 1) { while (l != this.Dim) { S.LinesDiff(l, k, S[l, j] / S[k, j]); l++; } //отнимать от строк снизу } //S.PrintMatrix();Console.WriteLine(); l = k - 1; if (k != 0) { while (l != -1) { S.LinesDiff(l, k, S[l, j] / S[k, j]); l--; } //отнимать от строк сверху } } for (int i = 0; i < this.Dim; i++) { this.x[i] = S[i, this.Dim] / S[i, i]; } }
public static IList <Complex> EvaluateAsComplexVector(this Expression <Object> expression) { CMatrix matrix = EvaluateAsCMatrix(expression); if (matrix.IsVector) { return(new List <Complex>(matrix)); } throw ExceptionHelper.ThrowInvalidArgumentType("complex vector", "complex matrix"); }
public static CPolynomial EvaluateAsCPolynomial(this Expression <Object> expression) { CMatrix matrix = EvaluateAsCMatrix(expression); if (matrix.IsVector) { return(new CPolynomial(matrix)); } throw ExceptionHelper.ThrowInvalidArgumentType("complex vector", "complex matrix"); }
public void Sqrt() { //arrange double TOL = 10E-14; //action CMatrix sq = CMatrix.Sqrt(_m); //assert CMatrix.FuzzyEquals(sq * sq, _m, TOL).Should().BeTrue(); }
public override ILinAlgMat _LinSolve(ILinAlgMat A, ILinAlgMat B) { using (new Matlab.NamedLock("LA")) { Matlab.PutMatrix("LA.A", A.ToArray()); Matlab.PutMatrix("LA.B", B.ToArray()); CMatrix X = Matlab.GetMatrix(@"LA.A \ LA.B"); /// A X = B => X = A\B Matlab.Clear(); return(X); } }
public static IList <double> AsRealVector(CMatrix matrix) { if (matrix.IsVector) { if (matrix.IsReal) { return(matrix.Select(x => (double)x).ToArray()); } throw ExceptionHelper.ThrowInvalidArgumentType("real vector", "complex vector"); } throw ExceptionHelper.ThrowInvalidArgumentType("real vector", "complex matrix"); }
/// <summary> /// Applies the zero threshold for each element of the complex matrix and returns the result. /// </summary> /// <param name="value">A complex matrix.</param> /// <param name="zeroThreshold">An integer representing the zero threshold.</param> /// <returns> /// The result of applying a zero threshold for each element of the complex matrix value. /// </returns> /// <exception cref="System.ArgumentOutOfRangeException"> /// zeroThreshold must be between 0 and 307. /// </exception> public static CMatrix ZeroThreshold(CMatrix value, int zeroThreshold) { CMatrix matrix = new CMatrix(value.RowCount, value.ColumnCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) matrix[i, j] = NumericUtil.ZeroThreshold(value[i, j], zeroThreshold); } return matrix; }
public static float Dot(CMatrix p_ma, CMatrix p_mb) { if (p_ma.m_rows != p_mb.m_rows) return -1.0f; if (p_ma.m_cols != p_mb.m_cols) return -1.0f; float sum = 0.0f; for (int i=0;i<p_ma.m_rows;i++) for (int j=0;j<p_mb.m_cols;j++) { sum += p_ma[i, j] * p_mb[i, j]; } return sum; }
public void Inverse() { //arrange const double TOL = 10E-16; CMatrix identity = CMatrix.Identity(_m.RowCount); //action CMatrix inv_m = CMatrix.Inverse(_m); //assert CMatrix.FuzzyEquals(_m * inv_m, identity, TOL).Should().BeTrue(); }
public void CharacteristicPolynomial() { //arrange double TOL = 10E-6; CMatrix zero = new CMatrix(_m.RowCount, _m.ColumnCount); //action CPolynomial poly = CMatrix.CharacteristicPolynomial(_m); CMatrix test = poly.Evaluate(_m); //assert CMatrix.FuzzyEquals(test, zero, TOL).Should().BeTrue(); }
public void CharacteristicPolynomial() { //arrange double TOL = 10E-6; CMatrix zero = new CMatrix(_m.RowCount, _m.ColumnCount); //action CPolynomial poly = CMatrix.CharacteristicPolynomial(_m); CMatrix test = poly.Evaluate(_m); //assert CMatrix.FuzzyEquals(test, zero, TOL).Should().BeTrue(); }
public static CMatrix Transpose(CMatrix p_m) { CMatrix res = new CMatrix(p_m.m_cols, p_m.m_rows); for (int i = 0; i < p_m.m_rows; i++) { for (int j = 0; j < p_m.m_cols; j++) { res[j, i] = p_m[i, j]; } } return(res); }
public void EvaluateMatrixTest2() { //arrange const string s = "{1, 2; 3, 8; 1+2, -0.8}"; CMatrix expected = new CMatrix(new Complex[,] { { 1, 2 }, { 3, 8 }, { 3, -0.8 } }); //action Expression<object> expr = TreeBuilder.BuildTree(s); CMatrix actual = (CMatrix)expr.Evaluate(); //assert CMatrix.Equals(actual, expected).Should().BeTrue(); }
public static CMatrix operator *(float p_s, CMatrix p_m) { CMatrix res = new CMatrix(p_m.m_rows, p_m.m_cols); for (int i = 0; i < p_m.m_rows; i++) { for (int j = 0; j < p_m.m_cols; j++) { res[i, j] = p_s * p_m[i, j]; } } return(res); }
/// <summary> /// Applies the zero threshold for each element of the complex matrix and returns the result. /// </summary> /// <param name="value">A complex matrix.</param> /// <param name="zeroThreshold">An integer representing the zero threshold.</param> /// <returns> /// The result of applying a zero threshold for each element of the complex matrix value. /// </returns> /// <exception cref="System.ArgumentOutOfRangeException"> /// zeroThreshold must be between 0 and 307. /// </exception> public static CMatrix ZeroThreshold(CMatrix value, int zeroThreshold) { CMatrix matrix = new CMatrix(value.RowCount, value.ColumnCount); for (int i = 0; i < matrix.RowCount; i++) { for (int j = 0; j < matrix.ColumnCount; j++) { matrix[i, j] = NumericUtil.ZeroThreshold(value[i, j], zeroThreshold); } } return(matrix); }
void updateChain() { // Just copy from objects Vector3 end = transform.position; for (int i = 0; i < m_chain.Count; i++) { Joint current = m_chain[i]; GameObject currentObj = m_chainObjs[i]; current.length = currentObj.transform.localScale.x; current.m_position = currentObj.transform.position - currentObj.transform.right * current.length * 0.5f; current.m_endPoint = currentObj.transform.position + currentObj.transform.right * current.length * 0.5f; end = current.m_endPoint; } //CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward); CMatrix J = Jacobian.calculateJacobian(m_chain, m_chainObjs, m_dofs, m_dofJointId, end + m_virtualForce); CMatrix Jt = CMatrix.Transpose(J); CMatrix force = new CMatrix(3, 1); force[0, 0] = m_virtualForce.x; force[1, 0] = m_virtualForce.y; force[2, 0] = m_virtualForce.z; //CMatrix torqueSet = Jt*force; Debug.Log(Jt.m_rows + "x" + Jt.m_cols); //Debug.Log(torqueSet.m_rows+"x"+torqueSet.m_cols); //for (int i = 0; i < m_chain.Count; i++) //{ // // store torque // m_torques[i] = Vector3.forward*Vector3.Dot(new Vector3(Jt[i,0],Jt[i,1],Jt[i,2]),m_virtualForce); // //Debug.Log(m_torques[i].ToString()); //} // This could either be a new kernel, or continuation of the old // Here in GPGPU, we could zero the torque position each time... for (int i = 0; i < m_chain.Count; i++) { m_torques[i] = Vector3.zero; // for each dof, for its joint id (as if it had been in the next loop essentially) } // ... but force sync here, so the following add-op isn't overwritten: for (int i = 0; i < m_dofs.Count; i++) { // store torque int x = m_dofJointId[i]; m_torques[x] += /*m_chainObjs[x].transform.TransformDirection(m_dofs[i])*/ m_dofs[i] * Vector3.Dot(new Vector3(Jt[i, 0], Jt[i, 1], Jt[i, 2]), m_virtualForce); //Debug.Log(m_torques[i].ToString()); } // Come to think of it, the jacobian and torque could be calculated in the same // kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above) }
// Use this for initialization void Start() { CMatrix a = new CMatrix(2, 2); CMatrix b = new CMatrix(2, 1); a[0, 0] = 1.0f; a[0, 1] = 0.0f; a[1, 0] = 2.0f; a[1, 1] = 3.0f; b[0, 0] = 2.0f; b[1, 0] = 4.0f; CMatrix g = CMatrix.Mul(a, b); Debug.Log(g.m_rows + "x" + g.m_cols); Debug.Log(g[1,0]+"=="+(a[1,0]*b[0,0]+a[1,1]*b[1,0])); }
public override ILinAlgMat _Mul(params ILinAlgMat[] mats) { using (new Matlab.NamedLock("LA")) { Matlab.PutMatrix("LA.mul", mats[0].ToArray(), true); for (int i = 1; i < mats.Length; i++) { Matlab.PutMatrix("LA.tmp", mats[i].ToArray(), true); Matlab.Execute("LA.mul = LA.mul * LA.tmp;"); } CMatrix mul = Matlab.GetMatrix("LA.mul", true); Matlab.Clear(); return(mul); } }
public override object Evaluate() { object value = SubExpression.Evaluate(); if (value is Complex) { return(Complex.Exp((Complex)value)); } else if (value is CMatrix) { return(CMatrix.Exp((CMatrix)value)); } throw ExceptionHelper.ThrowWrongArgumentType(value); }
public void EvaluateMatrixTest2() { //arrange const string s = "{1, 2; 3, 8; 1+2, -0.8}"; CMatrix expected = new CMatrix(new Complex[, ] { { 1, 2 }, { 3, 8 }, { 3, -0.8 } }); //action Expression <object> expr = TreeBuilder.BuildTree(s); CMatrix actual = (CMatrix)expr.Evaluate(); //assert CMatrix.Equals(actual, expected).Should().BeTrue(); }
void updateChain() { // Just copy from objects Vector3 end = transform.position; for (int i = 0; i < m_chain.Count; i++) { Joint current = m_chain[i]; GameObject currentObj = m_chainObjs[i]; current.length = currentObj.transform.localScale.x; current.m_position = currentObj.transform.position - currentObj.transform.right * current.length * 0.5f; current.m_endPoint = currentObj.transform.position + currentObj.transform.right * current.length * 0.5f; end = current.m_endPoint; } //CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward); CMatrix J = Jacobian.calculateJacobian(m_chain, m_chainObjs, m_dofs, m_dofJointId, end+m_virtualForce); CMatrix Jt = CMatrix.Transpose(J); CMatrix force = new CMatrix(3, 1); force[0, 0] = m_virtualForce.x; force[1, 0] = m_virtualForce.y; force[2, 0] = m_virtualForce.z; //CMatrix torqueSet = Jt*force; Debug.Log(Jt.m_rows + "x" + Jt.m_cols); //Debug.Log(torqueSet.m_rows+"x"+torqueSet.m_cols); //for (int i = 0; i < m_chain.Count; i++) //{ // // store torque // m_torques[i] = Vector3.forward*Vector3.Dot(new Vector3(Jt[i,0],Jt[i,1],Jt[i,2]),m_virtualForce); // //Debug.Log(m_torques[i].ToString()); //} // This could either be a new kernel, or continuation of the old // Here in GPGPU, we could zero the torque position each time... for (int i = 0; i < m_chain.Count; i++) { m_torques[i] = Vector3.zero; // for each dof, for its joint id (as if it had been in the next loop essentially) } // ... but force sync here, so the following add-op isn't overwritten: for (int i = 0; i < m_dofs.Count; i++) { // store torque int x = m_dofJointId[i]; m_torques[x] += /*m_chainObjs[x].transform.TransformDirection(m_dofs[i])*/m_dofs[i] *Vector3.Dot(new Vector3(Jt[i, 0], Jt[i, 1], Jt[i, 2]), m_virtualForce); //Debug.Log(m_torques[i].ToString()); } // Come to think of it, the jacobian and torque could be calculated in the same // kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above) }
public void Div() { //arrange double TOL = 10E-14; CMatrix m2 = new CMatrix(new Complex[,] { {new Complex(122, 5.8), new Complex(0, 0), new Complex(2, 24), new Complex(0, -8)}, {new Complex(16, 0), new Complex(164, 167), new Complex(0.25, 0.4), new Complex(10, -5)}, {new Complex(-44, 2.28), new Complex(2.4, 5.6), new Complex(0, 4.1), new Complex(-2, 0)}, {new Complex(185, -14), new Complex(2.4, 2), new Complex(25.14, 39.5), new Complex(122, 122)} }); //action CMatrix c = CMatrix.Divide(_m, m2); //assert CMatrix.FuzzyEquals(c * m2, _m, TOL).Should().BeTrue(); }
public static void updateJacobianTranspose(List<Joint> p_joints, Vector3 p_targetPos, Vector3 p_axis) { int linkCount = p_joints.Count; if (linkCount == 0) return; // Calculate Jacobian matrix CMatrix J = calculateJacobian(p_joints, linkCount, p_targetPos, -p_axis); // Calculate Jacobian transpose CMatrix Jt = CMatrix.Transpose(J); // Calculate error matrix CMatrix e = new CMatrix(3, 1); e[0, 0] = p_joints[linkCount - 1].m_endPoint.x - p_targetPos.x; e[1, 0] = p_joints[linkCount - 1].m_endPoint.y - p_targetPos.y; e[2, 0] = p_joints[linkCount - 1].m_endPoint.z - p_targetPos.z; float error = CMatrix.Dot(e, e); if (error < 0.0001f) return; // Calculate mu for inverse estimation // ie. a small scalar constant used as step size float mudiv = CMatrix.Dot(J * Jt * e, J * Jt * e); if (mudiv == 0.0f) return; float mu = CMatrix.Dot(e, J * Jt * e) / mudiv; // Step matrix CMatrix deltaAngle = Jt * (mu * e); // Make sure the matrix is correct if (deltaAngle.m_rows != linkCount) Debug.Log("Not correct amount of rows! (" + deltaAngle.m_rows + ") correct is " + linkCount); if (deltaAngle.m_cols != 1) Debug.Log("Not correct amount of cols! (" + deltaAngle.m_cols + ") correct is 1"); for (int i = 0; i < linkCount; i++) { p_joints[i].m_angle += p_axis * deltaAngle[i, 0]; } }
/// <summary> /// Returns the value of the complex polynomial evaluated at a specified value. /// </summary> /// <param name="value">A complex square matrix.</param> /// <returns>The evaluated value of the complex polynomial.</returns> /// <exception cref="MatrixSizeMismatchException">The matrix value is not square.</exception> public static CMatrix Evaluate(this CPolynomial poly, CMatrix value) { if (!value.IsSquare) throw new MatrixSizeMismatchException("The matrix must be square."); int len = poly.Degree + 1; int n = value.RowCount; CMatrix m = CMatrix.Identity(n); CMatrix result = new CMatrix(n, n); for (int i = 0; i < len; i++) { result += poly[i] * m; m *= value; } return result; }
public static CMatrix calculateJacobian(List<Joint> p_joints, int p_numberOfLinks, Vector3 p_targetPos, Vector3 p_axis) { int linkCount = p_numberOfLinks; if (linkCount == 0) return null; // Construct Jacobian matrix CMatrix J = new CMatrix(3, linkCount); // 3 is position in xyz for (int i = 0; i < linkCount; i++) { Vector3 linkPos = p_joints[i].m_position; // Currently only solve for z axis(ie. 2d) Vector3 rotAxis = p_axis; Vector3 dirTarget = Vector3.Cross(rotAxis, p_targetPos - linkPos); J[0, i] = dirTarget.x; J[1, i] = dirTarget.y; J[2, i] = dirTarget.z; } return J; }
public static bool FuzzyEquals(CMatrix value1, CMatrix value2, double relativeTolerance) { if (value1.RowCount != value2.RowCount || value1.ColumnCount != value2.ColumnCount) return false; int rows = value1.RowCount; int cols = value1.ColumnCount; for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { if (!NumericUtil.FuzzyEquals(value1[i, j], value2[i, j], relativeTolerance)) { return false; } } } return true; }
public static CMatrix Mul(CMatrix p_ma, CMatrix p_mb) { if (p_ma.m_cols!=p_mb.m_rows) return null; CMatrix res = new CMatrix(p_ma.m_rows, p_mb.m_cols); int y = p_ma.m_cols; for (int i=0;i<res.m_rows;i++) for (int j=0;j<res.m_cols;j++) { float s = 0.0f; for (int x = 0; x < y; x++) { s += p_ma[i, x] * p_mb[x, j]; // if (i == 1 && j == 0) // { // Debug.Log(" a"+i + "," + x + " * b" + x + "," + j + " +"); // } } // Debug.Log(i+","+j+"="+s); res[i, j] = s; } return res; }
public void Mult_TwoMatrices_MultResult() { //arrange var a = new CMatrix(new Complex[,] { {2, 3}, {5, 8} }); var b = new CMatrix(new Complex[,] { {1, 1}, {18, -1} }); var expected = new CMatrix(new Complex[,] { {56, -1}, {149, -3} }); //action var actual = a * b; //assert actual.Should().Equal(expected); }
/// <summary> /// Initializes a new instance of the CEigenproblem class, computing /// the eigenvalues and the right eigenvectors of a square complex matrix. /// </summary> /// <param name="matrix">A complex square matrix.</param> public CEigenproblem(CMatrix matrix) : this(matrix, true) { }
private static void Output(string v, string w, int i, int j, CMatrix matrix) { Console.WriteLine("Now in " + matrix.ToString() + " matrix"); if (i == 0 || j == 0) return; if (matrix == CMatrix.LOWER) { Result = Result + "-"; Result2 = Result2 + w[i - 1]; Console.WriteLine("String 1 has a gap, string 2 has a letter"); if (LowerBacktrackMatrix[i, j] == 2) matrix = CMatrix.MIDDLE; else if (LowerBacktrackMatrix[i, j] == 1) matrix = CMatrix.LOWER; Console.WriteLine("Now in " + matrix.ToString() + " matrix, calling backtrack"); Output(v, w, i - 1, j, matrix); } else if (matrix == CMatrix.UPPER) { Result = Result + v[j - 1]; Result2 = Result2 + "-"; Console.WriteLine("String 2 has a gap, string 1 has a letter"); if (UpperBacktrackMatrix[i, j] == 2) matrix = CMatrix.MIDDLE; else if (UpperBacktrackMatrix[i, j] == 1) matrix = CMatrix.UPPER; Console.WriteLine("Now in " + matrix.ToString() + " matrix, calling backtrack"); Output(v, w, i, j - 1, matrix); } else if (matrix == CMatrix.MIDDLE) { Result = Result + v[j - 1]; Result2 = Result2 + w[i - 1]; Console.WriteLine("Both strings has a letter"); if (MiddleBacktrackMatrix[i - 1, j - 1] == 3) matrix = CMatrix.MIDDLE; else if (MiddleBacktrackMatrix[i - 1, j - 1] == 2) matrix = CMatrix.UPPER; else if (MiddleBacktrackMatrix[i - 1, j - 1] == 1) matrix = CMatrix.LOWER; Console.WriteLine("Now in " + matrix.ToString() + " matrix, calling backtrack"); Output(v, w, i - 1, j - 1, matrix); } }
public void EvaluateMatrixTest_RaiseToPower() { //arrange const string s = "{1, 2; 3, 4} ^ 3"; CMatrix expected = new CMatrix(new Complex[,] { { 37, 54 }, { 81, 118 } }); //action Expression<object> expr = TreeBuilder.BuildTree(s); CMatrix actual = (CMatrix)expr.Evaluate(); //assert CMatrix.FuzzyEquals(actual, expected, Machine.Epsilon).Should().BeTrue(); }
public void EvaluateMatrixTest3() { //arrange const string s = "{1, 2; 3, 8; 1+2i, -0.8} * { 3, 8, 2i; 2, 2+8i, 2 } - 2"; CMatrix expected = new CMatrix(new Complex[,] { { 5, new Complex(10, 16), new Complex(2, 2) }, {23, new Complex(38, 64), new Complex(14, 6)}, { new Complex(-0.6, 6), new Complex(4.4, 9.6), new Complex(-7.6, 2) } }); //action Expression<object> expr = TreeBuilder.BuildTree(s); CMatrix actual = (CMatrix)expr.Evaluate(); //assert CMatrix.FuzzyEquals(actual, expected, Machine.Epsilon).Should().BeTrue(); }
/// <summary> /// Returns the Moore-Penrose inverse (pseudoinverse) matrix. /// </summary> /// <returns>The generalized inverse matrix.</returns> public CMatrix PseudoInverse() { double tol = Math.Max(_m, _n) * _s[0] * Machine.Epsilon; CMatrix s = new CMatrix(_n, _m); for (int i = 0; i < _s.Length; i++) { if (_s[i] > tol) s[i, i] = 1.0 / _s[i]; } return _vt.Adjoint * s * _u.Adjoint; }
public static CMatrix calculateJacobian(List<Joint> p_joints, List<GameObject> p_jointObjs, List<Vector3> p_dofs, List<int> p_dofJointIds, Vector3 p_targetPos, int p_dofListOffset=0, int p_dofListEnd=-1, int p_separateRootDofIdx = -1, int p_listStep=1) { // If GPGPU here // First, read dofjoint id, then position from joint array to registry // This is now done in the loop below of course // Then also read targetpos, dof into memory // The J matrix is in global memory and is written in the end (also do transpose, so really Jt) if (p_dofs.Count == 0) return null; // This means all Jt's are computed in parallel // One Jt per dof // calculate size // If a separate root object was specified // we need to account for that extra joint int extra = 0; int start = p_dofListOffset; if (p_separateRootDofIdx >= 0) { int id = p_dofJointIds[p_separateRootDofIdx]; Joint root = p_joints[id]; extra = root.m_dof.Length; start = p_separateRootDofIdx; } if (p_dofListEnd <= 0) p_dofListEnd = p_dofs.Count; int jointCount = (p_dofListEnd) - p_dofListOffset + extra; // Construct Jacobian matrix CMatrix J = new CMatrix(3, jointCount); // 3 is position in xyz //Debug.Log("DOF count="+jointCount+" from: "+start+" to: "+(p_dofListEnd-1)); Vector3 astart=Vector3.zero; int jIdx = 0; // incremented by one for each jacobian row for (int i = start; i < p_dofListEnd; i += p_listStep) // this is then the "thread pool" { // if we have separate root, account for its DOFs first // then reset to chain root if (extra > 0) extra--; else if (i<p_dofListOffset) i = p_dofListOffset; // Fetch the id for the DOF from the global list int id = p_dofJointIds[i]; //Debug.Log("D" + i + " n" + jIdx + " joint id: "+id); // Start calculating the jacobian for the current DOF Joint joint = p_joints[id]; Vector3 linkPos = joint.m_position; //Debug.Log(linkPos.ToString()); // Currently only solve for given axis Vector3 rotAxis = p_jointObjs[id].transform.TransformDirection(p_dofs[i]); Vector3 dirTarget = Vector3.Cross(rotAxis, p_targetPos - linkPos); J[0, jIdx] = dirTarget.x; J[1, jIdx] = dirTarget.y; J[2, jIdx] = dirTarget.z; jIdx++; } return J; }
/// <summary> /// Initializes a new instance of the CSVD class, /// computing the Singular Value Decomposition of a general complex matrix. /// </summary> /// <param name="matrix">A general complex matrix.</param> public CSVD(CMatrix matrix) : this(matrix, false) { }
public static CMatrix Transpose(CMatrix p_m) { CMatrix res = new CMatrix(p_m.m_cols, p_m.m_rows); for (int i=0;i<p_m.m_rows;i++) for (int j=0;j<p_m.m_cols;j++) { res[j, i] = p_m[i, j]; } return res; }
public void Solve() { //assert double TOL = 10E-15; CMatrix b = new CMatrix(new Complex[] { new Complex(3, 5), new Complex(-8.5, 0), new Complex(0.5, -144), new Complex(0, 2) }); //action CMatrix x = CMatrix.Solve(_m, b); //assert CMatrix.FuzzyEquals(_m * x, b, TOL).Should().BeTrue(); }
/// <summary> /// Applies the complex and zero threshold for each element of the complex matrix and returns the result. /// </summary> /// <param name="value">A complex matrix.</param> /// <param name="complexThreshold">An integer representing the complex threshold.</param> /// <param name="zeroThreshold">An integer representing the zero threshold.</param> /// <returns> /// The result of applying a complex and zero threshold for each element of the complex matrix value. /// </returns> /// <exception cref="System.ArgumentOutOfRangeException"> /// complexThreshold and zeroThreshold must be between 0 and 307. /// </exception> public static CMatrix ComplexZeroThreshold(CMatrix value, int complexThreshold, int zeroThreshold) { return ZeroThreshold(ComplexThreshold(value, complexThreshold), zeroThreshold); }