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

    23

void PhysicsWorld::tick () {

    24

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

    25

    26

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

    27

        (*i)->tick(); 

    28

    }

    29

    30

    tick_counter++;

    31

}

    32

    33

uint32_t PhysicsWorld::getTick (void) {

    34

    return tick_counter;

    35

}

    36

    37

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

    38

                              Vector position, Vector velocity)

    39

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

    40

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

    41

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

    42

    world.addObject(this);

    43

}

    44

    45

Vector PhysicsObject::walk (bool right) {

    47

    Vector reached = this->position;

    48

    49

    //for(int steps = 0; steps < 3; steps++) {

    50

    51

    // Go up but not if the wall is over two pixels

    52

    if(world.getType(cursor) != EMPTY) {

    54

    55

            if(height >= max)

    56

                return reached;

    57

    58

            cursor.y--;

    59

            if(world.getType(cursor) == EMPTY) {

    60

                // Check that the other parts of the worm don't collide with anything

    61

                if(possibleLocation(cursor)) {

    63

                    continue;

    64

                } else {

    65

                    // Can't get any further

    66

                    return reached;

    67

                }

    68

            }

    69

        }

    70

    } else {

    71

        if(possibleLocation(cursor)) {

    73

        }

    74

        // Start checking if the lower squares are empty

    75

    76

        for(int depth = 0, max = 3; depth < max+42; depth++) {

    77

    78

            if(depth >= max) {

    79

                // We can start a free fall now

    80

                //

    81

                // TODO it should be set inAir if it falls from a cliff

    82

                this->inAir = true;

    83

    84

                // Put some speed there to make loke smoother

    85

                //this->velocity.y = -5;

    86

                return reached;

    87

            }

    88

    89

            cursor.y++;

    90

            if(world.getType(cursor) == EMPTY) {

    91

                // Check that the other parts of the worm don't collide with anything

    92

                if(possibleLocation(cursor)) {

    94

                    continue;

    95

                } else {

    96

                    // Can't get any further

    97

                    return reached;

    98

                }

    99

            }

   100

        }

   101

    }      

   102

   103

    //    cursor.x += right ? 1 : -1;

   104

    //}

   105

    return reached;

   106

}

   107

   108

void PhysicsObject::jump () {

   109

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

   110

    if (!this->inAir) {

   111

 	    velocity.y = -100;

   112

        velocity.x += facingRight ? 20 : -20;

   113

	    inAir = true;

   114

    }

   115

}

   116

   117

bool PhysicsObject::possibleLocation (Vector loc) {

   118

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

   119

        if(world.getType(loc+shape[i]) != EMPTY)

   120

            return false;

   121

    }

   122

    return true;

   123

}

   124

   125

/**

   126

 * Updates object speed and position. This function organises force

   127

 * integration and collision detection.

   128

 */   

   129

void PhysicsObject::updatePosition () {

   130

   131

    // Add gravity to the force queue

   132

    forceq.push(world.gravity);

   133

   134

    // Go trough every force in the queue

   135

    Force total;

   136

    posAfterTick = position;

   137

    velAfterTick = velocity;

   138

    while (!forceq.empty()) {

   139

        total += forceq.front();

   140

        forceq.pop();

   141

    }

   142

   143

    // TODO: This is _ugly_ (but not so ugly as the last one I think)

   144

    // hack. I think we should handle walking from the collision

   145

    // detection code.

   146

   147

    if (!this->inAir) {

   148

        if (total.x != 0)

   149

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

   150

        return; 

   151

        //total.x = 0;

   152

    }

   153

   154

    integrate(total, PHYSICS_TICK_MS);

   155

   156

    Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;

   157

    this->velocity = velAfterTick;

   158

   159

   160

    // Collision detection

   161

    bool collided = false;

   162

   163

    const Vector diffVec = newPosition-position;

   164

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

   165

    Vector reached = position;

   166

   167

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

   168

        // Check if any of the shapes points collide

   169

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

   170

            if (world.getType(reached+shape[i]) != EMPTY) {  // Collision

   171

                if (inAir) {

   172

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

   173

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

   174

                                                 reached-unitVector+shape[i]));

   175

                    //this->velocity *= COLLISION_ELASTICITY;

   176

                }

   177

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

   178

                collided = true;

   179

                if (this->velocity.length() < PLAYER_MIN_SPEED) {

   180

                    this->inAir = false;

   181

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

   182

                }

   183

                break;

   184

            }

   185

        }

   186

        if (collided)

   187

            break;

   188

        reached += unitVector;

   189

    }

   190

   191

   192

    /*

   193

      bool collided = false;

   194

   195

      //goes 1 unit forward every step and check if has hit anything

   196

      Vector unitVector = (newPosition-position) / (newPosition-position).length();

   197

   198

      Vector tmpVector = position;

   199

      Vector reached = position;

   200

   201

      int steps = (int) (newPosition-position).length() + 2;

   202

   203

      //Engine::log(DEBUG, "physics.update_position") << unitVector-newPosition;

   204

      //Vector foo = position+unitVector*steps-newPosition;

   205

      //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Virhe: "<< foo;

   206

   207

      for(int i = 0; i < steps; i++) {

   208

      tmpVector += unitVector;

   209

   210

      float minVelocity = 10;

   211

      // Check if any of the four corners of the worm collide

   212

      for(int sh = 0; sh < 4; sh++) {

   213

      if(world.getType(tmpVector+shape[sh]) != EMPTY) {

   214

      reached = position + unitVector*(i-1);

   215

      collided = true;

   216

      this->bounce(world.getNormal(tmpVector+shape[sh], tmpVector-unitVector+shape[sh]));

   217

      this->velocity *= 0.4;

   218

      if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   219

      this->inAir = false;

   220

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

   221

      }

   222

      break;

   223

      }

   224

      }

   225

      if(collided)

   226

      break;

   227

      }

   228

    */

   229

   230

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

   231

    if(!collided) {

   232

        if(world.getType(newPosition+shape[0]) != EMPTY || (world.getType(newPosition+shape[1]) != EMPTY) 

   233

           || (world.getType(newPosition+shape[2]) != EMPTY) || (world.getType(newPosition+shape[3]) != EMPTY)) {

   234

   235

            //  Engine::log(DEBUG, "physics.update_position") << "didnt hit";

   236

            // There was error, and there is ground

   237

            //newPosition = tmpVector;

   238

        } else {

   239

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

   240

        }

   241

    } else {

   242

        newPosition = reached;

   243

        onCollision();

   244

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

   245

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

   246

    }

   247

    this->position = newPosition;

   248

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

   249

}

   250

   251

/**

   252

 * Bounces from straight wall in any direction.

   253

 * Direction given as normal of that wall

   254

 */

   255

void PhysicsObject::bounce (Vector normal) {

   256

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

   257

        // Engine::log(DEBUG, "PhysicsObject.bounce") << "Velocity: " << velocity;

   258

        Vector nvel = velocity;

   259

        //        Engine::log(DEBUG, "PhysicsObject.bounce") << "New Velocity: " << nvel;

   260

//        nvel = nvel - ((1+COLLISION_ELASTICITY)*((nvel*normal)/(normal*normal))*normal);

   261

   262

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

   263

//        Engine::log(DEBUG, "PhysicsObject.bounce") << "Projection: " << nvel;

   264

        velocity = nvel;

   265

        this->velocity *= COLLISION_ELASTICITY;

   266

    }

   267

}

   268

   269

/**

   270

 * Integrates given force over time and stores new position to

   271

 * posAfterTick and new velocity to velAfterTick.

   272

 * @param force Force vector.

   273

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

   274

 */

   275

void PhysicsObject::integrate(Force force, TimeMS dt) {

   276

    Derivative tmpd;

   277

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

   278

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

   279

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

   280

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

   281

   282

   283

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

   284

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

   285

   286

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

   287

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

   288

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

   289

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

   290

}

   291

   292

Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {

   293

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

   294

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

   295

   296

    Derivative out;

   297

    out.dx = curVel;

   298

    out.dv = acceleration(force);

   299

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

   300

    return out;

   301

}

   302

   303

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

   304

    return (force/mass);

   305

}

   306

   307

void PhysicsObject::applyForce (Force force) {

   308

    // Add applied force to the queue

   309

    forceq.push(force);

   310

}

   311

   312

void PhysicsObject::changeAim(float da) {

   313

    this->aim += da;

   314

   315

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

   316

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

   317

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

   318

}

   319

   320

void PhysicsObject::setFacing(bool facingRight) {

   321

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

   322

    this->facingRight = facingRight;

   323

}

   324

   325

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

   326

    this->position = position;

   327

    this->velocity = velocity;

   328

    this->inAir = inAir;

   329

}

   330

   331

Vector PhysicsObject::getPosition () {

   332

    return this->position;

   333

}

   334

   335

bool PhysicsObject::getFacing() {

   336

    return this->facingRight;

   337

}

   338

   339

float PhysicsObject::getAim() {

   340

    return this->aim;

   341

}

   342

   343

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

   344

    return this->shape;

   345

}

   346

   347

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

   348

    this->shape = shape;

   349

}

   350

   351

void PhysicsObject::tick () {

   352

    this->updatePosition();

   353

}

   354

   355

void PhysicsObject::draw(CL_GraphicContext *gc) {

   356

    CL_Quad player(

   357

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

   358

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

   359

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

   360

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

   361

                   );

   362

   363

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

   364

   365

    const uint16_t chlen = 10;

   366

    uint16_t x = player.center().x;

   367

    uint16_t y = player.center().y;

   368

    if (facingRight) {

   369

        gc->draw_line(x, y,

   370

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

   371

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

   372

                      CL_Color::black);

   373

    } else {

   374

        gc->draw_line(x, y,

   375

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

   376

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

   377

                      CL_Color::black);

   378

    }

   379

}

   380