public Jacobian(Jacobian arg) : this(kdlPINVOKE.new_Jacobian__SWIG_2(Jacobian.getCPtr(arg)), true)
{
if (kdlPINVOKE.SWIGPendingException.Pending)
{
throw kdlPINVOKE.SWIGPendingException.Retrieve();
}
}
public static void Execute(Manipulator manip)
{
var AD = Dispatcher.WorkspaceBuffer.AlgBuffer;
/*// generating random tree
* var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
* var planner = new RRT(Obstacles, solver, AD.k, true, AD.d);
* PathPlanner pp = planner;
* var resRRT = pp.Execute(manip, null);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resRRT.Item1;
* manip.States["Path"] = true;
* manip.Configs = resRRT.Item2;*/
/*// generating random tree
* var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
* var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);
* PathPlanner pp = planner;
* var resRRT = pp.Execute(manip, null);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resRRT.Item1;
* manip.States["Path"] = true;
* manip.Configs = resRRT.Item2;*/
var solver = new Jacobian(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);
planner.Start(manip, Vector3.Zero);
/*var resGA = PathPlanner.GeneticAlgorithm(manip, Obstacles, manip.Goal, resRRT.Item2.ToArray(),
* 0.99, manip.Joints.Length, 20, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);*/
/*var input = new (Vector3, float[])[resRRT.Item1.Count];
* for (int i = 0; i < input.Length; i++)
* {
* input[i].Item1 = resRRT.Item1[i];
* input[i].Item2 = resRRT.Item2[i];
* }
*
* var jac = new Jacobian(Obstacles, manip.q.Length, AD.Precision, AD.StepSize, AD.MaxTime);
* var resGA = PathPlanner.GeneticAlgorithmD(manip, Obstacles, manip.Goal, jac,
* input, 0.99, manip.Joints.Length, 4, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resGA.Item1;
* manip.States["Path"] = true;
* manip.Configs = resGA.Item2;*/
}
public virtual int JntToJac(JntArray q_in, Jacobian jac)
{
int ret = kdlPINVOKE.ChainJntToJacSolver_JntToJac__SWIG_1(swigCPtr, JntArray.getCPtr(q_in), Jacobian.getCPtr(jac));
if (kdlPINVOKE.SWIGPendingException.Pending)
{
throw kdlPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public void DebugInfo(StringBuilder b, int debugPage, PhysicalParticle particle)
{
if (debugPage == 1)
{
b.AppendLine($"Constraint Forces:\n{TotalConstraintForces?.Transpose().ToMatrixString()}");
b.AppendLine($"Lagrange Multipliers:\n{LagrangeMultipliers?.Transpose()?.ToMatrixString()}");
b.AppendLine($"Jacobian:\n{Jacobian?.ToMatrixString()}");
b.AppendLine($"Velocity:\n{Velocity?.Transpose()?.ToMatrixString()}");
b.AppendLine($"Coefficient Matrix: (det {CoefficientMatrix?.Determinant()})\n{CoefficientMatrix?.ToMatrixString()}");
b.AppendLine($"Equation Constants:\n{EquationConstants?.ToMatrixString()}");
}
}
// Update is called once per frame
void Update()
{
//for (int i = 0; i < 100; i++)
{
//for (int x = 0; x < 10; x++)
Jacobian.updateJacobianTranspose(m_chain, m_target.position, Vector3.right);
//for (int x = 0; x < 10; x++)
Jacobian.updateJacobianTranspose(m_chain, m_target.position, Vector3.up);
//for (int x = 0; x < 10; x++)
Jacobian.updateJacobianTranspose(m_chain, m_target.position, Vector3.forward);
updateChain();
}
}
void updateChain()
{
// Just copy from objects
Vector3 end = transform.position;
for (int i = 0; i < m_chain.Count; i++)
{
Joint current = m_chain[i];
GameObject currentObj = m_chainObjs[i];
current.length = currentObj.transform.localScale.x;
current.m_position = currentObj.transform.position - currentObj.transform.right * current.length * 0.5f;
current.m_endPoint = currentObj.transform.position + currentObj.transform.right * current.length * 0.5f;
end = current.m_endPoint;
}
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, m_chainObjs, m_dofs, m_dofJointId, end + m_virtualForce);
CMatrix Jt = CMatrix.Transpose(J);
CMatrix force = new CMatrix(3, 1);
force[0, 0] = m_virtualForce.x;
force[1, 0] = m_virtualForce.y;
force[2, 0] = m_virtualForce.z;
//CMatrix torqueSet = Jt*force;
Debug.Log(Jt.m_rows + "x" + Jt.m_cols);
//Debug.Log(torqueSet.m_rows+"x"+torqueSet.m_cols);
//for (int i = 0; i < m_chain.Count; i++)
//{
// // store torque
// m_torques[i] = Vector3.forward*Vector3.Dot(new Vector3(Jt[i,0],Jt[i,1],Jt[i,2]),m_virtualForce);
// //Debug.Log(m_torques[i].ToString());
//}
// This could either be a new kernel, or continuation of the old
// Here in GPGPU, we could zero the torque position each time...
for (int i = 0; i < m_chain.Count; i++)
{
m_torques[i] = Vector3.zero; // for each dof, for its joint id (as if it had been in the next loop essentially)
}
// ... but force sync here, so the following add-op isn't overwritten:
for (int i = 0; i < m_dofs.Count; i++)
{
// store torque
int x = m_dofJointId[i];
m_torques[x] += /*m_chainObjs[x].transform.TransformDirection(m_dofs[i])*/ m_dofs[i] * Vector3.Dot(new Vector3(Jt[i, 0], Jt[i, 1], Jt[i, 2]), m_virtualForce);
//Debug.Log(m_torques[i].ToString());
}
// Come to think of it, the jacobian and torque could be calculated in the same
// kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above)
}
private double[,] JacobianToDoubleFormat(Dictionary <string, FloatingPoint> values)
{
double[,] numJac = new double[Jacobian.GetLength(0), Jacobian.GetLength(1)];
for (int i = 0; i < Jacobian.GetLength(0); i++)
{
for (int j = 0; j < Jacobian.GetLength(1); j++)
{
//Перевод из FloatingPoint в double
numJac[i, j] = double.Parse(Jacobian[i, j].Evaluate(values).RealValue.ToString().Replace(".", ","));
}
}
return(numJac);
}
/// <summary>
/// Determines whether the specified <see cref="System.Object" />, is equal to this instance.
/// </summary>
/// <param name="obj">The <see cref="System.Object" /> to compare with this instance.</param>
/// <returns>
/// <c>true</c> if the specified <see cref="System.Object" /> is equal to this instance; otherwise, <c>false</c>.
/// </returns>
public override bool Equals(object obj)
{
if (obj is JacobianInfo ji)
{
if (!Jacobian.Equals(ji.Jacobian))
{
return(false);
}
if (!Rhs.Equals(ji.Rhs))
{
return(false);
}
return(true);
}
return(false);
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(Jacobian obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public IOde23tVariableSolverType WithJacobian(Jacobian jacobian)
{
model.Array.ConfigSet.Solver.AdditionalSolverOptions.SolverJacobianMethodControl = jacobian;
return(this);
}
public void Solve()
{
double max;
//вектор, в который записывается текущее приближение
_curr_vector = new double[X.Values.Count, 1];
//вектор, в котором хранится предыдущее приближение
_prev_vector = new double[X.Values.Count, 1];
//Создание временного словаря и присвоение ему начальных значений
var temp = X;
do
{
//Увеличение счетчика итераций
Counter++;
//Составление численного Якобиана из обновленных значений перменных
_numJac = new double[Jacobian.GetLength(0), Jacobian.GetLength(1)];
for (int i = 0; i < Jacobian.GetLength(0); i++)
{
for (int j = 0; j < Jacobian.GetLength(1); j++)
{
//Перевод из FloatingPoint в double
if (Jacobian[i, j] != null)
{
_numJac[i, j] = double.Parse(Jacobian[i, j].Evaluate(temp).RealValue.ToString().Replace(".", ","));
}
}
}
//Нахождение обратной матрицы Якобиана
_revJacobian = Matrix.getInverse(_numJac);;
//Обновление матрицы приближенных значений предыдущего шага
for (int i = 0; i < temp.Count; i++)
{
var elem = temp.ElementAt(i);
_prev_vector[i, 0] = double.Parse(elem.Value.RealValue.ToString().Replace(".", ","));
}
//Матрица числовых значений начальных функций
_curr_func_value = new double[Equations.Functions.GetLength(1), 1];
//Расчет значений для матрицы функций
for (int i = 0; i < Equations.Functions.GetLength(0); i++)
{
for (int j = 0; j < Equations.Functions.GetLength(1); j++)
{
_curr_func_value[i, 0] += double.Parse(Equations.Functions[i, j].Evaluate(temp).RealValue.ToString().Replace(".", ","));
}
}
//Расчет дельта X
var delta_x = Matrix.Multiply(_revJacobian, _curr_func_value);
//Получение нового приближения
_curr_vector = Matrix.Subtract(_prev_vector, delta_x);
max = Math.Abs(delta_x[0, 0]);
temp = new Dictionary <string, FloatingPoint>();
//Вычисление максимального значения в массиве delta_x и обновление словаря temp
for (int i = 0; i < X.Count; i++)
{
if (max < Math.Abs(delta_x[i, 0]))
{
max = Math.Abs(delta_x[i, 0]);
}
var elem = X.ElementAt(i);
temp.Add(elem.Key, Expr.Parse(_curr_vector[i, 0].ToString().Replace(",", ".")).RealNumberValue);
}
} while (max > Eps && Counter != _maxCounter);
Console.WriteLine($"Итоговое количество итераций: {Counter}");
Console.WriteLine("Ответ: ");
Matrix.Print(_curr_vector);
Console.WriteLine("\n");
}
public IExtrapolatedFixedSolverType WithJacobian(Jacobian jacobian)
{
model.Array.ConfigSet.Solver.AdditionalSolverOptions.SolverJacobianMethodControl = jacobian;
return(this);
}
/// <summary>
/// Initializes a new instance of the <see cref="LevenbergMarquardt"/> class.
/// </summary>
/// <param name="function">Cost function.</param>
/// <param name="jacobianFunction">Jacobian.</param>
public LevenbergMarquardt(Function function, Jacobian jacobianFunction)
{
this.function = function;
this.jacobianFunction = jacobianFunction;
}
private void InvertJacobian()
{
InvertedJacobian = Jacobian.Inverse();
}
/// <summary> /// Returns a hash code for this instance. /// </summary> /// <returns> /// A hash code for this instance, suitable for use in hashing algorithms and data structures like a hash table. /// </returns> public override int GetHashCode() => (Jacobian.GetHashCode() * 13) ^ (Rhs.GetHashCode());
private double iterate(double[][] inputs, double[] outputs)
{
if (Residuals is null || inputs.Length != Residuals.Length)
{
Residuals = new double[inputs.Length];
for (var i = 0; i < Jacobian.Length; i++)
{
Jacobian[i] = new double[inputs.Length];
}
}
for (var i = 0; i < inputs.Length; i++)
{
Residuals[i] = outputs[i] - Function(Solution, inputs[i]);
}
if (ParallelOptions.MaxDegreeOfParallelism == 1)
{
for (var i = 0; i < inputs.Length; i++)
{
Gradient(Solution, inputs[i], gradient);
for (var j = 0; j < gradient.Length; j++)
{
Jacobian[j][i] = -gradient[j];
}
if (Token.IsCancellationRequested)
{
break;
}
}
}
else
{
Parallel.For(0, inputs.Length, ParallelOptions,
() => new double[NumberOfParameters],
(i, state, grad) =>
{
Gradient(Solution, inputs[i], grad);
for (var j = 0; j < grad.Length; j++)
{
Jacobian[j][i] = -grad[j];
}
return(grad);
},
grad => { }
);
}
// Compute error gradient using Jacobian
Jacobian.Dot(Residuals, gradient);
// Compute Quasi-Hessian Matrix approximation
Jacobian.DotWithTransposed(Jacobian, Hessian);
decomposition = new JaggedSingularValueDecomposition(Hessian,
true, true, true);
Deltas = decomposition.Solve(gradient);
for (var i = 0; i < Deltas.Length; i++)
{
Solution[i] -= Deltas[i];
}
return(ComputeError(inputs, outputs));
}
// Compute the torque needed on swing legs to compensate for gravity
Vector3[] computeCGVFTorques(float p_phi, float p_dt)
{
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_useVFTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
// Calculate torques using each leg chain
for (int n = 0; n < LegFrame.c_legCount; n++) // for each leg
{
if (!lf.isInControlledStance(n, m_player.m_gaitPhase)) // only on swing
{
for (int m = 0; m < LegFrame.c_legSegments; m++) // for each segment in leg
{
// get the joints
int segIdx = n * LegFrame.c_legSegments + m + 1; // get segment index in list (+1 to offset for LF)
//Debug.Log("sidx: " + segIdx);
Rigidbody segment = m_joints[segIdx];
lf.calculateFgravcomp(segIdx - 1, segment);
//
// Calculate jacobian
//
// get the joints
int legFrameRoot = lf.m_id;
//legFrameRoot = -1;
int legRoot = lf.m_neighbourJointIds[n];
int legSegmentCount = m + 1; // the amount of segments decreases the further in in the hierarchy we get
// Use joint ids to get dof ids
// Start in chain
int legFrameRootDofId = -1; // if we have separate root as base link
if (legFrameRoot != -1)
{
legFrameRootDofId = m_chain[legFrameRoot].m_dofListIdx;
}
// otherwise, use first in chain as base link
int legRootDofId = m_chain[legRoot].m_dofListIdx;
// end in chain
int lastDofIdx = legRoot + m;
int legDofEnd = m_chain[lastDofIdx].m_dofListIdx + m_chain[lastDofIdx].m_dof.Length;
//
// get force for the leg
Vector3 VF = lf.m_legSegmentGravityCompVirtualForces[segIdx - 1];
// Calculate torques for each joint
// Start by updating joint information based on their gameobjects
Vector3 end = segment.transform.TransformPoint(segment.centerOfMass);
//foreach(Joint j in m_chain)
// Debug.Log("joint pos CC: " + j.m_position);
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, // Joints (Joint script)
m_chainObjs, // Gameobjects in chain
m_dofs, // Degrees Of Freedom (Per joint)
m_dofJointId, // Joint id per DOF
end + VF, // Target position
legRootDofId, // Starting link id in chain (start offset)
legDofEnd, // End of chain of link (ie. size)
legFrameRootDofId); // As we use the leg frame as base, we supply it separately (it will be actual root now)
CMatrix Jt = CMatrix.Transpose(J);
//Debug.DrawLine(end, end + VF * 0.4f, Color.magenta);
/*Color idxCol = new Color((float)n / (float)LegFrame.c_legCount, (float)m / (float)LegFrame.c_legSegments, (float)segIdx / (float)(LegFrame.c_legCount * LegFrame.c_legSegments));
* Debug.DrawLine(end, end + VF, idxCol);
* Debug.DrawLine(end, end + VF, idxCol);*/
int jIdx = 0;
int extra = 0;
int start = legRootDofId;
if (legFrameRootDofId >= 0)
{
start = legFrameRootDofId;
extra = m_chain[legFrameRoot].m_dof.Length;
}
for (int g = start; g < legDofEnd; g++)
{
if (extra > 0)
{
extra--;
}
else if (g < legRootDofId)
{
g = legRootDofId;
}
// store torque
int x = m_dofJointId[g];
Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + VF, new Color(1.0f, 153.0f / 255.0f, 153.0f / 255.0f));
Vector3 addT = m_dofs[g] * Vector3.Dot(new Vector3(Jt[jIdx, 0], Jt[jIdx, 1], Jt[jIdx, 2]), VF);
newTorques[x] += addT;
jIdx++;
Vector3 drawTorque = addT;
//Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + drawTorque * 0.5f, idxCol);
}
} // endfor legsegments
} // endif not-stance
} // endfor legs
}
}
return(newTorques);
}
public LevenbergMarquardt(Function function, Jacobian jacobianFunction)
{
this.function = function;
this.jacobianFunction = jacobianFunction;
}
// Compute the torque of all applied virtual forces
Vector3[] computeVFTorques(float p_phi, float p_dt)
{
Vector3[] newTorques = new Vector3[m_jointTorques.Length];
if (m_useVFTorque)
{
for (int i = 0; i < m_legFrames.Length; i++)
{
LegFrame lf = m_legFrames[i];
lf.calculateNetLegVF(p_phi, p_dt, m_currentVelocity, m_desiredVelocity);
// Calculate torques using each leg chain
for (int n = 0; n < LegFrame.c_legCount; n++)
{
// get the joints
int legFrameRoot = lf.m_id;
//legFrameRoot = -1;
int legRoot = lf.m_neighbourJointIds[n];
int legSegmentCount = LegFrame.c_legSegments; // hardcoded now
// Use joint ids to get dof ids
// Start in chain
int legFrameRootDofId = -1; // if we have separate root as base link
if (legFrameRoot != -1)
{
legFrameRootDofId = m_chain[legFrameRoot].m_dofListIdx;
}
// otherwise, use first in chain as base link
int legRootDofId = m_chain[legRoot].m_dofListIdx;
// end in chain
int lastDofIdx = legRoot + legSegmentCount - 1;
int legDofEnd = m_chain[lastDofIdx].m_dofListIdx + m_chain[lastDofIdx].m_dof.Length;
//
// get force for the leg
Vector3 VF = lf.m_netLegBaseVirtualForces[n];
// Calculate torques for each joint
// Start by updating joint information based on their gameobjects
Vector3 end = transform.localPosition;
//Debug.Log("legroot "+legRoot+" legseg "+legSegmentCount);
int jointstart = legRoot;
if (legFrameRoot != -1)
{
jointstart = legFrameRoot;
}
for (int x = jointstart; x < legRoot + legSegmentCount; x++)
{
if (legFrameRoot != -1 && x < legRoot && x != legFrameRoot)
{
x = legRoot;
}
Joint current = m_chain[x];
GameObject currentObj = m_chainObjs[x];
//Debug.Log("joint pos: " + currentObj.transform.localPosition);
// Update Joint
current.length = currentObj.transform.localScale.y;
current.m_position = currentObj.transform.position /*- (-currentObj.transform.up) * current.length * 0.5f*/;
current.m_endPoint = currentObj.transform.position + (-currentObj.transform.up) * current.length /* * 0.5f*/;
//m_chain[i] = current;
//Debug.DrawLine(current.m_position, current.m_endPoint, Color.red);
//Debug.Log(x+" joint pos: " + current.m_position + " = " + m_chain[x].m_position);
end = current.m_endPoint;
}
//foreach(Joint j in m_chain)
// Debug.Log("joint pos CC: " + j.m_position);
//CMatrix J = Jacobian.calculateJacobian(m_chain, m_chain.Count, end, Vector3.forward);
CMatrix J = Jacobian.calculateJacobian(m_chain, // Joints (Joint script)
m_chainObjs, // Gameobjects in chain
m_dofs, // Degrees Of Freedom (Per joint)
m_dofJointId, // Joint id per DOF
end + VF, // Target position
legRootDofId, // Starting link id in chain (start offset)
legDofEnd, // End of chain of link (ie. size)
legFrameRootDofId); // As we use the leg frame as base, we supply it separately (it will be actual root now)
CMatrix Jt = CMatrix.Transpose(J);
//Debug.DrawLine(end, end + VF, Color.magenta, 0.3f);
int jIdx = 0;
int extra = 0;
int start = legRootDofId;
if (legFrameRootDofId >= 0)
{
start = legFrameRootDofId;
extra = m_chain[legFrameRoot].m_dof.Length;
}
for (int g = start; g < legDofEnd; g++)
{
if (extra > 0)
{
extra--;
}
else if (g < legRootDofId)
{
g = legRootDofId;
}
// store torque
int x = m_dofJointId[g];
Vector3 addT = m_dofs[g] * Vector3.Dot(new Vector3(Jt[jIdx, 0], Jt[jIdx, 1], Jt[jIdx, 2]), VF);
Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + VF, new Color(0.0f, 153.0f / 256.0f, 0.0f));
newTorques[x] += addT;
jIdx++;
//Vector3 drawTorque = new Vector3(0.0f, 0.0f, -addT.x);
//Debug.DrawLine(m_joints[x].transform.position, m_joints[x].transform.position + drawTorque*0.1f, Color.cyan);
}
// Come to think of it, the jacobian and torque could be calculated in the same
// kernel as it lessens write to global memory and the need to fetch joint matrices several time (transform above)
}
}
}
return(newTorques);
}
private Jacobian m_jB; // Jacobian for contact bi-tangent (friction)
public Contact()
{
m_jN = new Jacobian(Jacobian.Type.Normal);
m_jT = new Jacobian(Jacobian.Type.Tangent);
m_jB = new Jacobian(Jacobian.Type.Tangent);
}
public override void GenerateJacobian()
{
int i = 0;
Jacobian.Clear();
var testEval = new Evaluator();
foreach (var equation in Equations)
{
var incidenceVector = equation.Incidence(testEval);
foreach (var variable in incidenceVector)
{
if (!variable.IsConstant && variable.DefiningExpression == null)
{
int j = -1;
if (_variableIndex.TryGetValue(variable, out j))
{
Jacobian.Add(new JacobianElement()
{
EquationIndex = i, VariableIndex = j, Value = 1.0
});
if (UseHessian)
{
var differential = (equation.Right - equation.Left).SymbolicDiff(variable);
foreach (var variable2 in incidenceVector)
{
if (!variable2.IsConstant && variable2.DefiningExpression == null)
{
int k = -1;
if (_variableIndex.TryGetValue(variable2, out k) && k <= j)
{
Hessian.Add(new HessianElement()
{
EquationIndex = i, Variable1Index = j, Variable2Index = k, Expression = differential, Value = 1.0
});
}
}
}
}
}
}
}
i++;
}
if (UseHessian)
{
var incidenceVector = ObjectiveFunction.Incidence();
foreach (var variable1 in incidenceVector)
{
if (!variable1.IsConstant && variable1.DefiningExpression == null)
{
int j = -1;
if (_variableIndex.TryGetValue(variable1, out j))
{
var differential = ObjectiveFunction.SymbolicDiff(variable1);
foreach (var variable2 in incidenceVector)
{
if (!variable2.IsConstant && variable2.DefiningExpression == null)
{
int k = -1;
if (_variableIndex.TryGetValue(variable2, out k) && k <= j)
{
ObjectiveHessian.Add(new HessianElement()
{
EquationIndex = 0, Variable1Index = j, Variable2Index = k, Expression = differential, Value = 1.0
});
}
}
}
}
}
}
}
foreach (var equation in Constraints)
{
var incidenceVector = equation.Incidence(testEval);
foreach (var variable in incidenceVector)
{
if (!variable.IsConstant && variable.DefiningExpression == null)
{
int j = -1;
if (_variableIndex.TryGetValue(variable, out j))
{
Jacobian.Add(new JacobianElement()
{
EquationIndex = i, VariableIndex = j, Value = 1.0
});
if (UseHessian)
{
var differential = (equation.Right - equation.Left).SymbolicDiff(variable);
foreach (var variable2 in incidenceVector)
{
if (!variable2.IsConstant && variable2.DefiningExpression == null)
{
int k = -1;
if (_variableIndex.TryGetValue(variable2, out k) && k <= j)
{
Hessian.Add(new HessianElement()
{
EquationIndex = i, Variable1Index = j, Variable2Index = k, Expression = differential, Value = 1.0
});
}
}
}
}
}
}
}
i++;
}
if (UseHessian)
{
GenerateHessianStructureInfo();
}
}
