1#include "Library/Collision/CollisionPartsTriangle.h"
2
3#include "Library/Collision/CollisionParts.h"
4#include "Library/Collision/KCollisionServer.h"
5#include "Library/Math/MathUtil.h"
6
7namespace al {
8
9Triangle::Triangle() : mCollisionParts(nullptr), mKCPrismData(nullptr), mKCPrismHeader(nullptr) {
10 // nullptrs above done to avoid initializing them in the other ctor
11
12 mNormals[0] = sead::Vector3f::zero;
13 mNormals[1] = sead::Vector3f::zero;
14 mNormals[2] = sead::Vector3f::zero;
15 mNormals[3] = sead::Vector3f::zero;
16 mPositions[0] = sead::Vector3f::zero;
17 mPositions[1] = sead::Vector3f::zero;
18 mPositions[2] = sead::Vector3f::zero;
19}
20
21Triangle::Triangle(const CollisionParts& parts, const KCPrismData* data,
22 const KCPrismHeader* header) {
23 fillData(parts, data, header);
24}
25
26void Triangle::fillData(const CollisionParts& parts, const KCPrismData* data,
27 const KCPrismHeader* header) {
28 mCollisionParts = &parts;
29 mKCPrismData = data;
30 mKCPrismHeader = header;
31
32 KCollisionServer* kCollisionServer = parts.getKCollisionServer();
33 const sead::Matrix34f& baseMtx = parts.getBaseMtx();
34
35 mNormals[0].setRotated(m: baseMtx, a: kCollisionServer->getFaceNormal(data, header: mKCPrismHeader));
36 mNormals[1].setRotated(m: baseMtx, a: kCollisionServer->getEdgeNormal1(data, header: mKCPrismHeader));
37 mNormals[2].setRotated(m: baseMtx, a: kCollisionServer->getEdgeNormal2(data, header: mKCPrismHeader));
38 mNormals[3].setRotated(m: baseMtx, a: kCollisionServer->getEdgeNormal3(data, header: mKCPrismHeader));
39
40 normalize(vec: &mNormals[0]);
41 normalize(vec: &mNormals[1]);
42 normalize(vec: &mNormals[2]);
43 normalize(vec: &mNormals[3]);
44
45 kCollisionServer->calcPosLocal(pos: &mPositions[0], data, location: 0, header: mKCPrismHeader);
46 kCollisionServer->calcPosLocal(pos: &mPositions[1], data, location: 1, header: mKCPrismHeader);
47 kCollisionServer->calcPosLocal(pos: &mPositions[2], data, location: 2, header: mKCPrismHeader);
48
49 mPositions[0].mul(m: baseMtx);
50 mPositions[1].mul(m: baseMtx);
51 mPositions[2].mul(m: baseMtx);
52}
53
54void Triangle::fill(const sead::Vector3f& pos1, const sead::Vector3f& pos2,
55 const sead::Vector3f& pos3) {
56 mPositions[0] = pos1;
57 mPositions[1] = pos2;
58 mPositions[2] = pos3;
59
60 sead::Vector3f vert1To2 = pos2 - pos1;
61 sead::Vector3f vert1To3 = pos3 - pos1;
62 sead::Vector3f vert2To3 = pos3 - pos2;
63
64 mNormals[0].setCross(a: vert1To2, b: vert1To3);
65 tryNormalizeOrZero(out: &mNormals[0]);
66
67 mNormals[1].setCross(a: vert1To2, b: mNormals[0]);
68 tryNormalizeOrZero(out: &mNormals[1]);
69 mNormals[2].setCross(a: mNormals[0], b: vert1To3);
70 tryNormalizeOrZero(out: &mNormals[2]);
71 mNormals[3].setCross(a: vert2To3, b: mNormals[0]);
72 tryNormalizeOrZero(out: &mNormals[3]);
73
74 mKCPrismData = nullptr;
75}
76
77const LiveActor* Triangle::getHostActor() const {
78 if (!mCollisionParts)
79 return nullptr;
80 return mCollisionParts->getConnectedHost();
81}
82
83bool Triangle::isHostMoved() const {
84 return mCollisionParts->get_15c() == 0 || mCollisionParts->isMoving();
85}
86
87bool Triangle::isValid() const {
88 return mKCPrismData != nullptr;
89}
90
91const sead::Vector3f& Triangle::getNormal(s32 index) const {
92 return mNormals[index];
93}
94
95const sead::Vector3f& Triangle::getFaceNormal() const {
96 return getNormal(index: 0);
97}
98
99const sead::Vector3f& Triangle::getEdgeNormal(s32 index) const {
100 return getNormal(index: index + 1);
101}
102
103const sead::Vector3f& Triangle::getPos(s32 index) const {
104 return mPositions[index];
105}
106
107void Triangle::calcCenterPos(sead::Vector3f* center) const {
108 *center = (getPos(index: 0) + getPos(index: 1) + getPos(index: 2)) * (1 / 3.0f);
109}
110
111const sead::Vector3f& Triangle::calcAndGetNormal(s32 index) {
112 const CollisionParts* collisionParts = mCollisionParts;
113 KCollisionServer* kCollisionServer = collisionParts->getKCollisionServer();
114 switch (index) {
115 case 0:
116 return calcAndGetFaceNormal();
117 case 1:
118 mNormals[1] = kCollisionServer->getEdgeNormal1(data: mKCPrismData, header: mKCPrismHeader);
119 mNormals[1].setRotated(m: collisionParts->getBaseMtx(), a: mNormals[1]);
120 normalize(vec: &mNormals[1]);
121 return getNormal(index);
122 case 2:
123 mNormals[2] = kCollisionServer->getEdgeNormal2(data: mKCPrismData, header: mKCPrismHeader);
124 mNormals[2].setRotated(m: collisionParts->getBaseMtx(), a: mNormals[2]);
125 normalize(vec: &mNormals[2]);
126 return getNormal(index);
127 case 3:
128 mNormals[3] = kCollisionServer->getEdgeNormal3(data: mKCPrismData, header: mKCPrismHeader);
129 mNormals[3].setRotated(m: collisionParts->getBaseMtx(), a: mNormals[3]);
130 normalize(vec: &mNormals[3]);
131 return getNormal(index);
132 default:
133 return getNormal(index);
134 }
135}
136
137const sead::Vector3f& Triangle::calcAndGetFaceNormal() {
138 const CollisionParts* collisionParts = mCollisionParts;
139 mNormals[0] =
140 collisionParts->getKCollisionServer()->getFaceNormal(data: mKCPrismData, header: mKCPrismHeader);
141 mNormals[0].setRotated(m: collisionParts->getBaseMtx(), a: mNormals[0]);
142 normalize(vec: &mNormals[0]);
143 return mNormals[0];
144}
145
146const sead::Vector3f& Triangle::calcAndGetEdgeNormal(s32 index) {
147 const CollisionParts* collisionParts = mCollisionParts;
148 KCollisionServer* kCollisionServer = collisionParts->getKCollisionServer();
149 switch (index) {
150 case 0:
151 mNormals[1] = kCollisionServer->getEdgeNormal1(data: mKCPrismData, header: mKCPrismHeader);
152 mNormals[1].rotate(m: collisionParts->getBaseMtx());
153 normalize(vec: &mNormals[1]);
154 return getEdgeNormal(index);
155 case 1:
156 mNormals[2] = kCollisionServer->getEdgeNormal2(data: mKCPrismData, header: mKCPrismHeader);
157 mNormals[2].rotate(m: collisionParts->getBaseMtx());
158 normalize(vec: &mNormals[2]);
159 return getEdgeNormal(index);
160 case 2:
161 mNormals[3] = kCollisionServer->getEdgeNormal3(data: mKCPrismData, header: mKCPrismHeader);
162 mNormals[3].rotate(m: collisionParts->getBaseMtx());
163 normalize(vec: &mNormals[3]);
164 return getEdgeNormal(index);
165 default:
166 return getEdgeNormal(index);
167 }
168}
169
170const sead::Vector3f& Triangle::calcAndGetPos(s32 index) {
171 mCollisionParts->getKCollisionServer()->calcPosLocal(pos: &mPositions[index], data: mKCPrismData, location: index,
172 header: mKCPrismHeader);
173 mPositions[index].mul(m: getBaseMtx());
174 return getPos(index);
175}
176
177void Triangle::getLocalPos(sead::Vector3f* pos, s32 index) const {
178 mCollisionParts->getKCollisionServer()->calcPosLocal(pos, data: mKCPrismData, location: index, header: mKCPrismHeader);
179}
180
181void Triangle::calcForceMovePower(sead::Vector3f* power, const sead::Vector3f& pos) const {
182 mCollisionParts->calcForceMovePower(power, pos);
183}
184
185void Triangle::calcForceRotatePower(sead::Quatf* power) const {
186 mCollisionParts->calcForceRotatePower(power);
187}
188
189bool Triangle::getAttributes(ByamlIter* iter) const {
190 if (!isValid())
191 return false;
192 return mCollisionParts->getKCollisionServer()->getAttributes(destIter: iter, data: mKCPrismData);
193}
194
195const HitSensor* Triangle::getSensor() const {
196 return mCollisionParts->getConnectedSensor();
197}
198
199const sead::Matrix34f& Triangle::getBaseMtx() const {
200 return mCollisionParts->getBaseMtx();
201}
202
203const sead::Matrix34f& Triangle::getBaseInvMtx() const {
204 return mCollisionParts->getBaseInvMtx();
205}
206
207const sead::Matrix34f& Triangle::getPrevBaseMtx() const {
208 return mCollisionParts->getPrevBaseMtx();
209}
210
211HitInfo::HitInfo() {}
212
213bool HitInfo::isCollisionAtFace() const {
214 return collisionLocation == CollisionLocation::Face;
215}
216
217bool HitInfo::isCollisionAtEdge() const {
218 return collisionLocation == CollisionLocation::Edge1 ||
219 collisionLocation == CollisionLocation::Edge2 ||
220 collisionLocation == CollisionLocation::Edge3;
221}
222
223bool HitInfo::isCollisionAtCorner() const {
224 return collisionLocation == CollisionLocation::Corner1 ||
225 collisionLocation == CollisionLocation::Corner2 ||
226 collisionLocation == CollisionLocation::Corner3;
227}
228
229const sead::Vector3f& HitInfo::tryGetHitEdgeNormal() const {
230 if (collisionLocation == CollisionLocation::Edge1)
231 return triangle.getEdgeNormal(index: 0);
232 if (collisionLocation == CollisionLocation::Edge2)
233 return triangle.getEdgeNormal(index: 1);
234 if (collisionLocation == CollisionLocation::Edge3)
235 return triangle.getEdgeNormal(index: 2);
236
237 return sead::Vector3f::zero;
238}
239
240void SphereHitInfo::calcFixVector(sead::Vector3f* a1, sead::Vector3f* a2) const {
241 // TODO add proper names here, once the missing names for _70 and _80 in HitInfo are found
242 if (hitInfo->isCollisionAtFace()) {
243 calcFixVectorNormal(a1, a2);
244 return;
245 }
246
247 sead::Vector3f v20;
248 v20.x = hitInfo->_80.x - hitInfo->collisionHitPos.x;
249 v20.y = hitInfo->_80.y - hitInfo->collisionHitPos.y;
250 v20.z = hitInfo->_80.z - hitInfo->collisionHitPos.z;
251 tryNormalizeOrZero(out: &v20);
252
253 sead::Vector3f scaled_a1;
254 sead::Vector3f scaled_a2;
255 f32 v13 = v20.dot(t: hitInfo->triangle.getFaceNormal() * hitInfo->_70);
256 f32 v12 = v20.dot(t: hitInfo->triangle.getFaceNormal());
257 sead::Vector3CalcCommon<f32>::multScalar(o&: scaled_a1, v: v20, t: v13);
258 sead::Vector3CalcCommon<f32>::multScalar(o&: scaled_a2, v: v20, t: v12);
259 *a1 = scaled_a1;
260 *a2 = scaled_a2;
261}
262
263void SphereHitInfo::calcFixVectorNormal(sead::Vector3f* a1, sead::Vector3f* a2) const {
264 f32 unk = hitInfo->_70;
265 a1->x = hitInfo->triangle.getFaceNormal().x * unk;
266 a1->y = hitInfo->triangle.getFaceNormal().y * unk;
267 a1->z = hitInfo->triangle.getFaceNormal().z * unk;
268 if (a2)
269 a2->set(hitInfo->triangle.getFaceNormal());
270}
271
272void DiskHitInfo::calcFixVector(sead::Vector3f* a1, sead::Vector3f* a2) const {
273 // TODO add proper names here, once the missing names for _70 and _80 in HitInfo are found
274 if (hitInfo->isCollisionAtFace()) {
275 calcFixVectorNormal(a1, a2);
276 return;
277 }
278
279 sead::Vector3f v20;
280 v20.x = hitInfo->_80.x - hitInfo->collisionHitPos.x;
281 v20.y = hitInfo->_80.y - hitInfo->collisionHitPos.y;
282 v20.z = hitInfo->_80.z - hitInfo->collisionHitPos.z;
283 tryNormalizeOrZero(out: &v20);
284
285 sead::Vector3f scaled_a1;
286 sead::Vector3f scaled_a2;
287 f32 v13 = v20.dot(t: hitInfo->triangle.getFaceNormal() * hitInfo->_70);
288 f32 v12 = v20.dot(t: hitInfo->triangle.getFaceNormal());
289 sead::Vector3CalcCommon<f32>::multScalar(o&: scaled_a1, v: v20, t: v13);
290 sead::Vector3CalcCommon<f32>::multScalar(o&: scaled_a2, v: v20, t: v12);
291 *a1 = scaled_a1;
292 *a2 = scaled_a2;
293}
294
295void DiskHitInfo::calcFixVectorNormal(sead::Vector3f* a1, sead::Vector3f* a2) const {
296 f32 unk = hitInfo->_70;
297 a1->x = hitInfo->triangle.getFaceNormal().x * unk;
298 a1->y = hitInfo->triangle.getFaceNormal().y * unk;
299 a1->z = hitInfo->triangle.getFaceNormal().z * unk;
300 if (a2)
301 a2->set(hitInfo->triangle.getFaceNormal());
302}
303
304} // namespace al
305
306bool operator==(const al::Triangle& lhs, const al::Triangle& rhs) {
307 return lhs.mCollisionParts == rhs.mCollisionParts && lhs.mKCPrismData == rhs.mKCPrismData;
308}
309
310bool operator!=(const al::Triangle& lhs, const al::Triangle& rhs) {
311 return !(lhs == rhs);
312}
313