/**
* This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
* phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
* the center of mass.
*/
override public float getFeedbackContribution(SimBiController con, CWJoint j, float phi, Vector3 d, Vector3 v)
{
Vector3 axis = Vector3.zero;
if (MathLib.getAxisIndex(feedbackProjectionAxis) == 0)
{
axis = con.getCharacter().getLocalLeftAxis();
}
else if (MathLib.getAxisIndex(feedbackProjectionAxis) == 1)
{
axis = con.getCharacter().getLocalUpAxis();
}
else if (MathLib.getAxisIndex(feedbackProjectionAxis) == 2)
{
axis = con.getCharacter().getLocalFrontAxis();
}
float dToUse = Vector3.Dot(d, axis);
if (reversedCD && dToUse < 0)
{
dToUse = -dToUse;
}
float vToUse = Vector3.Dot(v, axis);
if (reversedCV && vToUse < 0)
{
vToUse = -vToUse;
}
if (dToUse < dMin)
{
dToUse = dMin;
}
if (vToUse < vMin)
{
vToUse = vMin;
}
if (dToUse > dMax)
{
dToUse = dMax;
}
if (vToUse > vMax)
{
vToUse = vMax;
}
return(dToUse * cd + vToUse * cv);
}
/**
* This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
* phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
* the center of mass.
*/
override public float getFeedbackContribution(SimBiController con, CWJoint j, float phi, Vector3 d, Vector3 v)
{
CWRigidBody theBody;
//Vector3 tmpP;
float offset = 0;
if (j != null)
{
theBody = j.getChild();
}
else
{
theBody = con.getCharacter().getRoot();
}
//we might want to initialize it (or add to it) some default value in case we are trying to control its projection
offset = 0;
float dToUse = Vector3.Dot(MathLib.getComplexConjugate(theBody.getOrientation()) * con.getDoubleStanceCOMError(), feedbackProjectionAxis);
if (dToUse < dMin)
{
dToUse = dMin;
}
if (dToUse > dMax)
{
dToUse = dMax;
}
float vToUse = Vector3.Dot(MathLib.getComplexConjugate(theBody.getOrientation()) * con.getComVelocity(), feedbackProjectionAxis);
if (vToUse < vMin)
{
vToUse = vMin;
}
if (vToUse > vMax)
{
vToUse = vMax;
}
float err = (dToUse * cd - vToUse * cv + offset);
float result = err * totalMultiplier;
//if (j)
// tprintf("%s gets: %lf\n", j->getName(), result);
// else
//tprintf("root gets: %lf\n", result);
if (result < minFeedbackValue)
{
result = minFeedbackValue;
}
if (result > maxFeedbackValue)
{
result = maxFeedbackValue;
}
// if (j == NULL && cv > 0.4)
// logPrint("%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", theBody->getLocalCoordinates(con->COMOffset).dotProductWith(feedbackProjectionAxis), dToUse, theBody->getLocalCoordinates(con->comVelocity).dotProductWith(feedbackProjectionAxis), vToUse, result, forceRatioWeight);
return(result);
}