1#include "Project/Rail/LinearCurve.h"
2
3#include "Library/Math/MathUtil.h"
4
5namespace al {
6
7LinearCurve::LinearCurve() = default;
8
9void LinearCurve::set(const sead::Vector3f& start, const sead::Vector3f& end) {
10 mStart = start;
11 mDiff.x = end.x - start.x;
12 mDiff.y = end.y - start.y;
13 mDiff.z = end.z - start.z;
14 mDistance = sead::Mathf::sqrt(t: mDiff.x * mDiff.x + mDiff.y * mDiff.y + mDiff.z * mDiff.z);
15}
16
17void LinearCurve::calcPos(sead::Vector3f* pos, f32 param) const {
18 pos->x = (mDiff.x * param) + mStart.x;
19 pos->y = (mDiff.y * param) + mStart.y;
20 pos->z = (mDiff.z * param) + mStart.z;
21}
22
23void LinearCurve::calcVelocity(sead::Vector3f* vel, f32 param) const {
24 *vel = mDiff;
25}
26
27f32 LinearCurve::calcLength(f32 param_start, f32 param_end) const {
28 return mDistance * sead::Mathf::abs(x: param_end - param_start);
29}
30
31f32 LinearCurve::calcCurveParam(f32 param) const {
32 if (isNearZero(value: mDistance, tolerance: 0.001))
33 return 0;
34
35 return sead::Mathf::clamp(value: param, low: 0, high: mDistance) / mDistance;
36}
37
38f32 LinearCurve::calcNearestParam(const sead::Vector3f& pos) const {
39 if (isNearZero(value: mDistance, tolerance: 0.001))
40 return 0;
41
42 f32 dot = (pos - mStart).dot(t: mDiff);
43 return sead::Mathf::clamp(value: dot / sead::Mathf::square(t: mDistance), low: 0, high: 1);
44}
45
46f32 LinearCurve::calcNearestLength(f32* length, const sead::Vector3f& pos, f32 param) const {
47 f32 nearestParam = calcNearestParam(pos);
48
49 sead::Vector3f nearestPos;
50 calcPos(pos: &nearestPos, param: nearestParam);
51 sead::Vector3f diff = nearestPos - pos;
52 f32 len = diff.squaredLength();
53
54 *length = nearestParam * param;
55 return len;
56}
57
58// NON_MATCHING: Difference in loading for calcNearestParam
59void LinearCurve::calcNearestPos(sead::Vector3f* nearest, const sead::Vector3f& pos) const {
60 calcPos(pos: nearest, param: calcNearestParam(pos));
61}
62
63void LinearCurve::calcStartPos(sead::Vector3f* pos) const {
64 *pos = mStart;
65}
66
67void LinearCurve::calcEndPos(sead::Vector3f* pos) const {
68 pos->x = mStart.x + mDiff.x;
69 pos->y = mStart.y + mDiff.y;
70 pos->z = mStart.z + mDiff.z;
71}
72
73} // namespace al
74