third_party/angle/util/Matrix.cpp

   1 //
   2 // Copyright (c) 2014 The ANGLE Project Authors. All rights reserved.
   3 // Use of this source code is governed by a BSD-style license that can be
   4 // found in the LICENSE file.
   5 //
   6 // Matrix:
   7 //   Helper class for doing matrix math.
   8 //
   9
  10 #include "Matrix.h"
  11
  12 #define _USE_MATH_DEFINES
  13 #include <math.h>
  14 #include <cstddef>
  15
  16 using namespace angle;
  17
  18 Matrix4::Matrix4()
  19 {
  20     data[0]  = 1.0f;
  21     data[4]  = 0.0f;
  22     data[8]  = 0.0f;
  23     data[12] = 0.0f;
  24     data[1]  = 0.0f;
  25     data[5]  = 1.0f;
  26     data[9]  = 0.0f;
  27     data[13] = 0.0f;
  28     data[2]  = 0.0f;
  29     data[6]  = 0.0f;
  30     data[10] = 1.0f;
  31     data[14] = 0.0f;
  32     data[3]  = 0.0f;
  33     data[7]  = 0.0f;
  34     data[11] = 0.0f;
  35     data[15] = 1.0f;
  36 }
  37
  38 Matrix4::Matrix4(float m00,
  39                  float m01,
  40                  float m02,
  41                  float m03,
  42                  float m10,
  43                  float m11,
  44                  float m12,
  45                  float m13,
  46                  float m20,
  47                  float m21,
  48                  float m22,
  49                  float m23,
  50                  float m30,
  51                  float m31,
  52                  float m32,
  53                  float m33)
  54 {
  55     data[0]  = m00;
  56     data[4]  = m01;
  57     data[8]  = m02;
  58     data[12] = m03;
  59     data[1]  = m10;
  60     data[5]  = m11;
  61     data[9]  = m12;
  62     data[13] = m13;
  63     data[2]  = m20;
  64     data[6]  = m21;
  65     data[10] = m22;
  66     data[14] = m23;
  67     data[3]  = m30;
  68     data[7]  = m31;
  69     data[11] = m32;
  70     data[15] = m33;
  71 }
  72
  73 Matrix4 Matrix4::identity()
  74 {
  75     return Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
  76                    0.0f, 0.0f, 1.0f);
  77 }
  78
  79 Matrix4 Matrix4::rotate(float angle, const Vector3 &p)
  80 {
  81     Vector3 u   = p.normalized();
  82     float theta = static_cast<float>(angle * (M_PI / 180.0f));
  83     float cos_t = cosf(theta);
  84     float sin_t = sinf(theta);
  85
  86     return Matrix4(cos_t + (u.x() * u.x() * (1.0f - cos_t)),
  87                    (u.x() * u.y() * (1.0f - cos_t)) - (u.z() * sin_t),
  88                    (u.x() * u.z() * (1.0f - cos_t)) + (u.y() * sin_t), 0.0f,
  89                    (u.y() * u.x() * (1.0f - cos_t)) + (u.z() * sin_t),
  90                    cos_t + (u.y() * u.y() * (1.0f - cos_t)),
  91                    (u.y() * u.z() * (1.0f - cos_t)) - (u.x() * sin_t), 0.0f,
  92                    (u.z() * u.x() * (1.0f - cos_t)) - (u.y() * sin_t),
  93                    (u.z() * u.y() * (1.0f - cos_t)) + (u.x() * sin_t),
  94                    cos_t + (u.z() * u.z() * (1.0f - cos_t)), 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
  95 }
  96
  97 Matrix4 Matrix4::translate(const Vector3 &t)
  98 {
  99     return Matrix4(1.0f, 0.0f, 0.0f, t.x(), 0.0f, 1.0f, 0.0f, t.y(), 0.0f, 0.0f, 1.0f, t.z(), 0.0f,
 100                    0.0f, 0.0f, 1.0f);
 101 }
 102
 103 Matrix4 Matrix4::scale(const Vector3 &s)
 104 {
 105     return Matrix4(s.x(), 0.0f, 0.0f, 0.0f, 0.0f, s.y(), 0.0f, 0.0f, 0.0f, 0.0f, s.z(), 0.0f, 0.0f,
 106                    0.0f, 0.0f, 1.0f);
 107 }
 108
 109 Matrix4 Matrix4::frustum(float l, float r, float b, float t, float n, float f)
 110 {
 111     return Matrix4((2.0f * n) / (r - l), 0.0f, (r + l) / (r - l), 0.0f, 0.0f, (2.0f * n) / (t - b),
 112                    (t + b) / (t - b), 0.0f, 0.0f, 0.0f, -(f + n) / (f - n),
 113                    -(2.0f * f * n) / (f - n), 0.0f, 0.0f, -1.0f, 0.0f);
 114 }
 115
 116 Matrix4 Matrix4::perspective(float fovY, float aspectRatio, float nearZ, float farZ)
 117 {
 118     const float frustumHeight = tanf(static_cast<float>(fovY / 360.0f * M_PI)) * nearZ;
 119     const float frustumWidth = frustumHeight * aspectRatio;
 120     return frustum(-frustumWidth, frustumWidth, -frustumHeight, frustumHeight, nearZ, farZ);
 121 }
 122
 123 Matrix4 Matrix4::ortho(float l, float r, float b, float t, float n, float f)
 124 {
 125     return Matrix4(2.0f / (r - l), 0.0f, 0.0f, -(r + l) / (r - l), 0.0f, 2.0f / (t - b), 0.0f,
 126                    -(t + b) / (t - b), 0.0f, 0.0f, -2.0f / (f - n), -(f + n) / (f - n), 0.0f, 0.0f,
 127                    0.0f, 1.0f);
 128 }
 129
 130 Matrix4 Matrix4::rollPitchYaw(float roll, float pitch, float yaw)
 131 {
 132     return rotate(yaw, Vector3(0, 0, 1)) * rotate(pitch, Vector3(0, 1, 0)) *
 133            rotate(roll, Vector3(1, 0, 0));
 134 }
 135
 136 Matrix4 Matrix4::invert(const Matrix4 &mat)
 137 {
 138     Matrix4 inverted(
 139         mat.data[5] * mat.data[10] * mat.data[15] - mat.data[5] * mat.data[11] * mat.data[14] -
 140             mat.data[9] * mat.data[6] * mat.data[15] + mat.data[9] * mat.data[7] * mat.data[14] +
 141             mat.data[13] * mat.data[6] * mat.data[11] - mat.data[13] * mat.data[7] * mat.data[10],
 142         -mat.data[4] * mat.data[10] * mat.data[15] + mat.data[4] * mat.data[11] * mat.data[14] +
 143             mat.data[8] * mat.data[6] * mat.data[15] - mat.data[8] * mat.data[7] * mat.data[14] -
 144             mat.data[12] * mat.data[6] * mat.data[11] + mat.data[12] * mat.data[7] * mat.data[10],
 145         mat.data[4] * mat.data[9] * mat.data[15] - mat.data[4] * mat.data[11] * mat.data[13] -
 146             mat.data[8] * mat.data[5] * mat.data[15] + mat.data[8] * mat.data[7] * mat.data[13] +
 147             mat.data[12] * mat.data[5] * mat.data[11] - mat.data[12] * mat.data[7] * mat.data[9],
 148         -mat.data[4] * mat.data[9] * mat.data[14] + mat.data[4] * mat.data[10] * mat.data[13] +
 149             mat.data[8] * mat.data[5] * mat.data[14] - mat.data[8] * mat.data[6] * mat.data[13] -
 150             mat.data[12] * mat.data[5] * mat.data[10] + mat.data[12] * mat.data[6] * mat.data[9],
 151         -mat.data[1] * mat.data[10] * mat.data[15] + mat.data[1] * mat.data[11] * mat.data[14] +
 152             mat.data[9] * mat.data[2] * mat.data[15] - mat.data[9] * mat.data[3] * mat.data[14] -
 153             mat.data[13] * mat.data[2] * mat.data[11] + mat.data[13] * mat.data[3] * mat.data[10],
 154         mat.data[0] * mat.data[10] * mat.data[15] - mat.data[0] * mat.data[11] * mat.data[14] -
 155             mat.data[8] * mat.data[2] * mat.data[15] + mat.data[8] * mat.data[3] * mat.data[14] +
 156             mat.data[12] * mat.data[2] * mat.data[11] - mat.data[12] * mat.data[3] * mat.data[10],
 157         -mat.data[0] * mat.data[9] * mat.data[15] + mat.data[0] * mat.data[11] * mat.data[13] +
 158             mat.data[8] * mat.data[1] * mat.data[15] - mat.data[8] * mat.data[3] * mat.data[13] -
 159             mat.data[12] * mat.data[1] * mat.data[11] + mat.data[12] * mat.data[3] * mat.data[9],
 160         mat.data[0] * mat.data[9] * mat.data[14] - mat.data[0] * mat.data[10] * mat.data[13] -
 161             mat.data[8] * mat.data[1] * mat.data[14] + mat.data[8] * mat.data[2] * mat.data[13] +
 162             mat.data[12] * mat.data[1] * mat.data[10] - mat.data[12] * mat.data[2] * mat.data[9],
 163         mat.data[1] * mat.data[6] * mat.data[15] - mat.data[1] * mat.data[7] * mat.data[14] -
 164             mat.data[5] * mat.data[2] * mat.data[15] + mat.data[5] * mat.data[3] * mat.data[14] +
 165             mat.data[13] * mat.data[2] * mat.data[7] - mat.data[13] * mat.data[3] * mat.data[6],
 166         -mat.data[0] * mat.data[6] * mat.data[15] + mat.data[0] * mat.data[7] * mat.data[14] +
 167             mat.data[4] * mat.data[2] * mat.data[15] - mat.data[4] * mat.data[3] * mat.data[14] -
 168             mat.data[12] * mat.data[2] * mat.data[7] + mat.data[12] * mat.data[3] * mat.data[6],
 169         mat.data[0] * mat.data[5] * mat.data[15] - mat.data[0] * mat.data[7] * mat.data[13] -
 170             mat.data[4] * mat.data[1] * mat.data[15] + mat.data[4] * mat.data[3] * mat.data[13] +
 171             mat.data[12] * mat.data[1] * mat.data[7] - mat.data[12] * mat.data[3] * mat.data[5],
 172         -mat.data[0] * mat.data[5] * mat.data[14] + mat.data[0] * mat.data[6] * mat.data[13] +
 173             mat.data[4] * mat.data[1] * mat.data[14] - mat.data[4] * mat.data[2] * mat.data[13] -
 174             mat.data[12] * mat.data[1] * mat.data[6] + mat.data[12] * mat.data[2] * mat.data[5],
 175         -mat.data[1] * mat.data[6] * mat.data[11] + mat.data[1] * mat.data[7] * mat.data[10] +
 176             mat.data[5] * mat.data[2] * mat.data[11] - mat.data[5] * mat.data[3] * mat.data[10] -
 177             mat.data[9] * mat.data[2] * mat.data[7] + mat.data[9] * mat.data[3] * mat.data[6],
 178         mat.data[0] * mat.data[6] * mat.data[11] - mat.data[0] * mat.data[7] * mat.data[10] -
 179             mat.data[4] * mat.data[2] * mat.data[11] + mat.data[4] * mat.data[3] * mat.data[10] +
 180             mat.data[8] * mat.data[2] * mat.data[7] - mat.data[8] * mat.data[3] * mat.data[6],
 181         -mat.data[0] * mat.data[5] * mat.data[11] + mat.data[0] * mat.data[7] * mat.data[9] +
 182             mat.data[4] * mat.data[1] * mat.data[11] - mat.data[4] * mat.data[3] * mat.data[9] -
 183             mat.data[8] * mat.data[1] * mat.data[7] + mat.data[8] * mat.data[3] * mat.data[5],
 184         mat.data[0] * mat.data[5] * mat.data[10] - mat.data[0] * mat.data[6] * mat.data[9] -
 185             mat.data[4] * mat.data[1] * mat.data[10] + mat.data[4] * mat.data[2] * mat.data[9] +
 186             mat.data[8] * mat.data[1] * mat.data[6] - mat.data[8] * mat.data[2] * mat.data[5]);
 187
 188     float determinant = mat.data[0] * inverted.data[0] + mat.data[1] * inverted.data[4] +
 189                         mat.data[2] * inverted.data[8] + mat.data[3] * inverted.data[12];
 190
 191     if (determinant != 0.0f)
 192     {
 193         inverted *= 1.0f / determinant;
 194     }
 195     else
 196     {
 197         inverted = identity();
 198     }
 199
 200     return inverted;
 201 }
 202
 203 Matrix4 Matrix4::transpose(const Matrix4 &mat)
 204 {
 205     return Matrix4(mat.data[0], mat.data[1], mat.data[2], mat.data[3], mat.data[4], mat.data[5],
 206                    mat.data[6], mat.data[7], mat.data[8], mat.data[9], mat.data[10], mat.data[11],
 207                    mat.data[12], mat.data[13], mat.data[14], mat.data[15]);
 208 }
 209
 210 Vector3 Matrix4::transform(const Matrix4 &mat, const Vector3 &pt)
 211 {
 212     Vector4 transformed = (mat * Vector4(pt, 1.0f)).normalized();
 213     return Vector3(transformed.x(), transformed.y(), transformed.z());
 214 }
 215
 216 Vector3 Matrix4::transform(const Matrix4 &mat, const Vector4 &pt)
 217 {
 218     Vector4 transformed = (mat * pt).normalized();
 219     return Vector3(transformed.x(), transformed.y(), transformed.z());
 220 }
 221
 222 Matrix4 operator*(const Matrix4 &a, const Matrix4 &b)
 223 {
 224     return Matrix4(a.data[0] * b.data[0] + a.data[4] * b.data[1] + a.data[8] * b.data[2] +
 225                        a.data[12] * b.data[3],
 226                    a.data[0] * b.data[4] + a.data[4] * b.data[5] + a.data[8] * b.data[6] +
 227                        a.data[12] * b.data[7],
 228                    a.data[0] * b.data[8] + a.data[4] * b.data[9] + a.data[8] * b.data[10] +
 229                        a.data[12] * b.data[11],
 230                    a.data[0] * b.data[12] + a.data[4] * b.data[13] + a.data[8] * b.data[14] +
 231                        a.data[12] * b.data[15],
 232                    a.data[1] * b.data[0] + a.data[5] * b.data[1] + a.data[9] * b.data[2] +
 233                        a.data[13] * b.data[3],
 234                    a.data[1] * b.data[4] + a.data[5] * b.data[5] + a.data[9] * b.data[6] +
 235                        a.data[13] * b.data[7],
 236                    a.data[1] * b.data[8] + a.data[5] * b.data[9] + a.data[9] * b.data[10] +
 237                        a.data[13] * b.data[11],
 238                    a.data[1] * b.data[12] + a.data[5] * b.data[13] + a.data[9] * b.data[14] +
 239                        a.data[13] * b.data[15],
 240                    a.data[2] * b.data[0] + a.data[6] * b.data[1] + a.data[10] * b.data[2] +
 241                        a.data[14] * b.data[3],
 242                    a.data[2] * b.data[4] + a.data[6] * b.data[5] + a.data[10] * b.data[6] +
 243                        a.data[14] * b.data[7],
 244                    a.data[2] * b.data[8] + a.data[6] * b.data[9] + a.data[10] * b.data[10] +
 245                        a.data[14] * b.data[11],
 246                    a.data[2] * b.data[12] + a.data[6] * b.data[13] + a.data[10] * b.data[14] +
 247                        a.data[14] * b.data[15],
 248                    a.data[3] * b.data[0] + a.data[7] * b.data[1] + a.data[11] * b.data[2] +
 249                        a.data[15] * b.data[3],
 250                    a.data[3] * b.data[4] + a.data[7] * b.data[5] + a.data[11] * b.data[6] +
 251                        a.data[15] * b.data[7],
 252                    a.data[3] * b.data[8] + a.data[7] * b.data[9] + a.data[11] * b.data[10] +
 253                        a.data[15] * b.data[11],
 254                    a.data[3] * b.data[12] + a.data[7] * b.data[13] + a.data[11] * b.data[14] +
 255                        a.data[15] * b.data[15]);
 256 }
 257
 258 Matrix4 &operator*=(Matrix4 &a, const Matrix4 &b)
 259 {
 260     a = a * b;
 261     return a;
 262 }
 263
 264 Matrix4 operator*(const Matrix4 &a, float b)
 265 {
 266     Matrix4 ret(a);
 267     for (size_t i = 0; i < 16; i++)
 268     {
 269         ret.data[i] *= b;
 270     }
 271     return ret;
 272 }
 273
 274 Matrix4 &operator*=(Matrix4 &a, float b)
 275 {
 276     for (size_t i = 0; i < 16; i++)
 277     {
 278         a.data[i] *= b;
 279     }
 280     return a;
 281 }
 282
 283 Vector4 operator*(const Matrix4 &a, const Vector4 &b)
 284 {
 285     return Vector4(a.data[0] * b.x() + a.data[4] * b.y() + a.data[8] * b.z() + a.data[12] * b.w(),
 286                    a.data[1] * b.x() + a.data[5] * b.y() + a.data[9] * b.z() + a.data[13] * b.w(),
 287                    a.data[2] * b.x() + a.data[6] * b.y() + a.data[10] * b.z() + a.data[14] * b.w(),
 288                    a.data[3] * b.x() + a.data[7] * b.y() + a.data[11] * b.z() + a.data[15] * b.w());
 289 }
 290
 291 bool operator==(const Matrix4 &a, const Matrix4 &b)
 292 {
 293     for (size_t i = 0; i < 16; i++)
 294     {
 295         if (a.data[i] != b.data[i])
 296         {
 297             return false;
 298         }
 299     }
 300     return true;
 301 }
 302
 303 bool operator!=(const Matrix4 &a, const Matrix4 &b)
 304 {
 305     return !(a == b);
 306 }