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;
}
