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
7#include "Matrix.h"
8
9#define _USE_MATH_DEFINES
10#include <math.h>
11
12Matrix4::Matrix4()
13{
14    data[ 0] = 1.0f; data[ 4] = 0.0f; data[ 8] = 0.0f; data[12] = 0.0f;
15    data[ 1] = 0.0f; data[ 5] = 1.0f; data[ 9] = 0.0f; data[13] = 0.0f;
16    data[ 2] = 0.0f; data[ 6] = 0.0f; data[10] = 1.0f; data[14] = 0.0f;
17    data[ 3] = 0.0f; data[ 7] = 0.0f; data[11] = 0.0f; data[15] = 1.0f;
18}
19
20Matrix4::Matrix4(float m00, float m01, float m02, float m03,
21                 float m10, float m11, float m12, float m13,
22                 float m20, float m21, float m22, float m23,
23                 float m30, float m31, float m32, float m33)
24{
25    data[ 0] = m00; data[ 4] = m01; data[ 8] = m02; data[12] = m03;
26    data[ 1] = m10; data[ 5] = m11; data[ 9] = m12; data[13] = m13;
27    data[ 2] = m20; data[ 6] = m21; data[10] = m22; data[14] = m23;
28    data[ 3] = m30; data[ 7] = m31; data[11] = m32; data[15] = m33;
29}
30
31Matrix4 Matrix4::identity()
32{
33    return Matrix4(1.0f, 0.0f, 0.0f, 0.0f,
34                   0.0f, 1.0f, 0.0f, 0.0f,
35                   0.0f, 0.0f, 1.0f, 0.0f,
36                   0.0f, 0.0f, 0.0f, 1.0f);
37}
38
39Matrix4 Matrix4::rotate(float angle, const Vector3 &p)
40{
41    Vector3 u = Vector3::normalize(p);
42    float theta = angle * (M_PI / 180.0f);
43    float cos_t = cosf(theta);
44    float sin_t = sinf(theta);
45
46    return Matrix4(          cos_t + (u.x * u.x * (1.0f - cos_t)), (u.x * u.y * (1.0f - cos_t)) - (u.z * sin_t), (u.x * u.z * (1.0f - cos_t)) + (u.y * sin_t), 0.0f,
47                   (u.y * u.x * (1.0f - cos_t)) + (u.z * sin_t),           cos_t + (u.y * u.y * (1.0f - cos_t)), (u.y * u.z * (1.0f - cos_t)) - (u.x * sin_t), 0.0f,
48                   (u.z * u.x * (1.0f - cos_t)) - (u.y * sin_t),   (u.z * u.y * (1.0f - cos_t)) + (u.x * sin_t),         cos_t + (u.z * u.z * (1.0f - cos_t)), 0.0f,
49                                                           0.0f,                                           0.0f,                                         0.0f, 1.0f);
50}
51
52Matrix4 Matrix4::translate(const Vector3 &t)
53{
54    return Matrix4(1.0f, 0.0f, 0.0f,  t.x,
55                   0.0f, 1.0f, 0.0f,  t.y,
56                   0.0f, 0.0f, 1.0f,  t.z,
57                   0.0f, 0.0f, 0.0f, 1.0f);
58}
59
60Matrix4 Matrix4::scale(const Vector3 &s)
61{
62    return Matrix4( s.x, 0.0f, 0.0f, 0.0f,
63                   0.0f,  s.y, 0.0f, 0.0f,
64                   0.0f, 0.0f,  s.z, 0.0f,
65                   0.0f, 0.0f, 0.0f, 1.0f);
66}
67
68Matrix4 Matrix4::frustum(float l, float r, float b, float t, float n, float f)
69{
70    return Matrix4((2.0f * n) / (r - l),                 0.0f,  (r + l) / (r - l),                      0.0f,
71                                   0.0f, (2.0f * n) / (t - b),  (t + b) / (t - b),                      0.0f,
72                                   0.0f,                 0.0f, -(f + n) / (f - n), -(2.0f * f * n) / (f - n),
73                                   0.0f,                 0.0f,              -1.0f,                      0.0f);
74}
75
76Matrix4 Matrix4::perspective(float fovY, float aspectRatio, float nearZ, float farZ)
77{
78    const float frustumHeight = tanf(fovY / 360.0f * M_PI) * nearZ;
79    const float frustumWidth = frustumHeight * aspectRatio;
80    return frustum(-frustumWidth, frustumWidth, -frustumHeight, frustumHeight, nearZ, farZ);
81}
82
83Matrix4 Matrix4::ortho(float l, float r, float b, float t, float n, float f)
84{
85    return Matrix4(2.0f / (r - l),           0.0f,            0.0f, -(r + l) / (r - l),
86                             0.0f, 2.0f / (t - b),            0.0f, -(t + b) / (t - b),
87                             0.0f,           0.0f, -2.0f / (f - n), -(f + n) / (f - n),
88                             0.0f,           0.0f,            0.0f,               1.0f);
89}
90
91Matrix4 Matrix4::rollPitchYaw(float roll, float pitch, float yaw)
92{
93    return rotate(yaw,   Vector3(0, 0, 1)) *
94           rotate(pitch, Vector3(0, 1, 0)) *
95           rotate(roll,  Vector3(1, 0, 0));
96}
97
98Matrix4 Matrix4::invert(const Matrix4 &mat)
99{
100    Matrix4 inverted( mat.data[ 5] * mat.data[10] * mat.data[15] - mat.data[5] * mat.data[11] * mat.data[14] - mat.data[ 9] * mat.data[ 6] * mat.data[15] + mat.data[ 9] * mat.data[ 7] * mat.data[14] + mat.data[13] * mat.data[ 6] * mat.data[11] - mat.data[13] * mat.data[ 7] * mat.data[10],
101                     -mat.data[ 4] * mat.data[10] * mat.data[15] + mat.data[4] * mat.data[11] * mat.data[14] + mat.data[ 8] * mat.data[ 6] * mat.data[15] - mat.data[ 8] * mat.data[ 7] * mat.data[14] - mat.data[12] * mat.data[ 6] * mat.data[11] + mat.data[12] * mat.data[ 7] * mat.data[10],
102                      mat.data[ 4] * mat.data[ 9] * mat.data[15] - mat.data[4] * mat.data[11] * mat.data[13] - mat.data[ 8] * mat.data[ 5] * mat.data[15] + mat.data[ 8] * mat.data[ 7] * mat.data[13] + mat.data[12] * mat.data[ 5] * mat.data[11] - mat.data[12] * mat.data[ 7] * mat.data[ 9],
103                     -mat.data[ 4] * mat.data[ 9] * mat.data[14] + mat.data[4] * mat.data[10] * mat.data[13] + mat.data[ 8] * mat.data[ 5] * mat.data[14] - mat.data[ 8] * mat.data[ 6] * mat.data[13] - mat.data[12] * mat.data[ 5] * mat.data[10] + mat.data[12] * mat.data[ 6] * mat.data[ 9],
104                     -mat.data[ 1] * mat.data[10] * mat.data[15] + mat.data[1] * mat.data[11] * mat.data[14] + mat.data[ 9] * mat.data[ 2] * mat.data[15] - mat.data[ 9] * mat.data[ 3] * mat.data[14] - mat.data[13] * mat.data[ 2] * mat.data[11] + mat.data[13] * mat.data[ 3] * mat.data[10],
105                      mat.data[ 0] * mat.data[10] * mat.data[15] - mat.data[0] * mat.data[11] * mat.data[14] - mat.data[ 8] * mat.data[ 2] * mat.data[15] + mat.data[ 8] * mat.data[ 3] * mat.data[14] + mat.data[12] * mat.data[ 2] * mat.data[11] - mat.data[12] * mat.data[ 3] * mat.data[10],
106                     -mat.data[ 0] * mat.data[ 9] * mat.data[15] + mat.data[0] * mat.data[11] * mat.data[13] + mat.data[ 8] * mat.data[ 1] * mat.data[15] - mat.data[ 8] * mat.data[ 3] * mat.data[13] - mat.data[12] * mat.data[ 1] * mat.data[11] + mat.data[12] * mat.data[ 3] * mat.data[ 9],
107                      mat.data[ 0] * mat.data[ 9] * mat.data[14] - mat.data[0] * mat.data[10] * mat.data[13] - mat.data[ 8] * mat.data[ 1] * mat.data[14] + mat.data[ 8] * mat.data[ 2] * mat.data[13] + mat.data[12] * mat.data[ 1] * mat.data[10] - mat.data[12] * mat.data[ 2] * mat.data[ 9],
108                      mat.data[ 1] * mat.data[ 6] * mat.data[15] - mat.data[1] * mat.data[ 7] * mat.data[14] - mat.data[ 5] * mat.data[ 2] * mat.data[15] + mat.data[ 5] * mat.data[ 3] * mat.data[14] + mat.data[13] * mat.data[ 2] * mat.data[ 7] - mat.data[13] * mat.data[ 3] * mat.data[ 6],
109                     -mat.data[ 0] * mat.data[ 6] * mat.data[15] + mat.data[0] * mat.data[ 7] * mat.data[14] + mat.data[ 4] * mat.data[ 2] * mat.data[15] - mat.data[ 4] * mat.data[ 3] * mat.data[14] - mat.data[12] * mat.data[ 2] * mat.data[ 7] + mat.data[12] * mat.data[ 3] * mat.data[ 6],
110                      mat.data[ 0] * mat.data[ 5] * mat.data[15] - mat.data[0] * mat.data[ 7] * mat.data[13] - mat.data[ 4] * mat.data[ 1] * mat.data[15] + mat.data[ 4] * mat.data[ 3] * mat.data[13] + mat.data[12] * mat.data[ 1] * mat.data[ 7] - mat.data[12] * mat.data[ 3] * mat.data[ 5],
111                     -mat.data[ 0] * mat.data[ 5] * mat.data[14] + mat.data[0] * mat.data[ 6] * mat.data[13] + mat.data[ 4] * mat.data[ 1] * mat.data[14] - mat.data[ 4] * mat.data[ 2] * mat.data[13] - mat.data[12] * mat.data[ 1] * mat.data[ 6] + mat.data[12] * mat.data[ 2] * mat.data[ 5],
112                     -mat.data[ 1] * mat.data[ 6] * mat.data[11] + mat.data[1] * mat.data[ 7] * mat.data[10] + mat.data[ 5] * mat.data[ 2] * mat.data[11] - mat.data[ 5] * mat.data[ 3] * mat.data[10] - mat.data[ 9] * mat.data[ 2] * mat.data[ 7] + mat.data[ 9] * mat.data[ 3] * mat.data[ 6],
113                      mat.data[ 0] * mat.data[ 6] * mat.data[11] - mat.data[0] * mat.data[ 7] * mat.data[10] - mat.data[ 4] * mat.data[ 2] * mat.data[11] + mat.data[ 4] * mat.data[ 3] * mat.data[10] + mat.data[ 8] * mat.data[ 2] * mat.data[ 7] - mat.data[ 8] * mat.data[ 3] * mat.data[ 6],
114                     -mat.data[ 0] * mat.data[ 5] * mat.data[11] + mat.data[0] * mat.data[ 7] * mat.data[ 9] + mat.data[ 4] * mat.data[ 1] * mat.data[11] - mat.data[ 4] * mat.data[ 3] * mat.data[ 9] - mat.data[ 8] * mat.data[ 1] * mat.data[ 7] + mat.data[ 8] * mat.data[ 3] * mat.data[ 5],
115                      mat.data[ 0] * mat.data[ 5] * mat.data[10] - mat.data[0] * mat.data[ 6] * mat.data[ 9] - mat.data[ 4] * mat.data[ 1] * mat.data[10] + mat.data[ 4] * mat.data[ 2] * mat.data[ 9] + mat.data[ 8] * mat.data[ 1] * mat.data[ 6] - mat.data[ 8] * mat.data[ 2] * mat.data[ 5]);
116
117    float determinant = mat.data[0] * inverted.data[0] + mat.data[1] * inverted.data[4] + mat.data[2] * inverted.data[8] + mat.data[3] * inverted.data[12];
118
119    if (determinant != 0.0f)
120    {
121        inverted *= 1.0f / determinant;
122    }
123    else
124    {
125        inverted = identity();
126    }
127
128    return inverted;
129}
130
131Matrix4 Matrix4::transpose(const Matrix4 &mat)
132{
133    return Matrix4(mat.data[ 0], mat.data[ 1], mat.data[ 2], mat.data[ 3],
134                   mat.data[ 4], mat.data[ 5], mat.data[ 6], mat.data[ 7],
135                   mat.data[ 8], mat.data[ 9], mat.data[10], mat.data[11],
136                   mat.data[12], mat.data[13], mat.data[14], mat.data[15]);
137}
138
139Vector3 Matrix4::transform(const Matrix4 &mat, const Vector3 &pt)
140{
141    Vector4 transformed = Vector4::normalize(mat * Vector4(pt.x, pt.y, pt.z, 1.0f));
142    return Vector3(transformed.x, transformed.y, transformed.z);
143}
144
145Vector3 Matrix4::transform(const Matrix4 &mat, const Vector4 &pt)
146{
147    Vector4 transformed = Vector4::normalize(mat * pt);
148    return Vector3(transformed.x, transformed.y, transformed.z);
149}
150
151Matrix4 operator*(const Matrix4 &a, const Matrix4 &b)
152{
153    return Matrix4(a.data[ 0] * b.data[ 0] + a.data[ 4] * b.data[ 1] + a.data[ 8] * b.data[ 2] + a.data[12] * b.data[ 3],
154                   a.data[ 0] * b.data[ 4] + a.data[ 4] * b.data[ 5] + a.data[ 8] * b.data[ 6] + a.data[12] * b.data[ 7],
155                   a.data[ 0] * b.data[ 8] + a.data[ 4] * b.data[ 9] + a.data[ 8] * b.data[10] + a.data[12] * b.data[11],
156                   a.data[ 0] * b.data[12] + a.data[ 4] * b.data[13] + a.data[ 8] * b.data[14] + a.data[12] * b.data[15],
157                   a.data[ 1] * b.data[ 0] + a.data[ 5] * b.data[ 1] + a.data[ 9] * b.data[ 2] + a.data[13] * b.data[ 3],
158                   a.data[ 1] * b.data[ 4] + a.data[ 5] * b.data[ 5] + a.data[ 9] * b.data[ 6] + a.data[13] * b.data[ 7],
159                   a.data[ 1] * b.data[ 8] + a.data[ 5] * b.data[ 9] + a.data[ 9] * b.data[10] + a.data[13] * b.data[11],
160                   a.data[ 1] * b.data[12] + a.data[ 5] * b.data[13] + a.data[ 9] * b.data[14] + a.data[13] * b.data[15],
161                   a.data[ 2] * b.data[ 0] + a.data[ 6] * b.data[ 1] + a.data[10] * b.data[ 2] + a.data[14] * b.data[ 3],
162                   a.data[ 2] * b.data[ 4] + a.data[ 6] * b.data[ 5] + a.data[10] * b.data[ 6] + a.data[14] * b.data[ 7],
163                   a.data[ 2] * b.data[ 8] + a.data[ 6] * b.data[ 9] + a.data[10] * b.data[10] + a.data[14] * b.data[11],
164                   a.data[ 2] * b.data[12] + a.data[ 6] * b.data[13] + a.data[10] * b.data[14] + a.data[14] * b.data[15],
165                   a.data[ 3] * b.data[ 0] + a.data[ 7] * b.data[ 1] + a.data[11] * b.data[ 2] + a.data[15] * b.data[ 3],
166                   a.data[ 3] * b.data[ 4] + a.data[ 7] * b.data[ 5] + a.data[11] * b.data[ 6] + a.data[15] * b.data[ 7],
167                   a.data[ 3] * b.data[ 8] + a.data[ 7] * b.data[ 9] + a.data[11] * b.data[10] + a.data[15] * b.data[11],
168                   a.data[ 3] * b.data[12] + a.data[ 7] * b.data[13] + a.data[11] * b.data[14] + a.data[15] * b.data[15]);
169}
170
171Matrix4 &operator*=(Matrix4 &a, const Matrix4 &b)
172{
173    a = a * b;
174    return a;
175}
176
177Matrix4 operator*(const Matrix4 &a, float b)
178{
179    Matrix4 ret(a);
180    for (size_t i = 0; i < 16; i++)
181    {
182        ret.data[i] *= b;
183    }
184    return ret;
185}
186
187Matrix4 &operator*=(Matrix4 &a, float b)
188{
189    for (size_t i = 0; i < 16; i++)
190    {
191        a.data[i] *= b;
192    }
193    return a;
194}
195
196Vector4 operator*(const Matrix4 &a, const Vector4 &b)
197{
198    return Vector4(a.data[ 0] * b.x + a.data[ 4] * b.y + a.data[ 8] * b.z + a.data[12] * b.w,
199                   a.data[ 1] * b.x + a.data[ 5] * b.y + a.data[ 9] * b.z + a.data[13] * b.w,
200                   a.data[ 2] * b.x + a.data[ 6] * b.y + a.data[10] * b.z + a.data[14] * b.w,
201                   a.data[ 3] * b.x + a.data[ 7] * b.y + a.data[11] * b.z + a.data[15] * b.w);
202}
203
204bool operator==(const Matrix4 &a, const Matrix4 &b)
205{
206    for (size_t i = 0; i < 16; i++)
207    {
208        if (a.data[i] != b.data[i])
209        {
210            return false;
211        }
212    }
213    return true;
214}
215
216bool operator!=(const Matrix4 &a, const Matrix4 &b)
217{
218    return !(a == b);
219}
220