Improve LimbPath.cpp - rewrite ReportProgress, etc

The rewrite checks all path segments from the current one to the last and finds
the best one to continue from, instead of fixating on just the one. This helps
in cases where feet might be yanked along the path by excessive horizontal
acceleration and removes quite a bunch of stumbling.

Also fix Vector/Matrix division, making it the proper inverse of multiplication
and applicable to const matrix.

Author: Architector4

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<Vector>::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<Vector>::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<Vector>::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,119 @@ 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();
 
-		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.
+		m_SegProgress = distance > segMag ? 0.0F : (1.0F - (distance / segMag));
+
+		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<float>::max();
+		std::deque<Vector>::iterator closestSegment = m_CurrentSegment;
+
+		for (std::deque<Vector>::iterator itr = m_CurrentSegment; itr != m_Segments.end(); ++itr) {
+			if (itr != m_CurrentSegment) {
+				if (m_FootCollisionsDisabledSegment >= 0 && GetSegCount() - (itr - m_Segments.begin()) <= m_FootCollisionsDisabledSegment) {
+					// We've already picked a segment, 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 distanceToCurrentSegmentTarget;
+
+
+		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;
+
+				distanceToCurrentSegmentTarget = std::sqrt(closestSegmentTargetDistanceSqr);
+
+			} else {
+				// Time to switch to next segment!
 				m_SegTimer.Reset();
-				m_Ended = false;
+
+				m_CurrentSegment = closestSegment + 1;
+
+				Vector currentSegmentTarget = closestSegmentStartPos + *closestSegment + *m_CurrentSegment;
+				distanceToCurrentSegmentTarget = (currentSegmentTarget - limbPosLocal).GetMagnitude();
 			}
 		} 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;
+
+				distanceToCurrentSegmentTarget = std::sqrt(closestSegmentTargetDistanceSqr);
+			} else {
+				// Just get the distance to current segment's target.
+				Vector currentSegmentTarget = currentSegmentStartPos + *m_CurrentSegment;
+				distanceToCurrentSegmentTarget = (currentSegmentTarget - limbPosLocal).GetMagnitude();
+			}
 		}
+
+		// Now compute a normalized progress measure towards the current segment.
+
+		float currentSegmentMagnitude = m_CurrentSegment->GetMagnitude();
+
+		if (distanceToCurrentSegmentTarget > currentSegmentMagnitude)
+			// We're too far away from this target.
+			m_SegProgress = 0.0;
+		else
+			m_SegProgress = (1.0F - (distanceToCurrentSegmentTarget / currentSegmentMagnitude));
 	}
 }
 
@@ -343,13 +451,11 @@ float LimbPath::GetRegularProgress() const {
 }
 
 int LimbPath::GetCurrentSegmentNumber() const {
-	int progress = 0;
-	if (!m_Ended && !IsStaticPoint()) {
-		for (std::deque<Vector>::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 +485,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 +623,8 @@ void LimbPath::Draw(BITMAP* pTargetBitmap,
 	for (std::deque<Vector>::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));

Source/Entities/LimbPath.h

@@ -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();
 
@@ -383,7 +383,9 @@ namespace RTE {
 		// The iterator to the segment of the path that the limb ended up on the end of
 		std::deque<Vector>::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
@@ -449,6 +451,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

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;
 }
 

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.