void checkForCompletedAnchors()
{
// If any anchors have reached their drop points, then trigger a restriction zone rebuild
int readyForWorkIdleCheckCount = 0;
if (anchorPoints != null)
{
foreach (BMod_AnchorPoint anchorPoint in anchorPoints)
{
if (anchorPoint.completedLockingSignal)
{
restrictedMovementAreaUpdateRequired = true;
anchorPoint.completedLockingSignal = false; // Reset the signal
}
if (balPosWorkingStatus == balPolStatusEnum.startingUp)
{
// Part of start up sequence. Due to the rigid frame not being in a suitable
// position on loading, everything must wait until the anchorpoints have
// reached their lock positions before anything is sent to the linked frame.
if (anchorPoint.currentAction == BMod_AnchorPoint.AnchorCurrentActionEnum.idle)
{
readyForWorkIdleCheckCount++;
}
}
}
// If all the anchor points are idle, then the startup sequence is complete.
if (balPosWorkingStatus == balPolStatusEnum.startingUp)
{
if (readyForWorkIdleCheckCount == anchorPoints.Length)
{
balPosWorkingStatus = balPolStatusEnum.readyForWork;
}
}
}
}
// Update is called once per frame
void Update()
{
// TODO Balance position needs to be upgraded to use a centre of balance
// system, where the body is tilted and shifted as needed.
// Process each of the distance holder and anchor points attached to this
// class.
if (restrictedMovementAreaUpdateRequired)
{
updateRestrictionRegion(false);
restrictedMovementAreaUpdateRequired = false;
}
// Control pad movement
float hozAxis = Input.GetAxis("Horizontal") * maxSlideMovementSpeed;
float verAxis = Input.GetAxis("Vertical") * maxSlideMovementSpeed;
noncumulativeOffset.x = Input.GetAxis("Horizontal");
noncumulativeOffset.z = Input.GetAxis("Vertical");
float keyBiasOffset = 0.5f; // This goes towards how far forward the anchor point
// is positioned in the direction of movement for keyboard based control.
// Collect bias from external controls
if (Input.GetKey(KeyCode.UpArrow))
{
slideOffset.z = slideOffset.z + (maxSlideMovementSpeed * Time.deltaTime);
noncumulativeOffset.z = keyBiasOffset;
noncumulativeOffset.x = 0.0f;
}
if (Input.GetKey(KeyCode.DownArrow))
{
slideOffset.z = slideOffset.z - (maxSlideMovementSpeed * Time.deltaTime);
noncumulativeOffset.z = -keyBiasOffset;
noncumulativeOffset.x = 0.0f;
}
if (Input.GetKey(KeyCode.LeftArrow))
{
slideOffset.x = slideOffset.x + (maxSlideMovementSpeed * Time.deltaTime);
noncumulativeOffset.z = 0.0f;
noncumulativeOffset.x = keyBiasOffset;
}
if (Input.GetKey(KeyCode.RightArrow))
{
slideOffset.x = slideOffset.x - (maxSlideMovementSpeed * Time.deltaTime);
noncumulativeOffset.z = 0.0f;
noncumulativeOffset.x = -keyBiasOffset;
}
// Controls switches
bool isLeaningSwitch = Input.GetButton("Button_R");
bool isNoLegLiftingSwitch = Input.GetButton("Button_L");
// Manual leg lifting button presses
int anchorLift = -1;
if (Input.GetButtonDown("Button_A"))
{
anchorLift = 0;
}
if (Input.GetButtonDown("Button_B"))
{
anchorLift = 1;
}
if (Input.GetButtonDown("Button_X"))
{
anchorLift = 3;
}
if (Input.GetButtonDown("Button_Y"))
{
anchorLift = 2;
}
if (anchorLift != -1)
{
BMod_AnchorPoint anchorPoint = anchorPoints[anchorLift];
anchorPoint.needsToLift = true;
}
//Debug.Log("Hoz " + hozAxis + ", Ver " + verAxis);
// Only apply horizontal and vertical slide axis if bal pos is ready for work
if (balPosWorkingStatus == balPolStatusEnum.readyForWork)
{
slideOffset.x = slideOffset.x + (hozAxis * Time.deltaTime);
slideOffset.z = slideOffset.z + (verAxis * Time.deltaTime);
}
// Rotation around centre point slide calculations here
float rotAxis = Input.GetAxis("Hoz Rotate") * rotationDegreeDelta;
// Calculate the restriction centre and compare with the actual roving centre.
Vector3 biasActualCentre = new Vector3();
foreach (Vector3 biasVector in biasMovementRestrictionArea)
{
biasActualCentre = biasActualCentre + biasVector;
}
biasActualCentre = biasActualCentre / biasMovementRestrictionArea.Count;
if (Vector3.Distance(biasCentreTrack, biasActualCentre) > 0.0f)
{
// If the centre if off then the robot is adjusting its balance
balPosWorkingStatus = balPolStatusEnum.adjustingBalance;
Vector3 biasMoved = Vector3.MoveTowards(biasCentreTrack, biasActualCentre, maxSlideMovementSpeed * Time.deltaTime);
slideOffset = slideOffset + (biasCentreTrack - biasMoved) * 1.2f; // Boost the movement
// Move the bias centre.
biasCentreTrack = biasCentreTrack - (biasCentreTrack - biasMoved);
if (biasCentreRepresentation != null)
{
biasCentreRepresentation.transform.position = biasCentreTrack;
}
}
else
{
// If the balance point has been reached then declare ready for work, unless still in the start up sequence.
if (balPosWorkingStatus != balPolStatusEnum.startingUp)
{
balPosWorkingStatus = balPolStatusEnum.readyForWork;
}
}
// Roll through the anchor points and collect the limit reached offset vectices
foreach (BMod_AnchorPoint anchorPoint in anchorPoints)
{
// Only pull from the ones locked in place
if (anchorPoint.lockedInPosition)
{
Vector3 limitCorrection = anchorPoint.limitReachCorrectionVector();
slideOffset.x = slideOffset.x + limitCorrection.x;
slideOffset.z = slideOffset.z + limitCorrection.z;
}
}
// Update position sliders (Anchor points for now) with acceleration bias.
// Only allow movement when balanced.
if ((previousSlideOffset != slideOffset || rotAxis != 0.0f) && !isLeaningSwitch && anchorPoints != null)
{
// Apply the slide and rotation changes to the anchor points.
float angleRad = rotAxis * rotationDegreeDelta * Mathf.Deg2Rad; // Changed rotationDegreeDelta to movementSpeed
foreach (BMod_AnchorPoint anchorPoint in anchorPoints)
{
if (anchorPoint.lockedInPosition)
{
// Only apply offset as translation difference from previous offset if the anchor is locked.
Vector3 rotationOffset = Vector3.zero;
if (rotAxis != 0.0f)
{
// Rotation offset
Vector3 currentPosition = anchorPoint.anchorVertex.transform.position;
// The current position should be a vector from x = 0, z = 0. So be applying a rotation
// to the current position and keeping the offset, this should work as a way of creating rotation.
float xVal = currentPosition.x * Mathf.Cos(angleRad) - currentPosition.z * Mathf.Sin(angleRad);
float yVal = currentPosition.y; // We're not changing the height. Future versions may need to height check.
float zVal = currentPosition.x * Mathf.Sin(angleRad) + currentPosition.z * Mathf.Cos(angleRad);
rotationOffset = (new Vector3(xVal, yVal, zVal)) - currentPosition;
}
anchorPoint.anchorVertex.offset += (slideOffset - previousSlideOffset) + rotationOffset; // Apply the change in offset
}
}
previousSlideOffset = slideOffset;
}
// Check if any anchors have reached thier drop points.
checkForCompletedAnchors();
// If the counter bias has zero movement then any anchor points waiting for
// release can be processed.
// Check if prevent lift button is not pressed
if (!isNoLegLiftingSwitch)
{
checkForReleaseableAnchors();
} //else {
// Debug.Log("Button R pressed held");
// }
// Adjust distance holder offsets
float crouchAxis = Input.GetAxis("Crouch") * crouchRange;
if (distanceHolders != null)
{
if (isLeaningSwitch)
{
// This is to allow the rotational leaning of the robot.
Vector3 leanAxis = new Vector3(-Input.GetAxis("Hoz Rotate"), 0.0f, Input.GetAxis("Crouch"));
leanAxis = leanAxis * 1.5f; // Increase the movement range
foreach (BMod_DistanceHolder distanceHolder in distanceHolders)
{
Vector3 distHold = distanceHolder.vertexPoint.transform.position;
Vector3 groundHolder = new Vector3(distHold.x, 0.0f, distHold.z);
float distToCentre = Vector3.Distance(biasActualCentre, groundHolder);
float distToTracked = Vector3.Distance(biasActualCentre + leanAxis, groundHolder);
float ratioAlt = distToTracked / distToCentre;
float tiltOffset = 0.0f;
if (ratioAlt != 1.0)
{
tiltOffset = distanceHolder.distanceToHold - (distanceHolder.distanceToHold * ratioAlt);
}
distanceHolder.distanceToHoldOffset = tiltOffset;
}
}
else
{
// Alter the height of each distance holder by the crouch axis
foreach (BMod_DistanceHolder distanceHolder in distanceHolders)
{
distanceHolder.distanceToHoldOffset = crouchAxis;
}
}
}
}
void checkForReleaseableAnchors()
{
// Cycle through anchor points to check for any that need to be released.
// Priority given to those furthest away from this balance position, which any tied then on priority value.
if (anchorPoints != null)
{
BMod_AnchorPoint anchorToUnlock = null;
int unlockedCount = 0;
List <BMod_AnchorPoint> requestors = new List <BMod_AnchorPoint>();
foreach (BMod_AnchorPoint anchor in anchorPoints)
{
if (anchor.needsToLift)
{
requestors.Add(anchor);
}
if (anchor.lockedInPosition == false || anchor.currentAction == BMod_AnchorPoint.AnchorCurrentActionEnum.waitingForBalance)
{
unlockedCount++;
}
}
// Only proceed if there's less anchor points unlocked than the maximum allowed.
if (unlockedCount < maxReleasable && requestors.Count > 0)
{
if (requestors.Count > maxReleasable)
{
// Filter by distance and then by priority if needed.
// Distance is how far from original lock position
// Use a dictionary with the distance rounded to whole number. If a duplicat entry exists,
// it is overriden if the duplcate has a lower priority level.
// Dictionary is then sorted highest to lowest and first entry taken out.
Dictionary <int, BMod_AnchorPoint> distanceAnchors = new Dictionary <int, BMod_AnchorPoint>();
foreach (BMod_AnchorPoint anchorCheck in requestors)
{
// Only working with ground height of zero for the distances for now.
// Was used originally as part of the pushing through the ground test.
Vector3 nonHeightAnchor = new Vector3(anchorCheck.anchorVertex.transform.position.x, 0.0f, anchorCheck.anchorVertex.transform.position.z);
int distanceToOriginalLockPoint = Mathf.RoundToInt(Vector3.Distance(anchorCheck.lockedPosition, nonHeightAnchor) * 10.0f);
if (distanceAnchors.ContainsKey(distanceToOriginalLockPoint))
{
if (distanceAnchors[distanceToOriginalLockPoint].priorityIndex < anchorCheck.priorityIndex)
{
// Replace original anchor point
distanceAnchors[distanceToOriginalLockPoint] = anchorCheck;
}
}
else
{
distanceAnchors.Add(distanceToOriginalLockPoint, anchorCheck);
}
}
// Loop through keys and find the largest distance
int maxDistanceFound = -99;
foreach (int keyEntry in distanceAnchors.Keys)
{
if (keyEntry > maxDistanceFound)
{
maxDistanceFound = keyEntry;
}
}
// If a distance was found that wasn't the initialiser number then assign for unlocking.
if (maxDistanceFound != -99)
{
anchorToUnlock = distanceAnchors[maxDistanceFound];
}
}
else
{
// Allow the anchor to be unlocked.
anchorToUnlock = requestors[0];
}
}
// If an anchor point was found to release, then unlock it.
// Safety check status will need to be implemented to allow centre point time to move
// within restriction zone if restricted area shrinks past it.
if (anchorToUnlock != null)
{
anchorToUnlock.currentAction = BMod_AnchorPoint.AnchorCurrentActionEnum.waitingForBalance;
balPosWorkingStatus = balPolStatusEnum.adjustingBalance; // Make sure this is called.
//anchorToUnlock.lockedInPosition = false;
anchorToUnlock.needsToLift = false;
Vector3 nonHeightAnchor = new Vector3(anchorToUnlock.anchorVertex.transform.position.x, 0.0f, anchorToUnlock.anchorVertex.transform.position.z);
Vector3 currentPosition = nonHeightAnchor;
// Update the unlock position to be slightly above the current position
anchorToUnlock.unlockedPosition = new Vector3(currentPosition.x, currentPosition.y + anchorLiftHeight, currentPosition.z);
restrictedMovementAreaUpdateRequired = true;
}
}
}
