public void Train(Matrix <double> pattern)
{
if (pattern.RowCount != Dimension || pattern.ColumnCount != Dimension)
{
throw new Exception("Incompatible image dimensions");
}
if (PatternCount >= MaxPatternCount)
{
throw new Exception("Full Memory");
}
var tempMatrix = CreateMatrix.Dense <double>(NeuronsCount, NeuronsCount);
// convert 0 => -1
pattern.MapInplace(x => x == 0 ? -1 : x, Zeros.Include);
// to column vector
var col = pattern.AsColumnMajorArray();
var columnVector = CreateMatrix.DenseOfColumnArrays(col);
// create weighted matrix
columnVector.TransposeAndMultiply(columnVector, tempMatrix);
//set diagonal to 0
tempMatrix = tempMatrix.Subtract(CreateMatrix.DenseIdentity <double>(NeuronsCount, NeuronsCount));
// add pattern
Memory = Memory + tempMatrix;
PatternCount++;
}
public void ParseDirectory(String path)
{
FileList = Directory.EnumerateFiles(path).Select(x => Path.GetFileName(x)).ToList();
IEnumerable <char> distinctSymbols = FileList.
Aggregate <String>((x, y) => x + y).
ToCharArray().
Distinct().
Where(x => !Char.IsLetter(x)).
OrderByDescending(x => x);
List <string[]> splitNames = FileList.Select(x => x.Split(distinctSymbols.ToArray <char>(), StringSplitOptions.RemoveEmptyEntries)).ToList();
IEnumerable <IEnumerable <Tuple <string, string> > > splitSingleNames = splitNames.Select(x => x.Select(a => Tuple.Create("", a.ToLower())));
IEnumerable <IEnumerable <Tuple <string, string> > > splitPairNames = splitNames.Select(x => x.Zip(x.Skip(1), (a, b) => Tuple.Create(a.ToLower(), b.ToLower())));
IEnumerable <IEnumerable <Tuple <string, string> > > union = splitPairNames;//splitSingleNames.Zip(splitPairNames, (a, b) => a.Concat(b));
HashSet <Tuple <string, string> > vocabSet = new HashSet <Tuple <string, string> >();
foreach (IEnumerable <Tuple <string, string> > tokens in union)
{
foreach (Tuple <string, string> token in tokens)
{
vocabSet.Add(token);
}
}
Vocab = vocabSet.ToList();
ParsedNames = union.Select(x => x.Select(y => Vocab.FindIndex(z => z.Item1.ToLower() == y.Item1.ToLower() && z.Item2.ToLower() == y.Item2.ToLower())).ToList()).ToList();
IEnumerable <double> zeros = Enumerable.Repeat(0.0, Vocab.Count);
double[] v = ParsedNames
.Select(x => zeros.Select((a, i) => x.Contains(i) ? /*IndexLookup.IsNumeric(Vocab[i].Item2)?5.0:*/ 15.0 : 0.0))
.Aggregate((x, y) => x.Concat(y))
.ToArray();
Input = CreateMatrix.Dense(Vocab.Count, ParsedNames.Count, v).Transpose().NormalizeRows(2);
}
//Assumes full observability and no D matrix.
//Generates an empty set of matrices representing the state-space model.
public LinearStateSpaceModel(int numberOfStates = 6, int numberOfInputs = 2)
{
A = CreateMatrix.Dense <double>(numberOfStates, numberOfStates);
B = CreateMatrix.Dense <double>(numberOfStates, numberOfInputs);
C = CreateMatrix.DenseIdentity <double>(numberOfStates);
D = CreateMatrix.Dense <double>(0, 0);
}
public Neural(List <float> flatValues)
{
nodes = new Vector <float> [nodeSetup.Length];
weights = new Matrix <float> [nodeSetup.Length - 1];
biases = new Vector <float> [nodeSetup.Length - 1];
gammaValues = new Vector <float> [nodeSetup.Length - 1];
deltaWeights = new Matrix <float> [nodeSetup.Length - 1];
deltaBiases = new Vector <float> [nodeSetup.Length - 1];
for (int i = 0; i < nodeSetup.Length; i++)
{
nodes[i] = CreateVector.Dense <float>(nodeSetup[i]);
}
for (int i = 0; i < nodeSetup.Length - 1; i++)
{
weights[i] = CreateMatrix.Dense <float>(nodeSetup[i + 1], nodeSetup[i]);
deltaWeights[i] = CreateMatrix.Dense <float>(nodeSetup[i + 1], nodeSetup[i]);
biases[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
deltaBiases[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
gammaValues[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
}
LoadFromAllValues(flatValues);
}
/// <summary>
/// Helper function used to create the masking map from a single image represented as a
/// vector. We do this step so we can do ALL convolutions or pools for an image in one single
/// computational step.
/// </summary>
/// <param name="kernelSideLength">
/// The length of one side of the convolution/pool. All filters are assumed square.
/// </param>
/// <param name="strideSize">The stride length that will be used with this map.</param>
/// <param name="startingImage">
/// An images represented as a vector that we are creating the map for.
/// </param>
/// <returns></returns>
internal Matrix <double> CreateMaskingMap(int kernelSideLength, int strideSize, Vector <double> startingImage)
{
int _imageSideDimension = (int)Math.Sqrt(startingImage.Count);
int _endingImageSideDimension = (_imageSideDimension - kernelSideLength) / strideSize + 1;
Matrix <double> _result = CreateMatrix.Dense <double>((int)Math.Pow(kernelSideLength, 2), (int)Math.Pow(_endingImageSideDimension, 2));
for (int i = 0; i < _endingImageSideDimension; i += strideSize)
{
for (int j = 0; j < _endingImageSideDimension; j += strideSize)
{
int _arrayIndex = i * _imageSideDimension + j;
Vector <double> _dataPatch = Vector <double> .Build.Sparse((int)Math.Pow(kernelSideLength, 2));
for (int k = 0; k < kernelSideLength; k++)
{
for (int m = 0; m < kernelSideLength; m++)
{
_dataPatch[k * kernelSideLength + m] = startingImage[_arrayIndex + m + kernelSideLength * k];
}
}
_result.SetColumn(j + i * _endingImageSideDimension, _dataPatch);
}
}
return(_result);
}
/// <summary>
/// Objective model with a user supplied jacobian for non-linear least squares regression.
/// </summary>
public static IObjectiveModel NonlinearModel(Func <Vector <double>, double, double> function,
Func <Vector <double>, double, Vector <double> > derivatives,
Vector <double> observedX, Vector <double> observedY, Vector <double> weight = null)
{
Vector <double> func(Vector <double> point, Vector <double> x)
{
var functionValues = CreateVector.Dense <double>(x.Count);
for (int i = 0; i < x.Count; i++)
{
functionValues[i] = function(point, x[i]);
}
return(functionValues);
}
Matrix <double> prime(Vector <double> point, Vector <double> x)
{
var derivativeValues = CreateMatrix.Dense <double>(x.Count, point.Count);
for (int i = 0; i < x.Count; i++)
{
derivativeValues.SetRow(i, derivatives(point, x[i]));
}
return(derivativeValues);
}
var objective = new NonlinearObjectiveFunction(func, prime);
objective.SetObserved(observedX, observedY, weight);
return(objective);
}
private void Reshuffle(StateOperator state, SystemBipartition bipartition)
{
EvaluatedMatrix = new Transformation(CreateMatrix.Dense <Complex>(state.RowDimension, state.ColumnDimension));
int rowOffset = 0, columnOffset = 0;
for (int i = 0; i < state.RowDimension; i++)
{
int reshRowStart = rowOffset * bipartition.DimensionA, reshColumnStart = columnOffset * bipartition.DimensionB;
int reshRowIterator = reshRowStart, reshColumnIterator = reshColumnStart;
int reshColumnLimit = reshColumnStart + bipartition.DimensionB;
for (int j = 0; j < state.RowDimension; j++)
{
EvaluatedMatrix[reshRowIterator, reshColumnIterator] = state[i, j];
reshColumnIterator++;
if (reshColumnIterator == reshColumnLimit)
{
reshColumnIterator = reshColumnStart;
reshRowIterator++;
}
}
columnOffset++;
if (columnOffset == bipartition.DimensionA)
{
columnOffset = 0;
rowOffset++;
}
}
}
/// <summary>
/// Runs through a single convolution layer.
/// </summary>
/// <param name="layerInfo">The layer info used for this layer.</param>
/// <param name="inputImages">
/// A matrix of all the images that will be convolved. Each row is an image.
/// </param>
/// <returns>A matrix of all the resulting images. Each row is an image.</returns>
internal Matrix <double> Convolve(ConvolutionalLayerInformation layerInfo, Matrix <double> inputImages)
{
// Construct out return matrix that will include all layers of our images.
Matrix <double> _outputImages = null;
foreach (Tuple <int, Vector <double> > _imageDimensionAndIndex in inputImages.EnumerateRowsIndexed())
{
// Create the matrix so we can do all of the convolutions at once.
Matrix <double> _preConvolutionMap = this.CreateMaskingMap(layerInfo.KernelSize, layerInfo.Stride, _imageDimensionAndIndex.Item2);
Matrix <double> _filtersForThisDimension = CreateMatrix.Dense <double>(layerInfo.FlattenedFilters.Count, layerInfo.FlattenedFilters[0].ColumnCount);
foreach (Matrix <double> _filter in layerInfo.FlattenedFilters)
{
_filtersForThisDimension.InsertRow(_imageDimensionAndIndex.Item1, _filter.Row(_imageDimensionAndIndex.Item1));
}
// Create the result image matrix if it's not created yet
if (_outputImages == null)
{
_outputImages = CreateMatrix.Dense <double>(layerInfo.FilterCount, _preConvolutionMap.ColumnCount, 0.0);
}
// Store off the result of our filters multiplied by our map. This ends up being every filter passing over
// the entire image, and returning a dimentions for each kernel in the layer. We sum all the dimensional results in one dimension.
_outputImages = _outputImages.Add(_filtersForThisDimension.Multiply(_preConvolutionMap));
}
// Return all the resulting dimensions of the images after convolution
return(_outputImages);
}
void Construct(bool zeroInit)
{
// TODO: repair a bug (Maybe, last matrix is null? Or just index disorder?)
weights = new Matrix <double> [conf.numHidLayers + 2];
int prev, next;
prev = conf.numInputs;
for (int i = 0; i < conf.numHidLayers; i++)
{
next = conf.numHiddens;
if (zeroInit)
{
weights[i] = CreateMatrix.Dense <double>(next, prev);
}
else
{
weights[i] = CreateMatrix.Random <double>(next, prev, distrib);
}
prev = next;
}
next = conf.numOutputs;
if (zeroInit)
{
weights[conf.numHidLayers + 1] = CreateMatrix.Dense <double>(next, prev);
}
else
{
weights[conf.numHidLayers + 1] = CreateMatrix.Random <double>(next, prev, distrib);
}
}
public void GetGlobalKeepMatrix()
{
int NumberOfActiveDOF = 0;
int NumberOfAvailableDOF = 0;
int RowIndex;
foreach (Element i in Elements)
{
NumberOfActiveDOF += Convert.ToInt32(GetElementStateExistanceVector(i.ElementId).Sum()) / 2;
NumberOfAvailableDOF += 6;
}
Matrix <double> GlobalKeepMatrix = CreateMatrix.Dense <double>(NumberOfActiveDOF * 3, NumberOfAvailableDOF * 3);
foreach (Element i in Elements)
{
Vector <double> DOFExist = GetElementStateExistanceVector(i.ElementId);
RowIndex = 0;
for (int index = 0; index < 6; index++)
{
if (DOFExist[index] == 1)
{
GlobalKeepMatrix.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3), index * 3, RowIndex);
RowIndex += 3;
}
}
}
this.KeepMatrix = GlobalKeepMatrix;
}
public Neural()
{
nodes = new Vector <float> [nodeSetup.Length];
weights = new Matrix <float> [nodeSetup.Length - 1];
biases = new Vector <float> [nodeSetup.Length - 1];
gammaValues = new Vector <float> [nodeSetup.Length - 1];
deltaWeights = new Matrix <float> [nodeSetup.Length - 1];
deltaBiases = new Vector <float> [nodeSetup.Length - 1];
for (int i = 0; i < nodeSetup.Length; i++)
{
nodes[i] = CreateVector.Dense <float>(nodeSetup[i]);
}
for (int i = 0; i < nodeSetup.Length - 1; i++)
{
weights[i] = CreateMatrix.Dense <float>(nodeSetup[i + 1], nodeSetup[i]);
deltaWeights[i] = CreateMatrix.Dense <float>(nodeSetup[i + 1], nodeSetup[i]);
biases[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
deltaBiases[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
gammaValues[i] = CreateVector.Dense <float>(nodeSetup[i + 1]);
}
InitialiseWeights();
InitialiseBiases();
}
//Also assumes no D matrix.
public LinearStateSpaceModel(double[,] AMatrix, double[,] BMatrix,
double[,] CMatrix)
{
A = CreateMatrix.DenseOfArray <double>(AMatrix);
B = CreateMatrix.DenseOfArray <double>(BMatrix);
C = CreateMatrix.DenseOfArray <double>(CMatrix);
D = CreateMatrix.Dense <double>(0, 0);
}
public Network(int dimension, int numOfEpoch = 100)
{
this.numOfEpoch = numOfEpoch;
Dimension = dimension;
NeuronsCount = Dimension * Dimension;
MaxPatternCount = (int)(NeuronsCount / 2 * Math.Log(NeuronsCount));
Memory = CreateMatrix.Dense <double>(NeuronsCount, NeuronsCount);
}
/// <summary>
/// Calculates the local mass matrix
/// </summary>
/// <returns>The local mass matrix</returns>
private Matrix <double> GetLocalMassMatrix()
{
Matrix <double> LocalMassMatrix = CreateMatrix.Dense <double>(6, 6);
LocalMassMatrix.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3) * this.Mass, 0);
LocalMassMatrix.InsertAtIndex(this.Inertia, 3);
return(LocalMassMatrix);
}
public override AA1_MLP.Entities.DataSet LoadData(string datasetLocation, int featureVectorLength, int outputLength = 1, int skip = 0, bool Normalize = false, bool standardize = false, int?numberOfExamples = null, bool reportOsutput = true, bool permute = false, int?seed = null)
{
string l;
System.IO.StreamReader file = new System.IO.StreamReader(datasetLocation);
Matrix <double> input = CreateMatrix.Dense <double>(1, featureVectorLength, 0);
Matrix <double> output = CreateMatrix.Dense <double>(1, outputLength, 0);
int i = 0;
while ((l = file.ReadLine()) != null)
{
if (!string.IsNullOrWhiteSpace(l) && !l.StartsWith("#"))
{
var line = l.Split(',');
if (i == 0)
{
input.SetRow(i, line.Skip(skip).Take(featureVectorLength).Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray());
if (reportOsutput)
{
output.SetRow(i, line.Skip(featureVectorLength + skip).Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray());
}
}
else
{
input = input.InsertRow(i, CreateVector.Dense(line.Skip(skip).Take(featureVectorLength).Select(s => double.Parse(s)).ToArray()));
if (reportOsutput)
{
output = output.InsertRow(i, CreateVector.Dense(line.Skip(featureVectorLength + skip).Select(s => double.Parse(s)).ToArray()));
}
}
i++;
if (numberOfExamples != null)
{
numberOfExamples--;
if (numberOfExamples == 0)
{
break;
}
}
}
}
file.Close();
// string[] lines = File.ReadAllLines(datasetLocation);
// lines = lines.Select(s => !string.IsNullOrWhiteSpace(s));
// DataSet trainingSet = new DataSet(input.NormalizeColumns(2.0), output);
DataSet trainingSet = new DataSet(Normalize ? input.NormalizeColumns(2.0) : input, output);
if (standardize)
{
StandardizeData(trainingSet);
}
return(trainingSet);
}
public Matrix <double> softmax(Matrix <double> weightedActivation)
{
Matrix <double> result = CreateMatrix.Dense <double>(weightedActivation.RowCount, weightedActivation.ColumnCount);
foreach (MathNet.Numerics.Tuple <int, Vector <double> > row in weightedActivation.PointwiseExp().EnumerateRowsIndexed())
{
result.SetRow(row.Item1, row.Item2 / row.Item2.Sum());
}
return(result);
}
public static Matrix <double> GetRoesselMatrix(Vector <double> InputVector)
{
Matrix <double> RoesselMatrix = CreateMatrix.Dense <double>(3, 3);
RoesselMatrix[0, 1] = -InputVector[2];
RoesselMatrix[0, 2] = InputVector[1];
RoesselMatrix[1, 0] = InputVector[2];
RoesselMatrix[1, 2] = -InputVector[0];
RoesselMatrix[2, 0] = -InputVector[1];
RoesselMatrix[2, 1] = InputVector[0];
return(RoesselMatrix);
}
public LayerWeights(int rows, int columns)
{
weights = CreateMatrix.Dense <double>(rows, columns);
Func <double, double> randomize = x => (double)UnityEngine.Random.Range(-1f, 1f);
weights.MapInplace(randomize, Zeros.Include);
bias = CreateVector.Dense <double>(columns);
bias.MapInplace(randomize, Zeros.Include);
this.rows = rows;
this.columns = columns;
}
static void Main(string[] args)
{
var matrix = CreateMatrix.DenseOfArray <double>(new double[, ] {
{ 1.0, -1.0, 2.0 },
{ 0.0, -3.0, 1.0 }
});
var vector = CreateVector.Dense <double>(new double[] { 2, 1, 0 });
//Matrix-Vector product
Console.WriteLine(MatrixDotVector(matrix, vector)); //Expected: [1 -3]
//Hadamard Procut
Console.WriteLine(HadamardProduct(vector, vector)); //Expected: [4,1,0]
//Pointwise substraction
Console.WriteLine(PointwiseSubstraction(vector, vector)); //Expected: [0,0,0]
Console.WriteLine(PointwiseSubstraction(matrix, matrix)); //Expected: [0,0,0][0,0,0]
//Mask a row/column (but not clear it)
var uMatrix = CreateMatrix.Dense <double>(2, 3, 1.0);
uMatrix.ClearColumns(0, 2);
var cMatrix = CreateMatrix.Dense <double>(2, 3, 1.0);
cMatrix.ClearColumn(1);
//with 1st and 3rd columns masked
var newMatrix = matrix.PointwiseMultiply(uMatrix);
Console.WriteLine(newMatrix);
//original matrix
Console.WriteLine(matrix);
//update the newMatrix
newMatrix[0, 1] = 100;
newMatrix[1, 1] = 200;
Console.WriteLine(newMatrix);
//push new values back to original
matrix = matrix.PointwiseMultiply(cMatrix) + newMatrix.PointwiseMultiply(uMatrix);
Console.WriteLine(matrix);
//Vector equality
Vector <double> a1 = CreateVector.DenseOfArray <double>(new double[] { 1.2, 3.4, 5.6 });
Vector <double> a2 = CreateVector.DenseOfArray <double>(new double[] { 1.2, 3.4, 5.6 });
Console.WriteLine(a1 == a2);
Console.WriteLine(a1.Equals(a2));
//Find biggest element in vector
Vector <double> c = CreateVector.DenseOfArray <double>(new double[] { 1.2, 3.4, 5.6 });
Console.WriteLine(c.MaximumIndex());
}
public static Matrix <double> GenerateMaskingMatrix(int rows, int cols, int[] mask, bool vertical)
{
var matrix = CreateMatrix.Dense <double>(rows, cols, 1.0);
if (vertical)
{
matrix.ClearColumns(mask);
}
else
{
matrix.ClearRows(mask);
}
return(matrix);
}
public static Matrix <double> GenerateComplementaryMarix(int rows, int cols, int[] mask, bool vertical)
{
var matrix = CreateMatrix.Dense <double>(rows, cols, 1.0);
if (vertical)
{
matrix.ClearColumns(mask);
}
else
{
matrix.ClearRows(mask);
}
matrix = (matrix - 1).PointwiseAbs();
return(matrix);
}
// Start is called before the first frame update
void Start()
{
del = FindObjectOfType <GameManager>();
nn = new NeuralNet(inputNodes, hiddenNodes, outputNodes);
X = CreateMatrix.Dense <double>(trainingSize, inputNodes);
Y = CreateMatrix.Dense <double>(trainingSize, outputNodes);
remainingCups = new bool[10];
remainingIterations = trainingSize;
training = true;
Train();
Time.timeScale = 20.0f;
Debug.Log(nn.ToString());
}
/// <summary>
/// Calculates the derivative of the "Local Matrix", see 'GetLocalMatrix'
/// </summary>
/// <returns>Matrix with rotational matrices on its diagonal</returns>
private Matrix <double> GetLocalMatrixDerivative()
{
int NumElements = System.Elements.Count;
int ElementIndex = GetElementIndex() * 3;
Matrix <double> LocalMatrixRotationDerivative = CreateMatrix.Dense <double>(NumElements * 6 * 3, NumElements * 6 * 3);
for (int i = 0; i < LocalMatrixRotationDerivative.ColumnCount; i += 3)
{
LocalMatrixRotationDerivative.InsertAtIndex(this.LocalRotationMatrixTotalDerivative, i);
}
LocalMatrixRotationDerivative.InsertAtIndex(LocalRotationMatrixPartialDiffTotalGamma * LocalRotationMatrixPartialDiffBeta, ElementIndex + 3 * 3);
LocalMatrixRotationDerivative.InsertAtIndex(LocalRotationMatrixPartialDiffBeta, ElementIndex + 3 * 3 + 3);
LocalMatrixRotationDerivative.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3), ElementIndex + 3 * 3 + 6);
return(LocalMatrixRotationDerivative);
}
private void JTIterate()
{
var jacobian = ComputeJacobian();
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var deltaE = SafeDeltaE(goal, baseJoint.EndEffector);
var deltaEVec = CreateVector.Dense(new float[] { deltaE.X, deltaE.Y });
var denom = (jacobianMat * jacobianMat.Transpose() * deltaEVec).L2Norm();
var lambda = (deltaEVec * jacobianMat * jacobianMat.Transpose() * deltaEVec) / (denom * denom);
var deltaPhiVec = (float)lambda * jacobianMat.Transpose() * deltaEVec;
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
baseJoint.ApplyDofDeltas(deltaPhi);
}
private Matrix <double> BuildSimilarityMatrix(IList <IList <string> > sentances)
{
var matrix = CreateMatrix.Dense <double>(sentances.Count, sentances.Count);
for (int idx1 = 0; idx1 < sentances.Count; idx1++)
{
for (int idx2 = 0; idx2 < sentances.Count; idx2++)
{
if (idx1 != idx2)
{
matrix[idx1, idx2] = SentanceSimilarity(sentances[idx1], sentances[idx2]);
}
}
}
return(matrix);
}
private void DLSIterate()
{
var jacobian = ComputeJacobian();
var deltaE = SafeDeltaE(goal, baseJoint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var lambda = .5f;
var lambdaMatrix = lambda * lambda * CreateMatrix.DenseIdentity <float>(jacobianMat.RowCount);
var deltaPhiVec =
jacobianMat.Transpose() *
(jacobianMat * jacobianMat.Transpose() + (lambdaMatrix)).Inverse() *
CreateVector.DenseOfArray(new float[] { deltaE.X, deltaE.Y });
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
baseJoint.ApplyDofDeltas(deltaPhi);
}
public void IterateDLS(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var lambda = DLSLambda;
var lambdaMatrix = lambda * lambda * CreateMatrix.DenseIdentity <float>(jacobianMat.RowCount);
var deltaPhiVec =
jacobianMat.Transpose() *
(jacobianMat * jacobianMat.Transpose() + (lambdaMatrix)).Inverse() *
CreateVector.DenseOfArray(new float[] { deltaE.X, deltaE.Y });
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
joint.ApplyDofDeltas(deltaPhi);
}
/// <summary>
/// Calculates the "Local Matrix" (matrix with the partial diff rotation matrix and a unity matrix on the local states and the element rotation matrix on the others)
/// </summary>
/// <param name="LocalStateVector">Vector of the local states</param>
/// <returns>Matrix with Rotational matrices on its diagonal</returns>
private Matrix <double> GetLocalMatrixTranslation()
{
int NumElements = System.Elements.Count;
int ElementIndex = GetElementIndex() * 3;
Matrix <double> LocalMatrixTranslation = CreateMatrix.Dense <double>(NumElements * 6 * 3, NumElements * 6 * 3);
for (int i = 0; i < LocalMatrixTranslation.ColumnCount; i += 3)
{
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrix * 2, i);
}
LocalMatrixTranslation.InsertAtIndex(CreateMatrix.DenseIdentity <double>(9), ElementIndex);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffAlpha, ElementIndex + 3 * 3);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffBeta * 3, ElementIndex + 3 * 3 + 3);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffGamma * 4, ElementIndex + 3 * 3 + 6);
return(LocalMatrixTranslation);
}
private static Matrix <double> GenerateRandomGrid(int size)
{
var randoms = new double[size * size];
var loopLimit = Math.Min(size * size, 55);
for (long k = 1; k <= loopLimit; k++)
{
randoms[k - 1] = ((100003 - 200003 * k + 300007 * k * k * k) % 1000000) - 500000;
}
loopLimit = size * size;
for (long k = 56; k < loopLimit; k++)
{
randoms[k - 1] = ((randoms[k - 25] + randoms[k - 56] + 1000000) % 1000000) - 500000;
}
return(CreateMatrix.Dense(size, size, randoms));
}
public void IterateJI(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var inverse = jacobianMat.GeneralizedInverse();
var pInverse = jacobianMat.SVDPseudoInverse();
var deltaPhi = new LinkedList <float>();
foreach (var row in pInverse.ToRowArrays())
{
deltaPhi.AddLast((deltaE.X * row[0]) + (deltaE.Y * row[1]));
}
joint.ApplyDofDeltas(deltaPhi);
}
