/*
* Methods corresponding to IDL attributes and operations
*/
void PathServiceSVC_impl::get_path(RTC::Path2D_out path, const RTC::Pose2D& position, const RTC::Pose2D& target)
{
CSerializablePtr serializable_ptr;
// Convert the simple map from string format to serializable object
StringToObject(rtc_ptr_->get_map(), serializable_ptr);
// The path
std::deque<TPoint2D> deque_path;
COccupancyGridMap2D grid_map;
// Convert serializable simple map object to grid map
grid_map.loadFromSimpleMap(*CSimpleMapPtr(serializable_ptr));
CPathPlanningCircularRobot path_planning;
path_planning.robotRadius = 0.17f;
bool not_found;
// Find the path
path_planning.computePath(grid_map,
CPose2D(position.position.x, position.position.y, position.heading),
CPose2D(target.position.x, target.position.y, target.heading),
deque_path, not_found, 1000.0f);
#ifdef DEBUG
CImage img;
grid_map.getAsImage(img, false, true);
int R = round(path_planning.robotRadius / grid_map.getResolution() );
for (std::deque<TPoint2D>::const_iterator it=deque_path.begin();it!=deque_path.end();++it)
img.drawCircle( grid_map.x2idx(it->x),grid_map.getSizeY()-1-grid_map.y2idx(it->y),R, TColor(0,0,255) );
img.cross(grid_map.x2idx(position.position.x),grid_map.getSizeY()-1-grid_map.y2idx(position.position.y),TColor(0x20,0x20,0x20),'+',10);
img.cross(grid_map.x2idx(target.position.x),grid_map.getSizeY()-1-grid_map.y2idx(target.position.y),TColor(0x50,0x50,0x50),'x',10);
const std::string dest = "path_planning.png";
img.saveToFile(dest);
#endif
path = new RTC::Path2D();
if(!not_found) { // if found any path
// Copy path from deque_path to path
path->waypoints.length(deque_path.size());
int i = 0;
std::deque<TPoint2D>::const_iterator it = deque_path.begin();
for(;it != deque_path.end(); ++it, i++) {
path->waypoints[i].target.position.x = it->x;
path->waypoints[i].target.position.y = it->y;
path->waypoints[i].target.heading = 0.0f;
}
} else { // if didn't find the path
path->waypoints.length(0);
cout<<"Not Found"<<endl;
}
}
//-------------------------------------------------------------------------------------
HRESULT CDXUTEffectMap::SetStandardParameter( const WCHAR* strSemantic,
const WCHAR* strObject,
DWORD dwObjectIndex,
float* pData,
DWORD dwDataLen,
const WCHAR* strType,
const WCHAR* strUnits,
const WCHAR* strSpace )
{
// Map the semantic to the standard set
DXUT_SEMANTIC eSemantic = StringToSemantic( strSemantic );
if( eSemantic == DXUT_UNKNOWN_SEMANTIC )
return E_FAIL;
// Map the object to the standard set
DXUT_OBJECT eObject = StringToObject( strObject );
if( eObject == DXUT_UNKNOWN_OBJECT )
return E_FAIL;
return SetStandardParameter( eSemantic, eObject, dwObjectIndex, pData, dwDataLen, strType, strUnits, strSpace );
}
//-------------------------------------------------------------------------------------
DXUT_OBJECT CDXUTEffectMap::StringToObject( const WCHAR* strObject )
{
char cstr[MAX_PATH+1] = {0};
WideCharToMultiByte( CP_ACP, 0, strObject, -1, cstr, MAX_PATH, NULL, NULL );
return StringToObject( cstr );
}
//-------------------------------------------------------------------------------------
// Investigates all the parameters, looking at semantics and annotations and placing
// handles to these parameters within the internal database.
//-------------------------------------------------------------------------------------
HRESULT CDXUTEffectMap::AddEffect( ID3DXEffect* pEffect )
{
HRESULT hr;
if( pEffect == NULL )
return E_INVALIDARG;
// Get the number of parameters
D3DXEFFECT_DESC descEffect;
V_RETURN( pEffect->GetDesc( &descEffect ) );
// Enumerate the parameters
for( UINT iParam=0; iParam < descEffect.Parameters; iParam++ )
{
// Retrieve param
D3DXHANDLE hParameter = pEffect->GetParameter( NULL, iParam );
if( NULL == hParameter )
return E_FAIL;
// Grab description
D3DXPARAMETER_DESC desc;
V_RETURN( pEffect->GetParameterDesc( hParameter, &desc ) );
// If this parameter doesn't have a semantic, skip to the next parameter
if( desc.Semantic == NULL )
continue;
// Map the semantic to the standard set
DXUT_SEMANTIC eSemantic = StringToSemantic( desc.Semantic );
if( eSemantic == DXUT_UNKNOWN_SEMANTIC )
continue;
// Get the object annotation
const char* cstrObject = "Geometry";
D3DXHANDLE hAnnotation = pEffect->GetAnnotationByName( hParameter, "Object" );
if( hAnnotation )
{
V_RETURN( pEffect->GetString( hAnnotation, &cstrObject ) );
}
// Map the object to the standard set
DXUT_OBJECT eObject = StringToObject( cstrObject );
if( eObject == DXUT_UNKNOWN_OBJECT )
continue;
// Extract the index from the semantic
int index = 0;
const char* strIndex = desc.Semantic + strlen(desc.Semantic)-1;
// If there is a digit at the end of the semantic, locate the beginning of the index
// and convert to an integer
if( isdigit( (BYTE) (*strIndex) ) )
{
while( isdigit( (BYTE) (*(strIndex-1)) ) )
{
--strIndex;
}
index = atoi( strIndex );
}
// Check whether index is out of bounds
if( index < 0 || index >= MAX_INDEX )
continue;
// Store the handle
CGrowableArray<ParamList>* pBindings = &m_Bindings[ eSemantic ][ eObject ][ index ];
bool bBound = false;
for( int i=0; i < pBindings->GetSize(); i++ )
{
if( pBindings->GetAt(i).pEffect == pEffect )
{
// Found the containing effect for this parameter in the list, add the new handle
pBindings->GetAt(i).ahParameters.Add( hParameter );
bBound = true;
break;
}
}
if( !bBound )
{
// This parameter belongs to a new effect
ParamList newParamList;
newParamList.pEffect = pEffect;
pEffect->AddRef();
newParamList.ahParameters.Add( hParameter );
pBindings->Add( newParamList );
}
}
return S_OK;
}
