public LinearProblemProperties UpdateConstantsTerm(
JacobianConstraint[] constraints,
LinearProblemProperties input)
{
for (int i = 0; i < constraints.Length; i++)
{
var constraint = constraints[i];
double correctionVal = constraint.CorrectionValue * EngineParameters.TimeStep;
double correctionValue = (correctionVal) < 0 ?
Math.Max(correctionVal, -EngineParameters.MaxCorrectionValue) :
Math.Min(correctionVal, EngineParameters.MaxCorrectionValue);
double jacobianVelocityValue = 0.0;
if (constraint.LinearComponentA.HasValue)
{
jacobianVelocityValue += constraint.LinearComponentA.Value.Dot(constraint.ObjectA.LinearVelocity);
}
if (constraint.LinearComponentB.HasValue)
{
jacobianVelocityValue += constraint.LinearComponentB.Value.Dot(constraint.ObjectA.LinearVelocity);
}
jacobianVelocityValue += constraint.AngularComponentA.Dot(constraint.ObjectA.AngularVelocity) +
constraint.AngularComponentB.Dot(constraint.ObjectB.AngularVelocity);
input.B[i] = correctionValue - jacobianVelocityValue - constraint.ConstraintValue;
}
return(input);
}
public double[] Solve(
LinearProblemProperties linearProblemProperties,
JacobianConstraint[] constraints,
double[] x)
{
return(solver.Solve(linearProblemProperties.M, linearProblemProperties.B, SolverParameters.MaxIterations));
}
public double[] Solve(LinearProblemProperties input, JacobianConstraint[] constraints, double[] x)
{
x = gaussSeidelSolver.Solve(input, constraints, x);
return(x);
}
static void Main(string[] args)
{
GaussSeidel solver = new GaussSeidel(1.0);
double[] M = new double[9];
M[0] = 12.0;
M[1] = 3.0;
M[2] = -5.0;
M[3] = 1.0;
M[4] = 5.0;
M[5] = 3.0;
M[6] = 3.0;
M[7] = 7.0;
M[8] = 13.0;
double[] B = new double[3];
B[0] = 1.0;
B[1] = 28.0;
B[2] = 76.0;
double[] startX = new double[3];
startX[0] = 1.0;
startX[0] = 0.0;
startX[0] = 1.0;
LinearProblemProperties linearProblemProperties = new LinearProblemProperties(
M,
B,
startX,
3);
double[] X = new double[3];
NonLinearConjugateGradient nonLinearConjugateGradient = new NonLinearConjugateGradient(3);
double[] X1 = nonLinearConjugateGradient.Solve(linearProblemProperties);
for (int i = 0; i < 4; i++)
{
X = solver.Solve(linearProblemProperties);
}
double[] out0 = nonLinearConjugateGradient.CalculateError(linearProblemProperties.M, linearProblemProperties.B, X1);
double[] out1 = nonLinearConjugateGradient.CalculateError(linearProblemProperties.M, linearProblemProperties.B, X);
Console.ReadLine();
}
private double[] BuildMatrixAndExecuteSolver(
JacobianConstraint[] contactConstraints,
LinearProblemProperties linearProblemProperties,
int nIterations)
{
if (linearProblemProperties != null)
{
Solver.GetSolverParameters().SetSolverMaxIteration(nIterations);
double[] solutionValues = Solver.Solve(linearProblemProperties, contactConstraints, new double[linearProblemProperties.Count]);
return(solutionValues);
}
return(null);
}
private SubspaceValues GetIndexSets(LinearProblemProperties input, double[] x)
{
var result = new SubspaceValues();
for (int i = 0; i < x.Length; i++)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
if (x[i] <= 0.0)
{
result.LowerIndexes.Add(i);
}
else
{
result.UnboundedIndexes.Add(i);
}
break;
case ConstraintType.Friction:
int sign = (input.Constraints[i].Value < 0) ? -1 : 1;
int normalIndex = sign * input.Constraints[i].Value;
double frictionLimit = x[normalIndex] * input.ConstraintLimit[i];
int idx1 = normalIndex + sign;
int idx2 = normalIndex + sign * 2;
double directionA = x[idx1];
double directionB = x[idx2];
double frictionValue = Math.Sqrt(directionA * directionA + directionB * directionB);
if (frictionValue > frictionLimit)
{
Vector2d frictionNormal = new Vector2d(directionA / frictionValue, directionB / frictionValue);
}
break;
default:
break;
}
}
return(result);
}
private double[] Solve(JacobianConstraint[] jacobianConstraints)
{
LinearProblemProperties LCP = LinearSystemBuilder.BuildLCP(jacobianConstraints, 0.001);
double[] partialSolution = new double[LCP.Count];
double[] overallSolution = new double[LCP.Count];
Solver.GetSolverParameters().SetSolverMaxIteration(2);
for (int i = 0; i < 15; i++)
{
overallSolution = MathUtils.Add(overallSolution, partialSolution);
partialSolution = Solver.Solve(LCP, jacobianConstraints, overallSolution);
}
//double[] overallSolution = Solver.Solve(LCP, LCP.StartImpulse);
return(overallSolution);
}
private void SolveConstraints()
{
#region Contact and Joint elaboration
//SaveShapePreviousProperties(collisionPoints);
if (Partitions != null && Partitions.Any())
{
var rangePartitioner = Partitioner.Create(
0,
Partitions.Count,
Convert.ToInt32(Partitions.Count / EngineParameters.MaxThreadNumber) + 1);
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
JacobianConstraint[] jacobianConstraints = GetJacobianConstraints(
Partitions[i].PartitionedCollisionPoints.ToArray(),
Partitions[i].PartitionedJoints,
Shapes,
EngineParameters);
if (jacobianConstraints.Length > 0)
{
LinearProblemProperties LCP = GenerateLCP(jacobianConstraints);
double[] overallSolution = Solver.Solve(LCP, jacobianConstraints, LCP.StartImpulse);
//double[] overallSolution = Solve(jacobianConstraints);
IntegrateVelocityEngine.UpdateVelocity(jacobianConstraints, overallSolution);
}
}
});
}
#endregion
}
public LinearProblemProperties BuildLCP(
JacobianConstraint[] constraints,
double timeStep)
{
LinearProblemBaseProperties baseProperties = new LinearProblemBaseProperties(constraints.Length);
Dictionary <HashSetStruct, List <DictionaryConstraintValue> > constraintsDictionary = new Dictionary <HashSetStruct, List <DictionaryConstraintValue> >();
for (int i = 0; i < constraints.Length; i++)
{
JacobianConstraint itemConstraint = constraints[i];
HashSetStruct hash = new HashSetStruct(itemConstraint.ObjectA.ID, itemConstraint.ObjectB.ID);
if (constraintsDictionary.TryGetValue(hash, out List <DictionaryConstraintValue> jc))
{
jc.Add(new DictionaryConstraintValue(itemConstraint, i));
}
else
{
constraintsDictionary.Add(hash, new List <DictionaryConstraintValue> {
new DictionaryConstraintValue(itemConstraint, i)
});
}
}
Graph graph = new Graph(constraints.Length);
var dictionaryArray = constraintsDictionary.ToArray();
var key_ID_A = constraintsDictionary.ToLookup(x => x.Key.ID_A);
var key_ID_B = constraintsDictionary.ToLookup(x => x.Key.ID_B);
var rangePartitioner = Partitioner.Create(
0,
dictionaryArray.Length,
Convert.ToInt32(dictionaryArray.Length / EngineParameters.MaxThreadNumber) + 1);
double timeFreq = 1.0 / timeStep;
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
var constraintValues = dictionaryArray[i];
var constraintValueKey = constraintValues.Key;
int contactA_ID_A = constraintValues.Key.ID_A;
int contactA_ID_B = constraintValues.Key.ID_B;
for (int w = 0; w < constraintValues.Value.Count; w++)
{
JacobianConstraint contactA = constraintValues.Value[w].Constraint;
List <int> index = new List <int>();
List <double> values = new List <double>();
int indexVal = constraintValues.Value[w].Index;
double correctionVal = contactA.CorrectionValue * timeFreq;
double correctionValue = (correctionVal) < 0 ?
Math.Max(correctionVal, -EngineParameters.MaxCorrectionValue) :
Math.Min(correctionVal, EngineParameters.MaxCorrectionValue);
baseProperties.B[indexVal] = correctionValue - contactA.JacobianVelocity - contactA.ConstraintValue;
baseProperties.ConstraintsArray[indexVal] = contactA.ContactReference;
baseProperties.ConstraintLimit[indexVal] = contactA.ConstraintLimit;
baseProperties.ConstraintType[indexVal] = contactA.Type;
//Diagonal value
double mValue = GetLCPDiagonalValue(contactA) +
(contactA.CFM * timeFreq) +
EngineParameters.CFM +
1E-40;
baseProperties.D[indexVal] = mValue;
baseProperties.InvD[indexVal] = 1.0 / mValue;
//contactA_ID_A == contactB_ID_A && contactA_ID_B == contactB_ID_B
for (int j = 0; j < constraintValues.Value.Count; j++)
{
int innerIndex = constraintValues.Value[j].Index;
if (innerIndex != indexVal)
{
JacobianConstraint contactB = constraintValues.Value[j].Constraint;
mValue = GetLCPMatrixValue(
contactA.LinearComponentA,
contactB.LinearComponentA,
contactA.AngularComponentA,
contactB.AngularComponentA,
contactA.ObjectA.MassInfo);
mValue += GetLCPMatrixValue(
contactA.LinearComponentB,
contactB.LinearComponentB,
contactA.AngularComponentB,
contactB.AngularComponentB,
contactA.ObjectB.MassInfo);
AddValues(ref index, ref values, mValue, innerIndex);
}
}
//contactA_ID_A == contactB_ID_B && contactA_ID_B == contactB_ID_A
var symmetricHashSet = new HashSetStruct(contactA_ID_B, contactA_ID_A);
if (constraintsDictionary.TryGetValue(symmetricHashSet, out List <DictionaryConstraintValue> symmetricList))
{
foreach (var item in symmetricList)
{
int innerIndex = item.Index;
if (innerIndex != indexVal)
{
JacobianConstraint contactB = item.Constraint;
mValue = GetLCPMatrixValue(
contactA.LinearComponentA,
contactB.LinearComponentB,
contactA.AngularComponentA,
contactB.AngularComponentB,
contactA.ObjectA.MassInfo);
mValue += GetLCPMatrixValue(
contactA.LinearComponentB,
contactB.LinearComponentA,
contactA.AngularComponentB,
contactB.AngularComponentA,
contactA.ObjectB.MassInfo);
AddValues(ref index, ref values, mValue, innerIndex);
}
}
}
//contactA_ID_A == contactB_ID_A
foreach (var constraintCheckItem in key_ID_A[contactA_ID_A])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentA,
contactA.AngularComponentA,
contactA.ObjectA.MassInfo,
true);
}
//contactA_ID_A == contactB_ID_B
foreach (var constraintCheckItem in key_ID_B[contactA_ID_A])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentA,
contactA.AngularComponentA,
contactA.ObjectA.MassInfo,
false);
}
//contactA_ID_B == contactB_ID_A
foreach (var constraintCheckItem in key_ID_A[contactA_ID_B])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentB,
contactA.AngularComponentB,
contactA.ObjectB.MassInfo,
true);
}
//contactA_ID_B == contactB_ID_B
foreach (var constraintCheckItem in key_ID_B[contactA_ID_B])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentB,
contactA.AngularComponentB,
contactA.ObjectB.MassInfo,
false);
}
//Sparse Matrix
baseProperties.M.Rows[indexVal] = new SparseVector(
values.ToArray(),
index.ToArray(),
baseProperties.M.m);
UpdateGraph(ref graph, index, indexVal);
}
}
});
var lp = new LinearProblemProperties(
baseProperties,
graph,
EngineParameters.FrictionDirections);
return(lp);
}
//TODO Refactor
public LinearProblemProperties GetLCPFrictionMatrix(LinearProblemProperties linearProblem)
{
var collisionIndexes = linearProblem.ConstraintType
.Select((constraint, index) => new { constraint, index })
.Where(x => x.constraint == ConstraintType.Collision).ToList();
if (collisionIndexes.Count > 0)
{
var numberOfCollision = collisionIndexes.Count;
var lcpMatrixDim = linearProblem.Count + numberOfCollision;
var frictionIndexes = linearProblem.ConstraintType
.Select((constraint, index) => new { constraint, index })
.Where(x => x.constraint == ConstraintType.Friction).ToList();
LinearProblemBaseProperties baseProperties = new LinearProblemBaseProperties(lcpMatrixDim);
var b = linearProblem.B.ToList();
var d = linearProblem.D.ToList();
var invD = linearProblem.InvD.ToList();
var constraintType = linearProblem.ConstraintType.ToList();
var constraintLimit = linearProblem.ConstraintLimit.ToList();
var m = linearProblem.GetOriginalSparseMatrix().Rows.ToList();
var constraintsArray = linearProblem.Constraints.ToList();
var startValue = linearProblem.StartImpulse.ToList();
var E_dim = numberOfCollision * EngineParameters.FrictionDirections;
var newRowLength = b.Count + numberOfCollision;
//Add fields to friction rows
for (int i = 0; i < numberOfCollision; i++)
{
var colIdx = collisionIndexes[i].index;
var fIndexes = constraintsArray.Select((constraint, index) => new { constraint, index })
.Where(x => x.constraint.HasValue && x.constraint.Value == colIdx).ToList();
//Add elements to matrix M, friction values
var rowIndexes = new List <int>()
{
collisionIndexes[i].index
};
var rowValues = new List <double>()
{
constraintLimit[frictionIndexes[0].index]
};
//Columns and rows update
for (int j = 0; j < fIndexes.Count; j++)
{
var idx = fIndexes[j].index;
var mIdx = m[idx];
var bufIndex = new List <int>(mIdx.Index);
var bufValue = new List <double>(mIdx.Value);
bufIndex.Add(mIdx.Length + i);
bufValue.Add(1.0);
m[idx] = new SparseVector(
bufValue.ToArray(),
bufIndex.ToArray(),
mIdx.Length + numberOfCollision);
rowIndexes.Add(idx);
rowValues.Add(-1.0);
}
//Rows Update
constraintType.Add(ConstraintType.FrictionValue);
b.Add(0.0);
d.Add(0.0);
invD.Add(0.0);
constraintLimit.Add(0.0);
startValue.Add(0.0);
var sparseElement = new SparseVector(
rowValues.ToArray(),
rowIndexes.ToArray(),
newRowLength);
m.Add(sparseElement);
}
for (int i = 0; i < m.Count; i++)
{
m[i] = new SparseVector(m[i].Value, m[i].Index, newRowLength);
b[i] *= -1.0;
}
baseProperties.B = b.ToArray();
baseProperties.D = d.ToArray();
baseProperties.InvD = invD.ToArray();
baseProperties.ConstraintType = constraintType.ToArray();
baseProperties.ConstraintLimit = constraintLimit.ToArray();
baseProperties.StartValue = startValue.ToArray();
baseProperties.M.Rows = m.ToArray();
var lcp = new LinearProblemProperties(baseProperties, null, EngineParameters.FrictionDirections);
//TODO Test
//var test = lcp.GetOriginalMatrix();
return(lcp);
}
return(linearProblem);
}