1/*
2 * Copyright (c) 2022 Huawei Device Co., Ltd.
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 "wm_math.h"
16#include <cstdlib>
17
18namespace OHOS::Rosen {
19namespace TransformHelper {
20const Matrix3 Matrix3::Identity = { {
21    { 1, 0, 0 },
22    { 0, 1, 0 },
23    { 0, 0, 1 },
24} };
25const Matrix4 Matrix4::Identity = { {
26    { 1, 0, 0, 0 },
27    { 0, 1, 0, 0 },
28    { 0, 0, 1, 0 },
29    { 0, 0, 0, 1 },
30} };
31
32Matrix3 operator*(const Matrix3& left, const Matrix3& right)
33{
34    return  { {
35        // row 0
36        { left.mat_[0][0] * right.mat_[0][0] + left.mat_[0][1] * right.mat_[1][0] + left.mat_[0][2] * right.mat_[2][0],
37        left.mat_[0][0] * right.mat_[0][1] + left.mat_[0][1] * right.mat_[1][1] + left.mat_[0][2] * right.mat_[2][1],
38        left.mat_[0][0] * right.mat_[0][2] + left.mat_[0][1] * right.mat_[1][2] + left.mat_[0][2] * right.mat_[2][2] },
39
40        // row 1
41        { left.mat_[1][0] * right.mat_[0][0] + left.mat_[1][1] * right.mat_[1][0] + left.mat_[1][2] * right.mat_[2][0],
42        left.mat_[1][0] * right.mat_[0][1] + left.mat_[1][1] * right.mat_[1][1] + left.mat_[1][2] * right.mat_[2][1],
43        left.mat_[1][0] * right.mat_[0][2] + left.mat_[1][1] * right.mat_[1][2] + left.mat_[1][2] * right.mat_[2][2] },
44
45        // row 2
46        { left.mat_[2][0] * right.mat_[0][0] + left.mat_[2][1] * right.mat_[1][0] + left.mat_[2][2] * right.mat_[2][0],
47        left.mat_[2][0] * right.mat_[0][1] + left.mat_[2][1] * right.mat_[1][1] + left.mat_[2][2] * right.mat_[2][1],
48        left.mat_[2][0] * right.mat_[0][2] + left.mat_[2][1] * right.mat_[1][2] + left.mat_[2][2] * right.mat_[2][2] }
49    } };
50}
51
52Matrix3& Matrix3::operator*=(const Matrix3& right)
53{
54    *this = *this * right;
55    return *this;
56}
57
58Matrix4 operator*(const Matrix4& left, const Matrix4& right)
59{
60    return { {
61        // row 0
62        { left.mat_[0][0] * right.mat_[0][0] + left.mat_[0][1] * right.mat_[1][0] +
63        left.mat_[0][2] * right.mat_[2][0] + left.mat_[0][3] * right.mat_[3][0],
64        left.mat_[0][0] * right.mat_[0][1] + left.mat_[0][1] * right.mat_[1][1] +
65        left.mat_[0][2] * right.mat_[2][1] + left.mat_[0][3] * right.mat_[3][1],
66        left.mat_[0][0] * right.mat_[0][2] + left.mat_[0][1] * right.mat_[1][2] +
67        left.mat_[0][2] * right.mat_[2][2] + left.mat_[0][3] * right.mat_[3][2],
68        left.mat_[0][0] * right.mat_[0][3] + left.mat_[0][1] * right.mat_[1][3] +
69        left.mat_[0][2] * right.mat_[2][3] + left.mat_[0][3] * right.mat_[3][3] },
70
71        // row 1
72        { left.mat_[1][0] * right.mat_[0][0] + left.mat_[1][1] * right.mat_[1][0] +
73        left.mat_[1][2] * right.mat_[2][0] + left.mat_[1][3] * right.mat_[3][0],
74        left.mat_[1][0] * right.mat_[0][1] + left.mat_[1][1] * right.mat_[1][1] +
75        left.mat_[1][2] * right.mat_[2][1] + left.mat_[1][3] * right.mat_[3][1],
76        left.mat_[1][0] * right.mat_[0][2] + left.mat_[1][1] * right.mat_[1][2] +
77        left.mat_[1][2] * right.mat_[2][2] + left.mat_[1][3] * right.mat_[3][2],
78        left.mat_[1][0] * right.mat_[0][3] + left.mat_[1][1] * right.mat_[1][3] +
79        left.mat_[1][2] * right.mat_[2][3] + left.mat_[1][3] * right.mat_[3][3] },
80
81        // row 2
82        { left.mat_[2][0] * right.mat_[0][0] + left.mat_[2][1] * right.mat_[1][0] +
83        left.mat_[2][2] * right.mat_[2][0] + left.mat_[2][3] * right.mat_[3][0],
84        left.mat_[2][0] * right.mat_[0][1] + left.mat_[2][1] * right.mat_[1][1] +
85        left.mat_[2][2] * right.mat_[2][1] + left.mat_[2][3] * right.mat_[3][1],
86        left.mat_[2][0] * right.mat_[0][2] + left.mat_[2][1] * right.mat_[1][2] +
87        left.mat_[2][2] * right.mat_[2][2] + left.mat_[2][3] * right.mat_[3][2],
88        left.mat_[2][0] * right.mat_[0][3] + left.mat_[2][1] * right.mat_[1][3] +
89        left.mat_[2][2] * right.mat_[2][3] + left.mat_[2][3] * right.mat_[3][3] },
90
91        // row 3
92        { left.mat_[3][0] * right.mat_[0][0] + left.mat_[3][1] * right.mat_[1][0] +
93        left.mat_[3][2] * right.mat_[2][0] + left.mat_[3][3] * right.mat_[3][0],
94        left.mat_[3][0] * right.mat_[0][1] + left.mat_[3][1] * right.mat_[1][1] +
95        left.mat_[3][2] * right.mat_[2][1] + left.mat_[3][3] * right.mat_[3][1],
96        left.mat_[3][0] * right.mat_[0][2] + left.mat_[3][1] * right.mat_[1][2] +
97        left.mat_[3][2] * right.mat_[2][2] + left.mat_[3][3] * right.mat_[3][2],
98        left.mat_[3][0] * right.mat_[0][3] + left.mat_[3][1] * right.mat_[1][3] +
99        left.mat_[3][2] * right.mat_[2][3] + left.mat_[3][3] * right.mat_[3][3] }
100    } };
101}
102
103Matrix4& Matrix4::operator*=(const Matrix4& right)
104{
105    *this = *this * right;
106    return *this;
107}
108
109void Matrix4::SwapRow(int row1, int row2)
110{
111    float *p = mat_[row1];
112    float *q = mat_[row2];
113    float tmp = p[0];
114    p[0] = q[0];
115    q[0] = tmp;
116
117    tmp = p[1];
118    p[1] = q[1];
119    q[1] = tmp;
120
121    tmp = p[2]; // 2: row2
122    p[2] = q[2]; // 2: row2
123    q[2] = tmp; // 2: row2
124
125    tmp = p[3]; // 3: row3
126    p[3] = q[3]; // 3: row3
127    q[3] = tmp; // 3: row3
128}
129
130void Matrix4::Invert()
131{
132    // Inverse matrix with Gauss-Jordan method
133    Matrix4 tmp = Matrix4::Identity;
134    int i, j, k;
135    for (k = 0; k < MAT_SIZE; k++) {
136        float t = mat_[k][k];
137        if (t < MathHelper::POS_ZERO && t > MathHelper::NAG_ZERO) {
138            for (i = k + 1; i < MAT_SIZE; i++) {
139                if (mat_[i][k] < MathHelper::NAG_ZERO || mat_[i][k] > MathHelper::POS_ZERO) {
140                    SwapRow(k, i);
141                    tmp.SwapRow(k, i);
142                    break;
143                }
144            }
145            t = mat_[k][k];
146        }
147        for (j = 0; j <= k; j++) {
148            tmp.mat_[k][j] /= t;
149        }
150        for (; j < MAT_SIZE; j++) {
151            mat_[k][j] /= t;
152            tmp.mat_[k][j] /= t;
153        }
154        for (i = 0; i < MAT_SIZE; i++) {
155            if (i == k) {
156                continue;
157            }
158            float u = mat_[i][k];
159            for (j = 0; j <= k; j++) {
160                tmp.mat_[i][j] -= tmp.mat_[k][j] * u;
161            }
162            for (; j < MAT_SIZE; j++) {
163                mat_[i][j] -= mat_[k][j] * u;
164                tmp.mat_[i][j] -= tmp.mat_[k][j] * u;
165            }
166        }
167    }
168    *this = tmp;
169}
170
171Vector3 Matrix4::GetScale() const
172{
173    Vector3 retVal;
174    retVal.x_ = Vector3(mat_[0][0], mat_[0][1], mat_[0][2]).Length(); // 2: column2
175    retVal.y_ = Vector3(mat_[1][0], mat_[1][1], mat_[1][2]).Length(); // 2: column2
176    retVal.z_ = Vector3(mat_[2][0], mat_[2][1], mat_[2][2]).Length(); // 2: row2
177    return retVal;
178}
179
180Vector3 Matrix4::GetTranslation() const
181{
182    return Vector3(mat_[3][0], mat_[3][1], mat_[3][2]); // 3: row3, 2: column2
183}
184
185// Create a scale matrix with x and y scales
186Matrix3 CreateScale(float xScale, float yScale)
187{
188    return { {
189        { xScale, 0.0f, 0.0f },
190        { 0.0f, yScale, 0.0f },
191        { 0.0f, 0.0f, 1.0f },
192    } };
193}
194
195// Create a rotation matrix about the Z axis
196// theta is in radians
197Matrix3 CreateRotation(float theta)
198{
199    return { {
200        { std::cos(theta), std::sin(theta), 0.0f },
201        { -std::sin(theta), std::cos(theta), 0.0f },
202        { 0.0f, 0.0f, 1.0f },
203    } };
204}
205
206// Create a translation matrix (on the xy-plane)
207Matrix3 CreateTranslation(const Vector2& trans)
208{
209    return { {
210        { 1.0f, 0.0f, 0.0f },
211        { 0.0f, 1.0f, 0.0f },
212        { trans.x_, trans.y_, 1.0f },
213    } };
214}
215
216// Create a scale matrix with x, y, and z scales
217Matrix4 CreateScale(float xScale, float yScale, float zScale)
218{
219    return { {
220        { xScale, 0.0f, 0.0f, 0.0f },
221        { 0.0f, yScale, 0.0f, 0.0f },
222        { 0.0f, 0.0f, zScale, 0.0f },
223        { 0.0f, 0.0f, 0.0f, 1.0f },
224    } };
225}
226
227// Create a rotation matrix about X axis
228// theta is in radians
229Matrix4 CreateRotationX(float theta)
230{
231    return { {
232        { 1.0f, 0.0f, 0.0f, 0.0f },
233        { 0.0f, std::cos(theta), std::sin(theta), 0.0f },
234        { 0.0f, -std::sin(theta), std::cos(theta), 0.0f },
235        { 0.0f, 0.0f, 0.0f, 1.0f },
236    } };
237}
238
239// Create a rotation matrix about Y axis
240// theta is in radians
241Matrix4 CreateRotationY(float theta)
242{
243    return { {
244        { std::cos(theta), 0.0f, -std::sin(theta), 0.0f },
245        { 0.0f, 1.0f, 0.0f, 0.0f },
246        { std::sin(theta), 0.0f, std::cos(theta), 0.0f },
247        { 0.0f, 0.0f, 0.0f, 1.0f },
248    } };
249}
250
251// Create a rotation matrix about Z axis
252// theta is in radians
253Matrix4 CreateRotationZ(float theta)
254{
255    return { {
256        { std::cos(theta), std::sin(theta), 0.0f, 0.0f },
257        { -std::sin(theta), std::cos(theta), 0.0f, 0.0f },
258        { 0.0f, 0.0f, 1.0f, 0.0f },
259        { 0.0f, 0.0f, 0.0f, 1.0f },
260    } };
261}
262
263// Create a 3D translation matrix
264Matrix4 CreateTranslation(const Vector3& trans)
265{
266    return { {
267        { 1.0f, 0.0f, 0.0f, 0.0f },
268        { 0.0f, 1.0f, 0.0f, 0.0f },
269        { 0.0f, 0.0f, 1.0f, 0.0f },
270        { trans.x_, trans.y_, trans.z_, 1.0f },
271    } };
272}
273
274Matrix4 CreateLookAt(const Vector3& eye, const Vector3& target, const Vector3& up)
275{
276    Vector3 zaxis = Vector3::Normalize(target - eye);
277    Vector3 xaxis = Vector3::Normalize(Vector3::Cross(up, zaxis));
278    Vector3 yaxis = Vector3::Normalize(Vector3::Cross(zaxis, xaxis));
279    Vector3 trans;
280    trans.x_ = -Vector3::Dot(xaxis, eye);
281    trans.y_ = -Vector3::Dot(yaxis, eye);
282    trans.z_ = -Vector3::Dot(zaxis, eye);
283
284    return { {
285        { xaxis.x_, yaxis.x_, zaxis.x_, 0.0f },
286        { xaxis.y_, yaxis.y_, zaxis.y_, 0.0f },
287        { xaxis.z_, yaxis.z_, zaxis.z_, 0.0f },
288        { trans.x_, trans.y_, trans.z_, 1.0f }
289    } };
290}
291
292Matrix4 CreatePerspective(const Vector3& camera)
293{
294    return { {
295        { std::abs(camera.z_), 0.0f,                0.0f, 0.0f },
296        { 0.0f,                std::abs(camera.z_), 0.0f, 0.0f },
297        { camera.x_,           camera.y_,           0.0f, 1.0f },
298        { 0.0f,                0.0f,                1.0f, 0.0f },
299    } };
300}
301
302// Transform a Vector2 in xy-plane by matrix3
303Vector2 Transform(const Vector2& vec, const Matrix3& mat)
304{
305    Vector2 retVal;
306    retVal.x_ = vec.x_ * mat.mat_[0][0] + vec.y_ * mat.mat_[1][0] + mat.mat_[2][0]; // 2: row2
307    retVal.y_ = vec.x_ * mat.mat_[0][1] + vec.y_ * mat.mat_[1][1] + mat.mat_[2][1]; // 2: row2
308    return retVal;
309}
310
311// Transform a Vector3 in 3D world by matrix4
312Vector3 Transform(const Vector3& vec, const Matrix4& mat)
313{
314    Vector3 retVal;
315    retVal.x_ = vec.x_ * mat.mat_[0][0] + vec.y_ * mat.mat_[1][0] +
316        vec.z_ * mat.mat_[2][0] + mat.mat_[3][0]; // 2: row2, 3: row3
317    retVal.y_ = vec.x_ * mat.mat_[0][1] + vec.y_ * mat.mat_[1][1] +
318        vec.z_ * mat.mat_[2][1] + mat.mat_[3][1]; // 2: row2, 3: row3
319    retVal.z_ = vec.x_ * mat.mat_[0][2] + vec.y_ * mat.mat_[1][2] + // 2: row2
320        vec.z_ * mat.mat_[2][2] + mat.mat_[3][2]; // 2: row2, 3: row3
321    return retVal;
322}
323
324// Transform the vector and renormalize the w component
325Vector3 TransformWithPerspDiv(const Vector3& vec, const Matrix4& mat, float w)
326{
327    Vector3 retVal;
328    retVal.x_ = vec.x_ * mat.mat_[0][0] + vec.y_ * mat.mat_[1][0] +
329        vec.z_ * mat.mat_[2][0] + w * mat.mat_[3][0]; // 2: row2, 3: row3
330    retVal.y_ = vec.x_ * mat.mat_[0][1] + vec.y_ * mat.mat_[1][1] +
331        vec.z_ * mat.mat_[2][1] + w * mat.mat_[3][1]; // 2: row2, 3: row3
332    retVal.z_ = vec.x_ * mat.mat_[0][2] + vec.y_ * mat.mat_[1][2] +
333        vec.z_ * mat.mat_[2][2] + w * mat.mat_[3][2]; // 2: row2, 3: row3
334    float transformedW = vec.x_ * mat.mat_[0][3] + vec.y_ * mat.mat_[1][3] +
335        vec.z_ * mat.mat_[2][3] + w * mat.mat_[3][3]; // 2: row2, 3: row3
336    if (!MathHelper::NearZero(transformedW)) {
337        transformedW = 1.0f / transformedW;
338        retVal *= transformedW;
339    }
340    return retVal;
341}
342
343// Given a screen point, unprojects it into origin position at screen,
344// based on the current transform matrix
345Vector2 GetOriginScreenPoint(const Vector2& p, const Matrix4& mat)
346{
347    Matrix4 invertMat = mat;
348    invertMat.Invert();
349    // Get start point
350    Vector3 screenPoint(p.x_, p.y_, 0.1f);
351    Vector3 start = TransformWithPerspDiv(screenPoint, invertMat);
352    // Get end point
353    screenPoint.z_ = 0.9f;
354    Vector3 end = TransformWithPerspDiv(screenPoint, invertMat);
355    // Get the intersection point of line start-end and xy plane
356    float t = end.z_ / (end.z_ - start.z_);
357    return Vector2(t * start.x_ + (1 - t) * end.x_, t * start.y_ + (1 - t) * end.y_);
358}
359} // namespace TransformHelper
360} // namespace OHOS::Rosen
361