public double[] transformMoments(double[] forces) { double[] fforce = new double[6]; forces.CopyTo(fforce, 0); DenseVector forceV = new DenseVector(fforce); DenseVector tforce = (DenseVector)forceV.SubVector(0, 3); DenseVector moments = (DenseVector)forceV.SubVector(3, 3); DenseVectorCrossProduct crs = new DenseVectorCrossProduct(tforce); moments = (DenseVector)crs.crossProduct(directionalVector).Add(moments); forceV.SetSubVector(3, 3, moments); return forceV.Values; }
public void VarianceRatioTest(double[] data, int lag = 1, TimeSeriesType cor = TimeSeriesType.HOM) { DenseVector x = new DenseVector(data); //Mean double mu = x.Mean(); //Variance for 1st Order Difference double s1 = (x.SubVector(lag, x.Count - lag) - x.SubVector(0, x.Count - 2)).Variance(); double dLag = lag; double varvrt = double.NaN; switch (cor) { case TimeSeriesType.HOM: { varvrt = 2*(2*dLag - 1)*(dLag - 1)/(3*dLag*x.Count); break; } case TimeSeriesType.HET: { varvrt = 0; double sum2 = 0; for (int j = 0; j < lag; j++) { double sum1a = 0; //(x(j+2:n)-x(j+1:n-1)-mu).^2 double sum1b = 0; //(x(2:n-j)-x(1:n-j-1)-mu).^2; double sum1 = sum1a*sum1b; double delta = sum1/(Math.Pow(sum2, 2)); varvrt += 0; //(2*(q(i)-j)/q(i))^2)*delta; } break; } } ZScore = (VRatio - 1)/Math.Sqrt(varvrt); PValue = NormCDF(ZScore); }
/// <summary> /// Run example /// </summary> public void Run() { // Format vector output to console var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone(); formatProvider.TextInfo.ListSeparator = " "; // Create new empty vector var vectorA = new DenseVector(10); Console.WriteLine(@"Empty vector A"); Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 1. Fill vector by data using indexer [] for (var i = 0; i < vectorA.Count; i++) { vectorA[i] = i; } Console.WriteLine(@"1. Fill vector by data using indexer []"); Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Fill vector by data using SetValues method vectorA.SetValues(new[] { 9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0 }); Console.WriteLine(@"2. Fill vector by data using SetValues method"); Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Convert Vector to double[] var data = vectorA.ToArray(); Console.WriteLine(@"3. Convert vector to double array"); for (var i = 0; i < data.Length; i++) { Console.Write(data[i].ToString("#0.00\t", formatProvider) + @" "); } Console.WriteLine(); Console.WriteLine(); // 4. Convert Vector to column matrix. A matrix based on this vector in column form (one single column) var columnMatrix = vectorA.ToColumnMatrix(); Console.WriteLine(@"4. Convert vector to column matrix"); Console.WriteLine(columnMatrix.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 5. Convert Vector to row matrix. A matrix based on this vector in row form (one single row) var rowMatrix = vectorA.ToRowMatrix(); Console.WriteLine(@"5. Convert vector to row matrix"); Console.WriteLine(rowMatrix.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 6. Clone vector var cloneA = vectorA.Clone(); Console.WriteLine(@"6. Clone vector"); Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 7. Clear vector cloneA.Clear(); Console.WriteLine(@"7. Clear vector"); Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 8. Copy part of vector into another vector. If you need to copy all data then use CopoTy(vector) method. vectorA.CopySubVectorTo(cloneA, 3, 3, 4); Console.WriteLine(@"8. Copy part of vector into another vector"); Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 9. Get part of vector as another vector var subvector = vectorA.SubVector(0, 5); Console.WriteLine(@"9. Get subvector"); Console.WriteLine(subvector.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 10. Enumerator usage Console.WriteLine(@"10. Enumerator usage"); foreach (var value in vectorA) { Console.Write(value.ToString("#0.00\t", formatProvider) + @" "); } Console.WriteLine(); Console.WriteLine(); // 11. Indexed enumerator usage Console.WriteLine(@"11. Enumerator usage"); foreach (var value in vectorA.GetIndexedEnumerator()) { Console.WriteLine(@"Index = {0}; Value = {1}", value.Item1, value.Item2.ToString("#0.00\t", formatProvider)); } Console.WriteLine(); }
// Computes d(ei)/d(P) for ith line numericaly public void ComputeJacobianForLine_Numerical(Matrix<double> J, int l, int p0) { Vector<double> error_n = new DenseVector(_currentErrorVector.Count); Vector<double> error_p = new DenseVector(_currentErrorVector.Count); for(int k = 0; k < DistortionModel.ParametersCount; ++k) { double oldK = DistortionModel.Parameters[k]; double k_n = Math.Abs(oldK) > float.Epsilon ? oldK * (1 - NumericalDerivativeStep) : -NumericalDerivativeStep * 0.01; double k_p = Math.Abs(oldK) > float.Epsilon ? oldK * (1 + NumericalDerivativeStep) : NumericalDerivativeStep * 0.01; DistortionModel.Parameters[k] = k_n; UpdateAll(l); ComputeErrorVector(error_n); Vector<double> error_n_line = error_n.SubVector(p0, LinePoints[l].Count); DistortionModel.Parameters[k] = k_p; UpdateAll(l); ComputeErrorVector(error_p); Vector<double> error_p_line = error_p.SubVector(p0, LinePoints[l].Count); Vector<double> diff_e = (1.0 / (k_p - k_n)) * (error_p_line - error_n_line); J.SetColumn(k, p0, LinePoints[l].Count, diff_e); DistortionModel.Parameters[k] = oldK; // Throw if NaN found in jacobian bool nanInfFound = diff_e.Exists((e) => { return double.IsNaN(e) || double.IsInfinity(e); }); if(nanInfFound) { throw new DivideByZeroException("NaN or Infinity found on jacobian"); } } UpdateAll(l); }
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(); } }