/**
* this method gets called every time the controller transitions to a new state
*/
virtual public void conTransitionPlan()
{
lowLCon.updateSwingAndStanceReferences();
lowLCon.updateDAndV();
standFootStartPos = lowLCon.getStanceFoot().getWorldBottomPosition();
swingFootStartPos = lowLCon.getSwingFoot().getWorldBottomPosition();
//now prepare the step information for the following step:
lowLCon.swingFootHeightTrajectory.clear();
//lowLCon.standFootHeightTrajectory.clear();
lowLCon.swingFootCoronalTrajectory.clear();
lowLCon.standFootCoronalTrajectory.clear();
lowLCon.swingFootSagittalTrajectory.clear();
lowLCon.standFootSagittalTrajectory.clear();
stepRaycastHits.Clear();
if (lowLCon.getIsComputeIPStep())
{
lowLCon.swingFootHeightTrajectory.addKnot(0, 0);
lowLCon.swingFootHeightTrajectory.addKnot(0.5f, lowLCon.getStepHeight());
lowLCon.swingFootHeightTrajectory.addKnot(1, 0);
lowLCon.swingFootSagittalTrajectory.addKnot(0, 0);
lowLCon.swingFootSagittalTrajectory.addKnot(1, 0);
lowLCon.swingFootCoronalTrajectory.addKnot(0, 0);
lowLCon.swingFootCoronalTrajectory.addKnot(1, 0);
}
else
{
if (lowLCon.getIsAddFootJoint())
{
lowLCon.getSwingFoot().removeFixedPositionJoint();
lowLCon.getStanceFoot().removeFixedPositionJoint();
}
lowLCon.swingFootHeightTrajectory.addKnot(0, 0);
lowLCon.swingFootHeightTrajectory.addKnot(0.5f, lowLCon.getStepHeight());
lowLCon.swingFootHeightTrajectory.addKnot(1, 0);
float w = lowLCon.getLocalLeftAxisSign() * sagittalStepWidth;
w = (lowLCon.getStance() == SimGlobals.LEFT_STANCE)?w:-w;
lowLCon.swingFootSagittalTrajectory.addKnot(0, 0 + w);
lowLCon.swingFootSagittalTrajectory.addKnot(1, lowLCon.getSwingStepLength() + w);
lowLCon.standFootSagittalTrajectory.addKnot(0, 0 - w);
lowLCon.standFootSagittalTrajectory.addKnot(1, lowLCon.getStandStepLength() - w);
w = lowLCon.getLocalLeftAxisSign() * coronalStepWidth;
w = (lowLCon.getStance() == SimGlobals.LEFT_STANCE)?w:-w;
lowLCon.swingFootCoronalTrajectory.addKnot(0, 0 + w);
lowLCon.swingFootCoronalTrajectory.addKnot(1, 0 + w);
//lowLCon.standFootCoronalTrajectory.addKnot(0, 0);
//lowLCon.standFootCoronalTrajectory.addKnot(1, 0);
//float contactPosDis = (lowLCon.getStanceFoot().getWorldBottomPosition() - lowLCon.swingFootBasePosition).magnitude;
//if(Vector2.Dot(lowLCon.standFootBaseNormal, -1*Physics.gravity.normalized) > 0.7f/* && contactPosDis < 0.5f*/){
// lowLCon.getStanceFoot().addFixedPositionJoint(lowLCon.swingFootBasePosition, 800);
//}
//lowLCon.standFootHeightTrajectory.addKnot(0, lowLCon.getLegLength());
//lowLCon.standFootHeightTrajectory.addKnot(1, lowLCon.getLegLength());
}
}