void velocityNav::update() { //stopWatch.stop(); //stopWatch.start(); vpr::Interval cur_time = mNavWand->getTimeStamp(); vpr::Interval diff_time(cur_time-mLastTimeStamp); mTimeDelta = diff_time.secf(); /* Cluster Sync Debug code std::cout << "Delta: " << diff_time.getBaseVal() << std::endl; std::cout << "Current: " << cur_time.getBaseVal() << "Last: " << mLastTimeStamp.getBaseVal() << "\n" << std::endl; */ mLastTimeStamp = cur_time; if ( mTimeDelta > 2.0f ) // If the time is greater than 2 seconds ( 1/2 fps) { vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) << clrOutNORM(clrCYAN,"VelNav: timeInstant too large: ") << mTimeDelta << std::endl << vprDEBUG_FLUSH; //stopWatch.stop(); // Get a REALLY small delta time //stopWatch.start(); } //vprDEBUG_BEGIN(vprDBG_ALL,0) // << "VelNav: ----- Update ----\n" << vprDEBUG_FLUSH; //vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) // << "VelNav: timeInstant: " << mTimeDelta << std::endl // << vprDEBUG_FLUSH; // If we are not supposed to be active, then don't run if ( ! this->isActive() ) { return; } ////////////////////////////////// // do navigations... ////////////////////////////////// // Define axes, in Juggler/OpenGL coordinate system (right handed) gmtl::Vec3f trackerZaxis(0.0f, 0.0f, 1.0f); gmtl::Vec3f trackerXaxis(1.0f, 0.0f, 0.0f); gmtl::Vec3f trackerYaxis(0.0f, 1.0f, 0.0f); // 9.8 m/s (METERS) const gmtl::Vec3f gravity_meters_per_second(0.0f, -9.8f, 0.0f); // to be set by switch (mUnits == METERS) gmtl::Vec3f gravity(0.0f, -9.8f, 0.0f); switch (mUnits) { case FEET: meters2feet(gravity_meters_per_second, gravity); break; default: case METERS: gravity = gravity_meters_per_second; break; } if ((mAllowRot) && (mRotating)) { // Interpolates the rotation, and updates mCurPos matrix this->scaled_rotate(mRotationalAcceleration); } if (mMode == DRIVE) { // get the axes of the tracking/pointing device // NOTE: constrain to the Y axis in GROUND mode (no flying/hopping or // diving faster than gravity allows) gmtl::Matrix44f constrainedToY; //mRotationalAcceleration.constrainRotAxis(false, true, false, // constrainedToY); gmtl::EulerAngleXYZf euler(0.0f, gmtl::makeYRot(mRotationalAcceleration), 0.0f); gmtl::setRot(constrainedToY, euler); // Only allow Yaw (rot y) gmtl::xform(trackerZaxis, constrainedToY, trackerZaxis); gmtl::xform(trackerXaxis, constrainedToY, trackerXaxis); gmtl::xform(trackerYaxis, constrainedToY, trackerYaxis); } else if (mMode == FLY) { // get the axes of the tracking/pointing device gmtl::xform(trackerZaxis, mRotationalAcceleration, trackerZaxis); gmtl::xform(trackerXaxis, mRotationalAcceleration, trackerXaxis); gmtl::xform(trackerYaxis, mRotationalAcceleration, trackerYaxis); } // this is used to accumulate velocity added by navigation gmtl::Vec3f velocityAccumulator(0.0f, 0.0f, 0.0f); if (mMode == DRIVE) // if DRIVING --> we have GRAVITY { // add the velocity this timeslice/frame by the acceleration from // gravity. velocityAccumulator += mVelocityFromGravityAccumulator; //vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) // << "velNav: drive: gravAccum: " // << mVelocityFromGravityAccumulator << vprDEBUG_FLUSH; // recalculate the current downward velocity from gravity. // this vector then is accumulated with the rest of the velocity vectors // each frame. //mVelocityFromGravityAccumulator += (gravity * mTimeDelta); mVelocityFromGravityAccumulator += (gravity * mTimeDelta); //vprDEBUG_CONT(vprDBG_ALL,0) // << " new vel: " << velocityAccumulator // << " new grav: " << mVelocityFromGravityAccumulator << std::endl // << vprDEBUG_FLUSH; } if (mAllowTrans) { // add velocity with respect to the tracking/pointing device velocityAccumulator += (trackerZaxis * mVelocity[2]); // forward/reverse |reletive to tracker velocityAccumulator += (trackerXaxis * mVelocity[0]); // strafe |reletive to tracker velocityAccumulator += (trackerYaxis * mVelocity[1]); // rise/dive |reletive to tracker } // Get rid of some velocity due to damping in the system mVelocity -= (mVelocity * (1.0f - mDamping)); // navigation just calculated navigator's next velocity // now convert accumulated velocity to distance traveled this frame (by // cancelling out time) // NOTE: this is not the final distance, since we still have to do // collision correction. gmtl::Vec3f distanceToMove = velocityAccumulator * mTimeDelta; //vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) // << "velNav: distToMove = velAcum * instant: " << velocityAccumulator // << " * " << mTimeDelta << std::endl << vprDEBUG_FLUSH; // --- TRANSLATION and COLLISION DETECTION --- // bool did_collide; // Did we collide with anything // The total correction on the movement (in modelspace) gmtl::Vec3f total_correction; navTranslate(distanceToMove,did_collide,total_correction); if(did_collide) // If we hit something, stop falling { //vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) // << "Did collide: Setting gravAccum to 0,0,0\n" // << vprDEBUG_FLUSH; mVelocityFromGravityAccumulator.set(0.0f, 0.0f, 0.0f); } //vprDEBUG_END(vprDBG_ALL,0) << "---------------------\n" << vprDEBUG_FLUSH; }
bool Game::init() { // init sdl SDL_Init(SDL_INIT_EVERYTHING); // SDL_GL_SetAttribute(SDL_GL_MULTISAMPLEBUFFERS, 1); // SDL_GL_SetAttribute(SDL_GL_MULTISAMPLESAMPLES, 4); screen = SDL_SetVideoMode(screen_w, screen_h, screen_depth, SDL_HWSURFACE | SDL_DOUBLEBUF | SDL_OPENGL | SDL_FULLSCREEN); if (!screen) { fprintf(stderr, "Couldn't set GL video mode: %s\n", SDL_GetError()); SDL_Quit(); exit(2); } //SDL_ShowCursor(SDL_DISABLE); // init image library FreeImage_Initialise(); SDL_WM_SetCaption("OpenSoldat", NULL); glDisable(GL_DEPTH_TEST); glewExperimental = GL_TRUE; glewInit(); glEnable(GL_BLEND); glEnable(GL_TEXTURE_2D); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // Create and compile the vertex shader vertexShader = glCreateShader(GL_VERTEX_SHADER); glShaderSource(vertexShader, 1, &vertexSource, NULL); glCompileShader(vertexShader); // Create and compile the fragment shader fragmentShader = glCreateShader(GL_FRAGMENT_SHADER); glShaderSource(fragmentShader, 1, &fragmentSource, NULL); glCompileShader(fragmentShader); // Link the vertex and fragment shader into a shader program shaderProgram = glCreateProgram(); glAttachShader(shaderProgram, vertexShader); glAttachShader(shaderProgram, fragmentShader); glBindFragDataLocation(shaderProgram, 0, "outColor"); glLinkProgram(shaderProgram); glUseProgram(shaderProgram); reshape(screen->w, screen->h); //create world b2Vec2 gravity(0.0f, -15.0f); world = new b2World(gravity); //init map map = new Map(shaderProgram); char mapname[] = "maps/ZS_JungleRumble.PMS"; std::cout << map->getMapDir() << std::endl; map->load(mapname); map->addToWorld(world); //init animations // anims = new animations(); // anims->loadAnimations(); //add players players = new std::vector<Player*>(); std::string name = "Rambo"; std::vector<tagPMS_SPAWNPOINT> spawnPoints = map->getSpawnPoints(); player = new Player(name, 0, 0, this); player->setSpawnPoint(spawnPoints[1]); player->addToWorld(); // std::string name3 = "Rambo"; // Player *p2 = new Player(name3, world, 0, 0); // p2->setSpawnPoint(spawnPoints[3]); // p2->addToWorld(); //init bullets bullets = new std::vector<Bullet*>(); //active player //Player *ap = player; GLint status; glGetShaderiv(vertexShader, GL_COMPILE_STATUS, &status); char buffer[512]; glGetShaderInfoLog(vertexShader, 512, NULL, buffer); std::cout << buffer << std::endl; viewTrans = glGetUniformLocation(shaderProgram, "view"); return true; }
void ContactListener::WaterStep(Water* water) { //inside Step(), loop through all currently overlapping fixture pairs set<fixturePair>::iterator it = m_fixturePairs.begin(); set<fixturePair>::iterator end = m_fixturePairs.end(); while (it != end) { //fixtureA is the fluid b2Fixture* fixtureA = it->first; b2Fixture* fixtureB = it->second; float density = fixtureA->GetDensity(); vector<b2Vec2> intersectionPoints; if(density < 50) { if (water->findIntersectionOfFixtures(fixtureA, fixtureB, intersectionPoints) ) { //find centroid float area = 0; b2Vec2 centroid = water->ComputeCentroid( intersectionPoints, area); //apply buoyancy stuff here... b2Vec2 gravity( 0, -9.81f ); //apply buoyancy force (fixtureA is the fluid) float displacedMass = fixtureA->GetDensity() * area; b2Vec2 buoyancyForce = displacedMass * gravity; fixtureB->GetBody()->ApplyForce( buoyancyForce, centroid,false/*unsure?*/); //apply drag separately for each polygon edge for (int i = 0; i < intersectionPoints.size(); i++) { //the end points and mid-point of this edge b2Vec2 v0 = intersectionPoints[i]; b2Vec2 v1 = intersectionPoints[(i+1)%intersectionPoints.size()]; b2Vec2 midPoint = 0.5f * (v0+v1); //find relative velocity between object and fluid at edge midpoint b2Vec2 velDir = fixtureB->GetBody()->GetLinearVelocityFromWorldPoint( midPoint ) - fixtureA->GetBody()->GetLinearVelocityFromWorldPoint( midPoint ); float vel = velDir.Normalize(); b2Vec2 edge = v1 - v0; float edgeLength = edge.Normalize(); b2Vec2 normal = b2Cross(-1,edge); //gets perpendicular vector float dragDot = b2Dot(normal, velDir); if ( dragDot < 0 ) continue; //normal points backwards - this is not a leading edge float dragMag = dragDot * edgeLength * density * vel * vel; b2Vec2 dragForce = dragMag * -velDir; fixtureB->GetBody()->ApplyForce( dragForce, midPoint,false); } } } ++it; } }
inline std::pair<typename SteadyStateUpscaler<Traits>::permtensor_t, typename SteadyStateUpscaler<Traits>::permtensor_t> SteadyStateUpscaler<Traits>:: upscaleSteadyState(const int flow_direction, const std::vector<double>& initial_saturation, const double boundary_saturation, const double pressure_drop, const permtensor_t& upscaled_perm) { static int count = 0; ++count; int num_cells = this->ginterf_.numberOfCells(); // No source or sink. std::vector<double> src(num_cells, 0.0); Opm::SparseVector<double> injection(num_cells); // Gravity. Dune::FieldVector<double, 3> gravity(0.0); if (use_gravity_) { gravity[2] = Opm::unit::gravity; } if (gravity.two_norm() > 0.0) { OPM_MESSAGE("Warning: Gravity is experimental for flow solver."); } // Set up initial saturation profile. std::vector<double> saturation = initial_saturation; // Set up boundary conditions. setupUpscalingConditions(this->ginterf_, this->bctype_, flow_direction, pressure_drop, boundary_saturation, this->twodim_hack_, this->bcond_); // Set up solvers. if (flow_direction == 0) { this->flow_solver_.init(this->ginterf_, this->res_prop_, gravity, this->bcond_); } transport_solver_.initObj(this->ginterf_, this->res_prop_, this->bcond_); // Run pressure solver. this->flow_solver_.solve(this->res_prop_, saturation, this->bcond_, src, this->residual_tolerance_, this->linsolver_verbosity_, this->linsolver_type_, false, this->linsolver_maxit_, this->linsolver_prolongate_factor_, this->linsolver_smooth_steps_); double max_mod = this->flow_solver_.postProcessFluxes(); std::cout << "Max mod = " << max_mod << std::endl; // Do a run till steady state. For now, we just do some pressure and transport steps... std::vector<double> saturation_old = saturation; for (int iter = 0; iter < simulation_steps_; ++iter) { // Run transport solver. transport_solver_.transportSolve(saturation, stepsize_, gravity, this->flow_solver_.getSolution(), injection); // Run pressure solver. this->flow_solver_.solve(this->res_prop_, saturation, this->bcond_, src, this->residual_tolerance_, this->linsolver_verbosity_, this->linsolver_type_, false, this->linsolver_maxit_, this->linsolver_prolongate_factor_, this->linsolver_smooth_steps_); max_mod = this->flow_solver_.postProcessFluxes(); std::cout << "Max mod = " << max_mod << std::endl; // Print in-out flows if requested. if (print_inoutflows_) { std::pair<double, double> w_io, o_io; computeInOutFlows(w_io, o_io, this->flow_solver_.getSolution(), saturation); std::cout << "Pressure step " << iter << "\nWater flow [in] " << w_io.first << " [out] " << w_io.second << "\nOil flow [in] " << o_io.first << " [out] " << o_io.second << std::endl; } // Output. if (output_vtk_) { writeVtkOutput(this->ginterf_, this->res_prop_, this->flow_solver_.getSolution(), saturation, std::string("output-steadystate") + '-' + boost::lexical_cast<std::string>(count) + '-' + boost::lexical_cast<std::string>(flow_direction) + '-' + boost::lexical_cast<std::string>(iter)); } // Comparing old to new. int num_cells = saturation.size(); double maxdiff = 0.0; for (int i = 0; i < num_cells; ++i) { maxdiff = std::max(maxdiff, std::fabs(saturation[i] - saturation_old[i])); } #ifdef VERBOSE std::cout << "Maximum saturation change: " << maxdiff << std::endl; #endif if (maxdiff < sat_change_threshold_) { #ifdef VERBOSE std::cout << "Maximum saturation change is under steady state threshold." << std::endl; #endif break; } // Copy to old. saturation_old = saturation; } // Compute phase mobilities. // First: compute maximal mobilities. typedef typename Super::ResProp::Mobility Mob; Mob m; double m1max = 0; double m2max = 0; for (int c = 0; c < num_cells; ++c) { this->res_prop_.phaseMobility(0, c, saturation[c], m.mob); m1max = maxMobility(m1max, m.mob); this->res_prop_.phaseMobility(1, c, saturation[c], m.mob); m2max = maxMobility(m2max, m.mob); } // Second: set thresholds. const double mob1_abs_thres = relperm_threshold_ / this->res_prop_.viscosityFirstPhase(); const double mob1_rel_thres = m1max / maximum_mobility_contrast_; const double mob1_threshold = std::max(mob1_abs_thres, mob1_rel_thres); const double mob2_abs_thres = relperm_threshold_ / this->res_prop_.viscositySecondPhase(); const double mob2_rel_thres = m2max / maximum_mobility_contrast_; const double mob2_threshold = std::max(mob2_abs_thres, mob2_rel_thres); // Third: extract and threshold. std::vector<Mob> mob1(num_cells); std::vector<Mob> mob2(num_cells); for (int c = 0; c < num_cells; ++c) { this->res_prop_.phaseMobility(0, c, saturation[c], mob1[c].mob); thresholdMobility(mob1[c].mob, mob1_threshold); this->res_prop_.phaseMobility(1, c, saturation[c], mob2[c].mob); thresholdMobility(mob2[c].mob, mob2_threshold); } // Compute upscaled relperm for each phase. ReservoirPropertyFixedMobility<Mob> fluid_first(mob1); permtensor_t eff_Kw = Super::upscaleEffectivePerm(fluid_first); ReservoirPropertyFixedMobility<Mob> fluid_second(mob2); permtensor_t eff_Ko = Super::upscaleEffectivePerm(fluid_second); // Set the steady state saturation fields for eventual outside access. last_saturation_state_.swap(saturation); // Compute the (anisotropic) upscaled mobilities. // eff_Kw := lambda_w*K // => lambda_w = eff_Kw*inv(K); permtensor_t lambda_w(matprod(eff_Kw, inverse3x3(upscaled_perm))); permtensor_t lambda_o(matprod(eff_Ko, inverse3x3(upscaled_perm))); // Compute (anisotropic) upscaled relative permeabilities. // lambda = k_r/mu permtensor_t k_rw(lambda_w); k_rw *= this->res_prop_.viscosityFirstPhase(); permtensor_t k_ro(lambda_o); k_ro *= this->res_prop_.viscositySecondPhase(); return std::make_pair(k_rw, k_ro); }
void GravityCircle::update(const Scene & scene){ DynamicCircle::update(scene); //gravity goes down duuuh Vector gravity(0,-0.1); this->setVelocity((this->getVelocity() + gravity)); }
void LLViewerPartGroup::updateParticles(const F32 lastdt) { LLMemType mt(LLMemType::MTYPE_PARTICLES); F32 dt; LLVector3 gravity(0.f, 0.f, GRAVITY); LLViewerRegion *regionp = getRegion(); S32 end = (S32) mParticles.size(); for (S32 i = 0 ; i < (S32)mParticles.size();) { LLVector3 a(0.f, 0.f, 0.f); LLViewerPart* part = mParticles[i] ; dt = lastdt + mSkippedTime - part->mSkipOffset; part->mSkipOffset = 0.f; // Update current time const F32 cur_time = part->mLastUpdateTime + dt; const F32 frac = cur_time / part->mMaxAge; // "Drift" the object based on the source object if (part->mFlags & LLPartData::LL_PART_FOLLOW_SRC_MASK) { part->mPosAgent = part->mPartSourcep->mPosAgent; part->mPosAgent += part->mPosOffset; } // Do a custom callback if we have one... if (part->mVPCallback) { (*part->mVPCallback)(*part, dt); } if (part->mFlags & LLPartData::LL_PART_WIND_MASK) { LLVector3 tempVel(part->mVelocity); part->mVelocity *= 1.f - 0.1f*dt; part->mVelocity += 0.1f*dt*regionp->mWind.getVelocity(regionp->getPosRegionFromAgent(part->mPosAgent)); } // Now do interpolation towards a target if (part->mFlags & LLPartData::LL_PART_TARGET_POS_MASK) { F32 remaining = part->mMaxAge - part->mLastUpdateTime; F32 step = dt / remaining; step = llclamp(step, 0.f, 0.1f); step *= 5.f; // we want a velocity that will result in reaching the target in the // Interpolate towards the target. LLVector3 delta_pos = part->mPartSourcep->mTargetPosAgent - part->mPosAgent; delta_pos /= remaining; part->mVelocity *= (1.f - step); part->mVelocity += step*delta_pos; } if (part->mFlags & LLPartData::LL_PART_TARGET_LINEAR_MASK) { LLVector3 delta_pos = part->mPartSourcep->mTargetPosAgent - part->mPartSourcep->mPosAgent; part->mPosAgent = part->mPartSourcep->mPosAgent; part->mPosAgent += frac*delta_pos; part->mVelocity = delta_pos; } else { // Do velocity interpolation part->mPosAgent += dt*part->mVelocity; part->mPosAgent += 0.5f*dt*dt*part->mAccel; part->mVelocity += part->mAccel*dt; } // Do a bounce test if (part->mFlags & LLPartData::LL_PART_BOUNCE_MASK) { // Need to do point vs. plane check... // For now, just check relative to object height... F32 dz = part->mPosAgent.mV[VZ] - part->mPartSourcep->mPosAgent.mV[VZ]; if (dz < 0) { part->mPosAgent.mV[VZ] += -2.f*dz; part->mVelocity.mV[VZ] *= -0.75f; } } // Reset the offset from the source position if (part->mFlags & LLPartData::LL_PART_FOLLOW_SRC_MASK) { part->mPosOffset = part->mPosAgent; part->mPosOffset -= part->mPartSourcep->mPosAgent; } // Do color interpolation if (part->mFlags & LLPartData::LL_PART_INTERP_COLOR_MASK) { part->mColor.setVec(part->mStartColor); // note: LLColor4's v%k means multiply-alpha-only, // LLColor4's v*k means multiply-rgb-only part->mColor *= 1.f - frac; // rgb*k part->mColor %= 1.f - frac; // alpha*k part->mColor += frac%(frac*part->mEndColor); // rgb,alpha } // Do scale interpolation if (part->mFlags & LLPartData::LL_PART_INTERP_SCALE_MASK) { part->mScale.setVec(part->mStartScale); part->mScale *= 1.f - frac; part->mScale += frac*part->mEndScale; } // Set the last update time to now. part->mLastUpdateTime = cur_time; // Kill dead particles (either flagged dead, or too old) if ((part->mLastUpdateTime > part->mMaxAge) || (LLViewerPart::LL_PART_DEAD_MASK == part->mFlags)) { mParticles[i] = mParticles.back() ; mParticles.pop_back() ; delete part ; } else { F32 desired_size = calc_desired_size(part->mPosAgent, part->mScale); if (!posInGroup(part->mPosAgent, desired_size)) { // Transfer particles between groups LLViewerPartSim::getInstance()->put(part) ; mParticles[i] = mParticles.back() ; mParticles.pop_back() ; } else { i++ ; } } } S32 removed = end - (S32)mParticles.size(); if (removed > 0) { // we removed one or more particles, so flag this group for update if (mVOPartGroupp.notNull()) { gPipeline.markRebuild(mVOPartGroupp->mDrawable, LLDrawable::REBUILD_ALL, TRUE); } LLViewerPartSim::decPartCount(removed); } // Kill the viewer object if this particle group is empty if (mParticles.empty()) { gObjectList.killObject(mVOPartGroupp); mVOPartGroupp = NULL; } }
//TODO clean this shitty code void Player::move(Map* map){ if(fireTimer.isStarted() && fireTimer.getTicks() > PLAYER_FIRE_INTERVAL){ fireTimer.stop(); } //save current player position before trying to change it int initPosX = pos.x; int initPosY = pos.y; int initPlayerSpeed = playerSpeed; const Uint8* currentKeyStates = SDL_GetKeyboardState( NULL ); //get pressed key //TODO clean following code /************ HANDLE X ***************/ checkX: if(!isDying() && currentKeyStates[ SDL_SCANCODE_RIGHT ]) { //right key pressed pos.x += playerSpeed; } if(!isDying() && currentKeyStates[ SDL_SCANCODE_LEFT ]){ //left key pressed pos.x -= playerSpeed; } mCollider.x = pos.x; //if end level or collision if( ( pos.x < 0 ) || ( pos.x + PLAYER_WIDTH > map->getLevelWidth() ) || map->checkCollision(mCollider, false) ){ SDL_Rect upCollider = mCollider; upCollider.y -= PLAYER_CLIMB_STEP; //retry in case of 1 pixel step if( !jumpTimer.isStarted() && pos.x >= 0 && pos.x + PLAYER_WIDTH < map->getLevelWidth() && !map->checkCollision(upCollider, false) ){ pos.y = upCollider.y; //move player up (because of step) }else{ pos.x = initPosX; //revert position to initPos mCollider.x = pos.x; if(playerSpeed > 0 && !jumpTimer.isStarted()){ playerSpeed-=2; //retry with smaller displacement goto checkX; } } } /************ HANDLE Y ***************/ playerSpeed = initPlayerSpeed; //save initial displacement gravity(map); //(try to) move playing according to gravity checkY: pos.y += mVelY; //add vertical velocity to player position mCollider.y = pos.y; //if end level or collision if( ( pos.y < 0 ) || ( pos.y + PLAYER_HEIGHT > SCREEN_HEIGHT - MARGIN_BOTTOM ) || map->checkCollision(mCollider, false) ){ pos.y = initPosY; //revert to initial position mCollider.y = pos.y; if(jumpTimer.isStarted()){ //if in jump if(mVelY < 0){ //if going up mVelY = -1 * mVelY; //invert vertical velocity }else{ //going down jumpTimer.stop(); //end of jump (hit ground) playerSpeed=PLAYER_INIT_SPEED; goto checkY; } }else if(mVelY>0){ //if falling //mVelY=1; if(mVelY >= 1){ mVelY--; goto checkY; }else{ mVelY-=0.2; goto checkY; } } } }
void ocl_one_step_move(sotl_device_t *dev) { unsigned begin = atom_set_begin(&dev->atom_set); unsigned end = atom_set_end(&dev->atom_set); if (gravity_enabled) gravity (dev); #ifdef _SPHERE_MODE_ if (eating_enabled) eating_pacman (dev); if (growing_enabled) growing_ghost (dev); #endif if (force_enabled) { if (is_box_mode) { reset_box_buffer(dev); box_count_all_atoms(dev, begin, end); // Calc boxes offsets scan(dev, 0, dev->domain.total_boxes + 1); // Sort atoms in boxes { copy_box_buffer(dev); // Sort box_sort_all_atoms(dev, begin, end); // box_sort_all_atoms used alternate pos & speed buffer, so we should switch... dev->cur_pb = 1 - dev->cur_pb; dev->cur_sb = 1 - dev->cur_sb; } /* Compute potential */ box_lennard_jones(dev, begin, end); } else { // !BOX_MODE // Classic n^2 compute force version n2_lennard_jones (dev); } } if(detect_collision) atom_collision (dev); if(borders_enabled) border_collision (dev); update_position (dev); #ifdef HAVE_LIBGL if (dev->display) update_vertices (dev); #endif }
void InverseDynamicsExample::initPhysics() { //roboticists like Z up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); btVector3 gravity(0,0,0); // gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { SliderParams slider("Kp",&kp); slider.m_minVal=0; slider.m_maxVal=2000; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Kd",&kd); slider.m_minVal=0; slider.m_maxVal=50; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } if (m_option == BT_ID_PROGRAMMATICALLY) { ButtonParams button("toggle inverse model",0,true); button.m_callback = toggleUseInverseModel; m_guiHelper->getParameterInterface()->registerButtonParameter(button); } switch (m_option) { case BT_ID_LOAD_URDF: { BulletURDFImporter u2b(m_guiHelper,0,1); bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf"); if (loadOk) { int rootLinkIndex = u2b.getRootLinkIndex(); b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); btTransform identityTrans; identityTrans.setIdentity(); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) { m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); } m_multiBody = creation.getBulletMultiBody(); if (m_multiBody) { //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator //temporarily set some extreme damping factors until we have some joint control or constraints m_multiBody->setAngularDamping(0*0.99); m_multiBody->setLinearDamping(0*0.99); b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); } } break; } case BT_ID_PROGRAMMATICALLY: { btTransform baseWorldTrans; baseWorldTrans.setIdentity(); m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false); break; } default: { b3Error("Unknown option in InverseDynamicsExample::initPhysics"); b3Assert(0); } }; if(m_multiBody) { { if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface()) { m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory"); m_timeSeriesCanvas ->setupTimeSeries(3,100, 0); } } // construct inverse model btInverseDynamics::btMultiBodyTreeCreator id_creator; if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) { b3Error("error creating tree\n"); } else { m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator); } // add joint target controls qd.resize(m_multiBody->getNumDofs()); qd_name.resize(m_multiBody->getNumDofs()); q_name.resize(m_multiBody->getNumDofs()); if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface()) { for(std::size_t dof=0;dof<qd.size();dof++) { qd[dof] = 0; char tmp[25]; sprintf(tmp,"q_desired[%lu]",dof); qd_name[dof] = tmp; SliderParams slider(qd_name[dof].c_str(),&qd[dof]); slider.m_minVal=-3.14; slider.m_maxVal=3.14; sprintf(tmp,"q[%lu]",dof); q_name[dof] = tmp; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); btVector4 color = sJointCurveColors[dof&7]; m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255); } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
// This is a simple example of building and running a simulation // using Box2D. Here we create a large ground box and a small dynamic // box. // There are no graphics for this example. Box2D is meant to be used // with your rendering engine in your game engine. int main(int argc, char** argv) { B2_NOT_USED(argc); B2_NOT_USED(argv); // Define the gravity vector. b2Vec2 gravity(0.0f, -10.0f); // Do we want to let bodies sleep? bool doSleep = true; // Construct a world object, which will hold and simulate the rigid bodies. b2World world(gravity, doSleep); // Define the ground body. b2BodyDef groundBodyDef; groundBodyDef.position.Set(0.0f, -10.0f); // Call the body factory which allocates memory for the ground body // from a pool and creates the ground box shape (also from a pool). // The body is also added to the world. b2Body* groundBody = world.CreateBody(&groundBodyDef); // Define the ground box shape. b2PolygonShape groundBox; // The extents are the half-widths of the box. groundBox.SetAsBox(50.0f, 10.0f); // Add the ground fixture to the ground body. groundBody->CreateFixture(&groundBox, 0.0f); // Define the dynamic body. We set its position and call the body factory. b2BodyDef bodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position.Set(0.0f, 4.0f); b2Body* body = world.CreateBody(&bodyDef); // Define another box shape for our dynamic body. b2PolygonShape dynamicBox; dynamicBox.SetAsBox(1.0f, 1.0f); // Define the dynamic body fixture. b2FixtureDef fixtureDef; fixtureDef.shape = &dynamicBox; // Set the box density to be non-zero, so it will be dynamic. fixtureDef.density = 1.0f; // Override the default friction. fixtureDef.friction = 0.3f; // Add the shape to the body. body->CreateFixture(&fixtureDef); // Prepare for simulation. Typically we use a time step of 1/60 of a // second (60Hz) and 10 iterations. This provides a high quality simulation // in most game scenarios. float32 timeStep = 1.0f / 60.0f; int32 velocityIterations = 6; int32 positionIterations = 2; // This is our little game loop. for (int32 i = 0; i < 60; ++i) { // Instruct the world to perform a single step of simulation. // It is generally best to keep the time step and iterations fixed. world.Step(timeStep, velocityIterations, positionIterations); // Clear applied body forces. We didn't apply any forces, but you // should know about this function. world.ClearForces(); // Now print the position and angle of the body. b2Vec2 position = body->GetPosition(); float32 angle = body->GetAngle(); printf("%4.2f %4.2f %4.2f\n", position.x, position.y, angle); } // When the world destructor is called, all bodies and joints are freed. This can // create orphaned pointers, so be careful about your world management. return 0; }
// This is a simple example of building and running a simulation // using Box2D. Here we create a large ground box and a small dynamic // box. int main(int argc, char** argv) { //Setup SFML sf::RenderWindow window( sf::VideoMode( WWIDTH,WHEIGHT), "It really is Box2D"); B2_NOT_USED(argc); B2_NOT_USED(argv); // Textures sf::Texture GroundTexture; sf::Texture BoxTexture; GroundTexture.loadFromFile("grass.png"); BoxTexture.loadFromFile("wood.png"); // Sprites sf::Sprite GroundSprite; sf::Sprite BodySprite; GroundSprite.setTexture(GroundTexture); GroundSprite.setTextureRect(sf::IntRect(0, 0, 10 * SCALE, 2 * SCALE)); GroundSprite.setOrigin(5 * SCALE, 1 * SCALE); // origin in middle BodySprite.setTexture( BoxTexture); BodySprite.setTextureRect(sf::IntRect(0, 0, 2 * SCALE, 2 * SCALE)); BodySprite.setOrigin( 1 * SCALE, 1 * SCALE); //origin in middle // Define the gravity vector. b2Vec2 gravity(0.0f, -1.0f); // Construct a world object, which will hold and simulate the rigid bodies. b2World world(gravity); // Define the ground body. b2BodyDef groundBodyDef; groundBodyDef.position.Set(0.0f, -12.0f); // Call the body factory which allocates memory for the ground body // from a pool and creates the ground box shape (also from a pool). // The body is also added to the world. b2Body* groundBody = world.CreateBody(&groundBodyDef); // Define the ground box shape. b2PolygonShape groundBox; // The extents are the half-widths of the box. groundBox.SetAsBox(5.0f, 1.0f); // Add the ground fixture to the ground body. groundBody->CreateFixture(&groundBox, 0.0f); // Define the dynamic body. We set its position and call the body factory. b2BodyDef bodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position.Set(3.0f, 8.0f); b2Body* body = world.CreateBody(&bodyDef); body->SetTransform( body->GetPosition(), 1.2); //rotate // Define another box shape for our dynamic body. b2PolygonShape dynamicBox; dynamicBox.SetAsBox(1.0f, 1.0f); //Half widths. so actually 2x2 box // Define the dynamic body fixture. b2FixtureDef fixtureDef; fixtureDef.shape = &dynamicBox; // Set the box density to be non-zero, so it will be dynamic. fixtureDef.density = 1.0f; // Override the default friction. fixtureDef.friction = 0.3f; // Add the shape to the body. body->CreateFixture(&fixtureDef); // Prepare for simulation. Typically we use a time step of 1/60 of a // second (60Hz) and 10 iterations. This provides a high quality simulation // in most game scenarios. float32 timeStep = 1.0f / 60.0f; int32 velocityIterations = 6; int32 positionIterations = 2; // This is our little game loop. while (window.isOpen()) { // Check if it's time to go sf::Event event; while (window.pollEvent( event)) { if( event.type == sf::Event::Closed) window.close(); } window.clear(); // Instruct the world to perform a single step of simulation. // It is generally best to keep the time step and iterations fixed. world.Step(timeStep, velocityIterations, positionIterations); // Draw it // get the position and angle of the body b2Vec2 pos = body->GetPosition(); float32 angle = body->GetAngle(); pos = convertWorldToScreen( pos); BodySprite.setPosition(pos.x, pos.y); BodySprite.setRotation(180/b2_pi * angle); window.draw(BodySprite); // get the position and angle of the ground pos = groundBody->GetPosition(); angle = groundBody->GetAngle(); pos = convertWorldToScreen( pos); GroundSprite.setPosition(pos.x, pos.y); GroundSprite.setRotation(180/b2_pi * angle); window.draw(GroundSprite); window.display(); } // When the world destructor is called, all bodies and joints are freed. This can // create orphaned pointers, so be careful about your world management. return 0; }
// This is a simple example of building and running a simulation // using Box2D. Here we create a large ground box and a small dynamic // box. int main(int argc, char** argv) { B2_NOT_USED(argc); B2_NOT_USED(argv); // Define the size of the world. Simulation will still work // if bodies reach the end of the world, but it will be slower. b2AABB worldAABB; worldAABB.lowerBound.Set(-100.0f, -100.0f); worldAABB.upperBound.Set(100.0f, 100.0f); // Define the gravity vector. b2Vec2 gravity(0.0f, -10.0f); // Do we want to let bodies sleep? bool doSleep = true; // Construct a world object, which will hold and simulate the rigid bodies. b2World world(worldAABB, gravity, doSleep); // Define the ground body. b2BodyDef groundBodyDef; groundBodyDef.position.Set(0.0f, -10.0f); // Call the body factory which allocates memory for the ground body // from a pool and creates the ground box shape (also from a pool). // The body is also added to the world. b2Body* groundBody = world.CreateBody(&groundBodyDef); // Define the ground box shape. b2PolygonDef groundShapeDef; // The extents are the half-widths of the box. groundShapeDef.SetAsBox(50.0f, 10.0f); // Add the ground shape to the ground body. groundBody->CreateFixture(&groundShapeDef); // Define the dynamic body. We set its position and call the body factory. b2BodyDef bodyDef; bodyDef.position.Set(0.0f, 4.0f); b2Body* body = world.CreateBody(&bodyDef); // Define another box shape for our dynamic body. b2PolygonDef shapeDef; shapeDef.SetAsBox(1.0f, 1.0f); // Set the box density to be non-zero, so it will be dynamic. shapeDef.density = 1.0f; // Override the default friction. shapeDef.friction = 0.3f; // Add the shape to the body. body->CreateFixture(&shapeDef); // Now tell the dynamic body to compute it's mass properties base // on its shape. body->SetMassFromShapes(); // Prepare for simulation. Typically we use a time step of 1/60 of a // second (60Hz) and 10 iterations. This provides a high quality simulation // in most game scenarios. float32 timeStep = 1.0f / 60.0f; int32 velocityIterations = 8; int32 positionIterations = 1; // This is our little game loop. for (int32 i = 0; i < 60; ++i) { // Instruct the world to perform a single step of simulation. It is // generally best to keep the time step and iterations fixed. world.Step(timeStep, velocityIterations, positionIterations); // Now print the position and angle of the body. b2Vec2 position = body->GetPosition(); float32 angle = body->GetAngle(); printf("%4.2f %4.2f %4.2f\n", position.x, position.y, angle); } // When the world destructor is called, all bodies and joints are freed. This can // create orphaned pointers, so be careful about your world management. return 0; }
Box2DManager::Box2DManager() { b2Vec2 gravity(0, -9.8 * PPM); world = new b2World(gravity); }
//=================================================== /// This function assembles the matrix and the rhs: void GenMatRhsNS(MultiLevelProblem &ml_prob) { SystemTwo & my_system = ml_prob.get_system<SystemTwo>("Eqn_NS"); const unsigned Level = my_system.GetLevelToAssemble(); const uint _AdvPic_fl = 1; const uint _AdvNew_fl = 0; const uint _Stab_fl = 0; const double _Komp_fac = 0.; //========== GEOMETRIC ELEMENT ======== const uint space_dim = ml_prob._ml_msh->GetDimension(); //====== reference values ======================== //====== related to Quantities on which Operators act, and to the choice of the "LEADING" EQUATION Operator const double IRe = 1./ml_prob.GetInputParser().get("Re"); const double IFr = 1./ml_prob.GetInputParser().get("Fr"); //================================================ //================================================ //=======Operators @ gauss ======================= std::vector<double> dphijdx_g(space_dim); // ShapeDer(): used for Laplacian,Divergence, ..., associated to an Unknown Quantity std::vector<double> dphiidx_g(space_dim); // Test(): used for Laplacian,Advection,Divergence... associated to an Unknown Quantity std::vector<double> AdvRhs_g(space_dim); //Operator: Adv(u,u,phi) //================================================ my_system._LinSolver[Level]->_KK->zero(); my_system._LinSolver[Level]->_RESC->zero(); // ========================================== Mesh *mymsh = ml_prob._ml_msh->GetLevel(Level); elem *myel = mymsh->el; const unsigned myproc = mymsh->processor_id(); // ========================================== // ========================================== {//BEGIN VOLUME //======================== //======================== const uint mesh_vb = VV; const uint nel_e = ml_prob.GetMeshTwo()._off_el[mesh_vb][ml_prob.GetMeshTwo()._NoLevels*myproc+Level+1]; const uint nel_b = ml_prob.GetMeshTwo()._off_el[mesh_vb][ml_prob.GetMeshTwo()._NoLevels*myproc+Level]; for (int iel=0; iel < (nel_e - nel_b); iel++) { CurrentElem<double> currelem(iel,myproc,Level,VV,&my_system,ml_prob.GetMeshTwo(),ml_prob.GetElemType(),mymsh); CurrentGaussPointBase & currgp = CurrentGaussPointBase::build(currelem,ml_prob.GetQuadratureRule(currelem.GetDim())); //=========INTERNAL QUANTITIES (unknowns of the equation) ================== CurrentQuantity VelOldX(currgp); VelOldX._qtyptr = ml_prob.GetQtyMap().GetQuantity("Qty_Velocity0"); VelOldX._SolName = "Qty_Velocity0"; VelOldX.VectWithQtyFillBasic(); VelOldX.Allocate(); CurrentQuantity VelOldY(currgp); VelOldY._qtyptr = ml_prob.GetQtyMap().GetQuantity("Qty_Velocity1"); VelOldY._SolName = "Qty_Velocity1"; VelOldY.VectWithQtyFillBasic(); VelOldY.Allocate(); std::vector<CurrentQuantity*> VelOld_vec; VelOld_vec.push_back(&VelOldX); VelOld_vec.push_back(&VelOldY); const uint qtyzero_ord = VelOldX._FEord; const uint qtyzero_ndof = VelOldX._ndof; //same as Y //========= CurrentQuantity pressOld(currgp); pressOld._qtyptr = ml_prob.GetQtyMap().GetQuantity("Qty_Pressure"); pressOld._SolName = "Qty_Pressure"; pressOld.VectWithQtyFillBasic(); pressOld.Allocate(); const uint qtyone_ord = pressOld._FEord; const uint qtyone_ndof = pressOld._ndof; //order const uint qtyZeroToOne_DofOffset = VelOldX._ndof + VelOldY._ndof;//VelOld._ndof*VelOld._dim; //========= END INTERNAL QUANTITIES (unknowns of the equation) ================= //=========EXTERNAL QUANTITIES (couplings) ===== //========= //DOMAIN MAPPING CurrentQuantity xyz(currgp); //domain xyz._dim = space_dim; xyz._FEord = MESH_MAPPING_FE; xyz._ndof = currelem.GetElemType(xyz._FEord)->GetNDofs(); xyz.Allocate(); //other Physical constant Quantities //=======gravity================================== CurrentQuantity gravity(currgp); gravity._dim = space_dim; // gravity.Allocate(); CANNOT DO THIS NOW BECAUSE NOT ALL THE DATA FOR THE ALLOCATION ARE FILLED gravity._val_g.resize(gravity._dim); gravity._val_g[0] = ml_prob.GetInputParser().get("dirgx"); gravity._val_g[1] = ml_prob.GetInputParser().get("dirgy"); if ( space_dim == 3 ) gravity._val_g[2] = ml_prob.GetInputParser().get("dirgz"); //======================== //======================== currelem.Mat().zero(); currelem.Rhs().zero(); currelem.SetDofobjConnCoords(); currelem.SetElDofsBc(); currelem.ConvertElemCoordsToMappingOrd(xyz); //=======RETRIEVE the DOFS of the UNKNOWN QUANTITIES,i.e. MY EQUATION VelOldX.GetElemDofs(); VelOldY.GetElemDofs(); pressOld.GetElemDofs(); const uint el_ngauss = ml_prob.GetQuadratureRule(currelem.GetDim()).GetGaussPointsNumber(); for (uint qp = 0; qp < el_ngauss; qp++) { //=======here starts the "COMMON SHAPE PART"================== // the call of these things should be related to the Operator //also, the choice of the QuadratureRule should be dependent of the Involved Operators //and the FE orders on which these Operators act //these phi and dphi are used for the different stages: //BEFORE i: by the interpolation functions //INSIDE i: for the tEST functions and derivatives //INSIDE j: for the SHAPE functions and derivatives //again, it should be the Operator to decide what functions to be called, //for what FE ORDER and what DERIVATIVE ORDER //if we decide that the PREPARATION of the tEST and SHAPE //of a certain Unknown are COMMON TO ALL, //Then we must only concentrate on preparing the OTHER involved quantities in that Operator for (uint fe = 0; fe < QL; fe++) { currgp.SetPhiElDofsFEVB_g (fe,qp); currgp.SetDPhiDxezetaElDofsFEVB_g (fe,qp); } const double det = currgp.JacVectVV_g(xyz); const double dtxJxW_g = det*ml_prob.GetQuadratureRule(currelem.GetDim()).GetGaussWeight(qp); const double detb = det/el_ngauss; for (uint fe = 0; fe < QL; fe++) { currgp.SetDPhiDxyzElDofsFEVB_g (fe,qp); currgp.ExtendDphiDxyzElDofsFEVB_g(fe); } //=======end of the "COMMON SHAPE PART"================== //now we want to fill the element matrix and rhs with ONLY values AT GAUSS POINTS //we can divide the values at Gauss points into three parts: //1-constant values //2-values which depend on (i) //3-values which depend on (i,j) //1- can be used for filling both Ke and Fe //2- can be used to fill Fe and also Ke //3- can be used only for filling Ke //Here, before entering the (i,j) loop, you compute quantities that DO NOT depend on (i,j), //therefore the ELEMENT SUM, GAUSS SUM And tEST/SHAPE sums have ALL been performed //but, these quantities depend on idim and jdim, because they are involved in multiplications with tEST and SHAPE functions //Internal Quantities VelOldX.val_g(); VelOldX.grad_g(); VelOldY.val_g(); VelOldY.grad_g(); //Advection all VelOld for (uint idim=0; idim<space_dim; idim++) { AdvRhs_g[idim]=0.;} for (uint idim=0; idim<space_dim; idim++) { for (uint b=0; b<space_dim; b++) { AdvRhs_g[idim] += VelOld_vec[b]->_val_g[0]*VelOld_vec[idim]->_grad_g[0][b]; } } // grad is [ivar][idim], i.e. [u v w][x y z] //Divergence VelOld double Div_g=0.; for (uint idim=0; idim<space_dim; idim++) Div_g += VelOld_vec[idim]->_grad_g[0][idim]; //============================================================== //========= FILLING ELEMENT MAT/RHS (i loop) ==================== //============================================================== // TODO according to the order we should switch DIM loop and DOF loop for (uint i=0; i<qtyzero_ndof; i++) { //======="COMMON tEST PART for QTYZERO": func and derivative, of the QTYZERO FE ORD ========== const double phii_g = currgp._phi_ndsQLVB_g[qtyzero_ord][i]; for (uint idim=0; idim<space_dim; idim++) dphiidx_g[idim] = currgp._dphidxyz_ndsQLVB_g[qtyzero_ord][i+idim*qtyzero_ndof]; //======= END "COMMON tEST PART for QTYZERO" ========== for (uint idim=0; idim<space_dim; idim++) { const uint irowq=i+idim*qtyzero_ndof; // (i): dof of the tEST function //(idim): component of the tEST function currelem.Rhs()(irowq) += currelem.GetBCDofFlag()[irowq]* dtxJxW_g*( + _AdvNew_fl* AdvRhs_g[idim]*phii_g // NONLIN + IFr*gravity._val_g[idim]*phii_g // gravity ) + (1-currelem.GetBCDofFlag()[irowq])*detb*VelOld_vec[idim]->_val_dofs[i] //Dirichlet bc ; } // end filling element rhs u for (uint idim=0; idim<space_dim; idim++) { // filling diagonal for Dirichlet bc const uint irowq = i+idim*qtyzero_ndof; currelem.Mat()(irowq,irowq) += (1-currelem.GetBCDofFlag()[irowq])*detb; } // end filling diagonal for Dirichlet bc //============ QTYZERO x QTYZERO dofs matrix (A matrix) ============ for (uint j=0; j< qtyzero_ndof; j++) { //======="COMMON SHAPE PART for QTYZERO": func and derivative, of the QTYZERO FE ORD ========== // (j): dof of the SHAPE function double phij_g = currgp._phi_ndsQLVB_g[qtyzero_ord][j]; for (uint idim=0; idim<space_dim; idim++) dphijdx_g[idim] = currgp._dphidxyz_ndsQLVB_g[qtyzero_ord][j+idim*qtyzero_ndof]; //======= END "COMMON SHAPE PART for QTYZERO" ========== double Lap_g = Math::dot(&dphijdx_g[0],&dphiidx_g[0],space_dim); double Adv_g=0.; for (uint idim=0; idim<space_dim; idim++) Adv_g += VelOld_vec[idim]->_val_g[0] * dphijdx_g[idim]; // =Math::dot(&VelOld._val_g[0],dphijdx_g,space_dim); for (uint idim=0; idim<space_dim; idim++) { //filled in as 1-2-3 // 4-5-6 // 7-8-9 int irowq = i+idim*qtyzero_ndof; //(i) is still the dof of the tEST functions //(idim): component of the tEST functions currelem.Mat()(irowq,j+idim*qtyzero_ndof) // diagonal blocks [1-5-9] [idim(rows),idim(columns)] //(idim): component of the SHAPE functions += currelem.GetBCDofFlag()[irowq]* dtxJxW_g*( + _AdvPic_fl* Adv_g*phii_g //TODO NONLIN + _AdvNew_fl*phij_g*VelOld_vec[idim]->_grad_g[0][idim]*phii_g //TODO NONLIN + _AdvPic_fl*_Stab_fl* 0.5*Div_g*phij_g*phii_g //TODO NONLIN + IRe*( dphijdx_g[idim]*dphiidx_g[idim] + Lap_g) ); int idimp1=(idim+1)%space_dim; // block +1 [2-6-7] [idim(rows),idim+1(columns)] //(idimp1): component of the SHAPE functions currelem.Mat()(irowq,j+idimp1*qtyzero_ndof) += currelem.GetBCDofFlag()[irowq]* dtxJxW_g*( _AdvNew_fl*phij_g*VelOld_vec[idim]->_grad_g[0][idimp1]*phii_g //TODO NONLIN + IRe*( dphijdx_g[idim]*dphiidx_g[idimp1]) ); } } //============ END QTYZERO x QTYZERO dofs matrix (A matrix) ============ //============ QTYZERO x QTYONE dofs matrix (B^T matrix) // ( p*div(v) ) (NS eq) ============ for (uint j=0; j<qtyone_ndof; j++) { //======="COMMON SHAPE PART for QTYONE" ================== const double psij_g = currgp._phi_ndsQLVB_g[qtyone_ord][j]; //======="COMMON SHAPE PART for QTYONE" - END ============ const int jclml = j + qtyZeroToOne_DofOffset; for (uint idim=0; idim<space_dim; idim++) { uint irowq = i+idim*qtyzero_ndof; currelem.Mat()(irowq,jclml) += currelem.GetBCDofFlag()[irowq]* dtxJxW_g*(-psij_g*dphiidx_g[idim]); /** (-1.)*/ } } //============ END QTYZERO x QTYONE dofs matrix (B^T matrix) ============ if (i < qtyone_ndof) { //======="COMMON tEST PART for QTYONE" ============ double psii_g = currgp._phi_ndsQLVB_g[qtyone_ord][i]; //======= "COMMON tEST PART for QTYONE" - END ============ const uint irowl = i + qtyZeroToOne_DofOffset; currelem.Rhs()(irowl)=0.; // rhs // Mat()(irowl,j+space_dim*qtyzero_ndof) += (1./dt)*dtxJxW_g*(psii_g*psij_g)*_Komp_fac/dt; //no bc here (KOMP dp/dt=rho*div) for (uint j=0; j<qtyzero_ndof; j++) { // B element matrix q*div(u) //======="COMMON SHAPE PART for QTYZERO" ================== for (uint idim=0; idim<space_dim; idim++) dphijdx_g[idim] = currgp._dphidxyz_ndsQLVB_g[qtyzero_ord][j+idim*qtyzero_ndof]; //======="COMMON SHAPE PART for QTYZERO" - END ============ for (uint idim=0; idim<space_dim; idim++) currelem.Mat()(irowl,j+idim*qtyzero_ndof) += -/*(1./dt)**/dtxJxW_g*psii_g*dphijdx_g[idim]; } } // end pressure eq (cont) } //=================================================================== //========= END FILLING ELEMENT MAT/RHS (i loop) ===================== //=================================================================== } //============================================================== //================== END GAUSS LOOP (qp loop) ====================== //============================================================== /// Add element matrix and rhs to the global ones. my_system._LinSolver[Level]->_KK->add_matrix(currelem.Mat(),currelem.GetDofIndices()); my_system._LinSolver[Level]->_RESC->add_vector(currelem.Rhs(),currelem.GetDofIndices()); } // end of element loop }//END VOLUME my_system._LinSolver[Level]->_KK->close(); my_system._LinSolver[Level]->_RESC->close(); #ifdef DEFAULT_PRINT_INFO std::cout << " GenMatRhs " << my_system.name() << ": assembled Level " << Level << " with " << my_system._LinSolver[Level]->_KK->m() << " dofs" << std::endl; #endif return; }
int main() { //Creates a window using SFML sf::RenderWindow window(sf::VideoMode(800, 600, 32), "SFML Test", sf::Style::Default); //Defines gravity and sets up the game world in Box2D b2Vec2 gravity(0, 9.8f); b2World* m_world = new b2World(gravity); //Creates a new shape in SFML which we use later to draw the shapes sf::ConvexShape cShape; cShape.setFillColor(sf::Color::Red); /*To draw objects in Box2D we need a body and a fixture, think of a body as an overall container or model of the object with a fixture being the individual parts of the model ie a car where the overall car is the body with fixtures such as the wheels and steering wheel as part of the overall car body*/ //Define a new body b2BodyDef myBodyDef; //Set the body to be dynamic (means it can move through the gameworld and can be knocked around by other objects) myBodyDef.type = b2_dynamicBody; //Set its position in the world myBodyDef.position.Set(400, 120); //The starting angle of the body myBodyDef.angle = 0; //Add this body to the world b2Body* dynamicBody1 = m_world->CreateBody(&myBodyDef); //Create some vertices to make a shape, in this case it makes a 5 sided polygon b2Vec2 vertices[5]; vertices[0].Set(-10, 20); vertices[1].Set(-10, 0); vertices[2].Set( 0, -30); vertices[3].Set( 10, 0); vertices[4].Set( 10, 10); //Create a new polygon shape to store these vertices and turn them into a shape b2PolygonShape polyShape; polyShape.Set(vertices, 5); //Create a new fixture where you can store the shape b2FixtureDef polyFixtureDef; //Set the shape of the fixture to contain the polygon shape polyFixtureDef.shape = &polyShape; //Set the weight of the object. Note: having no weight means it wont react properly with objects! polyFixtureDef.density = 1; //Add this fixture to the body we made previously dynamicBody1->CreateFixture(&polyFixtureDef); //You can use the same definition of a body again, but make sure when you add it to the world it is something different! myBodyDef.position.Set(430, 120); //Add it to the game world b2Body* dynamicBody2 = m_world->CreateBody(&myBodyDef); //A reversed set of vertices than before vertices[0].Set(10, -20); vertices[1].Set(10, 0); vertices[2].Set(0, 30); vertices[3].Set(-10, 0); vertices[4].Set(-10, -10); //Set the poly shape to contain these new vertices polyShape.Set(vertices, 5); //Add the fixture to the body dynamicBody2->CreateFixture(&polyFixtureDef); //Set the body definition to static this time, meaning that it cant be moved around in the game world myBodyDef.type = b2_staticBody; //Set its position myBodyDef.position.Set(400,250); //Add it to the world b2Body* staticBody = m_world->CreateBody(&myBodyDef); //This is vertices to create a line along the bottom which infact is just an elongated rectangle b2Vec2 lineV[4]; lineV[0].Set(-150, 1); lineV[1].Set(-150, -1); lineV[2].Set(150, 50); lineV[3].Set(150, 52); //Set the shape to hold the line polyShape.Set(lineV, 4); //Add the line to the gameworld staticBody->CreateFixture(&polyFixtureDef); //This loops through the window, so while the window is open while (window.isOpen()) { //Creates a new event which handles what events are happening to the window sf::Event event; //So while there is an event while (window.pollEvent(event)) { //Check to see if someone has closed the window if (event.type == sf::Event::Closed) window.close(); } //Clear the window every frame to black window.clear(sf::Color::Black); //Step through the Box2D world, if we dont do this, box2D would not perform any physics calculations //If the world is running to fast or too slow try changing the 500.0f, lower if running to slow or higher if going to fast m_world->Step((1.0f / 500.0f), 10, 8); //Create a new size object, this is used for drawing the shapes in SFML size_t size = 0; /*Ok so now this code below gets every object that is currently in Box2D and sends it all to SFML to be drawn this is so we can actually get the shapes drawn on the screen as Box2D only has very basic draw functions*/ //This loops through every single body in the game world for(b2Body* b = m_world->GetBodyList(); b; b = b->GetNext()) { //This loops through every fixture in the current body for(b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) { //This checks to see if the type of the fixture is a polygon, if it is then draw the polygon if(f->GetType() == b2Shape::e_polygon) { //Stores a pointer to the shape stored in the fixture b2PolygonShape* s = (b2PolygonShape*)f->GetShape(); //Get the amount of vertices stored in the shape size = s->GetVertexCount(); //Set the size of the object in SFML so it knows how many vertices the shape should have cShape.setPointCount(size); //Loop through the vertices and send them from Box2D to the SFML shape for(int i = 0; i < size; i++) { //Stores the current vertex in v b2Vec2 v = s->GetVertex(i); //Converts the vertex from its local space to where it is in the world cShape.setPoint(i, sf::Vector2f(b->GetWorldVector(v).x + b->GetPosition().x, b->GetWorldVector(v).y + b->GetPosition().y)); } //Draws the shape onto the window window.draw(cShape); } /*TODO Add functions to allow lines, boxes and circles to be drawn */ } } //Displays the window on screen window.display(); } return 0; }
int main() { srand(time(NULL)); Level lvl; lvl.LoadFromFile("/home/oawad/Downloads/sfml/platformer/platformer.tmx"); b2Vec2 gravity(0.0f, 1.0f); b2World world(gravity); sf::Vector2i tileSize = lvl.GetTileSize(); std::vector<Object> block = lvl.GetObjects("block"); for(int i = 0; i < block.size(); i++) { b2BodyDef bodyDef; bodyDef.type = b2_staticBody; bodyDef.position.Set(block[i].rect.left + tileSize.x / 2 * (block[i].rect.width / tileSize.x - 1), block[i].rect.top + tileSize.y / 2 * (block[i].rect.height / tileSize.y - 1)); b2Body* body = world.CreateBody(&bodyDef); b2PolygonShape shape; shape.SetAsBox(block[i].rect.width / 2, block[i].rect.height / 2); body->CreateFixture(&shape,1.0f); } coin = lvl.GetObjects("coin"); for(int i = 0; i < coin.size(); i++) { b2BodyDef bodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position.Set(coin[i].rect.left + tileSize.x / 2 * (coin[i].rect.width / tileSize.x - 1), coin[i].rect.top + tileSize.y / 2 * (coin[i].rect.height / tileSize.y - 1)); bodyDef.fixedRotation = true; b2Body* body = world.CreateBody(&bodyDef); b2PolygonShape shape; shape.SetAsBox(coin[i].rect.width / 2, coin[i].rect.height / 2); body->CreateFixture(&shape,1.0f); coinBody.push_back(body); } enemy = lvl.GetObjects("enemy"); for(int i = 0; i < enemy.size(); i++) { b2BodyDef bodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position.Set(enemy[i].rect.left + tileSize.x / 2 * (enemy[i].rect.width / tileSize.x - 1), enemy[i].rect.top + tileSize.y / 2 * (enemy[i].rect.height / tileSize.y - 1)); bodyDef.fixedRotation = true; b2Body* body = world.CreateBody(&bodyDef); b2PolygonShape shape; shape.SetAsBox(enemy[i].rect.width / 2, enemy[i].rect.height / 2); body->CreateFixture(&shape,1.0f); enemyBody.push_back(body); } player = lvl.GetObject("player"); b2BodyDef bodyDef; bodyDef.type = b2_dynamicBody; bodyDef.position.Set(player.rect.left, player.rect.top); bodyDef.fixedRotation = true; playerBody = world.CreateBody(&bodyDef); b2PolygonShape shape; shape.SetAsBox(player.rect.width / 2, player.rect.height / 2); b2FixtureDef fixtureDef; fixtureDef.shape = &shape; fixtureDef.density = 1.0f; fixtureDef.friction = 0.3f; playerBody->CreateFixture(&fixtureDef); sf::Vector2i screenSize(800, 600); sf::RenderWindow window; window.create(sf::VideoMode(screenSize.x, screenSize.y), "Game"); sf::View view; view.reset(sf::FloatRect(0.0f, 0.0f, screenSize.x, screenSize.y)); view.setViewport(sf::FloatRect(0.0f, 0.0f, 2.0f, 2.0f)); while(window.isOpen()) { sf::Event evt; while(window.pollEvent(evt)) { switch(evt.type) { case sf::Event::Closed: window.close(); break; case sf::Event::KeyPressed: if(evt.key.code == sf::Keyboard::W && playerBody->GetLinearVelocity().y == 0) playerBody->SetLinearVelocity(b2Vec2(0.0f, -15.0f)); if(evt.key.code == sf::Keyboard::D) playerBody->SetLinearVelocity(b2Vec2(5.0f, 0.0f)); if(evt.key.code == sf::Keyboard::A) playerBody->SetLinearVelocity(b2Vec2(-5.0f, 0.0f)); break; } } world.Step(1.0f / 60.0f, 1, 1); for(b2ContactEdge* ce = playerBody->GetContactList(); ce; ce = ce->next) { b2Contact* c = ce->contact; for(int i = 0; i < coinBody.size(); i++) if(c->GetFixtureA() == coinBody[i]->GetFixtureList()) { coinBody[i]->DestroyFixture(coinBody[i]->GetFixtureList()); coin.erase(coin.begin() + i); coinBody.erase(coinBody.begin() + i); } for(int i = 0; i < enemyBody.size(); i++) if(c->GetFixtureA() == enemyBody[i]->GetFixtureList()) { if(playerBody->GetPosition().y < enemyBody[i]->GetPosition().y) { playerBody->SetLinearVelocity(b2Vec2(0.0f, -10.0f)); enemyBody[i]->DestroyFixture(enemyBody[i]->GetFixtureList()); enemy.erase(enemy.begin() + i); enemyBody.erase(enemyBody.begin() + i); } else { int tmp = (playerBody->GetPosition().x < enemyBody[i]->GetPosition().x) ? -1 : 1; playerBody->SetLinearVelocity(b2Vec2(10.0f * tmp, 0.0f)); } } } for(int i = 0; i < enemyBody.size(); i++) { if(enemyBody[i]->GetLinearVelocity() == b2Vec2_zero) { int tmp = (rand() % 2 == 1) ? 1 : -1; enemyBody[i]->SetLinearVelocity(b2Vec2(5.0f * tmp, 0.0f)); } } b2Vec2 pos = playerBody->GetPosition(); view.setCenter(pos.x + screenSize.x / 4, pos.y + screenSize.y / 4); window.setView(view); player.sprite.setPosition(pos.x, pos.y); for(int i = 0; i < coin.size(); i++) coin[i].sprite.setPosition(coinBody[i]->GetPosition().x, coinBody[i]->GetPosition().y); for(int i = 0; i < enemy.size(); i++) enemy[i].sprite.setPosition(enemyBody[i]->GetPosition().x, enemyBody[i]->GetPosition().y); window.clear(); lvl.Draw(window); window.draw(player.sprite); for(int i = 0; i < coin.size(); i++) window.draw(coin[i].sprite); for(int i = 0; i < enemy.size(); i++) window.draw(enemy[i].sprite); window.display(); } return 0; }
IMUTest::IMUTest(int& argc,char**& argv) :Vrui::Application(argc,argv), imu(0),tracker(0) { /* Parse the command line: */ IMUTracker::Scalar gravity(9.81); IMUTracker::Scalar driftCorrectionWeight(0.001); bool useMagnetometer=true; for(int i=1;i<argc;++i) { if(argv[i][0]=='-') { if(strcasecmp(argv[i]+1,"psmove")==0) { ++i; if(i<argc) { if(imu==0) { /* Connect to the PS Move device of the given index: */ PSMove* move=new PSMove(atoi(argv[i])); imu=move; } else std::cerr<<"Ignoring additional -psmove "<<argv[i]<<" argument"<<std::endl; } else std::cerr<<"Ignoring dangling -psmove argument"<<std::endl; } else if(strcasecmp(argv[i]+1,"rift")==0) { ++i; if(i<argc) { if(imu==0) { /* Connect to the Oculus Rift device of the given index: */ OculusRift* rift=new OculusRift(atoi(argv[i])); imu=rift; } else std::cerr<<"Ignoring additional -rift "<<argv[i]<<" argument"<<std::endl; } else std::cerr<<"Ignoring dangling -rift argument"<<std::endl; } else if(strcasecmp(argv[i]+1,"gravity")==0) { ++i; if(i<argc) { /* Update local gravity: */ gravity=IMUTracker::Scalar(atof(argv[i])); } else std::cerr<<"Ignoring dangling -gravity argument"<<std::endl; } else if(strcasecmp(argv[i]+1,"nomag")==0) { /* Disable magnetometer-based drift correction: */ useMagnetometer=false; } else if(strcasecmp(argv[i]+1,"drift")==0) { ++i; if(i<argc) { /* Update the drift correction weight: */ driftCorrectionWeight=IMUTracker::Scalar(atof(argv[i])); } else std::cerr<<"Ignoring dangling -drift argument"<<std::endl; } } } std::cout<<"Connected to IMU device "<<imu->getSerialNumber()<<std::endl; /* Set up the 6-DOF tracker: */ tracker=new IMUTracker(*imu); tracker->setGravity(gravity); tracker->setDriftCorrectionWeight(driftCorrectionWeight); tracker->setUseMagnetometer(useMagnetometer); /* Start streaming IMU measurements: */ imu->startStreamingCalibrated(Misc::createFunctionCall(this,&IMUTest::sampleCallback)); /* Add event tool classes to control the application: */ addEventTool("Reset Tracker",0,0); Vrui::setNavigationTransformation(Vrui::Point(0,0,0),Vrui::Scalar(15),Vrui::Vector(0,1,0)); }
void SolidSolver::solve() { Info << "Solve solid domain" << endl; scalar iCorr = 0; scalar displacementResidual = 1; scalar initialResidual = 1; lduMatrix::solverPerformance solverPerf; lduMatrix::debug = 0; scalar convergenceTolerance = absoluteTolerance; gradU = fvc::grad( U ); calculateEpsilonSigma(); dimensionedVector gravity( mesh.solutionDict().subDict( "solidMechanics" ).lookup( "gravity" ) ); for ( iCorr = 0; iCorr < maxIter; iCorr++ ) { U.storePrevIter(); tmp<surfaceTensorField> shearGradU = ( (I - n * n) & fvc::interpolate( gradU ) ); fvVectorMatrix UEqn ( rho * fvm::d2dt2( U ) == fvm::laplacian( 2 * muf + lambdaf, U, "laplacian(DU,U)" ) + fvc::div( mesh.magSf() * ( -(muf + lambdaf) * ( fvc::snGrad( U ) & (I - n * n) ) + lambdaf * tr( shearGradU() & (I - n * n) ) * n + muf * (shearGradU() & n) + muf * ( n & fvc::interpolate( gradU & gradU.T() ) ) + 0.5 * lambdaf * ( n * tr( fvc::interpolate( gradU & gradU.T() ) ) ) + ( n & fvc::interpolate( sigma & gradU ) ) ) ) ); // Add gravity UEqn -= rho * gravity; solverPerf = UEqn.solve(); U.relax(); gradU = fvc::grad( U ); calculateEpsilonSigma(); displacementResidual = gSumMag( U.internalField() - U.prevIter().internalField() ) / (gSumMag( U.internalField() ) + SMALL); displacementResidual = max( displacementResidual, solverPerf.initialResidual() ); if ( iCorr == 0 ) { initialResidual = displacementResidual; convergenceTolerance = std::max( relativeTolerance * displacementResidual, absoluteTolerance ); assert( convergenceTolerance > 0 ); } bool convergence = displacementResidual <= convergenceTolerance && iCorr >= minIter - 1; if ( convergence ) break; } lduMatrix::debug = 1; Info << "Solving for " << U.name(); Info << ", Initial residual = " << initialResidual; Info << ", Final residual = " << displacementResidual; Info << ", No outer iterations " << iCorr << endl; }
int main(int, char const**) { int width = 1000; int height = 600; char buff[15]; srand(time(NULL)); sf::Clock clock; sf::ContextSettings settings; settings.antialiasingLevel = 8; // Create the main window sf::RenderWindow window(sf::VideoMode(width, height), "My First SFML example", sf::Style::Default, settings); window.setFramerateLimit(60); sf::Font font; if (!font.loadFromFile("sansation.ttf")) { return EXIT_FAILURE; } sf::Text text; text.setFont(font); text.setString("Hello Benchmark!"); text.setCharacterSize(24); int text_x = (width - (text.getCharacterSize() * 5)) / 2; text.setPosition(text_x, 0); text.setColor(sf::Color::White); sf::Text fps; fps.setFont(font); fps.setCharacterSize(20); fps.setColor(sf::Color::Black); sf::Texture barrel; if (!barrel.loadFromFile("barrel.png")) { return EXIT_FAILURE; } std::vector<sf::Sprite> sprites(MAX_SPRITES); for (int i=0; i<MAX_SPRITES; i++) { sprites[i].setTexture(barrel); int x = rand() % width; int y = rand() % height; sprites[i].setPosition(x, y); } // Box2D b2Vec2 gravity(0.0f, 10.0f); b2World world(gravity); createGround(world); /** Prepare textures */ sf::Texture GroundTexture; sf::Texture BoxTexture; GroundTexture.loadFromFile("ground.png"); BoxTexture.loadFromFile("box.png"); createBox(world, 300, 0); createBox(world, 300, 20); while (window.isOpen()) { sf::Event event; while (window.pollEvent(event)) { if (event.type == sf::Event::Closed) window.close(); } // window.clear(sf::Color::Black); sprintf(buff, "FPS: %3.2f", framesPerSecond(clock)); fps.setString(sf::String(buff)); // //for (int i=0; i<MAX_SPRITES; i++) { // // sf::Sprite s = sprites[i]; // // s.setScale(rand() % 2 + 1, 2); // // s.setRotation((rand() % 720) - 360); // // window.draw(s); // //} // window.draw(text); // window.display(); //} if (sf::Mouse::isButtonPressed(sf::Mouse::Left)) { int MouseX = sf::Mouse::getPosition(window).x; int MouseY = sf::Mouse::getPosition(window).y; createBox(world, MouseX, MouseY); } world.Step(1.0f/60.f, 8, 3); window.clear(sf::Color::White); int BodyCount = 0; for (b2Body* BodyIterator = world.GetBodyList(); BodyIterator != 0; BodyIterator = BodyIterator->GetNext()) { if (BodyIterator->GetType() == b2_dynamicBody) { sf::Sprite Sprite; Sprite.setTexture(BoxTexture); Sprite.setOrigin(0.5f * SCALE, 0.5f * SCALE); Sprite.setPosition(SCALE * BodyIterator->GetPosition().x, SCALE * BodyIterator->GetPosition().y); Sprite.setRotation(BodyIterator->GetAngle() * 180/b2_pi); window.draw(Sprite); ++BodyCount; } else { sf::Sprite GroundSprite; GroundSprite.setTexture(GroundTexture); GroundSprite.setColor(sf::Color::Red); GroundSprite.setOrigin(40.0f * SCALE, 1.0f * SCALE); GroundSprite.setPosition(SCALE * BodyIterator->GetPosition().x, SCALE * BodyIterator->GetPosition().y); GroundSprite.setRotation(180/b2_pi * BodyIterator->GetAngle()); window.draw(GroundSprite); } } window.draw(fps); window.display(); } return 0; }