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