Boolean ListBoxItem :: targetDrop ( IDMTargetDropEvent &event ) { IListBox *tgtLB = (IListBox*)( event.window() ); // Turn off target emphasis: ListBoxItemProvider *provider = (ListBoxItemProvider*)( tgtLB->itemProvider() ); provider -> drawEmphasis( tgtLB, TgtLocation( after, nil ) ); // Calculate where dropped on: TgtLocation dropLoc = targetLocation( tgtLB, event.dropPosition() ); // Add or replace, based on drop location: switch ( dropLoc.type ) { case before: tgtLB -> add( dropLoc.index, contents() ); break; case on: tgtLB -> setItemText( dropLoc.index, contents() ); break; case after: tgtLB -> add( dropLoc.index + 1, contents() ); break; } // If source and target are the same and item moved // forward, update source index. IDMTargetOperation::Handle tgtOp = IDMTargetOperation::targetOperation(); if ( tgtOp->sourceWindow() == event.window() && tgtOp->operation() == IDMOperation::move ) { IDMItem::Handle srcItem = IDMItem::sourceItemFor( tgtOp->item( 1 ) ); unsigned srcIndex = (unsigned)( srcItem->object() ); if ( dropLoc.type != on && dropLoc.index < srcIndex ) srcItem->setObject( (void*)( srcIndex + 1 ) ); } return true; }
void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) { { float playstartf[3] = {0,0,100}; if (bspLoader.findVectorByName(&playstartf[0],"info_player_start")) { printf("found playerstart\n"); } else { if (bspLoader.findVectorByName(&playstartf[0],"info_player_deathmatch")) { printf("found deatchmatch start\n"); } } btVector3 playerStart (playstartf[0],playstartf[1],playstartf[2]); playerStart[2] += 20.f; //start a bit higher playerStart *= scaling; //progressBegin("Loading bsp"); for (int i=0;i<bspLoader.m_numleafs;i++) { printf("Reading bspLeaf %i from total %i (%f procent)\n",i, bspLoader.m_numleafs,(100.f*(float)i/float(bspLoader.m_numleafs)) ); bool isValidBrush = false; BSPLeaf& leaf = bspLoader.m_dleafs[i]; for (int b=0;b<leaf.numLeafBrushes;b++) { btAlignedObjectArray<btVector3> planeEquations; int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b]; BSPBrush& brush = bspLoader.m_dbrushes[brushid]; if (brush.shaderNum!=-1) { if (bspLoader.m_dshaders[ brush.shaderNum ].contentFlags & BSPCONTENTS_SOLID) { brush.shaderNum = -1; for (int p=0;p<brush.numSides;p++) { int sideid = brush.firstSide+p; BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid]; int planeid = brushside.planeNum; BSPPlane& plane = bspLoader.m_dplanes[planeid]; btVector3 planeEq; planeEq.setValue( plane.normal[0], plane.normal[1], plane.normal[2], scaling*-plane.dist); planeEquations.push_back(planeEq); isValidBrush=true; } if (isValidBrush) { btAlignedObjectArray<btVector3> vertices; btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices); bool isEntity = false; btVector3 entityTarget(0.f,0.f,0.f); addConvexVerticesCollider(vertices,isEntity,entityTarget); } } } } } #define USE_ENTITIES #ifdef USE_ENTITIES { int i; for (i=0;i<bspLoader.m_num_entities;i++) { const BSPEntity& entity = bspLoader.m_entities[i]; const char* cl = bspLoader.getValueForKey(&entity,"classname"); if ( !strcmp( cl, "trigger_push" ) ) { btVector3 targetLocation(0.f,0.f,0.f); cl = bspLoader.getValueForKey(&entity,"target"); if ( strcmp( cl, "" ) ) { //its not empty so ... /* //lookup the target position for the jumppad: const BSPEntity* targetentity = bspLoader.getEntityByValue( "targetname" , cl ); if (targetentity) { if (bspLoader.getVectorForKey( targetentity , "origin",&targetLocation[0])) { } } */ cl = bspLoader.getValueForKey(&entity,"model"); if ( strcmp( cl, "" ) ) { // add the model as a brush if (cl[0] == '*') { int modelnr = atoi(&cl[1]); if ((modelnr >=0) && (modelnr < bspLoader.m_nummodels)) { const BSPModel& model = bspLoader.m_dmodels[modelnr]; for (int n=0;n<model.numBrushes;n++) { btAlignedObjectArray<btVector3> planeEquations; bool isValidBrush = false; //convert brush const BSPBrush& brush = bspLoader.m_dbrushes[model.firstBrush+n]; { for (int p=0;p<brush.numSides;p++) { int sideid = brush.firstSide+p; BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid]; int planeid = brushside.planeNum; BSPPlane& plane = bspLoader.m_dplanes[planeid]; btVector3 planeEq; planeEq.setValue( plane.normal[0], plane.normal[1], plane.normal[2], scaling*-plane.dist); planeEquations.push_back(planeEq); isValidBrush=true; } if (isValidBrush) { btAlignedObjectArray<btVector3> vertices; btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices); bool isEntity=true; addConvexVerticesCollider(vertices,isEntity,targetLocation); } } } } } else { printf("unsupported trigger_push model, md3 ?\n"); } } } } } } #endif //USE_ENTITIES //progressEnd(); } }
void stewartGoughNodeNamespace::StewartGoughNode::onSetInstruction(const rexos_module::SetInstructionGoalConstPtr &goal){ REXOS_INFO_STREAM("parsing hardwareStep: " << goal->json); Json::Reader reader; Json::Value equipletStepNode; reader.parse(goal->json, equipletStepNode); rexos_datatypes::EquipletStep equipletStep(equipletStepNode); rexos_module::SetInstructionResult result; result.OID = goal->OID; rexos_stewart_gough::StewartGoughLocation origin; // the rotation of the axis of the tangent space against the normal space in radians double rotationX, rotationY, rotationZ = 0; // determine the position of the origin and the rotation of the axis switch(equipletStep.getOriginPlacement().getOriginPlacementType()) { case rexos_datatypes::OriginPlacement::RELATIVE_TO_IDENTIFIER: { // set the origin to the result of the lookup if(equipletStep.getOriginPlacement().getLookupResult().isMember("location") == false) { throw std::runtime_error("lookup result does not contain location"); } else if(equipletStep.getOriginPlacement().getLookupResult().isMember("rotation") == false) { throw std::runtime_error("lookup result does not contain rotation"); } Json::Value location = equipletStep.getOriginPlacement().getLookupResult()["location"]; origin.location.set(location["x"].asDouble(), location["y"].asDouble(), location["z"].asDouble()); origin.location = convertToModuleCoordinate(origin.location); Json::Value rotation = equipletStep.getOriginPlacement().getLookupResult()["rotation"]; rotationZ = rotation["z"].asDouble(); break; } case rexos_datatypes::OriginPlacement::RELATIVE_TO_CURRENT_POSITION: { // set the origin to the current position of the effector origin.location.set(stewartGough->getEffectorLocation().location.x, stewartGough->getEffectorLocation().location.y, stewartGough->getEffectorLocation().location.z); origin.rotationX = stewartGough->getEffectorLocation().rotationX; origin.rotationY = stewartGough->getEffectorLocation().rotationY; origin.rotationZ = stewartGough->getEffectorLocation().rotationZ; break; } case rexos_datatypes::OriginPlacement::RELATIVE_TO_MODULE_ORIGIN: { // set the origin to the origin of the module (eg set it to 0, 0, 0) origin.location.set(0, 0, 0); origin.rotationX = 0; origin.rotationY = 0; origin.rotationZ = 0; break; } case rexos_datatypes::OriginPlacement::RELATIVE_TO_EQUIPLET_ORIGIN: { // set the origin to the origin of the module (eg set it to 0, 0, 0) origin.location = convertToModuleCoordinate(Vector3(0, 0, 0)); origin.rotationX = 0; origin.rotationY = 0; origin.rotationZ = 0; break; } } // get the vector from the instruction data Json::Value instructionData = equipletStep.getInstructionData(); if(instructionData.isMember("move") == false) { throw std::runtime_error("instruction data does not contain move"); } Json::Value moveCommand = equipletStep.getInstructionData()["move"]; Vector3 offsetVector; if(moveCommand.isMember("x")) offsetVector.x = moveCommand["x"].asDouble(); else offsetVector.x = stewartGough->getEffectorLocation().location.x; if(moveCommand.isMember("y")) offsetVector.y = moveCommand["y"].asDouble(); else offsetVector.y = stewartGough->getEffectorLocation().location.y; if(moveCommand.isMember("z")) offsetVector.z = moveCommand["z"].asDouble(); else offsetVector.z = stewartGough->getEffectorLocation().location.z; // get the max acceleration double maxAcceleration; if(moveCommand.isMember("maxAcceleration") == false) { REXOS_WARN("move command does not contain maxAcceleration, assuming "); maxAcceleration = 50.0; } else { maxAcceleration = moveCommand["maxAcceleration"].asDouble(); } // get the rotation from the instruction data if(instructionData.isMember("rotate") == false) { throw std::runtime_error("instruction data does not contain rotate"); } Json::Value rotateCommand = equipletStep.getInstructionData()["rotate"]; double targetRotationX, targetRotationY, targetRotationZ; if(rotateCommand.isMember("x")) targetRotationX = rotateCommand["x"].asDouble(); else targetRotationX = stewartGough->getEffectorLocation().rotationX; if(rotateCommand.isMember("y")) targetRotationY = rotateCommand["y"].asDouble(); else targetRotationY = stewartGough->getEffectorLocation().rotationY; if(rotateCommand.isMember("z")) targetRotationZ = rotateCommand["z"].asDouble(); else targetRotationZ = stewartGough->getEffectorLocation().rotationZ; // calculate the target vector Matrix4 rotationMatrix; rotationMatrix.rotateX(rotationX); rotationMatrix.rotateY(rotationY); rotationMatrix.rotateZ(rotationZ); Vector3 targetVector = origin.location + rotationMatrix * offsetVector; targetRotationX += origin.rotationX; targetRotationY += origin.rotationY; targetRotationZ += origin.rotationZ; rexos_stewart_gough::StewartGoughLocation targetLocation(targetVector.x, targetVector.y, targetVector.z, targetRotationX, targetRotationY, targetRotationZ); REXOS_INFO_STREAM("moving from " << stewartGough->getEffectorLocation() << " to " << targetLocation); // finally move to point if(moveToPoint(targetLocation, maxAcceleration)) { setInstructionActionServer.setSucceeded(result); } else { REXOS_WARN("Failed moving to point"); setInstructionActionServer.setAborted(result); } }
void WallLevel::CheckValidWord() { if(selectedTarget == null || selectedBrick == null) { return; } string checkWord = selectedTarget->GetPrefix(); checkWord += selectedBrick->GetSuffix(); StateVariable points = mgr->GetStateManager()->GetVar(VAR_POINTS); if(wordBuilder->IsValidWord(checkWord.c_str())) { mgr->GetResourceManager()->PlayAudio(SOUND_DING); points.intValue++; } else { mgr->GetResourceManager()->PlayAudio(SOUND_BUZZ); points.intValue -= 2; if(points.intValue < 0) { points.intValue = 0; } } mgr->GetStateManager()->SetVar(VAR_POINTS, points); CIwFVec2 curLocation((float)selectedBrick->GetBaseLocation().x, (float)selectedBrick->GetBaseLocation().y); CIwFVec2 targetLocation((float)selectedTarget->GetBaseLocation().x, (float)selectedTarget->GetBaseLocation().y); float orientation = IW_ANGLE_TO_RADIANS(0); CIwFVec2 normal; normal.x = (float)cos(orientation); normal.y = (float)sin(orientation); CIwFVec2 cp1 = curLocation + normal * 120; CIwFVec2 cp2 = (targetLocation - curLocation) / 2; cp2 += curLocation + cp2 + normal * cp2.GetLength(); PathFollowingSprite *path = new PathFollowingSprite(mgr); path->SetTotalPathTimeMillis(2000); path->AddSegment(new PathSegmentBezier(Convert(curLocation), Convert(cp1), Convert(cp2), Convert(targetLocation))); path->SetCompletionCallback(selectedBrick, (EventCallbackPtr)&Brick::PlaceBrick, path); path->SetLoopCount(1); mgr->RegisterGameObject(path); selectedBrick->SetParent(path); selectedBrick->target = selectedTarget; selectedTarget->Hide(); selectedBrick->staticBrick = true; selectedTarget->staticTarget = true; bricks[selectedBrick->xPos] = null; s3eDebugTracePrintf("%d - Pathing brick %s", mgr->GetCurrentClockTime(), selectedBrick->debugId); selectedBrick = null; selectedTarget = null; }
int main(int argc,char* argv[]) { ros::init(argc, argv, "planner"); ros::NodeHandle nh; // nodeHandle ros::Publisher pub_path = nh.advertise<global_planner::Seed>("/path", 1000); //Publisher for Path ros::Subscriber sub_world_map = nh.subscribe("/world_map",10, update_world_map); //Subscriber for World Map ros::Subscriber sub_bot_pose = nh.subscribe("/bot_pose", 10 ,update_bot_pose); // topic should same with data published by GPS ros::Subscriber sub_target_pose = nh.subscribe("/target_Pose", 10 , update_target_pose); // topic published from GPS // ros::Subscriber sub3 = nh.subscribe("/pose", 1, update_pose); /* TO DO Custom Message for array of pose3D (check ROS tut).*/ // pub_path = nh.advertise<geometry_msgs::Twist > ("cmd_vel", 1); cvNamedWindow("[PLANNER] Map", 0); navigation::State botLocation(rand()%100,rand()%100,90,0),targetLocation(900,900,90,0); navigation::AStarSeed planner; ros::Rate loop_rate(LOOP_RATE); srand((unsigned int)time(NULL)); int iterations = 100; while (iterations-- && ros::ok()) { global_planner::Seed seed; cv::Mat img = cv::Mat::zeros(1000, 1000, CV_8UC1); // std::chrono::steady_clock::time_point startC=std::chrono::steady_clock::now(); navigation::addObstacles(img, 5); struct timeval t,c; gettimeofday(&t,NULL); std::pair<std::vector<navigation::StateOfCar>, navigation::Seed> path = planner.findPathToTargetWithAstar(img,botLocation, targetLocation); // planner.showPath(path.first); seed.x = path.second.finalState.x(); seed.y = path.second.finalState.y(); seed.theta = path.second.finalState.theta(); seed.costOfseed = path.second.costOfseed; seed.velocityRatio = path.second.velocityRatio; seed.leftVelocity = path.second.leftVelocity; seed.rightVelocity = path.second.rightVelocity; seed.curvature = path.second.finalState.curvature(); pub_path.publish(seed); gettimeofday(&c,NULL); double td = t.tv_sec + t.tv_usec/1000000.0; double cd = c.tv_sec + c.tv_usec/1000000.0; // time in seconds for thousand iterations std::cout<<"FPS:"<< 1/(cd-td) <<std::endl; ros::spinOnce(); loop_rate.sleep(); } ROS_INFO("Planner Exited"); return NULL; }