public void Add_External_Force(ExternalForce force)
{
m_Forces.Add(force);
}
/**
* 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 evaluateJointTargets()
{
ReducedCharacterState poseRS = new ReducedCharacterState(desiredPose);
//d and v are specified in the rotation (heading) invariant coordinate frame
updateDAndV();
//there are two stages here. First we will compute the pose (i.e. relative orientations), using the joint trajectories for the current state
//and then we will compute the PD torques that are needed to drive the links towards their orientations - here the special case of the
//swing and stance hips will need to be considered
//always start from a neutral desired pose, and build from there...
root.setExternalForce(Vector3.zero);
root.setExternalTorque(Vector3.zero);
CWJoint j;
for (int i = 0; i < jointCount; i++)
{
j = character.getJoint(i);
j.getChild().setExternalForce(Vector3.zero);
j.getChild().setExternalTorque(Vector3.zero);
poseRS.setJointRelativeOrientation(Quaternion.identity, i);
poseRS.setJointRelativeAngVelocity(Vector3.zero, i);
}
float phiToUse = phi;
//make sure that we only evaluate trajectories for phi values between 0 and 1
if (phiToUse > 1)
{
phiToUse = 1;
}
Vector3 force = Vector3.zero;
Vector3 torque = Vector3.zero;
// First compute external forces
for (int i = 0; i < currentState.getExternalForceCount(); i++)
{
ExternalForce ef = currentState.getExternalForce(i);
CWRigidBody arb = character.getArticulatedRigidBody(ef.getARBIndex(stance));
force = Vector3.zero;
force += character.getLocalFrontAxis() * ef.forceX.evaluate_catmull_rom(phiToUse);
force += character.getLocalUpAxis() * ef.forceY.evaluate_catmull_rom(phiToUse);
force += character.getLocalLeftAxis() * ef.forceZ.evaluate_catmull_rom(phiToUse);
force = characterFrame * force;
arb.addExternalForce(force);
torque = Vector3.zero;
torque += character.getLocalFrontAxis() * ef.torqueX.evaluate_catmull_rom(phiToUse);
torque += character.getLocalUpAxis() * ef.torqueY.evaluate_catmull_rom(phiToUse);
torque += character.getLocalLeftAxis() * ef.torqueZ.evaluate_catmull_rom(phiToUse);
torque = characterFrame * torque;
arb.addExternalTorque(torque);
}
//Debug.Log(Random.value + "ffffffffffff "+curState.getTrajectoryCount());
for (int i = 0; i < currentState.getTrajectoryCount(); i++)
{
Trajectory tra = currentState.getTrajectory(i);
//now we have the desired rotation angle and axis, so we need to see which joint this is intended for
int jIndex = tra.getJointIndex(stance, isLeftReverseStance);
//Debug.Log(Random.value + "00000000000 "+(curState.sTraj[i] as Trajectory).getJointName());
j = character.getJoint(jIndex);
//get the desired joint orientation to track - include the feedback if necessary/applicable
Vector3 d0 = new Vector3();
Vector3 v0 = new Vector3();
computeD0(phiToUse, ref d0);
computeV0(phiToUse, ref v0);
//if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error
if (jIndex == -1)
{
qRootD = tra.evaluateTrajectory(this, j, stance, phiToUse, d - d0, v - v0);
}
else
{
Quaternion newOrientation = tra.evaluateTrajectory(this, j, stance, phiToUse, d - d0, v - v0);
j.setRelToRootFrame(tra.relToCharFrame);
poseRS.setJointRelativeOrientation(newOrientation, jIndex);
}
if (tra.hitFeedback != null)
{
CWRigidBody arb = character.getArticulatedRigidBody(tra.hitFeedback.hitARBIndex);
if (phiToUse > tra.hitFeedback.minTime && arb.contactPoints.Count > 0)
{
ContactPointInfo cp = (ContactPointInfo)arb.contactPoints[0];
onHitOtherSBCharacter(tra.hitFeedback, cp);
}
}
}
}
