1

#include "Physics.hh"

     2

#include "Engine.hh"

     3

#include "GameState.hh"

     4

#include "Terrain.hh"

     5

     6

#include <algorithm>

     7

#include <functional>

     8

#include <cmath>

     9

#include <assert.h>

    10

    11

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)

    12

    : Terrain(1337), tick_timer(PHYSICS_TICK_MS), tick_counter(0), dimensions(dimensions),

    13

      gravity(gravity) {

    14

    slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);

    15

    tick_timer.enable();

    16

}

    17

    18

void PhysicsWorld::addPlayerObject (PhysicsObject *object) {

    19

    players.push_back(object);

    20

}

    21

    22

void PhysicsWorld::addProjectile (PhysicsObject *projectile) {

    23

    projectiles.push_back(projectile);

    24

}

    25

    26

void PhysicsWorld::tick () {

    27

    //    Engine::log(DEBUG, "physics.apply_force") << "*tick*";

    28

    29

    for (std::list<PhysicsObject*>::iterator i = players.begin(); i != players.end(); i++) {

    30

        (*i)->tick(); 

    31

    }

    32

    33

    for (std::list<PhysicsObject*>::iterator i = projectiles.begin(); i != projectiles.end(); i++) {

    34

        (*i)->tick();

    35

    }

    36

    37

    tick_counter++;

    38

}

    39

    40

uint32_t PhysicsWorld::getTick (void) {

    41

    return tick_counter;

    42

}

    43

    44

void PhysicsWorld::draw(CL_GraphicContext *gc) {

    45

    Terrain::draw(gc);

    46

    47

    // Draw players

    48

    for (std::list<PhysicsObject*>::iterator it = players.begin(); it != players.end(); it++) {

    49

        (*it)->draw(gc);

    50

    }

    51

    // Draw projectiles

    52

    for (std::list<PhysicsObject*>::iterator it = projectiles.begin(); it != projectiles.end(); it++) {

    53

        (*it)->draw(gc);

    54

    }

    55

}

    56

    57

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, 

    58

                              Vector position, Vector velocity)

    59

    : world(world), position(position), velocity(velocity),

    60

      mass(mass), inAir(true), aim(0), facingRight(true), reloadTimer(0) {

    61

    // TODO: Is thir the right way to do this?

    62

    //world.addPlayerObject(this);

    63

}

    64

    65

/**

    66

 * Player walks on floor.

    67

 */

    68

Vector PhysicsObject::walk_one_step (float partial, bool right) {

    69

    // which way we are walking

    70

    float deltaX = right ? partial : -partial;

    71

    Vector reached = this->position;

    72

    if(reached.roundToInt() == (reached+Vector(deltaX, 0)).roundToInt()) {

    73

        return reached+Vector(deltaX, 0);

    74

    }

    75

    // Is there upward ramp

    76

    if(!possibleLocation(position+Vector(deltaX, 0))) {

    77

        // Yes. Then we check n pixels up

    78

        for(int i = 1; i < 3; i++) {

    79

            if(possibleLocation(position+Vector(deltaX, -i))) {

    80

                // and when there is finally EMPTY, we can walk

    81

                reached = position+Vector(deltaX, -i);

    82

                break;

    83

            }

    84

        }

    85

    } else {

    86

        // Or downward ramp or flat

    87

        for(int i = 0; 1; i++) {

    88

    89

            // And when there is finally ground we can step on

    90

            // it. If there is no gound we still step there,

    91

            // but will fall one pixel down

    92

            if(possibleLocation(position+Vector(deltaX, i))) {

    93

                reached = position+Vector(deltaX, i);

    94

            } else {

    95

                break;

    96

            }

    97

    98

            // If the fall is big enough, set the worm in the air

    99

            if (i >= 2) {

   100

//                Vector back = walk(dt, !right);

   101

                this->inAir = true;

   102

//                this->velocity.x = right ? velocity : -velocity;

   103

                // Avoid stepping two pixels down when it starts to free fall

   104

                reached.y -= 2;

   105

//                this->velocity = (reached-back)*1000/dt;

   106

                break;

   107

            }

   108

        }

   109

    }

   110

    // And we return where we got

   111

    return reached;

   112

}

   113

Vector PhysicsObject::walk (TimeMS dt, bool right) {

   114

    float velocity = PLAYER_WALK_SPEED;

   115

    float walkAmount = (velocity*dt)/1000;

   116

    Vector reached = this->position;

   117

    while(walkAmount > 0) {

   118

        walk_one_step((1 < walkAmount ? 1 : walkAmount), right);

   119

        walkAmount--;

   120

    }

   122

}

   123

   124

/**

   125

 * Makes the player jump in the air.

   126

 * @param direction -1: jump left, 0: jump up, 1: jump right

   127

 */

   128

void PhysicsObject::jump (int direction) {

   129

    // Jump only if player is "on the ground"

   130

    if (!this->inAir) {

   131

 	    velocity.y = -100;

   132

        switch (direction) {

   133

            case 1:

   134

                velocity.x += 20;

   135

                break;

   136

            case -1:

   137

                velocity.x -= 20;

   138

                break;

   139

            case 0:

   140

                break;

   141

            default:

   142

                throw std::logic_error("Invalid jump direction");

   143

        }

   144

	    inAir = true;

   145

    }

   146

}

   147

   148

bool PhysicsObject::possibleLocation (Vector loc) {

   149

    for(unsigned int i = 0; i < this->shape.size(); i++) {

   150

        if(world.collides(loc+shape[i]))

   151

            return false;

   152

    }

   153

    return true;

   154

}

   155

   156

void func1() {

   157

   158

}

   159

   160

/**

   161

 * Updates object speed and position. This function organises force

   162

 * integration and collision detection.

   163

 */   

   164

void PhysicsObject::updatePosition () {

   165

   166

    // Reloads weapon if not reloaded

   167

    reloadTimer -= PHYSICS_TICK_MS;

   168

    if(reloadTimer < 0)

   169

        reloadTimer = 0;

   170

   171

    // Add gravity to the force queue

   172

    forceq.push(world.gravity);

   173

   174

    // Go trough every force in the queue

   175

    Force total;

   176

    while (!forceq.empty()) {

   177

        total += forceq.front();

   178

        forceq.pop();

   179

    }

   180

   181

    // If the player has stopped and there's some ground under some of the 3 some of the 3t

   182

    // set inAir false

   183

    if (this->velocity == Vector(0,0)) {

   184

        this->inAir = !world.collides(this->position+shape[1]+Vector(0, 1))

   185

                      && !world.collides(this->position+shape[2]+Vector(0, 1))

   186

                      && !world.collides(this->position+shape[3]+Vector(0, 1));

   187

        // If, however, there's a force caused by a bomb, e.g., set it in air.

   188

        // Still, we have to be able to separate forces caused by walking attempts

   189

        // and bombs etc (+0.1 because float comparison can be dangerous)

   190

        if (total.y < 0 || abs(total.x) > PLAYER_MOVE_FORCE + 0.1)

   191

            this->inAir = true;

   192

    }

   193

   194

    if(!possibleLocation(position)) {

   195

        //if we are trapped in ground form dirtball or something

   196

        //we might want to just return and set velocity to some value

   197

        //return;

   198

    }

   199

   200

    // If the worm is not in the air make it walk,

   201

    // otherwise integrate the new position and velocity

   202

    if (!this->inAir) {

   203

        //std::cout << "Tryin to walk" << std::endl;

   204

        // It walks only if there's some vertical force

   205

        if (total.x != 0) {

   206

            std::cout << "Succeeding to walk" << std::endl;

   207

            this->position = walk(PHYSICS_TICK_MS, total.x > 0);

   208

            this->velocity = Vector(0,0);

   209

        }

   210

    }

   211

   212

    if(!possibleLocation(position)) {

   213

        Engine::log(DEBUG, "great failure") << "great failure";

   214

        func1();

   215

    }

   216

    Vector newPosition;

   217

    Vector velAfterTick;

   218

    // Calculate new position and velocity to the given references

   219

    integrate(total, PHYSICS_TICK_MS, newPosition, velAfterTick);

   220

    this->velocity = velAfterTick;

   221

   222

   223

    // Collision detection

   224

    bool collided = false;

   225

   226

    const Vector diffVec = newPosition-position;

   227

    const Vector unitVector = diffVec / diffVec.length();

   228

    Vector reached = position;

   229

   230

    while ((position-reached).sqrLength() < diffVec.sqrLength()) {

   231

        reached += unitVector;

   232

        // Check if any of the shapes points collide

   233

        for (uint64_t i = 0; i < shape.size(); i++) {

   234

            if (world.collides(reached+shape[i])) {  // Collision

   235

                if (inAir) {

   236

                    //                    Engine::log(DEBUG, "Here");

   237

                    this->bounce(world.getNormal(reached+shape[i], 

   238

                                                 reached-unitVector+shape[i]));

   239

                    //this->velocity *= COLLISION_ELASTICITY;

   240

                }

   241

                reached = reached - unitVector; // Return to last point

   242

                collided = true;

   243

                if (this->velocity.sqrLength() < PLAYER_MIN_SPEED * PLAYER_MIN_SPEED) {

   244

                    this->velocity = Vector(0,0);

   245

                }

   246

                break;

   247

            }

   248

        }

   249

        if (collided)

   250

            break;

   251

//        reached += unitVector;

   252

    }

   253

   254

   255

    if(!possibleLocation(reached)) {

   256

        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "logic error reached should not be possible to be impossible.. diffVec: " << diffVec;

   257

        func1();

   258

    }

   259

   260

    // In case of some float error check the final coordinate

   261

    if(!collided) {

   262

        if(!possibleLocation(newPosition)) {

   263

            newPosition = reached;

   264

        } else {

   265

            // This means everything was ok, so no need to do anything

   266

        }

   267

    } else {

   268

        newPosition = reached;

   269

        onCollision();

   270

        //this->velocity = Vector(0, 0);

   271

        //TODO: it shouldn't just stop on collision

   272

    }

   273

    if(!possibleLocation(newPosition)) {

   274

        Engine::log(DEBUG, "great failure") << "great failure";

   275

        func1();

   276

    }

   277

    this->position = newPosition;

   278

    if(!possibleLocation(position)) {

   279

        Engine::log(DEBUG, "great failure") << "great failure";

   280

        func1();

   281

    }

   282

//    Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Pos: " << this->position;

   283

}

   284

   285

/**

   286

 * Bounces from straight wall in any direction.

   287

 * Direction given as normal of that wall

   288

 */

   289

void PhysicsObject::bounce (Vector normal) {

   290

    // normal.sqrLength can't be 0 when got from getNormal()

   291

    if (normal.sqrLength() != 0) {

   292

        Vector nvel = velocity;

   293

        // We project the velocity on normal and remove twice that much from velocity

   294

        nvel = nvel - ((2)*((nvel*normal)/(normal*normal))*normal);

   295

        velocity = nvel;

   296

        // We lose some of our speed on collision

   297

        this->velocity *= this->collision_elasticity;

   298

    }

   299

}

   300

   301

/**

   302

 * Integrates given force over time and stores new position to

   303

 * posAfterTick and new velocity to velAfterTick.

   304

 * @param force Force vector.

   305

 * @param dt The time the force is applied (<=PHYSICS_TICK_MS)

   306

 */

   307

void PhysicsObject::integrate(Force force, TimeMS dt, Vector &posAfterTick, Vector &velAfterTick) {

   308

    posAfterTick = position;

   309

    velAfterTick = velocity;

   310

    Derivative tmpd;

   311

    Derivative k1 = evaluate(force, 0, tmpd, posAfterTick, velAfterTick);

   312

    Derivative k2 = evaluate(force, 0.5f*dt, k1, posAfterTick, velAfterTick);

   313

    Derivative k3 = evaluate(force, 0.5f*dt, k2, posAfterTick, velAfterTick);

   314

    Derivative k4 = evaluate(force, dt, k3, posAfterTick, velAfterTick);

   315

   316

   317

    const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;

   318

    const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;

   319

   320

    //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;

   321

    posAfterTick = posAfterTick + (dxdt * dt)/1000;

   322

    velAfterTick = velAfterTick + (dvdt * dt)/1000;

   323

    //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;

   324

}

   325

   326

Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d, const Vector &posAfterTick, const Vector &velAfterTick) {

   327

    Vector curPos = posAfterTick + (d.dx*dt)/1000;

   328

    Vector curVel = velAfterTick + (d.dv*dt)/1000;

   329

   330

    Derivative out;

   331

    out.dx = curVel;

   332

    out.dv = acceleration(force);

   333

    //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;

   334

    return out;

   335

}

   336

   337

Vector PhysicsObject::acceleration(const Force &force) {

   338

    return (force/mass);

   339

}

   340

   341

void PhysicsObject::applyForce (Force force) {

   342

    // Add applied force to the queue

   343

    forceq.push(force);

   344

}

   345

   346

void PhysicsObject::changeAim(float da) {

   347

    this->aim += da;

   348

   349

    if (this->aim > PLAYER_AIM_MAX) this->aim = PLAYER_AIM_MAX;

   350

    if (this->aim < PLAYER_AIM_MIN) this->aim = PLAYER_AIM_MIN;

   351

    //Engine::log(DEBUG, "PhysicsObject.changeAim") << "Player aim: " << this->aim;

   352

}

   353

   354

void PhysicsObject::setFacing(bool facingRight) {

   355

    //Engine::log(DEBUG, "PhysicsObject.setFacing") << "Facing: " << right;

   356

    this->facingRight = facingRight;

   357

}

   358

   359

void PhysicsObject::updatePhysics (Vector position, Vector velocity, bool inAir) {

   360

    this->position = position;

   361

    this->velocity = velocity;

   362

    this->inAir = inAir;

   363

}

   364

   365

Vector PhysicsObject::getPosition () {

   366

    return this->position;

   367

}

   368

   369

bool PhysicsObject::getFacing() {

   370

    return this->facingRight;

   371

}

   372

   373

float PhysicsObject::getAim() {

   374

    return this->aim;

   375

}

   376

   377

std::vector<Vector>& PhysicsObject::getShape () {

   378

    return this->shape;

   379

}

   380

   381

void PhysicsObject::setShape (std::vector<Vector> shape) {

   382

    this->shape = shape;

   383

}

   384

   385

void PhysicsObject::tick () {

   386

    this->updatePosition();

   387

}

   388

   389

bool PhysicsObject::canShoot() {

   390

    return this->reloadTimer <= 0;

   391

}

   392

   393

void PhysicsObject::draw(CL_GraphicContext *gc) {

   394

    CL_Quad player(

   395

                   (position+shape[0]).x, (position+shape[0]).y,

   396

                   (position+shape[1]).x, (position+shape[1]).y,

   397

                   (position+shape[2]).x, (position+shape[2]).y,

   398

                   (position+shape[3]).x, (position+shape[3]).y

   399

                   );

   400

   401

    gc->fill_quad(player, CL_Color::green);

   402

   403

    const uint16_t chlen = 10;

   404

    uint16_t x = player.center().x;

   405

    uint16_t y = player.center().y;

   406

    if (facingRight) {

   407

        gc->draw_line(x, y,

   408

                      x + std::cos(aim)*chlen,

   409

                      y - std::sin(aim)*chlen,

   410

                      CL_Color::black);

   411

    } else {

   412

        gc->draw_line(x, y,

   413

                      x - std::cos(aim)*chlen,

   414

                      y - std::sin(aim)*chlen,

   415

                      CL_Color::black);

   416

    }

   417

}

   418