diff --git a/include/game/cLib/c_lib.hpp b/include/game/cLib/c_lib.hpp index db2b0dc28..5832c4f0a 100644 --- a/include/game/cLib/c_lib.hpp +++ b/include/game/cLib/c_lib.hpp @@ -1,12 +1,50 @@ #pragma once #include #include -/// @file /// @brief Provides common utilities. /// @ingroup clib namespace cLib { + void memSet(void *dst, int val, ulong size); ///< See memset. + /// @brief Checks if a floating-point value is effectively zero within epsilon tolerance. + /// @param val The float value to evaluate. + /// @return @p true if the absolute value is less than FLT_EPSILON, @p false otherwise. + inline bool isZero(float val) { + return (std::fabs(val) < FLT_EPSILON); + } + + /// @brief Calculates the yaw (Y-axis) orientation angle from an origin position to a target position. + /// @param vec1 The origin position vector. + /// @param vec2 The target position vector. + /// @return The calculated yaw value. s16 targetAngleY(const mVec3_c &vec1, const mVec3_c &vec2); + /// @brief Calculates the pitch (X-axis) orientation angle from an origin position to a target position. + /// @param vec1 The origin position vector. + /// @param vec2 The target position vector. + /// @return The calculated pitch value. + s16 targetAngleX(const mVec3_c &vec1, const mVec3_c &vec2); + + /** + * @brief Smoothly moves @p currentPos towards @p targetPos using proportional scaling. + * @details If the applied step overshoots the target, the position is snapped exactly to the target. + * @param currentPos The position to be updated. + * @param targetPos The target position. + * @param ratio The smoothing ratio (between @p 0.0f and @p 1.0f). Higher value means snappier movement. + * @param maxStep The maximum step for a single update. + * @param minStep The minimum step to prevent jittering. + * @return The absolute remaining distance to the target position. + */ + float addCalcPos(mVec3_c *currentPos, const mVec3_c &targetPos, float ratio, float maxStep, float minStep); + + /** @brief Moves @p currentPos towards @p targetPos by a fixed @p step amount. + * @details If the applied step overshoots the target, the position is snapped exactly to the target. + * @param currentPos The position to be updated. + * @param targetPos The target position. + * @param step The step value. + * @return @p true if the position reached the target, @p false otherwise. + */ + bool chasePos(mVec3_c *currentPos, const mVec3_c &targetPos, float step); + } // namespace cLib diff --git a/slices/wiimj2d.json b/slices/wiimj2d.json index aa394c969..d1560d23d 100644 --- a/slices/wiimj2d.json +++ b/slices/wiimj2d.json @@ -721,6 +721,13 @@ ".sbss": "0x7b0-0x7d0" } }, + { + "source": "dol/cLib/c_lib.cpp", + "memoryRanges": { + ".text": "0x15a490-0x15a8d0", + ".sdata2": "0x2b80-0x2b88" + } + }, { "source": "dol/cLib/c_line.cpp", "memoryRanges": { diff --git a/source/dol/cLib/c_lib.cpp b/source/dol/cLib/c_lib.cpp new file mode 100644 index 000000000..73b8e8016 --- /dev/null +++ b/source/dol/cLib/c_lib.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include + +inline float calcDistance(const mVec3_c &a, const mVec3_c &b) { + float dx = a.x - b.x; + float dy = a.y - b.y; + float dz = a.z - b.z; + return EGG::Mathf::sqrt(dx * dx + dy * dy + dz * dz); +} + +namespace cLib { + +void memSet(void *dst, int val, ulong size) { + memset(dst, val, size); +} + +float addCalcPos(mVec3_c *currPos, const mVec3_c &targetPos, float ratio, float maxStep, float minStep) { + if (*currPos != targetPos) { + mVec3_c stepVector; + stepVector = *currPos - targetPos; + float distance = PSVECMag(stepVector); + if (distance < minStep) { + *currPos = targetPos; + } else { + stepVector *= ratio; + float step = distance * ratio; + if (!isZero(step)) { + if (step > maxStep) { + stepVector *= maxStep / step; + } else if (step < minStep) { + stepVector *= minStep / step; + } + *currPos -= stepVector; + } else { + *currPos = targetPos; + } + } + } + + return calcDistance(*currPos, targetPos); +} + +bool chasePos(mVec3_c *currentPos, const mVec3_c &targetPos, float step) { + if (step) { + mVec3_c offset = *currentPos - targetPos; + float distance = PSVECMag(offset); + if (isZero(distance) || distance <= step) { + *currentPos = targetPos; + return true; + } + *currentPos -= (step / distance) * offset; + } else if (*currentPos == targetPos) { + return true; + } + return false; +} + +s16 targetAngleY(const mVec3_c &vec1, const mVec3_c &vec2) { + return cM::atan2s(vec2.x - vec1.x, vec2.z - vec1.z); +} + +s16 targetAngleX(const mVec3_c &vec1, const mVec3_c &vec2) { + mVec3_c diff = vec2 - vec1; + return cM::atan2s(diff.y, diff.xzLen()); +} + +} // namespace cLib diff --git a/source/dol/cLib/c_math.cpp b/source/dol/cLib/c_math.cpp index 5aea4f0fb..2046eb77b 100644 --- a/source/dol/cLib/c_math.cpp +++ b/source/dol/cLib/c_math.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include /// @file @@ -96,10 +97,6 @@ inline float getCircleUnit() { return 65536 / (2 * M_PI); } -inline bool isZero(float val) { - return (std::fabs(val) < FLT_EPSILON); -} - s16 rad2s(float rad) { // Clamp the value between -65535 and 65535 int mod = (float)fmod(rad, 2 * M_PI) * getCircleUnit(); @@ -125,11 +122,11 @@ namespace cM { inline u32 atan2i(float sin,float cos) { // 0° or 180° - if (isZero(sin)) { + if (cLib::isZero(sin)) { return (cos >= 0.0f) ? 0 : 0x8000; // 90° or 270° - } else if (isZero(cos)) { + } else if (cLib::isZero(cos)) { return (sin >= 0.0f) ? 0x4000 : 0xC000; } else if (sin >= 0.0f) {