1#include "Library/Matrix/MatrixUtil.h"
2
3#include "Library/Math/MathUtil.h"
4
5namespace al {
6
7void makeMtxRotateTrans(sead::Matrix34f* outMtx, const sead::Vector3f& rotate,
8 const sead::Vector3f& trans) {
9 sead::Vector3f rotateRad(sead::Mathf::deg2rad(deg: rotate.x), sead::Mathf::deg2rad(deg: rotate.y),
10 sead::Mathf::deg2rad(deg: rotate.z));
11 outMtx->makeRT(r: rotateRad, t: trans);
12}
13
14void makeMtxFromTwoAxis(sead::Matrix34f* outMtx, const sead::Vector3f& vectorA,
15 const sead::Vector3f& vectorB, s32 axisA, s32 axisB) {
16 sead::Vector3f dir[3];
17
18 s32 axisC;
19 if (axisA == 0)
20 axisC = axisB == 1 ? 2 : 1;
21 else if (axisA == 1)
22 axisC = axisB == 0 ? 2 : 0;
23 else
24 axisC = axisB == 0 ? 1 : 0;
25
26 normalize(out: &dir[axisA], vec: vectorA);
27 dir[axisB] = vectorB;
28
29 dir[axisC].setCross(a: dir[(axisC + 1) % 3], b: dir[(axisC + 2) % 3]);
30 normalize(vec: &dir[axisC]);
31
32 dir[axisB].setCross(a: dir[(axisB + 1) % 3], b: dir[(axisB + 2) % 3]);
33
34 outMtx->setBase(axis: 0, v: dir[0]); // side
35 outMtx->setBase(axis: 1, v: dir[1]); // up
36 outMtx->setBase(axis: 2, v: dir[2]); // front
37}
38
39void makeMtxFrontUp(sead::Matrix34f* outMtx, const sead::Vector3f& front,
40 const sead::Vector3f& up) {
41 sead::Vector3f frontNorm = front;
42 normalize(vec: &frontNorm);
43
44 sead::Vector3f side;
45 side.setCross(a: up, b: frontNorm);
46 normalize(vec: &side);
47
48 sead::Vector3f upNorm;
49 upNorm.setCross(a: frontNorm, b: side);
50
51 outMtx->setBase(axis: 0, v: side);
52 outMtx->setBase(axis: 1, v: upNorm);
53 outMtx->setBase(axis: 2, v: frontNorm);
54}
55
56void makeMtxFrontSide(sead::Matrix34f* outMtx, const sead::Vector3f& front,
57 const sead::Vector3f& side) {
58 sead::Vector3f frontNorm = front;
59 normalize(vec: &frontNorm);
60
61 sead::Vector3f up;
62 up.setCross(a: frontNorm, b: side);
63 normalize(vec: &up);
64
65 sead::Vector3f sideNorm;
66 sideNorm.setCross(a: up, b: frontNorm);
67
68 outMtx->setBase(axis: 0, v: sideNorm);
69 outMtx->setBase(axis: 1, v: up);
70 outMtx->setBase(axis: 2, v: frontNorm);
71}
72
73void makeMtxUpFront(sead::Matrix34f* outMtx, const sead::Vector3f& up,
74 const sead::Vector3f& front) {
75 sead::Vector3f upNorm = up;
76 normalize(vec: &upNorm);
77
78 sead::Vector3f side;
79 side.setCross(a: up, b: front);
80 normalize(vec: &side);
81
82 sead::Vector3f frontNorm;
83 frontNorm.setCross(a: side, b: upNorm);
84
85 outMtx->setBase(axis: 0, v: side);
86 outMtx->setBase(axis: 1, v: upNorm);
87 outMtx->setBase(axis: 2, v: frontNorm);
88}
89
90void makeMtxUpSide(sead::Matrix34f* outMtx, const sead::Vector3f& up, const sead::Vector3f& side) {
91 sead::Vector3f upNorm = up;
92 normalize(vec: &upNorm);
93
94 sead::Vector3f front;
95 front.setCross(a: side, b: up);
96 normalize(vec: &front);
97
98 sead::Vector3f sideNorm;
99 sideNorm.setCross(a: upNorm, b: front);
100
101 outMtx->setBase(axis: 0, v: sideNorm);
102 outMtx->setBase(axis: 1, v: upNorm);
103 outMtx->setBase(axis: 2, v: front);
104}
105
106void makeMtxSideUp(sead::Matrix34f* outMtx, const sead::Vector3f& side, const sead::Vector3f& up) {
107 makeMtxFromTwoAxis(outMtx, vectorA: side, vectorB: up, axisA: 0, axisB: 1);
108}
109
110void makeMtxSideFront(sead::Matrix34f* outMtx, const sead::Vector3f& side,
111 const sead::Vector3f& front) {
112 makeMtxFromTwoAxis(outMtx, vectorA: side, vectorB: front, axisA: 0, axisB: 2);
113}
114
115void makeMtxFrontNoSupport(sead::Matrix34f* outMtx, const sead::Vector3f& front) {
116 bool isYAxis = getMaxAbsElementIndex(vec: front) == 1;
117
118 sead::Vector3f up;
119 up.x = isYAxis ? sead::Vector3f::ez.x : sead::Vector3f::ey.x;
120 up.y = isYAxis ? sead::Vector3f::ez.y : sead::Vector3f::ey.y;
121 up.z = isYAxis ? sead::Vector3f::ez.z : sead::Vector3f::ey.z;
122
123 makeMtxFrontUp(outMtx, front, up);
124}
125
126void makeMtxFrontNoSupportPos(sead::Matrix34f* outMtx, const sead::Vector3f& front,
127 const sead::Vector3f& pos) {
128 makeMtxFrontNoSupport(outMtx, front);
129 outMtx->setBase(axis: 3, v: pos);
130}
131
132void makeMtxUpNoSupport(sead::Matrix34f* outMtx, const sead::Vector3f& up) {
133 bool isZAxis = getMaxAbsElementIndex(vec: up) == 2;
134
135 sead::Vector3f front;
136 front.x = isZAxis ? sead::Vector3f::ex.x : sead::Vector3f::ez.x;
137 front.y = isZAxis ? sead::Vector3f::ex.y : sead::Vector3f::ez.y;
138 front.z = isZAxis ? sead::Vector3f::ex.z : sead::Vector3f::ez.z;
139
140 makeMtxUpFront(outMtx, up, front);
141}
142
143void makeMtxUpNoSupportPos(sead::Matrix34f* outMtx, const sead::Vector3f& up,
144 const sead::Vector3f& pos) {
145 makeMtxUpNoSupport(outMtx, up);
146 outMtx->setBase(axis: 3, v: pos);
147}
148
149void makeMtxFrontUpPos(sead::Matrix34f* outMtx, const sead::Vector3f& front,
150 const sead::Vector3f& up, const sead::Vector3f& pos) {
151 makeMtxFrontUp(outMtx, front, up);
152 outMtx->setBase(axis: 3, v: pos);
153}
154
155void makeMtxFrontSidePos(sead::Matrix34f* outMtx, const sead::Vector3f& front,
156 const sead::Vector3f& side, const sead::Vector3f& pos) {
157 makeMtxFrontSide(outMtx, front, side);
158 outMtx->setBase(axis: 3, v: pos);
159}
160
161void makeMtxUpFrontPos(sead::Matrix34f* outMtx, const sead::Vector3f& up,
162 const sead::Vector3f& front, const sead::Vector3f& pos) {
163 makeMtxUpFront(outMtx, up, front);
164 outMtx->setBase(axis: 3, v: pos);
165}
166
167void makeMtxUpSidePos(sead::Matrix34f* outMtx, const sead::Vector3f& up, const sead::Vector3f& side,
168 const sead::Vector3f& pos) {
169 makeMtxUpSide(outMtx, up, side);
170 outMtx->setBase(axis: 3, v: pos);
171}
172
173void makeMtxSideUpPos(sead::Matrix34f* outMtx, const sead::Vector3f& side, const sead::Vector3f& up,
174 const sead::Vector3f& pos) {
175 makeMtxSideUp(outMtx, side, up);
176 outMtx->setBase(axis: 3, v: pos);
177}
178
179void makeMtxSideFrontPos(sead::Matrix34f* outMtx, const sead::Vector3f& side,
180 const sead::Vector3f& front, const sead::Vector3f& pos) {
181 makeMtxSideFront(outMtx, side, front);
182 outMtx->setBase(axis: 3, v: pos);
183}
184
185void makeMtxFollowTarget(sead::Matrix34f* outMtx, const sead::Matrix34f& baseMtx,
186 const sead::Vector3f& trans, const sead::Vector3f& rotate) {
187 sead::Matrix34f rotationMatrix;
188 sead::Vector3f rotateRad(sead::Mathf::deg2rad(deg: rotate.x), sead::Mathf::deg2rad(deg: rotate.y),
189 sead::Mathf::deg2rad(deg: rotate.z));
190 rotationMatrix.makeR(r: rotateRad);
191
192 sead::Matrix34f translationMatrix;
193 translationMatrix.makeRT(r: {0.0f, 0.0f, 0.0f}, t: trans);
194
195 sead::Matrix34f pose = rotationMatrix * translationMatrix;
196 *outMtx = baseMtx * pose;
197}
198
199void rotateMtxXDirDegree(sead::Matrix34f* outMtx, const sead::Matrix34f& baseMtx, f32 angle) {
200 sead::Matrix34f rotationMatrix;
201 rotationMatrix.makeR(r: sead::Mathf::deg2rad(deg: angle) * sead::Vector3f::ex);
202
203 *outMtx = baseMtx * rotationMatrix;
204}
205
206void rotateMtxYDirDegree(sead::Matrix34f* outMtx, const sead::Matrix34f& baseMtx, f32 angle) {
207 sead::Matrix34f rotationMatrix;
208 rotationMatrix.makeR(r: sead::Mathf::deg2rad(deg: angle) * sead::Vector3f::ey);
209
210 *outMtx = baseMtx * rotationMatrix;
211}
212
213void rotateMtxZDirDegree(sead::Matrix34f* outMtx, const sead::Matrix34f& baseMtx, f32 angle) {
214 sead::Matrix34f rotationMatrix;
215 rotationMatrix.makeR(r: sead::Mathf::deg2rad(deg: angle) * sead::Vector3f::ez);
216
217 *outMtx = baseMtx * rotationMatrix;
218}
219
220void calcMtxScale(sead::Vector3f* outMtx, const sead::Matrix34f& mtx) {
221 outMtx->x = sead::Mathf::sqrt(t: mtx.m[0][0] * mtx.m[0][0] + mtx.m[1][0] * mtx.m[1][0] +
222 mtx.m[2][0] * mtx.m[2][0]);
223
224 outMtx->y = sead::Mathf::sqrt(t: mtx.m[0][1] * mtx.m[0][1] + mtx.m[1][1] * mtx.m[1][1] +
225 mtx.m[2][1] * mtx.m[2][1]);
226
227 outMtx->z = sead::Mathf::sqrt(t: mtx.m[0][2] * mtx.m[0][2] + mtx.m[1][2] * mtx.m[1][2] +
228 mtx.m[2][2] * mtx.m[2][2]);
229}
230
231void calcMtxScale(sead::Vector3f* outMtx, const Matrix43f& mtx) {
232 outMtx->x = sead::Mathf::sqrt(t: mtx.m[0][0] * mtx.m[0][0] + mtx.m[0][1] * mtx.m[0][1] +
233 mtx.m[0][2] * mtx.m[0][2]);
234
235 outMtx->y = sead::Mathf::sqrt(t: mtx.m[1][0] * mtx.m[1][0] + mtx.m[1][1] * mtx.m[1][1] +
236 mtx.m[1][2] * mtx.m[1][2]);
237
238 outMtx->z = sead::Mathf::sqrt(t: mtx.m[2][0] * mtx.m[2][0] + mtx.m[2][1] * mtx.m[2][1] +
239 mtx.m[2][2] * mtx.m[2][2]);
240}
241
242void normalizeMtxScale(sead::Matrix34f* outMtx, const sead::Matrix34f& mtx) {
243 sead::Vector3f scale;
244 calcMtxScale(outMtx: &scale, mtx);
245
246 if (!isNearZero(value: scale.x, tolerance: 0.001f) && !isNearZero(value: scale.y, tolerance: 0.001f) &&
247 !isNearZero(value: scale.z, tolerance: 0.001f)) {
248 f32 xInv = 1.0f / scale.x;
249 f32 yInv = 1.0f / scale.y;
250 f32 zInv = 1.0f / scale.z;
251
252 *outMtx = mtx;
253 outMtx->scaleBases(sx: xInv, sy: yInv, sz: zInv);
254 }
255}
256
257bool tryNormalizeMtxScaleOrIdentity(sead::Matrix34f* outMtx, const sead::Matrix34f& mtx) {
258 sead::Vector3f scale;
259 calcMtxScale(outMtx: &scale, mtx);
260
261 if (!isNearZero(value: scale.x, tolerance: 0.001f) && !isNearZero(value: scale.y, tolerance: 0.001f) &&
262 !isNearZero(value: scale.z, tolerance: 0.001f)) {
263 f32 xInv = 1.0f / scale.x;
264 f32 yInv = 1.0f / scale.y;
265 f32 zInv = 1.0f / scale.z;
266
267 *outMtx = mtx;
268 outMtx->scaleBases(sx: xInv, sy: yInv, sz: zInv);
269 return true;
270 }
271
272 sead::Vector3f base = mtx.getBase(axis: 3);
273 outMtx->makeIdentity();
274 outMtx->setBase(axis: 3, v: base);
275
276 return false;
277}
278
279void preScaleMtx(sead::Matrix34f* outMtx, const sead::Vector3f& scale) {
280 outMtx->scaleBases(sx: scale.x, sy: scale.y, sz: scale.z);
281}
282
283} // namespace al
284