public List<Vector<double>> AddNoise(List<Vector<double>> points,
double variance, int seed = 0)
{
List<Vector<double>> noisedPoints = new List<Vector<double>>(points.Count);
int pointSize = points[0].Count;
GaussianNoiseGenerator noise = new GaussianNoiseGenerator();
noise.Variance = variance;
noise.Mean = 0.0;
noise.RandomSeed = seed != 0;
noise.Seed = seed;
noise.UpdateDistribution();
for(int i = 0; i < _pointsCount; ++i)
{
Vector<double> cpoint = new DenseVector(pointSize);
for(int p = 0; p < pointSize - 1; ++p)
{
cpoint[p] = points[i][p] + noise.GetSample();
}
cpoint[pointSize - 1] = 1.0f;
noisedPoints.Add(cpoint);
}
return noisedPoints;
}
public Point_Obj get_gaze_pt(Point_Obj right_eye, Point_Obj left_eye, float alpha, float beta, float gamma)
{
// Compute Z coordinate of head/eyes in the WCS (World Coordinate System)
float Z = (-f * EYE_DIST) / (left_eye.get_x() - right_eye.get_x());
//Console.WriteLine("EST Z: {0}", Z);
// Use computed Z coordinate to get X,Y world coordinates of the eyes
float X_r = Z * (right_eye.get_x() - princ_pt.get_x()) / (-f);
float Y_r = Z * (right_eye.get_y() - princ_pt.get_y()) / (-f);
float X_l = Z * (left_eye.get_x() - princ_pt.get_x()) / (-f);
float Y_l = Z * (left_eye.get_y() - princ_pt.get_y()) / (-f);
// Compute direction vector (d) of eyes using rotation angles (alpha, beta, gamma)
DenseVector k_hat = new DenseVector(3);
k_hat[2] = 1;
//DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
DenseVector d = R * k_hat;
// Get point of intersection using eye points and direction vector d
DenseVector P_r = new DenseVector(new[]{Convert.ToDouble(X_r), Convert.ToDouble(Y_r), Convert.ToDouble(Z)});
DenseVector P_l = new DenseVector(new[] { Convert.ToDouble(X_l), Convert.ToDouble(Y_l), Convert.ToDouble(Z)});
DenseVector p_hat_r = P_r + d * (-P_r[2] / d[2]);
DenseVector p_hat_l = P_l + d * (-P_l[2] / d[2]);
DenseVector p_hat_avg = (p_hat_r + p_hat_l) / 2;
return new Point_Obj(Convert.ToSingle(p_hat_avg[0]), Convert.ToSingle(p_hat_avg[1]));
}
/// <summary>
/// Run example
/// </summary>
public void Run()
{
// 1. Initialize a new instance of the empty vector with a given size
var vector1 = new DenseVector(5);
// 2. Initialize a new instance of the vector with a given size and each element set to the given value
var vector2 = new DenseVector(5, 3.0);
// 3. Initialize a new instance of the vector from an array.
var vector3 = new DenseVector(new[] { 1.0, 2.0, 3.0, 4.0, 5.0 });
// 4. Initialize a new instance of the vector by copying the values from another.
var vector4 = new DenseVector(vector3);
// Format vector output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
Console.WriteLine(@"Vector 1");
Console.WriteLine(vector1.ToString("#0.00\t", formatProvider));
Console.WriteLine();
Console.WriteLine(@"Vector 2");
Console.WriteLine(vector2.ToString("#0.00\t", formatProvider));
Console.WriteLine();
Console.WriteLine(@"Vector 3");
Console.WriteLine(vector3.ToString("#0.00\t", formatProvider));
Console.WriteLine();
Console.WriteLine(@"Vector 4");
Console.WriteLine(vector4.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
public void ICPStep()
{
double[] s = { 1, 1, 1, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 }; //scale
double[] p = { 0, 0, 0, 0, 0, 0, 100, 100, 100 }; //parameters
//jam the values into vectors so that we can multiply them and shiz
DenseVector scale = new DenseVector(s);
DenseVector parameters = new DenseVector(p);
//distance error in final iteration
double fval_old = double.MaxValue;
//change in distance error between two iterations
double fval_percep = 0;
//todo: some array to contain the transformed points
//number of iterations
int itt = 0;
//get the max and min points of the static points
double maxP = this.maxP();
double minP = this.minP();
double tolX = (maxP - minP) / 1000;
}
public virtual void Tan(double value, ref double returnValue)
{
Double.Vector input = new Double.DenseVector(new double[] { value });
Double.Vector output = new Double.DenseVector(new double[] { returnValue });
output = (Double.DenseVector)Double.Vector.Tan(input);
returnValue = output[0];
}
public static DenseVector Normalize0to1(this DenseVector data)
{
var d = new DenseVector(data);
var result = new DenseVector(d.Count);
d.CopyTo(result);
return (DenseVector) (result - d.Min())/(d.Max() - d.Min());
}
public virtual void Tan(double[] value, ref double[] returnValue)
{
Double.Vector input = new Double.DenseVector(value);
Double.Vector output = new Double.DenseVector(returnValue);
output = (Double.DenseVector)Double.Vector.Tan(input);
returnValue = output.ToArray();
}
private Plane NormalPlane(DenseMatrix pointsMat)
{
int num = pointsMat.RowCount;
var centroid = pointsMat.ColumnSums() / num;
for (int i = 0; i < num; i++)
{
pointsMat[i, 0] -= centroid[0];
pointsMat[i, 1] -= centroid[1];
pointsMat[i, 2] -= centroid[2];
}
var svd = pointsMat.Transpose().Svd(true);
var singularValues = svd.S;
var normalVec = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(3);
svd.U.Column(2, normalVec);
Vector3 normalVec3 = new Vector3((float)normalVec[0], (float)normalVec[1], (float)normalVec[2]);
Vector3 centroid3 = new Vector3((float)centroid[0], (float)centroid[1], (float)centroid[2]);
Common.Plane p = new Common.Plane(normalVec3, centroid3);
return(p);
}
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];
}
}
public void HeapSortWithDecreasingDoubleArray()
{
var sortedIndices = new int[10];
var values = new DenseVector(10);
values[0] = 9;
values[1] = 8;
values[2] = 7;
values[3] = 6;
values[4] = 5;
values[5] = 4;
values[6] = 3;
values[7] = 2;
values[8] = 1;
values[9] = 0;
for (var i = 0; i < sortedIndices.Length; i++)
{
sortedIndices[i] = i;
}
ILUTPElementSorter.SortDoubleIndicesDecreasing(0, sortedIndices.Length - 1, sortedIndices, values);
for (var i = 0; i < sortedIndices.Length; i++)
{
Assert.AreEqual(i, sortedIndices[i], "#01-" + i);
}
}
/// <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);
}
}
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 Vector<double> refsignal(Timer0Freq Timer0, Timer1Freq Timer1, Timer3Freq Timer3, string code, double Fs)
{
double f0 = Convert.ToDouble(Timer0);
double f1 = Convert.ToDouble(Timer1);
double f3 = Convert.ToDouble(Timer3);
double T = 1 / Fs;
int samplesperbit = Convert.ToInt32(Math.Round(Fs / f1));
int samples = (int)Math.Ceiling(Fs / f3);
Vector<double> signal = new DenseVector(samples);
string bincodestring = Convert.ToString(Convert.ToInt64(code, 16), 2);// Convert.ToString(code, 2); //string.Join("",code.Reverse().Select(x => Convert.ToString(x, 2).PadLeft(8, '0')));
int bitnumber = 0;
for (int ind = 0; ind < bincodestring.Length; ind++)
{
char s = bincodestring[ind];
bitnumber++;
if (s == '1')
{
int indexfrom = (bitnumber - 1) * samplesperbit + 1;
int indexto = bitnumber * samplesperbit;
for (int sample = indexfrom; sample <= indexto; sample++)
{
signal[sample] = 1.0f;
}
}
}
for (int i = 0; i < samples; i++)
{
double carrier = Math.Round(Math.Cos(2 * Math.PI * f0 / Fs * i) / 2 + 0.5);
signal[i] *= carrier;
}
return signal;
}
/// <summary>
/// Missing mapping to P objects
/// KdTree could be refactored to use P object instead of Math.Net
///
/// O(k * log n)
/// </summary>
/// <param name="s"></param>
/// <param name="origin"></param>
/// <param name="k"></param>
/// <param name="conf"></param>
/// <returns></returns>
public long UpdateKnn(IAlgorithm s, IP origin, KnnConfiguration conf)
{
if (conf == null) conf = new KnnConfiguration();
if (conf.SameTypeOnly) throw new NotImplementedException();
if (conf.MaxDistance.HasValue) throw new NotImplementedException();
var sw = new Stopwatch();
sw.Start();
var vector = new DenseVector(new[] { origin.X, origin.Y });
var nn = Tree.FindNearestNNeighbors(vector, conf.K).ToList();
s.Knn.Clear();
s.Knn.Origin = origin;
s.Knn.K = conf.K;
foreach (var i in nn)
{
var p = new P { X = i[0], Y = i[1] };
var dist = origin.Distance(p.X,p.Y);
s.Knn.NNs.Add(new PDist {Point = p, Distance = dist});
}
sw.Stop();
return sw.ElapsedMilliseconds;
}
public double[] transform(double[] displacement, bool isreverse)
{
DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
double[] dispcopy = new double[6];
displacement.CopyTo(dispcopy, 0);
DenseMatrix2String m2s = new DenseMatrix2String();
List2String l2s = new List2String();
DenseVector dispV = new DenseVector(dispcopy);
log.Debug("original disp: " + l2s.ToString(displacement));
if (isreverse)
{
dispV = (DenseVector)xform.Multiply(dispV);
log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
}
DenseVector newDisp = translate(dispV.Values, isreverse);
log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
DenseMatrix rollM = roll.create(dispV.Values[3]);
DenseMatrix pitchM = pitch.create(dispV.Values[4]);
DenseMatrix yawM = yaw.create(dispV.Values[5]);
DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
log.Debug("rotation: " + m2s.ToString(rotation));
DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
log.Debug("unt: " + l2s.ToString(unt.Values));
DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
dispV.SetSubVector(0, 3, newDisp1);
if (isreverse == false)
{
dispV = (DenseVector)xform.Multiply(dispV);
}
log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
return dispV.Values;
}
/// <summary>
/// Передаются ссылки, point и delta будут изменены
/// </summary>
/// <param name="delta"></param>
/// <param name="point"></param>
/// <returns></returns>
private Vector<double> ExplanotorySearch( Vector<double> point, Vector<double> delta)
{
var nvars = delta.Count;
var prevbest = Fh.Y(point);
var z = new DenseVector(nvars);
point.CopyTo(z);
int i;
var minf = prevbest;
for (i = 0; i < nvars; i++)
{
z[i] = point[i] + delta[i];
var ftmp = Fh.Y(z);
if (ftmp < minf)
minf = ftmp;
else
{
z[i] = point[i] -delta[i];
ftmp = Fh.Y(z);
if (ftmp < minf)
minf = ftmp;
else
{
z[i] = point[i];
}
}
}
return z;
}
/// <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();
}
private ExitCondition ExitCriteriaSatisfied(IEvaluation candidate_point, IEvaluation last_point, Vector <double> lower_bound, Vector <double> upper_bound, int iterations)
{
Vector <double> rel_grad = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(candidate_point.Point.Count);
double relative_gradient = 0.0;
double normalizer = Math.Max(Math.Abs(candidate_point.Value), 1.0);
for (int ii = 0; ii < rel_grad.Count; ++ii)
{
var projected_gradient = 0.0;
bool at_lower_bound = candidate_point.Point[ii] - lower_bound[ii] < VERY_SMALL;
bool at_upper_bound = upper_bound[ii] - candidate_point.Point[ii] < VERY_SMALL;
if (at_lower_bound && at_upper_bound)
{
projected_gradient = 0.0;
}
else if (at_lower_bound)
{
projected_gradient = Math.Min(candidate_point.Gradient[ii], 0.0);
}
else if (at_upper_bound)
{
projected_gradient = Math.Max(candidate_point.Gradient[ii], 0.0);
}
else
{
projected_gradient = candidate_point.Gradient[ii];
}
double tmp = projected_gradient * Math.Max(Math.Abs(candidate_point.Point[ii]), 1.0) / normalizer;
relative_gradient = Math.Max(relative_gradient, Math.Abs(tmp));
}
if (relative_gradient < this.GradientTolerance)
{
return(ExitCondition.RelativeGradient);
}
if (last_point != null)
{
double most_progress = 0.0;
for (int ii = 0; ii < candidate_point.Point.Count; ++ii)
{
var tmp = Math.Abs(candidate_point.Point[ii] - last_point.Point[ii]) / Math.Max(Math.Abs(last_point.Point[ii]), 1.0);
most_progress = Math.Max(most_progress, tmp);
}
if (most_progress < this.ParameterTolerance)
{
return(ExitCondition.LackOfProgress);
}
double function_change = candidate_point.Value - last_point.Value;
if (iterations > 500 && function_change < 0 && Math.Abs(function_change) < this.FunctionProgressTolerance)
{
return(ExitCondition.LackOfProgress);
}
}
return(ExitCondition.None);
}
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;
}
private static void SetRealVertices(Workspace workspace)
{
Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());
if (fittedPlaneVector == null)
{
return;
}
Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);
Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });
Point3D[] vertices3D = workspace.Vertices3D;
Point[] vertices = workspace.Vertices.ToArray();
for (int i = 0; i < vertices.Length; i++)
{
Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
Vector<double> pointOnLine = new DenseVector(new double[] { vertices3D[i].X, vertices3D[i].Y, vertices3D[i].Z });
double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));
Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);
workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
}
workspace.PlaneVector = fittedPlaneVector;
}
/// <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);
}
}
}
public static void Run()
{
List<Position> list = new List<Position>();
/*
list.Add(new Position());
list.Add(new Position("bond"));
list.Add(new Position("stock"));
* */
Portfolio p = new Portfolio();
double[] returns = {0.000, 0.13, -0.13};
DenseVector returns1 = new DenseVector(returns);
double[] stdev = {0, 7.4, 7.4};
double[,] covariance = {{1, -.4, -.45}, {-.4, 1, .35}, {-0.45, 0.35, 1}};
DenseMatrix covariance1 = StatisticsExtension.CorrelationToCovariance(new DenseMatrix(covariance),
new DenseVector(stdev));
PortfolioOptimizer po = new PortfolioOptimizer(p, .09002, covariance1, returns1);
po.BuildRiskModel();
Console.ReadLine();
}
public static double[] NormalizeZScore(this double[] data)
{
var d = new DenseVector(data);
var result = new DenseVector(d.Count);
d.CopyTo(result);
result = (DenseVector) ((result - d.Mean())/(d.StandardDeviation()));
return result.ToArray();
}
public void SolveWideMatrixThrowsArgumentException()
{
var matrix = new SparseMatrix(2, 3);
var input = new DenseVector(2);
var solver = new TFQMR();
Assert.That(() => matrix.SolveIterative(input, solver), Throws.ArgumentException);
}
private static DenseVector TransposeAndMult(Matrix m, Vector v)
{
var result = new DenseVector(m.ColumnCount);
for (int j = 0; j < m.RowCount; j++)
for (int i = 0; i < m.ColumnCount; i++)
result[i] += m[j, i] * v[j];
return result;
}
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);
}
public static double[] NormalizeNeg1to1(this double[] data)
{
var d = new DenseVector(data);
var result = new DenseVector(d.Count);
d.CopyTo(result);
result = (DenseVector) (result - ((d.Max() + d.Min())/2))/((d.Max() - d.Min())/2);
return result.ToArray();
}
public DenseVector crossProduct(DenseVector vnew)
{
DenseVector result = new DenseVector(3);
result[0] = vorig[1] * vnew[2] - vorig[2] * vnew[1];
result[1] = vorig[2] * vnew[0] - vorig[0] * vnew[2];
result[2] = vorig[0] * vnew[1] - vorig[1] * vnew[0];
return result;
}
public void SolveLongMatrixThrowsArgumentException()
{
var matrix = new SparseMatrix(3, 2);
var input = new DenseVector(3);
var solver = new GpBiCg();
Assert.Throws<ArgumentException>(() => matrix.SolveIterative(input, solver));
}
public void SolveLongMatrixThrowsArgumentException()
{
var matrix = new SparseMatrix(3, 2);
var input = new DenseVector(3);
var solver = new MlkBiCgStab();
Assert.That(() => matrix.SolveIterative(input, solver), Throws.ArgumentException);
}
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 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 void ApproximateWithNonInitializedPreconditionerThrowsArgumentException()
{
const int Size = 10;
var vector = CreateStandardBcVector(Size);
var preconditioner = CreatePreconditioner();
var result = new DenseVector(vector.Count);
Assert.That(() => preconditioner.Approximate(vector, result), Throws.ArgumentException);
}
public RSquared(int n, AbstractIndicator indicator = null) : base(n)
{
X = new MovingQueue<double>(n);
Y = new DenseVector(n);
int counter = 0;
for (double i = (double)-Y.Count / 2 + 0.5; i < (double)Y.Count / 2; i++) Y[counter++] = i;
this.indicator = indicator;
}
public void SolveWideMatrixThrowsArgumentException()
{
var matrix = new SparseMatrix(2, 3);
var input = new DenseVector(2);
var solver = new MlkBiCgStab();
Assert.Throws<ArgumentException>(() => matrix.SolveIterative(input, solver));
}
/// <summary>
/// Convert a Vector3 to VectorXD
/// </summary>
public static VectorXD ToVectorXD(Vector3 vin)
{
VectorXD vout = new DenseVectorXD(3);
vout[0] = vin[0];
vout[1] = vin[1];
vout[2] = vin[2];
return(vout);
}
/// <summary>
/// Convert a Quaternion to VectorXD
/// </summary>
public static VectorXD ToVectorXD(Quaternion vin)
{
VectorXD vout = new DenseVectorXD(4);
vout [0] = vin.x;
vout [1] = vin.y;
vout [2] = vin.z;
vout [3] = vin.w;
return(vout);
}
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();
}
}
public VectorXD getPositionVector()
{
VectorXD vxout = new DenseVectorXD(this.getNumDOF());
for (int i = 0; i < this.getNumNodes(); ++i)
{
vxout [3 * i + 0] = this.m_vnodes [i].Xt [0];
vxout [3 * i + 1] = this.m_vnodes [i].Xt [1];
vxout [3 * i + 2] = this.m_vnodes [i].Xt [2];
}
return(vxout);
}
public VectorXD getVelocityVector()
{
VectorXD vvout = new DenseVectorXD(this.getNumDOF());
for (int i = 0; i < this.getNumNodes(); ++i)
{
vvout [3 * i + 0] = this.m_vnodes [i].Vt [0];
vvout [3 * i + 1] = this.m_vnodes [i].Vt [1];
vvout [3 * i + 2] = this.m_vnodes [i].Vt [2];
}
return(vvout);
}
/// <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);
}
}
public VectorXD getMassVector()
{
VectorXD vmout = new DenseVectorXD(this.getNumDOF());
for (int i = 0; i < this.getNumNodes(); ++i)
{
for (int j = 0; j < 3; ++j) // Set X,Y,Z values
{
vmout [3 * i + j] = this.m_vnodes[i].Mass;
}
}
return(vmout);
}
/// <summary>
/// Cross product for 3D vectors
/// http://stackoverflow.com/a/20015626/158285
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static DLA.DenseVector Cross(this DLA.DenseVector left, DLA.DenseVector right)
{
if ((left.Count != 3 || right.Count != 3))
{
string message = "Vectors must have a length of 3.";
throw new Exception(message);
}
var result = new DLA.DenseVector(3);
result[0] = left[1] * right[2] - left[2] * right[1];
result[1] = -left[0] * right[2] + left[2] * right[0];
result[2] = left[0] * right[1] - left[1] * right[0];
return(result);
}
/// <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);
}
}
/// <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);
}
}
public virtual double Norm(double[] a, double p)
{
Double.DenseVector vector_a = new Double.DenseVector(a);
return(vector_a.Norm(p));
}
/// <summary>
/// Initializes a new instance of the <see cref="DenseVector"/> class by
/// copying the values from another.
/// </summary>
/// <param name="other">
/// The vector to create the new vector from.
/// </param>
public DenseVector(DenseVector other)
: this(other.Count)
{
Buffer.BlockCopy(other.Data, 0, Data, 0, Data.Length * Constants.SizeOfDouble);
}
/// <summary>
/// Clone the given vector.
/// </summary>
public static DenseVector Clone(DenseVector vector)
{
return(DenseVector.OfArray((double[])vector));
}
/// <summary>
/// Compute sum of scaled vectors, target = alpha * x + beta * y + z.
/// </summary>
public static void Add(double alpha, DenseVector x, double beta, DenseVector y, DenseVector z,
DenseVector target)
{
var vx = (double[])x;
var vy = (double[])y;
var vz = (double[])z;
var vt = (double[])target;
int length = vx.Length;
CommonParallel.For(0, length, 4096, (a, b) =>
{
for (int i = a; i < b; i++)
{
vt[i] = alpha * vx[i] + beta * vy[i] + vz[i];
}
});
}
/// <summary>
/// Set all vector elements to zero.
/// </summary>
public static void Clear(DenseVector vector)
{
var x = (double[])vector;
Array.Clear(x, 0, x.Length);
}
/// <summary>
/// Outer product of this and another vector.
/// </summary>
/// <param name="v">The vector to operate on.</param>
/// <returns>
/// Matrix M[i,j] = this[i] * v[j].
/// </returns>
/// <seealso cref="OuterProduct(DenseVector, DenseVector)"/>
public Matrix <double> OuterProduct(DenseVector v)
{
return(OuterProduct(this, v));
}
/// <summary>
/// Converts the string representation of a real dense vector to double-precision dense vector equivalent.
/// A return value indicates whether the conversion succeeded or failed.
/// </summary>
/// <param name="value">
/// A string containing a real vector to convert.
/// </param>
/// <param name="result">
/// The parsed value.
/// </param>
/// <returns>
/// If the conversion succeeds, the result will contain a complex number equivalent to value.
/// Otherwise the result will be <c>null</c>.
/// </returns>
public static bool TryParse(string value, out DenseVector result)
{
return(TryParse(value, null, out result));
}
/// <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);
}
}
private void HardConstraintsStep()
{
// Apuntes miki + apuntes clase
// Step de Fuertes:
// Tenemos que conseguir una matriz enorme formada por
//
// | A J^t |
// | J 0 |
// A es la matriz de masas-> 6 * cada componente como en soft
// J es la matriz jacobiana que rellenamos en Constraint.getConstraintJacobian
// J se forma 3 * numero de constraints, 6* num rigidbodies
// J^t se forma 6* num rigidbodies, 3 * numero de constraints
// Recordemos que cada rigidbody tiene 6 numdof
// C0 = 3 * numero de constraints<- Es similar a C en soft
MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6);
MatrixXD jacobian = new DenseMatrixXD(3 * Constraints.Count, 6 * m_objs.Count);
VectorXD C0 = new DenseVectorXD(3 * m_constraints.Count);
// paso 1: necesitamos settear la jacobiana y C0 para cada restricción
foreach (Constraint c in m_constraints)
{
c.getConstraintVector(C0);
c.getConstraintJacobian(jacobian);
}
// las fuerzas son 1 x 6 * m_objs.Count, igual que las velocidades.
// Parece que se mete el torque y demás. 3 * objetos no funciona
VectorXD total_force = new DenseVectorXD(6 * m_objs.Count);
VectorXD total_velocities = new DenseVectorXD(6 * m_objs.Count);
// PARA CADA RIGIDBODY
// Paso 2: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error
// Paso 3: Establecemos la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs
// Paso 4: Metemos valores en las velocidades anteriores, ¿Por qué? Porque Al final haremos un A * v = b
// como en las prácticas anteriores y b es un vector formado por b y -1 * C0 / TimeStep
// siendo b la fórmula (matriz_de_masas * velocidades) + (TimeStep * fuerzas);
// Paso 5: Añadimos fuerzas y matrices (que las hemos limpiado antes)
// Paso 6: Metemos las nuevas fuerzas (getForceVector) en el vector de fuerzas
foreach (RigidBody obj in m_objs)
{
obj.clearForcesAndMatrices();
obj.getMassMatrix(massMatrix);
obj.getVelocityVector(total_velocities);
obj.addForcesAndMatrices();
obj.getForceVector(total_force);
}
// Creamos la super matriz que hemos dicho con la de masas, jacobianas y ceros
// Al crear una MtrixXD se crea con ceros.
MatrixXD jacobianT = jacobian.Transpose();
MatrixXD ceroMatrix = new DenseMatrixXD(3 * m_constraints.Count, 3 * m_constraints.Count);
DenseMatrixXD megaMatrix = new DenseMatrixXD(jacobian.RowCount + massMatrix.RowCount, jacobianT.ColumnCount + massMatrix.ColumnCount);
// la matriz de masas es 0,0 a (m_objs.Count * 6)
// Una vez acaba esa, va la jacobiana traspuesta
// Debajo de la misma manera va la jacobiana
// y en la esquina abajo derecha va la de ceros
megaMatrix.SetSubMatrix(0, 0, massMatrix);
megaMatrix.SetSubMatrix(0, 0 + massMatrix.ColumnCount, jacobianT);
megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0, jacobian);
megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0 + massMatrix.ColumnCount, ceroMatrix);
// M * v = M * v0 + timestep * fuerzas
// V0 es la velocidad del step anterior
// en A * v = b, b lo dividiremos como b y b2
// y los concatenamos
VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force);
VectorXD b2 = -1 * C0 / TimeStep;
// b total - ambos bs
VectorXD realB = new DenseVectorXD(b.Count + b2.Count);
realB.SetSubVector(0, b.Count, b);
realB.SetSubVector(b.Count, b2.Count, b2);
// V se forma por velocidades y un vector de lamdas, que debe ser del tamaño de C0 ya que
// b2 es escalar * c0
VectorXD lamdas = new DenseVectorXD(C0.Count);
// conjunto de velocidades formada por las velocidades y las lamdas
VectorXD megaV = new DenseVectorXD(total_velocities.Count + lamdas.Count);
megaV.SetSubVector(0, total_velocities.Count, total_velocities);
megaV.SetSubVector(total_velocities.Count, lamdas.Count, lamdas);
// Resolvemos el sistema
megaV = megaMatrix.Solve(realB);
// nueva velocidad
VectorXD newVelocities = megaV.SubVector(0, total_velocities.Count);
// Establecemos las nuevas posiciones y velocidades
foreach (RigidBody obj in m_objs)
{
obj.setVelocityVector(newVelocities);
obj.advancePosition();
}
}
private void SoftConstraintsStep()
{
// Apuntes Miki + apuntes Marta + apuntes clase
// Step de Débiles:
// Podemos integrar cada rigidbody por separado o tener un solo sistema V siendo un vector V,W
// Teniendo M * v' = F
// Siendo M:
// | m*Identidad 0 |
// | 0 MInercia |
// y F = | F |
// | T |
// A * v = b
// M * V = M*V0 + timestep * F
// b = M*V0 + timestep * F
// La matriz de masas es numero de objetos * 6
MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6);
VectorXD total_force = new DenseVectorXD(m_objs.Count * 6);
VectorXD total_velocities = new DenseVectorXD(m_objs.Count * 6);
// Paso 0: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error - Podríamos meterlo después, but
foreach (RigidBody obj in m_objs)
{
obj.clearForcesAndMatrices();
}
// Paso 1: añadimos las fuerzas del constraint
// En ellas hacemos Fa y Fb (Si tienen los cuerpos)
// Y añadimos el torque a los rigidbodies
foreach (Constraint c in m_constraints)
{
c.addForces();
}
// Paso 2: Establecemos la parte del objeto en la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs
// Paso 3: Metemos las velocidades y las fuerzas (y rotaciones)
// Fuertes deberían ser practicamente iguales pero añadiendo las jacobianas
foreach (RigidBody obj in m_objs)
{
obj.getMassMatrix(massMatrix);
obj.getVelocityVector(total_velocities);
obj.addForcesAndMatrices();
obj.getForceVector(total_force);
}
// Resolvemos el sistema
VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force);
VectorXD newVelocities = massMatrix.Solve(b);
// Y modificamos posiciones
foreach (RigidBody obj in m_objs)
{
obj.setVelocityVector(newVelocities);
obj.advancePosition();
}
}
public virtual double Dot(double[] a, double[] b)
{
Double.DenseVector vector_a = new Double.DenseVector(a);
return(vector_a.DotProduct(new Double.DenseVector(b)));
}
