diff --git a/Source/Entities/LimbPath.cpp b/Source/Entities/LimbPath.cpp index 07532dd8c0..f519549b83 100644 --- a/Source/Entities/LimbPath.cpp +++ b/Source/Entities/LimbPath.cpp @@ -172,6 +172,23 @@ Vector LimbPath::RotatePoint(const Vector& point) const { return (((point - offset) * m_Rotation) + offset) + m_PositionOffset; } +Vector LimbPath::InverseRotatePoint(const Vector& point) const { + Vector offset = (m_RotationOffset).GetXFlipped(m_HFlipped); + return (((point - m_PositionOffset) - offset) / m_Rotation) + offset; +} + +Vector LimbPath::ToLocalSpace(const Vector& position) const { + // The position might be on one side of a border of a wrapping scene while the joint is on another. + // Account for that. + Vector posWrapped = m_JointPos + g_SceneMan.ShortestDistance(m_JointPos, position); + + return InverseRotatePoint(posWrapped - m_JointPos) / GetTotalScaleMultiplier(); +} + +Vector LimbPath::ToWorldSpace(const Vector& position) const { + return m_JointPos + (RotatePoint(position * GetTotalScaleMultiplier())); +} + int LimbPath::Save(Writer& writer) const { Entity::Save(writer); @@ -199,43 +216,44 @@ void LimbPath::Destroy(bool notInherited) { Clear(); } -Vector LimbPath::GetProgressPos() { +Vector LimbPath::GetCurrentSegStartLocal() const { Vector returnVec(m_Start); - if (IsStaticPoint()) { - return m_JointPos + (RotatePoint(returnVec * GetTotalScaleMultiplier())); - } - // Add all the segments before the current one - std::deque::const_iterator itr; - for (itr = m_Segments.begin(); itr != m_CurrentSegment; ++itr) { - returnVec += *itr; - } - - // Add any from the progress made on the current one - if (itr != m_Segments.end()) { - returnVec += *m_CurrentSegment * m_SegProgress; + if (!IsStaticPoint()) { + // Add all the segments before the current one + std::deque::const_iterator itr; + for (itr = m_Segments.begin(); itr != m_CurrentSegment; ++itr) { + returnVec += *itr; + } } - return m_JointPos + (RotatePoint(returnVec * GetTotalScaleMultiplier())); + return returnVec; } -Vector LimbPath::GetCurrentSegTarget() { - Vector returnVec(m_Start); - if (IsStaticPoint()) { - return m_JointPos + (RotatePoint(returnVec * GetTotalScaleMultiplier())); +Vector LimbPath::GetProgressPos() { + Vector returnVec = GetCurrentSegStartLocal(); + + if (!IsStaticPoint()) { + if (m_CurrentSegment != m_Segments.end()) { + // Add approximation based on progress. + returnVec += *m_CurrentSegment * m_SegProgress; + } } - std::deque::const_iterator itr; + return ToWorldSpace(returnVec); +} - for (itr = m_Segments.begin(); itr != m_CurrentSegment; ++itr) { - returnVec += *itr; - } +Vector LimbPath::GetCurrentSegTarget() { + Vector returnVec = GetCurrentSegStartLocal(); - if (itr != m_Segments.end()) { - returnVec += *m_CurrentSegment; + if (!IsStaticPoint()) { + if (m_CurrentSegment != m_Segments.end()) { + // Add the current one as well. + returnVec += *m_CurrentSegment; + } } - return m_JointPos + (RotatePoint(returnVec * GetTotalScaleMultiplier())); + return ToWorldSpace(returnVec); } Vector LimbPath::GetCurrentVel(const Vector& limbPos) { @@ -292,29 +310,126 @@ void LimbPath::ReportProgress(const Vector& limbPos) { if (IsStaticPoint()) { const float staticPointEndedThreshold = 1.0F; m_Ended = g_SceneMan.ShortestDistance(limbPos, GetCurrentSegTarget()).MagnitudeIsLessThan(staticPointEndedThreshold); - } else { - // Check if we are sufficiently close to the target to start going after the next one. + } else if (m_CurrentSegment == m_Segments.end()) { + // Current path has already come to an end. Compute progress and m_Ended based on last segment's target. Vector distVec = g_SceneMan.ShortestDistance(limbPos, GetCurrentSegTarget()); - float distance = distVec.GetMagnitude(); - float segMag = (*m_CurrentSegment * GetTotalScaleMultiplier()).GetMagnitude(); + float distanceSqr = distVec.GetSqrMagnitude(); + float segMagSqr = (*m_CurrentSegment * GetTotalScaleMultiplier()).GetSqrMagnitude(); - if (distance < m_SegmentEndedThreshold) { - if (++(m_CurrentSegment) == m_Segments.end()) { - --(m_CurrentSegment); - // Get normalized progress measure toward the target. - m_SegProgress = distance > segMag ? 0.0F : (1.0F - (distance / segMag)); - m_Ended = true; + // Get normalized progress measure toward the target. + + if (distanceSqr > segMagSqr) + // We're too far away from this target. + m_SegProgress = 0.0; + else + m_SegProgress = (1.0F - (std::sqrt(distanceSqr) / std::sqrt(segMagSqr))); + + m_Ended = distVec.MagnitudeIsLessThan(m_SegmentEndedThreshold); + } else { + // Presume we're not done with all path segments until proven otherwise. + m_Ended = false; + + // Check if we are sufficiently close to the current target, or any of the next ones, + // to start going to whatever target is after that one. + // + // The limb might have been yanked and is closer to one of the future targets, so check them too. + + // This rest of the code will be working in local space, so convert input limb pos to that. + Vector limbPosLocal = ToLocalSpace(limbPos); + + // + // Iterate over all segments and find one whose target is closest to the limb position. + // + + // Segment positions are accumulative, so keep an accumulator. + Vector currentSegmentStartPos = GetCurrentSegStartLocal(); // Will be needed later. + Vector segmentPosAccumulator = currentSegmentStartPos; + + Vector closestSegmentStartPos; + float closestSegmentTargetDistanceSqr = std::numeric_limits::max(); + std::deque::iterator closestSegment = m_CurrentSegment; + + for (std::deque::iterator itr = m_CurrentSegment; itr != m_Segments.end(); ++itr) { + if (itr != m_CurrentSegment) { + if (m_FootCollisionsDisabledSegment >= 0 && + m_Segments.size() - (itr - m_Segments.begin()) <= m_FootCollisionsDisabledSegment) { + // We've already picked a segment (at least the current one), + // and the remaining ones are ones with collisions disabled. + // Ignore these. + break; + } + } + + Vector thisSegmentStartPos = segmentPosAccumulator; + segmentPosAccumulator += *itr; // The accumulator's value is now the target pos of this segment. + float thisSegmentDistanceSqr = (segmentPosAccumulator - limbPosLocal).GetSqrMagnitude(); + + if (thisSegmentDistanceSqr < closestSegmentTargetDistanceSqr) { + // This one's closer. + closestSegmentStartPos = thisSegmentStartPos; + closestSegmentTargetDistanceSqr = thisSegmentDistanceSqr; + closestSegment = itr; + } else { + // This one is *farther* than the last one. + // Assuming next segments will be only farther and farther, just break now. + break; } - // Next segment! - else { - m_SegProgress = 0.0F; + } + + // We will want to compute progress to whatever the new segment is. + float distanceToCurrentSegmentTargetSqr; + + + if (closestSegmentTargetDistanceSqr < m_SegmentEndedThreshold * m_SegmentEndedThreshold) { + // We're sufficiently close to this segment's target to go on. + // Either declare this path ended, or continue from the next segment. + + if (closestSegment + 1 == m_Segments.end()) { + // Closest segment is the last segment and we are at its target. Declare done. + m_Ended = true; + m_CurrentSegment = closestSegment; + + distanceToCurrentSegmentTargetSqr = closestSegmentTargetDistanceSqr; + + } else { + // Time to switch to next segment! m_SegTimer.Reset(); - m_Ended = false; + + m_CurrentSegment = closestSegment + 1; + + Vector currentSegmentTarget = closestSegmentStartPos + *closestSegment + *m_CurrentSegment; + distanceToCurrentSegmentTargetSqr = (currentSegmentTarget - limbPosLocal).GetSqrMagnitude(); } } else { - m_SegProgress = distance > segMag ? 0.0F : (1.0F - (distance / segMag)); - m_Ended = false; + // We're not close enough to that closest segment's target, but we can still try to do better. + + Vector currentSegmentTargetPos = currentSegmentStartPos + *m_CurrentSegment; + float currentSegmentDistanceSqr = (currentSegmentTargetPos - limbPosLocal).GetSqrMagnitude(); + + if (closestSegmentTargetDistanceSqr < currentSegmentDistanceSqr) { + // The target for this closest segment is closer than the current segment's. + // Fast-forward to it. + m_SegTimer.Reset(); + + m_CurrentSegment = closestSegment; + + distanceToCurrentSegmentTargetSqr = closestSegmentTargetDistanceSqr; + } else { + // Just get the distance to current segment's target. + Vector currentSegmentTarget = currentSegmentStartPos + *m_CurrentSegment; + distanceToCurrentSegmentTargetSqr = (currentSegmentTarget - limbPosLocal).GetSqrMagnitude(); + } } + + // Now compute a normalized progress measure towards the current segment. + + float currentSegmentMagnitudeSqr = m_CurrentSegment->GetSqrMagnitude(); + + if (distanceToCurrentSegmentTargetSqr > currentSegmentMagnitudeSqr) + // We're too far away from this target. + m_SegProgress = 0.0; + else + m_SegProgress = (1.0F - (std::sqrt(distanceToCurrentSegmentTargetSqr) / std::sqrt(currentSegmentMagnitudeSqr))); } } @@ -343,13 +458,11 @@ float LimbPath::GetRegularProgress() const { } int LimbPath::GetCurrentSegmentNumber() const { - int progress = 0; - if (!m_Ended && !IsStaticPoint()) { - for (std::deque::const_iterator itr = m_Segments.begin(); itr != m_CurrentSegment; ++itr) { - progress++; - } + if (m_Ended || IsStaticPoint()) { + return 0; + } else { + return m_CurrentSegment - m_Segments.begin(); } - return progress; } void LimbPath::Terminate() { @@ -379,7 +492,7 @@ bool LimbPath::RestartFree(Vector& limbPos, MOID MOIDToIgnore, int ignoreTeam) { if (IsStaticPoint()) { Vector notUsed; - Vector targetPos = m_JointPos + (RotatePoint(m_Start * GetTotalScaleMultiplier())); + Vector targetPos = ToWorldSpace(m_Start); Vector beginPos = targetPos; // TODO: don't hardcode the beginpos beginPos.m_Y -= 24; @@ -517,8 +630,8 @@ void LimbPath::Draw(BITMAP* pTargetBitmap, for (std::deque::const_iterator itr = m_Segments.begin(); itr != m_Segments.end(); ++itr) { nextPoint += *itr; - Vector prevWorldPosition = m_JointPos + (RotatePoint(prevPoint * GetTotalScaleMultiplier()) - targetPos); - Vector nextWorldPosition = m_JointPos + (RotatePoint(nextPoint * GetTotalScaleMultiplier()) - targetPos); + Vector prevWorldPosition = ToWorldSpace(prevPoint) - targetPos; + Vector nextWorldPosition = ToWorldSpace(nextPoint) - targetPos; line(pTargetBitmap, prevWorldPosition.m_X, prevWorldPosition.m_Y, nextWorldPosition.m_X, nextWorldPosition.m_Y, color); Vector min(std::min(prevWorldPosition.m_X, nextWorldPosition.m_X), std::min(prevWorldPosition.m_Y, nextWorldPosition.m_Y)); diff --git a/Source/Entities/LimbPath.h b/Source/Entities/LimbPath.h index 3ab0c98eff..57e718aff9 100644 --- a/Source/Entities/LimbPath.h +++ b/Source/Entities/LimbPath.h @@ -20,7 +20,7 @@ namespace RTE { /// Public member variable, method and friend function declarations public: - // Concrete allocation and cloning definitions + /// Concrete allocation and cloning definitions EntityAllocation(LimbPath); SerializableOverrideMethods; ClassInfoGetters; @@ -83,7 +83,7 @@ namespace RTE { /// Gets the number of Vector:s the internal array of 'waypoints' or /// segments of this LimbPath. - /// @return An int with he count. + /// @return An int with the count. int GetSegCount() const { return m_Segments.size(); } /// Gets a pointer to the segment at the given index. Ownership is NOT transferred. @@ -109,7 +109,7 @@ namespace RTE { /// Gets the APPROXIMATE scene position that the limb was reported to be /// last frame. This really shouldn't be used by external clients. - /// @return A Vector with the APPROXIAMTE scene/world coordinates of the limb as + /// @return A Vector with the APPROXIMATE scene/world coordinates of the limb as /// reported last. Vector GetProgressPos(); @@ -369,75 +369,62 @@ namespace RTE { protected: static Entity::ClassInfo m_sClass; - // The starting point of the path. - Vector m_Start; + Vector m_Start; //!< The starting point of the path. - // The number of starting segments, counting into the path from its beginning, - // that upon restart of this path will be tried in reverse order till one which - // yields a starting position that is clear of terrain is found. + /// The number of starting segments, counting into the path from its beginning, + /// that upon restart of this path will be tried in reverse order till one which + /// yields a starting position that is clear of terrain is found. size_t m_StartSegCount; - // Array containing the actual 'waypoints' or segments for the path. - std::deque m_Segments; + std::deque m_Segments; //!< Array containing the actual 'waypoints' or segments for the path. - // The iterator to the segment of the path that the limb ended up on the end of + /// The iterator to the segment of the path that the limb ended up on the end of. std::deque::iterator m_CurrentSegment; - int m_FootCollisionsDisabledSegment; //!< The segment after which foot collisions will be disabled for this limbpath, if it's for legs. + /// Count of segments at the end of the segments list for which foot collisions should be disabled + /// for this limbpath, if it's for legs. + int m_FootCollisionsDisabledSegment; - // Normalized measure of how far the limb has progressed toward the - // current segment's target. 0.0 means its farther away than the - // magnitude of the entire segment. 0.5 means it's half the mag of the segment - // away from the target. + /// Normalized measure of how far the limb has progressed toward the current + /// segment's target. + /// + /// 0.0 means its farther away than the magnitude of the entire segment. + /// 0.5 means it's half the mag of the segment away from the target. float m_SegProgress; - // The constant speed that the limb traveling this path has in m/s. - float m_TravelSpeed; + float m_TravelSpeed; //!< The constant speed that the limb traveling this path has in m/s. - // How close we must get to the end of each segment to consider it finished - float m_SegmentEndedThreshold; + float m_SegmentEndedThreshold; //!< How close we must get to the end of each segment to consider it finished - // The base/current travel speed multiplier - float m_BaseTravelSpeedMultiplier; - float m_CurrentTravelSpeedMultiplier; + float m_BaseTravelSpeedMultiplier; //!< The base travel speed multiplier + float m_CurrentTravelSpeedMultiplier; //!< The current travel speed multiplier - // The base/current scale multiplier (we extend the walkpath when running fast to take longer strides) - // This is a vector to allow scaling on seperate axis + /// The base scale multiplier for both axes. Vector m_BaseScaleMultiplier; + /// The current scale multiplier for both axes. (we extend the walkpath when running fast to take longer strides) Vector m_CurrentScaleMultiplier; - // The max force that a limb travelling along this path can push. - // In kg * m/(s^2) - float m_PushForce; - - // The latest known position of the owning actor's joint in world coordinates. - Vector m_JointPos; - // The latest known velocity of the owning actor's joint in world coordinates. - Vector m_JointVel; - // The rotation applied to this walkpath. - Matrix m_Rotation; - // The point we should be rotated around, in local space. - Vector m_RotationOffset; - // The offset to apply to our walkpath position, in local space. - Vector m_PositionOffset; - - // If GetNextTimeSeg() couldn't use up all frame time because the current segment - // ended,this var stores the remainder of time that should be used to progress - // on the next segment during the same frame. + float m_PushForce; //!< The max force that a limb travelling along this path can push, in kg * m/(s^2). + + Vector m_JointPos; //!< The latest known position of the owning actor's joint in world coordinates. + Vector m_JointVel; //!< The latest known velocity of the owning actor's joint in world coordinates. + Matrix m_Rotation; //!< The rotation applied to this walkpath. + Vector m_RotationOffset; //!< The point we should be rotated around, in local space. + Vector m_PositionOffset; //!< The offset to apply to our walkpath position, in local space. + + /// If GetNextTimeSeg() couldn't use up all frame time because the current segment + /// ended, this var stores the remainder of time that should be used to progress + /// on the next segment during the same frame. float m_TimeLeft; - // Times the amount of sim time spent since the last path traversal was started - Timer m_PathTimer; - // Times the amount of sim time spent pursuing the current segment's target. - Timer m_SegTimer; + Timer m_PathTimer; //!< Records the amount of sim time spent since the last path traversal was started. + Timer m_SegTimer; //!< Records the amount of sim time spent pursuing the current segment's target. - // Total length of this LimbPath, including the alternative starting segments, in pixel units - float m_TotalLength; - // Length of this LimbPath, excluding the alternative starting segments. - float m_RegularLength; - bool m_SegmentDone; - bool m_Ended; - bool m_HFlipped; + float m_TotalLength; //!< Total length of this LimbPath, including the alternative starting segments, in pixel units + float m_RegularLength; //!< Length of this LimbPath, excluding the alternative starting segments. + bool m_SegmentDone; //!< Unused? + bool m_Ended; //!< True if this path has ended. See method `PathEnded` for more information. + bool m_HFlipped; //!< True if this path is horizontally flipped. /// Private member variable and method declarations private: @@ -449,6 +436,26 @@ namespace RTE { /// @param point The point to rotate. /// @return The rotated point. Vector RotatePoint(const Vector& point) const; + + /// Inverse of RotatePoint. + /// @param point The rotated point + /// @return The point pre-rotation. + Vector InverseRotatePoint(const Vector& point) const; + + /// Converts an input position, absolute and in scene/world space, to local space for this LimbPath, + /// taking into account rotation and scaling. + /// @param position World space position + /// @returns Local space position + Vector ToLocalSpace(const Vector& position) const; + + /// Converts an input position, in local space for this LimbPath, into scene/world space, + /// taking into account rotation and scaling. + /// @param position Local space position + /// @returns World space position + Vector ToWorldSpace(const Vector& position) const; + + /// Gets the current segment's starting position (i.e. last segment's target) in local space. + Vector GetCurrentSegStartLocal() const; }; } // namespace RTE diff --git a/Source/System/Matrix.cpp b/Source/System/Matrix.cpp index c0b15e3bb5..19418c0970 100644 --- a/Source/System/Matrix.cpp +++ b/Source/System/Matrix.cpp @@ -136,13 +136,14 @@ Vector Matrix::operator/(const Vector& rhs) { } Vector retVec = rhs; - // Apply flipping as set. - retVec.m_X = m_Flipped[X] ? -retVec.m_X : retVec.m_X; - retVec.m_Y = m_Flipped[Y] ? -retVec.m_Y : retVec.m_Y; // Do the matrix multiplication. retVec.SetXY(m_Elements[0][0] * retVec.m_X + m_Elements[1][0] * retVec.m_Y, m_Elements[0][1] * retVec.m_X + m_Elements[1][1] * retVec.m_Y); + // Apply flipping as set. + retVec.m_X = m_Flipped[X] ? -retVec.m_X : retVec.m_X; + retVec.m_Y = m_Flipped[Y] ? -retVec.m_Y : retVec.m_Y; + return retVec; } diff --git a/Source/System/Matrix.h b/Source/System/Matrix.h index aacbdb03c6..8b11da1bdd 100644 --- a/Source/System/Matrix.h +++ b/Source/System/Matrix.h @@ -238,6 +238,7 @@ namespace RTE { } /// Division operator overload for a Matrix and a Vector. The vector will be transformed according to the Matrix's elements. + /// Flipping, if set, is performed after rotating. /// @param rhs A Vector reference as the right hand side operand. /// @return The resulting transformed Vector. Vector operator/(const Vector& rhs); @@ -246,7 +247,10 @@ namespace RTE { /// @param lhs A Vector reference as the left hand side operand. /// @param rhs A Matrix reference as the right hand side operand. /// @return A reference to the resulting Vector. - friend Vector operator/(const Vector& lhs, Matrix& rhs) { return rhs / lhs; } + friend Vector operator/(const Vector& lhs, const Matrix& rhs) { + Matrix m(rhs); + return m / lhs; + } /// Self-multiplication operator overload for Vector with a Matrix. /// @param lhs A Vector reference as the left hand side operand.