public void setLegBend(float stanceAngle, float swingAngle)
{
SimBiConState curState = lowLCon.getCurrentState();
Trajectory tmpTraj = curState.getTrajectory(1);
if (tmpTraj.getTrajectoryComponentCount() > 0)
{
tmpTraj.getTrajectoryComponent(0).offset = swingAngle;
}
tmpTraj = curState.getTrajectory(2);
if (tmpTraj.getTrajectoryComponentCount() > 0)
{
tmpTraj.getTrajectoryComponent(0).offset = stanceAngle;
}
tmpTraj = curState.getTrajectory(3);
if (tmpTraj.getTrajectoryComponentCount() > 0)
{
tmpTraj.getTrajectoryComponent(0).offset = -swingAngle;
}
tmpTraj = curState.getTrajectory(4);
if (tmpTraj.getTrajectoryComponentCount() > 0)
{
tmpTraj.getTrajectoryComponent(0).offset = -stanceAngle;
}
}
/**
* This method is used to set the current FSM state of the controller to that of the index that
* is passed in.
*/
protected void setFSMStateTo(int index)
{
if (index < 0)
{
FSMStateIndex = -1;
}
else if (index >= statesData.getStateCount())
{
FSMStateIndex = statesData.getStateCount() - 1;
}
else
{
FSMStateIndex = index;
}
if (FSMStateIndex >= 0)
{
currentState = statesData.getState(FSMStateIndex);
}
}
virtual public void setUpperBodyPose(int index, float rootLean, float torsoLean, float headLean)
{
upperBodyPos[index] = rootLean;
SimBiConState curState = lowLCon.getCurrentState();
Trajectory tmpTraj = curState.getTrajectory("root");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = rootLean;
}
tmpTraj = curState.getTrajectory("pelvis_waist");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = torsoLean;
}
tmpTraj = curState.getTrajectory("torso_head");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = headLean;
}
}
/**
* This method is used to resolve the names (map them to their index) of the joints.
*/
private void resolveJoints(SimBiConState state)
{
string tmpName;
for (int i = 0; i < state.getExternalForceCount(); i++)
{
ExternalForce ef = state.getExternalForce(i);
//deal with the SWING_XXX' case
if (ef.bName.StartsWith("SWING_"))
{
tmpName = ef.bName.Substring(6);
tmpName = tmpName.Insert(0, "r");
ef.leftStanceARBIndex = getBodyIndex(tmpName);
tmpName = tmpName.Remove(0, 1);
tmpName = tmpName.Insert(0, "l");
ef.rightStanceARBIndex = getBodyIndex(tmpName);
continue;
}
//deal with the STANCE_XXX' case
if (ef.bName.StartsWith("STANCE_"))
{
tmpName = ef.bName.Substring(7);
tmpName = tmpName.Insert(0, "l");
ef.leftStanceARBIndex = getBodyIndex(tmpName);
tmpName = tmpName.Remove(0, 1);
tmpName = tmpName.Insert(0, "r");
ef.rightStanceARBIndex = getBodyIndex(tmpName);
continue;
}
//if we get here, it means it is just the name...
ef.leftStanceARBIndex = getBodyIndex(ef.bName);
ef.rightStanceARBIndex = ef.leftStanceARBIndex;
}
for (int i = 0; i < state.getTrajectoryCount(); i++)
{
Trajectory jt = state.getTrajectory(i);
//deal with the 'root' special case
if (jt.jName == "root")
{
jt.leftStanceIndex = jt.rightStanceIndex = -1;
}
else if (jt.jName.StartsWith("SWING_"))
{
tmpName = jt.jName.Substring(6);
tmpName = tmpName.Insert(0, "r");
jt.leftStanceIndex = getJointIndex(tmpName);
tmpName = tmpName.Remove(0, 1);
tmpName = tmpName.Insert(0, "l");
jt.rightStanceIndex = getJointIndex(tmpName);
}
else if (jt.jName.StartsWith("STANCE_"))
{
tmpName = jt.jName.Substring(7);
tmpName = tmpName.Insert(0, "l");
jt.leftStanceIndex = getJointIndex(tmpName);
tmpName = tmpName.Remove(0, 1);
tmpName = tmpName.Insert(0, "r");
jt.rightStanceIndex = getJointIndex(tmpName);
}
else
{
//if we get here, it means it is just the name...
jt.leftStanceIndex = getJointIndex(jt.jName);
jt.rightStanceIndex = jt.leftStanceIndex;
}
if (jt.hitFeedback != null)
{
jt.hitFeedback.hitARBIndex = getRBIndexBySymbolicName(jt.jName, state.getStance());
}
}
}
public void addState(SimBiConState state_disown)
{
states.Add(state_disown);
resolveJoints(state_disown);
}
/**
* This method loads all the pertinent information regarding the simbicon controller from a file.
*/
public void loadFromFile(string data)
{
if (data == null)
{
return;
}
//throwError("NULL file name provided.");
StringReader f = new StringReader(data);
if (f == null)
{
return;
}
//throwError("Could not open file: %s", data);
//UnityThreadHelper.CreateThread(()=>
//{
//to be able to load multiple controllers from multiple files,
//we will use this offset to make sure that the state numbers
//mentioned in each input file are updated correctly
int stateOffset = this.states.Count;
SimBiConState tempState;
int tempStateNr = -1;
//have a temporary buffer used to read the file line by line...
string buffer = f.ReadLine();
while (buffer != null)
{
if (buffer.Length > 195)
{
break;
}
string line = buffer.Trim();
int lineType = ConUtils.getConLineType(line);
string[] nrParams = line.Split(new char[] { ' ' }, System.StringSplitOptions.RemoveEmptyEntries);
switch (lineType)
{
case ConUtils.CON_STATE_START:
tempState = new SimBiConState();
tempStateNr = int.Parse(nrParams[1]);
states.Add(tempState);
tempState.readState(f, stateOffset);
//now we have to resolve all the joint names (i.e. figure out which joints they apply to).
resolveJoints(tempState);
//UnityThreadHelper.Dispatcher.Dispatch(() => onDataLoadProcess(tempStateNr + 1));
break;
case ConUtils.CON_NOT_IMPORTANT:
break;
default:
break;
}
buffer = f.ReadLine();
}
f.Close();
onDataLoadFinished(states.Count);
//UnityThreadHelper.Dispatcher.Dispatch(() => onDataLoadFinished(states.Count));
//});
}
