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 "Common/Math.hpp" 19 20 namespace sw 21 { diag(float m11,float m22,float m33,float m44)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 operator float*()30 Matrix::operator float*() 31 { 32 return &(*this)(1, 1); 33 } 34 operator +() const35 Matrix Matrix::operator+() const 36 { 37 return *this; 38 } 39 operator -() const40 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 operator !() const50 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 operator ~() const104 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 operator +=(const Matrix & N)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 operator -=(const Matrix & N)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 operator *=(float s)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 operator *=(const Matrix & M)150 Matrix &Matrix::operator*=(const Matrix &M) 151 { 152 return *this = *this * M; 153 } 154 operator /=(float s)155 Matrix &Matrix::operator/=(float s) 156 { 157 float r = 1.0f / s; 158 159 return *this *= r; 160 } 161 operator ==(const Matrix & M,const Matrix & N)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 operator !=(const Matrix & M,const Matrix & N)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 operator +(const Matrix & M,const Matrix & N)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 operator -(const Matrix & M,const Matrix & N)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 operator *(float s,const Matrix & M)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 operator *(const Matrix & M,float s)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 operator *(const Matrix & M,const Matrix & N)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 operator /(const Matrix & M,float s)224 Matrix operator/(const Matrix &M, float s) 225 { 226 float r = 1.0f / s; 227 228 return M * r; 229 } 230 operator *(const float4 & v) const231 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 det(const Matrix & M)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 det(float m11)257 float Matrix::det(float m11) 258 { 259 return m11; 260 } 261 det(float m11,float m12,float m21,float m22)262 float Matrix::det(float m11, float m12, 263 float m21, float m22) 264 { 265 return m11 * m22 - m12 * m21; 266 } 267 det(float m11,float m12,float m13,float m21,float m22,float m23,float m31,float m32,float m33)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 det(float m11,float m12,float m13,float m14,float m21,float m22,float m23,float m24,float m31,float m32,float m33,float m34,float m41,float m42,float m43,float m44)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 det(const Vector & v1,const Vector & v2,const Vector & v3)295 float Matrix::det(const Vector &v1, const Vector &v2, const Vector &v3) 296 { 297 return v1 * (v2 % v3); 298 } 299 det3(const Matrix & M)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 tr(const Matrix & M)307 float Matrix::tr(const Matrix &M) 308 { 309 return M(1, 1) + M(2, 2) + M(3, 3) + M(4, 4); 310 } 311 orthogonalise()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 eulerRotate(const Vector & v)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 eulerRotate(float x,float y,float z)354 Matrix Matrix::eulerRotate(float x, float y, float z) 355 { 356 return eulerRotate(Vector(x, y, z)); 357 } 358 translate(const Vector & v)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 translate(float x,float y,float z)367 Matrix Matrix::translate(float x, float y, float z) 368 { 369 return translate(Vector(x, y, z)); 370 } 371 scale(const Vector & v)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 scale(float x,float y,float z)379 Matrix Matrix::scale(float x, float y, float z) 380 { 381 return scale(Vector(x, y, z)); 382 } 383 lookAt(const Vector & v)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 lookAt(float x,float y,float z)398 Matrix Matrix::lookAt(float x, float y, float z) 399 { 400 return translate(Vector(x, y, z)); 401 } 402 } 403