/** Get edge to the left of given edge */
GEdge *
NodeNav::edgeInDir( GEdge * edge, NavDirection dir) const
{
/* Applicable only for top and bottom sectors */
if ( !isDirApplicable( dir, sector()))
{
return NULL;
}
GNode *n = otherEnd( edge);
/* item corresponding to other (than node_priv) node */
if ( isNullP( n))
return NULL;
NodeItem *item = n->item();
// in current node's coordinates
QPointF edge_point = node()->item()->mapFromItem( item, item->boundingRect().center());
GEdge *res = NULL;
qreal min_delta = 0;
/** For each edge */
for ( Node::EdgeIter e_iter = node()->edgesBegin(),
e_end = node()->edgesEnd();
e_iter != e_end;
e_iter++ )
{
GEdge *e = static_cast<GEdge *>( e_iter.edge());
if ( isEdgeInSector( e) && areNotEqP( e, edge))
{
NodeItem *p_item = static_cast<GNode *>( e_iter.node())->item();
QPointF point = node()->item()->mapFromItem( p_item, p_item->boundingRect().center());
if ( isPointInDir( point, edge_point, dir))
{
qreal delta = deltaInDir( point, edge_point, dir);
if ( isNullP( res)
|| delta < min_delta) // for selection of closest edge
{
res = e;
min_delta = delta;
}
}
}
}
return res;
}
/** Destructor */
GGraph::~GGraph()
{
freeMarker( nodeTextIsShown);
for ( GNode *node = firstNode();
isNotNullP( node);
)
{
GNode* next = node->nextNode();
int ir_id = node->irId();
deleteNode( node);
node = next;
}
foreach ( GStyle *style, styles)
{
delete style;
}
}
/* Set a new value for the romanized name
* string value in the GNode data tree, and
* create a node in the tree if needed.
*/
void GIndiEntry::setRomanizedName(const QString & romanName) {
// Append a new node to the tree if needed
if (!_romanNode) {
// Create the new node
_romanNode = new GNode("2 ROMN " + romanName);
// Append it to the end of _nameNode's children
GNode * n = _nameNode->firstChild();
if (!n) { // No existing first child
_nameNode->setFirstChild(_romanNode);
}
else { // Pre-existing sub-list
// Go to the end of the sub-list
while (n->next()) n = n->next();
// Append the new node there
n->setNext(_romanNode);
}
}
// Alter the existing node if needed
else {
_romanNode->setData(romanName);
}
}
bool GPath::FindPath(int sx, int sy, int destx, int desty) {
Path.clear();
App->Log << "Pathfinding session started" << std::endl;
App->Log << "sx : " << sx << std::endl;
App->Log << "sy : " << sy << std::endl;
App->Log << "destx : " << destx << std::endl;
App->Log << "desty : " << desty << std::endl;
DestX = destx;
DestY = desty;
GetAreaBounds();
if (destx == sx && desty == sy) //path found
return true;
for(int y = 0; y < MaxY; y++) {
for(int x = 0; x < MaxX; x++) {
Nodes.push_back(NULL);
}
}
//App->Log << "Empty nodes ready\n";
GNode* start = new GNode(sx, sy);
GNode* end = NULL;
int type = GetTileType(sx, sy);
start->SetType(type);
start->CalcPoints();
SetNode(start);
Open.push(start);
// App->Log << "start->GetX(): " << start->GetX() << std::endl;
// App->Log << "start->GetY(): " << start->GetY() << std::endl;
// App->Log << "Open.top(): " << Open.top() << std::endl;
// App->Log << "Nodes.size(): " << Nodes.size() << std::endl;
// App->Log << "Open.empty(): " << Open.empty() << std::endl;
bool found = false;
while (!Open.empty()) {
//take the node with lowest F value
GNode* node = Open.top();
//remove from open list
node->Close();
Open.pop();
App->Log << "||||Chosen one X: " << node->GetX() << " Y: " << node->GetY() << std::endl;
App->Log << "--------------------\n";
if (node->GetX() == DestX &&
node->GetY() == DestY) {
// App->Log << "found\n";
found = true;
end = node;
// break; doesn't work here, damn it!
while (!Open.empty()) Open.pop();
}
if (!found)
FindAdjacent(node);
}
if (found) {
GNode* node = end;
GPoint point;
point.x = node->GetX();
point.y = node->GetY();
// App->Log << "point.x: " << point.x << std::endl;
// App->Log << "point.y: " << point.y << std::endl;
Path.push_back(point);
while (true) {
GPoint point;
GNode* parent = node->GetParent();
if (NULL == parent) {
break;
}
if (*parent == *start)
break;
point.x = parent->GetX();
point.y = parent->GetY();
// App->Log << "point.x: " << point.x << std::endl;
// App->Log << "point.y: " << point.y << std::endl;
Path.push_back(point);
//App->Log << "Parent: " << parent << std::endl;
node = node->GetParent();
//App->Log << "new node: " << node << std::endl;
}
// App->Log << "X:\n";
// std::vector<GPoint>::iterator it = Path.begin();
// while (it != Path.end()) {
// App->Log << "X: " << it->x << std::endl;
// App->Log << "Y: " << it->y << std::endl;
// it++;
// }
App->Log << "done\n";
}
Cleanup();
return found;
}
void
TestParser::parseLine( QString line)
{
QString n_str("Node ");
QString e_str("Edge ");
/** Node recognition */
QRegExp node_rx("CF ?(Enter)? ?(\\w*) Node (\\d+)");
/** Edge recognition */
QRegExp edge_rx("CF EDGE (\\d+) \\[(\\d+)->(\\d+)\\]");
QTextStream stream( stdout);
/** Expression recognition */
int pos = 0;
if ( edge_rx.indexIn( line) != -1)
{
QString name = e_str.append( edge_rx.cap(1));
QString pred_name("Node ");
pred_name.append( edge_rx.cap( 2));
QString succ_name("Node ");
succ_name.append( edge_rx.cap( 3));
/** Back edge */
QRegExp back_rx("Back");
/** Add edge to symtab */
if ( symtab.find( name) == symtab.end()
&& symtab.find( pred_name) != symtab.end()
&& symtab.find( succ_name) != symtab.end())
{
SymEdge *edge = new SymEdge( name);
edge->setPred( pred_name);
edge->setSucc( succ_name);
symtab[ name] = edge;
/** Add edge to graph */
GNode* pred = static_cast< SymNode *>( symtab[ pred_name])->node();
GNode* succ = static_cast< SymNode *>( symtab[ succ_name])->node();
GEdge* e = graph->graph()->newEdge( pred, succ);
#ifdef _DEBUG
//stream << name << ": " << pred_name << "->" << succ_name << endl;
#endif
if ( back_rx.indexIn( line) != -1 && !e->isSelf())
{
GNode* label = e->insertLabelNode( QPointF( 0,0));
label->item()->setPlainText( "Back");
}
}
} else if ( node_rx.indexIn( line) != -1 )
{
bool good_id = false;
int ir_id = node_rx.cap(3).toInt( &good_id);
QString text = QString("Node ").append( node_rx.cap(3));
QString name = n_str.append( node_rx.cap(3));
if ( !node_rx.cap( 1).isEmpty())
{
text.append("\n").append( node_rx.cap(1));
}
if ( !node_rx.cap( 2).isEmpty())
{
text.append("\n").append( node_rx.cap(2));
}
/** Add node to symtab */
if ( symtab.find( name ) == symtab.end())
{
SymNode* node = new SymNode( name);
curr_node = static_cast<CFNode *>( graph->graph()->newNode());
curr_node->setDoc( new QTextDocument());
node->setNode( curr_node);
node->node()->item()->setPlainText( text);
if ( good_id)
{
node->node()->setIRId( ir_id);
}
symtab[ name] = node;
#ifdef _DEBUG
//stream << name << endl;
#endif
}
} else
{
if ( !isStateNode())
setStateDefault();
}
if ( isStateNode())
{
node_text.append( line).append( "\n");
}
}
/// ==================================================================================
/// search(std::vector<std::vector<GNode> >& paths, GNode st)
/// Theta* search
/// ==================================================================================
int Grid_planner::thetastar_search(std::vector<std::vector<GNode> >& paths, GNode st){
// prep start and goal nodes
Tpoint gc = getEnclosingCell(goal_.x, goal_.y);
// GNode gl(gc.x*cellwidth_, gc.y*cellheight_);
GNode gl(goal_.x, goal_.y);
// GNode start(st.x*cellwidth_, st.y*cellheight_);
// TODO: Need to test it!!!
GNode start(start_.x,start_.y);
ROS_INFO("Global Planner --> Start: %d, %d --- Goal: %d, %d ",(int)start.x,(int)start.y,(int)gl.x,(int) gl.y);
/// STL THETA*
std::vector<GNode> sol;
std::vector< std::vector<GNode> > path_sol;
ThetaStarSearch<GNode> thetastarsearch( false);
unsigned int SearchCount = 0;
const unsigned int NumSearches = 1;
while(SearchCount < NumSearches)
{
// Set Start and goal states
thetastarsearch.SetStartAndGoalStates( start, gl );
unsigned int SearchState;
unsigned int SearchSteps = 0;
do
{
SearchState = thetastarsearch.SearchStep();
SearchSteps++;
#if DEBUG_LISTS
ROS_INFO("Global Planner --> Steps: %d", (int)SearchSteps);
int len = 0;
ROS_INFO("Open:");
GNode *p = thetastarsearch.GetOpenListStart();
while( p )
{
len++;
#if !DEBUG_LIST_LENGTHS_ONLY
((GNode *)p)->PrintNodeInfo();
#endif
p = thetastarsearch.GetOpenListNext();
}
ROS_INFO("Global Planner --> Open list has %d",len);
len = 0;
ROS_INFO("Global Planner --> Closed");
p = thetastarsearch.GetClosedListStart();
while( p )
{
len++;
#if !DEBUG_LIST_LENGTHS_ONLY
p->PrintNodeInfo();
#endif
p = thetastarsearch.GetClosedListNext();
}
ROS_INFO("Global Planner --> Closed list has %d nodes",len);
#endif
}
while( SearchState == ThetaStarSearch<GNode>::SEARCH_STATE_SEARCHING && SearchSteps < MAX_LIMIT_SEARCH_STEPS_);
if(SearchSteps >= MAX_LIMIT_SEARCH_STEPS_){
ROS_INFO("Grid Planner !! Search did not find a path, MAX_LIMIT_SEARCH_STEPS_ exceeded!");
}
if( SearchState == ThetaStarSearch<GNode>::SEARCH_STATE_SUCCEEDED )
{
ROS_INFO("Global Planner --> Search found goal state");
GNode *node = thetastarsearch.GetSolutionStart();
int steps = 0;
int xs,ys;
node->PrintNodeInfo();
xs=node->x;
ys=node->y;
sol.push_back(GNode(xs,ys));
ROS_INFO("Global Planner --> Nodes: %d,%d", xs,ys);
for( ;; )
{
node = thetastarsearch.GetSolutionNext();
if( !node )
{
break;
}
node->PrintNodeInfo();
xs=node->x;
ys=node->y;
sol.push_back(GNode(xs,ys));
ROS_INFO("Global Planner --> Nodes : %d, %d ",xs, ys);
steps ++;
};
// Once you're done with the solution you can free the nodes up
thetastarsearch.FreeSolutionNodes();
}
else if( SearchState == ThetaStarSearch<GNode>::SEARCH_STATE_FAILED )
{
ROS_INFO("Global Planner --> Search terminated. Did not find goal state");
}
// Display the number of loops the search went through
ROS_INFO("Global Planner --> SearchSteps: %d ",(int)SearchSteps);
SearchCount ++;
thetastarsearch.EnsureMemoryFreed();
}
std::reverse(sol.begin(), sol.end());
paths.push_back(sol);
int path_size=paths.size();
ROS_INFO("Global Planner --> Found [ %d ] paths",path_size);
if (path_size>0){
return 1;
}
else
return 0;
}
int main(int argc, char** argv)
{
glutInit(&argc,argv);
sofa::helper::parse("This is a SOFA application.")
(argc,argv);
sofa::gui::initMain();
sofa::gui::GUIManager::Init(argv[0]);
// The graph root node : gravity already exists in a GNode by default
GNode* groot = new GNode;
groot->setName( "root" );
groot->setGravityInWorld( Vec3dTypes::Coord(0,0,0) );
groot->setDt(0.02);
/*
* collision pipeline: instead of calling ObjectCreator
*/
// collision pipeline
DefaultPipeline* collisionPipeline = new DefaultPipeline;
collisionPipeline->setName("Collision Pipeline");
groot->addObject(collisionPipeline);
// collision detection system
BruteForceDetection* collisionDetection = new BruteForceDetection;
collisionDetection->setName("Collision Detection");
groot->addObject(collisionDetection);
// component to detection intersection
NewProximityIntersection* detectionProximity = new NewProximityIntersection;
detectionProximity->setName("Detection Proximity");
detectionProximity->setAlarmDistance(0.3);
detectionProximity->setContactDistance(0.2);
groot->addObject(detectionProximity);
// contact manager
DefaultContactManager* contactManager = new DefaultContactManager;
contactManager->setName("Contact Manager");
contactManager->setDefaultResponseType("default");
groot->addObject(contactManager);
// tree collision group
TreeCollisionGroupManager* collisionGroupManager = new TreeCollisionGroupManager;
collisionGroupManager->setName("Collision Group Manager");
groot->addObject(collisionGroupManager);
/*
* Sub nodes: DRE
*/
GNode* dreNode = new GNode;
dreNode->setName("DRE");
GNode* cylNode = new GNode;
cylNode->setName("Cylinder");
// solvers
typedef CGLinearSolver<GraphScatteredMatrix, GraphScatteredVector> CGLinearSolverGraph;
EulerImplicitSolver* implicitSolver = new EulerImplicitSolver;
CGLinearSolverGraph* cgLinearSolver = new CGLinearSolverGraph;
implicitSolver->setName("eulerImplicitSolver");
implicitSolver->f_rayleighStiffness.setValue(0.01);
//implicitSolver->f_rayleighMass.setValue(0.1);
implicitSolver->f_printLog = false;
cgLinearSolver->setName("cgLinearSolver");
cgLinearSolver->f_maxIter.setValue(25);
cgLinearSolver->f_tolerance.setValue(1.0e-9);
cgLinearSolver->f_smallDenominatorThreshold.setValue(1.0e-9);
// sparse grid topology
sofa::component::topology::SparseGridTopology* sparseGridTopology = new sofa::component::topology::SparseGridTopology;
sparseGridTopology->setName("SparseGrid Topology");
std::string topologyFilename = "mesh/truthcylinder1.obj";
sparseGridTopology->load(topologyFilename.c_str());
sparseGridTopology->setN(Vec<3,int>(8, 6, 6));
// mechanical object
typedef MechanicalObject< Vec3dTypes > MechanicalObject3d;
MechanicalObject3d* mechanicalObject = new MechanicalObject3d;
mechanicalObject->setTranslation(0,0,0);
mechanicalObject->setRotation(0,0,0);
mechanicalObject->setScale(1,1,1);
// mass
typedef UniformMass< Vec3dTypes,double > UniformMass3d;
UniformMass3d* uniformMass = new UniformMass3d;
uniformMass->setTotalMass(5);
// hexahedron fem forcefield
typedef HexahedronFEMForceField< Vec3dTypes > HexahedronFEMForceField3d;
HexahedronFEMForceField3d* hexaFEMFF = new HexahedronFEMForceField3d;
hexaFEMFF->setName("HexahedronFEM Forcefield");
hexaFEMFF->setMethod(HexahedronFEMForceField3d::POLAR);
hexaFEMFF->setPoissonRatio(0.3);
hexaFEMFF->setYoungModulus(250);
// quad bending springs
typedef sofa::component::interactionforcefield::QuadBendingSprings< Vec3dTypes > QuadBendingSprings3d;
QuadBendingSprings3d* quadBendingSprings = new QuadBendingSprings3d;
quadBendingSprings->setName("QuadBending springs");
quadBendingSprings->setStiffness(1000);
quadBendingSprings->setDamping(1);
quadBendingSprings->setObject1(mechanicalObject);
// fixed constraint
typedef FixedConstraint< StdVectorTypes<Vec<3,double>,Vec<3,double>,double> > FixedConstraint3d;
FixedConstraint3d* fixedConstraints = new FixedConstraint3d;
fixedConstraints->setName("Box Constraints");
fixedConstraints->addConstraint(0);
fixedConstraints->addConstraint(1); fixedConstraints->addConstraint(2); fixedConstraints->addConstraint(6); fixedConstraints->addConstraint(12); fixedConstraints->addConstraint(17); fixedConstraints->addConstraint(21); fixedConstraints->addConstraint(22);
fixedConstraints->addConstraint(24); fixedConstraints->addConstraint(25); fixedConstraints->addConstraint(26); fixedConstraints->addConstraint(30); fixedConstraints->addConstraint(36); fixedConstraints->addConstraint(41); fixedConstraints->addConstraint(46); fixedConstraints->addConstraint(47);
fixedConstraints->addConstraint(50); fixedConstraints->addConstraint(51); fixedConstraints->addConstraint(52); fixedConstraints->addConstraint(56); fixedConstraints->addConstraint(62); fixedConstraints->addConstraint(68); fixedConstraints->addConstraint(73); fixedConstraints->addConstraint(74);
fixedConstraints->addConstraint(77); fixedConstraints->addConstraint(78); fixedConstraints->addConstraint(79); fixedConstraints->addConstraint(83); fixedConstraints->addConstraint(89); fixedConstraints->addConstraint(95); fixedConstraints->addConstraint(100); fixedConstraints->addConstraint(101);
fixedConstraints->addConstraint(104); fixedConstraints->addConstraint(105); fixedConstraints->addConstraint(106); fixedConstraints->addConstraint(110); fixedConstraints->addConstraint(116); fixedConstraints->addConstraint(122); fixedConstraints->addConstraint(127); fixedConstraints->addConstraint(128);
fixedConstraints->addConstraint(131); fixedConstraints->addConstraint(132); fixedConstraints->addConstraint(133); fixedConstraints->addConstraint(137); fixedConstraints->addConstraint(143); fixedConstraints->addConstraint(149); fixedConstraints->addConstraint(154); fixedConstraints->addConstraint(155);
fixedConstraints->addConstraint(158); fixedConstraints->addConstraint(159); fixedConstraints->addConstraint(160); fixedConstraints->addConstraint(164); fixedConstraints->addConstraint(170); fixedConstraints->addConstraint(175); fixedConstraints->addConstraint(180); fixedConstraints->addConstraint(181);
fixedConstraints->addConstraint(184); fixedConstraints->addConstraint(185); fixedConstraints->addConstraint(186); fixedConstraints->addConstraint(190); fixedConstraints->addConstraint(196); fixedConstraints->addConstraint(201); fixedConstraints->addConstraint(206); fixedConstraints->addConstraint(205);
// visual node
GNode* cylVisualNode = new GNode;
cylVisualNode->setName("Cylinder Visual");
OglModel* cylOglModel = new OglModel;
cylOglModel->setName("Visual");
std::string visualFilename = "mesh/truthcylinder1.obj";
cylOglModel->setFilename(DataRepository.getFile(visualFilename).c_str());
cylOglModel->setColor("red");
typedef BarycentricMapping< Vec3dTypes, ExtVec3fTypes > BarycentricMapping3d_to_Ext3f;
BarycentricMapping3d_to_Ext3f* barycentricMapping = new BarycentricMapping3d_to_Ext3f(mechanicalObject, cylOglModel);
barycentricMapping->setName("Barycentric");
//barycentricMapping->setPathInputObject("../..");
//barycentricMapping->setPathOutputObject("Visual");
// collision node
GNode* cylCollisionNode = new GNode;
cylCollisionNode->setName("Cylinder Collision");
sofa::component::container::MeshLoader* cylSurfMeshLoader = new sofa::component::container::MeshLoader;
std::string collisionFilename = "mesh/truthcylinder1.msh";
cylSurfMeshLoader->setFilename(DataRepository.getFile(collisionFilename).c_str());
MeshTopology* cylSurfaceTopology = new MeshTopology;
MechanicalObject3d* cylSurfMechanicalObject = new MechanicalObject3d;
TriangleModel* triangleModel = new TriangleModel;
typedef BarycentricMapping< Vec3dTypes, Vec3dTypes > BarycentricMechanicalMapping3d_to_3d;
BarycentricMechanicalMapping3d_to_3d* cylSurfBarycentricMapping = new BarycentricMechanicalMapping3d_to_3d(mechanicalObject, cylSurfMechanicalObject);
//cylSurfBarycentricMapping->setPathInputObject("../..");
//cylSurfBarycentricMapping->setPathOutputObject("..");
cylVisualNode->addObject(cylOglModel);
cylVisualNode->addObject(barycentricMapping);
cylCollisionNode->addObject(cylSurfMeshLoader);
cylCollisionNode->addObject(cylSurfaceTopology);
cylCollisionNode->addObject(cylSurfMechanicalObject);
cylCollisionNode->addObject(triangleModel);
cylCollisionNode->addObject(cylSurfBarycentricMapping);
quadBendingSprings->setObject2(cylSurfMechanicalObject);
cylNode->addObject(implicitSolver);
cylNode->addObject(cgLinearSolver);
cylNode->addObject(sparseGridTopology);
cylNode->addObject(mechanicalObject);
cylNode->addObject(uniformMass);
cylNode->addObject(hexaFEMFF);
cylNode->addObject(quadBendingSprings);
cylNode->addObject(fixedConstraints);
cylNode->addChild(cylVisualNode);
cylNode->addChild(cylCollisionNode);
dreNode->addChild(cylNode);
groot->addChild(dreNode);
#ifdef SOFA_GPU_CUDA
sofa::gpu::cuda::mycudaInit();
#endif
// Init the scene
sofa::simulation::tree::getSimulation()->init(groot);
groot->setAnimate(false);
groot->setShowNormals(false);
groot->setShowInteractionForceFields(false);
groot->setShowMechanicalMappings(false);
groot->setShowCollisionModels(false);
groot->setShowBoundingCollisionModels(false);
groot->setShowMappings(false);
groot->setShowForceFields(true);
groot->setShowWireFrame(true);
groot->setShowVisualModels(true);
//=======================================
// Run the main loop
sofa::gui::GUIManager::MainLoop(groot);
return 0;
}
/* Appends an empty DEAT node just after
* the last NAME, SEX or BIRT node
*/
void GIndiEntry::appendDeathNode() {
GNode * n = _birthNode ? _birthNode : _sexNode ? _sexNode : _nameNode;
// Append BIRT node here
_deathNode = n->insertNext(new GNode(ENTRY_DEATH));
}
int
StructProcessor::getFieldSize(GNode* node)
{
GNode* sizeNode = globalASTTreePointer->findNodeByIndex(node->getProperty("size")->mNodeProperty);
return Util::stringToInt(sizeNode->getProperty("low")->mStringProperty);
}
/**
* Destructor for node - removes edge controls on incidient edges and disconnects item from scene
*/
GNode::~GNode()
{
graph()->invalidateRanking();
if ( ( isEdgeControl() || isEdgeLabel())
&& isNotNullP( firstPred())
&& isNotNullP( firstSucc())
&& isNotNullP( firstPred()->pred())
&& isNotNullP( firstSucc()->succ()))
{
GRAPH_ASSERTD( areEqP( firstPred()->style(), firstSucc()->style()),
"Different styles on the same edge");
GEdge *e = graph()->newEdge( firstPred()->pred(), firstSucc()->succ());
e->setStyle( firstPred()->style());
} else if ( isSimple())
{
QList< GNode *> nodes;
GEdge* edge;
Marker m = graph()->newMarker();
for ( edge = firstSucc(); isNotNullP( edge); edge = edge->nextSucc())
{
edge->item()->adjust();
GNode* succ = edge->succ();
while ( succ->isEdgeControl() || succ->isEdgeLabel())
{
assert( isNotNullP( succ->firstSucc()));
if ( succ->mark( m))
{
nodes << succ;
}
succ = succ->firstSucc()->succ();
}
}
for ( edge = firstPred(); isNotNullP( edge); edge = edge->nextPred())
{
if ( edge->isSelf()) // We've already processed this one in previous loop
continue;
edge->item()->adjust();
GNode* pred = edge->pred();
while ( pred->isEdgeControl() || pred->isEdgeLabel())
{
assert( isNotNullP( pred->firstPred()));
if ( pred->mark( m))
{
nodes << pred;
}
pred = pred->firstPred()->pred();
}
}
foreach ( GNode *n, nodes)
{
graph()->deleteNode( n);
}
graph()->freeMarker( m);
}
/**
* @brief Builds the SOFA physical model and data and instanciates the SofaThread class
*
* @param pMesh : pointer to the triangular mesh
* @param service : pointer to the SofaService object
*/
void SofaBusiness::loadMesh(::fwData::Mesh::sptr pMesh, ::fwServices::IService::sptr service)
{
// Default value : 100 millisecond
timeStepAnimation = 100;
// Creation du noeud principal (correspond a la scene)
groot = new GNode;
groot->setName( "root" );
groot->setGravityInWorld( Coord3(0,-10,0) ); // on definit la gravite
// Creation d'un solveur (permet de calculer les nouvelles positions des particules)
CGImplicitSolver* solver = new CGImplicitSolver;
groot->addObject(solver);
// On definit les degres de liberte du Tetrahedre (coordonnees, vitesses...)
MechanicalObject3* DOF = new MechanicalObject3;
groot->addObject(DOF);
DOF->resize(4);
DOF->setName("DOF");
VecCoord3& x = *DOF->getX(); // On definit les coordonnees
x[0] = Coord3(0,10,0);
x[1] = Coord3(10,0,0);
x[2] = Coord3(-10*0.5,0,10*0.866);
x[3] = Coord3(-10*0.5,0,-10*0.866);
// On definit la masse du Tetrahedre
UniformMass3* mass = new UniformMass3;
groot->addObject(mass);
mass->setMass(2);
mass->setName("mass");
// On definit le maillage du Tetrahedre (peut etre compose de lignes, triangles...)
MeshTopology* topology = new MeshTopology;
topology->setName("mesh topology");
groot->addObject( topology );
topology->addTetra(0,1,2,3);
// On definit les contraintes du Tetrahedre
FixedConstraint3* constraints = new FixedConstraint3;
constraints->setName("constraints");
groot->addObject(constraints);
constraints->addConstraint(0);
// On definit les forces du Tetrahedre
TetrahedronFEMForceField3* fem = new TetrahedronFEMForceField3;
fem->setName("FEM");
groot->addObject(fem);
fem->setMethod("polar");
fem->setUpdateStiffnessMatrix(true);
fem->setYoungModulus(6);
// Creation d'un noeud enfant (a la scene) pour accueillir le visuel du fichier .trian
GNode* skin = new GNode("skin",groot);
// Creation de la partie visuel du fichier .trian
OglModelF4S *visual = new OglModelF4S();
visual->setName( "visual" );
visual->loadMesh(pMesh);
visual->setColor("red");
visual->applyScale(1);
skin->addObject(visual);
// Creation du mapping entre les deux objets (effectue une liaison entre deux objets
// pour que le rendu suive le mouvement de la partie simulation)
BarycentricMapping3_to_Ext3* mapping = new BarycentricMapping3_to_Ext3(DOF, visual);
mapping->setName( "mapping" );
skin->addObject(mapping);
// Initialisation de la scene
getSimulation()->init(groot);
// Create Thread
meshs = new std::vector<fwData::Mesh::sptr>();
meshs->push_back(pMesh);
thread = new SofaThread(this, meshs, service);
// Translate pointer between sofa and fw4spl
translationPointer(visual, pMesh);
}
void GPath::FindTile(GNode* gnode, int x, int y, int dir) {
if(TileInBounds(x, y)) {
//check whether we already have it
GNode* node = GetNode(x, y);
if (NULL == node) {
int type = GetTileType(x, y);
if (!IsWrongType(type)) {
GNode* node = new GNode(x, y);
node->SetParent(gnode);
node->CalcF(DestX, DestY, dir);
App->Log << "**********\n";
App->Log << "Open tile:\n";
App->Log << "x: " << node->GetX();
App->Log << " y: " << node->GetY();
App->Log << " G: " << node->GetG();
App->Log << " H: " << node->GetH();
App->Log << " F: " << node->GetF() << std::endl;
SetNode(node);
Open.push(node);
}
} // null == node
else if (node->IsOpen()) {
//check if this path is better
int pts;
if (DIR_HV == dir)
pts = PTS_HV;
else
pts = PTS_DIAG;
if (gnode->GetG() + pts < node->GetG()) {
node->SetParent(gnode);
node->CalcF(DestX, DestY, dir);
}
} //IsOpen()
} // in bounds
}
void ReducibleIfAnalyzer::analyzeNode(GNode *node, const vector<int> &context)
{
GNode *testingNode = NodeProcessor::getOperand(node, 0);
GNode *ifIfNode = NodeProcessor::getOperand(node, 2);
GNode *logicLeftNode, *logicRightNode;
if(NodeProcessor::isLogicAnd(testingNode))
{
logicLeftNode = NodeProcessor::getOperand(testingNode, 0);
logicRightNode = NodeProcessor::getOperand(testingNode, 1);
if(NodeProcessor::isCompareOperation(logicLeftNode))
{
LogicUnit temp;
if(NULL != logicLeftNode)
temp.ope = logicLeftNode->getTreeCode();
GNode *op0Node = NodeProcessor::getOperand(logicLeftNode, 0);
GNode *op1Node = NodeProcessor::getOperand(logicLeftNode, 1);
temp.left = NodeProcessor::getSimpleOperation(op0Node);
temp.right = NodeProcessor::getSimpleOperation(op1Node);
vec_LogicUnit.push_back(temp);
if(NodeProcessor::isCompareOperation(logicRightNode))
{
if(NULL != logicRightNode)
temp.ope = logicRightNode->getTreeCode();
GNode *op0Node = NodeProcessor::getOperand(logicRightNode, 0);
GNode *op1Node = NodeProcessor::getOperand(logicRightNode, 1);
temp.left = NodeProcessor::getSimpleOperation(op0Node);
temp.right = NodeProcessor::getSimpleOperation(op1Node);
vec_LogicUnit.push_back(temp);
}
}
}
else
{
return ;
}
if(NodeProcessor::isCondExpr(ifIfNode))
{
GNode *ifIfTesting = NodeProcessor::getOperand(ifIfNode, 0);
if(NodeProcessor::isCompareOperation(ifIfTesting))
{
LogicUnit temp;
if(NULL != ifIfTesting)
temp.ope = ifIfTesting->getTreeCode();
GNode *op0Node = NodeProcessor::getOperand(ifIfTesting, 0);
GNode *op1Node = NodeProcessor::getOperand(ifIfTesting, 1);
temp.left = NodeProcessor::getSimpleOperation(op0Node);
temp.right = NodeProcessor::getSimpleOperation(op1Node);
vec_LogicUnit.push_back(temp);
GNode *ifIfElseNode = NodeProcessor::getOperand(ifIfNode, 2);
if(NodeProcessor::isCondExpr(ifIfElseNode))
{
if(vec_LogicUnit[0].anti(vec_LogicUnit[2]))
{
int lineNum;
if(NULL != node)
lineNum = Util::stringToInt(node->getProperty("line")->mStringProperty);
Logger::a("ReducibleAnalyzer") << "if statement is reducible" \
<< SrcManager::getInstance().getFullFileName() << lineNum \
<< SrcManager::getInstance().getLine(lineNum) << endl;
stringstream reportMsgStream;
reportMsgStream << "ReducibleAnalyzer: if statement is reducible" << endl;
string reportMsg = reportMsgStream.str();
ReportManager::getInstance().insertReport(SrcManager::getInstance().getFullFileName(), lineNum, reportMsg);
}
}
}
}
}
/**********************************************************************
* displayLevel - level order traversal and display
***********************************************************************/
void displayLevel(GNode* &aHead)
{
const int MAX = 150;
GNode* queue[MAX];
GNode* temp = aHead;
int front = 0;
int back = 0;
int saveBack = 1;
int generation = 0;
bool showGeneration = true;
while (temp->getINum() != "I1")
temp = temp->getNext();
queue[back++] = temp;
cout << "The Ancestors of " << temp->getFName() << " " << temp->getLName() << ":" << endl;
while (front != back)
{
temp = queue[front];
if (front == saveBack)
{
generation++;
saveBack = back;
if (generation == 1)
cout << "Parents:" << endl;
else if (generation == 2)
cout << "Grandparents:" << endl;
else if (generation == 3)
cout << "Great Grandparents:" << endl;
else if (generation == 4)
cout << "2nd Great Grandparents:" << endl;
else if (generation == 5)
cout << "3rd Great Grandparents:" << endl;
else if (generation == 6)
cout << "4th Great Grandparents:" << endl;
else if (generation == 7)
cout << "5th Great Grandparents:" << endl;
}
front = (front + 1) % MAX;
if (temp != NULL)
{
if (front != 1)
{
cout << " ";
if (temp->getFName() != "")
cout << temp->getFName();
if (temp->getLName() != "" && temp->getFName() != "")
cout << " " << temp->getLName();
else
cout << temp->getLName();
if (temp->getDay() != "" || temp->getMonth() != ""
|| temp->getYear() != "")
cout<< ", b. ";
if (temp->getDay() != "")
cout << temp->getDay() << " ";
if (temp->getMonth() != "")
cout << temp->getMonth() << " ";
if (temp->getYear() != "")
cout << temp->getYear();
cout << endl;
}
queue[back] = temp->getFather();
back = (back + 1) % MAX;
queue[back] = temp->getMother();
back = (back + 1) % MAX;
}
}
}
/**********************************************************************
* displayList - displays a linked list, starting with the head argument.
***********************************************************************/
void displayList(GNode* &aHead)
{
GNode* ptr = aHead;
while (ptr != NULL)
{
if (ptr->getFName() != "")
cout << ptr->getFName();
if (ptr->getLName() != "" && ptr->getFName() != "")
cout << " " << ptr->getLName();
else
cout << ptr->getLName();
if (ptr->getDay() != "" || ptr->getMonth() != "" || ptr->getYear() != "")
cout<< ", b. ";
if (ptr->getDay() != "")
cout << ptr->getDay() << " ";
if (ptr->getMonth() != "")
cout << ptr->getMonth() << " ";
if (ptr->getYear() != "")
cout << ptr->getYear();
cout << endl;
ptr = ptr->getNext();
}
}
/**********************************************************************
* insert - a sorted insertion
***********************************************************************/
void insert(GNode* &listHead, GNode* &aPtr)
{
//aPtr not rdy to be inserted. Occurs on first individual in parseFile()
if(aPtr == NULL)
return;
//first insertion
if(listHead == NULL)
{
listHead = aPtr;
return;
}
string tempLName = aPtr->getLName();
string ptrLName;
for (int i = 0; i < tempLName.length(); i++)
ptrLName += toupper(tempLName[i]);
tempLName = listHead->getLName();
string headLName;
for (int i = 0; i < tempLName.length(); i++)
headLName += toupper(tempLName[i]);
// head insert
if (ptrLName < headLName)
{
aPtr->setNext(listHead);
listHead = aPtr;
return;
}
else if (ptrLName == headLName && aPtr->getFName() < listHead->getFName())
{
aPtr->setNext(listHead);
listHead = aPtr;
return;
}
GNode* p = listHead;
GNode* c = listHead->getNext();
string cLName;
//traverse, comparing last names,then first Name, then date
while (c != NULL)
{
tempLName = c->getLName();
cLName.clear();
for (int i = 0; i < tempLName.length(); i++)
cLName += toupper(tempLName[i]);
if (ptrLName > cLName)
{
p = c;
c = c->getNext();
}
else if (ptrLName == cLName && aPtr->getFName() > c->getFName())
{
p = c;
c = c->getNext();
}
else if (ptrLName == cLName && aPtr->getFName() == c->getFName()
&& aPtr->getYear() > c->getYear())
{
p = c;
c = c->getNext();
}
else
break;
}
p->setNext(aPtr);
aPtr->setNext(c);
}
/**********************************************************************
* parseFile -
***********************************************************************/
void parseFile(ifstream &inFile, GNode* &aHead)
{
GNode* ptr = NULL; // traversal pointer
string temp1; // assign every word in file to temp1
string temp2; // helper string in parse logic
int i = 0;
while (true)
{
getline (inFile, temp1);
if (temp1 == "1 DIV Y")
break;
temp2 = temp1.substr(0, 4); // create substring
//We've reached a new node. Insert previous Node, then start new Node.
if (temp2 == "0 @I")
{
insert(aHead, ptr);
temp2.clear();
temp1.erase(0, 3);
//create INum string
while (temp1[0] != '@')
{
temp2 += temp1[0];
temp1.erase(0,1);
}
ptr = new GNode;
ptr->setINum(temp2);
}
//parse and store first Name
else if (temp2 == "2 GI")
{
temp1.erase(0,7);
ptr->setFName(temp1);
}
//parse and store last Name
else if (temp2 == "2 SU")
{
temp1.erase(0, 7);
ptr->setLName(temp1);
}
//parse and store Birth Date
else if (temp2 == "1 BI")
{
getline(inFile, temp1);
if (temp1.find("DATE") != std::string::npos) // make sure date exists
{
temp1.erase(0,7);
temp2.clear();
if (temp1.length() != 0)
{
for (int i = 0; i < temp1.length(); i++)
{
if (isalpha(temp1[i]))
{
temp2 += temp1[i];
temp1.erase(i, 1);
i--;
}
}
ptr->setMonth(temp2);
temp2.erase();
if (temp1[0] == ' ')
temp1.erase(0, 1);
while (temp1.length() != 0 && (isdigit(temp1[0])
|| temp1[0] == '/'))
{
temp2 += temp1[0];
temp1.erase(0, 1);
}
if (temp2.length() > 2)
ptr->setYear(temp2);
else
ptr->setDay(temp2);
if (temp1.length() != 0)
{
temp1.erase(0, 2);
temp2.clear();
while (temp1.length() != 0)
{
temp2 += temp1[0];
temp1.erase(0, 1);
}
ptr->setYear(temp2);
}
}
}
}
}
insert(aHead, ptr);
// Start Algorithm to Build Binary Tree
string fatherINum;
string motherINum;
string childINum;;
ptr = aHead; // reset traversal pointer. Use to traverse children
GNode* ptr2 = ptr; // another traversal pointer. Use to traverse to parents.
while (true)
{
getline(inFile, temp1);
if (temp1 == "1 CALN 0567673")
break;
temp2 = temp1.substr(0, 4); // create substring
if (temp2 == "1 HU") // check for father
{
temp1.erase(0,8);
temp2.clear();
while (temp1[0] != '@')
{
temp2 += temp1[0];
temp1.erase(0,1);
}
fatherINum = temp2;
}
else if (temp2 == "1 WI") // check for mother
{
temp1.erase(0,8);
temp2.clear();
while (temp1[0] != '@')
{
temp2 += temp1[0];
temp1.erase(0,1);
}
motherINum = temp2;
}
else if (temp2 == "1 CH") // node whose mother/father need to be set
{
temp1.erase(0,8);
temp2.clear();
while (temp1[0] != '@')
{
temp2 += temp1[0];
temp1.erase(0,1);
}
childINum = temp2;
ptr = aHead;
ptr2 = aHead;
while (childINum != ptr->getINum())
ptr = ptr->getNext();
while (fatherINum != "" && fatherINum != ptr2->getINum())
ptr2 = ptr2->getNext();
if (fatherINum != "")
ptr->setFather(ptr2);
ptr2 = aHead;
while (motherINum != "" && motherINum != ptr2->getINum())
ptr2 = ptr2->getNext();
if (motherINum != "")
ptr->setMother(ptr2);
fatherINum.clear();
motherINum.clear();
}
}
}
void
EdgeItem::keyPressEvent(QKeyEvent *event)
{
int key = event->key();
GNode *n = NULL;
GEdge *e = NULL;
VEdge vedge( edge());
NavSector sector = edge()->graph()->nodeNavigationSector();
GNode *curr_node = edge()->graph()->nodeInFocus();
NodeNav node_nav( curr_node, sector);
switch( key)
{
case Qt::Key_Up:
if ( isNotNullP( curr_node)
&& ( sector == LEFT_SECTOR || sector == RIGHT_SECTOR))
{
e = node_nav.edgeInDir( edge(), NAV_DIR_UP);
} else
{
n = vedge.nodeUp();
}
break;
case Qt::Key_Down:
if ( isNotNullP( curr_node)
&& ( sector == LEFT_SECTOR || sector == RIGHT_SECTOR) )
{
e = node_nav.edgeInDir( edge(), NAV_DIR_DOWN);
} else
{
n = vedge.nodeDown();
}
break;
case Qt::Key_Left:
if ( isNotNullP( curr_node)
&& ( sector == TOP_SECTOR || sector == BOTTOM_SECTOR) )
{
e = node_nav.edgeInDir( edge(), NAV_DIR_LEFT);
} else
{
n = vedge.nodeLeft();
}
break;
case Qt::Key_Right:
if ( isNotNullP( curr_node)
&& ( sector == TOP_SECTOR || sector == BOTTOM_SECTOR) )
{
e = node_nav.edgeInDir( edge(), NAV_DIR_RIGHT);
} else
{
n = vedge.nodeRight();
}
break;
default:
break;
}
if ( isNotNullP( n))
{
if ( edge()->graph()->view()->isContext())
{
edge()->graph()->emptySelection();
edge()->graph()->selectNode( n);
edge()->graph()->view()->findContext();
}
edge()->graph()->view()->focusOnNode( n, true);
scene()->clearFocus();
scene()->clearSelection();
n->item()->setFocus();
n->item()->setSelected( true);
} else if ( isNotNullP( e))
{
// Get focus on edge
scene()->clearFocus();
scene()->clearSelection();
e->item()->setFocus();
e->item()->setSelected( true);
}
//QGraphicsItem::keyPressEvent( event);
}
string
StructProcessor::getFieldName(GNode* node)
{
GNode* idNode = globalASTTreePointer->findNodeByIndex(node->getProperty("name")->mNodeProperty);
return idNode->getProperty("strg")->mStringProperty;
}