1#include "Project/Rail/BezierCurve.h"
2
3namespace al {
4
5BezierCurve::BezierCurve() = default;
6
7void BezierCurve::set(const sead::Vector3f& start, const sead::Vector3f& startHandle,
8 const sead::Vector3f& endHandle, const sead::Vector3f& end) {
9 sead::Vector3f diff1 = startHandle - start;
10 sead::Vector3f diff2 = endHandle - startHandle;
11 sead::Vector3f diff3 = end - endHandle;
12
13 sead::Vector3f diffDiff1 = diff2 - diff1;
14 sead::Vector3f diffDiff2 = diff3 - diff2;
15
16 sead::Vector3f diffDiffDiff = diffDiff2 - diffDiff1;
17
18 mStart = start;
19 mControlPoint1 = diff1 * 3;
20 mControlPoint2 = diffDiff1 * 3;
21 mControlPoint3 = diffDiffDiff;
22
23 mDistance = calcLength(startParam: 0.0, endParam: 1.0, stepCount: 10);
24}
25
26f32 BezierCurve::calcLength(f32 startParam, f32 endParam, s32 stepCount) const {
27 f32 avgVelocity = (calcDeltaLength(param: startParam) + calcDeltaLength(param: endParam)) / 2;
28 f32 halfStepSize = (endParam - startParam) * (1.0f / (stepCount * 2.0f));
29
30 f32 sumVelHalfStep = 0.0f;
31 f32 sumVelFullStep = 0.0f;
32 for (s32 i = 1; i <= stepCount; i++) {
33 f32 doubleI = i * 2.0f;
34
35 sumVelHalfStep += calcDeltaLength(param: (halfStepSize * (doubleI - 1)) + startParam);
36
37 if (i != stepCount)
38 sumVelFullStep += calcDeltaLength(param: (halfStepSize * (doubleI)) + startParam);
39 }
40
41 return std::floor(lcpp_x: (halfStepSize * 0.33333f) *
42 (avgVelocity + (sumVelFullStep * 2) + (sumVelHalfStep * 4)) * 1024.0f) /
43 1024.0f;
44}
45
46void BezierCurve::calcPos(sead::Vector3f* pos, f32 param) const {
47 f32 square = param * param;
48 f32 cube = square * param;
49
50 pos->x = (mControlPoint1.x * param) + mStart.x;
51 pos->y = (mControlPoint1.y * param) + mStart.y;
52 pos->z = (mControlPoint1.z * param) + mStart.z;
53
54 pos->x = (mControlPoint2.x * square) + pos->x;
55 pos->y = (mControlPoint2.y * square) + pos->y;
56 pos->z = (mControlPoint2.z * square) + pos->z;
57
58 pos->x = (mControlPoint3.x * cube) + pos->x;
59 pos->y = (mControlPoint3.y * cube) + pos->y;
60 pos->z = (mControlPoint3.z * cube) + pos->z;
61}
62
63void BezierCurve::calcVelocity(sead::Vector3f* vel, f32 param) const {
64 f32 fac1 = param + param;
65 f32 fac2 = 3 * param * param;
66
67 vel->x = (mControlPoint2.x * fac1) + mControlPoint1.x;
68 vel->y = (mControlPoint2.y * fac1) + mControlPoint1.y;
69 vel->z = (mControlPoint2.z * fac1) + mControlPoint1.z;
70
71 vel->x = (mControlPoint3.x * fac2) + vel->x;
72 vel->y = (mControlPoint3.y * fac2) + vel->y;
73 vel->z = (mControlPoint3.z * fac2) + vel->z;
74}
75
76f32 BezierCurve::calcDeltaLength(f32 param) const {
77 sead::Vector3f tmp;
78 calcVelocity(vel: &tmp, param);
79 return tmp.length();
80}
81
82// NON_MATCHING: flipped parts of if in last statement and unoptimized 1.0f - load
83f32 BezierCurve::calcCurveParam(f32 distance) const {
84 f32 percent = distance / mDistance;
85 f32 partLength = calcLength(startParam: 0, endParam: percent, stepCount: 10);
86 if (sead::Mathf::abs(x: distance - partLength) <= 0.01f)
87 return percent;
88
89 for (s32 i = 0; i <= 4; i++) {
90 f32 len = std::max(a: calcDeltaLength(param: percent), b: 0.001f);
91 f32 newPercent = percent + ((distance - partLength) / len);
92
93 percent = sead::Mathf::clamp(value: newPercent, low: 0.0f, high: 1.0f);
94 partLength = calcLength(startParam: 0.0f, endParam: percent, stepCount: 10);
95 if (sead::Mathf::abs(x: distance - partLength) <= 0.01f)
96 return percent;
97 }
98
99 if (partLength < 0.0f || percent > 1.0f)
100 return sead::Mathf::clamp(value: percent, low: 0.0f, high: 1.0f);
101 return percent;
102}
103
104f32 BezierCurve::calcNearestParam(const sead::Vector3f& pos, f32 interval) const {
105 f32 currentParam = 0.0;
106 f32 bestParam = -1.0;
107 f32 bestDist = 3.4028e38;
108 do {
109 sead::Vector3f nearest;
110 calcPos(pos: &nearest, param: currentParam);
111 f32 currentDist = (nearest - pos).squaredLength();
112
113 if (currentDist < bestDist) {
114 bestParam = currentParam;
115 bestDist = currentDist;
116 }
117 currentParam = currentParam + interval;
118 } while (currentParam <= 1.0);
119 return bestParam;
120}
121
122f32 BezierCurve::calcNearestLength(f32* param, const sead::Vector3f& pos, f32 max,
123 f32 interval) const {
124 f32 bestParam = -1.0;
125 f32 currentParam = 0.0;
126 f32 bestDist = 3.4028e38;
127 while (currentParam < max) {
128 sead::Vector3f nearest;
129 calcPos(pos: &nearest, param: calcCurveParam(distance: currentParam));
130 f32 currentDist = (nearest - pos).squaredLength();
131
132 if (currentDist < bestDist) {
133 bestParam = currentParam;
134 bestDist = currentDist;
135 }
136 currentParam = currentParam + interval;
137 }
138 *param = bestParam;
139 return bestDist;
140}
141
142// NON_MATCHING: Difference in loading for calcNearestParam
143void BezierCurve::calcNearestPos(sead::Vector3f* nearest, const sead::Vector3f& pos,
144 f32 interval) const {
145 calcPos(pos: nearest, param: calcNearestParam(pos, interval));
146}
147
148void BezierCurve::calcStartPos(sead::Vector3f* pos) const {
149 *pos = mStart;
150}
151
152void BezierCurve::calcCtrlPos1(sead::Vector3f* pos) const {
153 pos->x = mControlPoint1.x * 0.333333f;
154 pos->y = mControlPoint1.y * 0.333333f;
155 pos->z = mControlPoint1.z * 0.333333f;
156
157 pos->x = pos->x + mStart.x;
158 pos->y = pos->y + mStart.y;
159 pos->z = pos->z + mStart.z;
160}
161
162void BezierCurve::calcCtrlPos2(sead::Vector3f* pos) const {
163 pos->x = mControlPoint2.x * 0.333333f;
164 pos->y = mControlPoint2.y * 0.333333f;
165 pos->z = mControlPoint2.z * 0.333333f;
166
167 pos->x = (mControlPoint1.x * 0.6666667f) + pos->x;
168 pos->y = (mControlPoint1.y * 0.6666667f) + pos->y;
169 pos->z = (mControlPoint1.z * 0.6666667f) + pos->z;
170
171 pos->x = pos->x + mStart.x;
172 pos->y = pos->y + mStart.y;
173 pos->z = pos->z + mStart.z;
174}
175
176void BezierCurve::calcEndPos(sead::Vector3f* pos) const {
177 pos->x = mStart.x + mControlPoint1.x;
178 pos->y = mStart.y + mControlPoint1.y;
179 pos->z = mStart.z + mControlPoint1.z;
180
181 pos->x = pos->x + mControlPoint2.x;
182 pos->y = pos->y + mControlPoint2.y;
183 pos->z = pos->z + mControlPoint2.z;
184
185 pos->x = pos->x + mControlPoint3.x;
186 pos->y = pos->y + mControlPoint3.y;
187 pos->z = pos->z + mControlPoint3.z;
188}
189
190} // namespace al
191