void CGameMap::ShowFog(){
CRect mapRect(0,0,sizeX-1,sizeY-1);
CPoint gridPoint(CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6)));
//gridPoint = CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6));
for(int y=0;y<SIZE_Y/32+17;y++){//17為最佳化的值
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x+=1;
gridPoint.y-=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint)){
if(mapTile[gridPoint.y][gridPoint.x].VisionCount()==0 && mapTile[gridPoint.y][gridPoint.x].IsExplored()){
fog.SetTopLeft(screenPoint.x-fog.Width()/2,screenPoint.y);
fog.ShowBitmap();
}
}
}
gridPoint.x+=1;
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x-=1;
gridPoint.y+=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint)){
if(mapTile[gridPoint.y][gridPoint.x].VisionCount()==0 && mapTile[gridPoint.y][gridPoint.x].IsExplored()){
fog.SetTopLeft(screenPoint.x-fog.Width()/2,screenPoint.y);
fog.ShowBitmap();
}
}
}
gridPoint.y+=1;
}
}
OpticalFlowTransitionModel::OpticalFlowTransitionModel(
shared_ptr<TransitionModel> fallback, double scatter, Size gridSize, bool circle, Size windowSize, int maxLevel) :
fallback(fallback),
templatePoints(),
gridSize(gridSize),
windowSize(windowSize),
maxLevel(maxLevel),
previousPyramid(),
currentPyramid(),
points(),
forwardPoints(),
backwardPoints(),
forwardStatus(),
backwardStatus(),
error(),
squaredDistances(),
correctFlowCount(0),
scatter(scatter),
generator(boost::mt19937(time(0)), boost::normal_distribution<>()) {
float gridY = 1 / static_cast<float>(gridSize.height);
float gridX = 1 / static_cast<float>(gridSize.width);
Point2f gridPoint(-0.5f + 0.5 * gridX, -0.5f + 0.5 * gridY);
for (int y = 0; y < gridSize.height; ++y) {
for (int x = 0; x < gridSize.width; ++x) {
float r = 1.f / 2.f;
if (!circle || gridPoint.x * gridPoint.x + gridPoint.y * gridPoint.y < r * r)
templatePoints.push_back(gridPoint);
gridPoint.x += gridX;
}
gridPoint.x = -0.5f + gridX / 2;
gridPoint.y += gridY;
}
}
inline void frommxArray(const mxArray *ParamArray) {
/*
* This function expects the ParamArray to be a MATLAB structure with
* at-least the following fields
*
* a - single - scalar
* b - single - scalar
* c - single - scalar
* d - single - scalar
*
* GridXSpec - single - vector of length 3
* GridYSpec - single - vector of length 3
*
* GridXSpec = [GridXBegin, GridXStep, GridXEnd]
* GridYSpec = [GridYBegin, GridYStep, GridYEnd]
*
* onemsbyTstep - uint32_t - scalar
*
* InitialPointSet - should be a valid 'PointVector' struct representing
* the region of points from which to search ahead.
*/
getInputfromStruct<float>(ParamArray, "a", this->a, getInputOps(2, "is_required", "required_size", 1));
getInputfromStruct<float>(ParamArray, "b", this->b, getInputOps(2, "is_required", "required_size", 1));
getInputfromStruct<float>(ParamArray, "c", this->c, getInputOps(2, "is_required", "required_size", 1));
getInputfromStruct<float>(ParamArray, "d", this->d, getInputOps(2, "is_required", "required_size", 1));
uint32_t onemsbyTstep;
getInputfromStruct<uint32_t>(ParamArray, "onemsbyTstep", onemsbyTstep, getInputOps(2, "is_required", "required_size", 1));
MexVector<float> GridXSpec;
MexVector<float> GridYSpec;
getInputfromStruct<float>(ParamArray, "GridXSpec", GridXSpec, getInputOps(2, "is_required", "required_size", 3));
getInputfromStruct<float>(ParamArray, "GridYSpec", GridYSpec, getInputOps(2, "is_required", "required_size", 3));
float eps = 1E-10; // epsilon used for floating point comparisons
uint32_t XGridMax, YGridMax;
XGridMax = uint32_t((GridXSpec[2] - GridXSpec[0])/GridXSpec[1] + 2*eps) + 1;// largest n such that (n-1)*GridXSpec[1] + GridXSpec[0] <= GridXSpec[2]
YGridMax = uint32_t((GridYSpec[2] - GridYSpec[0])/GridYSpec[1] + 2*eps) + 1;// largest n such that (n-1)*GridYSpec[1] + GridYSpec[0] <= GridYSpec[2]
this->PrivateTransform.scaleX = GridXSpec[1];
this->PrivateTransform.scaleY = GridYSpec[1];
this->PrivateTransform.shiftX = GridXSpec[0];
this->PrivateTransform.shiftY = GridYSpec[0];
this->XRange = {0, XGridMax};
this->YRange = {0, YGridMax};
this->timeStep = 1.0f / onemsbyTstep;
// Calculate the Grid Y Coordinate for 30.0V
auto GridY30V = this->Transform.toGridCoords(SinglePoint(0, 30.0f)).y;
GridY30V = (GridY30V >= YGridMax)? YGridMax : GridY30V;
for(uint32_t i=0; i < XGridMax; ++i) {
Point gridPoint(i, uint32_t(GridY30V+0.5f));
PrivateInitialPointSet.insert(gridPoint);
}
}
/** calculate the time point denoted by this SMPTE timecode,
* by summing up the timecode's components */
TimeValue
Smpte::evaluate (SmpteTC const& tc, QuantR quantiser)
{
uint frameRate = tc.getFps();
int64_t gridPoint(tc.frames);
gridPoint += int64_t(tc.secs) * frameRate;
gridPoint += int64_t(tc.mins) * frameRate * 60;
gridPoint += int64_t(tc.hours) * frameRate * 60 * 60;
return quantiser.timeOf (tc.sgn * gridPoint);
}
void CGameMap::ShowMapTile(){
CPoint gridPoint(CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6)));
//取得整個地圖的範圍，用於判斷是否再地圖內
CRect mapRect(0,0,sizeX-1,sizeY-1);
for(int y=0;y<SIZE_Y/32+17;y++){//17為最佳化的值
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x+=1;
gridPoint.y-=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint) && this->mapTile[gridPoint.y][gridPoint.x].IsExplored()){
this->mapTile[gridPoint.y][gridPoint.x].OnShow();
}
}
gridPoint.x+=1;
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x-=1;
gridPoint.y+=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint) && this->mapTile[gridPoint.y][gridPoint.x].IsExplored()){
this->mapTile[gridPoint.y][gridPoint.x].OnShow();
}
}
gridPoint.y+=1;
}
gridPoint = CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6));
for(int y=0;y<SIZE_Y/32+17;y++){//17為最佳化的值
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x+=1;
gridPoint.y-=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint) && this->mapTile[gridPoint.y][gridPoint.x].IsExplored()){
//this->mapTile[gridPoint.y][gridPoint.x].OnShow();
}
}
gridPoint.x+=1;
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x-=1;
gridPoint.y+=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint) && this->mapTile[gridPoint.y][gridPoint.x].IsExplored()){
//this->mapTile[gridPoint.y][gridPoint.x].OnShow();
}
}
gridPoint.y+=1;
}
}
OpticalFlowTransitionModel::OpticalFlowTransitionModel(shared_ptr<TransitionModel> fallback,
double positionDeviation, double sizeDeviation, Size gridSize, bool circle, Size windowSize, int maxLevel) :
fallback(fallback),
templatePoints(),
gridSize(gridSize),
windowSize(windowSize),
maxLevel(maxLevel),
previousPyramid(),
currentPyramid(),
points(),
forwardPoints(),
backwardPoints(),
forwardStatus(),
backwardStatus(),
error(),
squaredDistances(),
correctFlowCount(0),
positionDeviation(positionDeviation),
sizeDeviation(sizeDeviation),
generator(boost::mt19937(time(0)), boost::normal_distribution<>()) {
// float gridY = 1 / static_cast<float>(gridSize.height);
// float gridX = 1 / static_cast<float>(gridSize.width);
// Point2f gridPoint(-0.5f + 0.5 * gridX, -0.5f + 0.5 * gridY);
// for (int y = 0; y < gridSize.height; ++y) {
// for (int x = 0; x < gridSize.width; ++x) {
// float r = 1.f / 2.f;
// if (!circle || gridPoint.x * gridPoint.x + gridPoint.y * gridPoint.y < r * r)
// templatePoints.push_back(gridPoint);
// gridPoint.x += gridX;
// }
// gridPoint.x = -0.5f + gridX / 2;
// gridPoint.y += gridY;
// }
// TODO die variante erscheint bissl besser (größerer abstand der punkte zum rand)
// braucht aber evtl. bissl mehr scatter nötig
float gridY = 1.f / static_cast<float>(gridSize.height + 2);
float gridX = 1.f / static_cast<float>(gridSize.width + 2);
float r = 0.5f - 0.5f * (gridX + gridY);
Point2f gridPoint(-0.5f + 0.5 * gridX + gridX, -0.5f + 0.5 * gridY + gridY);
for (int y = 0; y < gridSize.height; ++y) {
for (int x = 0; x < gridSize.width; ++x) {
if (!circle || gridPoint.x * gridPoint.x + gridPoint.y * gridPoint.y < r * r)
templatePoints.push_back(gridPoint);
gridPoint.x += gridX;
}
gridPoint.x = -0.5f + gridX / 2 + gridX;
gridPoint.y += gridY;
}
}
void CGameMap::ShowSprites(){
CRect mapRect(0,0,sizeX-1,sizeY-1);
CPoint gridPoint(CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6)));
//gridPoint = CConvert::GetGridPointByScreenPoint(CPoint(-64*6,-32*6));
for(int y=0;y<SIZE_Y/32+17;y++){//17為最佳化的值
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x+=1;
gridPoint.y-=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint)){
if(!mapTile[gridPoint.y][gridPoint.x].IsExplored()){
continue;
}
if(mapTile[gridPoint.y][gridPoint.x].VisionCount()==0){//視野外只顯示房子
set<CSprite*>::iterator it;
for(it = mapTile[gridPoint.y][gridPoint.x].Sprites().begin();it!= mapTile[gridPoint.y][gridPoint.x].Sprites().end();it++){
if((*it)->IsBuilding() || (*it)->IsResource()){
(*it)->OnShow();
}
}
}else{//再視野內
this->mapTile[gridPoint.y][gridPoint.x].OnShowSprites();
}
}
}
gridPoint.x+=1;
for(int x=0;x<SIZE_X/64+12;x++){
gridPoint.x-=1;
gridPoint.y+=1;
CPoint screenPoint(CConvert::GetScreenPointByGridPoint(gridPoint));
if(mapRect.PtInRect(gridPoint)){
if(!mapTile[gridPoint.y][gridPoint.x].IsExplored()){
continue;
}
if(mapTile[gridPoint.y][gridPoint.x].VisionCount()==0){//視野外只顯示房子
set<CSprite*>::iterator it;
for(it = mapTile[gridPoint.y][gridPoint.x].Sprites().begin();it!= mapTile[gridPoint.y][gridPoint.x].Sprites().end();it++){
if((*it)->IsBuilding()){
(*it)->OnShow();
}
}
}else{//再視野內
this->mapTile[gridPoint.y][gridPoint.x].OnShowSprites();
}
}
}
gridPoint.y+=1;
}
}
bool CGameMap::CanPutHere(CPoint mapPoint){
CPoint gridPoint(CConvert::GetGridPointByMapPoint(mapPoint));
return CGameMap::GetMapTile(gridPoint)->Sprites().empty();
}
void USObstacleGridProvider::addUsMeasurement(bool actuatorChanged, float m, SensorData::UsActuatorMode mappedActuatorMode, unsigned int timeStamp,
const FilteredSensorData& theFilteredSensorData, const FrameInfo& theFrameInfo,
const OdometryData& theOdometryData)
{
ASSERT(mappedActuatorMode >= 0);
ASSERT(mappedActuatorMode < 4);
bool isNew = timeStamp == theFilteredSensorData.usTimeStamp;
if(timeStamp > bufferedMeasurements[mappedActuatorMode].timeStamp)
{
if(actuatorChanged || m < bufferedMeasurements[mappedActuatorMode].value)
bufferedMeasurements[mappedActuatorMode].value = m;
bufferedMeasurements[mappedActuatorMode].timeStamp = timeStamp;
}
if(m < parameters.minValidUSDist)
return;
switch(mappedActuatorMode)
{
case SensorData::leftToLeft:
for(std::vector<UsMeasurement>::iterator it = postponedLeftToRightMeasurements.begin(); it != postponedLeftToRightMeasurements.end();)
{
if(timeStamp - it->timeStamp > 1000) // postponed measurement is to old
it = postponedLeftToRightMeasurements.erase(it);
else if(abs(m - it->value) < 200)
{
addUsMeasurement(false, it->value, SensorData::leftToRight, it->timeStamp, theFilteredSensorData,theFrameInfo,theOdometryData);
it = postponedLeftToRightMeasurements.erase(it);
}
else
++it;
}
break;
case SensorData::rightToRight:
for(std::vector<UsMeasurement>::iterator it = postponedRightToLeftMeasurements.begin(); it != postponedRightToLeftMeasurements.end();)
{
if(timeStamp - it->timeStamp > 1000) // postponed measurement is to old
it = postponedRightToLeftMeasurements.erase(it);
else if(abs(m - it->value) < 200)
{
addUsMeasurement(false, it->value, SensorData::rightToLeft, it->timeStamp, theFilteredSensorData,theFrameInfo,theOdometryData);
it = postponedRightToLeftMeasurements.erase(it);
}
else
++it;
}
break;
case SensorData::leftToRight:
if(isNew && (timeStamp - bufferedMeasurements[SensorData::leftToLeft].timeStamp > 1000 || abs(bufferedMeasurements[SensorData::leftToLeft].value - m) > 200))
{
postponedLeftToRightMeasurements.push_back(UsMeasurement(m, timeStamp));
return; // Don't fill cells, as leftToRight measurements are only valid if left Sensor measured something
}
break;
case SensorData::rightToLeft:
if(isNew && (timeStamp - bufferedMeasurements[SensorData::rightToRight].timeStamp > 1000 || abs(bufferedMeasurements[SensorData::rightToRight].value - m) > 200))
{
postponedRightToLeftMeasurements.push_back(UsMeasurement(m, timeStamp));
return; // Don't fill cells, as rightToLeft measurements are only valid if right Sensor measured something
}
break;
default:
ASSERT(false);
break;
}
// Compute and draw relevant area:
if(m > parameters.maxValidUSDist)
m = static_cast<float>(parameters.maxValidUSDist);
Vector2<> measurement(m, 0.0f);
Vector2<> base, leftOfCone(measurement), rightOfCone(measurement);
switch(mappedActuatorMode)
{
case SensorData::leftToLeft: // left transmitter, left sensor => left
base = parameters.usLeftPose.translation;
leftOfCone.rotate(parameters.usOuterOpeningAngle);
rightOfCone.rotate(parameters.usInnerOpeningAngle);
leftOfCone = parameters.usLeftPose * leftOfCone;
rightOfCone = parameters.usLeftPose * rightOfCone;
break;
case SensorData::rightToRight: // right transmitter, right sensor => right
base = parameters.usRightPose.translation;
leftOfCone.rotate(-parameters.usInnerOpeningAngle);
rightOfCone.rotate(-parameters.usOuterOpeningAngle);
leftOfCone = parameters.usRightPose * leftOfCone;
rightOfCone = parameters.usRightPose * rightOfCone;
break;
case SensorData::leftToRight: // left transmitter, right sensor => center left
base = parameters.usCenterPose.translation;
leftOfCone.rotate(parameters.usCenterOpeningAngle);
rightOfCone.rotate(parameters.usCenterOpeningAngle * 0.25f);
leftOfCone = parameters.usCenterPose * leftOfCone;
rightOfCone = parameters.usCenterPose * rightOfCone;
break;
case SensorData::rightToLeft: // right transmitter, left sensor => center right
base = parameters.usCenterPose.translation;
leftOfCone.rotate(-parameters.usCenterOpeningAngle * 0.25f);
rightOfCone.rotate(-parameters.usCenterOpeningAngle);
leftOfCone = parameters.usCenterPose * leftOfCone;
rightOfCone = parameters.usCenterPose * rightOfCone;
break;
default:
ASSERT(false);
break;
}
// Draw current (positive) measurement:
if(m < parameters.maxValidUSDist)
{
/*LINE("module:USObstacleGridProvider:us", base.x, base.y, leftOfCone.x, leftOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));
LINE("module:USObstacleGridProvider:us", base.x, base.y, rightOfCone.x, rightOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));
LINE("module:USObstacleGridProvider:us", rightOfCone.x, rightOfCone.y, leftOfCone.x, leftOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));*/
}
// Compute additional cones:
// *Free is used for clearing cells between the robot and the obstacle
// *Far is used to add a second obstacle line to the grid (should help in case of high walk speeds and imprecision)
Vector2<> leftOfConeFree(leftOfCone - base);
Vector2<> rightOfConeFree(rightOfCone - base);
Vector2<> leftOfConeFar(leftOfCone - base);
Vector2<> rightOfConeFar(rightOfCone - base);
float cellDiameter = sqrt(static_cast<float>(USObstacleGrid::CELL_SIZE * USObstacleGrid::CELL_SIZE + USObstacleGrid::CELL_SIZE * USObstacleGrid::CELL_SIZE));
leftOfConeFree.normalize(leftOfConeFree.abs() - cellDiameter);
rightOfConeFree.normalize(rightOfConeFree.abs() - cellDiameter);
leftOfConeFar.normalize(leftOfConeFar.abs() + cellDiameter);
rightOfConeFar.normalize(rightOfConeFar.abs() + cellDiameter);
leftOfConeFree += base;
rightOfConeFree += base;
leftOfConeFar += base;
rightOfConeFar += base;
// Transfer cones to grid coordinate system:
const Vector2<int> leftOfConeCells =
worldToGrid(Vector2<int>(static_cast<int>(leftOfCone.x), static_cast<int>(leftOfCone.y)), theOdometryData);
const Vector2<int> leftOfConeCellsFree =
worldToGrid(Vector2<int>(static_cast<int>(leftOfConeFree.x), static_cast<int>(leftOfConeFree.y)), theOdometryData);
const Vector2<int> leftOfConeCellsFar =
worldToGrid(Vector2<int>(static_cast<int>(leftOfConeFar.x), static_cast<int>(leftOfConeFar.y)), theOdometryData);
const Vector2<int> rightOfConeCells =
worldToGrid(Vector2<int>(static_cast<int>(rightOfCone.x), static_cast<int>(rightOfCone.y)), theOdometryData);
const Vector2<int> rightOfConeCellsFree =
worldToGrid(Vector2<int>(static_cast<int>(rightOfConeFree.x), static_cast<int>(rightOfConeFree.y)), theOdometryData);
const Vector2<int> rightOfConeCellsFar =
worldToGrid(Vector2<int>(static_cast<int>(rightOfConeFar.x), static_cast<int>(rightOfConeFar.y)), theOdometryData);
// Free empty space until obstacle:
polyPoints.clear();
// Origin (sensor position):
Vector2<int> p1(static_cast<int>(base.x), static_cast<int>(base.y));
p1 = worldToGrid(p1, theOdometryData);
polyPoints.push_back(p1);
// Left corner of "cone":
polyPoints.push_back(Point(leftOfConeCellsFree, Point::NO_OBSTACLE));
// Right corner of "cone":
const Vector2<> ll(rightOfConeFree);
float f1 = ll.abs(),
f2 = f1 ? leftOfConeFree.abs() / f1 : 0;
Vector2<> gridPoint(ll);
gridPoint *= f2;
const Vector2<int> gridPointInt(static_cast<int>(gridPoint.x), static_cast<int>(gridPoint.y));
polyPoints.push_back(Point(worldToGrid(gridPointInt, theOdometryData), polyPoints.back().flags));
polyPoints.push_back(Point(rightOfConeCellsFree, Point::NO_OBSTACLE));
// Sensor position again:
polyPoints.push_back(p1);
// Clip and fill:
for(int j = 0; j < (int) polyPoints.size(); ++j)
clipPointP2(p1, polyPoints[j]);
fillScanBoundary(theFrameInfo);
// Enter obstacle to grid:
// If the sensor measures a high value, cells are cleared but
// no obstacles are entered
if(m != parameters.maxValidUSDist)
{
line(leftOfConeCells, rightOfConeCells, theFrameInfo);
line(leftOfConeCellsFar, rightOfConeCellsFar, theFrameInfo);
}
}
CPoint CConvert::GetGridPointByScreenPoint(CPoint sp){//輸入螢幕點得到格子座標**********
CPoint mapPoint(GetMapPointByScreenPoint(sp));//螢幕轉成地圖座標
CPoint gridPoint(GetGridPointByMapPoint(mapPoint));//地圖座標轉成格子座標
return gridPoint;
}
