1

     2

#include "Physics.hh"

     3

#include "Engine.hh"

     4

#include "GameState.hh"

     5

#include "Terrain.hh"

     6

     7

#include <algorithm>

     8

#include <functional>

     9

#include <cmath>

    10

#include <assert.h>

    11

    12

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)

    13

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

    14

      gravity(gravity) {

    15

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

    16

    tick_timer.enable();

    17

}

    18

    19

void PhysicsWorld::addObject (PhysicsObject *object) {

    20

    objects.push_back(object);

    21

}

    22

    27

void PhysicsWorld::tick () {

    28

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

    29

    30

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

    31

        (*i)->tick(); 

    32

    }

    33

    34

    tick_counter++;

    35

}

    36

    37

uint32_t PhysicsWorld::getTick (void) {

    38

    return tick_counter;

    39

}

    40

    41

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

    42

                              Vector position, Vector velocity)

    43

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

    44

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

    45

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

    46

    world.addObject(this);

    47

}

    48

    49

/**

    50

 * Player walks on floor.

    51

 */

    52

Vector PhysicsObject::walk (bool right) {

    53

    float deltaX = right ? 1 : -1; // which way we are walking

    54

    Vector reached = this->position;

    55

    56

    // Is there upward ramp

    57

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

    58

        // Yes. Then we check n pixels up

    59

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

    60

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

    61

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

    62

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

    63

                break;

    64

            }

    65

        }

    66

    } else {

    67

        // Or downward ramp or flat

    68

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

    69

    70

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

    71

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

    72

            // but will fall one pixel down

    73

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

    74

                reached = position+Vector(deltaX, i);

    75

            } else {

    76

                break;

    77

            }

    78

    79

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

    80

            if (i >= 2) {

    81

                this->inAir = true;

    82

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

    83

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

    84

                reached.y -= 2;

    85

                break;

    86

            }

    87

        }

    88

    }

    89

    // And we return where we got

    90

    return reached;

    91

    92

}

    93

    94

/**

    95

 * Makes the player jump in the air.

    96

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

    97

 */

    98

void PhysicsObject::jump (int direction) {

    99

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

   100

    if (!this->inAir) {

   101

 	    velocity.y = -100;

   102

        switch (direction) {

   103

            case 1:

   104

                velocity.x += 20;

   105

                break;

   106

            case -1:

   107

                velocity.x -= 20;

   108

                break;

   109

            case 0:

   110

                break;

   111

            default:

   112

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

   113

        }

   114

	    inAir = true;

   115

    }

   116

}

   117

   118

bool PhysicsObject::possibleLocation (Vector loc) {

   119

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

   120

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

   121

            return false;

   122

    }

   123

    return true;

   124

}

   125

   126

void func1() {

   127

   128

}

   129

   130

/**

   131

 * Updates object speed and position. This function organises force

   132

 * integration and collision detection.

   133

 */   

   134

void PhysicsObject::updatePosition () {

   135

   136

    // Add gravity to the force queue

   137

    forceq.push(world.gravity);

   138

   139

    // Go trough every force in the queue

   140

    Force total;

   141

    while (!forceq.empty()) {

   142

        total += forceq.front();

   143

        forceq.pop();

   144

    }

   145

   146

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

   147

    // set inAir false

   148

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

   149

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

   150

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

   151

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

   152

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

   153

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

   154

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

   155

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

   156

            this->inAir = true;

   157

    }

   158

   159

    if(!possibleLocation(position)) {

   160

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

   161

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

   162

        //return;

   163

    }

   164

   165

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

   166

    // otherwise integrate the new position and velocity

   167

    if (!this->inAir) {

   168

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

   169

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

   170

        if (total.x != 0) {

   171

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

   172

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

   173

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

   174

        }

   175

    }

   176

   177

    if(!possibleLocation(position)) {

   178

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

   179

        func1();

   180

    }

   181

    Vector newPosition;

   182

    Vector velAfterTick;

   183

    // Calculate new position and velocity to the given references

   184

    integrate(total, PHYSICS_TICK_MS, newPosition, velAfterTick);

   185

    this->velocity = velAfterTick;

   186

   187

   188

    // Collision detection

   189

    bool collided = false;

   190

   191

    const Vector diffVec = newPosition-position;

   192

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

   193

    Vector reached = position;

   194

   195

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

   196

        reached += unitVector;

   197

        // Check if any of the shapes points collide

   198

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

   199

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

   200

                if (inAir) {

   201

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

   202

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

   203

                                                 reached-unitVector+shape[i]));

   204

                    //this->velocity *= COLLISION_ELASTICITY;

   205

                }

   206

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

   207

                collided = true;

   208

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

   209

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

   210

                }

   211

                break;

   212

            }

   213

        }

   214

        if (collided)

   215

            break;

   216

//        reached += unitVector;

   217

    }

   218

   219

   220

    if(!possibleLocation(reached)) {

   221

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

   222

        func1();

   223

    }

   224

   225

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

   226

    if(!collided) {

   227

        if(!possibleLocation(newPosition)) {

   228

            newPosition = reached;

   229

        } else {

   230

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

   231

        }

   232

    } else {

   233

        newPosition = reached;

   234

        onCollision();

   235

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

   236

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

   237

    }

   238

    if(!possibleLocation(newPosition)) {

   239

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

   240

        func1();

   241

    }

   242

    this->position = newPosition;

   243

    if(!possibleLocation(position)) {

   244

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

   245

        func1();

   246

    }

   247

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

   248

}

   249

   250

/**

   251

 * Bounces from straight wall in any direction.

   252

 * Direction given as normal of that wall

   253

 */

   254

void PhysicsObject::bounce (Vector normal) {

   255

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

   256

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

   257

        Vector nvel = velocity;

   258

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

   259

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

   260

        velocity = nvel;

   261

        // We lose some of our speed on collision

   262

        this->velocity *= COLLISION_ELASTICITY;

   263

    }

   264

}

   265

   266

/**

   267

 * Integrates given force over time and stores new position to

   268

 * posAfterTick and new velocity to velAfterTick.

   269

 * @param force Force vector.

   270

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

   271

 */

   272

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

   273

    posAfterTick = position;

   274

    velAfterTick = velocity;

   275

    Derivative tmpd;

   276

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

   277

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

   278

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

   279

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

   280

   281

   282

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

   283

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

   284

   285

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

   286

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

   287

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

   288

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

   289

}

   290

   291

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

   292

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

   293

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

   294

   295

    Derivative out;

   296

    out.dx = curVel;

   297

    out.dv = acceleration(force);

   298

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

   299

    return out;

   300

}

   301

   302

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

   303

    return (force/mass);

   304

}

   305

   306

void PhysicsObject::applyForce (Force force) {

   307

    // Add applied force to the queue

   308

    forceq.push(force);

   309

}

   310

   311

void PhysicsObject::changeAim(float da) {

   312

    this->aim += da;

   313

   314

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

   315

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

   316

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

   317

}

   318

   319

void PhysicsObject::setFacing(bool facingRight) {

   320

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

   321

    this->facingRight = facingRight;

   322

}

   323

   324

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

   325

    this->position = position;

   326

    this->velocity = velocity;

   327

    this->inAir = inAir;

   328

}

   329

   330

Vector PhysicsObject::getPosition () {

   331

    return this->position;

   332

}

   333

   334

bool PhysicsObject::getFacing() {

   335

    return this->facingRight;

   336

}

   337

   338

float PhysicsObject::getAim() {

   339

    return this->aim;

   340

}

   341

   342

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

   343

    return this->shape;

   344

}

   345

   346

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

   347

    this->shape = shape;

   348

}

   349

   350

void PhysicsObject::tick () {

   351

    this->updatePosition();

   352

}

   353

   354

void PhysicsObject::draw(CL_GraphicContext *gc) {

   355

    CL_Quad player(

   356

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

   357

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

   358

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

   359

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

   360

                   );

   361

   362

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

   363

   364

    const uint16_t chlen = 10;

   365

    uint16_t x = player.center().x;

   366

    uint16_t y = player.center().y;

   367

    if (facingRight) {

   368

        gc->draw_line(x, y,

   369

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

   370

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

   371

                      CL_Color::black);

   372

    } else {

   373

        gc->draw_line(x, y,

   374

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

   375

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

   376

                      CL_Color::black);

   377

    }

   378

}

   379