bool Map::canIBlockIt(int sizeX, int sizeY, int i, int j)
{
if (((_map[v(sizeX, i)][v(sizeY, j)] != Tile::CHARACTER)
&& (_map[v(sizeX, i)][v(sizeY, j)] != Tile::WALL)
&& (_map[v(sizeX, i)][v(sizeY, j)] != Tile::INTEL))
&& (isNear(sizeX, sizeY, i, j, Tile::CHARACTER, 1))
&& (isNear(sizeX, sizeY, i, j, Tile::INTEL, 1))
&& (isNear(sizeX, sizeY, i, j, Tile::CHARACTER, 2))
&& (isNear(sizeX, sizeY, i, j, Tile::INTEL, 2)))
return (true);
return false;
}
bool Selection::isCloseToCorner(const QPointF& pt, corner_t corner) const {
switch(corner) {
case LEFT_TOP:
return isNear(borderDistance(pt, Selection::LEFT), borderDistance(pt, Selection::TOP));
case LEFT_BOTTOM:
return isNear(borderDistance(pt, Selection::LEFT), borderDistance(pt, Selection::BOTTOM));
case RIGHT_TOP:
return isNear(borderDistance(pt, Selection::RIGHT), borderDistance(pt, Selection::TOP));
case RIGHT_BOTTOM:
return isNear(borderDistance(pt, Selection::RIGHT), borderDistance(pt, Selection::BOTTOM));
default: // should never be here
return false;
}
}
void printPrompt(char (*dungeon)[DUNGEON_X][DUNGEON_Y], uint8_t awake, uint8_t playerPos[2],
uint8_t trapPos[2][2], uint8_t monsterPos[2], uint8_t treasurePos[2]){//trapPos useful if some kind of detector is added
uint8_t canSeeMonster = isNear(playerPos, monsterPos) || awake;
uint8_t canSeeTreasure = isNear(playerPos, treasurePos);
uint8_t canSeeTrap = 0;//Going to implement it anyway just in case I ever want trap detectors
printf("Map key: ");
if (canSeeMonster) printf("%c : The monster | ", MONSTER_CHAR);
if (canSeeTrap) printf("%c : A trap | ", TRAP_CHAR);
if (canSeeTreasure) printf("%c : The treasure | ", TREASURE_CHAR);
printf("%c : A wall | %c : The floor | %c : You\n", WALL_CHAR, FLOOR_CHAR, PLAYER_CHAR);
puts("Controls: w : Go up | a : Go left | s : Go down | d : Go right");
}
void MainWindow::play(int row1, int col1 , int row2 , int col2)
{
if(isNear(row1,col1,row2,col2))
{
if(gobj.arr[row1][col1] != gobj.arr[row2][col2])
{
gobj.swap(gobj.arr[row1][col1],gobj.arr[row2][col2]);
if(gobj.arr[row1][col1]<7 && gobj.arr[row2][col2]<7)
{
if(!(gobj.checkLine(row1,col1)||gobj.checkLine(row2,col2)))
gobj.swap(gobj.arr[row1][col1],gobj.arr[row2][col2]);
}
else if (gobj.arr[row1][col1] > 6 && gobj.arr[row2][col2]<7)
{
gobj.checkLine(row2,col2);
gobj.typeBomb(gobj.arr[row1][col1],row1,col1,gobj.arr[row2][col2]);
}
else if (gobj.arr[row2][col2] > 6 && gobj.arr[row1][col1]<7)
{
gobj.checkLine(row1,col1);
gobj.typeBomb(gobj.arr[row2][col2],row2,col2,gobj.arr[row1][col1]);
}
gobj.moveDown();
resetImage();
step++;
ui->lcdNumber->display(gobj.score);
ui->lcdNumber_2->display(step);
checkWin();
}
}
}
void drawDungeon(char (*dungeon)[DUNGEON_X][DUNGEON_Y], uint8_t playerPos[2], uint8_t trapPos[2][2],
uint8_t monsterPos[2], uint8_t awake, uint8_t treasurePos[2]){
for(uint8_t iy = 0; iy < DUNGEON_Y; iy++){//Has to start with y because it's not possible to go back up after a \n
for(uint8_t ix = 0; ix < DUNGEON_X; ix++){
uint8_t currPos[2] = {ix, iy};//Because memcmp likes arrays but dislikes their literals
uint8_t isPlayerNearMonster = isNear(playerPos, monsterPos);
uint8_t isCurrPosMonsterPos = !memcmp(monsterPos, currPos, 2);//! because of memcmp's return style. Checks the first 2 bytes of array element
/* Lines can be uncommented for debugging
printf("isPlayerNearMonster returned: %u\n", isPlayerNearMonster);
printf("isCurrPosMonsterPos: %u\n", isCurrPosMonsterPos);
printf("awake: %u\n", awake);
*/
if(isCurrPosMonsterPos && (!awake) && !isPlayerNearMonster){//Bug here fixed - caused by wrong awake at compile time
/*If the current observed position is the monster's position AND the monster hasn't been
*seen AND the player isn't adjacent to the monster
*/
printf(" %c ", FLOOR_CHAR);//then put a floor character there,
}
else{//Otherwise, just print the character at the position
printf(" %c ", (*dungeon)[ix][iy]);
}
}
puts("\n");
}
}
/**
* Test whether command line contains one of the generic actions
*/
bool Parser_v3d::isGenericVerb(object_t *obj, char *comment) {
debugC(1, kDebugParser, "isGenericVerb(object_t *obj, %s)", comment);
if (!obj->genericCmd)
return false;
// Following is equivalent to switch, but couldn't do one
if (isWordPresent(_vm->_arrayVerbs[_vm->_look]) && isNear(obj, _vm->_arrayVerbs[_vm->_look][0], comment)) {
// Test state-dependent look before general look
if ((obj->genericCmd & LOOK_S) == LOOK_S) {
Utils::Box(BOX_ANY, "%s", _vm->_textData[obj->stateDataIndex[obj->state]]);
} else {
if ((LOOK & obj->genericCmd) == LOOK) {
if (obj->dataIndex != 0)
Utils::Box(BOX_ANY, "%s", _vm->_textData[obj->dataIndex]);
else
return false;
} else {
Utils::Box(BOX_ANY, "%s", _vm->_textParser[kTBUnusual]);
}
}
} else if (isWordPresent(_vm->_arrayVerbs[_vm->_take]) && isNear(obj, _vm->_arrayVerbs[_vm->_take][0], comment)) {
if (obj->carriedFl)
Utils::Box(BOX_ANY, "%s", _vm->_textParser[kTBHave]);
else if ((TAKE & obj->genericCmd) == TAKE)
takeObject(obj);
else if (obj->cmdIndex) // No comment if possible commands
return false;
else if (!obj->verbOnlyFl && (TAKE & obj->genericCmd) == TAKE) // Make sure not taking object in context!
Utils::Box(BOX_ANY, "%s", _vm->_textParser[kTBNoUse]);
else
return false;
} else if (isWordPresent(_vm->_arrayVerbs[_vm->_drop])) {
if (!obj->carriedFl && ((DROP & obj->genericCmd) == DROP))
Utils::Box(BOX_ANY, "%s", _vm->_textParser[kTBDontHave]);
else if (obj->carriedFl && ((DROP & obj->genericCmd) == DROP))
dropObject(obj);
else if (obj->cmdIndex == 0)
Utils::Box(BOX_ANY, "%s", _vm->_textParser[kTBNeed]);
else
return false;
} else { // It was not a generic cmd
return false;
}
return true;
}
/**
* Test whether command line contains a verb allowed by this object.
* If it does, and the object is near and passes the tests in the command
* list then carry out the actions in the action list and return TRUE
*/
bool Parser_v3d::isObjectVerb(object_t *obj, char *comment) {
debugC(1, kDebugParser, "isObjectVerb(object_t *obj, %s)", comment);
// First, find matching verb in cmd list
uint16 cmdIndex = obj->cmdIndex; // ptr to list of commands
if (cmdIndex == 0) // No commands for this obj
return false;
int i;
for (i = 0; _vm->_cmdList[cmdIndex][i].verbIndex != 0; i++) { // For each cmd
if (isWordPresent(_vm->_arrayVerbs[_vm->_cmdList[cmdIndex][i].verbIndex])) // Was this verb used?
break;
}
if (_vm->_cmdList[cmdIndex][i].verbIndex == 0) // No verbs used.
return false;
// Verb match found. Check if object is Near
char *verb = *_vm->_arrayVerbs[_vm->_cmdList[cmdIndex][i].verbIndex];
if (!isNear(obj, verb, comment))
return false;
// Check all required objects are being carried
cmd *cmnd = &_vm->_cmdList[cmdIndex][i]; // ptr to struct cmd
if (cmnd->reqIndex) { // At least 1 thing in list
uint16 *reqs = _vm->_arrayReqs[cmnd->reqIndex]; // ptr to list of required objects
for (i = 0; reqs[i]; i++) { // for each obj
if (!_vm->_object->isCarrying(reqs[i])) {
Utils::Box(BOX_ANY, "%s", _vm->_textData[cmnd->textDataNoCarryIndex]);
return true;
}
}
}
// Required objects are present, now check state is correct
if ((obj->state != cmnd->reqState) && (cmnd->reqState != DONT_CARE)) {
Utils::Box(BOX_ANY, "%s", _vm->_textData[cmnd->textDataWrongIndex]);
return true;
}
// Everything checked. Change the state and carry out any actions
if (cmnd->reqState != DONT_CARE) // Don't change new state if required state didn't care
obj->state = cmnd->newState;
Utils::Box(BOX_ANY, "%s", _vm->_textData[cmnd->textDataDoneIndex]);
_vm->_scheduler->insertActionList(cmnd->actIndex);
// See if any additional generic actions
if ((verb == _vm->_arrayVerbs[_vm->_look][0]) || (verb == _vm->_arrayVerbs[_vm->_take][0]) || (verb == _vm->_arrayVerbs[_vm->_drop][0]))
isGenericVerb(obj, comment);
return true;
}
bool FaceFrameWalrus::hasSimilarAUs(const FaceFrameWalrus& otherFace) const {
bool foundDifferent = false;
int i = 0;
int t = 0;
while (i < animUnitCount && !foundDifferent) {
float tolerance = 0.1f;
if (!isNear (animUnits[i], otherFace.animUnits[i], tolerance)) {
foundDifferent = true;
t++;
}
i++;
}
return !foundDifferent;
}
Vec2f Animal::getCohesionForce(vector<Animal> animals, float dist) {
float neighbourDist = dist;
Vec2f sum = Vec2f(0, 0);
int count = 0;
for (vector<Animal>::iterator it = animals.begin(); it != animals.end(); ++it) {
if (isNear(*it, neighbourDist)) {
sum = sum + pointToVec(it->getPos());
count++;
}
}
if (count > 0) {
sum = sum / count;
return getSeek(sum);
} else {
return Vec2f(0, 0);
}
}
Vec2f Animal::getAlignmentForce(vector<Animal> animals, float dist) {
float neighbourDist = dist;
Vec2f sum = Vec2f(0, 0);
int count = 0;
for (vector<Animal>::iterator it = animals.begin(); it != animals.end(); ++it) {
if (isNear(*it, neighbourDist)) {
sum = sum + it->velocity;
count++;
}
}
if (count > 0) {
sum = sum / count;
return getSteer(sum);
} else {
return Vec2f(0, 0);
}
}
static bool areMassParametersNear(massParameters *left, massParameters *right) {
bool areEqual = true;
if (!isNear(left->mass[0], right->mass[0], EPSILON)) {
areEqual = false;
} else if (!isNear(left->mass[0], right->mass[0], EPSILON)) {
areEqual = false;
} else if (!isNear(left->totalMass, right->totalMass, EPSILON)) {
areEqual = false;
} else if (!isNear(left->eta, right->eta, EPSILON)) {
areEqual = false;
} else if (!isNear(left->chirpMass, right->chirpMass, EPSILON)) {
areEqual = false;
} else if (!isNear(left->m1_m2, right->m1_m2, EPSILON)) {
areEqual = false;
} else if (!isNear(left->mu, right->mu, EPSILON)) {
areEqual = false;
} else if (!isNear(left->nu, right->nu, EPSILON)) {
areEqual = false;
}
return areEqual;
}
Vec2f Animal::getSeparationForce(vector<Animal> animals, float dist, AnimalRole r) {
float separationDist = dist;
Vec2f sum = Vec2f(0, 0);
int count = 0;
for (vector<Animal>::iterator it = animals.begin(); it != animals.end(); ++it) {
float d = distance(Object::getPos(), it->getPos());
if (isNear(*it, separationDist) && Animal::role == r) {
Vec2f diff = Object::getPos() - it->getPos();
sum = sum + (normalize(diff) / d);
count++;
}
}
if (count > 0) {
sum = sum / count;
Vec2f steer = (normalize(sum) * ANIMAL_MAXSPEED) - Animal::velocity;
return limit (steer);
} else {
return Vec2f (0, 0);
}
}
void markPoit(int myMaze[][9], stack* s, Postion end) {
Postion* p = stack_pop(s);
Queue* q = QueueInit();
Postion current = end;
while(p != 0) {
if (isNear(*p, current) == 1) {
myMaze[p->y][p->x] = 1;
current = *p;
QueueAdd(q, p);
} else {
tk_free(p);
}
p = stack_pop(s);
}
p = QueueGet(q);
printf("(5,5)");
while(p != 0) {
printf("<-(%d,%d)", p->x, p->y);
tk_free(p);
p = QueueGet(q);
}
printf("\n");
QueueDestory(q);
}
/**
* Parse the user's line of text input. Generate events as necessary
*/
void Parser_v2d::lineHandler() {
debugC(1, kDebugParser, "lineHandler()");
status_t &gameStatus = _vm->getGameStatus();
// Toggle God Mode
if (!strncmp(_vm->_line, "PPG", 3)) {
_vm->_sound->playSound(!_vm->_soundTest, kSoundPriorityHigh);
gameStatus.godModeFl = !gameStatus.godModeFl;
return;
}
Utils::strlwr(_vm->_line); // Convert to lower case
// God Mode cheat commands:
// goto <screen> Takes hero to named screen
// fetch <object name> Hero carries named object
// fetch all Hero carries all possible objects
// find <object name> Takes hero to screen containing named object
if (gameStatus.godModeFl) {
// Special code to allow me to go straight to any screen
if (strstr(_vm->_line, "goto")) {
for (int i = 0; i < _vm->_numScreens; i++) {
if (!scumm_stricmp(&_vm->_line[strlen("goto") + 1], _vm->_text->getScreenNames(i))) {
_vm->_scheduler->newScreen(i);
return;
}
}
}
// Special code to allow me to get objects from anywhere
if (strstr(_vm->_line, "fetch all")) {
for (int i = 0; i < _vm->_object->_numObj; i++) {
if (_vm->_object->_objects[i].genericCmd & TAKE)
takeObject(&_vm->_object->_objects[i]);
}
return;
}
if (strstr(_vm->_line, "fetch")) {
for (int i = 0; i < _vm->_object->_numObj; i++) {
if (!scumm_stricmp(&_vm->_line[strlen("fetch") + 1], _vm->_text->getNoun(_vm->_object->_objects[i].nounIndex, 0))) {
takeObject(&_vm->_object->_objects[i]);
return;
}
}
}
// Special code to allow me to goto objects
if (strstr(_vm->_line, "find")) {
for (int i = 0; i < _vm->_object->_numObj; i++) {
if (!scumm_stricmp(&_vm->_line[strlen("find") + 1], _vm->_text->getNoun(_vm->_object->_objects[i].nounIndex, 0))) {
_vm->_scheduler->newScreen(_vm->_object->_objects[i].screenIndex);
return;
}
}
}
}
if (!strcmp("exit", _vm->_line) || strstr(_vm->_line, "quit")) {
if (Utils::Box(kBoxYesNo, "%s", _vm->_text->getTextParser(kTBExit_1d)) != 0)
_vm->endGame();
return;
}
// SAVE/RESTORE
if (!strcmp("save", _vm->_line)) {
_vm->_config.soundFl = false;
if (gameStatus.gameOverFl)
Utils::gameOverMsg();
else
_vm->_file->saveGame(-1, Common::String());
return;
}
if (!strcmp("restore", _vm->_line)) {
_vm->_config.soundFl = false;
_vm->_file->restoreGame(-1);
_vm->_scheduler->restoreScreen(*_vm->_screen_p);
gameStatus.viewState = kViewPlay;
return;
}
if (*_vm->_line == '\0') // Empty line
return;
if (strspn(_vm->_line, " ") == strlen(_vm->_line)) // Nothing but spaces!
return;
if (gameStatus.gameOverFl) { // No commands allowed!
Utils::gameOverMsg();
return;
}
// Find the first verb in the line
char *verb = findVerb();
char *noun = 0; // Noun not found yet
char farComment[kCompLineSize * 5] = ""; // hold 5 line comment if object not nearby
if (verb) { // OK, verb found. Try to match with object
do {
noun = findNextNoun(noun); // Find a noun in the line
// Must try at least once for objects allowing verb-context
for (int i = 0; i < _vm->_object->_numObj; i++) {
object_t *obj = &_vm->_object->_objects[i];
if (isNear(verb, noun, obj, farComment)) {
if (isObjectVerb(verb, obj) // Foreground object
|| isGenericVerb(verb, obj)) // Common action type
return;
}
}
if ((*farComment != '\0') && isBackgroundWord(noun, verb, _vm->_backgroundObjects[*_vm->_screen_p]))
return;
} while (noun);
}
noun = findNextNoun(noun);
if ( !isCatchallVerb(true, noun, verb, _vm->_backgroundObjects[*_vm->_screen_p])
&& !isCatchallVerb(true, noun, verb, _vm->_catchallList)
&& !isCatchallVerb(false, noun, verb, _vm->_backgroundObjects[*_vm->_screen_p])
&& !isCatchallVerb(false, noun, verb, _vm->_catchallList)) {
if (*farComment != '\0') { // An object matched but not near enough
Utils::Box(kBoxAny, "%s", farComment);
} else if (_maze.enabledFl && (verb == _vm->_text->getVerb(_vm->_look, 0))) {
Utils::Box(kBoxAny, "%s", _vm->_text->getTextParser(kTBMaze));
_vm->_object->showTakeables();
} else if (verb && noun) { // A combination I didn't think of
Utils::Box(kBoxAny, "%s", _vm->_text->getTextParser(kTBNoUse_2d));
} else if (verb || noun) {
Utils::Box(kBoxAny, "%s", _vm->_text->getTextParser(kTBNoun));
} else {
Utils::Box(kBoxAny, "%s", _vm->_text->getTextParser(kTBEh_2d));
}
}
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
bool RigCell::isLongPyramidCell(double maxHeightFactor, double nodeNearTolerance ) const
{
cvf::ubyte faceVertexIndices[4];
double squaredMaxHeightFactor = maxHeightFactor*maxHeightFactor;
const std::vector<cvf::Vec3d>& nodes = m_hostGrid->mainGrid()->nodes();
bool isPyramidCell = false;
int face;
for ( face = 0; face < 6 ; ++face)
{
cvf::StructGridInterface::cellFaceVertexIndices(static_cast<cvf::StructGridInterface::FaceType>(face), faceVertexIndices);
int zeroLengthEdgeCount = 0;
const cvf::Vec3d& c0 = nodes[m_cornerIndices[faceVertexIndices[0]]];
const cvf::Vec3d& c1 = nodes[m_cornerIndices[faceVertexIndices[1]]];
const cvf::Vec3d& c2 = nodes[m_cornerIndices[faceVertexIndices[2]]];
const cvf::Vec3d& c3 = nodes[m_cornerIndices[faceVertexIndices[3]]];
if (isNear(c0, c1, nodeNearTolerance)) { ++zeroLengthEdgeCount; }
if (isNear(c1, c2, nodeNearTolerance)) { ++zeroLengthEdgeCount; }
if (isNear(c2, c3, nodeNearTolerance)) { ++zeroLengthEdgeCount; }
if (zeroLengthEdgeCount == 3)
{
return true;
// Collapse of a complete face is detected. This is possibly the top of a pyramid
// "face" has the index to the collapsed face. We need the size of the opposite face
// to compare it with the pyramid "roof" length.
cvf::StructGridInterface::FaceType oppositeFace = cvf::StructGridInterface::POS_I;
switch (face)
{
case cvf::StructGridInterface::POS_I:
oppositeFace = cvf::StructGridInterface::NEG_I;
break;
case cvf::StructGridInterface::POS_J:
oppositeFace = cvf::StructGridInterface::NEG_J;
break;
case cvf::StructGridInterface::POS_K:
oppositeFace = cvf::StructGridInterface::NEG_K;
break;
case cvf::StructGridInterface::NEG_I:
oppositeFace = cvf::StructGridInterface::POS_I;
break;
case cvf::StructGridInterface::NEG_J:
oppositeFace = cvf::StructGridInterface::POS_J;
break;
case cvf::StructGridInterface::NEG_K:
oppositeFace = cvf::StructGridInterface::POS_K;
break;
default:
CVF_ASSERT(false);
break;
}
cvf::StructGridInterface::cellFaceVertexIndices(oppositeFace, faceVertexIndices);
const cvf::Vec3d& c0opp = nodes[m_cornerIndices[faceVertexIndices[0]]];
const cvf::Vec3d& c1opp = nodes[m_cornerIndices[faceVertexIndices[1]]];
const cvf::Vec3d& c2opp = nodes[m_cornerIndices[faceVertexIndices[2]]];
const cvf::Vec3d& c3opp = nodes[m_cornerIndices[faceVertexIndices[3]]];
// Check if any of the opposite face vertexes are also degenerated to the pyramid top
int okVertexCount = 0;
cvf::Vec3d okVxs[4];
if (!isNear(c0opp, c0, nodeNearTolerance)) { okVxs[okVertexCount] = c0opp; ++okVertexCount; }
if (!isNear(c1opp, c0, nodeNearTolerance)) { okVxs[okVertexCount] = c1opp; ++okVertexCount; }
if (!isNear(c2opp, c0, nodeNearTolerance)) { okVxs[okVertexCount] = c2opp; ++okVertexCount; }
if (!isNear(c3opp, c0, nodeNearTolerance)) { okVxs[okVertexCount] = c3opp; ++okVertexCount; }
if (okVertexCount < 2)
{
return true;
}
else
{
// Use the good vertices to calculate a face size that can be compared to the pyramid height:
double typicalSquaredEdgeLength = 0;
for (int i = 1; i < okVertexCount; ++i)
{
typicalSquaredEdgeLength += (okVxs[i-1] - okVxs[i]).lengthSquared();
}
typicalSquaredEdgeLength /= okVertexCount;
double pyramidHeightSquared = (okVxs[0] - c0).lengthSquared();
if (pyramidHeightSquared > squaredMaxHeightFactor*typicalSquaredEdgeLength)
{
return true;
}
}
}
// Check the ratio of the length of opposite edges.
// both ratios have to be above threshold to detect a pyramid-ish cell
// Only test this if we have all nonzero edge lenghts.
else if (zeroLengthEdgeCount == 0) // If the four first faces are ok, the two last must be as well
{
double e0SquareLenght = (c1 - c0).lengthSquared();
double e2SquareLenght = (c3 - c2).lengthSquared();
if ( e0SquareLenght / e2SquareLenght > squaredMaxHeightFactor
|| e2SquareLenght / e0SquareLenght > squaredMaxHeightFactor )
{
double e1SquareLenght = (c2 - c1).lengthSquared();
double e3SquareLenght = (c0 - c3).lengthSquared();
if ( e1SquareLenght / e3SquareLenght > squaredMaxHeightFactor
|| e3SquareLenght / e1SquareLenght > squaredMaxHeightFactor )
{
return true;
}
}
}
}
return false;
}
void main(int argc, char** argv)
{
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
float Z=1;
/*cvNamedWindow("img", CV_WINDOW_AUTOSIZE);
cvNamedWindow("warp", CV_WINDOW_AUTOSIZE);*/
dstQuad[0].x = 250; //src Top left
dstQuad[0].y = 100;
dstQuad[1].x = 430; //src Top right
dstQuad[1].y = 115;
dstQuad[2].x = 50; //src Bottom left
dstQuad[2].y = 170;
dstQuad[3].x = 630; //src Bot right
dstQuad[3].y = 250;
int lOff = 50, tOff = 150;
srcQuad[0].x = tOff; //dst Top left
srcQuad[0].y = lOff;
srcQuad[1].x = 640-tOff; //dst Top right
srcQuad[1].y = lOff;
srcQuad[2].x = tOff; //dst Bottom left
srcQuad[2].y = 480-lOff;
srcQuad[3].x = 640-tOff; //dst Bot right
srcQuad[3].y = 480-lOff;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
int ik=0, ni = 0, niX = 22-1;
char names[22][25] = {
"../../Data/6 Dec/009.jpg",
"../../Data/6 Dec/011.jpg",
"../../Data/6 Dec/012.jpg",
"../../Data/6 Dec/016.jpg",
"../../Data/6 Dec/018.jpg",
"../../Data/6 Dec/019.jpg",
"../../Data/6 Dec/020.jpg",
"../../Data/6 Dec/022.jpg",
"../../Data/6 Dec/024.jpg",
"../../Data/6 Dec/064.jpg",
"../../Data/6 Dec/065.jpg",
"../../Data/6 Dec/066.jpg",
"../../Data/6 Dec/067.jpg",
"../../Data/6 Dec/068.jpg",
"../../Data/6 Dec/069.jpg",
"../../Data/6 Dec/070.jpg",
"../../Data/6 Dec/071.jpg",
"../../Data/6 Dec/072.jpg",
"../../Data/6 Dec/073.jpg",
"../../Data/6 Dec/074.jpg",
"../../Data/6 Dec/075.jpg",
"../../Data/6 Dec/076.jpg"
};
int lwSum = 0, nopf = 0;
//CvCapture *capture = cvCaptureFromCAM(0);
/*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
IplImage* image = cvRetrieveFrame(capture);
CvSize imgSize;
imgSize.width = image->width;
imgSize.height = image->height;
CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/
while(1)
{
//IplImage* img = cvQueryFrame(capture);
IplImage* img = cvLoadImage( "../../Data/23 Jan/c.jpg", CV_LOAD_IMAGE_COLOR);
//cvSaveImage(nameGen(ik++), img, 0);
//cvShowImage("img", img);
IplImage* warp_img = cvCloneImage(img);
CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z;
cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
//cvReleaseImage(&img);
//cvWaitKey(0);
IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1);
cvCvtColor( warp_img, grayimg, CV_RGB2GRAY );
cvReleaseImage(&warp_img);
cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0);
cvEqualizeHist(grayimg, grayimg);
cvThreshold(grayimg, grayimg, PercentileThreshold(grayimg, 10.0), 255, CV_THRESH_BINARY);
IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3);
CvMemStorage* line_storage=cvCreateMemStorage(0);
CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10);
cvReleaseImage(&grayimg);
double angle = 0.0, temp;
double lengthSqd, wSum=0;
CvPoint center = cvPoint(0, 0);
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
//lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y);
wSum += 1;//lengthSqd;
if(line[0].y > line[1].y)
temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x));
else
temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x));
if(temp < 0)
angle += (90 + 180/3.14*temp)/* * lengthSqd*/;
else
angle += (180/3.14*temp - 90)/* * lengthSqd*/;
center.x += (line[0].x + line[1].x)/2;
center.y += (line[0].y + line[1].y)/2;
}
angle /= wSum; // Angle Direction: Left == -ve and Right == +ve
// Angle is calculated w.r.t Vertical
//angle+=10; // Angle Offset (Depends on camera's position)
center.x /= results->total;
center.y /= results->total;
double m = (angle != 0) ? tan(CV_PI*(0.5-angle/180)) : 100000; // 100000 represents a very large slope (near vertical)
//m=-m; // Slope Correction
CvPoint leftCenter = cvPoint(0, 0), rightCenter = cvPoint(0, 0);
double leftSlope = 0, rightSlope = 0, leftCount = 0, rightCount = 0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
CvPoint midPoint = cvPoint((line[0].x + line[1].x)/2, (line[0].y + line[1].y)/2);
double L11 = (0-center.y + m*(0-center.x + 0.0))/m;
double L22 = (midPoint.y-center.y + m*(midPoint.x-center.x + 0.0))/m;
if(L11*L22 > 0)
{
leftCenter.x += midPoint.x;
leftCenter.y += midPoint.y;
leftSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
leftCount++;
}
else
{
rightCenter.x += midPoint.x;
rightCenter.y += midPoint.y;
rightSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
rightCount++;
}
}
cvReleaseMemStorage(&line_storage);
leftCenter.x /= leftCount; leftCenter.y /= leftCount; leftSlope /= leftCount;
rightCenter.x /= rightCount; rightCenter.y /= rightCount; rightSlope /= rightCount;
CvPoint botCenter = cvPoint(finalimg->width/2, finalimg->height);
int dL = abs(botCenter.y-leftCenter.y + m * (botCenter.x-leftCenter.x)) / sqrt(m*m + 1);
int dR = abs(botCenter.y-rightCenter.y + m * (botCenter.x-rightCenter.x)) / sqrt(m*m + 1);
int lw = abs((leftCenter.y - rightCenter.y) + m*(leftCenter.x - rightCenter.x)) / sqrt(m*m + 1);
lwSum += lw;
nopf++;
if(lw <= SINGLE_LANE_WIDTH)
{
double L11 = (0-leftCenter.y + m*(0-leftCenter.x + 0.0))/m;
double L22 = (botCenter.y-leftCenter.y + m*(botCenter.x-leftCenter.x + 0.0))/m;
if(L11*L22 < 0)
dR = lwSum/nopf - dL; // Only Left Lane is visible
else
dL = lwSum/nopf - dR; // Only Right Lane is visible
}
//cvSaveImage("test.jpg", finalimg, 0);
printf("Bot:\t(%d, %d, %.3f)\n", dL, (finalimg->height)/10, 90.0-angle);
printf("Target:\t(%d, %d, %.3f)\n", (dL+dR)/2, (finalimg->height)*9/10, 90.0);
location bot, target;
bot.x = dL; bot.y = (finalimg->height)/10; bot.theta = 90.0-angle;
target.x = (dL+dR)/2; target.y = (finalimg->height)*9/10; target.theta = 90.0;
cvReleaseImage(&finalimg);
list *ol = NULL, *cl = NULL;
elem e,vare;
e.l = bot; e.g = 0; e.h = 0; e.id = UNDEFINED;
int n = 15;
elem* np = loadPosData(n);
while(1)
{
cl = append(cl, e);
//printList(cl);
if(isNear(e.l, target))
break;
ol = update(ol, e, target, np, n);
//printList(ol);
e = findMin(ol);
//printf("Min: (%.3f, %.3f, %.3f, %d)\n", e.l.x, e.l.y, e.l.theta, e.id);
ol = detach(ol, e);
//printList(ol);
//getchar();
}
free(np);
vare = e;
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
while(!((abs(vare.l.x-bot.x) < 1.25) && (abs(vare.l.y-bot.y) < 1.25)))
{
vare=search(cl,vare.parent.x,vare.parent.y);
if(vare.id != -1)
{
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
e = vare;
}
}
printf("\n(%.3f, %.3f, %.3f) : %d\n", e.l.x, e.l.y, e.l.theta, e.id);
//navCommand(10-e.id, e.id);
releaseList(ol);
releaseList(cl);
getchar();
int c = cvWaitKey(0);
if(c == '4')
{
if(ni != 0)
ni--;
}
else if(c == '6')
{
if(ni != niX)
ni++;
}
}
}
int _tmain(int argc, _TCHAR* argv[])
{
location bot, target;
bot.x = 300; bot.y = 20; bot.theta = 90.0;
target.x = 300; target.y = 450; target.theta = 90.000;
list *ol = NULL, *cl = NULL;
elem e,vare;
e.l = bot; e.g = 0; e.h = 0; e.id = UNDEFINED;
int n = 13;
elem* np = loadPosData(n);
while(1)
{
cl = append(cl, e);
//printList(cl);
if(isNear(e.l, target))
break;
ol = update(ol, e, target, np, n);
//printList(ol);
e = findMin(ol);
printf("Min: (%.3f, %.3f, %.3f)\n", e.l.x, e.l.y, e.l.theta);
ol = detach(ol, e);
//printList(ol);
//getchar();
}
//getchar();
cvNamedWindow("hello",CV_WINDOW_AUTOSIZE);
IplImage *img = cvCreateImage(cvSize(500, 500), IPL_DEPTH_8U, 3);
cvCircle(img, cvPoint(300, 500-300), 45, CV_RGB(0, 15, 200), 1, CV_AA, 0);
//list *t = cl;
//while(t)
//{
// cvLine(img,cvPoint(t->p.parent.x*40,500-(t->p.parent.y*40)),cvPoint(t->p.parent.x*40+2,500-(t->p.parent.y*40)-2),CV_RGB(255,255,0),2,CV_AA,0);
// //printf("(%.3f, %.3f) ", t->p.l.x, t->p.l.y);
// t=t->next;
//}
CvPoint a = cvPoint(target.x, 500 - (target.y));
CvPoint b = cvPoint((target.x + 10*cos(target.theta*(CV_PI/180))), 500 - ((target.y+10*sin(target.theta*(CV_PI/180)))));
cvLine(img, a, b, CV_RGB(0,255,0), 2, CV_AA, 0);
a = cvPoint(bot.x, 500 - (bot.y));
b = cvPoint((bot.x + 10*cos(bot.theta*(CV_PI/180))), 500 - ((bot.y+10*sin(bot.theta*(CV_PI/180)))));
cvLine(img, a, b, CV_RGB(0,0,255), 2, CV_AA, 0);
vare = e;
a = cvPoint(vare.l.x, 500 - (vare.l.y));
b = cvPoint((vare.l.x + 10*cos(vare.l.theta*(CV_PI/180))), 500 - ((vare.l.y+10*sin(vare.l.theta*(CV_PI/180)))));
cvLine(img, a, b, CV_RGB(255,0,0), 2, CV_AA, 0);
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
while(!((abs(vare.l.x-bot.x) < 1.25) && (abs(vare.l.y-bot.y) < 1.25)))
{
vare=searchforcoor(cl,vare.parent.x,vare.parent.y);
if(vare.id != -1)
{
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
a = cvPoint(vare.l.x, 500 - (vare.l.y));
b = cvPoint((vare.l.x + 10*cos(vare.l.theta*(CV_PI/180))), 500 - ((vare.l.y+10*sin(vare.l.theta*(CV_PI/180)))));
cvLine(img, a, b, CV_RGB(255,0,0), 2, CV_AA, 0);
}
}
cvShowImage("hello",img);
cvWaitKey(0);
}
