//
// void updateState()
// Last modified: 27Aug2006
//
// Updates the state of the cell based upon the
// current states of the neighbors of the cell.
//
// Returns: <none>
// Parameters: <none>
//
void Cell::updateState()
{
Neighbor currNbr;
for (GLint i = 0; i < getNNbrs(); ++i)
{
if (!getHead(currNbr)) break;
// change formation if a neighbor has changed formation
if (getNbr(0)->formation.getFormationID() > formation.getFormationID())
changeFormation(getNbr(0)->formation, *getNbr(0));
getNbr(0)->relActual = getRelationship(currNbr.ID);
++(*this);
}
rels = getRelationships();
if(rels.getSize())
{
// reference the neighbor with the smallest gradient
// to establish correct position in formation
Neighbor *refNbr = nbrWithMinGradient();
Relationship *nbrRel = relWithID(refNbr->rels, ID);
if ((formation.getSeedID() != ID) && (refNbr != NULL) && (nbrRel != NULL))
{
// error (state) is based upon the accumulated error in the formation
nbrRel->relDesired.rotateRelative(-refNbr->rotError);
GLfloat theta = scaleDegrees(nbrRel->relActual.angle() -
(-refNbr->relActual).angle());
rotError = scaleDegrees(theta + refNbr->rotError);
transError = nbrRel->relDesired - nbrRel->relActual +
refNbr->transError;
transError.rotateRelative(-theta);
if (transError.norm() > threshold()) moveArc(transError);
else
if (abs(rotError) > angThreshold())
moveArc(0.0, degreesToRadians(-rotError));
/*if (abs(scaleDegrees(refNbr->relActual.angle() -
refNbr->relDesired.angle())) > angThreshold())
orientTo(refNbr->relActual, refNbr->relDesired.angle());*/
else moveStop();
}
else moveStop();
}
} // updateState()
//
// Behavior orientToBehavior(target, theta)
// Last modified: 03Sep2006
//
// Rotates the robot to the parameterized heading
// relative to the paratermized target,
// returning the appropriate robot behavior.
//
// Returns: the appropriate robot behavior
// Parameters:
// target in/out the target to orient to
// theta in the heading to maintain to the target
//
Behavior Robot::orientToBehavior(const Vector &target, const GLfloat theta)
{
GLfloat delta = scaleDegrees(target.angle() - theta);
if (abs(delta) <= angThreshold()) return moveStopBehavior();
return moveArcBehavior(0.0f, degreesToRadians(delta));
} // orientToBehavior(const Vector &, const GLfloat)
