private static bool isCurrentTheGoal(double[,] stateVars, userDefinedGoals udg)
{
double xP, y, z, theta1, theta2, theta3;
int gearcount = stateVars.GetLength(0) - 1;
xP = stateVars[gearcount, 3] - udg.outputLocation[0, 3];
y = stateVars[gearcount, 4] - udg.outputLocation[1, 3];
z = stateVars[gearcount, 5] - udg.outputLocation[2, 3];
theta1 = Math.Abs(stateVars[gearcount, 6]) - Math.Abs(udg.outputLocation[0, 2]);
theta2 = Math.Abs(stateVars[gearcount, 7]) - Math.Abs(udg.outputLocation[1, 2]);
theta3 = Math.Abs(stateVars[gearcount, 8]) - Math.Abs(udg.outputLocation[2, 2]);
double hVal = Math.Sqrt(xP * xP + y * y + z * z + theta1 * theta1 + theta2 * theta2 + theta3 * theta3);
if (hVal < .05)
{
return(true);
}
else
{
return(false);
}
}
// constructor
public GearEvaluator(userDefinedGoals udg)
{
this.udg = udg;
gearFamilies = GearFamily.loadDefaults();
}
private static bool isCurrentTheGoal(double[,] stateVars, userDefinedGoals udg)
{
double xP, y, z, theta1, theta2, theta3;
int gearcount = stateVars.GetLength(0)-1;
xP = stateVars[gearcount, 3] - udg.outputLocation[0, 3];
y = stateVars[gearcount, 4] - udg.outputLocation[1, 3];
z = stateVars[gearcount, 5] - udg.outputLocation[2, 3];
theta1 = Math.Abs(stateVars[gearcount, 6]) - Math.Abs(udg.outputLocation[0, 2]);
theta2 = Math.Abs(stateVars[gearcount, 7]) - Math.Abs(udg.outputLocation[1, 2]);
theta3 = Math.Abs(stateVars[gearcount, 8]) - Math.Abs(udg.outputLocation[2, 2]);
double hVal = Math.Sqrt(xP * xP + y * y + z * z + theta1 * theta1 + theta2 * theta2 + theta3 * theta3);
if (hVal < .05)
{
return true;
}
else
{
return false;
}
}
// constructor
public GearEvaluator(userDefinedGoals udg)
{
this.udg = udg;
gearFamilies = GearFamily.loadDefaults();
}
