1#pragma once
2
3#ifdef cafe
4#include <cafe.h>
5#endif // cafe
6
7#include <math/seadMathCalcCommon.h>
8#include <math/seadQuatCalcCommon.h>
9#ifndef SEAD_MATH_VECTOR_CALC_COMMON_H_
10#include <math/seadVectorCalcCommon.h>
11#endif
12
13namespace sead
14{
15template <typename T>
16inline void Vector2CalcCommon<T>::add(Base& o, const Base& a, const Base& b)
17{
18 o.x = a.x + b.x;
19 o.y = a.y + b.y;
20}
21
22template <typename T>
23inline void Vector2CalcCommon<T>::multScalar(Base& o, const Base& v, T t)
24{
25 o.x = v.x * t;
26 o.y = v.y * t;
27}
28
29template <typename T>
30inline void Vector2CalcCommon<T>::sub(Base& o, const Base& a, const Base& b)
31{
32 o.x = a.x - b.x;
33 o.y = a.y - b.y;
34}
35
36template <typename T>
37inline void Vector2CalcCommon<T>::negate(Base& v)
38{
39 v.x = -v.x;
40 v.y = -v.y;
41}
42template <typename T>
43inline void Vector2CalcCommon<T>::set(Base& o, const Base& v)
44{
45 o = v;
46}
47
48template <typename T>
49inline void Vector2CalcCommon<T>::set(Base& v, T x, T y)
50{
51 v.x = x;
52 v.y = y;
53}
54
55template <typename T>
56inline T Vector2CalcCommon<T>::dot(const Base& a, const Base& b)
57{
58 return a.x * b.x + a.y * b.y;
59}
60
61template <typename T>
62inline T Vector2CalcCommon<T>::cross(const Base& a, const Base& b)
63{
64 return a.x * b.y - a.y * b.x;
65}
66
67template <typename T>
68inline T Vector2CalcCommon<T>::squaredLength(const Base& v)
69{
70 return v.x * v.x + v.y * v.y;
71}
72
73template <typename T>
74inline T Vector2CalcCommon<T>::length(const Base& v)
75{
76 return MathCalcCommon<T>::sqrt(squaredLength(v));
77}
78
79template <typename T>
80T Vector2CalcCommon<T>::normalize(Base& v)
81{
82 const T len = length(v);
83 if (len > 0)
84 {
85 const T inv_len = 1 / len;
86 v.x *= inv_len;
87 v.y *= inv_len;
88 }
89
90 return len;
91}
92
93template <typename T>
94inline void Vector3CalcCommon<T>::add(Base& o, const Base& a, const Base& b)
95{
96 o.x = a.x + b.x;
97 o.y = a.y + b.y;
98 o.z = a.z + b.z;
99}
100
101#ifdef cafe
102
103template <>
104inline void Vector3CalcCommon<f32>::add(Base& o, const Base& a, const Base& b)
105{
106 // TODO: Implement using intrinsics
107 ASM_VECAdd((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
108}
109
110#endif // cafe
111
112template <typename T>
113inline void Vector3CalcCommon<T>::sub(Base& o, const Base& a, const Base& b)
114{
115 o.x = a.x - b.x;
116 o.y = a.y - b.y;
117 o.z = a.z - b.z;
118}
119
120#ifdef cafe
121
122template <>
123inline void Vector3CalcCommon<f32>::sub(Base& o, const Base& a, const Base& b)
124{
125 // TODO: Implement using intrinsics
126 ASM_VECSubtract((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
127}
128
129#endif // cafe
130
131template <typename T>
132inline void Vector3CalcCommon<T>::mul(Base& o, const Mtx33& m, const Base& a)
133{
134 const Base tmp = a;
135 o.x = m.m[0][0] * tmp.x + m.m[0][1] * tmp.y + m.m[0][2] * tmp.z;
136 o.y = m.m[1][0] * tmp.x + m.m[1][1] * tmp.y + m.m[1][2] * tmp.z;
137 o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z;
138}
139
140template <typename T>
141inline void Vector3CalcCommon<T>::mul(Base& o, const Mtx34& m, const Base& a)
142{
143 const Base tmp = a;
144 o.x = m.m[0][0] * tmp.x + m.m[0][1] * tmp.y + m.m[0][2] * tmp.z + m.m[0][3];
145 o.y = m.m[1][0] * tmp.x + m.m[1][1] * tmp.y + m.m[1][2] * tmp.z + m.m[1][3];
146 o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z + m.m[2][3];
147}
148
149template <typename T>
150inline void Vector3CalcCommon<T>::rotate(Base& o, const Mtx33& m, const Base& a)
151{
152 mul(o, m, a);
153}
154
155template <typename T>
156inline void Vector3CalcCommon<T>::rotate(Base& o, const Mtx34& m, const Base& a)
157{
158 const Base tmp = a;
159 o.x = m.m[0][0] * tmp.x + m.m[0][1] * tmp.y + m.m[0][2] * tmp.z;
160 o.y = m.m[1][0] * tmp.x + m.m[1][1] * tmp.y + m.m[1][2] * tmp.z;
161 o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z;
162}
163
164template <typename T>
165inline void Vector3CalcCommon<T>::rotate(Base& o, const Quat& q, const Base& v)
166{
167 Quat r; // quat-multiplication with 0 on w for v
168 r.x = (q.y * v.z) - (q.z * v.y) + (q.w * v.x);
169 r.y = -(q.x * v.z) + (q.z * v.x) + (q.w * v.y);
170 r.z = (q.x * v.y) - (q.y * v.x) + (q.w * v.z);
171 r.w = -(q.x * v.x) - (q.y * v.y) - (q.z * v.z);
172
173 // quat-multiplication with zero on o.w and everything is negated
174 o.x = (r.x * q.w) - (r.y * q.z) + (r.z * q.y) - (r.w * q.x);
175 o.y = (r.x * q.z) + (r.y * q.w) - (r.z * q.x) - (r.w * q.y);
176 o.z = -(r.x * q.y) + (r.y * q.x) + (r.z * q.w) - (r.w * q.z);
177}
178
179template <typename T>
180inline void Vector3CalcCommon<T>::cross(Base& o, const Base& a, const Base& b)
181{
182 Vector3CalcCommon<T>::set(o, (a.y * b.z) - (a.z * b.y), (a.z * b.x) - (a.x * b.z),
183 (a.x * b.y) - (a.y * b.x));
184}
185
186#ifdef cafe
187
188template <>
189inline void Vector3CalcCommon<f32>::cross(Base& o, const Base& a, const Base& b)
190{
191 ASM_VECCrossProduct((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
192}
193
194#endif // cafe
195
196template <typename T>
197inline T Vector3CalcCommon<T>::dot(const Base& a, const Base& b)
198{
199 return a.x * b.x + a.y * b.y + a.z * b.z;
200}
201
202#ifdef cafe
203
204template <>
205inline f32 Vector3CalcCommon<f32>::dot(const Base& a, const Base& b)
206{
207 // TODO: Implement using intrinsics
208 return ASM_VECDotProduct((const Vec*)&a, (const Vec*)&b);
209}
210
211#endif // cafe
212
213template <typename T>
214inline T Vector3CalcCommon<T>::squaredLength(const Base& v)
215{
216 return v.x * v.x + v.y * v.y + v.z * v.z;
217}
218
219template <typename T>
220inline T Vector3CalcCommon<T>::length(const Base& v)
221{
222 return MathCalcCommon<T>::sqrt(squaredLength(v));
223}
224
225#ifdef cafe
226
227template <>
228inline f32 Vector3CalcCommon<f32>::length(const Base& v)
229{
230 return ASM_VECMag((const Vec*)&v);
231}
232
233#endif // cafe
234
235template <typename T>
236inline bool Vector3CalcCommon<T>::equals(const Base& lhs, const Base& rhs, T epsilon)
237{
238 return MathCalcCommon<T>::equalsEpsilon(lhs.x, rhs.x, epsilon) &&
239 MathCalcCommon<T>::equalsEpsilon(lhs.y, rhs.y, epsilon) &&
240 MathCalcCommon<T>::equalsEpsilon(lhs.z, rhs.z, epsilon);
241}
242
243template <typename T>
244inline void Vector3CalcCommon<T>::multScalar(Base& o, const Base& v, T t)
245{
246 o.x = v.x * t;
247 o.y = v.y * t;
248 o.z = v.z * t;
249}
250
251#ifdef cafe
252
253template <>
254inline void Vector3CalcCommon<f32>::multScalar(Base& o, const Base& v, f32 t)
255{
256 // TODO: Implement using intrinsics
257 ASM_VECScale((const Vec*)&v, (Vec*)&o, t);
258}
259
260#endif // cafe
261
262template <typename T>
263inline void Vector3CalcCommon<T>::multScalarAdd(Base& o, T t, const Base& a, const Base& b)
264{
265 o.x = a.x * t + b.x;
266 o.y = a.y * t + b.y;
267 o.z = a.z * t + b.z;
268}
269
270#ifdef cafe
271
272template <>
273inline void Vector3CalcCommon<f32>::multScalarAdd(Base& o, f32 t, const Base& a, const Base& b)
274{
275 // TODO: Implement using intrinsics
276 ASM_VECScale((const Vec*)&a, (Vec*)&o, t);
277 ASM_VECAdd((const Vec*)&o, (const Vec*)&b, (Vec*)&o);
278}
279
280#endif // cafe
281
282template <typename T>
283T Vector3CalcCommon<T>::normalize(Base& v)
284{
285 const T len = length(v);
286 if (len > 0)
287 {
288 const T inv_len = 1 / len;
289 v.x *= inv_len;
290 v.y *= inv_len;
291 v.z *= inv_len;
292 }
293
294 return len;
295}
296
297template <typename T>
298inline void Vector3CalcCommon<T>::negate(Base& v)
299{
300 v.x = -v.x;
301 v.y = -v.y;
302 v.z = -v.z;
303}
304
305template <typename T>
306inline void Vector3CalcCommon<T>::set(Base& o, const Base& v)
307{
308 o = v;
309}
310
311template <typename T>
312inline void Vector3CalcCommon<T>::set(Base& v, T x, T y, T z)
313{
314 v.x = x;
315 v.y = y;
316 v.z = z;
317}
318
319template <typename T>
320T Vector4CalcCommon<T>::normalize(Base& v)
321{
322 const T len = length(v);
323 if (len > 0)
324 {
325 const T inv_len = 1 / len;
326 v.x *= inv_len;
327 v.y *= inv_len;
328 v.z *= inv_len;
329 v.w *= inv_len;
330 }
331
332 return len;
333}
334
335template <typename T>
336inline void Vector4CalcCommon<T>::negate(Base& v)
337{
338 v.x = -v.x;
339 v.y = -v.y;
340 v.z = -v.z;
341 v.w = -v.w;
342}
343
344template <typename T>
345inline T Vector4CalcCommon<T>::squaredLength(const Base& v)
346{
347 return v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w;
348}
349
350template <typename T>
351inline T Vector4CalcCommon<T>::length(const Base& v)
352{
353 return MathCalcCommon<T>::sqrt(squaredLength(v));
354}
355
356template <typename T>
357inline void Vector4CalcCommon<T>::set(Base& o, const Base& v)
358{
359 o = v;
360}
361
362template <typename T>
363inline void Vector4CalcCommon<T>::set(Base& v, T x, T y, T z, T w)
364{
365 v.x = x;
366 v.y = y;
367 v.z = z;
368 v.w = w;
369}
370
371} // namespace sead
372