void RONet::checkFlows(SUMOTime time) { std::vector<std::string> toRemove; for (NamedObjectCont<SUMOVehicleParameter*>::IDMap::const_iterator i = myFlows.getMyMap().begin(); i != myFlows.getMyMap().end(); ++i) { SUMOVehicleParameter* pars = i->second; while (pars->repetitionsDone < pars->repetitionNumber) { SUMOTime depart = static_cast<SUMOTime>(pars->depart + pars->repetitionsDone * pars->repetitionOffset); if (myDepartures.find(pars->id) != myDepartures.end()) { depart = myDepartures[pars->id].back(); } if (depart >= time + DELTA_T) { break; } if (myDepartures.find(pars->id) != myDepartures.end()) { myDepartures[pars->id].pop_back(); } SUMOVehicleParameter* newPars = new SUMOVehicleParameter(*pars); newPars->id = pars->id + "." + toString(pars->repetitionsDone); newPars->depart = depart; pars->repetitionsDone++; // try to build the vehicle SUMOVTypeParameter* type = getVehicleTypeSecure(pars->vtypeid); RORouteDef* route = getRouteDef(pars->routeid)->copy("!" + newPars->id); ROVehicle* veh = new ROVehicle(*newPars, route, type, this); addVehicle(newPars->id, veh); delete newPars; } if (pars->repetitionsDone == pars->repetitionNumber) { toRemove.push_back(i->first); } } for (std::vector<std::string>::const_iterator i = toRemove.begin(); i != toRemove.end(); ++i) { myFlows.erase(*i); } }
void WarBackground::setup(){ for( int i = 0; i < NUM_OF_VEHICLES ; i++){ addVehicle(); } }
// parses values of input and separates them into tokens which then go to addVehicle // to be stored as xloc, yloc, speed, and angle (theta) void stringtokenizer(char input[]) { int i = 0; float temp; char* strArray[5]; float inputs[5]; char *token = strtok(input, ","); while(token != NULL) { strArray[i] = strdup(token); token = strtok(NULL, ","); i++; } for (i=0; i<4; i++) { temp = strtod(strArray[i], NULL); inputs[i] = temp; } addVehicle(inputs); }
void ForkLiftDemo::initPhysics() { int upAxis = 1; m_guiHelper->setUpAxis(upAxis); btVector3 groundExtents(50,50,50); groundExtents[upAxis]=3; btCollisionShape* groundShape = new btBoxShape(groundExtents); m_collisionShapes.push_back(groundShape); m_collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax); if (useMCLPSolver) { btDantzigSolver* mlcp = new btDantzigSolver(); //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btMLCPSolver* sol = new btMLCPSolver(mlcp); m_constraintSolver = sol; } else { m_constraintSolver = new btSequentialImpulseConstraintSolver(); } m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration); if (useMCLPSolver) { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix } else { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead } m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.00001; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //m_dynamicsWorld->setGravity(btVector3(0,0,0)); btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(0,-3,0)); //either use heightfield or triangle mesh //create ground object localCreateRigidBody(0,tr,groundShape); btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f)); m_collisionShapes.push_back(chassisShape); btCompoundShape* compound = new btCompoundShape(); m_collisionShapes.push_back(compound); btTransform localTrans; localTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis localTrans.setOrigin(btVector3(0,1,0)); compound->addChildShape(localTrans,chassisShape); { btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f)); btTransform suppLocalTrans; suppLocalTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis suppLocalTrans.setOrigin(btVector3(0,1.0,2.5)); compound->addChildShape(suppLocalTrans, suppShape); } tr.setOrigin(btVector3(0,0.f,0)); m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape); //m_carChassis->setDamping(0.2,0.2); m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); m_guiHelper->createCollisionShapeGraphicsObject(m_wheelShape); int wheelGraphicsIndex = m_wheelShape->getUserIndex(); const float position[4]={0,10,10,0}; const float quaternion[4]={0,0,0,1}; const float color[4]={0,1,0,1}; const float scaling[4] = {1,1,1,1}; for (int i=0;i<4;i++) { m_wheelInstances[i] = m_guiHelper->registerGraphicsInstance(wheelGraphicsIndex, position, quaternion, color, scaling); } { btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f)); m_collisionShapes.push_back(liftShape); btTransform liftTrans; m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f); liftTrans.setIdentity(); liftTrans.setOrigin(m_liftStartPos); m_liftBody = localCreateRigidBody(10,liftTrans, liftShape); btTransform localA, localB; localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, M_PI_2, 0); localA.setOrigin(btVector3(0.0, 1.0, 3.05)); localB.getBasis().setEulerZYX(0, M_PI_2, 0); localB.setOrigin(btVector3(0.0, -1.5, -0.05)); m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB); // m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS); m_liftHinge->setLimit(0.0f, 0.0f); m_dynamicsWorld->addConstraint(m_liftHinge, true); btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f)); m_collisionShapes.push_back(forkShapeA); btCompoundShape* forkCompound = new btCompoundShape(); m_collisionShapes.push_back(forkCompound); btTransform forkLocalTrans; forkLocalTrans.setIdentity(); forkCompound->addChildShape(forkLocalTrans, forkShapeA); btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeB); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeB); btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeC); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeC); btTransform forkTrans; m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f); forkTrans.setIdentity(); forkTrans.setOrigin(m_forkStartPos); m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, 0, M_PI_2); localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f)); localB.getBasis().setEulerZYX(0, 0, M_PI_2); localB.setOrigin(btVector3(0.0, 0.0, -0.1)); m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true); m_forkSlider->setLowerLinLimit(0.1f); m_forkSlider->setUpperLinLimit(0.1f); // m_forkSlider->setLowerAngLimit(-LIFT_EPS); // m_forkSlider->setUpperAngLimit(LIFT_EPS); m_forkSlider->setLowerAngLimit(0.0f); m_forkSlider->setUpperAngLimit(0.0f); m_dynamicsWorld->addConstraint(m_forkSlider, true); btCompoundShape* loadCompound = new btCompoundShape(); m_collisionShapes.push_back(loadCompound); btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f)); m_collisionShapes.push_back(loadShapeA); btTransform loadTrans; loadTrans.setIdentity(); loadCompound->addChildShape(loadTrans, loadShapeA); btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeB); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeB); btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeC); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeC); loadTrans.setIdentity(); m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f); loadTrans.setOrigin(m_loadStartPos); m_loadBody = localCreateRigidBody(loadMass, loadTrans, loadCompound); } /// create vehicle { m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld); m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster); ///never deactivate the vehicle m_carChassis->setActivationState(DISABLE_DEACTIVATION); m_dynamicsWorld->addVehicle(m_vehicle); float connectionHeight = 1.2f; bool isFrontWheel=true; //choose coordinate system m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex); btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); isFrontWheel = false; m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); for (int i=0;i<m_vehicle->getNumWheels();i++) { btWheelInfo& wheel = m_vehicle->getWheelInfo(i); wheel.m_suspensionStiffness = suspensionStiffness; wheel.m_wheelsDampingRelaxation = suspensionDamping; wheel.m_wheelsDampingCompression = suspensionCompression; wheel.m_frictionSlip = wheelFriction; wheel.m_rollInfluence = rollInfluence; } } resetForklift(); // setCameraDistance(26.f); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }