1#include "Library/Collision/KCollisionServer.h"
2
3#include "Library/Math/MathUtil.h"
4
5namespace al {
6
7void SphereInterpolator::startInterp(const sead::Vector3f& posStart, const sead::Vector3f& posEnd,
8 f32 sizeStart, f32 sizeEnd, f32 steps) {
9 mCurrentStep = 0.0f;
10 mPrevStep = 0.0f;
11 mPos = posStart;
12 mMove = posEnd - posStart;
13 mSizeStart = sizeStart;
14 mSizeEnd = sizeEnd;
15
16 f32 dist = mMove.length() + sizeEnd - sizeStart;
17 mStepSize = (dist <= 0.0f) ? 1.0f : steps / dist;
18}
19
20void SphereInterpolator::nextStep() {
21 // re-interpreting between f32/s32 required to match
22 s32 curStep = *(s32*)&mCurrentStep;
23 f32 stepAsFloat = *(f32*)&curStep;
24 f32 newStep = sead::Mathf::clampMax(val: stepAsFloat + mStepSize, max_: 1.0f);
25 *(s32*)&mPrevStep = curStep;
26 mCurrentStep = newStep;
27}
28
29void SphereInterpolator::calcInterpPos(sead::Vector3f* pos) const {
30 f32 step = mCurrentStep;
31 pos->x = mMove.x * step + mPos.x;
32 pos->y = mMove.y * step + mPos.y;
33 pos->z = mMove.z * step + mPos.z;
34}
35
36void SphereInterpolator::calcInterp(sead::Vector3f* pos, f32* size,
37 sead::Vector3f* remainMoveVec) const {
38 calcInterpPos(pos);
39 *size = mSizeStart + (mSizeEnd - mSizeStart) * mCurrentStep;
40 calcRemainMoveVector(remainMoveVec);
41}
42
43void SphereInterpolator::calcRemainMoveVector(sead::Vector3f* remainMoveVec) const {
44 if (remainMoveVec) {
45 f32 remainStep = 1.0f - mCurrentStep;
46 remainMoveVec->x = mMove.x * remainStep;
47 remainMoveVec->y = mMove.y * remainStep;
48 remainMoveVec->z = mMove.z * remainStep;
49 }
50}
51
52void SphereInterpolator::getMoveVector(sead::Vector3f* moveVec) {
53 f32 step = mCurrentStep;
54 moveVec->x = mMove.x * step;
55 moveVec->y = mMove.y * step;
56 moveVec->z = mMove.z * step;
57}
58
59void SphereInterpolator::calcStepMoveVector(sead::Vector3f* moveVec) const {
60 f32 step = mCurrentStep - mPrevStep;
61 moveVec->x = mMove.x * step;
62 moveVec->y = mMove.y * step;
63 moveVec->z = mMove.z * step;
64}
65
66void SpherePoseInterpolator::startInterp(const sead::Vector3f& posStart,
67 const sead::Vector3f& posEnd, f32 sizeStart, f32 sizeEnd,
68 const sead::Quatf& quatStart, const sead::Quatf& quatEnd,
69 f32 steps) {
70 mCurrentStep = 0.0f;
71 mPrevStep = 0.0f;
72 mPos = posStart;
73 mMove = posEnd - posStart;
74
75 mQuatStart.x = quatStart.x;
76 mQuatStart.y = quatStart.y;
77 mQuatStart.z = quatStart.z;
78 mQuatStart.w = quatStart.w;
79
80 mQuatEnd.x = quatEnd.x;
81 mQuatEnd.y = quatEnd.y;
82 mQuatEnd.z = quatEnd.z;
83 mQuatEnd.w = quatEnd.w;
84
85 mSizeStart = sizeStart;
86 mSizeEnd = sizeEnd;
87
88 f32 dist = mMove.length() + sizeEnd - sizeStart;
89 mStepSize = (dist <= 0.0f) ? 1.0f : steps / dist;
90}
91
92void SpherePoseInterpolator::nextStep() {
93 // re-interpreting between f32/s32 required to match
94 s32 curStep = *(s32*)&mCurrentStep;
95 f32 stepAsFloat = *(f32*)&curStep;
96 f32 newStep = sead::Mathf::clampMax(val: stepAsFloat + mStepSize, max_: 1.0f);
97 *(s32*)&mPrevStep = curStep;
98 mCurrentStep = newStep;
99}
100
101void SpherePoseInterpolator::calcInterpPos(sead::Vector3f* pos) const {
102 f32 step = mCurrentStep;
103 pos->x = mMove.x * step + mPos.x;
104 pos->y = mMove.y * step + mPos.y;
105 pos->z = mMove.z * step + mPos.z;
106}
107
108void SpherePoseInterpolator::calcInterp(sead::Vector3f* pos, f32* size, sead::Quatf* quat,
109 sead::Vector3f* remainMoveVec) const {
110 calcInterpPos(pos);
111 *size = mSizeStart + (mSizeEnd - mSizeStart) * mCurrentStep;
112 slerpQuat(quat, mQuatStart, mQuatEnd, mCurrentStep);
113 quat->normalize();
114 calcRemainMoveVector(remainMoveVec);
115}
116
117void SpherePoseInterpolator::calcRemainMoveVector(sead::Vector3f* remainMoveVec) const {
118 if (remainMoveVec) {
119 f32 remainStep = 1.0f - mCurrentStep;
120 remainMoveVec->x = mMove.x * remainStep;
121 remainMoveVec->y = mMove.y * remainStep;
122 remainMoveVec->z = mMove.z * remainStep;
123 }
124}
125
126f32 SpherePoseInterpolator::calcRadiusBaseScale(f32 unk) const {
127 return calcRate01(unk, 0.0f, mSizeEnd);
128}
129
130void SpherePoseInterpolator::getMoveVector(sead::Vector3f* moveVec) {
131 f32 step = mCurrentStep;
132 moveVec->x = mMove.x * step;
133 moveVec->y = mMove.y * step;
134 moveVec->z = mMove.z * step;
135}
136
137} // namespace al
138