virtual public void adjustStepHeight()
{
Vector3 footPos = lowLCon.getSwingFoot().getWorldBottomPosition();
//Vector3 swingFootV = lowLCon.getSwingFoot().getAbsoluteVelocityForLocalPoint(lowLCon.getSwingFoot().getLocalBottomPosition());
Vector3 swingFootV = lowLCon.getCharacterFrame() * (lowLCon.getVelSagittal() * lowLCon.getCharacter().getLocalFrontAxis() - lowLCon.getVelDCoronal() * lowLCon.getCharacter().getLocalLeftAxis());
swingFootV[lowLCon.getWorldUpAxisID()] = 0;
//swingFootV.Normalize();
Vector3 rayPos = footPos + 0.7f * swingFootV;
float rayStartY = lowLCon.getComPosition() [lowLCon.getWorldUpAxisID()];
Vector3 rayStartPos = rayPos;
rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
float rayEndY = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
Vector3 rayEndPos = rayPos;
rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY;
RaycastHit raycastHit = new RaycastHit();
if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
{
Debug.DrawLine(rayStartPos, rayEndPos, Color.blue);
lowLCon.swingFootFrontPosition = raycastHit.point;
}
else
{
Vector3 pos = lowLCon.getSwingHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis;
Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.blue);
lowLCon.swingFootFrontPosition = pos - lowLCon.additionHeight * SimGlobals.upAxis;
}
//Debug.Log("aaaaaaaaaaaaa "+lowLCon.swingFootFrontPosition);
rayPos = footPos;
rayStartPos = rayPos;
rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
rayEndPos = rayPos;
rayEndPos [lowLCon.getWorldUpAxisID()] = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
{
Debug.DrawLine(rayStartPos, rayEndPos, Color.green);
lowLCon.swingFootBasePosition = raycastHit.point;
lowLCon.swingFootBaseNormal = raycastHit.normal;
}
else
{
lowLCon.swingFootBasePosition = lowLCon.swingFootFrontPosition;
lowLCon.swingFootBaseNormal = Vector3.zero;
}
//Debug.Log("bbbbbbbbbbbbb "+lowLCon.swingFootBasePosition);
// Debug.Log("aaaaaaaaaaaaaaaaa "+lowLCon.swingFootBaseNormal);
footPos = lowLCon.getStanceFoot().getWorldBottomPosition();
rayPos = footPos;
rayEndY = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
rayStartPos = rayPos;
rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
rayEndPos = rayPos;
rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY;
if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
{
Debug.DrawLine(rayStartPos, rayEndPos, Color.green);
lowLCon.standFootBasePosition = raycastHit.point;
lowLCon.standFootBaseNormal = raycastHit.normal;
}
else
{
Vector3 pos = lowLCon.getStandHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis;
Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.green);
lowLCon.standFootBasePosition = pos - lowLCon.additionHeight * SimGlobals.upAxis;
lowLCon.standFootBaseNormal = Vector3.zero;
}
// Debug.Log("bbbbbbbbbbbbbbbbb "+lowLCon.standFootBaseNormal);
lowLCon.swingToStandBaseVector = Quaternion.Inverse(lowLCon.getCharacterFrame()) * (lowLCon.standFootBasePosition - lowLCon.swingFootBasePosition);
//Debug.Log("bbbbbbbbbbbbb "+lowLCon.swingToStandBaseVector);
float h = lowLCon.getDoubleStanceCOMError().magnitude * 1.6f;
h = Mathf.Clamp(h, lowLCon.getStepHeight(), maxStepHeight);
lowLCon.swingLegPanicHeight = h - lowLCon.getStepHeight();
//Debug.Log("bbbbbbbbbbbbbbbbb "+lowLCon.swingLegPanicHeight);
}