godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-12-05 17:15:09 +00:00
Code Releases Wiki Activity

bullet: Sync with upstream 3.25

Browse Source 
Remove upstreamed patch.
This commit is contained in:
Rémi Verschelde
2023-05-22 15:10:36 +02:00
parent 1538b870f1
commit b958e8a236
12 changed files with 125 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp vendored
View File

@@ -169,9 +169,12 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
/*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
bool isRayTermination = false;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex, z0Row, isRayTermination);
if (isRayTermination)
{
break; // ray termination
}
if (z0Row == pivotRowIndex)
{ //if z0 leaves the basis the solution is found --> one last elimination step is necessary
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
@@ -217,79 +220,100 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
return solutionVector;
}
int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination)
{
int RowIndex = 0;
isRayTermination = false;
btAlignedObjectArray<int> activeRows;
bool firstRow = true;
btScalar currentMin = 0.0;
int dim = A.rows();
btAlignedObjectArray<btVectorXu> Rows;
for (int row = 0; row < dim; row++)
{
btVectorXu vec(dim + 1);
vec.setZero(); //, INIT, 0.)
Rows.push_back(vec);
btScalar a = A(row, pivotColIndex);
if (a > 0)
{
Rows[row][0] = A(row, 2 * dim + 1) / a;
Rows[row][1] = A(row, 2 * dim) / a;
for (int j = 2; j < dim + 1; j++)
Rows[row][j] = A(row, j - 1) / a;
const btScalar denom = A(row, pivotColIndex);
#ifdef BT_DEBUG_OSTREAM
// if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl;
// }
#endif
if (denom > btMachEps())
{
const btScalar q = A(row, dim + dim + 1) / denom;
if (firstRow)
{
currentMin = q;
activeRows.push_back(row);
firstRow = false;
}
else if (fabs(currentMin - q) < btMachEps())
{
activeRows.push_back(row);
}
else if (currentMin > q)
{
currentMin = q;
activeRows.clear();
activeRows.push_back(row);
}
}
}
for (int i = 0; i < Rows.size(); i++)
if (activeRows.size() == 0)
{
if (Rows[i].nrm2() > 0.)
isRayTermination = true;
return 0;
}
else if (activeRows.size() == 1)
{
return activeRows[0];
}
// if there are multiple rows, check if they contain the row for z_0.
for (int i = 0; i < activeRows.size(); i++)
{
if (activeRows[i] == z0Row)
{
int j = 0;
for (; j < Rows.size(); j++)
{
if (i != j)
{
if (Rows[j].nrm2() > 0.)
{
btVectorXu test(dim + 1);
for (int ii = 0; ii < dim + 1; ii++)
{
test[ii] = Rows[j][ii] - Rows[i][ii];
}
//=Rows[j] - Rows[i]
if (!LexicographicPositive(test))
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
return z0Row;
}
}
return RowIndex;
}
// look through the columns of the inverse of the basic matrix from left to right until the tie is broken.
for (int col = 0; col < dim ; col++)
{
btAlignedObjectArray<int> activeRowsCopy(activeRows);
activeRows.clear();
firstRow = true;
for (int i = 0; i<activeRowsCopy.size();i++)
{
const int row = activeRowsCopy[i];
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
{
int i = 0;
// if (DEBUGLEVEL)
// cout << "v " << v << endl;
// denom is positive here as an invariant.
const btScalar denom = A(row, pivotColIndex);
const btScalar ratio = A(row, col) / denom;
if (firstRow)
{
currentMin = ratio;
activeRows.push_back(row);
firstRow = false;
}
else if (fabs(currentMin - ratio) < btMachEps())
{
activeRows.push_back(row);
}
else if (currentMin > ratio)
{
currentMin = ratio;
activeRows.clear();
activeRows.push_back(row);
}
}
while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
i++;
if (v[i] > 0)
return true;
return false;
if (activeRows.size() == 1)
{
return activeRows[0];
}
}
// must not reach here.
isRayTermination = true;
return 0;
}
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)