//
// LinkedList<Vector> getRelationships(c)
// Last modified: 28Aug2006
//
// Calculates the intersections of the set of functions of this formation
// and a circle centered at the parameterized vector position c with
// the appropriate radius, returning a list of these vectors.
//
// Returns: list of the desired relationship vectors
// Parameters:
// c in the position to be centered at
//
LinkedList<Vector> Formation::getRelationships(const Vector c)
{
if (isEmpty()) return LinkedList<Vector>();
LinkedList<Vector> rels;
Function curr = NULL;
for (GLint i = 0; i < getSize(); ++i)
{
if (!getHead(curr)) break;
rels.insertTail(getRelationship(curr, -radius, c, heading));
rels.insertTail(getRelationship(curr, radius, c, heading));
++(*this);
}
return rels;
} // getRelationships(const Vector)
//
// Vector getRelationship(pos, r, c, theta)
// Last modified: 28Aug2006
//
// Calculates the intersection of the function at the parameterized position
// and a circle centered at the parameterized vector position c with
// the appropriate radius, returning a vector from c to this intersection.
//
// Returns: vector from the parameterized vector position c
// to the intersection of the function and appropriate circle
// Parameters:
// pos in the position of the desired function (default 0)
// r in the radius of the intersecting circle
// c in the position to be centered at
// theta in the rotation of the relationship (default 0)
//
Vector Formation::getRelationship(const GLint pos,
const GLfloat r,
const Vector c,
const GLfloat theta)
{
return getRelationship(getFunction(pos), r, c, theta);
} // getRelationship(const GLint, const GLfloat, const Vector, const GLfloat)
//
// 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()
//
// GLfloat getAngle(toID)
// Last modified: 03Sep2006
//
// Returns the angle from this robot
// to the robot with the parameterized ID.
//
// Returns: the angle from this robot to another robot
// Parameters:
// toID in the ID of the robot being related to
//
GLfloat Robot::getAngleTo(const GLint toID) const
{
return getRelationship(toID).angle();
} // getAngleTo(const GLint) const
//
// GLfloat getDistance(toID)
// Last modified: 07Nov2009
//
// Returns the distance from this robot
// to the robot with the parameterized ID.
//
// Returns: the distance from this robot to another robot
// Parameters:
// toID in the ID of the robot being related to
//
GLfloat Robot::getDistanceTo(const GLint toID) const
{
return getRelationship(toID).magnitude();
} // getDistanceTo(const GLint) const
//
// GLfloat getAngleTo(target)
// Last modified: 06Mar2007
//
// Returns the angle from this robot
// to the parameterized target vector.
//
// Returns: the angle from this robot to the target vector
// Parameters:
// target in/out the target vector being related to
//
GLfloat Robot::getAngleTo(Vector &target) const
{
return getRelationship(target).angle();
} // getAngleTo(Vector &) const
//
// GLfloat getDistanceTo(target)
// Last modified: 07Nov2009
//
// Returns the distance from this robot
// to the parameterized target vector.
//
// Returns: the distance from this robot to the target vector
// Parameters:
// target in/out the target vector being related to
//
GLfloat Robot::getDistanceTo(Vector &target) const
{
return getRelationship(target).magnitude();
} // getDistanceTo(Vector &) const
