forked from dgruss/ustl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ulaalgo.h
72 lines (64 loc) · 1.95 KB
/
ulaalgo.h
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
// This file is part of the uSTL library, an STL implementation.
//
// Copyright (c) 2005 by Mike Sharov <[email protected]>
// This file is free software, distributed under the MIT License.
#pragma once
#include "umatrix.h"
namespace ustl {
/// \brief Creates an identity matrix in \p m
/// \ingroup NumericAlgorithms
template <size_t NX, size_t NY, typename T>
void load_identity (matrix<NX,NY,T>& m)
{
fill_n (m.begin(), NX * NY, 0);
for (typename matrix<NX,NY,T>::iterator i = m.begin(); i < m.end(); i += NX + 1)
*i = 1;
}
/// \brief Multiplies two matrices
/// \ingroup NumericAlgorithms
template <size_t NX, size_t NY, typename T>
matrix<NY,NY,T> operator* (const matrix<NX,NY,T>& m1, const matrix<NY,NX,T>& m2)
{
matrix<NY,NY,T> mr;
for (uoff_t ry = 0; ry < NY; ++ ry) {
for (uoff_t rx = 0; rx < NY; ++ rx) {
T dpv (0);
for (uoff_t x = 0; x < NX; ++ x)
dpv += m1[ry][x] * m2[x][rx];
mr[ry][rx] = dpv;
}
}
return mr;
}
/// \brief Transforms vector \p t with matrix \p m
/// \ingroup NumericAlgorithms
template <size_t NX, size_t NY, typename T>
tuple<NX,T> operator* (const tuple<NY,T>& t, const matrix<NX,NY,T>& m)
{
tuple<NX,T> tr;
for (uoff_t x = 0; x < NX; ++ x) {
T dpv (0);
for (uoff_t y = 0; y < NY; ++ y)
dpv += t[y] * m[y][x];
tr[x] = dpv;
}
return tr;
}
/// \brief Transposes (exchanges rows and columns) matrix \p m.
/// \ingroup NumericAlgorithms
template <size_t N, typename T>
void transpose (matrix<N,N,T>& m)
{
for (uoff_t x = 0; x < N; ++ x)
for (uoff_t y = x; y < N; ++ y)
swap (m[x][y], m[y][x]);
}
/// Specialization for 4-component vector transform, the slow part of 3D graphics.
template <> inline tuple<4,float> operator* (const tuple<4,float>& t, const matrix<4,4,float>& m)
{
tuple<4,float> tr;
for (uoff_t i = 0; i < 4; ++ i)
tr[i] = t[0] * m[0][i] + t[1] * m[1][i] + t[2] * m[2][i] + t[3] * m[3][i];
return tr;
}
} // namespace ustl