public DenseMatrix createMatrix(Model model)
{
int numConstraints = model.Constraints.Count;
int numDecisionVars = model.Goal.Coefficients.Length;
int varCounter = numDecisionVars;
// matrix(rows, columns)
DenseMatrix coefficients = new DenseMatrix(numConstraints, numDecisionVars);
DenseMatrix artificialVars = new DenseMatrix(numConstraints, 1);
var constraintCounter = 0;
this.rhsValues = new DenseVector(numConstraints);
this.basics = new List<int>();
this.artificialRows = new List<int>();
foreach (var constraint in model.Constraints) {
rhsValues[constraintCounter] = constraint.Value;
// if the constraint RHS is negative, invert the coefficients and flip the inequality sign
if (constraint.Value < 0)
{
for (int i = 0; i< model.Goal.Coefficients.Length; i++) {
model.Goal.Coefficients[i] = model.Goal.Coefficients[i] * -1;
}
if (constraint.Relationship == Relationship.LessThanOrEquals)
{
constraint.Relationship = Relationship.GreaterThanOrEquals;
}
else if (constraint.Relationship == Relationship.GreaterThanOrEquals)
{
constraint.Relationship = Relationship.LessThanOrEquals;
}
// also flip the rhs value which we already put in the array for the simplex setup
rhsValues[constraintCounter] = rhsValues[constraintCounter] * -1;
}
coefficients.SetRow(constraintCounter, 0, constraint.Coefficients.Length, new DenseVector(constraint.Coefficients));
// if it's a less than, add a slack column to the coefs matrix
if (constraint.Relationship == Relationship.LessThanOrEquals)
{
DenseVector slack = DenseVector.Create(model.Constraints.Count, delegate(int s) { return 0; });
slack.At(constraintCounter, 1);
coefficients = (DenseMatrix)coefficients.Append(slack.ToColumnMatrix());
this.basics.Add(varCounter);
}
else
{
// Need to add an artificial variable for >= and = constraints
DenseVector surplus = DenseVector.Create(model.Constraints.Count, delegate(int s) { return 0; });
surplus.At(constraintCounter, -1);
coefficients = (DenseMatrix)coefficients.Append(surplus.ToColumnMatrix());
DenseVector artificial = DenseVector.Create(model.Constraints.Count, delegate(int s) { return 0; });
artificial.At(constraintCounter, 1);
artificialVars = (DenseMatrix)artificialVars.Append(artificial.ToColumnMatrix());
// Keeps track of the rows with artificial variable, for setting w
artificialRows.Add(constraintCounter);
}
varCounter++;
constraintCounter++;
}
// put the constraints and stuff into the matrix
if (artificialVars.ColumnCount > 1)
{
artificialVars = (DenseMatrix)artificialVars.SubMatrix(0, artificialVars.RowCount, 1, artificialVars.ColumnCount - 1);
for (int i = coefficients.ColumnCount; i < coefficients.ColumnCount + artificialVars.ColumnCount; i++)
{
this.basics.Add(i);
}
coefficients = (DenseMatrix)coefficients.Append(artificialVars);
numArtificial = artificialVars.ColumnCount;
}
else
{
numArtificial = 0;
}
return coefficients;
}
/// <summary>
/// Run example
/// </summary>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create square matrix
var matrix = new DenseMatrix(5);
var k = 0;
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrix[i, j] = k++;
}
}
Console.WriteLine(@"Initial matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Create vector
var vector = new DenseVector(new[] { 50.0, 51.0, 52.0, 53.0, 54.0 });
Console.WriteLine(@"Sample vector");
Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Insert new column
var result = matrix.InsertColumn(3, vector);
Console.WriteLine(@"1. Insert new column");
Console.WriteLine(result.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Insert new row
result = matrix.InsertRow(3, vector);
Console.WriteLine(@"2. Insert new row");
Console.WriteLine(result.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Set column values
matrix.SetColumn(2, (Vector)vector);
Console.WriteLine(@"3. Set column values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 4. Set row values.
matrix.SetRow(3, (double[])vector);
Console.WriteLine(@"4. Set row values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 5. Set diagonal values. SetRow/SetColumn/SetDiagonal accepts Vector and double[] as input parameter
matrix.SetDiagonal(new[] { 5.0, 4.0, 3.0, 2.0, 1.0 });
Console.WriteLine(@"5. Set diagonal values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 6. Set submatrix values
matrix.SetSubMatrix(1, 3, 1, 3, DenseMatrix.Identity(3));
Console.WriteLine(@"6. Set submatrix values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Permutations.
// Initialize a new instance of the Permutation class. An array represents where each integer is permuted too:
// indices[i] represents that integer "i" is permuted to location indices[i]
var permutations = new Permutation(new[] { 0, 1, 3, 2, 4 });
// 7. Permute rows 3 and 4
matrix.PermuteRows(permutations);
Console.WriteLine(@"7. Permute rows 3 and 4");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 8. Permute columns 1 and 2, 3 and 5
permutations = new Permutation(new[] { 1, 0, 4, 3, 2 });
matrix.PermuteColumns(permutations);
Console.WriteLine(@"8. Permute columns 1 and 2, 3 and 5");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 9. Concatenate the matrix with the given matrix
var append = matrix.Append(matrix);
// Concatenate into result matrix
matrix.Append(matrix, append);
Console.WriteLine(@"9. Append matrix to matrix");
Console.WriteLine(append.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 10. Stack the matrix on top of the given matrix matrix
var stack = matrix.Stack(matrix);
// Stack into result matrix
matrix.Stack(matrix, stack);
Console.WriteLine(@"10. Stack the matrix on top of the given matrix matrix");
Console.WriteLine(stack.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 11. Diagonally stack the matrix on top of the given matrix matrix
var diagoinalStack = matrix.DiagonalStack(matrix);
// Diagonally stack into result matrix
matrix.DiagonalStack(matrix, diagoinalStack);
Console.WriteLine(@"11. Diagonally stack the matrix on top of the given matrix matrix");
Console.WriteLine(diagoinalStack.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
private void optimize(DenseMatrix coefficients, DenseVector objFunValues, bool artifical)
{
//for calculations on the optimal solution row
int cCounter,
width = coefficients.ColumnCount;
DenseVector cBVect = new DenseVector(basics.Count);
//Sets up the b matrix
DenseMatrix b = new DenseMatrix(basics.Count, 1);
//basics will have values greater than coefficients.ColumnCount - 1 if there are still artificial variables
//or if Nathan is bad and didn't get rid of them correctly
foreach (int index in basics)
{
b = (DenseMatrix)b.Append(DenseVector.OfVector(coefficients.Column(index)).ToColumnMatrix());
}
// removes the first column
b = (DenseMatrix)b.SubMatrix(0, b.RowCount, 1, b.ColumnCount - 1);
double[] cPrimes = new double[width];
double[] rhsOverPPrime;
DenseMatrix[] pPrimes = new DenseMatrix[width];
DenseMatrix bInverse;
int newEntering, exitingRow;
bool optimal = false;
if(artifical)
{
rhsOverPPrime = new double[numConstraints + 1];
}
else
{
rhsOverPPrime = new double[numConstraints];
}
while (!optimal)
{
//calculates the inverse of b for this iteration
bInverse = (DenseMatrix)b.Inverse();
//updates the C vector with the most recent basic variables
cCounter = 0;
foreach (int index in basics)
{
cBVect[cCounter++] = objFunValues.At(index);
}
//calculates the pPrimes and cPrimes
for (int i = 0; i < coefficients.ColumnCount; i++)
{
if (!basics.Contains(i))
{
pPrimes[i] = (DenseMatrix)bInverse.Multiply((DenseMatrix)coefficients.Column(i).ToColumnMatrix());
//c' = objFunVals - cB * P'n
//At(0) to turn it into a double
cPrimes[i] = objFunValues.At(i) - (pPrimes[i].LeftMultiply(cBVect)).At(0);
}
else
{
pPrimes[i] = null;
}
}
//RHS'
xPrime = (DenseMatrix)bInverse.Multiply((DenseMatrix)rhsValues.ToColumnMatrix());
//Starts newEntering as the first nonbasic
newEntering = -1;
int iter = 0;
while(newEntering == -1)
{
if(!basics.Contains(iter))
{
newEntering = iter;
}
iter++;
}
//new entering becomes the small cPrime that corresponds to a non-basic value
for (int i = 0; i < cPrimes.Length; i++)
{
if (cPrimes[i] < cPrimes[newEntering] && !basics.Contains(i))
{
newEntering = i;
}
}
//if the smallest cPrime is >= 0, ie they are all positive
if (cPrimes[newEntering] >= 0)
{
optimal = true;
}
else
{
//fix me to deal with if all these values are negative
exitingRow = 0;
for (int i = 0; i < xPrime.RowCount; i++)
{
double[,] pPrime = pPrimes[newEntering].ToArray();
rhsOverPPrime[i] = xPrime.ToArray()[i, 0] / pPrime[i, 0];
if (rhsOverPPrime[i] < rhsOverPPrime[exitingRow] && rhsOverPPrime[i] > 0 )
{
exitingRow = i;
}
}
//translates from the index in the basics list to the actual row
exitingRow = basics[exitingRow];
//make sure you're not being stupid here!!!!
int tempIndex = basics.IndexOf(exitingRow);
basics.Remove(exitingRow);
basics.Insert(tempIndex, newEntering);
b.SetColumn(basics.IndexOf(newEntering), coefficients.Column(newEntering));
}
}
}
public static void TestPosePrimitives()
{
DenseVector poseQuat = new DenseVector(new double[] {1, 0, 0, 0});
DenseVector posePosition = new DenseVector(new double[] {0, 0, 1});
DenseMatrix inverseCalibration = (DenseMatrix)Pose3D.CreateCalibrationMatrix(525, 320, 240).Inverse();
DenseMatrix dPixels = new DenseMatrix(3, 1);
dPixels[0, 0] = 320;
dPixels[1, 0] = 240;
dPixels[2, 0] = 1;
dPixels = (DenseMatrix)dPixels.Append(dPixels);
Console.WriteLine("New: Using calibration Matrix");
Console.WriteLine(Pose3D.DPixelToWorld(poseQuat, posePosition, inverseCalibration, dPixels, new DenseMatrix(1, dPixels.ColumnCount, 1.0)).ToString("0.0"));
//Console.WriteLine("Old: Using calibration Matrix");
//Console.WriteLine(Pose3D.DPixelToWorld_Old(poseQuat, posePosition, inverseCalibration, dPixels).ToString("0.0"));
Console.WriteLine("New: No calibration Matrix");
Console.WriteLine(Pose3D.DPixelToWorld(poseQuat, posePosition, DenseMatrix.Identity(3), dPixels, new DenseMatrix(1, dPixels.ColumnCount, 1.0)).ToString("0.0"));
//Console.WriteLine("Old: No calibration Matrix");
//Console.WriteLine(Pose3D.DPixelToWorld_Old(poseQuat, posePosition, DenseMatrix.Identity(4), dPixels).ToString("0.0"));
}
