Line data Source code
1 : #pragma once
2 : #include "vec3.hpp"
3 : #include "quat.hpp"
4 : #include <iomanip>
5 :
6 : namespace m3d
7 : {
8 :
9 : struct mat3
10 : {
11 : vec3 cols[3]; // Column-major
12 :
13 112439 : mat3()
14 449756 : {
15 112439 : cols[0] = {1, 0, 0};
16 112439 : cols[1] = {0, 1, 0};
17 112439 : cols[2] = {0, 0, 1};
18 112439 : }
19 :
20 44789 : mat3(scalar m00, scalar m01, scalar m02,
21 : scalar m10, scalar m11, scalar m12,
22 : scalar m20, scalar m21, scalar m22)
23 179156 : {
24 44789 : cols[0] = {m00, m10, m20};
25 44789 : cols[1] = {m01, m11, m21};
26 44789 : cols[2] = {m02, m12, m22};
27 44789 : }
28 :
29 337356 : vec3 &operator[](int i) { return cols[i]; }
30 541801 : const vec3 &operator[](int i) const { return cols[i]; }
31 :
32 268624 : vec3 operator*(const vec3 &v) const
33 : {
34 : return {
35 268624 : cols[0].x * v.x + cols[1].x * v.y + cols[2].x * v.z,
36 268624 : cols[0].y * v.x + cols[1].y * v.y + cols[2].y * v.z,
37 268624 : cols[0].z * v.x + cols[1].z * v.y + cols[2].z * v.z};
38 : }
39 :
40 44772 : mat3 operator*(const mat3 &rhs) const
41 : {
42 44772 : mat3 res;
43 179088 : for (int c = 0; c < 3; c++)
44 : {
45 134316 : res[c] = (*this) * rhs[c];
46 : }
47 44772 : return res;
48 : }
49 :
50 2 : bool operator==(const mat3 &other) const
51 : {
52 2 : return cols[0] == other[0] && cols[1] == other[1] && cols[2] == other[2];
53 : }
54 :
55 4 : bool is_approx(const mat3 &other, scalar precision = EPSILON) const
56 : {
57 7 : return cols[0].is_approx(other[0], precision) &&
58 7 : cols[1].is_approx(other[1], precision) &&
59 7 : cols[2].is_approx(other[2], precision);
60 : }
61 :
62 44770 : mat3 transpose() const
63 : {
64 : return mat3(
65 44770 : cols[0].x, cols[0].y, cols[0].z,
66 44770 : cols[1].x, cols[1].y, cols[1].z,
67 44770 : cols[2].x, cols[2].y, cols[2].z);
68 : }
69 :
70 8 : scalar determinant() const
71 : {
72 8 : return cols[0].x * (cols[1].y * cols[2].z - cols[1].z * cols[2].y) -
73 8 : cols[1].x * (cols[0].y * cols[2].z - cols[0].z * cols[2].y) +
74 8 : cols[2].x * (cols[0].y * cols[1].z - cols[0].z * cols[1].y);
75 : }
76 :
77 5 : mat3 inverse() const
78 : {
79 5 : scalar det = this->determinant();
80 5 : if (m3d::abs(det) < EPSILON)
81 1 : return mat3(); // Return identity on failure
82 :
83 4 : scalar invDet = 1.0 / det;
84 4 : mat3 res;
85 4 : res[0].x = (cols[1].y * cols[2].z - cols[1].z * cols[2].y) * invDet;
86 4 : res[0].y = (cols[0].z * cols[2].y - cols[0].y * cols[2].z) * invDet;
87 4 : res[0].z = (cols[0].y * cols[1].z - cols[0].z * cols[1].y) * invDet;
88 4 : res[1].x = (cols[1].z * cols[2].x - cols[1].x * cols[2].z) * invDet;
89 4 : res[1].y = (cols[0].x * cols[2].z - cols[0].z * cols[2].x) * invDet;
90 4 : res[1].z = (cols[1].x * cols[0].z - cols[0].x * cols[1].z) * invDet;
91 4 : res[2].x = (cols[1].x * cols[2].y - cols[1].y * cols[2].x) * invDet;
92 4 : res[2].y = (cols[0].y * cols[2].x - cols[0].x * cols[2].y) * invDet;
93 4 : res[2].z = (cols[0].x * cols[1].y - cols[1].x * cols[0].y) * invDet;
94 4 : return res;
95 : }
96 :
97 : friend std::ostream &operator<<(std::ostream &os, const mat3 &m)
98 : {
99 : os << "\n";
100 : for (int r = 0; r < 3; r++)
101 : {
102 : os << "| " << std::setw(8) << m[0][r] << " "
103 : << std::setw(8) << m[1][r] << " "
104 : << std::setw(8) << m[2][r] << " |\n";
105 : }
106 : return os;
107 : }
108 : };
109 :
110 22890 : inline mat3 mat3_cast(const quat &q)
111 : {
112 22890 : mat3 m;
113 22890 : scalar x2 = q.x + q.x, y2 = q.y + q.y, z2 = q.z + q.z;
114 22890 : scalar xx = q.x * x2, xy = q.x * y2, xz = q.x * z2;
115 22890 : scalar yy = q.y * y2, yz = q.y * z2, zz = q.z * z2;
116 22890 : scalar wx = q.w * x2, wy = q.w * y2, wz = q.w * z2;
117 :
118 22890 : m[0] = {1 - (yy + zz), xy + wz, xz - wy};
119 22890 : m[1] = {xy - wz, 1 - (xx + zz), yz + wx};
120 22890 : m[2] = {xz + wy, yz - wx, 1 - (xx + yy)};
121 22890 : return m;
122 : }
123 :
124 : struct smat3
125 : {
126 : scalar xx, yy, zz;
127 : scalar xy, xz, yz;
128 :
129 1091 : smat3() : xx(0), yy(0), zz(0), xy(0), xz(0), yz(0) {}
130 15728 : smat3(scalar xx, scalar yy, scalar zz, scalar xy, scalar xz, scalar yz)
131 15728 : : xx(xx), yy(yy), zz(zz), xy(xy), xz(xz), yz(yz) {}
132 :
133 44770 : explicit smat3(const mat3 &m)
134 44770 : : xx(m[0].x), yy(m[1].y), zz(m[2].z), xy(0.5 * (m[1].x + m[0].y)) // Average off-diagonal elements
135 : ,
136 44770 : xz(0.5 * (m[2].x + m[0].z)), yz(0.5 * (m[2].y + m[1].z))
137 : {
138 44770 : }
139 :
140 44767 : vec3 operator*(const vec3 &v) const
141 : {
142 : return {
143 44767 : xx * v.x + xy * v.y + xz * v.z,
144 44767 : xy * v.x + yy * v.y + yz * v.z,
145 44767 : xz * v.x + yz * v.y + zz * v.z};
146 : }
147 :
148 15028 : smat3 operator*(scalar s) const
149 : {
150 15028 : return smat3(xx * s, yy * s, zz * s, xy * s, xz * s, yz * s);
151 : }
152 :
153 97 : scalar determinant() const
154 : {
155 97 : return xx * yy * zz + 2 * xy * xz * yz - xx * yz * yz - yy * xz * xz - zz * xy * xy;
156 : }
157 :
158 95 : smat3 inverse() const
159 : {
160 95 : scalar det = determinant();
161 :
162 95 : if (m3d::abs(det) < EPSILON)
163 1 : return smat3(); // return zero matrix on failure
164 :
165 94 : scalar invDet = 1.0 / det;
166 :
167 94 : smat3 result;
168 :
169 : // Diagonal cofactors
170 94 : result.xx = (yy * zz - yz * yz) * invDet;
171 94 : result.yy = (xx * zz - xz * xz) * invDet;
172 94 : result.zz = (xx * yy - xy * xy) * invDet;
173 :
174 : // Off-diagonal cofactors (note symmetry preserved)
175 94 : result.xy = (xz * yz - xy * zz) * invDet;
176 94 : result.xz = (xy * yz - xz * yy) * invDet;
177 94 : result.yz = (xy * xz - xx * yz) * invDet;
178 :
179 94 : return result;
180 : }
181 :
182 4 : bool is_approx(const smat3 &o, scalar p = EPSILON) const
183 : {
184 10 : return m3d::abs(xx - o.xx) < p && m3d::abs(yy - o.yy) < p && m3d::abs(zz - o.zz) < p &&
185 10 : m3d::abs(xy - o.xy) < p && m3d::abs(xz - o.xz) < p && m3d::abs(yz - o.yz) < p;
186 : }
187 :
188 : friend std::ostream &operator<<(std::ostream &os, const smat3 &m)
189 : {
190 : return os << "smat3(diag: " << m.xx << "," << m.yy << "," << m.zz
191 : << " off: " << m.xy << "," << m.xz << "," << m.yz << ")";
192 : }
193 : };
194 :
195 : // Free function for scalar * smat3 (commutative)
196 15027 : inline smat3 operator*(scalar s, const smat3 &m)
197 : {
198 15027 : return m * s;
199 : }
200 :
201 : // mat3 * smat3 multiplication
202 44769 : inline mat3 operator*(const mat3 &lhs, const smat3 &rhs)
203 : {
204 : // Expand smat3 to full matrix and multiply
205 : // rhs as matrix is:
206 : // | xx xy xz |
207 : // | xy yy yz |
208 : // | xz yz zz |
209 :
210 44769 : mat3 result;
211 : // Each column of result = lhs * column of rhs
212 44769 : result[0] = lhs * vec3{rhs.xx, rhs.xy, rhs.xz};
213 44769 : result[1] = lhs * vec3{rhs.xy, rhs.yy, rhs.yz};
214 44769 : result[2] = lhs * vec3{rhs.xz, rhs.yz, rhs.zz};
215 44769 : return result;
216 : }
217 :
218 : }
|
