//--------------------------------------HELPER METHODS-------------------------------------------//
void facePos(float target[3], float myPos[3]){
//Rotate to target
float attTarget[3];
mathVecSubtract(attTarget,target,myPos,3);
mathVecNormalize(attTarget,3);
api.setAttitudeTarget(attTarget);
if (distanceVec(myPos, target)< 0.50)
{
//DEBUG(("The SPHERE is close enough to take a picture. ")); //
if (game.alignLine(targetPOI)==true)
{
if (timeSinceArrival > 4) //5 seconds between pictures, 5 seconds after arriving at POI
{
DEBUG(("\n Pictures: %d", picturesTaken+1)); //
game.takePic(targetPOI);
picturesTaken++;
timeSinceArrival = 0;
}
timeSinceArrival++;
}
}
//return distance(attTarget, myPos);
}
void DistanceSensor::drawObject(const VisualizationParameterSet& visParams)
{
if(visParams.visualizeSensors && !visParams.drawForSensor)
{
glColor3d(visualizationColor.v[0],
visualizationColor.v[1],
visualizationColor.v[2]);
Vector3d camPos(position);
if(projection == SPHERICAL_PROJECTION)
{
glBegin(GL_LINES);
for(unsigned int i=0; i< sensorDataSize; i++)
{
Vector3d distanceVec(sensorReading.data.doubleArray[i],0,0);
double maxAngle(angleX);
double zRotation(maxAngle/(double)resolutionX);
zRotation *= (double)i;
zRotation -= maxAngle/2.0;
distanceVec.rotateZ(zRotation);
distanceVec.rotate(rotation);
distanceVec+=position;
glVertex3d (camPos.v[0], camPos.v[1], camPos.v[2]);
glVertex3d (distanceVec.v[0], distanceVec.v[1], distanceVec.v[2]);
}
glEnd();
}
else
{
Vector3d distanceVec(sensorReading.data.doubleArray[0],0,0);
distanceVec.rotate(rotation);
distanceVec+=position;
//Draw distance vector
glBegin(GL_LINES);
glVertex3d (camPos.v[0], camPos.v[1], camPos.v[2]);
glVertex3d (distanceVec.v[0], distanceVec.v[1], distanceVec.v[2]);
glEnd();
}
}
}
void updateScene(double currentTime, double deltaTime)
{
float speed = user_avatar->getSpeed(); // speed of user_avatar
float orientation = user_avatar->getOrientation();
SbVec3f originalPosition = user_avatar->getPosition(); // save current user_avatar->position
// UP KEY
// user_avatar->speed: acceleration
if(keyboard[KeyRec::UP].isDown)
{
if(speed < maxSpeed)
{
speed += (float)deltaTime*speedUp;
if(speed>maxSpeed)
speed = maxSpeed;
}
}else
{
if(speed > 0)
{
speed -= (float)deltaTime*slowDown;
if(speed<0)
speed = 0;
}
}
// KEY DOWN
// user_avatar->speed: braking
if(keyboard[KeyRec::DOWN].isDown)
{
if(speed > minSpeed)
{
speed -= (float)deltaTime*speedUp;
if(speed < minSpeed)
speed = minSpeed;
}
}else
{
if(speed < 0)
{
speed += (float)deltaTime*slowDown;
if(speed > 0)
speed = 0;
}
}
// KEY LEFT
// rotation speed: turn left
if(keyboard[KeyRec::LEFT].isDown)
{
user_avatar->rotationSpeed += (float)deltaTime*rotSpeedUp;
if(user_avatar->rotationSpeed > maxRotSpeed)
user_avatar->rotationSpeed = maxRotSpeed;
}else
{
if(user_avatar->rotationSpeed > 0)
{
user_avatar->rotationSpeed -= (float)deltaTime*rotSlowDown;
if(user_avatar->rotationSpeed < 0)
user_avatar->rotationSpeed = 0;
}
}
// KEY RIGHT
// rotation speed: turn right
if(keyboard[KeyRec::RIGHT].isDown)
{
user_avatar->rotationSpeed -= (float)deltaTime*rotSpeedUp;
if(user_avatar->rotationSpeed < minRotSpeed)
user_avatar->rotationSpeed = minRotSpeed;
}else
{
if(user_avatar->rotationSpeed < 0)
{
user_avatar->rotationSpeed += (float)deltaTime*rotSlowDown;
if(user_avatar->rotationSpeed > 0)
user_avatar->rotationSpeed = 0;
}
}
// user_avatar->orientation
// note: negative values must be used when going back
if(speed >= 0)
orientation += (float)deltaTime*user_avatar->rotationSpeed;
else
orientation -= (float)deltaTime*user_avatar->rotationSpeed;
// set new user_avatar->orientation
user_avatar->setOrientation(orientation);
// set new user_avatar->position
float distance = speed*(float)deltaTime;
SbVec3f pos = user_avatar->getPosition();
SbVec3f distanceVec(distance*sinf(orientation), 0.f, distance*cosf(orientation));
pos -= distanceVec;
user_avatar->setPosition(pos);
STOP_AT_BORDER( pos , speed );
user_avatar->direction = user_avatar->getPosition() - originalPosition;
BCAST_POSITION( deltaTime );
if(!gui::free_camera)
{
// 3rd person camera
float tilt = -.20f; // camera tilt (in radians)
float cameraDistance = 15; // initial camera distance from the user_avatar
float cameraHeight = 8.0f; // camera height from the user_avatar
SbRotation cameraOrientation, cameraTilt;
cameraOrientation.setValue(SbVec3f(0, 1, 0), orientation);
cameraTilt.setValue(SbVec3f(1, 0, 0), tilt);
camera->orientation = cameraTilt*cameraOrientation;
if(speed < 0)
{
cameraDistance = 18.0f-speed*0.44f; // camera distance increases when going back
}
SbVec3f cameraPosition(user_avatar->getPosition());
cameraPosition += SbVec3f(cameraDistance*sinf(orientation),
cameraHeight, cameraDistance*cosf(orientation));
camera->position.setValue(cameraPosition);
}
}
void ViBeBGModule::segmentation()
{
int numSampleInRadius, sample;
float distance, maxFloat = 30000000;
double featureVal, maxDist;
std::vector < float> distanceVec(this->numSamples);
this->foreground = cv::Mat(height, width, CV_8UC1, cv::Scalar(FOREGROUND));
for(int row = 0; row < height; row++)
{
for(int col = 0; col < width; col++)
{
std::fill(distanceVec.begin(), distanceVec.end(), maxFloat);
numSampleInRadius = 0;
for( sample = 0; sample < this->numSamples; sample++)
{
distance = cv::norm( currImgC3.at<cv::Vec3b>(row,col) - this->samples[sample].at<cv::Vec3b>
(row,col) );
distanceVec[sample] = distance;
if(distance < this->radius)
{
numSampleInRadius++;
if(numSampleInRadius == this->minSample)
{
this->foreground.at<uchar>(row,col) = BACKGROUND;
}
if(numSampleInRadius == minSampleForRealiability )
break;
}
}
//maxmin strategy to get the realiability of the pixel classification
std::sort(distanceVec.begin(), distanceVec.end());
featureVal = distanceVec[this->minSample -1];
if(sample == this->numSamples)
maxDist = distanceVec[sample -1];
else
maxDist = distanceVec[sample];
// if(/*movingPixel*/true)//TODO:
// {
this->featureMap.at<uchar> (row,col) = reliabilityBg.normalizeScale(featureVal, minScale, maxScale);
this->reliabilityNormalized.at<uchar> (row,col) = reliabilityBg.normalizeBothSideScale
// (0, this->radius, minScale, maxScale);
(featureVal, this->radius, minScale, maxScale);
/*
}
else
{
this->featureMap.at<uchar> (row,col) = reliabilityBg.normalizeScale(maxDistminScale,maxScale);
this->reliabilityNormalized.at<uchar> (row,col) = reliabilityBg.normalizeBothSideScale
(maxDist, this->radius, minScale, maxScale);
}
*/
}
}
}
void loop()
{
DEBUG(("\n# of Captured POIs: %d", countPOI));
//DEBUG(("\nPOIs Visited 1: %d", pastPOIs[0]));
//DEBUG(("\nPOIs Visited 2: %d", pastPOIs[1]));
//DEBUG(("\nPOIs Visited 3: %d", pastPOIs[2]));
DEBUG(("\n targetPOI: %d", targetPOI));
//----Base Functions----//
time = api.getTime();
stayInBounds();
api.getMyZRState(myState);
for (int i = 0; i<3; i++) {
myPos[i] = myState[i];
}
if (game.getFuelRemaining() <= 0)
{
game.turnOff();
//DEBUG(("\nGame over man, game over!"));
}
/*else if (game.getNextFlare() != -1 && game.getNextFlare() < 5)
{
game.turnOff();
DEBUG(("Flare incoming"));
}
else
{
game.turnOn();
}*/
/*while (game.getScore() > 14)
{
api.setPositionTarget(safetyZone);
}*/
if (0.9*game.getScore() > game.getOtherScore())
{
strategyType = 1;
}
else
{
if (time > 200)
{
if (game.getScore() < game.getOtherScore())
{
strategyType = 1;
}
else
{
strategyType = 0;
}
}
else
{
strategyType = 0;
}
}
if (time > 16)
{
if (strategyType == 0) {
//----TargetPOI----//
if (time == 60 || time % 60 == 0)
{
DEBUG(("\n POIs Updated..."));
pastPOIs[0] = -1;
pastPOIs[1] = -1;
pastPOIs[2] = -1;
countPOI = 0;
targetPOI = findClosestPOI(myPos, pastPOIs);
DEBUG(("\ntargetPOI: %d", targetPOI));
pastPOIs[countPOI] = targetPOI;
countPOI++;
picturesTaken = 0;
}
/*if (targetPOI > 2 || countPOI >= 3)
{
pastPOIs[0] = -1;
pastPOIs[1] = -1;
pastPOIs[2] = -1;
countPOI = 0;
targetPOI = findClosestPOI(myPos, pastPOIs);
pastPOIs[countPOI] = targetPOI;
countPOI++;
}*/
game.getPOILoc(poiPos, targetPOI);
for (int i = 0; i<3; i++)
{ //Reset POI vector to half its original magnitude
poiPos[i] *= 0.5;
}
float posTarget[3] = {poiPos[0]*3.9, poiPos[1]*3.9, poiPos[2]*3.9};
//Find coordinates for SPHERE to be at while taking pictures
float uploadTarget[3] = {poiPos[0]*7.25, poiPos[1]*7.25, poiPos[2]*7.25};
//Find coordinates for SPHERE to be at while uploading
if (picturesTaken > 3)// && time > 60)
{
//If SPHERE has attempted more than 3 pictures (valid or not), move to next POI
if (countPOI == 3)
{
DEBUG(("\n All POIs captured"));
pastPOIs[0] = -1;
pastPOIs[1] = -1;
pastPOIs[2] = -1;
countPOI = 0;
targetPOI = findClosestPOI(myPos, pastPOIs);
pastPOIs[countPOI] = targetPOI;
countPOI++;
picturesTaken = 0;
DEBUG(("\n Moving on~~"));
DEBUG(("\n New targetPOI: %d", targetPOI));
}
else
{
targetPOI = findClosestPOI(myPos, pastPOIs);
pastPOIs[countPOI] = targetPOI;
countPOI++;
picturesTaken = 0;
DEBUG(("\n Moving on~~"));
DEBUG(("\n targetPOI: %d", targetPOI));
}
}
else {
//Otherwise, continue taking pictures
facePos(poiPos, myPos);
}
//----Collision Avoidance & Positioning----//
//float midpoint[3] = {(myPos[0]+posTarget[0])/2, (myPos[1]+posTarget[1])/2, (myPos[2]+posTarget[2])/2};
//float qtl1[3] = {(myPos[0]+midpoint[0])/2, (myPos[1]+midpoint[1])/2, (myPos[2]+midpoint[2])/2};
//float qtl3[3] = {(posTarget[0]+midpoint[0])/2, (posTarget[1]+midpoint[1])/2, (posTarget[2]+midpoint[2])/2};
arcMove(posTarget);
/* if (mathVecMagnitude(midpoint,3) < 0.35) {
// this part is really rough - with some tweaking of magnitudes and such, it might work better.
arcMove(midpoint, posTarget);
}
else { //Proceed to target coordinates as usual
/*if(mathVecMagnitude(qtl3, 3)>0.35)
{
if (mathVecMagnitude(qtl1,3)>0.35)
{
api.setPositionTarget(posTarget);
}
else
{
arcMove(midpoint, posTarget);
}
}
else
{
arcMove(midpoint, posTarget);
}
*/
// }
if (game.getMemoryFilled()>0 && game.getScore() < targetScore) { //If SPHERE has a valid picture
DEBUG(("\n Moving to upload"));
float attTarget[3];
attTarget[0] = 0.64 -myPos[0]; // [0.64,0,0] is the position of Earth
attTarget[1] = 0 -myPos[1];
attTarget[2] = 0 -myPos[2];
mathVecNormalize(attTarget,3);
api.setAttitudeTarget(attTarget);
api.setPositionTarget(uploadTarget); //Move to upload position
if (distanceVec(myPos, zero)> 0.53 && distanceVec(myPos,attTarget)> 0.05) //When SPHERE is out of both orbits, upload
{
float oldScore = game.getScore();
game.uploadPic();
if (oldScore < game.getScore())
{
//DEBUG(("Upload was successful!"));
}
if (oldScore >= game.getScore())
{
//DEBUG(("Upload failed!"));
}
}
}
}
else if (strategyType == 1) {
api.getOtherZRState(otherState);
for (int i = 0; i<3; i++) {
otherPos[i] = 0.5*otherState[i];
}
if (distanceVec(myPos,otherPos) < 0.38) {
arcMove(otherPos);
DEBUG((" | Heading to otherPos | "));
}
else {
arcMove(otherState);
DEBUG((" | Heading to otherState | "));
}
}
}
else
{
float initialShotTarget[3] = {-0.2, 0.0, -0.35};
api.setPositionTarget(initialShotTarget);
}
}
int findClosestPOI(float myPos[3],int pastPOI[3])
{
float poi0[3];
float poi1[3];
float poi2[3];
game.getPOILoc(poi0, 0);
float aDistance = distanceVec(poi0, myPos);
game.getPOILoc(poi1, 1);
float bDistance = distanceVec(poi1, myPos);
game.getPOILoc(poi2, 2);
float cDistance = distanceVec(poi2, myPos);
switch(pastPOI[0])
{
case 0: aDistance = 1000;
break;
case 1: bDistance = 1000;
break;
case 2: cDistance = 1000;
break;
default:
break;
}
switch(pastPOI[1])
{
case 0: aDistance = 1000;
break;
case 1: bDistance = 1000;
break;
case 2: cDistance = 1000;
break;
default:
break;
}
switch(pastPOI[2])
{
case 0: aDistance = 1000;
break;
case 1: bDistance = 1000;
break;
case 2: cDistance = 1000;
break;
default:
break;
}
if (findMin(aDistance, findMin(bDistance, cDistance)) == aDistance)
{
return 0;
}
else if (findMin(aDistance, findMin(bDistance, cDistance)) == bDistance)
{
return 1;
}
else if (findMin(aDistance, findMin(bDistance, cDistance)) == cDistance)
{
return 2;
}
else
{
DEBUG(("\n Targeting error has occurred :("));
}
}
