int AI::betterMemory()
{
int position = 0;
int nextPos = 0;
int lastPos = 0;
bool ok = false;
Direction lastDir = AI::UP;
bool keepPosAndDir = false;
do
{
// Check if an active ship is available
if(shipActive())
{
if(!keepPosAndDir)
{
// Get information about the last turn
lastPos = _alreadyTried.last();
lastDir = lastDirection();
keepPosAndDir = false;
}
// Get next position
if(lastHit())
{
nextPos = positionInDirection(lastDir, lastPos);
}
else
{
if(_currentHitsOnShip >= 2)
{
if(!keepPosAndDir)
lastDir = oppositeDirection(lastDir);
nextPos = positionInDirection(lastDir, _shipFirstHit);
}
else
{
if(!keepPosAndDir)
lastDir = nextDirection(lastDir);
nextPos = positionInDirection(lastDir, _shipFirstHit);
}
}
// If the position is not valid, try an other one
if(nextPos < 0)
{
if(_currentHitsOnShip >= 2)
lastDir = oppositeDirection(lastDir);
else
lastDir = nextDirection(lastDir);
lastPos = _shipFirstHit;
keepPosAndDir = true;
continue;
}
// If position was already tried, try an other one
if(_alreadyTried.contains(nextPos))
{
lastDir = nextDirection(lastDir);
lastPos = _shipFirstHit;
keepPosAndDir = true;
continue;
}
else
{
ok = true;
}
// Set the next position
position = nextPos;
}
else
{
// If no active ship is available, pick random position
position = simpleMemory();
_shipSunken = false;
ok = true;
}
}while(!ok);
_alreadyTried.append(position);
setLastDirection(lastDir);
return position;
}
void urgDisplay::fillPointMeshTWPRXY(float speed_, float alignYaw_, float alignPitch_, float alignRoll_, bool bCorrectGait) {
speed = speed_;
alignYaw = alignYaw_;
alignPitch = alignPitch_;
alignRoll = alignRoll_;
pointMesh.clear();
int startIndex = 0;
int endIndex = nScans;
// starting orientation, location and time
ofVec3f lastPosition(0., 0., 0.);
float yawZero = fmod(csv.getFloat(startIndex, 1) + 360., 360.); // put in range {0, 360}
ofVec3f lastDirection(-1., 0., 0.); // oriention of the IMU is to the left (negative x axis)
float timeZero = csv.getFloat(startIndex, 0) / 1000.;
float lastTime = timeZero;
// add every scan (cross section) to the mesh
for (int i = startIndex; i < endIndex; i++) {
// get the current location of scan by projecting the last scan direction with a magnitude of the last time interval from the last scan origin
float thisTime = csv.getFloat(i, 0) / 1000.; // in seconds
float distanceTraveled = (thisTime - lastTime) * speed;
ofVec3f changeInPosition = lastDirection.getScaled(distanceTraveled);
ofVec3f thisPosition = lastPosition + changeInPosition;
// get current orientation of scan with respect to the starting orientation
float thisYaw = fmod(csv.getFloat(i, 1) + 720. - yawZero, 360.);
ofVec3f thisDirection = ofVec3f(-1., 0., 0.).rotate(-thisYaw, ofVec3f(0., 1., 0.));
// for every point in the scan, rotate it accordingly, then translate it and add it to the mesh
for (int j = minIndex + 4; j < maxIndex + 4; j++) {
// get the initial (recorded) coordinates
float px = csv.getFloat(i, 2 * j);
float py = csv.getFloat(i, 2 * j + 1);
ofVec3f recordedPosition(px, py, 0.);
// cull out coorinates that are too close to the camera
// use manhattan distance to speed up processing
// double sqDist = recordedPosition.distanceSquared(ofVec3f(0.,0.,0.));
// if (sqDist < minSqDist2Cam) continue;
// first rotate so it's perpendicular to thisDirection (align virtual lidar scanner to coorindate system of razor IMU)
ofVec3f alignedPosition = recordedPosition;
alignedPosition.rotate(-alignRoll, 0., 0.); // first roll (rotate right)
alignedPosition.rotate(0., 0., -alignPitch); // then pitch (pull back)
alignedPosition.rotate(0., -alignYaw, 0.); // finally yaw (turn clockwise)
ofVec3f orientedPosition = alignedPosition;
// optionally, correct for gait here (recorded pitch and roll)
if (bCorrectGait) {
float thisRoll = csv.getFloat(i, 3);
orientedPosition.rotate(-thisRoll, 0., 0.);
float thisPitch = csv.getFloat(i, 2);
orientedPosition.rotate(0., 0., -thisPitch);
}
// orient position to thisDirection
orientedPosition.rotate(0., -thisYaw, 0.);
// move point to actual location
ofVec3f translatedPosition = orientedPosition + thisPosition;
// add translated position to the mesh
pointMesh.addVertex(translatedPosition);
pointMesh.addColor(ofFloatColor(1., 1., 1.));
}
lastTime = thisTime;
lastDirection = thisDirection;
lastPosition = thisPosition;
}
}
