1// Copyright 2016 The SwiftShader Authors. All Rights Reserved. 2// 3// Licensed under the Apache License, Version 2.0 (the "License"); 4// you may not use this file except in compliance with the License. 5// You may obtain a copy of the License at 6// 7// http://www.apache.org/licenses/LICENSE-2.0 8// 9// Unless required by applicable law or agreed to in writing, software 10// distributed under the License is distributed on an "AS IS" BASIS, 11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12// See the License for the specific language governing permissions and 13// limitations under the License. 14 15#include "Matrix.hpp" 16 17#include "Point.hpp" 18#include "Math.hpp" 19 20namespace sw 21{ 22 Matrix Matrix::diag(float m11, float m22, float m33, float m44) 23 { 24 return Matrix(m11, 0, 0, 0, 25 0, m22, 0, 0, 26 0, 0, m33, 0, 27 0, 0, 0, m44); 28 } 29 30 Matrix::operator float*() 31 { 32 return &(*this)(1, 1); 33 } 34 35 Matrix Matrix::operator+() const 36 { 37 return *this; 38 } 39 40 Matrix Matrix::operator-() const 41 { 42 const Matrix &M = *this; 43 44 return Matrix(-M(1, 1), -M(1, 2), -M(1, 3), -M(1, 4), 45 -M(2, 1), -M(2, 2), -M(2, 3), -M(2, 4), 46 -M(3, 1), -M(3, 2), -M(3, 3), -M(3, 4), 47 -M(4, 1), -M(4, 2), -M(4, 3), -M(4, 4)); 48 } 49 50 Matrix Matrix::operator!() const 51 { 52 const Matrix &M = *this; 53 Matrix I; 54 55 float M3344 = M(3, 3) * M(4, 4) - M(4, 3) * M(3, 4); 56 float M2344 = M(2, 3) * M(4, 4) - M(4, 3) * M(2, 4); 57 float M2334 = M(2, 3) * M(3, 4) - M(3, 3) * M(2, 4); 58 float M3244 = M(3, 2) * M(4, 4) - M(4, 2) * M(3, 4); 59 float M2244 = M(2, 2) * M(4, 4) - M(4, 2) * M(2, 4); 60 float M2234 = M(2, 2) * M(3, 4) - M(3, 2) * M(2, 4); 61 float M3243 = M(3, 2) * M(4, 3) - M(4, 2) * M(3, 3); 62 float M2243 = M(2, 2) * M(4, 3) - M(4, 2) * M(2, 3); 63 float M2233 = M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3); 64 float M1344 = M(1, 3) * M(4, 4) - M(4, 3) * M(1, 4); 65 float M1334 = M(1, 3) * M(3, 4) - M(3, 3) * M(1, 4); 66 float M1244 = M(1, 2) * M(4, 4) - M(4, 2) * M(1, 4); 67 float M1234 = M(1, 2) * M(3, 4) - M(3, 2) * M(1, 4); 68 float M1243 = M(1, 2) * M(4, 3) - M(4, 2) * M(1, 3); 69 float M1233 = M(1, 2) * M(3, 3) - M(3, 2) * M(1, 3); 70 float M1324 = M(1, 3) * M(2, 4) - M(2, 3) * M(1, 4); 71 float M1224 = M(1, 2) * M(2, 4) - M(2, 2) * M(1, 4); 72 float M1223 = M(1, 2) * M(2, 3) - M(2, 2) * M(1, 3); 73 74 // Adjoint Matrix 75 I(1, 1) = M(2, 2) * M3344 - M(3, 2) * M2344 + M(4, 2) * M2334; 76 I(2, 1) = -M(2, 1) * M3344 + M(3, 1) * M2344 - M(4, 1) * M2334; 77 I(3, 1) = M(2, 1) * M3244 - M(3, 1) * M2244 + M(4, 1) * M2234; 78 I(4, 1) = -M(2, 1) * M3243 + M(3, 1) * M2243 - M(4, 1) * M2233; 79 80 I(1, 2) = -M(1, 2) * M3344 + M(3, 2) * M1344 - M(4, 2) * M1334; 81 I(2, 2) = M(1, 1) * M3344 - M(3, 1) * M1344 + M(4, 1) * M1334; 82 I(3, 2) = -M(1, 1) * M3244 + M(3, 1) * M1244 - M(4, 1) * M1234; 83 I(4, 2) = M(1, 1) * M3243 - M(3, 1) * M1243 + M(4, 1) * M1233; 84 85 I(1, 3) = M(1, 2) * M2344 - M(2, 2) * M1344 + M(4, 2) * M1324; 86 I(2, 3) = -M(1, 1) * M2344 + M(2, 1) * M1344 - M(4, 1) * M1324; 87 I(3, 3) = M(1, 1) * M2244 - M(2, 1) * M1244 + M(4, 1) * M1224; 88 I(4, 3) = -M(1, 1) * M2243 + M(2, 1) * M1243 - M(4, 1) * M1223; 89 90 I(1, 4) = -M(1, 2) * M2334 + M(2, 2) * M1334 - M(3, 2) * M1324; 91 I(2, 4) = M(1, 1) * M2334 - M(2, 1) * M1334 + M(3, 1) * M1324; 92 I(3, 4) = -M(1, 1) * M2234 + M(2, 1) * M1234 - M(3, 1) * M1224; 93 I(4, 4) = M(1, 1) * M2233 - M(2, 1) * M1233 + M(3, 1) * M1223; 94 95 // Division by determinant 96 I /= M(1, 1) * I(1, 1) + 97 M(2, 1) * I(1, 2) + 98 M(3, 1) * I(1, 3) + 99 M(4, 1) * I(1, 4); 100 101 return I; 102 } 103 104 Matrix Matrix::operator~() const 105 { 106 const Matrix &M = *this; 107 108 return Matrix(M(1, 1), M(2, 1), M(3, 1), M(4, 1), 109 M(1, 2), M(2, 2), M(3, 2), M(4, 2), 110 M(1, 3), M(2, 3), M(3, 3), M(4, 3), 111 M(1, 4), M(2, 4), M(3, 4), M(4, 4)); 112 } 113 114 Matrix &Matrix::operator+=(const Matrix &N) 115 { 116 Matrix &M = *this; 117 118 M(1, 1) += N(1, 1); M(1, 2) += N(1, 2); M(1, 3) += N(1, 3); M(1, 4) += N(1, 4); 119 M(2, 1) += N(2, 1); M(2, 2) += N(2, 2); M(2, 3) += N(2, 3); M(2, 4) += N(2, 4); 120 M(3, 1) += N(3, 1); M(3, 2) += N(3, 2); M(3, 3) += N(3, 3); M(3, 4) += N(3, 4); 121 M(4, 1) += N(4, 1); M(4, 2) += N(4, 2); M(4, 3) += N(4, 3); M(4, 4) += N(4, 4); 122 123 return M; 124 } 125 126 Matrix &Matrix::operator-=(const Matrix &N) 127 { 128 Matrix &M = *this; 129 130 M(1, 1) -= N(1, 1); M(1, 2) -= N(1, 2); M(1, 3) -= N(1, 3); M(1, 4) -= N(1, 4); 131 M(2, 1) -= N(2, 1); M(2, 2) -= N(2, 2); M(2, 3) -= N(2, 3); M(2, 4) -= N(2, 4); 132 M(3, 1) -= N(3, 1); M(3, 2) -= N(3, 2); M(3, 3) -= N(3, 3); M(3, 4) -= N(3, 4); 133 M(4, 1) -= N(4, 1); M(4, 2) -= N(4, 2); M(4, 3) -= N(4, 3); M(4, 4) -= N(4, 4); 134 135 return M; 136 } 137 138 Matrix &Matrix::operator*=(float s) 139 { 140 Matrix &M = *this; 141 142 M(1, 1) *= s; M(1, 2) *= s; M(1, 3) *= s; M(1, 4) *= s; 143 M(2, 1) *= s; M(2, 2) *= s; M(2, 3) *= s; M(2, 4) *= s; 144 M(3, 1) *= s; M(3, 2) *= s; M(3, 3) *= s; M(3, 4) *= s; 145 M(4, 1) *= s; M(4, 2) *= s; M(4, 3) *= s; M(4, 4) *= s; 146 147 return M; 148 } 149 150 Matrix &Matrix::operator*=(const Matrix &M) 151 { 152 return *this = *this * M; 153 } 154 155 Matrix &Matrix::operator/=(float s) 156 { 157 float r = 1.0f / s; 158 159 return *this *= r; 160 } 161 162 bool operator==(const Matrix &M, const Matrix &N) 163 { 164 if(M(1, 1) == N(1, 1) && M(1, 2) == N(1, 2) && M(1, 3) == N(1, 3) && M(1, 4) == N(1, 4) && 165 M(2, 1) == N(2, 1) && M(2, 2) == N(2, 2) && M(2, 3) == N(2, 3) && M(2, 4) == N(2, 4) && 166 M(3, 1) == N(3, 1) && M(3, 2) == N(3, 2) && M(3, 3) == N(3, 3) && M(3, 4) == N(3, 4) && 167 M(4, 1) == N(4, 1) && M(4, 2) == N(4, 2) && M(4, 3) == N(4, 3) && M(4, 4) == N(4, 4)) 168 return true; 169 else 170 return false; 171 } 172 173 bool operator!=(const Matrix &M, const Matrix &N) 174 { 175 if(M(1, 1) != N(1, 1) || M(1, 2) != N(1, 2) || M(1, 3) != N(1, 3) || M(1, 4) != N(1, 4) || 176 M(2, 1) != N(2, 1) || M(2, 2) != N(2, 2) || M(2, 3) != N(2, 3) || M(2, 4) != N(2, 4) || 177 M(3, 1) != N(3, 1) || M(3, 2) != N(3, 2) || M(3, 3) != N(3, 3) || M(3, 4) != N(3, 4) || 178 M(4, 1) != N(4, 1) || M(4, 2) != N(4, 2) || M(4, 3) != N(4, 3) || M(4, 4) != N(4, 4)) 179 return true; 180 else 181 return false; 182 } 183 184 Matrix operator+(const Matrix &M, const Matrix &N) 185 { 186 return Matrix(M(1, 1) + N(1, 1), M(1, 2) + N(1, 2), M(1, 3) + N(1, 3), M(1, 4) + N(1, 4), 187 M(2, 1) + N(2, 1), M(2, 2) + N(2, 2), M(2, 3) + N(2, 3), M(2, 4) + N(2, 4), 188 M(3, 1) + N(3, 1), M(3, 2) + N(3, 2), M(3, 3) + N(3, 3), M(3, 4) + N(3, 4), 189 M(4, 1) + N(4, 1), M(4, 2) + N(4, 2), M(4, 3) + N(4, 3), M(4, 4) + N(4, 4)); 190 } 191 192 Matrix operator-(const Matrix &M, const Matrix &N) 193 { 194 return Matrix(M(1, 1) - N(1, 1), M(1, 2) - N(1, 2), M(1, 3) - N(1, 3), M(1, 4) - N(1, 4), 195 M(2, 1) - N(2, 1), M(2, 2) - N(2, 2), M(2, 3) - N(2, 3), M(2, 4) - N(2, 4), 196 M(3, 1) - N(3, 1), M(3, 2) - N(3, 2), M(3, 3) - N(3, 3), M(3, 4) - N(3, 4), 197 M(4, 1) - N(4, 1), M(4, 2) - N(4, 2), M(4, 3) - N(4, 3), M(4, 4) - N(4, 4)); 198 } 199 200 Matrix operator*(float s, const Matrix &M) 201 { 202 return Matrix(s * M(1, 1), s * M(1, 2), s * M(1, 3), s * M(1, 4), 203 s * M(2, 1), s * M(2, 2), s * M(2, 3), s * M(2, 4), 204 s * M(3, 1), s * M(3, 2), s * M(3, 3), s * M(3, 4), 205 s * M(4, 1), s * M(4, 2), s * M(4, 3), s * M(4, 4)); 206 } 207 208 Matrix operator*(const Matrix &M, float s) 209 { 210 return Matrix(M(1, 1) * s, M(1, 2) * s, M(1, 3) * s, M(1, 4) * s, 211 M(2, 1) * s, M(2, 2) * s, M(2, 3) * s, M(2, 4) * s, 212 M(3, 1) * s, M(3, 2) * s, M(3, 3) * s, M(3, 4) * s, 213 M(4, 1) * s, M(4, 2) * s, M(4, 3) * s, M(4, 4) * s); 214 } 215 216 Matrix operator*(const Matrix &M, const Matrix &N) 217 { 218 return Matrix(M(1, 1) * N(1, 1) + M(1, 2) * N(2, 1) + M(1, 3) * N(3, 1) + M(1, 4) * N(4, 1), M(1, 1) * N(1, 2) + M(1, 2) * N(2, 2) + M(1, 3) * N(3, 2) + M(1, 4) * N(4, 2), M(1, 1) * N(1, 3) + M(1, 2) * N(2, 3) + M(1, 3) * N(3, 3) + M(1, 4) * N(4, 3), M(1, 1) * N(1, 4) + M(1, 2) * N(2, 4) + M(1, 3) * N(3, 4) + M(1, 4) * N(4, 4), 219 M(2, 1) * N(1, 1) + M(2, 2) * N(2, 1) + M(2, 3) * N(3, 1) + M(2, 4) * N(4, 1), M(2, 1) * N(1, 2) + M(2, 2) * N(2, 2) + M(2, 3) * N(3, 2) + M(2, 4) * N(4, 2), M(2, 1) * N(1, 3) + M(2, 2) * N(2, 3) + M(2, 3) * N(3, 3) + M(2, 4) * N(4, 3), M(2, 1) * N(1, 4) + M(2, 2) * N(2, 4) + M(2, 3) * N(3, 4) + M(2, 4) * N(4, 4), 220 M(3, 1) * N(1, 1) + M(3, 2) * N(2, 1) + M(3, 3) * N(3, 1) + M(3, 4) * N(4, 1), M(3, 1) * N(1, 2) + M(3, 2) * N(2, 2) + M(3, 3) * N(3, 2) + M(3, 4) * N(4, 2), M(3, 1) * N(1, 3) + M(3, 2) * N(2, 3) + M(3, 3) * N(3, 3) + M(3, 4) * N(4, 3), M(3, 1) * N(1, 4) + M(3, 2) * N(2, 4) + M(3, 3) * N(3, 4) + M(3, 4) * N(4, 4), 221 M(4, 1) * N(1, 1) + M(4, 2) * N(2, 1) + M(4, 3) * N(3, 1) + M(4, 4) * N(4, 1), M(4, 1) * N(1, 2) + M(4, 2) * N(2, 2) + M(4, 3) * N(3, 2) + M(4, 4) * N(4, 2), M(4, 1) * N(1, 3) + M(4, 2) * N(2, 3) + M(4, 3) * N(3, 3) + M(4, 4) * N(4, 3), M(4, 1) * N(1, 4) + M(4, 2) * N(2, 4) + M(4, 3) * N(3, 4) + M(4, 4) * N(4, 4)); 222 } 223 224 Matrix operator/(const Matrix &M, float s) 225 { 226 float r = 1.0f / s; 227 228 return M * r; 229 } 230 231 float4 Matrix::operator*(const float4 &v) const 232 { 233 const Matrix &M = *this; 234 float Mx = M(1, 1) * v.x + M(1, 2) * v.y + M(1, 3) * v.z + M(1, 4) * v.w; 235 float My = M(2, 1) * v.x + M(2, 2) * v.y + M(2, 3) * v.z + M(2, 4) * v.w; 236 float Mz = M(3, 1) * v.x + M(3, 2) * v.y + M(3, 3) * v.z + M(3, 4) * v.w; 237 float Mw = M(4, 1) * v.x + M(4, 2) * v.y + M(4, 3) * v.z + M(4, 4) * v.w; 238 239 return {Mx, My, Mz, Mw}; 240 } 241 242 float Matrix::det(const Matrix &M) 243 { 244 float M3344 = M(3, 3) * M(4, 4) - M(4, 3) * M(3, 4); 245 float M2344 = M(2, 3) * M(4, 4) - M(4, 3) * M(2, 4); 246 float M2334 = M(2, 3) * M(3, 4) - M(3, 3) * M(2, 4); 247 float M1344 = M(1, 3) * M(4, 4) - M(4, 3) * M(1, 4); 248 float M1334 = M(1, 3) * M(3, 4) - M(3, 3) * M(1, 4); 249 float M1324 = M(1, 3) * M(2, 4) - M(2, 3) * M(1, 4); 250 251 return M(1, 1) * (M(2, 2) * M3344 - M(3, 2) * M2344 + M(4, 2) * M2334) - 252 M(2, 1) * (M(1, 2) * M3344 - M(3, 2) * M1344 + M(4, 2) * M1334) + 253 M(3, 1) * (M(1, 2) * M2344 - M(2, 2) * M1344 + M(4, 2) * M1324) - 254 M(4, 1) * (M(1, 2) * M2334 - M(2, 2) * M1334 + M(3, 2) * M1324); 255 } 256 257 float Matrix::det(float m11) 258 { 259 return m11; 260 } 261 262 float Matrix::det(float m11, float m12, 263 float m21, float m22) 264 { 265 return m11 * m22 - m12 * m21; 266 } 267 268 float Matrix::det(float m11, float m12, float m13, 269 float m21, float m22, float m23, 270 float m31, float m32, float m33) 271 { 272 return m11 * (m22 * m33 - m32 * m23) - 273 m21 * (m12 * m33 - m32 * m13) + 274 m31 * (m12 * m23 - m22 * m13); 275 } 276 277 float Matrix::det(float m11, float m12, float m13, float m14, 278 float m21, float m22, float m23, float m24, 279 float m31, float m32, float m33, float m34, 280 float m41, float m42, float m43, float m44) 281 { 282 float M3344 = m33 * m44 - m43 * m34; 283 float M2344 = m23 * m44 - m43 * m24; 284 float M2334 = m23 * m34 - m33 * m24; 285 float M1344 = m13 * m44 - m43 * m14; 286 float M1334 = m13 * m34 - m33 * m14; 287 float M1324 = m13 * m24 - m23 * m14; 288 289 return m11 * (m22 * M3344 - m32 * M2344 + m42 * M2334) - 290 m21 * (m12 * M3344 - m32 * M1344 + m42 * M1334) + 291 m31 * (m12 * M2344 - m22 * M1344 + m42 * M1324) - 292 m41 * (m12 * M2334 - m22 * M1334 + m32 * M1324); 293 } 294 295 float Matrix::det(const Vector &v1, const Vector &v2, const Vector &v3) 296 { 297 return v1 * (v2 % v3); 298 } 299 300 float Matrix::det3(const Matrix &M) 301 { 302 return M(1, 1) * (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) - 303 M(2, 1) * (M(1, 2) * M(3, 3) - M(3, 2) * M(1, 3)) + 304 M(3, 1) * (M(1, 2) * M(2, 3) - M(2, 2) * M(1, 3)); 305 } 306 307 float Matrix::tr(const Matrix &M) 308 { 309 return M(1, 1) + M(2, 2) + M(3, 3) + M(4, 4); 310 } 311 312 Matrix &Matrix::orthogonalise() 313 { 314 // NOTE: Numnerically instable, won't return exact the same result when already orhtogonal 315 316 Matrix &M = *this; 317 318 Vector v1(M(1, 1), M(2, 1), M(3, 1)); 319 Vector v2(M(1, 2), M(2, 2), M(3, 2)); 320 Vector v3(M(1, 3), M(2, 3), M(3, 3)); 321 322 v2 -= v1 * (v1 * v2) / (v1 * v1); 323 v3 -= v1 * (v1 * v3) / (v1 * v1); 324 v3 -= v2 * (v2 * v3) / (v2 * v2); 325 326 v1 /= Vector::N(v1); 327 v2 /= Vector::N(v2); 328 v3 /= Vector::N(v3); 329 330 M(1, 1) = v1.x; M(1, 2) = v2.x; M(1, 3) = v3.x; 331 M(2, 1) = v1.y; M(2, 2) = v2.y; M(2, 3) = v3.y; 332 M(3, 1) = v1.z; M(3, 2) = v2.z; M(3, 3) = v3.z; 333 334 return *this; 335 } 336 337 Matrix Matrix::eulerRotate(const Vector &v) 338 { 339 float cz = cos(v.z); 340 float sz = sin(v.z); 341 float cx = cos(v.x); 342 float sx = sin(v.x); 343 float cy = cos(v.y); 344 float sy = sin(v.y); 345 346 float sxsy = sx * sy; 347 float sxcy = sx * cy; 348 349 return Matrix(cy * cz - sxsy * sz, -cy * sz - sxsy * cz, -sy * cx, 350 cx * sz, cx * cz, -sx, 351 sy * cz + sxcy * sz, -sy * sz + sxcy * cz, cy * cx); 352 } 353 354 Matrix Matrix::eulerRotate(float x, float y, float z) 355 { 356 return eulerRotate(Vector(x, y, z)); 357 } 358 359 Matrix Matrix::translate(const Vector &v) 360 { 361 return Matrix(1, 0, 0, v.x, 362 0, 1, 0, v.y, 363 0, 0, 1, v.z, 364 0, 0, 0, 1); 365 } 366 367 Matrix Matrix::translate(float x, float y, float z) 368 { 369 return translate(Vector(x, y, z)); 370 } 371 372 Matrix Matrix::scale(const Vector &v) 373 { 374 return Matrix(v.x, 0, 0, 375 0, v.y, 0, 376 0, 0, v.z); 377 } 378 379 Matrix Matrix::scale(float x, float y, float z) 380 { 381 return scale(Vector(x, y, z)); 382 } 383 384 Matrix Matrix::lookAt(const Vector &v) 385 { 386 Vector y = v; 387 y /= Vector::N(y); 388 389 Vector x = y % Vector(0, 0, 1); 390 x /= Vector::N(x); 391 392 Vector z = x % y; 393 z /= Vector::N(z); 394 395 return ~Matrix(x, y, z); 396 } 397 398 Matrix Matrix::lookAt(float x, float y, float z) 399 { 400 return translate(Vector(x, y, z)); 401 } 402} 403