1#include "Library/Math/ParabolicPath.h"
2
3#include "Library/Math/MathUtil.h"
4
5namespace al {
6
7ParabolicPath::ParabolicPath() {}
8
9void ParabolicPath::initFromUpVector(const sead::Vector3f& start, const sead::Vector3f& end,
10 const sead::Vector3f& up) {
11 f32 scalar;
12 sead::Vector3f upDir;
13 separateScalarAndDirection(&scalar, &upDir, up);
14 initFromUpVector(start, end, up: upDir, maxHeight: scalar);
15}
16
17void ParabolicPath::initFromUpVector(const sead::Vector3f& start, const sead::Vector3f& end,
18 const sead::Vector3f& up, f32 maxHeight) {
19 mUp.set(up);
20 sead::Vector3f diff = end - start;
21 f32 verticalDistance = diff.dot(t: mUp);
22 mHorizontalDirection = diff - (mUp * verticalDistance);
23 separateScalarAndDirection(&mHorizontalDistance, &mHorizontalDirection, mHorizontalDirection);
24 calcParabolicFunctionParam(&mGravity, &mInitialVelY, maxHeight, verticalDistance);
25 mStart.set(start);
26}
27
28void ParabolicPath::initFromMaxHeight(const sead::Vector3f& start, const sead::Vector3f& end,
29 const sead::Vector3f& projectedEnd) {
30 sead::Vector3f up;
31 f32 scalar;
32 separateScalarAndDirection(&scalar, &up, projectedEnd - end);
33 initFromUpVector(start, end, up, maxHeight: (projectedEnd - start).dot(t: up));
34}
35
36void ParabolicPath::initFromUpVectorAddHeight(const sead::Vector3f& start,
37 const sead::Vector3f& end, const sead::Vector3f& up,
38 f32 height) {
39 f32 verticalDistance = sead::Mathf::clampMin(val: (end - start).dot(t: up), min_: 0.0f);
40 initFromUpVector(start, end, up, maxHeight: verticalDistance + height);
41}
42
43f32 ParabolicPath::getLength(f32 start, f32 end, s32 iterations) const {
44 s32 steps = sead::Mathi::clampMin(val: iterations, min_: 1);
45 f32 stepSize = (end - start) / steps;
46 f32 squaredHStepSize = sead::Mathf::square(t: stepSize * mHorizontalDistance);
47 f32 vDist = (mGravity * start + mInitialVelY) * start;
48 f32 length = 0;
49
50 for (s32 i = 0; i < steps;) {
51 i++;
52 f32 prevVDist = vDist;
53 f32 curStep = start + i * stepSize;
54 vDist = (mGravity * curStep + mInitialVelY) * curStep;
55 length += sead::Mathf::sqrt(t: squaredHStepSize + sead::Mathf::square(t: vDist - prevVDist));
56 }
57
58 return length;
59}
60
61f32 ParabolicPath::getTotalLength(s32 iterations) const {
62 return getLength(start: 0.0f, end: 1.0f, iterations);
63}
64
65void ParabolicPath::calcPositionHV(sead::Vector3f* pos, f32 h, f32 v) const {
66 f32 hDist = mHorizontalDistance * h;
67 f32 vDist = (mGravity * v + mInitialVelY) * v;
68 *pos = mStart + (vDist * mUp) + (hDist * mHorizontalDirection);
69}
70
71void ParabolicPath::calcPosition(sead::Vector3f* pos, f32 prog) const {
72 calcPositionHV(pos, h: prog, v: prog);
73}
74
75void ParabolicPath::calcPositionEaseOutH(sead::Vector3f* pos, f32 prog) const {
76 calcPositionHV(pos, h: easeOut(t: prog), v: prog);
77}
78
79void ParabolicPath::calcDirection(sead::Vector3f* pos, f32 prog, f32 stepSize) const {
80 f32 prog1, prog2;
81 if (prog < stepSize) {
82 prog1 = 0.0f;
83 prog2 = stepSize;
84 } else if ((1.0f - stepSize) < prog) {
85 prog2 = 1.0f;
86 prog1 = 1.0f - stepSize;
87 } else {
88 prog2 = prog + stepSize;
89 prog1 = prog;
90 }
91
92 sead::Vector3f pos1, pos2;
93 calcPosition(pos: &pos1, prog: prog1);
94 calcPosition(pos: &pos2, prog: prog2);
95 *pos = pos2 - pos1;
96 tryNormalizeOrZero(out: pos);
97}
98
99f32 ParabolicPath::calcPathSpeedFromGravityAccel(f32 frames) const {
100 return sead::Mathf::abs(x: frames / mGravity);
101}
102
103f32 ParabolicPath::calcPathSpeedFromAverageSpeed(f32 frames) const {
104 return frames / getTotalLength(iterations: 10);
105}
106
107f32 ParabolicPath::calcPathSpeedFromHorizontalSpeed(f32 frames) const {
108 return frames / mHorizontalDistance;
109}
110
111s32 ParabolicPath::calcPathTimeFromGravityAccel(f32 frames) const {
112 return 1.0f / calcPathSpeedFromGravityAccel(frames);
113}
114
115s32 ParabolicPath::calcPathTimeFromAverageSpeed(f32 frames) const {
116 return 1.0f / calcPathSpeedFromAverageSpeed(frames);
117}
118
119s32 ParabolicPath::calcPathTimeFromHorizontalSpeed(f32 frames) const {
120 return 1.0f / calcPathSpeedFromHorizontalSpeed(frames);
121}
122
123} // namespace al
124