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