public void FixedUpdate()
{
// Calculate Velocity for the feet //
// =============================== //
newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position; // Before: LeftFoot
newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
var mediaLeft = (newIKLeftPosition - oldIKLeftPosition);
var mediaRight = (newIKRightPosition - oldIKRightPosition);
feetSpeedLeft = new Vector3((mediaLeft.x / Time.fixedDeltaTime), (mediaLeft.y / Time.fixedDeltaTime), (mediaLeft.z / Time.fixedDeltaTime));
feetSpeedRight = new Vector3((mediaRight.x / Time.fixedDeltaTime), (mediaRight.y / Time.fixedDeltaTime), (mediaRight.z / Time.fixedDeltaTime));
oldIKLeftPosition = newIKLeftPosition;
oldIKRightPosition = newIKRightPosition;
newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position;
newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
if (drawVelocities)
{
DrawForce.ForDebug3D(newIKLeftPosition, -feetSpeedLeft, Color.cyan, 0.0025f);
}
}
// Update is called once per frame
void Update()
{
// Initial Information //
// =============================== //
// Define type of terrain where we are
if (brushPhysicalFootprint)
{
if (useTerrainPrefabs)
{
if (terrain.CompareTag("Snow"))
{
DefineSnow();
}
else if (terrain.CompareTag("Dry Sand"))
{
DefineDrySand();
}
else if (terrain.CompareTag("Mud"))
{
DefineMud();
}
else
{
DefineDefault();
}
}
}
// If we change the terrain, we change the data as well - both must have different GameObject names
if (terrain.name != myBipedalCharacter.GetComponent <RigidBodyControllerSimpleAnimator>().currentTerrain.name)
{
// Extract terrain information
terrain = myBipedalCharacter.GetComponent <RigidBodyControllerSimpleAnimator>().currentTerrain;
Debug.Log("[INFO] Updating to new terrain: " + terrain.name);
terrain_collider = terrain.GetComponent <Collider>();
terrain_data = terrain.terrainData;
terrain_size = terrain_data.size;
heightmap_width = terrain_data.heightmapResolution;
heightmap_height = terrain_data.heightmapResolution;
heightmap_data = terrain_data.GetHeights(0, 0, heightmap_width, heightmap_height);
heightmap_data_constant = terrain_data.GetHeights(0, 0, heightmap_width, heightmap_height);
heightmap_data_filtered = terrain_data.GetHeights(0, 0, heightmap_width, heightmap_height);
}
// Saving other variables for debugging purposes
heightIKLeft = _feetPlacement.LeftFootIKPosition.y;
heightIKRight = _feetPlacement.RightFootIKPosition.y;
isLeftFootGrounded = _feetPlacement.isLeftFootGrounded;
isRightFootGrounded = _feetPlacement.isRightFootGrounded;
centerGridLeftFoot = World2Grid(_feetPlacement.LeftFootIKPosition.x, _feetPlacement.LeftFootIKPosition.z);
centerGridRightFoot = World2Grid(_feetPlacement.RightFootIKPosition.x, _feetPlacement.RightFootIKPosition.z);
centerGridLeftFootHeight = new Vector3(_feetPlacement.LeftFootIKPosition.x, Get(centerGridLeftFoot.x, centerGridLeftFoot.z), _feetPlacement.LeftFootIKPosition.z);
centerGridRightFootHeight = new Vector3(_feetPlacement.RightFootIKPosition.x, Get(centerGridRightFoot.x, centerGridRightFoot.z), _feetPlacement.RightFootIKPosition.z);
// Calculate Proportion Feet Pivot //
// =============================== //
// 1. Bipedal -- _anim.pivotWeight only for bipedals
// 2. Quadrupeds -- New method on the way based on barycentric coordinates
if (isLeftFootGrounded && isRightFootGrounded)
{
weightInLeftFoot = (1 - _anim.pivotWeight);
weightInRightFoot = (_anim.pivotWeight);
}
else
{
if (!isLeftFootGrounded)
{
weightInLeftFoot = 0f;
weightInRightFoot = 1f;
}
else if (!isRightFootGrounded)
{
weightInLeftFoot = 1f;
weightInRightFoot = 0f;
}
}
// Bipedal Information //
// =============================== //
// Calculate Velocity for the feet //
// =============================== //
//newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position;
//newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
//var mediaLeft = (newIKLeftPosition - oldIKLeftPosition);
//var mediaRight = (newIKRightPosition - oldIKRightPosition);
//feetSpeedLeft = new Vector3((mediaLeft.x / Time.deltaTime), (mediaLeft.y / Time.deltaTime), (mediaLeft.z / Time.deltaTime));
//feetSpeedRight = new Vector3((mediaRight.x / Time.deltaTime), (mediaRight.y / Time.deltaTime), (mediaRight.z / Time.deltaTime));
//oldIKLeftPosition = newIKLeftPosition;
//oldIKRightPosition = newIKRightPosition;
//newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position;
//newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
//if (drawVelocities)
//{
// DrawForce.ForDebug3D(newIKLeftPosition, -feetSpeedLeft, Color.cyan, 0.0025f);
//}
// Calculate Forces for the feet //
// =============================== //
if (brushPhysicalFootprint)
{
// Weight Forces - Negative Y-component
weightForce = mass * (Physics.gravity);
weightForceLeft = weightForce * (weightInLeftFoot);
weightForceRight = weightForce * (weightInRightFoot);
// Extra for plotting //
// ============================================= //
weightForceLeftYFloat = weightForceLeft.y;
weightForceRightYFloat = weightForceRight.y;
weightForceYFloat = weightForce.y;
// ============================================= //
// Weight Force is already zero if the foot is not grounded - however, we draw only when foot is grounded
if (drawWeightForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, weightForceLeft, Color.blue, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, weightForceRight, Color.blue, 0.0025f);
}
else
{
if (drawWeightForces && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, weightForceLeft, Color.blue, 0.0025f);
}
if (drawWeightForces && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, weightForceRight, Color.blue, 0.0025f);
}
}
//--------------
// Impulse per foot - Linear Momentum change (final velocity for the feet is 0)
//feetImpulseLeft = mass * weightInLeftFoot * (Vector3.zero - feetSpeedLeft);
//feetImpulseRight = mass * weightInRightFoot * (Vector3.zero - feetSpeedRight);
// PROVISIONAL TEST - CAREFUL! Take only velocities going downward //
// =============================================================== //
if (feetSpeedLeft.y <= 0f)
{
feetImpulseLeft = mass * weightInLeftFoot * (Vector3.zero - feetSpeedLeft);
}
else
{
feetImpulseLeft = Vector3.zero;
}
if (feetSpeedRight.y <= 0f)
{
feetImpulseRight = mass * weightInRightFoot * (Vector3.zero - feetSpeedRight);
}
else
{
feetImpulseRight = Vector3.zero;
}
// =========================== //
//--------------
// Net force exerted by ground to each foot - Calculated using Impulse and Contact Time
// Positive (upward) if foot lands (negative velocity)
// Negative (downward) if foot rises (positive velocity)
momentumForceLeft = feetImpulseLeft / contactTime;
momentumForceRight = feetImpulseRight / contactTime;
momentumForce = momentumForceLeft + momentumForceRight;
// Extra for plotting (only positive values - when feet hit the ground) //
// ===================================================================== //
if (momentumForceLeft.y > 0f)
{
momentumForceLeftYFloat = momentumForceLeft.y;
}
if (momentumForceRight.y > 0f)
{
momentumForceRightYFloat = momentumForceRight.y;
}
if (momentumForce.y > 0f)
{
momentumForceYFloat = momentumForce.y;
}
// ===================================================================== //
// Momentum Forces are created when we hit the ground (that is, when such forces are positive in y, and the feet are grounded)
//if (drawMomentumForces && isMoving && momentumForceLeft.y > 0f && isLeftFootGrounded)
//{
// DrawForce.ForDebug3D(centerGridLeftFootHeight, momentumForceLeft, Color.red, 0.0025f);
//}
//if (drawMomentumForces && isMoving && momentumForceRight.y > 0f && isRightFootGrounded)
//{
// DrawForce.ForDebug3D(centerGridRightFootHeight, momentumForceRight, Color.red, 0.0025f);
//}
if (drawMomentumForces && isMoving && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, momentumForceLeft, Color.red, 0.0025f);
}
if (drawMomentumForces && isMoving && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, momentumForceRight, Color.red, 0.0025f);
}
//--------------
// GRF (Ground Reaction Force) that the ground exerts to each foot
totalGRForceLeft = momentumForceLeft - (weightForceLeft);
totalGRForceRight = momentumForceRight - (weightForceRight);
totalGRForce = totalGRForceLeft + totalGRForceRight;
// Extra for plotting //
// ============================================= //
totalGRForceLeftYFloat = totalGRForceLeft.y;
totalGRForceRightYFloat = totalGRForceRight.y;
totalGRForceYFloat = totalGRForce.y;
// ============================================= //
// Other color
Color darkGreen = new Color(0.074f, 0.635f, 0.062f, 1f);
// GRF is already zero if the foot is not grounded - however, we draw only when foot is grounded
if (drawGRForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalGRForceLeft, darkGreen, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, totalGRForceRight, darkGreen, 0.0025f);
}
else
{
if (drawGRForces && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalGRForceLeft, darkGreen, 0.0025f);
}
if (drawGRForces && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, totalGRForceRight, darkGreen, 0.0025f);
}
}
//--------------
// Reaction Force for the feet (3rd Newton Law)
totalForceLeftFoot = -totalGRForceLeft;
totalForceRightFoot = -totalGRForceRight;
totalForceFoot = totalForceLeftFoot + totalForceRightFoot;
//--------------
// Save max/min values reached for the feet forces in Z
maxTotalForceLeftFootZOld = totalForceLeftFoot.z;
if (maxTotalForceLeftFootZOld > maxTotalForceLeftFootZ)
{
maxTotalForceLeftFootZ = maxTotalForceLeftFootZOld;
maxTotalForceLeftFootZNorm = totalForceLeftFoot.normalized.z;
}
minTotalForceLeftFootZOld = totalForceLeftFoot.z;
if (minTotalForceLeftFootZOld < minTotalForceLeftFootZ)
{
minTotalForceLeftFootZ = minTotalForceLeftFootZOld;
minTotalForceLeftFootZNorm = totalForceLeftFoot.normalized.z;
}
// Reset Values
if (!isLeftFootGrounded)
{
maxTotalForceLeftFootZ = 0f;
minTotalForceLeftFootZ = 0f;
maxTotalForceLeftFootZNorm = 0f;
minTotalForceLeftFootZNorm = 0f;
}
// Save max/min values reached for the feet forces in Z
maxTotalForceRightFootZOld = totalForceRightFoot.z;
if (maxTotalForceRightFootZOld > maxTotalForceRightFootZ)
{
maxTotalForceRightFootZ = maxTotalForceRightFootZOld;
maxTotalForceRightFootZNorm = totalForceRightFoot.normalized.z;
}
minTotalForceRightFootZOld = totalForceRightFoot.z;
if (minTotalForceRightFootZOld < minTotalForceRightFootZ)
{
minTotalForceRightFootZ = minTotalForceRightFootZOld;
minTotalForceRightFootZNorm = totalForceRightFoot.normalized.z;
}
// Reset Values
if (!isRightFootGrounded)
{
maxTotalForceRightFootZ = 0f;
minTotalForceRightFootZ = 0f;
maxTotalForceRightFootZNorm = 0f;
minTotalForceRightFootZNorm = 0f;
}
//--------------
// Feet Forces are created when we hit the ground (that is, when the Y-component of the Momentum Force is positive)
// Only when the feet rise up, Feet Forces do not exist. The muscle is the responsable to lift the foot up
// Also, the foot must be grounded to have a Feet Force actuating onto the ground
if (drawFeetForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeftFoot, Color.black, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRightFoot, Color.black, 0.0025f);
}
else
{
if (drawFeetForces && momentumForceLeft.y > 0f && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeftFoot, Color.black, 0.0025f);
}
if (drawFeetForces && momentumForceRight.y > 0f && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRightFoot, Color.black, 0.0025f);
}
//if (drawFeetForces && isLeftFootGrounded)
//{
// DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeftFoot, Color.black, 0.0025f);
//}
//if (drawFeetForces && isRightFootGrounded)
//{
// DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRightFoot, Color.black, 0.0025f);
//}
}
}
// Quadrupeds Information //
/////////// TODO ///////////
// =============================== //
// Print the position of the feet in both systems (world and grid)
if (printFeetPositions)
{
Debug.Log("[INFO] Left Foot Coords (World): " + _feetPlacement.LeftFootIKPosition.ToString());
Debug.Log("[INFO] Left Foot Coords (Grid): " + centerGridLeftFoot.ToString());
Debug.Log("[INFO] Right Foot Coords (World): " + _feetPlacement.RightFootIKPosition.ToString());
Debug.Log("[INFO] Right Foot Coords (Grid): " + centerGridRightFoot.ToString());
Debug.Log("-----------------------------------------");
}
// Print the forces
if (printFeetForces)
{
Debug.Log("[INFO] Weight Force: " + weightForce);
Debug.Log("[INFO] Left Foot Speed: " + feetSpeedLeft);
Debug.Log("[INFO] Right Foot Speed: " + feetSpeedRight);
Debug.Log("[INFO] Left Foot Impulse: " + feetImpulseLeft);
Debug.Log("[INFO] Right Foot Impulse: " + feetImpulseRight);
Debug.Log("[INFO] GRF Left Foot: " + totalGRForceLeft);
Debug.Log("[INFO] GRF Right Foot: " + totalGRForceRight);
Debug.Log("[INFO] Total Force Left Foot: " + totalForceLeftFoot);
Debug.Log("[INFO] Total Force Right Foot: " + totalForceRightFoot);
Debug.Log("-----------------------------------------");
}
// =============================== //
// Apply brush to feet
if (brushPhysicalFootprint)
{
// Brush is only called if we are within the contactTime.
// Due to the small values, the provisional solution requires to add an offset to give the system enough time to create the footprint.
timePassed += Time.deltaTime;
if (timePassed <= contactTime + offset)
{
// Brush that takes limbs positions and creates physically-based footprints
brushPhysicalFootprint.CallFootprint(_feetPlacement.LeftFootIKPosition.x, _feetPlacement.LeftFootIKPosition.z,
_feetPlacement.RightFootIKPosition.x, _feetPlacement.RightFootIKPosition.z);
}
}
// Provisional: We reset the time passed everytime when we lift the feet.
// Not very accurate, it would be better to create a time variable per feet and pass it though the method.
if ((!isLeftFootGrounded || !isRightFootGrounded) && isMoving)
{
timePassed = 0f;
}
// Provisional: When is still (once) - Stopping when reaching the deformation required was not giving very good results -> TODO: Improve!
if (!isMoving && (!isLeftFootGrounded || !isRightFootGrounded) && provCounter <= 3)
{
timePassed = 0f;
provCounter += 1;
}
// Provisional: Each time I change motion, resets the time.
isMoving = _anim.GetBool("isWalking");
if (isMoving != oldIsMoving)
{
timePassed = 0f;
oldIsMoving = isMoving;
provCounter = 0;
}
}
// Update is called once per frame
void Update()
{
// Initial Information //
// =============================== //
// Saving other variables for debugging purposes
heightIKLeft = _feetPlacement.LeftFootIKPosition.y;
heightIKRight = _feetPlacement.RightFootIKPosition.y;
isLeftFootGrounded = _feetPlacement.isLeftFootGrounded;
isRightFootGrounded = _feetPlacement.isRightFootGrounded;
centerGridLeftFoot = World2Grid(_feetPlacement.LeftFootIKPosition.x, _feetPlacement.LeftFootIKPosition.z);
centerGridRightFoot = World2Grid(_feetPlacement.RightFootIKPosition.x, _feetPlacement.RightFootIKPosition.z);
centerGridLeftFootHeight = new Vector3(_feetPlacement.LeftFootIKPosition.x, Get(centerGridLeftFoot.x, centerGridLeftFoot.z), _feetPlacement.LeftFootIKPosition.z);
centerGridRightFootHeight = new Vector3(_feetPlacement.RightFootIKPosition.x, Get(centerGridRightFoot.x, centerGridRightFoot.z), _feetPlacement.RightFootIKPosition.z);
// Calculate Proportion Feet Pivot //
// =============================== //
// 1. Bipedal -- Doesnt work for certain animations -- Not for quadrupeds
if (isLeftFootGrounded && isRightFootGrounded)
{
weightInLeftFoot = (1 - _anim.pivotWeight);
weightInRightFoot = (_anim.pivotWeight);
}
else
{
if (!isLeftFootGrounded)
{
weightInLeftFoot = 0f;
weightInRightFoot = 1f;
}
else if (!isRightFootGrounded)
{
weightInLeftFoot = 1f;
weightInRightFoot = 0f;
}
}
// 2. Based on COM for all creatures (quadrupeds) - TODO
// Bipedal Information //
// =============================== //
// Calculate Velocity for the feet //
// =============================== //
newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position;
newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
var mediaLeft = (newIKLeftPosition - oldIKLeftPosition);
var mediaRight = (newIKRightPosition - oldIKRightPosition);
feetSpeedLeft = new Vector3((mediaLeft.x / Time.deltaTime), (mediaLeft.y / Time.deltaTime), (mediaLeft.z / Time.deltaTime));
feetSpeedRight = new Vector3((mediaRight.x / Time.deltaTime), (mediaRight.y / Time.deltaTime), (mediaRight.z / Time.deltaTime));
oldIKLeftPosition = newIKLeftPosition;
oldIKRightPosition = newIKRightPosition;
newIKLeftPosition = _anim.GetBoneTransform(HumanBodyBones.LeftFoot).position;
newIKRightPosition = _anim.GetBoneTransform(HumanBodyBones.RightFoot).position;
if (drawVelocities)
{
DrawForce.ForDebug3D(newIKLeftPosition, -feetSpeedLeft, Color.cyan, 0.0025f);
}
// Calculate Forces for the feet //
// =============================== //
if (brushPhysicalFootprint)
{
// Forces Feet - TODO: Maybe a gravityForce per foot using the proportional mass - NEGATIVE
//gravityForceLeft = customMass * (weightInLeftFoot) * (Physics.gravity);
//gravityForceRight = customMass * (weightInRightFoot) * (Physics.gravity);
//gravityForce = gravityForceLeft + gravityForceRight;
// New weight forces feet
gravityForce = customMass * (Physics.gravity);
gravityForceLeft = gravityForce * (weightInLeftFoot);
gravityForceRight = gravityForce * (weightInRightFoot);
// Extra for plotting
gravityForceLeftYFloat = gravityForceLeft.y;
gravityForceRightYFloat = gravityForceRight.y;
gravityForceYFloat = gravityForce.y;
// The weight force for each foot is already zero if it is not grounded - however, drawing only when foot is grounded
if (drawWeightForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, gravityForceLeft, Color.blue, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, gravityForceRight, Color.blue, 0.0025f);
}
else
{
if (drawWeightForces && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, gravityForceLeft, Color.blue, 0.0025f);
}
if (drawWeightForces && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, gravityForceRight, Color.blue, 0.0025f);
}
}
//--------------
// Impulse per foot - Linear Momentum Change (final velocity for feet is 0)
feetImpulseLeft = customMass * weightInLeftFoot * (Vector3.zero - feetSpeedLeft);
feetImpulseRight = customMass * weightInRightFoot * (Vector3.zero - feetSpeedRight);
//--------------
// Net force exerted by ground to each foot - Calculated using Impulse and Contact Time
// Positive (upward) if foot lands (Negative velocity)
// Negative (downward) if foot rises (Positive velocity)
forceNetExertedByGroundLeft = feetImpulseLeft / contactTime;
forceNetExertedByGroundRight = feetImpulseRight / contactTime;
forceNetExertedByGround = forceNetExertedByGroundLeft + forceNetExertedByGroundRight;
// Extra for plotting (only positive values - when feet hit the ground)
if (forceNetExertedByGroundLeft.y > 0f)
{
forceNetExertedByGroundLeftYFloat = forceNetExertedByGroundLeft.y;
}
if (forceNetExertedByGroundRight.y > 0f)
{
forceNetExertedByGroundRightYFloat = forceNetExertedByGroundRight.y;
}
if (forceNetExertedByGround.y > 0f)
{
forceNetExertedByGroundYFloat = forceNetExertedByGround.y;
}
// Momentum Forces are created when we hit the ground - when such forces are positive in y, and the feet are grounded.
if (drawMomentumForces && isMoving && forceNetExertedByGroundLeft.y > 0f && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, forceNetExertedByGroundLeft, Color.red, 0.0025f);
}
if (drawMomentumForces && isMoving && forceNetExertedByGroundRight.y > 0f && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, forceNetExertedByGroundRight, Color.red, 0.0025f);
}
//--------------
// Total Force that each foot applys to ground and vicecersa (3rd Newton Law)
// Sum so it can give me directly the F of the object hitting the ground, and not viceversa.
totalForceLeft = forceNetExertedByGroundLeft - (gravityForceLeft);
totalForceRight = forceNetExertedByGroundRight - (gravityForceRight);
totalForce = totalForceLeft + totalForceRight;
// Extra for plotting
totalForceLeftYFloat = totalForceLeft.y;
totalForceRightYFloat = totalForceRight.y;
totalForceYFloat = totalForce.y;
// GRFs are always there like Weight Forces - as long as the feet are grounded
if (drawGRForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeft, Color.green, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRight, Color.green, 0.0025f);
}
else
{
if (drawGRForces && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeft, Color.green, 0.0025f);
}
if (drawGRForces && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRight, Color.green, 0.0025f);
}
}
//--------------
// Reaction Force for the feet: 3rd Newton Law
totalForceLeftFoot = -totalForceLeft;
totalForceRightFoot = -totalForceRight;
totalForceFoot = totalForceLeftFoot + totalForceRightFoot;
// We will plot these when Momentum Forces are positive in y, and the feet are grounded.
if (drawFeetForces && !isMoving)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeftFoot, Color.black, 0.0025f);
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRightFoot, Color.black, 0.0025f);
}
else
{
if (drawFeetForces && forceNetExertedByGroundLeft.y > 0f && isLeftFootGrounded)
{
DrawForce.ForDebug3D(centerGridLeftFootHeight, totalForceLeftFoot, Color.black, 0.0025f);
}
if (drawFeetForces && forceNetExertedByGroundRight.y > 0f && isRightFootGrounded)
{
DrawForce.ForDebug3D(centerGridRightFootHeight, totalForceRightFoot, Color.black, 0.0025f);
}
}
}
// Quadrupeds Information //
/////////// TODO ///////////
// =============================== //
// Print the position of the feet in both systems
if (printFeetPositions)
{
Debug.Log("[INFO] Left Foot Coords (World): " + _feetPlacement.LeftFootIKPosition.ToString());
Debug.Log("[INFO] Left Foot Coords (Grid): " + centerGridLeftFoot.ToString());
Debug.Log("[INFO] Right Foot Coords (World): " + _feetPlacement.RightFootIKPosition.ToString());
Debug.Log("[INFO] Right Foot Coords (Grid): " + centerGridRightFoot.ToString());
Debug.Log("-----------------------------------------");
}
// Print the position of the feet in both systems
if (printFeetForces)
{
Debug.Log("[INFO] Gravity Force: " + gravityForce);
Debug.Log("[INFO] Left Foot Speed: " + feetSpeedLeft);
Debug.Log("[INFO] Right Foot Speed: " + feetSpeedRight);
Debug.Log("[INFO] Left Foot Impulse: " + feetImpulseLeft);
Debug.Log("[INFO] Right Foot Impulse: " + feetImpulseRight);
Debug.Log("[INFO] Total Force Left Foot: " + totalForceLeft);
Debug.Log("[INFO] Total Force Right Foot: " + totalForceRight);
Debug.Log("-----------------------------------------");
}
// =============================== //
// Apply brush to feet
if (brushPhysicalFootprint)
{
// High-level controller for the contact time
/*
* Brush is only called if we are within the contactTime.
* Due to the small values, the provisional solution requires to add an offset
* to give the system enough time to create the footprint.
*/
//brushPhysicalFootprint.CallFootprint(_feetPlacement.LeftFootIKPosition.x, _feetPlacement.LeftFootIKPosition.z,
// _feetPlacement.RightFootIKPosition.x, _feetPlacement.RightFootIKPosition.z);
timePassed += Time.deltaTime;
if (timePassed <= contactTime + offset)
{
// Brush that takes limbs positions and creates footprints
brushPhysicalFootprint.CallFootprint(_feetPlacement.LeftFootIKPosition.x, _feetPlacement.LeftFootIKPosition.z,
_feetPlacement.RightFootIKPosition.x, _feetPlacement.RightFootIKPosition.z);
}
}
// We reset the time passed everytime when we lift the feet
// Not very accurate, it would be better to create a time variable per feet and pass it though the method.
// Provisional: For moving gate
if ((!isLeftFootGrounded || !isRightFootGrounded) && isMoving)
{
//Debug.Log("RESET TIME");
timePassed = 0f;
}
// Provisional: When is still (once) - BIG CHANGE MUST BE DONE HERE - IF POSSIBLE ALWAYS WITH DISTANCE DEFORMED AS CONDITION
if (!isMoving && (!isLeftFootGrounded || !isRightFootGrounded) && provCounter <= 3)
{
timePassed = 0f;
provCounter += 1;
}
// Provisional: Each time I change motion, resets the time
isMoving = _anim.GetBool("isWalking");
if (isMoving != oldIsMoving)
{
timePassed = 0f;
oldIsMoving = isMoving;
provCounter = 0;
}
//// Provisional: Reset button
if (resetTimeOnce != oldResetTimeOnce)
{
timePassed = 0f;
provCounter = 0;
oldResetTimeOnce = resetTimeOnce;
}
////////
//if (!isLeftFootGrounded || !isRightFootGrounded)
//{
// //Debug.Log("RESET TIME");
// timePassed = 0f;
//}
//else if (isLeftFootGrounded && isRightFootGrounded)
//{
// if (Get(centerGridLeftFoot.x, centerGridLeftFoot.z) >= GetConstant(centerGridLeftFoot.x, centerGridLeftFoot.z) - brushPhysicalFootprint.HeightCellDisplacementYoungLeftCopy)
// {
// Debug.Log("RESET TIME");
// timePassed = 0f;
// }
// else if (Get(centerGridRightFoot.x, centerGridRightFoot.z) >= GetConstant(centerGridRightFoot.x, centerGridRightFoot.z) - brushPhysicalFootprint.HeightCellDisplacementYoungRightCopy)
// {
// Debug.Log("RESET TIME");
// timePassed = 0f;
// }
//}
/////////
//if (isLeftFootGrounded && isRightFootGrounded)
//{
// //Debug.Log("RESET TIME");
// Debug.Log("Height now: " + Get(centerGridLeftFoot.x, centerGridLeftFoot.z));
// Debug.Log("Height init: " + GetConstant(centerGridLeftFoot.x, centerGridLeftFoot.z));
// Debug.Log("Need to remove: " + brushPhysicalFootprint.HeightCellDisplacementYoungLeftCopy);
// if (Get(centerGridLeftFoot.x, centerGridLeftFoot.z) >= GetConstant(centerGridLeftFoot.x, centerGridLeftFoot.z) - brushPhysicalFootprint.HeightCellDisplacementYoungLeftCopy)
// {
// Debug.Log("RESET TIME");
// timePassed = 0f;
// }
//}
/////////
//if (isLeftFootGrounded || isRightFootGrounded)
//{
// if (brushPhysicalFootprint)
// {
// //Debug.Log("NOW IN LEFT: " + Get(centerGridLeftFoot.x, centerGridLeftFoot.z));
// //Debug.Log("HOW MUCH WAS INITIALLY: " + GetConstant(centerGridLeftFoot.x, centerGridLeftFoot.z));
// //Debug.Log("HOW MUCH SHOULD I REDUCE: " + brushPhysicalFootprint.HeightCellDisplacementYoungLeftCopy);
// if (Get(centerGridLeftFoot.x, centerGridLeftFoot.z) >= GetConstant(centerGridLeftFoot.x, centerGridLeftFoot.z) - brushPhysicalFootprint.HeightCellDisplacementYoungLeftCopy)
// {
// Debug.Log("RESET TIME");
// timePassed = 0f;
// }
// if (Get(centerGridRightFoot.x, centerGridRightFoot.z) >= GetConstant(centerGridRightFoot.x, centerGridRightFoot.z) - brushPhysicalFootprint.HeightCellDisplacementYoungRightCopy)
// {
// Debug.Log("RESET TIME");
// timePassed = 0f;
// }
// }
//}
}
