From 5bfb7b182315ff31a3b8ee29beadd7f986f92da2 Mon Sep 17 00:00:00 2001 From: LagoLunatic Date: Fri, 22 Dec 2023 13:57:27 -0500 Subject: [PATCH] d_jnt_hit OK --- configure.py | 2 +- include/SSystem/SComponent/c_xyz.h | 6 +- include/d/d_jnt_hit.h | 32 ++- src/d/d_jnt_hit.cpp | 350 +++++++++++++++++++++++++++-- src/d/d_lib.cpp | 5 +- 5 files changed, 354 insertions(+), 41 deletions(-) diff --git a/configure.py b/configure.py index cd0c1cfa8..031c0e3ec 100644 --- a/configure.py +++ b/configure.py @@ -342,7 +342,7 @@ config.libs = [ Object(NonMatching, "d/d_save.cpp"), Object(Matching, "d/d_save_init.cpp"), Object(Matching, "d/d_shop.cpp", extra_cflags=['-pragma "nosyminline on"']), - Object(NonMatching, "d/d_jnt_hit.cpp"), + Object(Matching, "d/d_jnt_hit.cpp"), Object(Matching, "d/d_chain.cpp"), Object(NonMatching, "d/d_cloth_packet.cpp"), Object(NonMatching, "d/d_a_obj.cpp"), diff --git a/include/SSystem/SComponent/c_xyz.h b/include/SSystem/SComponent/c_xyz.h index 2c4d42c8a..32e1e7e20 100644 --- a/include/SSystem/SComponent/c_xyz.h +++ b/include/SSystem/SComponent/c_xyz.h @@ -113,10 +113,10 @@ struct cXyz : Vec { } } - float getSquareMag() const { return VECSquareMag(this); } + f32 getSquareMag() const { return VECSquareMag(this); } f32 getSquareDistance(const Vec& other) const { return VECSquareDistance(this, &other); } - static float getNearZeroValue() { return 8e-11f; } + static f32 getNearZeroValue() { return 8e-11f; } bool isNearZeroSquare() const { return (this->getSquareMag() < getNearZeroValue()); } f32 abs2() const { return this->getSquareMag(); } @@ -143,7 +143,7 @@ struct cXyz : Vec { // void inprodXZ(const Vec&) const {} // void isNearZeroSquare(const cXyz&) const {} // void operator*=(const Vec&) {} - // void operator/=(float) {} + // void operator/=(f32) {} // void operator=(const cXyz&) {} // void print(const char*) {} }; diff --git a/include/d/d_jnt_hit.h b/include/d/d_jnt_hit.h index 03ae8ef7a..e069f0e12 100644 --- a/include/d/d_jnt_hit.h +++ b/include/d/d_jnt_hit.h @@ -24,18 +24,28 @@ public: , field_0x1C(NULL) {} BOOL CreateInit(); - void CylHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32); - void Cyl2HitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32); - void SphHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, f32); - void HitBufferUpdate(int*, cXyz*, int, csXyz*, cXyz*); + BOOL CylHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32); + BOOL Cyl2HitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32); + BOOL SphHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, f32); + BOOL HitBufferUpdate(int*, cXyz*, int, csXyz*, cXyz*); s32 searchJntHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*); J3DModel* getModel() { return mpModel; } - // setSearchData(__jnt_hit_data_c*); - // setMdlPtr(J3DModel*); - // setMaxNum(short); - // isThrow(int); - // isDelete(int); + void setSearchData(__jnt_hit_data_c* data) { mpSearchData = data; } + void setMdlPtr(J3DModel* model) { mpModel = model; } + void setMaxNum(s16 num) { mMaxNum = num; } + bool isThrow(int type) { + if (type == 4 || type == 3) { + return true; + } + return false; + } + bool isDelete(int type) { + if (type == 8 || type == 7) { + return true; + } + return false; + } bool isCylinder(int type) { if (type == 0 || (u32)type-2 <= 1 || type == 5 || type == 7) { return true; @@ -53,13 +63,13 @@ public: // DBdrawSph(MtxP, cXyz*, f32); // DBdrawAll(); - /* 0x00 */ __jnt_hit_data_c* mpHitData; + /* 0x00 */ __jnt_hit_data_c* mpSearchData; /* 0x04 */ J3DModel* mpModel; /* 0x08 */ cXyz* mpOffsets; /* 0x0C */ f32* mpRadiuses; /* 0x10 */ s16* mpShapeTypes; /* 0x14 */ s16* mpJointIndexes; - /* 0x18 */ s16 mHitDataCount; + /* 0x18 */ s16 mMaxNum; /* 0x1A */ u8 field_0x1A[2]; /* 0x1C */ void* field_0x1C; }; diff --git a/src/d/d_jnt_hit.cpp b/src/d/d_jnt_hit.cpp index 52efad121..3f88311e3 100644 --- a/src/d/d_jnt_hit.cpp +++ b/src/d/d_jnt_hit.cpp @@ -4,14 +4,16 @@ // #include "d/d_jnt_hit.h" -#include "dolphin/types.h" +#include "m_Do/m_Do_mtx.h" +#include "SSystem/SComponent/c_math.h" +#include "SSystem/SComponent/c_lib.h" /* 80060BE8-80060C44 .text __ct__12JntHit_HIO_cFv */ JntHit_HIO_c::JntHit_HIO_c() { m06 = 0; m08 = 0; m0C = 50.0f; - m10.set(cXyz::Zero); + m10 = cXyz::Zero; m1C = 0.0f; m20 = 0.0f; m24 = 100.0f; @@ -20,10 +22,10 @@ JntHit_HIO_c::JntHit_HIO_c() { /* 80060C44-80060EC4 .text CreateInit__8JntHit_cFv */ BOOL JntHit_c::CreateInit() { - __jnt_hit_data_c* pHitData = mpHitData; + __jnt_hit_data_c* pHitData = mpSearchData; int posCount = 0; int i; - for (i = 0; i < mHitDataCount; i++) { + for (i = 0; i < mMaxNum; i++) { if (isCylinder(pHitData->mShapeType)) { posCount += 2; } else if (isSphere(pHitData->mShapeType)) { @@ -32,20 +34,20 @@ BOOL JntHit_c::CreateInit() { pHitData++; } - mpShapeTypes = new s16[mHitDataCount]; + mpShapeTypes = new s16[mMaxNum]; mpOffsets = new cXyz[posCount]; - mpRadiuses = new f32[mHitDataCount]; - mpJointIndexes = new s16[mHitDataCount]; + mpRadiuses = new f32[mMaxNum]; + mpJointIndexes = new s16[mMaxNum]; if (!mpShapeTypes || !mpOffsets || !mpRadiuses || !mpJointIndexes) { return FALSE; } - pHitData = mpHitData; + pHitData = mpSearchData; s16* pShapeType = mpShapeTypes; s16* pJointIndex = mpJointIndexes; f32* pRadius = mpRadiuses; cXyz* pOffset = mpOffsets; - for (i = 0; i < mHitDataCount;) { + for (i = 0; i < mMaxNum;) { pShapeType[0] = pHitData->mShapeType; pJointIndex[0] = pHitData->mJointIndex; pRadius[0] = pHitData->mRadius; @@ -70,28 +72,332 @@ BOOL JntHit_c::CreateInit() { } /* 80060EC4-80061440 .text CylHitPosAngleOffset__8JntHit_cFP4cXyzP5csXyzP4cXyzP5csXyz4cXyz4cXyzf */ -void JntHit_c::CylHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32) { - /* Nonmatching */ +BOOL JntHit_c::CylHitPosAngleOffset(cXyz* r27, csXyz* r28, cXyz* r29, csXyz* r30, cXyz r31, cXyz r9, f32 f28) { + cXyz r1_100 = r9 - r31; + cXyz r1_F4 = r1_100.normZP(); + f32 f30 = r1_100.abs(); + f32 f1 = r27->inprod(r1_F4); + cXyz r1_E8; + cXyz r1_DC = *r27; + cXyz r1_D0 = r1_F4; + r1_D0 *= f1; + r1_DC -= r1_D0; + + f32 sinX = cM_ssin(r28->x); + f32 cosX = cM_scos(r28->x); + f32 x = cM_ssin(r28->y) * cosX; + f32 z = cM_scos(r28->y) * cosX; + cXyz r1_C4(x, -sinX, z); + f1 = r1_C4.inprod(r1_F4); + cXyz r1_B8 = r1_C4; + r1_D0 = r1_F4; + r1_D0 *= f1; + r1_B8 -= r1_D0; + if (r1_B8.normalizeRS()) { + f32 f31 = r1_DC.inprod(r1_B8); + f32 temp = f31*f31 + f28*f28 - r1_DC.abs2(); + if (temp < 0.0f) + temp = 0.0f; + r1_E8 = r1_C4 * (-f31 - sqrtf(temp)); + r1_E8 += *r27; + if (temp == 0.0f) { + cXyz r1_AC = r1_E8; + r1_AC -= r1_F4 * r1_E8.inprod(r1_F4); + r1_AC.normalizeZP(); + r1_AC = r1_AC * f28; + r1_AC += r1_F4 * r1_E8.inprod(r1_F4); + r1_E8 = r1_AC; + } + } else { + r1_E8 = *r27; + cXyz r1_A0 = *r27; + r1_A0 -= r1_F4 * r1_E8.inprod(r1_F4); + r1_A0.normalizeZP(); + r1_A0 = r1_A0 * f28; + r1_A0 += r1_F4 * r1_E8.inprod(r1_F4); + r1_E8 = r1_A0; + } + + f32 temp = r1_E8.inprod(r1_F4); + f32 temp2 = 0.0f; + if (temp < 0.0f) + temp2 = -temp; + else if (temp > f30) + temp2 = f30 - temp; + r1_E8 += r1_F4 * temp2; + r1_E8 += r31; + + mDoMtx_stack_c::inverse(); + mDoMtx_stack_c::transM(r1_E8); + mDoMtx_stack_c::ZXYrotM(*r28); + mDoMtx_stack_c::multVecZero(r29); + mDoMtx_MtxToRot(mDoMtx_stack_c::get(), r30); + + return TRUE; } /* 80061440-80061830 .text Cyl2HitPosAngleOffset__8JntHit_cFP4cXyzP5csXyzP4cXyzP5csXyz4cXyz4cXyzf */ -void JntHit_c::Cyl2HitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, cXyz, f32) { - /* Nonmatching */ +BOOL JntHit_c::Cyl2HitPosAngleOffset(cXyz* r27, csXyz* r29, cXyz* r30, csXyz* r31, cXyz r28, cXyz r9, f32 f31) { + cXyz r1_94 = r9 - r28; + cXyz r1_88 = r1_94.normZP(); + f32 f29 = r1_94.abs(); + f32 f30 = r27->inprod(r1_88); + + f32 sinX = cM_ssin(r29->x); + f32 cosX = cM_scos(r29->x); + f32 x = cM_ssin(r29->y) * cosX; + f32 z = cM_scos(r29->y) * cosX; + cXyz r1_7C; + cXyz r1_70(x, -sinX, z); + f32 f1 = r1_70.inprod(r1_88); + if (!(fabsf(f1) < cXyz::getNearZeroValue())) { // TODO: is this an inline? + if (f1 > 0.0f) { + f1 = -f30 / f1; + } else { + f1 = (f29 - f30) / f1; + } + cXyz r1_64 = r1_70; + r1_64 *= f1; + r1_7C = *r27 + r1_64; + } else { + r1_7C = *r27; + } + + cXyz r1_58 = r1_7C; + f32 temp = r1_58.inprod(r1_88); + cXyz r1_4C = r1_88; + r1_4C *= temp; + r1_58 -= r1_4C; + cXyz r1_40 = r1_58; + if (r1_58.abs() > f31 && r1_58.normalizeRS()) { + r1_58 *= f31; + r1_58 = r1_40 - r1_58; + r1_7C -= r1_58; + } + r1_7C += r28; + + mDoMtx_stack_c::inverse(); + mDoMtx_stack_c::transM(r1_7C); + mDoMtx_stack_c::ZXYrotM(*r29); + mDoMtx_stack_c::multVecZero(r30); + mDoMtx_MtxToRot(mDoMtx_stack_c::get(), r31); + + return TRUE; } /* 80061830-80061ACC .text SphHitPosAngleOffset__8JntHit_cFP4cXyzP5csXyzP4cXyzP5csXyz4cXyzf */ -void JntHit_c::SphHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*, cXyz, f32) { - /* Nonmatching */ +BOOL JntHit_c::SphHitPosAngleOffset(cXyz* r27, csXyz* r29, cXyz* r30, csXyz* r31, cXyz r28, f32 f30) { + f32 sinX = cM_ssin(r29->x); + f32 cosX = cM_scos(r29->x); + f32 x = cM_ssin(r29->y) * cosX; + f32 z = cM_scos(r29->y) * cosX; + cXyz r1_34(x, -sinX, z); + f32 f31 = r27->inprod(r1_34); + f32 temp = f31*f31 + f30*f30 - r27->abs2(); + if (temp < 0.0f) + temp = 0.0f; + + cXyz r1_28 = r1_34 * (-f31 - sqrtf(temp)); + r1_28 += *r27; + if (r1_28.abs() > f30) { + r1_28.normalizeZP(); + r1_28 *= f30; + } + r1_28 += r28; + + mDoMtx_stack_c::inverse(); + mDoMtx_stack_c::transM(r1_28); + mDoMtx_stack_c::ZXYrotM(*r29); + mDoMtx_stack_c::multVecZero(r30); + mDoMtx_MtxToRot(mDoMtx_stack_c::get(), r31); + + return TRUE; } /* 80061ACC-80061C28 .text HitBufferUpdate__8JntHit_cFPiP4cXyziP5csXyzP4cXyz */ -void JntHit_c::HitBufferUpdate(int*, cXyz*, int, csXyz*, cXyz*) { - /* Nonmatching */ +BOOL JntHit_c::HitBufferUpdate(int* r28, cXyz* r29, int r30, csXyz* r31, cXyz* r27) { + mDoMtx_stack_c::copy(mpModel->getAnmMtx(r30)); + cXyz r1_2C; + mDoMtx_stack_c::multVec(r27, &r1_2C); + if (*r28 >= 0) { + cXyz r1_20( + cM_scos(r31->x) * cM_ssin(r31->y), + -cM_ssin(r31->x), + cM_scos(r31->x) * cM_scos(r31->y) + ); + cXyz r1_14 = r1_2C - *r29; + if (r1_14.inprod(r1_20) < 0.0f) { + *r28 = r30; + *r29 = r1_2C; + return TRUE; + } else { + return FALSE; + } + } else { + *r28 = r30; + *r29 = r1_2C; + return TRUE; + } } /* 80061C28-800627DC .text searchJntHitPosAngleOffset__8JntHit_cFP4cXyzP5csXyzP4cXyzP5csXyz */ -s32 JntHit_c::searchJntHitPosAngleOffset(cXyz*, csXyz*, cXyz*, csXyz*) { - /* Nonmatching */ +s32 JntHit_c::searchJntHitPosAngleOffset(cXyz* r18, csXyz* r28, cXyz* r29, csXyz* r30) { + cXyz r1_1A0; + cXyz r1_194; + cXyz r1_188; + cXyz r1_17C; + cXyz r1_170; + cXyz r1_164; + cXyz r1_158; + cXyz r1_14C; + cXyz r1_140; + cXyz r1_134; + cXyz r1_128; + cXyz r1_11C; + cXyz r1_110; + cXyz r1_104; + cXyz r1_2C; + csXyz r1_24; + J3DModel* model = mpModel; + f32 f31 = 10000.0f; + int j = 0; + int jointIndex = -1; + s16* pShapeType = mpShapeTypes; + s16* pJointIndex = mpJointIndexes; + f32* pRadius = mpRadiuses; + cXyz* pOffset = mpOffsets; + int i = 0; + int posIndex = 0; + int hitIndex = 0; + int hitPosIndex = 0; + int r1_20 = -1; + for (; j++ < mMaxNum; i++) { + mDoMtx_stack_c::copy(model->getAnmMtx(*pJointIndex)); + + if (isCylinder(*pShapeType)) { + mDoMtx_stack_c::multVec(pOffset, &r1_17C); + mDoMtx_stack_c::multVec(pOffset+1, &r1_170); + r1_164 = r1_170 - r1_17C; + r1_158 = r1_164; + r1_1A0 = *r18 - r1_17C; + if (!r1_158.normalizeRS()) { + f32 temp = cLib_minLimit(r1_1A0.abs() - *pRadius, 0.0f); + if (temp <= f31) { + jointIndex = *pJointIndex; + f31 = temp; + hitIndex = i; + hitPosIndex = posIndex; + if (f31 < *pRadius) { + SphHitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_17C, *pRadius); + if (HitBufferUpdate(&r1_20, &r1_194, jointIndex, r28, r29)) { + r1_188 = *r29; + r1_24 = *r30; + } + } + } + } else { + r1_14C = r1_158.outprod(r1_1A0); + f32 f29 = cLib_minLimit(r1_14C.abs() - *pRadius, 0.0f); + f32 f30 = r1_158.inprod(r1_1A0); + f32 f5; + if (f30 < 0.0f) { + f5 = -f30; + } else { + if (f30 > r1_164.abs()) { + f5 = f30 - r1_164.abs(); + } else { + f5 = 0.0f; + } + } + f32 f4 = sqrtf(f5*f5 + f29*f29); + if (f4 <= f31) { + jointIndex = *pJointIndex; + f31 = f4; + hitIndex = i; + hitPosIndex = posIndex; + if (fabsf(f5) < cXyz::getNearZeroValue() && fabsf(f29) < cXyz::getNearZeroValue()) { + if (!isThrow(*pShapeType)) { + if (*pShapeType == 0) { + CylHitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_17C, r1_170, *pRadius); + } else if (*pShapeType == 2) { + Cyl2HitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_17C, r1_170, *pRadius); + } + if (isDelete(*pShapeType)) { + return -3; + } + if (HitBufferUpdate(&r1_20, &r1_194, jointIndex, r28, r29)) { + r1_188 = *r29; + r1_24 = *r30; + } + } + } + } + } + pOffset += 2; + posIndex += 2; + } else if (isSphere(*pShapeType)) { + mDoMtx_stack_c::multVec(pOffset, &r1_140); + r1_1A0 = *r18 - r1_140; + f32 temp = cLib_minLimit(r1_1A0.abs() - *pRadius, 0.0f); + if (temp <= f31) { + jointIndex = *pJointIndex; + f31 = temp; + hitIndex = i; + hitPosIndex = posIndex; + if (fabsf(f31) < cXyz::getNearZeroValue()) { + SphHitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_140, *pRadius); + if (isDelete(*pShapeType)) { + jointIndex = -3; + return jointIndex; + } else if (isThrow(*pShapeType)) { + // Do nothing + } else if (HitBufferUpdate(&r1_20, &r1_194, jointIndex, r28, r29)) { + r1_188 = *r29; + r1_24 = *r30; + } + } + } + pOffset += 1; + posIndex += 1; + } + + pShapeType++; + pJointIndex++; + pRadius++; + } + + if (r1_20 >= 0) { + *r29 = r1_188; + *r30 = r1_24; + return r1_20; + } + + pShapeType = mpShapeTypes; + pRadius = mpRadiuses; + pOffset = mpOffsets; + + mDoMtx_stack_c::copy(model->getAnmMtx(jointIndex)); + if (isThrow(pShapeType[hitIndex])) { + jointIndex = -1; + } else if (isDelete(pShapeType[hitIndex])) { + jointIndex = -3; + } else if (pShapeType[hitIndex] == 0) { + mDoMtx_stack_c::multVec(&pOffset[hitPosIndex], &r1_134); + mDoMtx_stack_c::multVec(&pOffset[hitPosIndex+1], &r1_128); + r1_1A0 = *r18 - r1_134; + CylHitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_134, r1_128, pRadius[hitIndex]); + } else if (pShapeType[hitIndex] == 1) { + mDoMtx_stack_c::multVec(&pOffset[hitPosIndex], &r1_11C); + r1_1A0 = *r18 - r1_11C; + SphHitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_11C, pRadius[hitIndex]); + } else { + mDoMtx_stack_c::multVec(&pOffset[hitPosIndex], &r1_110); + mDoMtx_stack_c::multVec(&pOffset[hitPosIndex+1], &r1_104); + r1_1A0 = *r18 - r1_110; + Cyl2HitPosAngleOffset(&r1_1A0, r28, r29, r30, r1_110, r1_104, pRadius[hitIndex]); + } + + return jointIndex; } /* 800627DC-8006286C .text JntHit_create__FP8J3DModelP16__jnt_hit_data_cs */ @@ -99,9 +405,9 @@ JntHit_c* JntHit_create(J3DModel* model, __jnt_hit_data_c* jntHitData, s16 hitDa JntHit_c * pJntHit = new JntHit_c(); if (pJntHit != NULL) { - pJntHit->mpHitData = jntHitData; - pJntHit->mpModel = model; - pJntHit->mHitDataCount = hitDataCount; + pJntHit->setSearchData(jntHitData); + pJntHit->setMdlPtr(model); + pJntHit->setMaxNum(hitDataCount); if (pJntHit->CreateInit()) return pJntHit; } diff --git a/src/d/d_lib.cpp b/src/d/d_lib.cpp index 5c51c0897..5d3952767 100644 --- a/src/d/d_lib.cpp +++ b/src/d/d_lib.cpp @@ -14,15 +14,12 @@ Quaternion ZeroQuat = {0.0f, 0.0f, 0.0f, 1.0f}; /* 80057000-800570CC .text dLib_setCirclePath__FP18dLib_circle_path_c */ void dLib_setCirclePath(dLib_circle_path_c* path) { - /* Nonmatching */ path->mAngle += path->mAngleSpeed; f32 rad = path->mRadius + path->mWobbleAmplitude * cM_ssin(path->mAngle); mDoMtx_stack_c::transS(path->mTranslation); mDoMtx_stack_c::YrotM(path->mAngle); mDoMtx_stack_c::transM(rad, 0.0f, 0.0f); - path->mPos.x = mDoMtx_stack_c::get()[0][3]; - path->mPos.y = mDoMtx_stack_c::get()[1][3]; - path->mPos.z = mDoMtx_stack_c::get()[2][3]; + mDoMtx_stack_c::multVecZero(&path->mPos); } /* 800570CC-8005716C .text dLib_getWaterY__FR4cXyzR12dBgS_ObjAcch */