1#include "Project/RollingCubePose.h"
2
3#include "Library/Math/Axis.h"
4#include "Library/Math/MathUtil.h"
5#include "Library/Placement/PlacementFunction.h"
6#include "Library/Placement/PlacementInfo.h"
7
8namespace al {
9
10RollingCubePose::RollingCubePose() = default;
11
12bool RollingCubePose::isMovementRotate() const {
13 return mMovementType == MovementType::Rotate;
14}
15
16bool RollingCubePose::isMovementSlide() const {
17 return mMovementType == MovementType::Slide;
18}
19
20void RollingCubePose::setCubeSize(const sead::BoundBox3f& cubeSize) {
21 mCubeSize = cubeSize;
22}
23
24void RollingCubePose::calcNearPose(sead::Quatf* nearPose, const sead::Quatf& quat) const {
25 calcFittingBoxPose(nearPose, mCubeSize, quat, mQuat);
26}
27
28void RollingCubePose::fittingToBoundingBox(sead::Quatf* quat, sead::Vector3f* trans) const {
29 calcNearPose(nearPose: quat, quat: *quat);
30
31 sead::Vector3f currentCenter;
32 calcBoundingBoxCenter(center: &currentCenter);
33
34 sead::Vector3f boxCenter;
35 calcBoundingBoxCenter(center: &boxCenter, quat: *quat, trans: *trans);
36
37 *trans += currentCenter - boxCenter;
38}
39
40void RollingCubePose::calcBoundingBoxCenter(sead::Vector3f* center) const {
41 calcBoundingBoxCenter(center, quat: mQuat, trans: mTrans);
42}
43
44void RollingCubePose::calcBoundingBoxCenter(sead::Vector3f* center, const sead::Quatf& quat,
45 const sead::Vector3f& trans) const {
46 center->set(mCubeSize.getCenter());
47
48 sead::Matrix34f mtx;
49 mtx.makeQT(q: quat, t: trans);
50
51 center->mul(m: mtx);
52}
53
54void RollingCubePose::setNextCubePose(const RollingCubePose* nextPose) {
55 mSlideVec = sead::Vector3f::zero;
56
57 sead::Vector3f currentBottomFacePoints[4];
58 calcBottomFacePoint(facePoints: currentBottomFacePoints);
59
60 sead::Vector3f nextBottomFacePoints[4];
61 nextPose->calcBottomFacePoint(facePoints: nextBottomFacePoints);
62
63 // finds points that remain mostly unchanged between the current and next pose
64 // if two are found: rotate over the edge connecting the two points
65 // otherwise: slide to next position (standard interpolation)
66 s32 pointCount = 0;
67 sead::Vector3f firstPoint = sead::Vector3f::zero;
68 sead::Vector3f secondPoint = sead::Vector3f::zero;
69
70 for (s32 i = 0; i < 4; i++) {
71 for (s32 e = 0; e < 4; e++) {
72 if ((currentBottomFacePoints[i] - nextBottomFacePoints[e]).length() <= 10.0f) {
73 if (pointCount == 0) {
74 pointCount = 1;
75 firstPoint.set(nextBottomFacePoints[e]);
76 continue;
77 }
78 if (pointCount == 1) {
79 pointCount = 2;
80 secondPoint.set(nextBottomFacePoints[e]);
81 continue;
82 }
83 pointCount++;
84 }
85 }
86 }
87
88 mMovementType = MovementType::None;
89 if (pointCount == 2) {
90 mRotateAxis = secondPoint - firstPoint;
91 mRotateCenter = firstPoint;
92 if (tryNormalizeOrZero(out: &mRotateAxis)) {
93 sead::Vector3f currentCenter;
94 calcBoundingBoxCenter(center: &currentCenter);
95
96 sead::Vector3f nextCenter;
97 nextPose->calcBoundingBoxCenter(center: &nextCenter);
98
99 mRotateDegree = calcAngleOnPlaneDegree(a: currentCenter - mRotateCenter,
100 b: nextCenter - mRotateCenter, vertical: mRotateAxis);
101 mMovementType = MovementType::Rotate;
102 }
103 }
104
105 if (mMovementType == MovementType::None) {
106 mMovementType = MovementType::Slide;
107
108 sead::Vector3f currentCenter;
109 calcBoundingBoxCenter(center: &currentCenter);
110
111 sead::Vector3f nextCenter;
112 nextPose->calcBoundingBoxCenter(center: &nextCenter);
113
114 sead::Quatf nearPose;
115 nextPose->calcNearPose(nearPose: &nearPose, quat: mQuat);
116 calcQuatRotateAxisAndDegree(&mRotateAxis, &mRotateDegree, mQuat, nearPose);
117
118 mRotateCenter = currentCenter;
119 mSlideVec.setSub(a: nextCenter, b: currentCenter);
120 }
121}
122
123void RollingCubePose::calcBottomFacePoint(sead::Vector3f facePoints[4]) const {
124 calcBoxFacePoint(facePoints, mCubeSize, calcBottomFaceIndex(), mQuat, mTrans);
125}
126
127void RollingCubePose::init(const PlacementInfo& placementInfo) {
128 tryGetQuat(quat: &mQuat, placementInfo);
129 tryGetTrans(trans: &mTrans, placementInfo);
130 mPlacementInfo = new PlacementInfo(placementInfo);
131}
132
133void RollingCubePose::calcRotateQT(sead::Quatf* outQuat, sead::Vector3f* outTrans,
134 const sead::Quatf& quat, const sead::Vector3f& trans,
135 f32 rate) const {
136 rotateQuatAndTransDegree(outQuat, outTrans, quat, trans, mRotateAxis, mRotateCenter,
137 mRotateDegree * rate);
138 outTrans->setScaleAdd(t: rate, a: mSlideVec, b: *outTrans);
139}
140
141Axis RollingCubePose::calcBottomFaceIndex() const {
142 sead::Vector3f xAxis;
143 sead::Vector3f yAxis;
144 sead::Vector3f zAxis;
145 calcQuatLocalAxisAll(mQuat, &xAxis, &yAxis, &zAxis);
146 f32 x = sead::Mathf::abs(x: xAxis.y);
147 f32 y = sead::Mathf::abs(x: yAxis.y);
148 f32 z = sead::Mathf::abs(x: zAxis.y);
149
150 if (x > y) {
151 if (x > z)
152 return 0.0f > xAxis.y ? Axis::X : Axis::InvertX;
153 return 0.0f > zAxis.y ? Axis::Z : Axis::InvertZ;
154 }
155
156 if (y >= z)
157 return 0.0f > yAxis.y ? Axis::Y : Axis::InvertY;
158 return 0.0f > zAxis.y ? Axis::Z : Axis::InvertZ;
159}
160
161} // namespace al
162