Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
GaussianElimination.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
8
18template <class MatrixA, class MatrixB>
19bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance = 1.0e-16f)
20{
21 // Get problem dimensions
22 const uint n = ioA.GetCols();
23 const uint m = ioB.GetCols();
24
25 // Check matrix requirement
26 JPH_ASSERT(ioA.GetRows() == n);
27 JPH_ASSERT(ioB.GetRows() == n);
28
29 // Create array for bookkeeping on pivoting
30 int *ipiv = (int *)JPH_STACK_ALLOC(n * sizeof(int));
31 memset(ipiv, 0, n * sizeof(int));
32
33 for (uint i = 0; i < n; ++i)
34 {
35 // Initialize pivot element as the diagonal
36 uint pivot_row = i, pivot_col = i;
37
38 // Determine pivot element
39 float largest_element = 0.0f;
40 for (uint j = 0; j < n; ++j)
41 if (ipiv[j] != 1)
42 for (uint k = 0; k < n; ++k)
43 {
44 if (ipiv[k] == 0)
45 {
46 float element = abs(ioA(j, k));
47 if (element >= largest_element)
48 {
49 largest_element = element;
50 pivot_row = j;
51 pivot_col = k;
52 }
53 }
54 else if (ipiv[k] > 1)
55 {
56 return false;
57 }
58 }
59
60 // Mark this column as used
61 ++ipiv[pivot_col];
62
63 // Exchange rows when needed so that the pivot element is at ioA(pivot_col, pivot_col) instead of at ioA(pivot_row, pivot_col)
64 if (pivot_row != pivot_col)
65 {
66 for (uint j = 0; j < n; ++j)
67 swap(ioA(pivot_row, j), ioA(pivot_col, j));
68 for (uint j = 0; j < m; ++j)
69 swap(ioB(pivot_row, j), ioB(pivot_col, j));
70 }
71
72 // Get diagonal element that we are about to set to 1
73 float diagonal_element = ioA(pivot_col, pivot_col);
74 if (abs(diagonal_element) < inTolerance)
75 return false;
76
77 // Divide the whole row by the pivot element, making ioA(pivot_col, pivot_col) = 1
78 for (uint j = 0; j < n; ++j)
79 ioA(pivot_col, j) /= diagonal_element;
80 for (uint j = 0; j < m; ++j)
81 ioB(pivot_col, j) /= diagonal_element;
82 ioA(pivot_col, pivot_col) = 1.0f;
83
84 // Next reduce the rows, except for the pivot one,
85 // after this step the pivot_col column is zero except for the pivot element which is 1
86 for (uint j = 0; j < n; ++j)
87 if (j != pivot_col)
88 {
89 float element = ioA(j, pivot_col);
90 for (uint k = 0; k < n; ++k)
91 ioA(j, k) -= ioA(pivot_col, k) * element;
92 for (uint k = 0; k < m; ++k)
93 ioB(j, k) -= ioB(pivot_col, k) * element;
94 ioA(j, pivot_col) = 0.0f;
95 }
96 }
97
98 // Success
99 return true;
100}
101
#define JPH_STACK_ALLOC(n)
Definition: Core.h:479
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
JPH_NAMESPACE_BEGIN bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance=1.0e-16f)
Definition: GaussianElimination.h:19
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33