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