/// <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);
}
