/// <summary>
/// Performs a simulation step using Symplectic integration.
/// </summary>
private void stepSymplectic()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
v += TimeStep * (Minv * f);
x += TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
public imageConditionAndData(object inputDenseMatrix, Image <Gray, Byte> binaryMaskImage = null)
{
if (inputDenseMatrix.GetType() != typeof(DenseMatrix))
{
return;
}
dmSourceData = (DenseMatrix)((DenseMatrix)inputDenseMatrix).Clone();
if (binaryMaskImage == null)
{
maskImageBinary = new Image <Gray, byte>(dmSourceData.ColumnCount, dmSourceData.RowCount);
maskImageBinary.SetValue(new Gray(1));
}
else
{
maskImageBinary = binaryMaskImage.Copy();
}
isImageGrayscaled = false;
//sourceImageBgr = imgPr.tmpImage.Copy();
//sourceImageGrayscale = null;
thresholdingUsageTop = false;
thresholdingUsageBtm = true;
thresholdingValue = 255;
selection = null;
currentColorScheme = new ColorScheme("");
//makeThresholdedDataImage();
dataHasBeenModified = true;
HighlightMask = null;
highlightedArea = 0.0d;
}
//attention!
//int channelNum - zero-based channel number with B-G-R scheme
public imageConditionAndData(Bitmap coloredBitmap, int channelNum)
{
ImageProcessing imgPr = new ImageProcessing(coloredBitmap, true);
maskImageBinary = imgPr.significantMaskImageBinary;
//imgPr.Dispose();
isImageGrayscaled = true;
sourceImageGrayscale = new Image <Bgr, Byte>(coloredBitmap)[channelNum];
sourceImageBgr = null;
sourceImageGrayscale = sourceImageGrayscale.Mul(maskImageBinary);
thresholdingUsageTop = false;
thresholdingUsageBtm = true;
thresholdingValue = 255;
//Image<Gray, Byte> thresholdedImage = imgPr.getMaskedImageChannelBitmapThresholded(channelNum, thresholdingValue, thresholdingUsageTop, thresholdingUsageBtm);
dmSourceData = ImageProcessing.DenseMatrixFromImage(sourceImageGrayscale);
currentColorScheme = new ColorScheme("");
currentColorSchemeRuler = new ColorSchemeRuler(this, dataMinValue(), dataMaxValue());
selection = null;
//makeThresholdedDataImage();
dataHasBeenModified = true;
HighlightMask = null;
highlightedArea = 0.0d;
}
/// <summary>
/// Initializes a new instance of the <see cref="imageConditionAndData"/> class.
/// note: grayscaled, with fixed grayscale ruler 0.0d-255.0d
/// </summary>
/// <param name="imgPr">The img pr.</param>
/// <param name="channelNum">The channel number.</param>
public imageConditionAndData(ImageProcessing imgPr, int channelNum)
{
maskImageBinary = imgPr.significantMaskImageBinary.Copy();
isImageGrayscaled = true;
sourceImageGrayscale = imgPr.getMaskedImageChannelImage(channelNum);
sourceImageBgr = null;
thresholdingUsageTop = false;
thresholdingUsageBtm = true;
thresholdingValue = 255;
dmSourceData = ImageProcessing.DenseMatrixFromImage(sourceImageGrayscale);
currentColorScheme = new ColorScheme("");
currentColorSchemeRuler = new ColorSchemeRuler(this, dataMinValue(), dataMaxValue());
selection = null;
//makeThresholdedDataImage();
dataHasBeenModified = true;
HighlightMask = null;
highlightedArea = 0.0d;
}
public static Matrix<double> CreateDCTMatrix(int n)
{
if (dctMatrixes.ContainsKey(n))
{
return dctMatrixes[n];
}
var matrix = new DenseMatrix(n);
var val = 1d / Math.Sqrt(n);
for (int i = 0; i < n; i++)
{
matrix[0, i] = val;
}
var sqrt2 = Math.Sqrt(2d / n);
for (int x = 0; x < n; x++)
{
for (int y = 1; y < n; y++)
{
matrix[x, y] = sqrt2 * Math.Cos((Math.PI / 2 / n) * y * (2 * x + 1));
}
}
dctMatrixes[n] = matrix;
return matrix;
}
// Return convolution A * B
// Size of returned matrix = size of A, but
// elements on boundaries ( of length rows(cols) of B / 2 )
// are not computed and set to 0
// Size of B must be odd
public static Matrix Convolve(Matrix A, Matrix B)
{
Matrix conv = new DenseMatrix(A.RowCount, A.ColumnCount);
int row2 = B.RowCount / 2;
int col2 = B.ColumnCount / 2;
int xmax = A.ColumnCount - col2;
int ymax = A.RowCount - row2;
int y, x, dx, dy;
double maskSum;
for ( y = row2; y < ymax; ++y)
for ( x = col2; x < xmax; ++x)
{
maskSum = 0.0f;
for ( dy = -row2; dy <= row2; ++dy)
for ( dx = -col2; dx <= col2; ++dx)
{
maskSum += A[y + dy, x + dx] * B[row2 + dy, col2 + dx];
}
conv[y, x] = maskSum;
}
return conv;
}
public SalesmanProblem(DenseMatrix costs, double startRecord, IEnumerable<int> startChain)
: this(costs)
{
Record = startRecord;
RecordChain = new List<int>(startChain);
AnswerFound = true;
}
/// <summary>
/// Creates a new Input from double values.
/// Note that we store each example as a row in the X matrix. While calculating Theta vector, we need to insert the top column of all ones into the X matrix - this will allow us to treat theta0 as just another feature.
/// </summary>
internal Input(double[,] x, double[] y, int skip, int take)
{
if (take == 0) {
X = null;
Y = null;
return;
}
var samples = x.GetLength(0);
var features = x.GetLength(1);
//make sure we add first column of ones
var x1 = new double[take, features + 1];
var y1 = new double[take];
for (int sample = 0; sample < samples; sample++) {
if (sample < skip) {
continue;
}
for (int feature = 0; feature < features + 1; feature++) {
x1[sample - skip, feature] = (feature == 0) ? 1 : x[sample, feature - 1];
}
y1[sample - skip] = y[sample];
take--;
if (take == 0) {
break;
}
}
X = new DenseMatrix(x1);
Y = new DenseVector(y1).ToColumnMatrix();
}
public void FindTransformation(ArrayList kinectCoors, ArrayList projectorCoors)
{
PrepareMatrices(kinectCoors, projectorCoors);
DenseMatrix problem = foundCoordinatesMatrix;
result = (DenseMatrix) problem.QR().Solve(rightSideMatrix);
Console.Out.WriteLine(result);
}
public void GetMassInverse(MatrixXD massInv)
{
massInv.SetSubMatrix(index, index, massInv.SubMatrix(index, 3, index, 3)
+ 1.0f / Mass * DenseMatrixXD.CreateIdentity(3));
massInv.SetSubMatrix(index + 3, index + 3, massInv.SubMatrix(index + 3, 3, index + 3, 3)
+ m_inertiainv);
}
public static void WriteDataToJson(string[] symbols, DateTime[] dateTimes, DenseMatrix data, string filename)
{
StringBuilder jsonStringBuilder = new StringBuilder();
jsonStringBuilder.Append("[");
for (int i = 0; i < data.ColumnCount; i++)
{
jsonStringBuilder.Append("[");
jsonStringBuilder.Append(dateTimes[i].Subtract(new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc)).TotalMilliseconds.ToString());
foreach (double d in data.Column(i))
{
jsonStringBuilder.Append(",");
jsonStringBuilder.Append((d.Equals(double.NaN) ? "null" : Math.Round(d, 8).ToString()));
}
jsonStringBuilder.Append("]");
if (i != data.ColumnCount - 1) jsonStringBuilder.Append(",\n");
}
jsonStringBuilder.Append("]");
using (var file = new StreamWriter(QSConstants.DEFAULT_DATA_FILEPATH + @filename))
{
file.WriteLine(jsonStringBuilder.ToString());
}
}
public Matrix3d(int NumberOfRows, int NumberOfColumns, int NumberOfLayers)
{
_data = new DenseMatrix[NumberOfLayers];
for (int i = 0; i < NumberOfLayers; i++)
_data[i] = new DenseMatrix(NumberOfRows, NumberOfColumns);
}
public MainWindow()
{
InitializeComponent();
Graph = new UserControl1();
this.Content = Graph;
var t = Generate();
// var s = t.Item1.Svd(true);
//var nd = t.Item1.Multiply(s.VT().SubMatrix(0, 8, 0, 8));
var mlp = new MLP(8, 100, 1);
var r = mlp.Train(t.Item1, t.Item2, null, null, 2500);
Graph.Set(r.TrainingSquaredError, r.TrainingError);
DenseMatrix data = new DenseMatrix(new double[,] { { 1, 1 }, { 1, 0 }, { 0, 0 }, { 0, 1 } });
DenseMatrix labels = new DenseMatrix(new double[,] { { 1, 0 }, { 0, 1 }, { 1, 0 }, { 0, 1 } });
//var mlp = new MLP(2, 100, 2);
//var r = mlp.Train(data, labels, data, labels, 5000);
//Graph.Set(r.TrainingSquaredError, r.TrainingError);
}
public static DenseMatrix ToMatrix(this object[,] data, int rowStart, int rowEnd, int colStart, int colEnd,
bool reverseColumns)
{
var d = new DenseMatrix(rowEnd - rowStart + 1, colEnd - colStart + 1);
if (reverseColumns)
{
for (int i = rowEnd; i >= rowStart; i--)
{
for (int j = colStart; j <= colEnd; j++)
{
d[rowEnd - i, j - colStart] = (double) data[i, j];
}
}
}
else
{
for (int i = rowStart; i <= rowEnd; i++)
{
for (int j = colStart; j <= colEnd; j++)
{
d[i - rowStart, j - colStart] = (double) data[i, j];
}
}
}
return d;
}
public void Optimize(Dictionary<double, double> values)
{
var n = _f.Functions.Count();
var xs = values.Select(v => v.Key).ToList();
var ys = values.Select(v => v.Value).ToList();
var fs = new List<List<double>>(n);
for (var i = 0; i < n; i++)
{
fs[i] = _f.Functions[i].Evaluate(xs);
}
var matrix = new DenseMatrix(n, n);
var vector = new DenseVector(n);
for (var i = 0; i < n; i++)
{
for (var j = 0; j < n; j++)
{
matrix[i, j] = fs[i].ScalarProduct(fs[j]);
}
vector[i] = ys.ScalarProduct(fs[i]);
}
var matrixInverse = matrix.Inverse();
var result = matrixInverse * vector;
for (var i = 0; i < n; i++)
{
_f.LinearParameters[i].Value = result[i];
}
}
DenseMatrix[] sigmas; //covariance of the 3D gaussians.
#endregion Fields
#region Constructors
public CD_HMM(MarkovChain A, DenseVector pi, DenseVector[] mus, DenseMatrix[] sigmas)
{
this.A = A;
this.pi = pi;
this.mus = mus;
this.sigmas = sigmas;
}
public void GetMass(MatrixXD mass)
{
mass.SetSubMatrix(index, index, mass.SubMatrix(index, 3, index, 3)
+ Mass * DenseMatrixXD.CreateIdentity(3));
mass.SetSubMatrix(index + 3, index + 3, mass.SubMatrix(index + 3, 3, index + 3, 3)
+ m_inertia);
}
private static void GetLinRegArgs(IEnumerable<double[]> items, out DenseMatrix xM, out DenseVector yV)
{
var original = items;
var transformed = original.Select(a => new Tuple<double[], double>(Phi(a), a[2])).ToList();
xM = DenseMatrix.Create(transformed.Count, transformed[0].Item1.Length, (r, c) => transformed[r].Item1[c]);
yV = DenseVector.OfEnumerable(transformed.Select(t => t.Item2));
}
public static Matrix<double> CorrelationMatrix(List<List<double>> x)
{
Matrix<double> correlM = new DenseMatrix(x.Count, x.Count);
//off diagonal elements
for (int i = 0; i < x.Count; i++)
{
var rowx = x[i];
for (int j = 0; j < x.Count; j++)
{
var rowy = x[j];
if (i < j)
{
correlM[i, j] = Correlation.Pearson(rowx, rowy);
}
if (i > j)
{
correlM[i, j] = correlM[j, i];
}
}
}
//Diagonal elements
for (int i = 0; i < x.Count; i++)
{
correlM[i, i] = 1;
}
return correlM;
}
public void Load(string FileName)
{
using (StreamReader sr = new StreamReader(FileName))
{
string line = sr.ReadLine();
NumberOfColumns = int.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
line = sr.ReadLine();
NumberOfRows = int.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
line = sr.ReadLine();
XOrigin = double.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
line = sr.ReadLine();
YOrigin = double.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
line = sr.ReadLine();
GridSize = double.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
line = sr.ReadLine();
DeleteValue = double.Parse(line.Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries)[1]);
Data = new DenseMatrix(NumberOfRows, NumberOfColumns);
string[] DataRead;
for (int j = 0; j < NumberOfRows; j++)
{
DataRead = sr.ReadLine().Split(new Char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
for (int i = 0; i < NumberOfColumns; i++)
{
Data[NumberOfRows - j - 1, i] = double.Parse(DataRead[i]);
}
}
}
}
public void Compute(DenseMatrix x1, int size)
{
//Setting Preparation.....................................................................................................
_estimationLength = x1.Values.Length;
//Creation of a Z matrix..................................................................................................
// ReSharper disable once CSharpWarnings::CS0618
var z = new DenseMatrix(_estimationLength, size, 1.0);
for (var i = 0; i <= _estimationLength - 1; i++)
{
for (var j = 0; j <= size - 1; j++)
{
z[i, j] = Math.Pow(i,j);
}
}
//Computation of Theta matrix.............................................................................................
var zt = z.Transpose();
var ztz = zt * z;
var theta = ztz.Inverse() * zt * x1;
// ReSharper disable once CSharpWarnings::CS0618
_output = new DenseMatrix(_estimationLength, 1, 1.0);
//Output creation.........................................................................................................
for (var i = 0; i <= _estimationLength - 1; i++)
{
for (var j = 0; j <= size - 1; j++)
{
_output[i, 0] += theta[j,0]*Math.Pow(i,j);
}
}
_estimationDone = true;
}
public void Factorize(Matrix meanMatrix)
{
int count = 0;
movieCount = meanMatrix.RowCount;
userCount = meanMatrix.ColumnCount;
convergedMovie = DenseMatrix.Build.Dense(K, movieCount, 0);
convergedUser = DenseMatrix.Build.Dense(userCount, K, 0);
A = new DenseMatrix(DenseColumnMajorMatrixStorage<double>.OfMatrix(DenseMatrix.Build.Dense(K, movieCount, 0.1).Storage));
B = new DenseMatrix(DenseColumnMajorMatrixStorage<double>.OfMatrix(DenseMatrix.Build.Dense(userCount, K, 0.1).Storage));
while (count < 50)
{
for (int k = 0; k < K; k++)
{
for (int m = 0; m < movieCount; m++)
{
for (int u = 0; u < userCount; u++)
{
train(k, u, m, meanMatrix[m, u]);
}
}
}
Console.WriteLine(count++);
if (IsConverged())
break;
}
}
/// <summary>
/// 计算模糊矩阵
/// </summary>
private void CalculateFuzzyMatrix()
{
MemberShipFun _memeberShipFun = new MemberShipFun();
for (int i = AHPIndexHierarchyUtil.totalLevelCount - 2; i >= 0; i--)
{
List<AHPIndexHierarchy> iLevelAhpIndexs = _ahpIndexUtil.FindbyLevel(i);
foreach (AHPIndexHierarchy _iLevelAhpIndex in iLevelAhpIndexs)
{
List<string> childrenNames = _iLevelAhpIndex.ChildrenNames;
DenseMatrix _iLevelMatrix = new DenseMatrix(childrenNames.Count, MemberShipFun.HealthLevelCount);
DenseVector _childrenValue = new DenseVector(childrenNames.Count);
for (int j = 0; j < childrenNames.Count; j++)
{
string name = childrenNames[j];
AHPIndexHierarchy _ahpIndex = _ahpIndexUtil.FindbyName(name);
if (i == AHPIndexHierarchyUtil.totalLevelCount - 2)//是底层
{
_ahpIndex.FuzzyValue = _memeberShipFun.TrapezoiMebership(_ahpIndex.IndexValue);
}
_iLevelMatrix = (DenseMatrix)_iLevelMatrix.InsertRow(j, _ahpIndex.FuzzyValue);
_iLevelMatrix = (DenseMatrix)_iLevelMatrix.RemoveRow(j + 1);
//_ahpIndex.ChildrenFuzzyMatrix = (DenseMatrix)_iLevelMatrix;
_childrenValue[j] = _ahpIndex.IndexValue;
}
_iLevelAhpIndex.ChildrenFuzzyMatrix = _iLevelMatrix;
_iLevelAhpIndex.IndexValue = _iLevelAhpIndex.ChildrenWeightVector * _childrenValue;
_iLevelAhpIndex.FuzzyValue = FuzzyOperator.WeightedAverage(_iLevelAhpIndex.ChildrenWeightVector, _iLevelAhpIndex.ChildrenFuzzyMatrix);
}
}
}
// Returns normalisation matrix : xn = Mx
// Uses list of points : each point is 3-vector
// Assusmes that weight of each point is 1
public static Matrix<double> Normalize2D(List<Vector<double>> points)
{
Matrix<double> norm = new DenseMatrix(3, 3);
int n = points.Count;
// Compute center of image points
double xc = 0, yc = 0;
for(int c = 0; c < n; ++c)
{
xc += points[c].At(0);
yc += points[c].At(1);
}
xc /= n;
yc /= n;
// Get mean distance of points from center
double dist = 0;
for(int c = 0; c < n; ++c)
{
dist += Math.Sqrt((points[c].At(0) - xc) * (points[c].At(0) - xc) +
(points[c].At(1) - yc) * (points[c].At(1) - yc));
}
dist /= n;
// Normalize in a way that mean dist = sqrt(2)
double ratio = Math.Sqrt(2) / dist;
// Noramlize matrix - homonogeus point must be multiplied by it
norm[0, 0] = ratio;
norm[1, 1] = ratio;
norm[0, 2] = -ratio * xc;
norm[1, 2] = -ratio * yc;
norm[2, 2] = 1.0;
return norm;
}
public static DenseVector GetRegularizedWeights(DenseMatrix m, double lambda, DenseVector yV)
{
var mt = m.Transpose();
var what = (mt * m);
var identity = DenseMatrix.Identity(what.ColumnCount);
return (DenseVector)((what + lambda * identity).Inverse() * mt * yV);
//return (DenseMatrix)(what.Inverse() * mt);
}
public FedKF(KF[] filters, DenseMatrix dcm, DenseMatrix covariances)
{
_filters = filters;
_filteredSignals = new DenseMatrix(_filters.Length, 1);
_dcm = dcm;
_dcmt = dcm.Transpose();
_cInv = covariances.Inverse();
}
public PortfolioOptimizer(Portfolio p, double minReturn, DenseMatrix cov, DenseVector returns)
{
portfolio = p;
this.returns = returns;
covariance = cov;
this.minReturn = minReturn;
resultWeights = new DenseVector(returns.Count);
}
/// <summary>
/// ctor with params
/// </summary>
/// <param name="N">number of rows of each matrix, point count</param>
/// <param name="L">number of cols of each matrix, it is height of the tree</param>
public BasicFuncBoxes(int N,int L)
{
n = N;
l = L;
x = new DenseMatrix(n, l);
y = new DenseMatrix(n, l);
z = new DenseMatrix(n, l);
}
private void btnTestMatrix_Click(object sender, RoutedEventArgs e)
{
DenseMatrix ds = new DenseMatrix(1);
ds[0, 0] = 1;
//XMLIO.OutputTestMatrix(ds);
//BinaryIO.OutputMatrix(ds);
DenseMatrix ds2 = BinaryIO.ReadMatrix();
}
public DenseMatrix ToMatrix()
{
var res = new DenseMatrix(3, 1);
res[0, 0] = this.X;
res[1, 0] = this.Y;
res[2, 0] = this.Z;
return res;
}
public RigidTransform(double[] motionCenter, double[,] transformation)
{
this.motionCenter = new DenseVector(motionCenter);
this.transformation = DenseMatrix.OfArray(transformation);
List2String l2s = new List2String();
log.Debug("motionCenter: " + l2s.ToString(this.motionCenter.Values) +
" platformCenter: " + l2s.ToString(this.platformCenter.Values));
}
public override ulong Create(BitmapData image)
{
var data = new DenseMatrix(32, 32);
using (var unmanaged = UnmanagedImage.FromManagedImage(image))
using (var filtered = ExtractChannel.Apply(unmanaged))
{
new Convolution(Filter, 9).ApplyInPlace(filtered);
using (var imgdata = new ResizeNearestNeighbor(32, 32).Apply(filtered))
{
unsafe
{
byte* src = (byte*)imgdata.ImageData.ToPointer();
int offset = imgdata.Stride - imgdata.Width;
int width = imgdata.Width;
int height = imgdata.Height;
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++, src++)
{
data.At(y, x, (float)*src / 255);
}
src += offset;
}
}
}
}
var dct = DctMatrix.FastDCT(data);
int dctsize = 8;
var vals = new List<double>();
for (int r = 1; r <= dctsize; r++)
{
for (int c = 1; c <= dctsize; c++)
{
vals.Add(dct[r, c]);
}
}
var sorted = new List<double>(vals);
sorted.Sort();
var mid = dctsize * dctsize / 2;
double median = (sorted[mid - 1] + sorted[mid]) / 2d;
ulong index = 1;
ulong result = 0;
for (int i = 0; i < dctsize * dctsize; i++)
{
if (vals[i] > median)
{
result |= index;
}
index = index << 1;
}
return result;
}
public override void ComputeRectificationMatrices()
{
//function[T1, T2, Pn1, Pn2] = rectify(Po1, Po2)
//% RECTIFY: compute rectification matrices
//% factorize old PPMs
//[A1, R1, t1] = art(Po1);
//[A2, R2, t2] = art(Po2);
//% optical centers(unchanged)
//c1 = - inv(Po1(:,1:3))*Po1(:,4);
// c2 = - inv(Po2(:,1:3))*Po2(:,4);
//% new x axis(= direction of the baseline)
//v1 = (c1-c2);
//% new y axes(orthogonal to new x and old z)
//v2 = cross(R1(3,:)',v1);
//% new z axes(orthogonal to baseline and y)
//v3 = cross(v1, v2);
//% new extrinsic parameters
//R = [v1'/norm(v1)
//v2'/norm(v2)
//v3'/norm(v3)];
//% translation is left unchanged
//% new intrinsic parameters(arbitrary)
//A = (A1 + A2)./2;
//A(1,2)=0; % no skew
//% new projection matrices
//Pn1 = A*[R - R * c1];
//Pn2 = A*[R - R * c2];
//% rectifying image transformation
//T1 = Pn1(1:3, 1:3) * inv(Po1(1:3, 1:3));
//T2 = Pn2(1:3,1:3)* inv(Po2(1:3,1:3));
Vector<double> c1 = CalibrationData.Data.TranslationLeft;
Vector<double> c2 = CalibrationData.Data.TranslationRight;
Vector<double> v1 = c1 - c2;
Vector<double> v2 = CalibrationData.Data.RotationLeft.Row(2).Cross(v1);
Vector<double> v3 = v1.Cross(v2);
_R = new DenseMatrix(3, 3);
_R.SetRow(0, v1.Normalize(2));
_R.SetRow(1, v2.Normalize(2));
_R.SetRow(2, v3.Normalize(2));
Matrix<double> halfRevolve = new DenseMatrix(3, 3);
RotationConverter.EulerToMatrix(new double[] { 0.0, 0.0, Math.PI }, halfRevolve);
_R = halfRevolve * _R;
_K = (CalibrationData.Data.CalibrationLeft + CalibrationData.Data.CalibrationRight).Multiply(0.5);
_K[0, 1] = 0.0;
RectificationLeft = (_K * _R) * ((CalibrationData.Data.CalibrationLeft * CalibrationData.Data.RotationLeft).Inverse());
RectificationRight = (_K * _R) * ((CalibrationData.Data.CalibrationRight * CalibrationData.Data.RotationRight).Inverse());
ComputeScalingMatrices(ImageWidth, ImageHeight);
RectificationLeft = _Ht_L * RectificationLeft;
RectificationLeft = _Ht_R * RectificationRight;
RectificationLeft_Inverse = RectificationLeft.Inverse();
RectificationRight_Inverse = RectificationRight.Inverse();
}
public void Execute()
{
_normalizedTrainingData = NormalizeData(trainingData);
_normalizedPredictionData = NormalizeData(predictionData);
network = CreateNetwork();
IMLDataSet training = GenerateTraining();
Train(training);
Predict();
}
public FFT2d(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmReal, MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmImag)
{
dmInput = null;
dmComplexInput = MathNet.Numerics.LinearAlgebra.Complex.DenseMatrix.Create(dmReal.RowCount,
dmReal.ColumnCount, new Func <int, int, Complex>((row, column) =>
{
return(new Complex(dmReal[row, column], dmImag[row, column]));
}));
}
/// <summary>
/// Run example
/// </summary>
/// <seealso cref="http://en.wikipedia.org/wiki/Triangular_matrix">Triangular matrix</seealso>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create square matrix
var matrix = new DenseMatrix(10);
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 square matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Retrieve a new matrix containing the lower triangle of the matrix
var lower = matrix.LowerTriangle();
// Puts the lower triangle of the matrix into the result matrix.
matrix.LowerTriangle(lower);
Console.WriteLine(@"1. Lower triangle of the matrix");
Console.WriteLine(lower.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Retrieve a new matrix containing the upper triangle of the matrix
var upper = matrix.UpperTriangle();
// Puts the upper triangle of the matrix into the result matrix.
matrix.UpperTriangle(lower);
Console.WriteLine(@"2. Upper triangle of the matrix");
Console.WriteLine(upper.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Retrieve a new matrix containing the strictly lower triangle of the matrix
var strictlylower = matrix.StrictlyLowerTriangle();
// Puts the strictly lower triangle of the matrix into the result matrix.
matrix.StrictlyLowerTriangle(strictlylower);
Console.WriteLine(@"3. Strictly lower triangle of the matrix");
Console.WriteLine(strictlylower.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 4. Retrieve a new matrix containing the strictly upper triangle of the matrix
var strictlyupper = matrix.StrictlyUpperTriangle();
// Puts the strictly upper triangle of the matrix into the result matrix.
matrix.StrictlyUpperTriangle(strictlyupper);
Console.WriteLine(@"4. Strictly upper triangle of the matrix");
Console.WriteLine(strictlyupper.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
public Job_SymbolSet(string[] symbols, string timeframe, int numTicks, AbstractIndicator indicator)
{
this.symbols = symbols;
this.indicator = indicator;
this.timeframe = timeframe;
this.numTicks = numTicks;
mktData = new List<MarketDataEventArg>();
graphData = new DenseMatrix(symbols.Length, numTicks);
}
public FFT2d(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm2Process)
{
dmInput = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm2Process.Clone();
dmComplexInput = new MathNet.Numerics.LinearAlgebra.Complex.DenseMatrix(dmInput.RowCount,
dmInput.ColumnCount);
dmComplexInput.MapIndexedInplace(new Func <int, int, Complex, Complex>(
(row, column, val) =>
{
return(new Complex(dmInput[row, column], 0.0d));
}));
}
private void HardConstraintsStep()
{
// Vectors and matrices to store data
VectorXD C0 = new DenseVectorXD(m_numcs);
VectorXD f = new DenseVectorXD(m_numdofs);
VectorXD v = new DenseVectorXD(m_numdofs);
MatrixXD M = new DenseMatrixXD(m_numdofs, m_numdofs);
MatrixXD J = new DenseMatrixXD(m_numcs, m_numdofs);
MatrixXD System = new DenseMatrixXD(m_numcs + m_numdofs, m_numcs + m_numdofs);
VectorXD Independent = new DenseVectorXD(m_numdofs + m_numcs);
// Compute forces and retrieve the rigid bodies data
foreach (ISimulable i in m_objs)
{
i.clearForcesAndMatrices();
i.addForcesAndMatrices();
i.getForceVector(f);
i.getVelocityVector(v);
i.getMassMatrix(M);
}
// Compute and retrieve restrictions and Jacobians
foreach (Constraint c in m_constraints)
{
c.getConstraintVector(C0);
c.getConstraintJacobian(J);
}
// Set up left-side system
System.SetSubMatrix(0, m_numdofs, 0, m_numdofs, M);
System.SetSubMatrix(m_numdofs, m_numcs, 0, m_numdofs, J);
System.SetSubMatrix(0, m_numdofs, m_numdofs, m_numcs, J.Transpose());
System.SetSubMatrix(m_numdofs, m_numcs, m_numdofs, m_numcs, DenseMatrixXD.Create(m_numcs, m_numcs, 0.0));
// Set up right-side system
VectorXD b = M * v + TimeStep * f;
VectorXD AtC0 = (-1.0f / TimeStep) * C0;
Independent.SetSubVector(0, m_numdofs, b);
Independent.SetSubVector(m_numdofs, m_numcs, AtC0);
// Solve system
VectorXD newVelocities = System.Solve(Independent);
// Update bodies
foreach (ISimulable i in m_objs)
{
i.setVelocityVector(newVelocities);
i.advancePosition();
}
}
/// <summary>
/// Return the skew-symmetric matrix corresponding to the cross product
/// </summary>
public static MatrixXD Skew(Vector3 v)
{
MatrixXD mout = new DenseMatrixXD(3, 3);
mout[0, 1] = -v.z;
mout[0, 2] = v.y;
mout[1, 0] = v.z;
mout[1, 2] = -v.x;
mout[2, 0] = -v.y;
mout[2, 1] = v.x;
return(mout);
}
/// <summary>
/// Warp a matrix M as R * M * R^T
/// </summary>
public static MatrixXD WarpMatrix(Quaternion R, MatrixXD M)
{
MatrixXD mout = new DenseMatrixXD(3, 3);
mout.SetRow(0, ToVectorXD(R * ToVector3(M.Row(0))));
mout.SetRow(1, ToVectorXD(R * ToVector3(M.Row(1))));
mout.SetRow(2, ToVectorXD(R * ToVector3(M.Row(2))));
mout.SetColumn(0, ToVectorXD(R * ToVector3(mout.Column(0))));
mout.SetColumn(1, ToVectorXD(R * ToVector3(mout.Column(1))));
mout.SetColumn(2, ToVectorXD(R * ToVector3(mout.Column(2))));
return(mout);
}
public void GetForce(VectorXD force)
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
/////////////////////////////////////////////////////
///Calculo de C
float cx;
float cy;
float cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
VectorXD c = Utils.ToVectorXD(new Vector3(cx, cy, cz)); //|xa - xb|
/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
///Calculo de GradC y F
MatrixXD identity = DenseMatrixXD.CreateIdentity(3);
MatrixXD dcdax = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdaTheta = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdbx = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdbTheta = MatrixXD.Build.Dense(3, 3);
if (bodyA != null)
{
dcdax = identity;
dcdaTheta = -Utils.Skew(posA - bodyA.m_pos);
VectorXD fax = -Stiffness *dcdax.Transpose() * c;
VectorXD faTheta = -Stiffness *dcdaTheta.Transpose() * c;
force.SetSubVector(bodyA.index, 3, force.SubVector(bodyA.index, 3) + fax);
force.SetSubVector(bodyA.index + 3, 3, force.SubVector(bodyA.index + 3, 3) + faTheta);
}
if (bodyB != null)
{
dcdbx = -identity;
dcdbTheta = Utils.Skew(posB - bodyB.m_pos);
VectorXD fbx = -Stiffness *dcdbx.Transpose() * c;
VectorXD fbTheta = -Stiffness *dcdbTheta.Transpose() * c;
force.SetSubVector(bodyB.index, 3, force.SubVector(bodyB.index, 3) + fbx);
force.SetSubVector(bodyB.index + 3, 3, force.SubVector(bodyB.index + 3, 3) + fbTheta);
}
/////////////////////////////////////////////////////
}
public void GetForceJacobian(MatrixXD dFdx, MatrixXD dFdv)
{
// TO BE COMPLETED
//Derivative of damping on the linear force
dFdv.SetSubMatrix(index, index, dFdv.SubMatrix(index, 3, index, 3)
- Damping * Mass * DenseMatrixXD.CreateIdentity(3));
//Derivative of damping on the torque and of the quadratic velocity vector
// T = skew(M * w) * w = - skew(w) * (M * w)
// dTdw = skew(M * w) - skew(w) * M
dFdv.SetSubMatrix(index + 3, index + 3, dFdv.SubMatrix(index + 3, 3, index + 3, 3) - Damping * m_inertia
+ Utils.Skew(Utils.ToVector3(m_inertia * Utils.ToVectorXD(m_omega))) - Utils.Skew(m_omega) * m_inertia);
}
/// <summary>
/// Performs a simulation step using Symplectic integration with constrained dynamics.
/// The constraints are treated as implicit
/// </summary>
private void stepSymplecticConstraints()
{
// TO BE COMPLETED
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
MatrixXD J = new DenseMatrixXD(m_numConstraints, m_numDoFs);
MatrixXD A = new DenseMatrixXD(m_numDoFs);
VectorXD B = new DenseVectorXD(m_numDoFs);
VectorXD c = new DenseVectorXD(m_numConstraints);
VectorXD b = new DenseVectorXD(m_numDoFs);
VectorXD lamda = new DenseVectorXD(m_numConstraints);
MatrixXD I = DenseMatrixXD.CreateIdentity(m_numDoFs);
f.Clear();
Minv.Clear();
J.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetVelocity(v);
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (IConstraint constraint in m_constraints)
{
constraint.GetForce(f);
constraint.GetConstraints(c);
constraint.GetConstraintJacobian(J);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
b = v + TimeStep * Minv * f;
A = J * I * J.Transpose();
B = J * I * b + (1.0f / TimeStep) * c;
lamda = A.Solve(B);
v = I * (b - J.Transpose() * lamda);
VectorXD x = TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.AdvanceIncrementalPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Makes the thresholded image representing the data, using setting like
/// double thresholdingValue
/// bool thresholdingUsageTop
/// bool thresholdingUsageBtm
///
/// DEPRECATED - legacy only
/// </summary>
public void makeThresholdedDataImage()
{
if (currentColorScheme == null)
{
return;
}
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix tmpDM = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)(dmSourceData.Clone());
ImageProcessing.maskedImageChannelDenseMatrixThresholdedInplace(tmpDM, maskImageBinary, (double)thresholdingValue, 0.0d, thresholdingUsageTop, thresholdingUsageBtm);
thresholdedImage = ImageProcessing.grayscaleImageFromDenseMatrix(tmpDM);
ServiceTools.FlushMemory(null, null);
}
// Compute the velocity and angular velocity derivatives for a body, if exists
private void getConstraintJacobianForBody(RigidBody body, Vector3 localPoint, MatrixXD J, float sign)
{
if (body)
{
int vi = body.getSimIndex();
int wi = vi + 3;
MatrixXD Jv = DenseMatrixXD.CreateIdentity(3);
MatrixXD Jw = Utils.Skew(-body.VecLocalToGlobal(localPoint));
J.SetSubMatrix(m_index, 3, vi, 3, (Jv * sign));
J.SetSubMatrix(m_index, 3, wi, 3, (Jw * sign));
}
}
/// <summary>
/// Performs a simulation step using Verlet integration.
/// </summary>
private void stepVerlet()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
VectorXD x0 = x;
//Compute forces at t0
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
if (first)
{
x = x + TimeStep * v + 0.5f * TimeStep * TimeStep * (Minv * f);
first = false;
}
else
{
x = 2 * x - xOld + TimeStep * TimeStep * (Minv * f);
}
v = (1.0f / TimeStep) * (x - x0);
xOld = x0;
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Implicit integration.
/// </summary>
private void stepImplicit()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD M = new DenseMatrixXD(m_numDoFs);
MatrixXD Jk = new DenseMatrixXD(m_numDoFs);
MatrixXD Jd = new DenseMatrixXD(m_numDoFs);
MatrixXD A = new DenseMatrixXD(m_numDoFs);
VectorXD B = new DenseVectorXD(m_numDoFs);
M.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMass(M);
obj.GetForceJacobian(Jk, Jd);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(M);
}
A = M + TimeStep * Jd + TimeStep * TimeStep * -Jk;
B = (M + TimeStep * Jd) * v + TimeStep * f;
v = A.Solve(B);
x += TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.FixVector(v);
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
public void GetForce(VectorXD force)
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
float cx, cy, cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
MatrixXD dcdPosa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdPosb = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQb = MatrixXD.Build.Dense(3, 3);
MatrixXD I = DenseMatrixXD.CreateIdentity(3);
if (bodyA != null)
{
dcdPosa = I;
dcdQa = -Utils.Skew(posA - bodyA.m_pos);
VectorXD faPos = -Stiffness *dcdPosa.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
VectorXD faQ = -Stiffness *dcdQa.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
force.SetSubVector(bodyA.index, 3, force.SubVector(bodyA.index, 3) + faPos);
force.SetSubVector(bodyA.index + 3, 3, force.SubVector(bodyA.index + 3, 3) + faQ);
}
if (bodyB != null)
{
dcdPosb = -I;
dcdQb = Utils.Skew(posB - bodyB.m_pos);
VectorXD fbPos = -Stiffness *dcdPosb.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
VectorXD fbQ = -Stiffness *dcdQb.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
force.SetSubVector(bodyB.index, 3, force.SubVector(bodyB.index, 3) + fbPos);
force.SetSubVector(bodyB.index + 3, 3, force.SubVector(bodyB.index + 3, 3) + fbQ);
}
}
public void FFT2dInverse()
{
dmResultComplex = new MathNet.Numerics.LinearAlgebra.Complex.DenseMatrix(dmComplexInput.RowCount,
dmComplexInput.ColumnCount, new Complex[] { new Complex(0.0d, 0.0d) });
IEnumerable <Tuple <int, Vector <Complex> > > columnEnumerator =
dmComplexInput.EnumerateColumnsIndexed();
foreach (Tuple <int, Vector <Complex> > theColumnTuple in columnEnumerator)
{
Vector <Complex> theVector = theColumnTuple.Item2;
Complex[] theVectorArray = theVector.ToArray();
Fourier.Inverse(theVectorArray);
Vector <Complex> theVectorSpectrum = new MathNet.Numerics.LinearAlgebra.Complex.DenseVector(theVectorArray);
dmResultComplex.SetColumn(theColumnTuple.Item1, theVectorSpectrum);
}
IEnumerable <Tuple <int, Vector <Complex> > > rowEnumerator =
dmResultComplex.EnumerateRowsIndexed();
foreach (Tuple <int, Vector <Complex> > theRowTuple in rowEnumerator)
{
Vector <Complex> theVector = theRowTuple.Item2;
Complex[] theVectorArray = theVector.ToArray();
Fourier.Inverse(theVectorArray);
Vector <Complex> theVectorSpectrum = new MathNet.Numerics.LinearAlgebra.Complex.DenseVector(theVectorArray);
dmResultComplex.SetRow(theRowTuple.Item1, theVectorSpectrum);
}
dmOutputReal = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(dmResultComplex.RowCount, dmResultComplex.ColumnCount, new Func <int, int, double>(
(row, column) =>
{
return(dmResultComplex[row, column].Real);
}));
dmOutputImaginary = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(dmResultComplex.RowCount, dmResultComplex.ColumnCount, new Func <int, int, double>(
(row, column) =>
{
return(dmResultComplex[row, column].Imaginary);
}));
}
/// <summary>
/// Adds the elastic Jacobian corresponding to the string
/// to the specified global Jacobian matrix at the index
/// associated with edge nodes. Note |mJSum| = (nDOF,nDOF).
/// </summary>
public void addJacobian(MatrixXD mJsum)
{
Node a = Edge.Node1;
Node b = Edge.Node0;
int index1 = a.SimIdx * 3;
int index0 = b.SimIdx * 3;
Vector3 LR3 = a.Xt - b.Xt;
float L = LR3.magnitude;
float kLL0 = Ke * (L - L0);
Vector3 U = LR3.normalized;
MatrixXD I = DenseMatrixXD.CreateIdentity(3);
MatrixXD uut = DenseMatrixXD.Create(3, 3, 0.0);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
uut [i, j] = U [i] * U [j];
}
}
MatrixXD dUdxa = 1.0f / L * (I - uut);
MatrixXD dFadxa = kLL0 * dUdxa + Ke * uut;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
mJsum [index1 + i, index1 + j] += dFadxa[i, j];
mJsum [index0 + i, index0 + j] += dFadxa[i, j];
mJsum [index1 + i, index0 + j] += -dFadxa[i, j];
mJsum [index0 + i, index1 + j] += -dFadxa[i, j];
}
}
}
public void GetConstraintJacobian(MatrixXD dcdx) //Gradiente de C
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
MatrixXD dcdPosa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdPosb = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQb = MatrixXD.Build.Dense(3, 3);
MatrixXD I = DenseMatrixXD.CreateIdentity(3);
float cx, cy, cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
if (bodyA != null)
{
dcdPosa = I;
dcdQa = -Utils.Skew(posA - bodyA.m_pos);
for (int i = 0; i < 3; i++)
{
dcdx.SetSubMatrix(index, bodyA.index, dcdx.SubMatrix(index, 3, bodyA.index, 3) + dcdPosa);
dcdx.SetSubMatrix(index, bodyA.index + 3, dcdx.SubMatrix(index, 3, bodyA.index + 3, 3) + dcdQa);
}
}
if (bodyB != null)
{
dcdPosb = -I;
dcdQb = Utils.Skew(posB - bodyB.m_pos);
for (int i = 0; i < 3; i++)
{
dcdx.SetSubMatrix(index, bodyB.index, dcdx.SubMatrix(index, 3, bodyB.index, 3) + dcdPosb);
dcdx.SetSubMatrix(index, bodyB.index + 3, dcdx.SubMatrix(index, 3, bodyB.index + 3, 3) + dcdQb);
}
}
//Esto es gracias a mi explicación super a sucio que voy a pasar a limpio a que sí Nerea? :D
}
public void Initialize(int ind, PhysicsManager m)
{
Manager = m;
index = ind;
// Initialize inertia. We assume that the object is connected to a Cube mesh.
Transform xform = GetComponent <Transform>();
if (xform == null)
{
System.Console.WriteLine("[ERROR] Couldn't find any transform to the rigid body");
}
else
{
System.Console.WriteLine("[TRACE] Succesfully found transform connected to the rigid body");
}
if (xform != null)
{
m_inertia0 = DenseMatrixXD.CreateIdentity(3);
double[] vals;
vals = new double[3];
vals[0] = 1.0f / 12.0f * Mass * (xform.localScale.y * xform.localScale.y + xform.localScale.z * xform.localScale.z);
vals[1] = 1.0f / 12.0f * Mass * (xform.localScale.x * xform.localScale.x + xform.localScale.z * xform.localScale.z);
vals[2] = 1.0f / 12.0f * Mass * (xform.localScale.x * xform.localScale.x + xform.localScale.y * xform.localScale.y);
m_inertia0.SetDiagonal(vals);
}
// Initialize kinematics
m_pos = xform.position;
m_rot = xform.rotation;
m_vel = Vector3.zero;
m_omega = Vector3.zero;
// Initialize all inertia terms
m_inertia0inv = m_inertia0.Inverse();
m_inertia = Utils.WarpMatrix(m_rot, m_inertia0);
m_inertiainv = Utils.WarpMatrix(m_rot, m_inertia0inv);
}
public void getConstraintJacobian(MatrixXD J)
{
// TO BE COMPLETED: WRITE CONSTRAINT JACOBIANS TO J
// Apuntes del día 5 :
// La jacobiana es un conjunto de V y W formada como
// V,W,V,W... siendo (V,W) un rigidbody
// Wa y Wb son matrices 3X3 formadas por (-Ra * ra) * <- Skewed
// Va y Vb son matrices identidad 3x3
// Metemos (si hay cuerpo) -> Identidad y después Wa/Wb
// un truco es meter la identidad en B como negativa y así ya es opuesto a Va
// Va = -Vb
MatrixXD IdentityA;
MatrixXD IdentityB;
MatrixXD Wa, Wb;
if (BodyA != null)
{
Wa = Utils.Skew(-(BodyA.Rotation * m_pA));
IdentityA = DenseMatrixXD.CreateIdentity(3);
J.SetSubMatrix(m_index, BodyA.getSimIndex(), IdentityA);
J.SetSubMatrix(m_index, BodyA.getSimIndex() + 3, Wa);
}
if (BodyB != null)
{
Wb = Utils.Skew((BodyB.Rotation * m_pB));
IdentityB = -DenseMatrixXD.CreateIdentity(3);
J.SetSubMatrix(m_index, BodyB.getSimIndex(), IdentityB);
J.SetSubMatrix(m_index, BodyB.getSimIndex() + 3, Wb);
}
}
// Nothing to do here
#endregion
#region ISimulable
public void initialize(int index, PhysicsManager manager)
{
m_manager = manager;
// Initialize indices
m_index = index;
// Initialize inertia. We assume that the object is connected to a Cube mesh.
Transform xform = this.GetComponent <Transform>();
if (xform == null)
{
System.Console.WriteLine("[ERROR] Couldn't find any transform connected to the rigid body");
}
else
{
System.Console.WriteLine("[TRACE] Succesfully found transform connected to the rigid body");
}
if (xform != null)
{
this.m_inertia0 = DenseMatrixXD.CreateIdentity(3);
double[] vals;
vals = new double[3];
vals[0] = 1.0f / 12.0f * mass * (xform.localScale.y * xform.localScale.y + xform.localScale.z * xform.localScale.z);
vals[1] = 1.0f / 12.0f * mass * (xform.localScale.x * xform.localScale.x + xform.localScale.z * xform.localScale.z);
vals[2] = 1.0f / 12.0f * mass * (xform.localScale.x * xform.localScale.x + xform.localScale.y * xform.localScale.y);
this.m_inertia0.SetDiagonal(vals);
this.m_inertia = m_inertia0;
}
// Initialize kinematics
this.m_pos = xform.position;
this.m_rot = xform.rotation;
this.m_vel = Vector3.zero;
this.m_omega = Vector3.zero;
}
/// <summary>
/// Performs a simulation step using Symplectic integration.
/// </summary>
private void stepSymplectic()
{
// TO BE COMPLETED
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
f.Clear();
Minv.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetVelocity(v);
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (IConstraint constraint in m_constraints)
{
constraint.GetForce(f);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
v += TimeStep * (Minv * f);
VectorXD x = TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.AdvanceIncrementalPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="imageConditionAndData"/> class.
/// note: with the fixed ruler bounds
/// </summary>
/// <param name="imgPr">The image processing object contains image data and some processing templates.</param>
public imageConditionAndData(ImageProcessing imgPr)
{
maskImageBinary = imgPr.significantMaskImageBinary.Copy();
isImageGrayscaled = false;
sourceImageBgr = imgPr.tmpImage.Copy();
sourceImageGrayscale = null;
thresholdingUsageTop = false;
thresholdingUsageBtm = true;
thresholdingValue = 255;
dmSourceData = ImageProcessing.DenseMatrixFromImage(sourceImageBgr.Convert <Gray, Byte>());
currentColorScheme = new ColorScheme("");
//currentColorSchemeRuler = new ColorSchemeRuler(currentColorScheme, dataMinValue(), dataMaxValue());
currentColorSchemeRuler = new ColorSchemeRuler(this, -255.0d, 255.0d);
selection = null;
//makeThresholdedDataImage();
dataHasBeenModified = true;
HighlightMask = null;
highlightedArea = 0.0d;
}
public void getMassMatrix(MatrixXD mmout)
{
mmout.SetSubMatrix(m_index, m_index, this.mass * DenseMatrixXD.CreateIdentity(3));
mmout.SetSubMatrix(m_index + 3, m_index + 3, this.m_inertia);
}
/// <summary>
/// v1をなるべくv2に近づけるような回転行列を求める。戻り値は、残差の二乗和を個数で割ったもの
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="result"></param>
/// <returns></returns>
public static double GetRotationMatrix(Vector3DBase[] v1, Vector3DBase[] v2, ref Matrix3D result, Matrix3D initialRotation = null)
{
if (initialRotation == null)
{
//初期回転行列を求める
initialRotation = new Matrix3D();
}
var euler = Euler.GetEulerAngle(initialRotation);
int length = v1.Length;
double phi = euler.Phi, theta = euler.Theta, psi = euler.Psi;
double phi_New, theta_New, psi_New;
Matrix3D rot = initialRotation, rot_New;
Vector3DBase[,] diff = new Vector3DBase[3, length];
var Alpha = new DMat(3, 3);
var Beta = new DMat(3, 1);
Vector3DBase[] ResidualCurrent = new Vector3DBase[length];
Vector3DBase[] ResidualNew = new Vector3DBase[length];
double ResidualSquareCurrent, ResidualSquareNew = 0;
int count = 0;
//現在の残差を計算
ResidualSquareCurrent = 0;
for (int i = 0; i < length; i++)
{
ResidualCurrent[i] = rot * v1[i] - v2[i];
ResidualSquareCurrent += ResidualCurrent[i] * ResidualCurrent[i];
}
double lambda = 1;
while (lambda < 1000000000 && count < 3000)//lambdaが大きくなりすぎた時か、試行回数が一定以上になった時、止める
{
count++;
double cosPhi = Math.Cos(phi), sinPhi = Math.Sin(phi), cosTheta = Math.Cos(theta), sinTheta = Math.Sin(theta), cosPsi = Math.Cos(psi), sinPsi = Math.Sin(psi);
for (int i = 0; i < length; i++)//偏微分を作る 「回転行列の偏微分」というMathematicaファイルを参照
{
//∂F/∂phi
diff[0, i] = new Vector3DBase(
v1[i].X * (-cosPsi * sinPhi - cosPhi * cosTheta * sinPsi) + v1[i].Y * (cosPhi * cosPsi * cosTheta - sinPhi * sinPsi) + v1[i].Z * cosPhi * sinTheta,
v1[i].Y * (-cosPsi * cosTheta * sinPhi - cosPhi * sinPsi) + v1[i].X * (-cosPhi * cosPsi + cosTheta * sinPhi * sinPsi) - v1[i].Z * sinPhi * sinTheta,
0
);
//∂F/∂theta
diff[1, i] = new Vector3DBase(
v1[i].Z * cosTheta * sinPhi - v1[i].Y * cosPsi * sinPhi * sinTheta + v1[i].X * sinPhi * sinPsi * sinTheta,
v1[i].Z * cosPhi * cosTheta - v1[i].Y * cosPhi * cosPsi * sinTheta + v1[i].X * cosPhi * sinPsi * sinTheta,
-v1[i].Y * cosPsi * cosTheta + v1[i].X * cosTheta * sinPsi - v1[i].Z * sinTheta
);
//∂F/∂psi
diff[2, i] = new Vector3DBase(
v1[i].X * (-cosPsi * cosTheta * sinPhi - cosPhi * sinPsi) + v1[i].Y * (cosPhi * cosPsi - cosTheta * sinPhi * sinPsi),
v1[i].Y * (-cosPsi * sinPhi - cosPhi * cosTheta * sinPsi) + v1[i].X * (-cosPhi * cosPsi * cosTheta + sinPhi * sinPsi),
v1[i].X * cosPsi * sinTheta + v1[i].Y * sinPsi * sinTheta
);
}
//行列Alpha, Betaを作る
for (int k = 0; k < 3; k++)
{
for (int l = 0; l < 3; l++)
{
Alpha[k, l] = 0;
for (int i = 0; i < length; i++)
{
Alpha[k, l] += diff[k, i] * diff[l, i];
}
if (k == l)
{
Alpha[k, l] *= (1 + lambda);
}
}
Beta[k, 0] = 0;
for (int i = 0; i < length; i++)
{
Beta[k, 0] += ResidualCurrent[i] * diff[k, i];
}
}
if (!Alpha.TryInverse(out Matrix alphaInv))
{
return(-1);
}
var delta = alphaInv * Beta;
phi_New = phi - delta[0, 0];
theta_New = theta - delta[1, 0];
psi_New = psi + delta[2, 0];
//あたらしいパラメータでの残差の二乗和を計算
ResidualSquareNew = 0;
rot_New = Euler.SetEulerAngle(phi_New, theta_New, psi_New);
for (int i = 0; i < length; i++)
{
ResidualNew[i] = rot_New * v1[i] - v2[i];
ResidualSquareNew += ResidualNew[i] * ResidualNew[i];
}
if (ResidualSquareCurrent > ResidualSquareNew) //新旧の残差の二乗和を比較
{ //改善したとき
if ((ResidualSquareCurrent - ResidualSquareNew) / ResidualSquareCurrent > 0.0000000001 || count < 15 || lambda > 1)
{ //改善率が0.0000000001 以上 (まだまだ改善の余地がある)、あるいはcountが15以下 (回数が少ない)、あるいはlambdaが1より大きいとき (まだまだ改善の余地がある)
ResidualSquareCurrent = ResidualSquareNew; //残差の二乗和を書き換える
lambda *= 0.4; //lambdaを小さくする
for (int i = 0; i < length; i++)
{
ResidualCurrent[i] = ResidualNew[i]; //残差行列を書き換える
}
phi = phi_New; theta = theta_New; psi = psi_New; //新旧パラメータを書き換える
}
else
{
break;
}
}
else//改善しなかったとき
{
lambda *= 2.5;//lambdaを大きくする
}
}
return(ResidualSquareCurrent);
}
/// <summary>
/// Performs a simulation step using Midpoint integration.
/// </summary>
private void stepMidpoint()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
VectorXD x0 = x;
VectorXD v0 = v;
//Compute forces at t0
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
//Integrate to h/2
x += 0.5f * TimeStep * v;
v += 0.5f * TimeStep * (Minv * f);
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
//Compute forces at h/2
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
//Integrate to h
x = x0 + TimeStep * v;
v = v0 + TimeStep * (Minv * f);
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
