bool CRobotMap::SetCone(Position stPos, float fRobotAngle, Cone stCone, int distance)
{
Position fMinPos, fMaxPos;
Index iMinIndex, iMaxIndex;
fRobotAngle = -fRobotAngle;
/*fMinPos.fX_m = stPos.fX_m - stCone.fRange_m;
fMinPos.fY_m = stPos.fY_m - stCone.fRange_m;
fMaxPos.fX_m = stPos.fX_m + stCone.fRange_m;
fMaxPos.fY_m = stPos.fY_m + stCone.fRange_m;*/
fMinPos.fX_m = stPos.fX_m - distance;
fMinPos.fY_m = stPos.fY_m - distance;
fMaxPos.fX_m = stPos.fX_m + distance;
fMaxPos.fY_m = stPos.fY_m + distance;
iMinIndex = PositionToIndex(fMinPos);
iMaxIndex = PositionToIndex(fMaxPos);
//Check if it is -1
for (int y = iMinIndex.iY; y <= iMaxIndex.iY; y++)
{
for (int x = iMinIndex.iX; x <= iMaxIndex.iX; x++)
{
Index tempIndex;
Position tempPosition;
tempIndex.iX = x;
tempIndex.iY = y;
tempPosition = IndexToPosition(tempIndex);
float CO = tempPosition.fX_m - stPos.fX_m;
float CA = tempPosition.fY_m - stPos.fY_m;
float H = sqrt((CO * CO) + (CA * CA));
float RelativeAngle = -(atan2(CA,CO) * 180 / 3.1415); //TODO fix constants
float fBeamAngle_deg = (((int)(fRobotAngle) % 360) + fRobotAngle - ((float)((int)fRobotAngle)) + stCone.fAzimuth_deg); /* precisao corrigida*/
if (RelativeAngle > (fBeamAngle_deg - stCone.fViewAngle_deg / 2) &&
RelativeAngle < (fBeamAngle_deg + stCone.fViewAngle_deg / 2) &&
//(H < stCone.fRange_m))
(H < distance))
{
Map[x][y].bSonarViewed = true;
}
}
}
return true;
}
bool CRobotMap::SetCellWalked(Position stPos, bool bWalked)
{
Index tempIndex = PositionToIndex(stPos);
if (tempIndex.iX < MATRIX_X_SIZE && tempIndex.iY < MATRIX_Y_SIZE)
{
Map[tempIndex.iX][tempIndex.iY].bWalkedOver = bWalked;
return true;
}
else
return false;
}
//Vector3
List<TRI>* OcTree::GetTrianglesAt(const Vector3& pos)
{
foundTriangles.Clear();
int idx = PositionToIndex(pos);
drawIdx = idx;
int start = octreeTable[idx].TriIdxStart;
int end = octreeTable[idx].TriIdxStart + octreeTable[idx].TriIdxCount;
for (int i = start; i < end; i++)
{
unsigned int triIdx = triangleIdxTable[i];
foundTriangles.Add(triangles[triIdx]);
}
return &foundTriangles;
}
