// assumes cell headings are already computed
void FovObstacleMap::computeHeadingRangeOfGridPoint(
GridSquare pt, Heading& minHeading, Heading& maxHeading)
{
// the coordinates of the four corners of this grid point
FractionalGridSquare corners[] = {
FractionalGridSquare(pt.x - 0.5, pt.y + 0.5), // top left
FractionalGridSquare(pt.x + 0.5, pt.y + 0.5), // top right
FractionalGridSquare(pt.x - 0.5, pt.y - 0.5), // bottom left
FractionalGridSquare(pt.x + 0.5, pt.y - 0.5) // bottom right
};
// all of the possible pairs of corners
// we're interested in the one with the largest heading difference between corners
// we can't just take min and max heading of the corners, because of the
// discontinuity in headings at +/-pi
int cornerPairs[6][2] = { // the length 6 is used below
{0, 1}, {0, 2}, {0, 3}, {1, 2}, {1, 3}, {2, 3}
};
int maxIndex = -1;
Heading maxHeadingDiff = -1; // valid values are never be negative
for(int i = 0; i < 6; i++) {
FractionalGridSquare c1 = corners[cornerPairs[i][0]];
FractionalGridSquare c2 = corners[cornerPairs[i][1]];
if(computeHeadingDiff(computeHeading(c1), computeHeading(c2)) > maxHeadingDiff) {
maxIndex = i;
}
}
Heading h1 = computeHeading(corners[cornerPairs[maxIndex][0]]);
Heading h2 = computeHeading(corners[cornerPairs[maxIndex][1]]);
minHeading = std::min(h1, h2);
maxHeading = std::max(h1, h2);
}
/**
this method is used to rotate the character (well, the character whose state is passed in as a parameter)
about the vertical axis, so that it's default heading has the value that is given as a parameter
*/
void Character::setHeading(Quaternion heading, ReducedCharacterStateArray* state, int start){
ReducedCharacterState chS(state, start);
Quaternion oldHeading, newHeading, qRoot;
//get the current root orientation, that contains information regarding the current heading
qRoot = chS.getOrientation();
//get the twist about the vertical axis...
oldHeading = computeHeading(qRoot);
//now we cancel the initial twist and add a new one of our own choosing
newHeading = heading * oldHeading.getComplexConjugate();
//add this component to the root.
chS.setOrientation(newHeading * qRoot);
//and also update the root velocity and angular velocity
chS.setVelocity(newHeading.rotate(chS.getVelocity()));
chS.setAngularVelocity(newHeading.rotate(chS.getAngularVelocity()));
}
FovObstacleMap::GridSquareState FovObstacleMap::getState(GridSquare pt) const
{
if(!inFieldOfView(pt)) { return UNKNOWN; }
const CellData& cellData = map.cell(pt.x, pt.y);
if(cellData.isKnownObstacle) {
return OBSTACLE;
}
// if this is not an obstacle, it's considered free if there are no obstacles
// between this and the robot (it's not occluded)
else if(computeDistance(pt) < closestObstacleByHeading.cell(computeHeading(pt))) {
return FREE;
}
else {
return UNKNOWN;
}
}
/**
this method is used to return the current heading of the character
*/
Quaternion Character::getHeading(){
//get the current root orientation, that contains information regarding the current heading and retrieve the twist about the vertical axis
return computeHeading(root->getOrientation());
}
bool FovObstacleMap::inFieldOfView(GridSquare pt) const
{
FractionalCellCount distance = computeDistance(pt);
return distance < maxRange && distance > minRange
&& computeHeadingDiff(computeHeading(pt), robotHeading) < fov/2;
}
