OLD | NEW |
(Empty) | |
| 1 // Copyright 2016 The Chromium Authors. All rights reserved. |
| 2 // Use of this source code is governed by a BSD-style license that can be |
| 3 // found in the LICENSE file. |
| 4 |
| 5 #include "device/vr/vr_math.h" |
| 6 |
| 7 #include <cmath> |
| 8 |
| 9 #include "base/logging.h" |
| 10 |
| 11 namespace vr { |
| 12 |
| 13 // Internal matrix layout: |
| 14 // |
| 15 // m[0][0], m[0][1], m[0][2], m[0][3], |
| 16 // m[1][0], m[1][1], m[1][2], m[1][3], |
| 17 // m[2][0], m[2][1], m[2][2], m[2][3], |
| 18 // m[3][0], m[3][1], m[3][2], m[3][3], |
| 19 // |
| 20 // The translation component is in the right column m[i][3]. |
| 21 // |
| 22 // The bottom row m[3][i] is (0, 0, 0, 1) for non-perspective transforms. |
| 23 // |
| 24 // These matrices are intended to be used to premultiply column vectors |
| 25 // for transforms, so successive transforms need to be left-multiplied. |
| 26 |
| 27 void SetIdentityM(Matf* mat) { |
| 28 for (int i = 0; i < 4; i++) { |
| 29 for (int j = 0; j < 4; j++) { |
| 30 (*mat)[i][j] = i == j ? 1 : 0; |
| 31 } |
| 32 } |
| 33 } |
| 34 |
| 35 // Left multiply a translation matrix. |
| 36 void TranslateM(const Matf& mat, const gfx::Vector3dF& translation, Matf* out) { |
| 37 if (out != &mat) { |
| 38 for (int i = 0; i < 4; ++i) { |
| 39 for (int j = 0; j < 4; ++j) { |
| 40 (*out)[i][j] = mat[i][j]; |
| 41 } |
| 42 } |
| 43 } |
| 44 (*out)[0][3] += translation.x(); |
| 45 (*out)[1][3] += translation.y(); |
| 46 (*out)[2][3] += translation.z(); |
| 47 } |
| 48 |
| 49 // Left multiply a scale matrix. |
| 50 void ScaleM(const Matf& mat, const gfx::Vector3dF& scale, Matf* out) { |
| 51 if (out != &mat) { |
| 52 for (int i = 0; i < 4; ++i) { |
| 53 for (int j = 0; j < 3; ++j) { |
| 54 (*out)[i][j] = mat[i][j]; |
| 55 } |
| 56 } |
| 57 } |
| 58 // Multiply all rows including translation components. |
| 59 for (int j = 0; j < 4; ++j) { |
| 60 (*out)[0][j] *= scale.x(); |
| 61 (*out)[1][j] *= scale.y(); |
| 62 (*out)[2][j] *= scale.z(); |
| 63 } |
| 64 } |
| 65 |
| 66 gfx::Vector3dF MatrixVectorMul(const Matf& m, const gfx::Vector3dF& v) { |
| 67 return gfx::Vector3dF( |
| 68 m[0][0] * v.x() + m[0][1] * v.y() + m[0][2] * v.z() + m[0][3], |
| 69 m[1][0] * v.x() + m[1][1] * v.y() + m[1][2] * v.z() + m[1][3], |
| 70 m[2][0] * v.x() + m[2][1] * v.y() + m[2][2] * v.z() + m[2][3]); |
| 71 } |
| 72 |
| 73 // Rotation only, ignore translation components. |
| 74 gfx::Vector3dF MatrixVectorRotate(const Matf& m, const gfx::Vector3dF& v) { |
| 75 return gfx::Vector3dF(m[0][0] * v.x() + m[0][1] * v.y() + m[0][2] * v.z(), |
| 76 m[1][0] * v.x() + m[1][1] * v.y() + m[1][2] * v.z(), |
| 77 m[2][0] * v.x() + m[2][1] * v.y() + m[2][2] * v.z()); |
| 78 } |
| 79 |
| 80 void MatrixMul(const Matf& matrix1, const Matf& matrix2, Matf* out) { |
| 81 DCHECK(out != &matrix1 && out != &matrix2); |
| 82 for (int i = 0; i < 4; ++i) { |
| 83 for (int j = 0; j < 4; ++j) { |
| 84 (*out)[i][j] = 0.0f; |
| 85 for (int k = 0; k < 4; ++k) { |
| 86 (*out)[i][j] += matrix1[i][k] * matrix2[k][j]; |
| 87 } |
| 88 } |
| 89 } |
| 90 } |
| 91 |
| 92 void PerspectiveMatrixFromView(const gfx::RectF& fov, |
| 93 float z_near, |
| 94 float z_far, |
| 95 Matf* out) { |
| 96 const float x_left = -std::tan(fov.x() * M_PI / 180.0f) * z_near; |
| 97 const float x_right = std::tan(fov.right() * M_PI / 180.0f) * z_near; |
| 98 const float y_bottom = -std::tan(fov.bottom() * M_PI / 180.0f) * z_near; |
| 99 const float y_top = std::tan(fov.y() * M_PI / 180.0f) * z_near; |
| 100 |
| 101 DCHECK(x_left < x_right && y_bottom < y_top && z_near < z_far && |
| 102 z_near > 0.0f && z_far > 0.0f); |
| 103 const float X = (2 * z_near) / (x_right - x_left); |
| 104 const float Y = (2 * z_near) / (y_top - y_bottom); |
| 105 const float A = (x_right + x_left) / (x_right - x_left); |
| 106 const float B = (y_top + y_bottom) / (y_top - y_bottom); |
| 107 const float C = (z_near + z_far) / (z_near - z_far); |
| 108 const float D = (2 * z_near * z_far) / (z_near - z_far); |
| 109 |
| 110 for (int i = 0; i < 4; ++i) { |
| 111 for (int j = 0; j < 4; ++j) { |
| 112 (*out)[i][j] = 0.0f; |
| 113 } |
| 114 } |
| 115 (*out)[0][0] = X; |
| 116 (*out)[0][2] = A; |
| 117 (*out)[1][1] = Y; |
| 118 (*out)[1][2] = B; |
| 119 (*out)[2][2] = C; |
| 120 (*out)[2][3] = D; |
| 121 (*out)[3][2] = -1; |
| 122 } |
| 123 |
| 124 gfx::Vector3dF GetForwardVector(const Matf& matrix) { |
| 125 // Same as multiplying the inverse of the rotation component of the matrix by |
| 126 // (0, 0, -1, 0). |
| 127 return gfx::Vector3dF(-matrix[2][0], -matrix[2][1], -matrix[2][2]); |
| 128 } |
| 129 |
| 130 gfx::Vector3dF GetTranslation(const Matf& matrix) { |
| 131 return gfx::Vector3dF(matrix[0][3], matrix[1][3], matrix[2][3]); |
| 132 } |
| 133 |
| 134 float NormalizeVector(gfx::Vector3dF* vec) { |
| 135 float len = vec->Length(); |
| 136 if (len == 0) |
| 137 return 0; |
| 138 vec->Scale(1.0f / len); |
| 139 return len; |
| 140 } |
| 141 |
| 142 void NormalizeQuat(Quatf* quat) { |
| 143 float len = sqrt(quat->qx * quat->qx + quat->qy * quat->qy + |
| 144 quat->qz * quat->qz + quat->qw * quat->qw); |
| 145 quat->qx /= len; |
| 146 quat->qy /= len; |
| 147 quat->qz /= len; |
| 148 quat->qw /= len; |
| 149 } |
| 150 |
| 151 Quatf QuatFromAxisAngle(const RotationAxisAngle& axis_angle) { |
| 152 // Rotation angle is the product of |angle| and the magnitude of |axis|. |
| 153 gfx::Vector3dF normal(axis_angle.x, axis_angle.y, axis_angle.z); |
| 154 float length = NormalizeVector(&normal); |
| 155 float angle = axis_angle.angle * length; |
| 156 |
| 157 Quatf res; |
| 158 float s = sin(angle / 2); |
| 159 res.qx = normal.x() * s; |
| 160 res.qy = normal.y() * s; |
| 161 res.qz = normal.z() * s; |
| 162 res.qw = cos(angle / 2); |
| 163 return res; |
| 164 } |
| 165 |
| 166 Quatf QuatMultiply(const Quatf& a, const Quatf& b) { |
| 167 Quatf res; |
| 168 res.qw = a.qw * b.qw - a.qx * b.qx - a.qy * b.qy - a.qz * b.qz; |
| 169 res.qx = a.qw * b.qx + a.qx * b.qw + a.qy * b.qz - a.qz * b.qy; |
| 170 res.qy = a.qw * b.qy - a.qx * b.qz + a.qy * b.qw + a.qz * b.qx; |
| 171 res.qz = a.qw * b.qz + a.qx * b.qy - a.qy * b.qx + a.qz * b.qw; |
| 172 return res; |
| 173 } |
| 174 |
| 175 void QuatToMatrix(const Quatf& quat, Matf* out) { |
| 176 const float x2 = quat.qx * quat.qx; |
| 177 const float y2 = quat.qy * quat.qy; |
| 178 const float z2 = quat.qz * quat.qz; |
| 179 const float xy = quat.qx * quat.qy; |
| 180 const float xz = quat.qx * quat.qz; |
| 181 const float xw = quat.qx * quat.qw; |
| 182 const float yz = quat.qy * quat.qz; |
| 183 const float yw = quat.qy * quat.qw; |
| 184 const float zw = quat.qz * quat.qw; |
| 185 |
| 186 const float m11 = 1.0f - 2.0f * y2 - 2.0f * z2; |
| 187 const float m12 = 2.0f * (xy - zw); |
| 188 const float m13 = 2.0f * (xz + yw); |
| 189 const float m21 = 2.0f * (xy + zw); |
| 190 const float m22 = 1.0f - 2.0f * x2 - 2.0f * z2; |
| 191 const float m23 = 2.0f * (yz - xw); |
| 192 const float m31 = 2.0f * (xz - yw); |
| 193 const float m32 = 2.0f * (yz + xw); |
| 194 const float m33 = 1.0f - 2.0f * x2 - 2.0f * y2; |
| 195 |
| 196 *out = {{{{m11, m12, m13, 0.0f}}, |
| 197 {{m21, m22, m23, 0.0f}}, |
| 198 {{m31, m32, m33, 0.0f}}, |
| 199 {{0.0f, 0.0f, 0.0f, 1.0f}}}}; |
| 200 } |
| 201 |
| 202 gfx::Point3F GetRayPoint(const gfx::Point3F& rayOrigin, |
| 203 const gfx::Vector3dF& rayVector, |
| 204 float scale) { |
| 205 return rayOrigin + gfx::ScaleVector3d(rayVector, scale); |
| 206 } |
| 207 |
| 208 float Distance(const gfx::Point3F& p1, const gfx::Point3F& p2) { |
| 209 return std::sqrt(p1.SquaredDistanceTo(p2)); |
| 210 } |
| 211 |
| 212 bool XZAngle(const gfx::Vector3dF& vec1, |
| 213 const gfx::Vector3dF& vec2, |
| 214 float* angle) { |
| 215 float len1 = vec1.Length(); |
| 216 float len2 = vec2.Length(); |
| 217 if (len1 == 0 || len2 == 0) |
| 218 return false; |
| 219 float cross_p = vec1.x() * vec2.z() - vec1.z() * vec2.x(); |
| 220 *angle = asin(cross_p / (len1 * len2)); |
| 221 return true; |
| 222 } |
| 223 |
| 224 } // namespace vr |
OLD | NEW |