1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
// cs/renderer/linalg/matrix4x4.cc
#include "cs/renderer/linalg/matrix4x4.hh"
#include "cs/renderer/numbers/in_range.hh"
#include "cs/result.hh"
using ::cs::Error;
using ::cs::Ok;
using ::cs::Result;
using ::cs::ResultOr;
using ::cs::numbers::in_range;
using ::cs::renderer::linalg::Matrix4x4;
#include <memory.h>
#include <stdint.h>
#include "matrix4x4.hh"
namespace {
Result EnsureIndexArgs(uint8_t x, uint8_t y) {
if (!in_range<uint8_t>(x, 0, 4)) {
return TRACE(Error("x is out of range"));
}
if (!in_range<uint8_t>(y, 0, 4)) {
return TRACE(Error("y is out of range"));
}
return Ok();
}
} // namespace
ResultOr<float> cs::renderer::linalg::Matrix4x4::get(
uint8_t x, uint8_t y) const {
OK_OR_RET(EnsureIndexArgs(x, y));
return data_[x][y];
}
Result cs::renderer::linalg::Matrix4x4::set(uint8_t x,
uint8_t y,
float datum) {
OK_OR_RET(EnsureIndexArgs(x, y));
data_[x][y] = datum;
return Ok();
}
// Pulled from PBRT:
// https://github.com/mmp/pbrt-v3/blob/aaa552a4b9cbf9dccb71450f47b268e0ed6370e2/src/core/transform.cpp#L82-L136
ResultOr<Matrix4x4>
cs::renderer::linalg::Matrix4x4::inverse() const {
int indxc[4], indxr[4];
int ipiv[4] = {0, 0, 0, 0};
float minv[4][4];
memcpy(minv, data_, 4 * 4 * sizeof(float));
for (int i = 0; i < 4; i++) {
int irow = 0, icol = 0;
float big = 0.f;
// Choose pivot
for (int j = 0; j < 4; j++) {
if (ipiv[j] != 1) {
for (int k = 0; k < 4; k++) {
if (ipiv[k] == 0) {
if (std::abs(minv[j][k]) >= big) {
big = float(std::abs(minv[j][k]));
irow = j;
icol = k;
}
} else if (ipiv[k] > 1) {
return TRACE( // LCOV_EXCL_LINE
Error("Singular matrix in MatrixInvert"));
}
}
}
}
++ipiv[icol];
// Swap rows _irow_ and _icol_ for pivot
if (irow != icol) {
for (int k = 0; k < 4; ++k)
std::swap(minv[irow][k], minv[icol][k]);
}
indxr[i] = irow;
indxc[i] = icol;
if (minv[icol][icol] == 0.f) {
return TRACE( // LCOV_EXCL_LINE
Error("Singular matrix in MatrixInvert"));
}
// Set $m[icol][icol]$ to one by scaling row _icol_
// appropriately
float pivinv = 1. / minv[icol][icol];
minv[icol][icol] = 1.;
for (int j = 0; j < 4; j++) minv[icol][j] *= pivinv;
// Subtract this row from others to zero out their
// columns
for (int j = 0; j < 4; j++) {
if (j != icol) {
float save = minv[j][icol];
minv[j][icol] = 0;
for (int k = 0; k < 4; k++)
minv[j][k] -= minv[icol][k] * save;
}
}
}
// Swap columns to reflect permutation
for (int j = 3; j >= 0; j--) {
if (indxr[j] != indxc[j]) {
for (int k = 0; k < 4; k++)
std::swap(minv[k][indxr[j]], minv[k][indxc[j]]);
}
}
return Matrix4x4(minv);
}
Matrix4x4 cs::renderer::linalg::Matrix4x4::transpose()
const {
// clang-format off
return Matrix4x4(
data_[0][0], data_[1][0], data_[2][0], data_[3][0],
data_[0][1], data_[1][1], data_[2][1], data_[3][1],
data_[0][2], data_[1][2], data_[2][2], data_[3][2],
data_[0][3], data_[1][3], data_[2][3], data_[3][3]);
// clang-format on
}