Direct-BT v3.3.0-1-gc2d430c
Direct-BT - Direct Bluetooth Programming.
test_math_mat4f_03_inv.cpp
Go to the documentation of this file.
1/*
2 * Author: Sven Gothel <sgothel@jausoft.com>
3 * Copyright (c) 2024 Gothel Software e.K.
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining
6 * a copy of this software and associated documentation files (the
7 * "Software"), to deal in the Software without restriction, including
8 * without limitation the rights to use, copy, modify, merge, publish,
9 * distribute, sublicense, and/or sell copies of the Software, and to
10 * permit persons to whom the Software is furnished to do so, subject to
11 * the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be
14 * included in all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23 */
24#include <thread>
25#include <cinttypes>
26#include <cstring>
27
29
30#include <jau/math/mat4f.hpp>
31
32using namespace jau;
33using namespace jau::math;
34
35static const float EPSILON = std::numeric_limits<float>::epsilon();
36
37static float* makeIdentity(float m[]) {
38 m[0+4*0] = 1;
39 m[1+4*0] = 0;
40 m[2+4*0] = 0;
41 m[3+4*0] = 0;
42
43 m[0+4*1] = 0;
44 m[1+4*1] = 1;
45 m[2+4*1] = 0;
46 m[3+4*1] = 0;
47
48 m[0+4*2] = 0;
49 m[1+4*2] = 0;
50 m[2+4*2] = 1;
51 m[3+4*2] = 0;
52
53 m[0+4*3] = 0;
54 m[1+4*3] = 0;
55 m[2+4*3] = 0;
56 m[3+4*3] = 1;
57 return m;
58}
59
60static float* invertMatrix(float msrc[], float mres[], float temp[/*4*4*/]) {
61 int i, j, k, swap;
62 float t;
63 for (i = 0; i < 4; i++) {
64 const int i4 = i*4;
65 for (j = 0; j < 4; j++) {
66 temp[i4+j] = msrc[i4+j];
67 }
68 }
69 makeIdentity(mres);
70
71 for (i = 0; i < 4; i++) {
72 const int i4 = i*4;
73
74 //
75 // Look for largest element in column
76 //
77 swap = i;
78 for (j = i + 1; j < 4; j++) {
79 if (std::abs(temp[j*4+i]) > std::abs(temp[i4+i])) {
80 swap = j;
81 }
82 }
83
84 if (swap != i) {
85 const int swap4 = swap*4;
86 //
87 // Swap rows.
88 //
89 for (k = 0; k < 4; k++) {
90 t = temp[i4+k];
91 temp[i4+k] = temp[swap4+k];
92 temp[swap4+k] = t;
93
94 t = mres[i4+k];
95 mres[i4+k] = mres[swap4+k];
96 mres[swap4+k] = t;
97 }
98 }
99
100 if (temp[i4+i] == 0) {
101 //
102 // No non-zero pivot. The matrix is singular, which shouldn't
103 // happen. This means the user gave us a bad matrix.
104 //
105 return nullptr;
106 }
107
108 t = temp[i4+i];
109 for (k = 0; k < 4; k++) {
110 temp[i4+k] /= t;
111 mres[i4+k] /= t;
112 }
113 for (j = 0; j < 4; j++) {
114 if (j != i) {
115 const int j4 = j*4;
116 t = temp[j4+i];
117 for (k = 0; k < 4; k++) {
118 temp[j4+k] -= temp[i4+k] * t;
119 mres[j4+k] -= mres[i4+k]*t;
120 }
121 }
122 }
123 }
124 return mres;
125}
126
127static void testImpl(float matrix[]) {
128 float inv1_0[16];
129 float inv2_0[16];
130 float temp[16];
131
132 // System.err.println(FloatUtil.matrixToString(null, "orig : ", "%10.7f", matrix, 0, 4, 4, false /* rowMajorOrder */));
133 invertMatrix(matrix, inv1_0, temp);
134 invertMatrix(inv1_0, inv2_0, temp);
135 // System.err.println(FloatUtil.matrixToString(null, "inv1_0: ", "%10.7f", inv1_0, 0, 4, 4, false /* rowMajorOrder */));
136 // System.err.println(FloatUtil.matrixToString(null, "inv2_0: ", "%10.7f", inv2_0, 0, 4, 4, false /* rowMajorOrder */));
137
138 COMPARE_NARRAYS_EPS(matrix, inv2_0, 16, EPSILON);
139
140 //
141 // Mat4f
142 //
143
144 Mat4f matrix_m(matrix);
145 Mat4f inv1_4a(matrix_m);
146 REQUIRE( true == inv1_4a.invert() );
147 Mat4f inv2_4a(inv1_4a);
148 REQUIRE( true == inv2_4a.invert() );
149
150 {
151 Mat4f a(inv1_0), b( inv1_4a.get(temp) );
152 std::cout << "A: " << a << std::endl;
153 std::cout << "B: " << b << std::endl;
154 }
155 COMPARE_NARRAYS_EPS(inv1_0, inv1_4a.get(temp), 16, Mat4f::inv_deviation);
156 COMPARE_NARRAYS_EPS(inv2_0, inv2_4a.get(temp), 16, Mat4f::inv_deviation);
157 REQUIRE_MSG( "I4 failure: "+matrix_m.toString()+" != "+inv2_4a.toString(), matrix_m.equals(inv2_4a, Mat4f::inv_deviation));
158
159 Mat4f inv1_4b;
160 REQUIRE( true == inv1_4b.invert(matrix_m) );
161 Mat4f inv2_4b;
162 REQUIRE( true == inv2_4b.invert(inv1_4b) );
163
164 // Assert.assertEquals(new Mat4f(inv1_2), inv1_4b);
165 // Assert.assertEquals(new Mat4f(inv2_2), inv2_4b);
166 COMPARE_NARRAYS_EPS(inv1_0, inv1_4b.get(temp), 16, Mat4f::inv_deviation);
167 COMPARE_NARRAYS_EPS(inv2_0, inv2_4b.get(temp), 16, Mat4f::inv_deviation);
168 REQUIRE_MSG( "I4 failure: "+matrix_m.toString()+" != "+inv2_4b.toString(), matrix_m.equals(inv2_4b, Mat4f::inv_deviation));
169}
170
171TEST_CASE( "Test 02", "[mat4f][linear_algebra][math]" ) {
172 float p[] = { 2.3464675f, 0, 0, 0,
173 0, 2.4142134f, 0, 0,
174 0, 0, -1.0002f, -1,
175 0, 0, -20.002f, 0 };
176 testImpl(p);
177}
178
179TEST_CASE( "Test 03", "[mat4f][linear_algebra][math]" ) {
180 float mv[] = { 1, 0, 0, 0,
181 0, 1, 0, 0,
182 0, 0, 1, 0,
183 0, 0, -200, 1 } ;
184 testImpl(mv);
185}
186
187TEST_CASE( "Test 04", "[mat4f][linear_algebra][math]" ) {
188 float p[] = { 2.3464675f, 0, 0, 0,
189 0, 2.4142134f, 0, 0,
190 0, 0, -1.0002f, -1,
191 0, 0, -20.002f, 0 };
192
193 testImpl(p);
194}
195
196TEST_CASE( "Test 05 Perf01", "[mat4f][linear_algebra][math]" ) {
197 float p1[] = { 2.3464675f, 0, 0, 0,
198 0, 2.4142134f, 0, 0,
199 0, 0, -1.0002f, -1,
200 0, 0, -20.002f, 0 };
201 Mat4f p1_m(p1);
202
203 float p2[] = { 26, 59, 143, 71,
204 59, 174, 730, 386,
205 143, 730, 9770, 5370,
206 71, 386, 5370, 2954 };
207 Mat4f p2_m(p2);
208
209 Mat4f res_m;
210
211 const size_t warmups = 1000_u64;
212 const size_t loops = 10_u64*1000000_u64;
215
216 // avoid optimizing out unused computation results by simply adding up determinat
217 double dr = 1;
218
219 //
220 // Mat4f
221 //
222
223 // warm-up
224 for(size_t i=0; i<warmups; i++) {
225 res_m.invert(p1_m);
226 dr += res_m.determinant();
227 res_m.invert(p2_m);
228 dr += res_m.determinant();
229 }
231 for(size_t i=0; i<loops; i++) {
232 res_m.invert(p1_m);
233 dr += res_m.determinant();
234 res_m.invert(p2_m);
235 dr += res_m.determinant();
236 }
237 tI4a = (getMonotonicTime() - t_0).to_fraction_i64();
238 REQUIRE( false == jau::is_zero(dr) );
239
240 // warm-up
241 for(size_t i=0; i<warmups; i++) {
242 res_m.load(p1_m).invert();
243 dr += res_m.determinant();
244 res_m.load(p2_m).invert();
245 dr += res_m.determinant();
246 }
247
248 t_0 = jau::getMonotonicTime();
249 for(size_t i=0; i<loops; i++) {
250 res_m.load(p1_m).invert();
251 dr += res_m.determinant();
252 res_m.load(p2_m).invert();
253 dr += res_m.determinant();
254 }
255 tI4b = (getMonotonicTime() - t_0).to_fraction_i64();
256 REQUIRE( false == jau::is_zero(dr) );
257
258 printf("Checkmark %f\n", dr);
259 printf("Summary loops %6zu: I4a %6s ms total (%s us), %f ns/inv, I4a / I4b %f%%\n", loops,
260 jau::to_decstring(tI4a.to_ms()).c_str(), jau::to_decstring(tI4a.to_us()).c_str(),
261 (double)tI4a.to_ns()/2.0/(double)loops, tI4a.to_double()/tI4b.to_double()*100.0);
262 printf("Summary loops %6zu: I4b %6s ms total (%s us), %f ns/inv, I4b / I4a %f%%\n", loops,
263 jau::to_decstring(tI4b.to_ms()).c_str(), jau::to_decstring(tI4b.to_us()).c_str(),
264 (double)tI4b.to_ns()/2.0/(double)loops, tI4b.to_double()/tI4a.to_double()*100.0);
265}
266
267
#define REQUIRE_MSG(MSG,...)
Definition: catch2_ext.hpp:58
#define COMPARE_NARRAYS_EPS(lhs, rhs, len, eps)
Definition: catch2_ext.hpp:102
constexpr double to_double() const noexcept
Returns the converted fraction to lossy double.
constexpr int_type to_ms() const noexcept
Convenient shortcut to to_num_of(1_ms)
constexpr int_type to_us() const noexcept
Convenient shortcut to to_num_of(1_us)
constexpr int_type to_ns() const noexcept
Convenient shortcut to to_num_of(1_ns)
Basic 4x4 value_type matrix implementation using fields for intensive use-cases (host operations).
Definition: mat4f.hpp:112
constexpr value_type get(const jau::nsize_t i) const noexcept
Returns the ith component of the given column-major order matrix, 0 <= i < 16, w/o boundary check.
Definition: mat4f.hpp:306
bool invert() noexcept
Invert this matrix.
Definition: mat4f.hpp:518
value_type determinant() const noexcept
Returns the determinant of this matrix.
Definition: mat4f.hpp:438
constexpr bool equals(const Matrix4 &o, const value_type epsilon=std::numeric_limits< value_type >::epsilon()) const noexcept
Definition: mat4f.hpp:194
std::string toString(const std::string &rowPrefix, const std::string &f) const noexcept
Returns a formatted string representation of this matrix.
Definition: mat4f.hpp:1841
constexpr Matrix4 & load(const_iterator src) noexcept
Load the values of the given matrix src to this matrix w/o boundary check.
Definition: mat4f.hpp:264
void swap(cow_darray< Value_type, Size_type, Alloc_type > &rhs, cow_darray< Value_type, Size_type, Alloc_type > &lhs) noexcept
std::enable_if< std::is_floating_point_v< T >, bool >::type constexpr is_zero(const T &a, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if the given value is less than epsilon, w/ epsilon > 0.
Definition: float_math.hpp:255
bool to_fraction_i64(fraction_i64 &result, const std::string &value, const fraction_i64 &min_allowed, const fraction_i64 &max_allowed) noexcept
Stores the fraction_i64 value of the given string value in format <num>/<denom>, which may contain wh...
fraction_timespec getMonotonicTime() noexcept
Returns current monotonic time since Unix Epoch 00:00:00 UTC on 1970-01-01.
Definition: basic_types.cpp:52
constexpr T abs(const T x) noexcept
Returns the absolute value of an arithmetic number (w/ branching) in O(1)
Definition: base_math.hpp:155
std::string to_decstring(const value_type &v, const char separator=',', const nsize_t width=0) noexcept
Produce a decimal string representation of an integral integer value.
constexpr const jau::fraction_i64 zero(0l, 1lu)
zero is 0/1
__pack(...): Produces MSVC, clang and gcc compatible lead-in and -out macros.
Definition: backtrace.hpp:32
Timespec structure using int64_t for its components in analogy to struct timespec_t on 64-bit platfor...
static void testImpl(float matrix[])
static float * makeIdentity(float m[])
TEST_CASE("Test 02", "[mat4f][linear_algebra][math]")
static const float EPSILON
static float * invertMatrix(float msrc[], float mres[], float temp[])
static int loops
int printf(const char *format,...)
Operating Systems predefined macros.