Improve LimbPath.cpp - rewrite ReportProgress, etc

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.